From 13267a9e97dce849ed0807453d8e93b627667d5b Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 25 Aug 2017 17:05:40 -0400 Subject: [PATCH] add gps goal control, re-structured launch dir and add height to buzz moveto closure --- buzz_scripts/flock.bzz | 6 +- buzz_scripts/graphform.bzz | 20 +-- buzz_scripts/include/uavstates.bzz | 73 +++++++--- buzz_scripts/testalone.bzz | 9 +- include/buzzuav_closures.h | 2 +- launch/husky.launch | 59 --------- launch/launch_config/topics.yaml | 12 ++ launch/rosbuzz-solo.launch | 57 -------- launch/rosbuzz-testing-sitl.launch | 51 ------- launch/rosbuzz-testing.launch | 43 ------ launch/rosbuzz.launch | 32 +++-- launch/rosbuzz_2_parallel.launch | 27 ---- launch/rosbuzzm100.launch | 16 --- src/buzz_utility.cpp | 6 +- src/buzzuav_closures.cpp | 95 ++++++------- src/roscontroller.cpp | 205 ++++++++++++++--------------- 16 files changed, 261 insertions(+), 452 deletions(-) delete mode 100644 launch/husky.launch create mode 100644 launch/launch_config/topics.yaml delete mode 100644 launch/rosbuzz-solo.launch delete mode 100644 launch/rosbuzz-testing-sitl.launch delete mode 100644 launch/rosbuzz-testing.launch delete mode 100644 launch/rosbuzz_2_parallel.launch delete mode 100644 launch/rosbuzzm100.launch diff --git a/buzz_scripts/flock.bzz b/buzz_scripts/flock.bzz index 3db9d25..bb2da81 100644 --- a/buzz_scripts/flock.bzz +++ b/buzz_scripts/flock.bzz @@ -38,7 +38,7 @@ function action() { # Move according to vector print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum)) - uav_moveto(accum.x, accum.y) + uav_moveto(accum.x, accum.y, 0.0) UAVSTATE = "LENNARDJONES" # if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS @@ -47,11 +47,11 @@ function action() { # } else if(timeW>=WAIT_TIMEOUT/2) { # UAVSTATE ="GOEAST" # timeW = timeW+1 -# uav_moveto(0.0,5.0) +# uav_moveto(0.0, 5.0, 0.0) # } else { # UAVSTATE ="GONORTH" # timeW = timeW+1 -# uav_moveto(5.0,0.0) +# uav_moveto(5.0, 0.0, 0.0) # } } diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index adb8139..ae0bd5b 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -446,7 +446,7 @@ function TransitionToJoined(){ m_navigation.x=0.0 m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) } # @@ -471,7 +471,7 @@ while(iROBOT_MAXVEL*2) m_navigation = math.vec2.scale(m_navigation, ROBOT_MAXVEL*2/math.vec2.length(m_navigation)) - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) }else{ #no joined robots in sight i=0 var tempvec={.x=0.0,.y=0.0} @@ -545,7 +545,7 @@ function DoFree() { i=i+1 } m_navigation=math.vec2.scale(tempvec,1.0/i) - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) } @@ -554,7 +554,7 @@ function DoFree() { if(step_cunt<=1){ m_navigation.x=0.0 m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) } #set message m_selfMessage.State=s2i(m_eState) @@ -614,7 +614,7 @@ function DoAsking(){ } m_navigation.x=0.0 m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) } # @@ -662,7 +662,7 @@ function DoJoining(){ S2Target = math.vec2.scale(S2Target, ROBOT_MAXVEL/math.vec2.length(S2Target)) m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing) - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) @@ -760,7 +760,7 @@ function DoJoined(){ m_navigation.x=0.0 m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) #check if should to transists to lock @@ -789,7 +789,7 @@ if(m_nLabel!=0){ m_navigation=motion_vector() } #move -uav_moveto(m_navigation.x,m_navigation.y) +uav_moveto(m_navigation.x, m_navigation.y, 0.0) } function action(){ @@ -912,7 +912,7 @@ function destroy() { #clear neighbour message m_navigation.x=0.0 m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) m_vecNodes={} #stop listening neighbors.ignore("m") diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 4ecac31..10d7118 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -5,8 +5,10 @@ ######################################## TARGET_ALTITUDE = 5.0 UAVSTATE = "TURNEDOFF" +GOTO_MAXVEL = 2 +goal = {.range=0, .bearing=0} -function uav_initswarm(){ +function uav_initswarm() { s = swarm.create(1) s.join() statef=turnedoff @@ -26,13 +28,10 @@ function idle() { function takeoff() { UAVSTATE = "TAKEOFF" statef=takeoff - #log("TakeOff: ", flight.status) - #log("Relative position: ", position.altitude) if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { barrier_set(ROBOTS,action,land) barrier_ready() - #statef=hexagon } else { log("Altitude: ", TARGET_ALTITUDE) @@ -44,7 +43,7 @@ function takeoff() { function land() { UAVSTATE = "LAND" statef=land - #log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ neighbors.broadcast("cmd", 21) uav_land() @@ -58,6 +57,28 @@ function land() { } } +function goto() { + UAVSTATE = "GOTO" + statef=goto + # print(rc_goto.id, " is going alone to lat=", rc_goto.latitude) + if(rc_goto.id==id){ + s.leave() + rb_from_gps(rc_goto.latitude, rc_goto.longitude) + print(id, " has to move ", goal.range) + m_navigation = math.vec2.newp(goal.range, goal.bearing) + if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { + m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) + } else if(math.vec2.length(m_navigation)>0.25) + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) + else + statef = idle + } else { + neighbors.broadcast("cmd", 16) + neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) + } +} + function follow() { if(size(targets)>0) { UAVSTATE = "FOLLOW" @@ -67,7 +88,7 @@ function follow() { force=(0.05)*(tab.range)^4 attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing)) }) - uav_moveto(attractor.x, attractor.y) + uav_moveto(attractor.x, attractor.y, 0.0) } else { log("No target in local table!") #statef=idle @@ -90,11 +111,8 @@ function uav_rccmd() { neighbors.broadcast("cmd", 21) } else if(flight.rc_cmd==16) { flight.rc_cmd=0 - UAVSTATE = "FOLLOW" - log(rc_goto.latitude, " ", rc_goto.longitude) - add_targetrb(0,rc_goto.latitude,rc_goto.longitude) - statef = follow - #uav_goto() + UAVSTATE = "GOTO" + statef = goto } else if(flight.rc_cmd==400) { flight.rc_cmd=0 uav_arm() @@ -114,13 +132,38 @@ function uav_neicmd() { function(vid, value, rid) { print("Got (", vid, ",", value, ") from robot #", rid) if(value==22 and UAVSTATE!="TAKEOFF") { - statef=takeoff + statef=takeoff } else if(value==21) { - statef=land + statef=land } else if(value==400 and UAVSTATE=="IDLE") { - uav_arm() + uav_arm() } else if(value==401 and UAVSTATE=="IDLE"){ - uav_disarm() + uav_disarm() + } else if(value==16){ + neighbors.listen("gt",function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + # uav_storegoal(lat, lon, alt) + }) + statef=goto } }) +} + +function LimitAngle(angle){ + if(angle>2*math.pi) + return angle-2*math.pi + else if (angle<0) + return angle+2*math.pi + else + return angle +} + +function rb_from_gps(lat, lon) { + print("rb dbg: ",lat,lon,position.latitude,position.longitude) + d_lon = lon - position.longitude + d_lat = lat - position.latitude + ned_x = d_lat/180*math.pi * 6371000.0; + ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi); + goal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); + goal.bearing = LimitAngle(math.atan(ned_y,ned_x)); } \ No newline at end of file diff --git a/buzz_scripts/testalone.bzz b/buzz_scripts/testalone.bzz index 17b66a1..e607d4e 100644 --- a/buzz_scripts/testalone.bzz +++ b/buzz_scripts/testalone.bzz @@ -7,17 +7,22 @@ include "vstigenv.bzz" function action() { statef=action # test moveto cmd dx, dy -# uav_moveto(0.5, 0.5) +# uav_moveto(0.5, 0.5, 0.0) } # Executed once at init time. function init() { + uav_initswarm() + uav_initstig() } # Executed at each time step. function step() { - log("Altitude: ", position.altitude) uav_rccmd() + + statef() + + log("Current state: ", UAVSTATE) } # Executed once when the robot (or the simulator) is reset. diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index d1f7619..4723269 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -36,7 +36,7 @@ int buzzros_print(buzzvm_t vm); * commands the UAV to go to a position supplied */ int buzzuav_moveto(buzzvm_t vm); -int buzzuav_goto(buzzvm_t vm); +int buzzuav_storegoal(buzzvm_t vm); /* Returns the current command from local variable*/ int getcmd(); /*Sets goto position */ diff --git a/launch/husky.launch b/launch/husky.launch deleted file mode 100644 index bb883c5..0000000 --- a/launch/husky.launch +++ /dev/null @@ -1,59 +0,0 @@ - - - - - - - - - - - - - - - - magnetic_declination_radians: 0 - roll_offset: 0 - pitch_offset: 0 - yaw_offset: 0 - zero_altitude: false - broadcast_utm_transform: false - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/launch_config/topics.yaml b/launch/launch_config/topics.yaml new file mode 100644 index 0000000..8f1295b --- /dev/null +++ b/launch/launch_config/topics.yaml @@ -0,0 +1,12 @@ +topics: + gps : global_position/global + battery : battery + status : state + estatus : extended_state + fcclient: cmd/command + setpoint: setpoint_position/local + armclient: cmd/arming + modeclient: set_mode + localpos: local_position/pose + stream: set_stream_rate + altitude: global_position/rel_alt diff --git a/launch/rosbuzz-solo.launch b/launch/rosbuzz-solo.launch deleted file mode 100644 index fce3b00..0000000 --- a/launch/rosbuzz-solo.launch +++ /dev/null @@ -1,57 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/rosbuzz-testing-sitl.launch b/launch/rosbuzz-testing-sitl.launch deleted file mode 100644 index c09d2c5..0000000 --- a/launch/rosbuzz-testing-sitl.launch +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/rosbuzz-testing.launch b/launch/rosbuzz-testing.launch deleted file mode 100644 index d955758..0000000 --- a/launch/rosbuzz-testing.launch +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch index d79e016..de7857d 100644 --- a/launch/rosbuzz.launch +++ b/launch/rosbuzz.launch @@ -1,14 +1,22 @@ - - + + + + - - - - - - - - - - + + + + + + + + + + + + + + + + diff --git a/launch/rosbuzz_2_parallel.launch b/launch/rosbuzz_2_parallel.launch deleted file mode 100644 index 5bc52f4..0000000 --- a/launch/rosbuzz_2_parallel.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/rosbuzzm100.launch b/launch/rosbuzzm100.launch deleted file mode 100644 index 69da82f..0000000 --- a/launch/rosbuzzm100.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 1d296c9..8b883e9 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -304,8 +304,8 @@ void in_message_process(){ buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm)); @@ -349,7 +349,7 @@ void in_message_process(){ buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index da8d9b4..95d2656 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -123,27 +123,27 @@ namespace buzzuav_closures{ / Buzz closure to move following a 2D vector /----------------------------------------*/ int buzzuav_moveto(buzzvm_t vm) { - buzzvm_lnum_assert(vm, 2); - buzzvm_lload(vm, 1); /* dx */ - buzzvm_lload(vm, 2); /* dy */ - //buzzvm_lload(vm, 3); /* Latitude */ - //buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); - buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); - buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); - float dy = buzzvm_stack_at(vm, 1)->f.value; - float dx = buzzvm_stack_at(vm, 2)->f.value; - double d = sqrt(dx*dx+dy*dy); //range - goto_pos[0]=dx; - goto_pos[1]=dy; - goto_pos[2]=height; - /*double b = atan2(dy,dx); //bearing - printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); - gps_from_rb(d, b, goto_pos); - cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/ - //printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); - //ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], goto_pos[1], goto_pos[2]); - buzz_cmd= COMMAND_MOVETO; // TO DO what should we use - return buzzvm_ret0(vm); + buzzvm_lnum_assert(vm, 3); + buzzvm_lload(vm, 1); /* dx */ + buzzvm_lload(vm, 2); /* dy */ + buzzvm_lload(vm, 3); /* dheight */ + buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + float dh = buzzvm_stack_at(vm, 1)->f.value; + float dy = buzzvm_stack_at(vm, 2)->f.value; + float dx = buzzvm_stack_at(vm, 3)->f.value; + goto_pos[0]=dx; + goto_pos[1]=dy; + goto_pos[2]=height+dh; + /*double b = atan2(dy,dx); //bearing + printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); + gps_from_rb(d, b, goto_pos); + cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/ + //printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); + //ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], goto_pos[1], goto_pos[2]); + buzz_cmd= COMMAND_MOVETO; // TO DO what should we use + return buzzvm_ret0(vm); } int buzzuav_update_targets(buzzvm_t vm) { @@ -224,23 +224,23 @@ namespace buzzuav_closures{ int buzzuav_addNeiStatus(buzzvm_t vm){ buzzvm_lnum_assert(vm, 5); - buzzvm_lload(vm, 1); // fc - buzzvm_lload(vm, 2); // xbee - buzzvm_lload(vm, 3); // batt - buzzvm_lload(vm, 4); // gps - buzzvm_lload(vm, 5); // id - buzzvm_type_assert(vm, 5, BUZZTYPE_INT); - buzzvm_type_assert(vm, 4, BUZZTYPE_INT); - buzzvm_type_assert(vm, 3, BUZZTYPE_INT); - buzzvm_type_assert(vm, 2, BUZZTYPE_INT); - buzzvm_type_assert(vm, 1, BUZZTYPE_INT); + buzzvm_lload(vm, 1); // fc + buzzvm_lload(vm, 2); // xbee + buzzvm_lload(vm, 3); // batt + buzzvm_lload(vm, 4); // gps + buzzvm_lload(vm, 5); // id + buzzvm_type_assert(vm, 5, BUZZTYPE_INT); + buzzvm_type_assert(vm, 4, BUZZTYPE_INT); + buzzvm_type_assert(vm, 3, BUZZTYPE_INT); + buzzvm_type_assert(vm, 2, BUZZTYPE_INT); + buzzvm_type_assert(vm, 1, BUZZTYPE_INT); buzz_utility::neighbors_status newRS; uint8_t id = buzzvm_stack_at(vm, 5)->i.value; - newRS.gps_strenght= buzzvm_stack_at(vm, 4)->i.value; - newRS.batt_lvl= buzzvm_stack_at(vm, 3)->i.value; - newRS.xbee= buzzvm_stack_at(vm, 2)->i.value; - newRS.flight_status= buzzvm_stack_at(vm, 1)->i.value; - map< int, buzz_utility::neighbors_status >::iterator it = neighbors_status_map.find(id); + newRS.gps_strenght= buzzvm_stack_at(vm, 4)->i.value; + newRS.batt_lvl= buzzvm_stack_at(vm, 3)->i.value; + newRS.xbee= buzzvm_stack_at(vm, 2)->i.value; + newRS.flight_status= buzzvm_stack_at(vm, 1)->i.value; + map< int, buzz_utility::neighbors_status >::iterator it = neighbors_status_map.find(id); if(it!=neighbors_status_map.end()) neighbors_status_map.erase(it); neighbors_status_map.insert(make_pair(id, newRS)); @@ -265,14 +265,17 @@ namespace buzzuav_closures{ return payload_out; } /*----------------------------------------/ - / Buzz closure to go directly to a GPS destination from the Mission Planner + / Buzz closure to store locally a GPS destination from the fleet /----------------------------------------*/ - int buzzuav_goto(buzzvm_t vm) { - rc_goto_pos[2]=height; - set_goto(rc_goto_pos); - cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT; - printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]); - buzz_cmd=COMMAND_GOTO; + int buzzuav_storegoal(buzzvm_t vm) { + buzzvm_lnum_assert(vm, 3); + buzzvm_lload(vm, 1); // latitude + buzzvm_lload(vm, 2); // longitude + buzzvm_lload(vm, 3); // altitude + buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + rc_set_goto(buzzvm_stack_at(vm, 1)->f.value, buzzvm_stack_at(vm, 2)->f.value, buzzvm_stack_at(vm, 3)->f.value, (int)buzz_utility::get_robotid()); return buzzvm_ret0(vm); } @@ -346,7 +349,7 @@ namespace buzzuav_closures{ return cmd; } - void rc_set_goto(int id, double longitude, double latitude, double altitude) { + void rc_set_goto(int id, double latitude, double longitude, double altitude) { rc_id = id; rc_goto_pos[0] = latitude; rc_goto_pos[1] = longitude; @@ -529,6 +532,10 @@ namespace buzzuav_closures{ buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1)); buzzvm_pusht(vm); buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "id", 1)); + buzzvm_pushi(vm, rc_id); + buzzvm_tput(vm); + buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); buzzvm_pushf(vm, rc_goto_pos[0]); buzzvm_tput(vm); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 3a19b05..aa04afa 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -266,7 +266,8 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c) /*Obtain .bzz file name from parameter server*/ if (n_c.getParam("bzzfile_name", bzzfile_name)) ; - else { + else + { ROS_ERROR("Provide a .bzz file to run in Launch file"); system("rosnode kill rosbuzz_node"); } @@ -284,13 +285,11 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c) { ROS_INFO("RC service is disabled"); } - } else { + } else + { ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node"); } - /*Obtain robot_id from parameter server*/ - // n_c.getParam("robot_id", robot_id); - // robot_id=(int)buzz_utility::get_robotid(); /*Obtain out payload name*/ n_c.getParam("out_payload", out_payload); /*Obtain in payload name*/ @@ -300,22 +299,22 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c) if (n_c.getParam("xbee_plugged", xbeeplugged)) ; - else { + else + { ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node"); } if (!xbeeplugged) { if (n_c.getParam("name", robot_name)) ; - else { + else + { ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node"); } } else n_c.getParam("xbee_status_srv", xbeesrv_name); - std::cout << "////////////////// " << xbeesrv_name; - GetSubscriptionParameters(n_c); // initialize topics to null? } @@ -325,70 +324,71 @@ used /-----------------------------------------------------------------------------------*/ void roscontroller::GetSubscriptionParameters(ros::NodeHandle &node_handle) { - // todo: make it as an array in yaml? m_sMySubscriptions.clear(); std::string gps_topic, gps_type; if (node_handle.getParam("topics/gps", gps_topic)) ; - else { + else + { ROS_ERROR("Provide a gps topic in Launch file"); system("rosnode kill rosbuzz_node"); } - node_handle.getParam("type/gps", gps_type); - m_smTopic_infos.insert(pair(gps_topic, gps_type)); + m_smTopic_infos.insert(pair(gps_topic, "sensor_msgs/NavSatFix")); - std::string battery_topic, battery_type; + std::string battery_topic; node_handle.getParam("topics/battery", battery_topic); - node_handle.getParam("type/battery", battery_type); - m_smTopic_infos.insert( - pair(battery_topic, battery_type)); + m_smTopic_infos.insert(pair(battery_topic, "mavros_msgs/BatteryState")); - std::string status_topic, status_type; + std::string status_topic; node_handle.getParam("topics/status", status_topic); - node_handle.getParam("type/status", status_type); - m_smTopic_infos.insert( - pair(status_topic, status_type)); + m_smTopic_infos.insert(pair(status_topic, "mavros_msgs/State")); + node_handle.getParam("topics/estatus", status_topic); + m_smTopic_infos.insert(pair(status_topic, "mavros_msgs/ExtendedState")); - std::string altitude_topic, altitude_type; + std::string altitude_topic; node_handle.getParam("topics/altitude", altitude_topic); - node_handle.getParam("type/altitude", altitude_type); - m_smTopic_infos.insert( - pair(altitude_topic, altitude_type)); + m_smTopic_infos.insert(pair(altitude_topic, "std_msgs/Float64")); - /*Obtain fc client name from parameter server*/ + // Obtain required topic and service names from the parameter server if (node_handle.getParam("topics/fcclient", fcclient_name)) ; - else { + else + { ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node"); } if (node_handle.getParam("topics/setpoint", setpoint_name)) ; - else { + else + { ROS_ERROR("Provide a Set Point name in Launch file"); system("rosnode kill rosbuzz_node"); } if (node_handle.getParam("topics/armclient", armclient)) ; - else { + else + { ROS_ERROR("Provide an arm client name in Launch file"); system("rosnode kill rosbuzz_node"); } if (node_handle.getParam("topics/modeclient", modeclient)) ; - else { + else + { ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node"); } if (node_handle.getParam("topics/stream", stream_client_name)) ; - else { + else + { ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node"); } if (node_handle.getParam("topics/localpos", local_pos_sub_name)) ; - else { + else + { ROS_ERROR("Provide a localpos name in YAML file"); system("rosnode kill rosbuzz_node"); } @@ -405,33 +405,28 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c) Subscribe(n_c); - payload_sub = - n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this); + payload_sub = n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this); - obstacle_sub = n_c.subscribe(obstacles_topic, 5, - &roscontroller::obstacle_dist, this); + obstacle_sub = n_c.subscribe(obstacles_topic, 5, &roscontroller::obstacle_dist, this); /*publishers*/ payload_pub = n_c.advertise(out_payload, 5); MPpayload_pub = n_c.advertise("fleet_status", 5); neigh_pos_pub = n_c.advertise("neighbours_pos", 5); - localsetpoint_nonraw_pub = - n_c.advertise(setpoint_name, 5); + localsetpoint_nonraw_pub = n_c.advertise(setpoint_name, 5); + /* Service Clients*/ arm_client = n_c.serviceClient(armclient); mode_client = n_c.serviceClient(modeclient); mav_client = n_c.serviceClient(fcclient_name); if (rcclient == true) - service = - n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this); + service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this); ROS_INFO("Ready to receive Mav Commands from RC client"); xbeestatus_srv = n_c.serviceClient(xbeesrv_name); - stream_client = - n_c.serviceClient(stream_client_name); + stream_client = n_c.serviceClient(stream_client_name); users_sub = n_c.subscribe("users_pos", 5, &roscontroller::users_pos, this); - local_pos_sub = n_c.subscribe(local_pos_sub_name, 5, - &roscontroller::local_pos_callback, this); + local_pos_sub = n_c.subscribe(local_pos_sub_name, 5, &roscontroller::local_pos_callback, this); multi_msg = true; } @@ -444,20 +439,15 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c) m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it) { if (it->second == "mavros_msgs/ExtendedState") { - flight_status_sub = n_c.subscribe( - it->first, 100, &roscontroller::flight_extended_status_update, this); + flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_extended_status_update, this); } else if (it->second == "mavros_msgs/State") { - flight_status_sub = n_c.subscribe( - it->first, 100, &roscontroller::flight_status_update, this); + flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_status_update, this); } else if (it->second == "mavros_msgs/BatteryStatus") { - battery_sub = - n_c.subscribe(it->first, 5, &roscontroller::battery, this); + battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this); } else if (it->second == "sensor_msgs/NavSatFix") { - current_position_sub = - n_c.subscribe(it->first, 5, &roscontroller::current_pos, this); + current_position_sub = n_c.subscribe(it->first, 5, &roscontroller::current_pos, this); } else if (it->second == "std_msgs/Float64") { - relative_altitude_sub = - n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this); + relative_altitude_sub = n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this); } std::cout << "Subscribed to: " << it->first << endl; @@ -1059,13 +1049,10 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) { nei_pos.longitude = neighbours_pos_payload[1]; nei_pos.altitude = neighbours_pos_payload[2]; double cvt_neighbours_pos_payload[3]; - // cout<<"Got" << neighbours_pos_payload[0] <<", " << - //neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl; gps_rb(nei_pos, cvt_neighbours_pos_payload); /*Extract robot id of the neighbour*/ uint16_t *out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3)); - ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], - cvt_neighbours_pos_payload[1]); + //ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); /*pass neighbour position to local maintaner*/ buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1], @@ -1089,60 +1076,60 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res) { int rc_cmd; switch (req.command) { - case mavros_msgs::CommandCode::NAV_TAKEOFF: - ROS_INFO("RC_call: TAKE OFF!!!!"); - rc_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF; - buzzuav_closures::rc_call(rc_cmd); - res.success = true; - break; - case mavros_msgs::CommandCode::NAV_LAND: - ROS_INFO("RC_Call: LAND!!!! sending land"); - rc_cmd = mavros_msgs::CommandCode::NAV_LAND; - buzzuav_closures::rc_call(rc_cmd); - res.success = true; - break; - case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM: - rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; - armstate = req.param1; - if (armstate) { - ROS_INFO("RC_Call: ARM!!!!"); + case mavros_msgs::CommandCode::NAV_TAKEOFF: + ROS_INFO("RC_call: TAKE OFF!!!!"); + rc_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF; buzzuav_closures::rc_call(rc_cmd); res.success = true; - } else { - ROS_INFO("RC_Call: DISARM!!!!"); - buzzuav_closures::rc_call(rc_cmd + 1); + break; + case mavros_msgs::CommandCode::NAV_LAND: + ROS_INFO("RC_Call: LAND!!!!"); + rc_cmd = mavros_msgs::CommandCode::NAV_LAND; + buzzuav_closures::rc_call(rc_cmd); res.success = true; - } - break; - case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH: - ROS_INFO("RC_Call: GO HOME!!!!"); - rc_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; - buzzuav_closures::rc_call(rc_cmd); - res.success = true; - break; - case mavros_msgs::CommandCode::NAV_WAYPOINT: - ROS_INFO("RC_Call: GO TO!!!! "); - buzzuav_closures::rc_set_goto(req.param1,req.param5,req.param6,req.param7); - rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT; - buzzuav_closures::rc_call(rc_cmd); - res.success = true; - break; - case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL: - ROS_INFO("RC_Call: Gimbal!!!! "); - buzzuav_closures::rc_set_gimbal(req.param1,req.param2,req.param3); - rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; - buzzuav_closures::rc_call(rc_cmd); - res.success = true; - break; - case CMD_REQUEST_UPDATE: - ROS_INFO("RC_Call: Update Fleet Status!!!!"); - rc_cmd = 666; - buzzuav_closures::rc_call(rc_cmd); - res.success = true; - break; - default: - res.success = false; - break; + break; + case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM: + rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; + armstate = req.param1; + if (armstate) { + ROS_INFO("RC_Call: ARM!!!!"); + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + } else { + ROS_INFO("RC_Call: DISARM!!!!"); + buzzuav_closures::rc_call(rc_cmd + 1); + res.success = true; + } + break; + case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH: + ROS_INFO("RC_Call: GO HOME!!!!"); + rc_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case mavros_msgs::CommandCode::NAV_WAYPOINT: + ROS_INFO("RC_Call: GO TO!!!! "); + buzzuav_closures::rc_set_goto(req.param1,req.param5,req.param6,req.param7); + rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL: + ROS_INFO("RC_Call: Gimbal!!!! "); + buzzuav_closures::rc_set_gimbal(req.param1,req.param2,req.param3); + rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case CMD_REQUEST_UPDATE: + //ROS_INFO("RC_Call: Update Fleet Status!!!!"); + rc_cmd = CMD_REQUEST_UPDATE; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + default: + res.success = false; + break; } return true; }