Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fixes from spiri HITL

This commit is contained in:
mistSpiri_Valmiki 2018-09-27 12:18:43 -04:00
parent 67e8d9b04b
commit b2ab2ea9ec
4 changed files with 29 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
buzz_scripts/include/act/states.bzz
View File

@ -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)

4
buzz_scripts/main.bzz
View File

@ -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()

2
include/roscontroller.h
View File

@ -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;

31
src/roscontroller.cpp
View File

@ -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(