From 4dec1ca100675d22bc48896fb5da518a6eb3cd97 Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 19 Oct 2018 02:34:18 -0400 Subject: [PATCH] individual waypoint ctl, map-based waypoint in webctl in formation --- buzz_scripts/include/act/naviguation.bzz | 16 +++-- buzz_scripts/include/act/neighborcomm.bzz | 23 ++++++- buzz_scripts/include/act/states.bzz | 19 ++++-- .../include/utils/takeoff_heights.bzz | 3 +- include/buzzuav_closures.h | 4 ++ src/buzzuav_closures.cpp | 67 ++++++++++++++----- src/roscontroller.cpp | 11 +-- 7 files changed, 108 insertions(+), 35 deletions(-) diff --git a/buzz_scripts/include/act/naviguation.bzz b/buzz_scripts/include/act/naviguation.bzz index 300d3b6..7cd6879 100644 --- a/buzz_scripts/include/act/naviguation.bzz +++ b/buzz_scripts/include/act/naviguation.bzz @@ -1,15 +1,21 @@ +GOTO_MAXVEL = 1.5 # m/steps +GOTO_MAXDIST = 150 # m. +GOTODIST_TOL = 0.4 # m. +GOTOANG_TOL = 0.1 # rad. + # Core naviguation function to travel to a GPS target location. function goto_gps(transf) { - m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) + m_navigation = vec_from_gps(cur_goal.latitude, cur_goal.longitude, 0) #print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) if(math.vec2.length(m_navigation)>GOTO_MAXDIST) log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )") - else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination - transf() - else { + else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL){ # reached destination + if(transf!=nil) + transf() + } else { m_navigation = LimitSpeed(m_navigation, 1.0) #m_navigation = LCA(m_navigation) - goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) + goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0) } } diff --git a/buzz_scripts/include/act/neighborcomm.bzz b/buzz_scripts/include/act/neighborcomm.bzz index 5c3211d..1761039 100644 --- a/buzz_scripts/include/act/neighborcomm.bzz +++ b/buzz_scripts/include/act/neighborcomm.bzz @@ -18,9 +18,9 @@ function rc_cmd_listen() { barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20) barrier_ready(20) neighbors.broadcast("cmd", 20) - } else if(flight.rc_cmd==16) { - flight.rc_cmd=0 - BVMSTATE = "PATHPLAN" +# } else if(flight.rc_cmd==16) { +# flight.rc_cmd=0 +# BVMSTATE = "PATHPLAN" } else if(flight.rc_cmd==400) { flight.rc_cmd=0 arm() @@ -110,3 +110,20 @@ function nei_cmd_listen() { #} }) } + +# broadcast GPS goals +function bd_goal() { + neighbors.broadcast("goal", {.id=rc_goto.id, .la=rc_goto.latitude, .lo=rc_goto.longitude, .al=rc_goto.altitude}) +} + +# listens to neighbors broadcasting gps goals +function nei_goal_listen() { + neighbors.listen("goal", + function(vid, value, rid) { + print("Got (", vid, ",", value.id, value.la, value.lo, ") #", rid) + if(value.id==id) { + print("Got (", vid, ",", value, ") #", rid) + storegoal(value.la, value.lo, pose.position.altitude) + } + }) +} diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index dce6374..01fda7f 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -14,10 +14,6 @@ TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 1.5 # m/steps -GOTO_MAXDIST = 150 # m. -GOTODIST_TOL = 0.4 # m. -GOTOANG_TOL = 0.1 # rad. path_it = 0 pic_time = 0 g_it = 0 @@ -100,8 +96,23 @@ function stop() { } # State function for individual waypoint control +firsttimeinwp = 1 function indiWP() { + if(firsttimeinwp) { + nei_goal_listen() + storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude) + firsttimeinwp = 0 + } BVMSTATE = "INDIWP" + if(rc_goto.id != -1) { + log(rc_goto.id) + if(rc_goto.id == id) { + storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude) + } else + bd_goal() + } + + goto_gps(nil) } diff --git a/buzz_scripts/include/utils/takeoff_heights.bzz b/buzz_scripts/include/utils/takeoff_heights.bzz index 588afb4..81081aa 100644 --- a/buzz_scripts/include/utils/takeoff_heights.bzz +++ b/buzz_scripts/include/utils/takeoff_heights.bzz @@ -2,10 +2,11 @@ takeoff_heights ={ .1 = 21.0, .2 = 18.0, .3 = 12.0, - .5 = 24.0, + .5 = 12.0, .6 = 3.0, .9 = 15.0, .11 = 6.0, + .14 = 18.0, .16 = 9.0, .17 = 3.0, .18 = 6.0, diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 71b99f1..46ef816 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -50,6 +50,10 @@ int buzzuav_takepicture(buzzvm_t vm); * Returns the current command from local variable */ int getcmd(); +/* + * update GPS goal value + */ +void set_gpsgoal(double goal[3]); /* * Sets goto position from rc client */ diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 4c8b2a5..b76aa21 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -14,23 +14,29 @@ namespace buzzuav_closures // TODO: Minimize the required global variables and put them in the header // static const rosbzz_node::roscontroller* roscontroller_ptr; static double goto_pos[4]; -static double rc_goto_pos[3]; -static float rc_gimbal[4]; -static float batt[3]; -static float obst[5] = { 0, 0, 0, 0, 0 }; +static double goto_gpsgoal[3]; static double cur_pos[4]; static double cur_NEDpos[2]; -static uint8_t status; -static int cur_cmd = 0; -static int rc_cmd = 0; + static int rc_id = -1; +static int rc_cmd = 0; +static double rc_gpsgoal[3]; +static float rc_gimbal[4]; + +static float batt[3]; +static float obst[5] = { 0, 0, 0, 0, 0 }; +static uint8_t status; + +static int cur_cmd = 0; static int buzz_cmd = 0; static float height = 0; + static bool deque_full = false; static int rssi = 0; static float raw_packet_loss = 0.0; static int filtered_packet_loss = 0; static float api_rssi = 0.0; + string WPlistname = ""; std::map targets_map; @@ -231,7 +237,7 @@ int buzzuav_moveto(buzzvm_t vm) // DEBUG // 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 = NAV_SPLINE_WAYPOINT; // TODO: standard mavros? + buzz_cmd = NAV_SPLINE_WAYPOINT; return buzzvm_ret0(vm); } @@ -386,14 +392,24 @@ int buzzuav_storegoal(buzzvm_t vm) wplist_map.erase(wplist_map.begin()->first); } - double rb[3]; + set_gpsgoal(goal); + // prevent an overwrite + rc_id = -1; + return buzzvm_ret0(vm); +} + +void set_gpsgoal(double goal[3]) +/* +/ update GPS goal value +-----------------------------------*/ +{ + double rb[3]; rb_from_gps(goal, rb, cur_pos); if (fabs(rb[0]) < 150.0) { - rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]); - ROS_INFO("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); + goto_gpsgoal[0] = goal[0];goto_gpsgoal[1] = goal[1];goto_gpsgoal[2] = goal[2]; + ROS_INFO("Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); } - return buzzvm_ret0(vm); } int buzzuav_arm(buzzvm_t vm) @@ -504,9 +520,9 @@ 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; - rc_goto_pos[2] = altitude; + rc_gpsgoal[0] = latitude; + rc_gpsgoal[1] = longitude; + rc_gpsgoal[2] = altitude; } void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t) @@ -773,15 +789,30 @@ int buzzuav_update_flight_status(buzzvm_t vm) buzzvm_tput(vm); buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); - buzzvm_pushf(vm, rc_goto_pos[0]); + buzzvm_pushf(vm, rc_gpsgoal[0]); buzzvm_tput(vm); buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); - buzzvm_pushf(vm, rc_goto_pos[1]); + buzzvm_pushf(vm, rc_gpsgoal[1]); buzzvm_tput(vm); buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); - buzzvm_pushf(vm, rc_goto_pos[2]); + buzzvm_pushf(vm, rc_gpsgoal[2]); + buzzvm_tput(vm); + buzzvm_gstore(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "cur_goal", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); + buzzvm_pushf(vm, goto_gpsgoal[0]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); + buzzvm_pushf(vm, goto_gpsgoal[1]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); + buzzvm_pushf(vm, goto_gpsgoal[2]); buzzvm_tput(vm); buzzvm_gstore(vm); return vm->state; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index fbcc069..ca27a9d 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -762,7 +762,10 @@ script Arm(); // Registering HOME POINT. home = cur_pos; - BClpose = true; + // Initialize GPS goal for safety. + double gpspgoal[3] = {cur_pos.latitude,cur_pos.longitude,cur_pos.altitude}; + buzzuav_closures::set_gpsgoal(gpspgoal); + BClpose = true; } if (current_mode != "GUIDED" && setmode) SetMode("GUIDED", 2000); // added for compatibility with 3DR Solo @@ -792,7 +795,7 @@ script { armstate = 0; Arm(); - BClpose = false; + BClpose = false; } if (mav_client.call(cmd_srv)) { @@ -944,7 +947,7 @@ void roscontroller::gps_rb(POSE nei_pos, double out[]) void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, POSE t) /* -/ Get GPS from NED and a reference GPS point (struct input) +/ Get NED coordinates from a reference GPS point (struct input) ----------------------------------------------------------- */ { gps_convert_ned(ned_x, ned_y, t.longitude, t.latitude, cur_pos.longitude, cur_pos.latitude); @@ -953,7 +956,7 @@ void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, POSE t) void roscontroller::gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon, double gps_r_lat) /* -/ Get GPS from NED and a reference GPS point +/ Get NED coordinates from a reference GPS point and current GPS location ----------------------------------------------------------- */ { double d_lon = gps_t_lon - gps_r_lon;