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

set home only once

This commit is contained in:
isvogor 2017-04-27 12:36:12 -04:00
parent 187e05aca8
commit 31718bc01e
3 changed files with 25 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
launch/rosbuzz-testing-sitl.launch
View File

@ -33,6 +33,8 @@
<param name="in_payload" value="/inMavlink"/>
<param name="out_payload" value="/outMavlink"/>
<param name="xbee_status_srv" value="/xbee_status"/>
<param name="xbee_plugged" value="false"/>
<param name="name" value="solos1"/>
<param name="stand_by" value="/home/ivan/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
</node>

23
src/roscontroller.cpp
View File

@ -74,6 +74,7 @@ namespace rosbzz_node{
//////////////////////////////////////////////////////
while (ros::ok() && !buzz_utility::buzz_script_done())
{
std::cout<<std::setprecision(17)<<"Home: " << home[0] << home[1] << std::endl;
/*Update neighbors position inside Buzz*/
buzz_utility::neighbour_pos_callback(neighbours_pos_map);
/*Neighbours of the robot published with id in respective topic*/
@ -219,8 +220,8 @@ namespace rosbzz_node{
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
if(rcclient==true)
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this);
if(rcclient==true)
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<mavros_msgs::ParamGet>(xbeesrv_name);
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
@ -425,12 +426,19 @@ namespace rosbzz_node{
break;
case mavros_msgs::CommandCode::NAV_TAKEOFF:
if(!armstate){
SetMode("LOITER", 0);
armstate = 1;
Arm();
ros::Duration(0.5).sleep();
// Registering HOME POINT.
home[0] = cur_pos[0];home[1] = cur_pos[1];home[2] = cur_pos[2];
// Registering HOME POINT.
if(home[0] == 0){
//test #1: set home only once -- ok
home[0] = cur_pos[0]; home[1] = cur_pos[1]; home[2] = cur_pos[2];
//test #2: set home mavros -- nope
//SetMavHomePosition(cur_pos[0], cur_pos[1], cur_pos[2]);
}
}
if(current_mode != "GUIDED")
SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
@ -510,9 +518,9 @@ namespace rosbzz_node{
void roscontroller::set_cur_pos(double latitude,
double longitude,
double altitude){
cur_pos [0] =latitude;
cur_pos [1] =longitude;
cur_pos [2] =altitude;
cur_pos [0] = latitude;
cur_pos [1] = longitude;
cur_pos [2] = altitude;
}
/*-----------------------------------------------------------
@ -781,6 +789,7 @@ namespace rosbzz_node{
}
}
void roscontroller::SetStreamRate(int id, int rate, int on_off){
mavros_msgs::StreamRate message;
message.request.stream_id = id;

10
src/testflockfev.bzz
View File

@ -49,8 +49,12 @@ function hexagon() {
timeW =0
statef=land
} else {
timeW = timeW+1
uav_moveto(0.2,0.0)
if(timeW >= (WAIT_TIMEOUT / 2)){
uav_moveto(5.0,0.0)
} else {
uav_moveto(0.0,5.0)
}
timeW = timeW+1
}
}
@ -86,7 +90,7 @@ function barrier_ready() {
#
# Executes the barrier
#
WAIT_TIMEOUT = 200
WAIT_TIMEOUT = 300
timeW=0
function barrier_wait(threshold, transf) {
barrier.get(id)