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

ensure compatibility with M100

This commit is contained in:
David St-Onge 2017-04-05 17:02:35 -04:00
parent 9f3a02e8ec
commit 0dbccdde44
4 changed files with 38 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/roscontroller.h
View File

@ -194,6 +194,7 @@ private:
void SetStreamRate(int id, int rate, int on_off); void SetStreamRate(int id, int rate, int on_off);
void get_number_of_robots(); void get_number_of_robots();
void GetRobotId();
}; };
} }

4
launch/launch_config/m100.yaml
View File

@ -5,7 +5,11 @@ topics:
fcclient : /dji_mavcmd fcclient : /dji_mavcmd
armclient: /dji_mavarm armclient: /dji_mavarm
modeclient: /dji_mavmode modeclient: /dji_mavmode
altitude: /rel_alt
stream: /set_stream_rate
type: type:
gps : sensor_msgs/NavSatFix gps : sensor_msgs/NavSatFix
battery : mavros_msgs/BatteryStatus battery : mavros_msgs/BatteryStatus
status : mavros_msgs/ExtendedState status : mavros_msgs/ExtendedState
altitude: std_msgs/Float64

44
src/roscontroller.cpp
View File

@ -20,8 +20,10 @@ namespace rosbzz_node{
SetMode("LOITER", 0); SetMode("LOITER", 0);
armstate = 0; armstate = 0;
multi_msg = true; multi_msg = true;
// set stream rate // set stream rate - wait for the FC to be started
SetStreamRate(0, 10, 1); SetStreamRate(0, 10, 1);
/// Get Robot Id - wait for Xbee to be started
GetRobotId();
} }
/*--------------------- /*---------------------
@ -36,26 +38,25 @@ namespace rosbzz_node{
uav_done(); uav_done();
} }
/*------------------------------------------------- void roscontroller::GetRobotId()
/rosbuzz_node loop method executed once every step {
/--------------------------------------------------*/
void roscontroller::RosControllerRun(){
mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id"; mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id";
mavros_msgs::ParamGet::Response robot_id_srv_response; mavros_msgs::ParamGet::Response robot_id_srv_response;
/*
while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){ while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){
ros::spinOnce(); ros::Duration(0.1).sleep();
ros::Rate loop_rate(10);
loop_rate.sleep();
ROS_ERROR("Waiting for Xbee to respond to get device ID"); ROS_ERROR("Waiting for Xbee to respond to get device ID");
} }
robot_id=robot_id_srv_response.value.integer; robot_id=robot_id_srv_response.value.integer;
*/
robot_id = 32;
//robot_id = 84; }
/*-------------------------------------------------
/rosbuzz_node loop method executed once every step
/--------------------------------------------------*/
void roscontroller::RosControllerRun(){
/* Set the Buzz bytecode */ /* Set the Buzz bytecode */
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) { if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
fprintf(stdout, "Bytecode file found and set\n"); fprintf(stdout, "Bytecode file found and set\n");
@ -69,7 +70,7 @@ namespace rosbzz_node{
/*Check updater state and step code*/ /*Check updater state and step code*/
update_routine(bcfname.c_str(), dbgfname.c_str()); update_routine(bcfname.c_str(), dbgfname.c_str());
/*Step buzz script */ /*Step buzz script */
buzz_utility::buzz_script_step(); buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/ /*Prepare messages and publish them in respective topic*/
prepare_msg_and_publish(); prepare_msg_and_publish();
/*call flight controler service to set command long*/ /*call flight controler service to set command long*/
@ -380,7 +381,6 @@ namespace rosbzz_node{
/*FC call for takeoff,land, gohome and arm/disarm*/ /*FC call for takeoff,land, gohome and arm/disarm*/
int tmp = buzzuav_closures::bzz_cmd(); int tmp = buzzuav_closures::bzz_cmd();
double* goto_pos = buzzuav_closures::getgoto(); double* goto_pos = buzzuav_closures::getgoto();
ros::Rate loop_rate(0.25);
if (tmp == buzzuav_closures::COMMAND_TAKEOFF || tmp== buzzuav_closures::COMMAND_LAND || tmp==buzzuav_closures::COMMAND_GOHOME) { if (tmp == buzzuav_closures::COMMAND_TAKEOFF || tmp== buzzuav_closures::COMMAND_LAND || tmp==buzzuav_closures::COMMAND_GOHOME) {
cmd_srv.request.param7 = goto_pos[2]; cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd(); cmd_srv.request.command = buzzuav_closures::getcmd();
@ -401,7 +401,7 @@ namespace rosbzz_node{
SetMode("LOITER", 0); SetMode("LOITER", 0);
armstate = 1; armstate = 1;
Arm(); Arm();
loop_rate.sleep(); ros::Duration(0.1).sleep();
} }
if(current_mode != "GUIDED") if(current_mode != "GUIDED")
SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm) SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
@ -526,9 +526,9 @@ namespace rosbzz_node{
// TODO: Handle landing // TODO: Handle landing
std::cout << "Message: " << msg->mode << std::endl; std::cout << "Message: " << msg->mode << std::endl;
if(msg->mode == "GUIDED") if(msg->mode == "GUIDED")
buzzuav_closures::flight_status_update(1); buzzuav_closures::flight_status_update(2);
else if (msg->mode == "LAND") else if (msg->mode == "LAND")
buzzuav_closures::flight_status_update(4); buzzuav_closures::flight_status_update(1);
else // ground standby = LOITER? else // ground standby = LOITER?
buzzuav_closures::flight_status_update(7);//? buzzuav_closures::flight_status_update(7);//?
} }
@ -543,7 +543,7 @@ namespace rosbzz_node{
/ Update current position into BVM from subscriber / Update current position into BVM from subscriber
/-------------------------------------------------------------*/ /-------------------------------------------------------------*/
void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){ void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
ROS_INFO("Altitude out: %f", cur_rel_altitude); //ROS_INFO("Altitude out: %f", cur_rel_altitude);
set_cur_pos(msg->latitude,msg->longitude, cur_rel_altitude);//msg->altitude); set_cur_pos(msg->latitude,msg->longitude, cur_rel_altitude);//msg->altitude);
buzzuav_closures::set_currentpos(msg->latitude,msg->longitude, cur_rel_altitude);//msg->altitude); buzzuav_closures::set_currentpos(msg->latitude,msg->longitude, cur_rel_altitude);//msg->altitude);
} }
@ -551,7 +551,7 @@ namespace rosbzz_node{
/ Update altitude into BVM from subscriber / Update altitude into BVM from subscriber
/-------------------------------------------------------------*/ /-------------------------------------------------------------*/
void roscontroller::current_rel_alt(const std_msgs::Float64::ConstPtr& msg){ void roscontroller::current_rel_alt(const std_msgs::Float64::ConstPtr& msg){
ROS_INFO("Altitude in: %f", msg->data); //ROS_INFO("Altitude in: %f", msg->data);
cur_rel_altitude = (double)msg->data; cur_rel_altitude = (double)msg->data;
} }
@ -811,7 +811,7 @@ namespace rosbzz_node{
while(!stream_client.call(message)){ while(!stream_client.call(message)){
ROS_INFO("Set stream rate call failed!, trying again..."); ROS_INFO("Set stream rate call failed!, trying again...");
std::this_thread::sleep_for( std::chrono::milliseconds ( 2000 ) ); ros::Duration(0.1).sleep();
} }
ROS_INFO("Set stream rate call successful"); ROS_INFO("Set stream rate call successful");
} }

21
src/testflockfev.bzz
View File

@ -43,14 +43,15 @@ function hexagon() {
math.vec2.scale(accum, 1.0 / neighbors.count()) math.vec2.scale(accum, 1.0 / neighbors.count())
# Move according to vector # Move according to vector
#print("Robot ", id, "must push ",accum.length, "; ", accum.angle) #print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
uav_moveto(accum.x,accum.y)
if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS # if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
timeW =0 # timeW =0
statef=land # statef=land
} else { # } else {
timeW = timeW+1 # timeW = timeW+1
uav_moveto(0.5,0.0) # uav_moveto(0.5,0.0)
} # }
} }
######################################## ########################################
@ -115,7 +116,7 @@ function takeoff() {
log("TakeOff: ", flight.status) log("TakeOff: ", flight.status)
log("Relative position: ", position.altitude) log("Relative position: ", position.altitude)
if( flight.status == 1 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon) barrier_set(ROBOTS,hexagon)
barrier_ready() barrier_ready()
#statef=hexagon #statef=hexagon
@ -130,7 +131,7 @@ function land() {
CURSTATE = "LAND" CURSTATE = "LAND"
statef=land statef=land
log("Land: ", flight.status) log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 1){ if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21) neighbors.broadcast("cmd", 21)
uav_land() uav_land()
} }
@ -200,4 +201,4 @@ function reset() {
# Executed once at the end of experiment. # Executed once at the end of experiment.
function destroy() { function destroy() {
} }