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

set stream rate

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

20
src/roscontroller.cpp
View File

@ -16,10 +16,12 @@ namespace rosbzz_node{
Compile_bzz(); Compile_bzz();
set_bzz_file(bzzfile_name.c_str()); set_bzz_file(bzzfile_name.c_str());
/*Initialize variables*/ /*Initialize variables*/
// // Solo things
SetMode("LOITER", 0); SetMode("LOITER", 0);
armstate = 0; armstate = 0;
multi_msg = true; multi_msg = true;
// set stream rate
SetStreamRate(0, 10, 1);
} }
/*--------------------- /*---------------------
@ -356,7 +358,7 @@ namespace rosbzz_node{
break; break;
case mavros_msgs::CommandCode::NAV_TAKEOFF: case mavros_msgs::CommandCode::NAV_TAKEOFF:
if(current_mode != "GUIDED") 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; break;
} }
@ -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!");
}
}
} }