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

set stream rate

This commit is contained in:
isvogor 2017-03-29 14:57:31 -04:00
parent 91a398c02b
commit e0ac252f9d
2 changed files with 22 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/roscontroller.h
View File

@ -16,6 +16,7 @@
#include "mavros_msgs/WaypointPush.h"
#include "mavros_msgs/Waypoint.h"
#include "mavros_msgs/PositionTarget.h"
#include "mavros_msgs/StreamRate.h"
#include <sensor_msgs/LaserScan.h>
#include <rosbuzz/neigh_pos.h>
#include <sstream>
@ -166,6 +167,8 @@ private:
void fc_command_setup();
void SetLocalPosition(float x, float y, float z, float yaw);
void SetStreamRate(int id, int rate, int on_off);
};
}

22
src/roscontroller.cpp
View File

@ -16,10 +16,12 @@ namespace rosbzz_node{
Compile_bzz();
set_bzz_file(bzzfile_name.c_str());
/*Initialize variables*/
//
// Solo things
SetMode("LOITER", 0);
armstate = 0;
multi_msg = true;
// set stream rate
SetStreamRate(0, 10, 1);
}
/*---------------------
@ -356,7 +358,7 @@ namespace rosbzz_node{
break;
case mavros_msgs::CommandCode::NAV_TAKEOFF:
if(current_mode != "GUIDED")
SetMode("GUIDED", 0); // 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)
break;
}
@ -386,7 +388,7 @@ namespace rosbzz_node{
Arm();
} else if (tmp == 4) {
armstate = 0;
SetMode("LOITER", 0);
SetMode("LOITER", 0);
Arm();
} else if (tmp == 5) { /*Buzz call for moveto*/
/*prepare the goto publish message */
@ -677,4 +679,18 @@ namespace rosbzz_node{
}
}
void roscontroller::SetStreamRate(int id, int rate, int on_off){
mavros_msgs::StreamRate message;
message.request.stream_id = id;
message.request.message_rate = rate;
message.request.on_off = on_off;
if(mav_client.call(message)){
ROS_INFO("Set Mode Service call successful!");
} else {
ROS_INFO("Set Mode Service call failed!");
}
}
}