diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 78e9285..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) } @@ -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) @@ -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 { diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index fe36fc3..836bf81 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 @@ -32,11 +33,11 @@ goal_list = { function init() { init_stig() init_swarm() - + # initGraph() TARGET_ALTITUDE = takeoff_heights[id] - + # 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..fbcc069 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,34 +741,40 @@ 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] = 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; } 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 +788,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 +840,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 +1015,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(