From 4234f895f8a42b05755497232fee32298a544b4b Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Sun, 2 Apr 2017 14:18:50 -0400 Subject: [PATCH] extend status srv --- include/CommunicationManager.h | 5 ++--- src/CommunicationManager.cpp | 20 +++++++++++--------- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/include/CommunicationManager.h b/include/CommunicationManager.h index 2e128cc..74dbfba 100644 --- a/include/CommunicationManager.h +++ b/include/CommunicationManager.h @@ -87,7 +87,7 @@ private: void Process_In_Fragments(); void Process_In_Packets(); void Process_Command_Responses(); - bool Get_ID(mavros_msgs::ParamGet::Request& req, mavros_msgs::ParamGet::Response& res); + bool Get_Param(mavros_msgs::ParamGet::Request& req, mavros_msgs::ParamGet::Response& res); Mist::Xbee::SerialDevice serial_device_; Mist::Xbee::PacketsHandler packets_handler_; @@ -99,10 +99,9 @@ private: ros::NodeHandle node_handle_; ros::Subscriber mavlink_subscriber_; ros::Publisher mavlink_publisher_; - //ros::Publisher Robot_Id_Publisher_; ros::ServiceClient mav_dji_client_; ros::ServiceServer mav_dji_server_; - ros::ServiceServer Robot_Id_; + ros::ServiceServer StatusSrv_; std_msgs::UInt8 device_id_out; std::shared_ptr service_thread_; // TO DO delete !? }; diff --git a/src/CommunicationManager.cpp b/src/CommunicationManager.cpp index 30bcc1d..3f1156d 100644 --- a/src/CommunicationManager.cpp +++ b/src/CommunicationManager.cpp @@ -143,7 +143,7 @@ void CommunicationManager::Run_In_Swarm_Mode() std::string in_messages_topic; bool success_1 = false; bool success_2 = false; - Robot_Id_ = node_handle_.advertiseService("/Robot_ID_srv", &CommunicationManager::Get_ID, this); + StatusSrv_ = node_handle_.advertiseService("/xbee_status", &CommunicationManager::Get_Param, this); if (node_handle_.getParam("Xbee_In_From_Buzz", out_messages_topic)) { mavlink_subscriber_ = node_handle_.subscribe(out_messages_topic.c_str(), 1000, @@ -157,9 +157,6 @@ void CommunicationManager::Run_In_Swarm_Mode() { mavlink_publisher_ = node_handle_.advertise( in_messages_topic.c_str(), 1000); - //Robot_Id_Publisher_= node_handle_.advertise( - // "/device_id_xbee_", 1000); - //device_id_out.data = packets_handler_.get_device_id(); success_2 = true; } else @@ -171,7 +168,6 @@ void CommunicationManager::Run_In_Swarm_Mode() ros::Rate loop_rate(LOOP_RATE); while (ros::ok()) { - //Robot_Id_Publisher_.publish(device_id_out); Process_In_Standard_Messages(); Process_In_Fragments(); Process_In_Acks_and_Pings(); @@ -545,10 +541,16 @@ void CommunicationManager::Process_Command_Responses() } } -bool CommunicationManager::Get_ID (mavros_msgs::ParamGet::Request& req, mavros_msgs::ParamGet::Response& res){ -mavros_msgs::ParamValue id; -id.integer=packets_handler_.get_device_id(); -res.value= id; +bool CommunicationManager::Get_Param (mavros_msgs::ParamGet::Request& req, mavros_msgs::ParamGet::Response& res){ +mavros_msgs::ParamValue val; +if(req.param_id=="id"){ + val.integer=packets_handler_.get_device_id(); +} else if(req.param_id=="rssi"){ + val.integer=-1; +} else if(req.param_id=="pl"){ + val.integer=-1; +} +res.value= val; res.success=true; return true; }