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

set stream rate fix

Browse Source
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 armstate;
int barrier; int barrier;
int message_number=0; 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 rcclient;
bool multi_msg; bool multi_msg;
ros::ServiceClient mav_client; ros::ServiceClient mav_client;
@ -75,6 +75,7 @@ private:
ros::Subscriber flight_status_sub; ros::Subscriber flight_status_sub;
ros::Subscriber obstacle_sub; ros::Subscriber obstacle_sub;
ros::Subscriber Robot_id_sub; ros::Subscriber Robot_id_sub;
ros::ServiceClient stream_client;
/*Commands for flight controller*/ /*Commands for flight controller*/
//mavros_msgs::CommandInt cmd_srv; //mavros_msgs::CommandInt cmd_srv;

1
launch/launch_config/solo.yaml
View File

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

6
src/roscontroller.cpp
View File

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