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

set stream rate fix

This commit is contained in:
isvogor 2017-03-29 17:32:56 -04:00
parent e0ac252f9d
commit a9335d973a
3 changed files with 7 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/roscontroller.h
View File

@ -61,7 +61,7 @@ private:
int armstate;
int barrier;
int message_number=0;
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by;
std::string bzzfile_name, fcclient_name, armclient, stream_client_name, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by;
bool rcclient;
bool multi_msg;
ros::ServiceClient mav_client;
@ -75,6 +75,7 @@ private:
ros::Subscriber flight_status_sub;
ros::Subscriber obstacle_sub;
ros::Subscriber Robot_id_sub;
ros::ServiceClient stream_client;
/*Commands for flight controller*/
//mavros_msgs::CommandInt cmd_srv;

1
launch/launch_config/solo.yaml
View File

@ -5,6 +5,7 @@ topics:
fcclient: /mavros/cmd/command
armclient: /mavros/cmd/arming
modeclient: /mavros/set_mode
stream: /mavros/set_stream_rate
type:
gps : sensor_msgs/NavSatFix
# for SITL Solo

6
src/roscontroller.cpp
View File

@ -133,7 +133,6 @@ namespace rosbzz_node{
node_handle.getParam("type/battery", battery_type);
m_smTopic_infos.insert(pair <std::string, std::string>(battery_topic, battery_type));
std::string status_topic, status_type;
node_handle.getParam("topics/status", status_topic);
node_handle.getParam("type/status", status_type);
@ -146,6 +145,8 @@ namespace rosbzz_node{
else {ROS_ERROR("Provide an arm client name in Launch file"); system("rosnode kill rosbuzz_node");}
if(node_handle.getParam("topics/modeclient", modeclient));
else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");}
if(node_handle.getParam("topics/stream", stream_client_name));
else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");}
}
/*--------------------------------------------------------
@ -170,6 +171,7 @@ 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);
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
multi_msg=true;
}
@ -684,7 +686,7 @@ namespace rosbzz_node{
message.request.stream_id = id;
message.request.message_rate = rate;
message.request.on_off = on_off;
if(mav_client.call(message)){
if(stream_client.call(message)){
ROS_INFO("Set Mode Service call successful!");
} else {
ROS_INFO("Set Mode Service call failed!");