From b2ab2ea9ec5b0094b76db0c8cbe292578921c3ef Mon Sep 17 00:00:00 2001 From: mistSpiri_Valmiki <vivekshankarv@gmail.com> Date: Thu, 27 Sep 2018 12:18:43 -0400 Subject: [PATCH 1/7] fixes from spiri HITL --- buzz_scripts/include/act/states.bzz | 2 +- buzz_scripts/main.bzz | 4 ++-- include/roscontroller.h | 2 ++ src/roscontroller.cpp | 31 ++++++++++++++++++++++------- 4 files changed, 29 insertions(+), 10 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 78e9285..7b20410 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -77,7 +77,7 @@ function stop() { if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND neighbors.broadcast("cmd", 21) uav_land() - if(pose.position.altitude <= 0.3) { + if(pose.position.altitude <= 0.2) { BVMSTATE = "TURNEDOFF" #barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) #barrier_ready(21) diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index fe36fc3..427e61a 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -32,11 +32,11 @@ goal_list = { function init() { init_stig() init_swarm() - + # initGraph() TARGET_ALTITUDE = takeoff_heights[id] - + log("----------------------->alt",TARGET_ALTITUDE) # start the swarm command listener nei_cmd_listen() diff --git a/include/roscontroller.h b/include/roscontroller.h index e79c589..74f12ef 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -98,6 +98,7 @@ private: typedef struct POSE ros_pose; ros_pose target, home, cur_pos; + double* goto_pos; struct MsgData { @@ -141,6 +142,7 @@ private: uint8_t old_val = 0; bool debug = false; bool setmode = false; + bool BClpose = false; std::string bzzfile_name; std::string bcfname, dbgfname; std::string stand_by; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 23fc145..fa0ac58 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -39,6 +39,7 @@ logical_clock(ros::Time()), previous_step_time(ros::Time()) cur_pos.longitude = 0; cur_pos.latitude = 0; cur_pos.altitude = 0; + goto_pos = buzzuav_closures::getgoto(); // set stream rate - wait for the FC to be started SetStreamRate(0, 10, 1); @@ -162,6 +163,9 @@ void roscontroller::RosControllerRun() prepare_msg_and_publish(); // Call the flight controler service flight_controller_service_call(); + // Broadcast local position to FCU + if(BClpose) + SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); // Set ROBOTS variable (swarm size) get_number_of_robots(); buzz_utility::set_robot_var(no_of_robots); @@ -737,12 +741,13 @@ script /-------------------------------------------------------------------------------*/ { // flight controller client call if requested from Buzz - double* goto_pos; float* gimbal; switch (buzzuav_closures::bzz_cmd()) { case NAV_TAKEOFF: goto_pos = buzzuav_closures::getgoto(); + goto_pos[0] = cur_pos.x; + goto_pos[1] = cur_pos.y; cmd_srv.request.param5 = cur_pos.latitude; cmd_srv.request.param6 = cur_pos.longitude; cmd_srv.request.param7 = goto_pos[2]; @@ -756,15 +761,19 @@ script ros::Duration(0.5).sleep(); // Registering HOME POINT. home = cur_pos; + BClpose = true; } if (current_mode != "GUIDED" && setmode) SetMode("GUIDED", 2000); // added for compatibility with 3DR Solo - if (mav_client.call(cmd_srv)) + if(setmode) { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + if (mav_client.call(cmd_srv)) + { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } + else + ROS_ERROR("Failed to call service from flight controller"); } - else - ROS_ERROR("Failed to call service from flight controller"); break; case NAV_LAND: @@ -778,10 +787,11 @@ script armstate = 0; Arm(); } - else if(cur_pos.altitude < 0.3) //disarm only when close to ground + else if(cur_pos.altitude < 0.4) //disarm only when close to ground { armstate = 0; Arm(); + BClpose = false; } if (mav_client.call(cmd_srv)) { @@ -829,7 +839,6 @@ script case NAV_SPLINE_WAYPOINT: goto_pos = buzzuav_closures::getgoto(); - roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); break; case DO_MOUNT_CONTROL: @@ -1005,6 +1014,14 @@ void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPt cur_pos.x = msg->pose.position.x; cur_pos.y = msg->pose.position.y; + if(!BClpose) + { + goto_pos[0]=cur_pos.x; + goto_pos[1]=cur_pos.y; + goto_pos[2]=0.0; + BClpose = true; + } + buzzuav_closures::set_currentNEDpos(msg->pose.position.y,msg->pose.position.x); // cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead tf::Quaternion q( From b38b9df043b14e243fb71540420ff8efe38615f2 Mon Sep 17 00:00:00 2001 From: David St-Onge <david.st-onge@polymtl.ca> Date: Thu, 27 Sep 2018 12:30:46 -0400 Subject: [PATCH 2/7] more detailed log far WP --- buzz_scripts/include/act/states.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 7b20410..edce3b4 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -105,7 +105,7 @@ function goto_gps(transf) { m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.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.") + 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 { From b7d3a0c3ac613f43310d4a01da7bf57b1914ee07 Mon Sep 17 00:00:00 2001 From: mistSpiri_Valmiki <vivekshankarv@gmail.com> Date: Thu, 11 Feb 2016 11:40:13 -0500 Subject: [PATCH 3/7] minor fixes for the spiri --- buzz_scripts/include/act/states.bzz | 4 ++-- buzz_scripts/main.bzz | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index edce3b4..9b137d3 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -12,7 +12,7 @@ TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 0.85 # m/steps +GOTO_MAXVEL = 1.5 # m/steps GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 0.4 # m. GOTOANG_TOL = 0.1 # rad. @@ -35,7 +35,7 @@ function cusfun(){ log("cusfun: yay!!!") LOCAL_TARGET = math.vec2.new(5 ,0 ) local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location - if(math.vec2.length(local_set_point) > GOTODIST_TOL){ + if(math.vec2.length(local_set_point) > GOTODIST_TOL){ local_set_point = LimitSpeed(local_set_point, 1.0) goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0) } diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 427e61a..ca87ef9 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -9,6 +9,7 @@ include "vstigenv.bzz" include "utils/takeoff_heights.bzz" #State launched after takeoff + AUTO_LAUNCH_STATE = "TASK_ALLOCATE" TARGET = 9.0 EPSILON = 30.0 From 0f54378b269e116bc9c04fd6ec45c505691392ff Mon Sep 17 00:00:00 2001 From: mistSpiri_Valmiki <vivekshankarv@gmail.com> Date: Thu, 11 Feb 2016 11:32:52 -0500 Subject: [PATCH 4/7] changes made on the field --- buzz_scripts/main.bzz | 3 ++- src/roscontroller.cpp | 9 +++++---- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index ca87ef9..7791ccc 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -10,7 +10,8 @@ include "utils/takeoff_heights.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "TASK_ALLOCATE" +#AUTO_LAUNCH_STATE = "TASK_ALLOCATE" +AUTO_LAUNCH_STATE = "IDLE" TARGET = 9.0 EPSILON = 30.0 ROOT_ID = 3 diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index fa0ac58..fbcc069 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -746,19 +746,20 @@ script { case NAV_TAKEOFF: goto_pos = buzzuav_closures::getgoto(); - goto_pos[0] = cur_pos.x; - goto_pos[1] = cur_pos.y; + goto_pos[0] = 0.0; + goto_pos[1] = 0.0; cmd_srv.request.param5 = cur_pos.latitude; cmd_srv.request.param6 = cur_pos.longitude; cmd_srv.request.param7 = goto_pos[2]; cmd_srv.request.command = buzzuav_closures::getcmd(); if (!armstate) { - if(setmode) + if(setmode){ SetMode("LOITER", 0); + ros::Duration(0.5).sleep(); + } armstate = 1; Arm(); - ros::Duration(0.5).sleep(); // Registering HOME POINT. home = cur_pos; BClpose = true; From 1da2084419dde143323c3bfbdd1fce3c3de5e1ef Mon Sep 17 00:00:00 2001 From: Rafael Braga <faerugb@gmail.com> Date: Thu, 27 Sep 2018 19:44:18 -0400 Subject: [PATCH 5/7] Revert "changes made on the field" This reverts commit 5654274acf8db38896cfafe41b10a592bc602284 --- buzz_scripts/main.bzz | 3 +-- src/roscontroller.cpp | 9 ++++----- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 7791ccc..ca87ef9 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -10,8 +10,7 @@ include "utils/takeoff_heights.bzz" #State launched after takeoff -#AUTO_LAUNCH_STATE = "TASK_ALLOCATE" -AUTO_LAUNCH_STATE = "IDLE" +AUTO_LAUNCH_STATE = "TASK_ALLOCATE" TARGET = 9.0 EPSILON = 30.0 ROOT_ID = 3 diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index fbcc069..fa0ac58 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -746,20 +746,19 @@ script { case NAV_TAKEOFF: goto_pos = buzzuav_closures::getgoto(); - goto_pos[0] = 0.0; - goto_pos[1] = 0.0; + goto_pos[0] = cur_pos.x; + goto_pos[1] = cur_pos.y; cmd_srv.request.param5 = cur_pos.latitude; cmd_srv.request.param6 = cur_pos.longitude; cmd_srv.request.param7 = goto_pos[2]; cmd_srv.request.command = buzzuav_closures::getcmd(); if (!armstate) { - if(setmode){ + if(setmode) SetMode("LOITER", 0); - ros::Duration(0.5).sleep(); - } armstate = 1; Arm(); + ros::Duration(0.5).sleep(); // Registering HOME POINT. home = cur_pos; BClpose = true; From 92f7a8d1e66ab432567b333d095fee91ebd36211 Mon Sep 17 00:00:00 2001 From: mistSpiri_Vachkiri <faerugb@gmail.com> Date: Thu, 27 Sep 2018 19:51:09 -0400 Subject: [PATCH 6/7] changes made on the field 27/09 --- buzz_scripts/main.bzz | 3 ++- src/roscontroller.cpp | 9 +++++---- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index ca87ef9..7791ccc 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -10,7 +10,8 @@ include "utils/takeoff_heights.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "TASK_ALLOCATE" +#AUTO_LAUNCH_STATE = "TASK_ALLOCATE" +AUTO_LAUNCH_STATE = "IDLE" TARGET = 9.0 EPSILON = 30.0 ROOT_ID = 3 diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index fa0ac58..fbcc069 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -746,19 +746,20 @@ script { case NAV_TAKEOFF: goto_pos = buzzuav_closures::getgoto(); - goto_pos[0] = cur_pos.x; - goto_pos[1] = cur_pos.y; + goto_pos[0] = 0.0; + goto_pos[1] = 0.0; cmd_srv.request.param5 = cur_pos.latitude; cmd_srv.request.param6 = cur_pos.longitude; cmd_srv.request.param7 = goto_pos[2]; cmd_srv.request.command = buzzuav_closures::getcmd(); if (!armstate) { - if(setmode) + if(setmode){ SetMode("LOITER", 0); + ros::Duration(0.5).sleep(); + } armstate = 1; Arm(); - ros::Duration(0.5).sleep(); // Registering HOME POINT. home = cur_pos; BClpose = true; From e5e7500b20770df29d594a7d07d1cd8e9f6ee6f9 Mon Sep 17 00:00:00 2001 From: David St-Onge <david.st-onge@polymtl.ca> Date: Thu, 27 Sep 2018 23:59:54 -0400 Subject: [PATCH 7/7] Revert debug stuff from field in main.bzz --- buzz_scripts/main.bzz | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 7791ccc..836bf81 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -10,8 +10,7 @@ include "utils/takeoff_heights.bzz" #State launched after takeoff -#AUTO_LAUNCH_STATE = "TASK_ALLOCATE" -AUTO_LAUNCH_STATE = "IDLE" +AUTO_LAUNCH_STATE = "TASK_ALLOCATE" TARGET = 9.0 EPSILON = 30.0 ROOT_ID = 3 @@ -38,7 +37,7 @@ function init() { # initGraph() TARGET_ALTITUDE = takeoff_heights[id] - log("----------------------->alt",TARGET_ALTITUDE) + # start the swarm command listener nei_cmd_listen()