Spiri
/
XbeeMav
Archived
13
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

extend status srv

This commit is contained in:
David St-Onge 2017-04-02 14:18:50 -04:00
parent a96b6abda5
commit 4234f895f8
2 changed files with 13 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
include/CommunicationManager.h
View File

@ -87,7 +87,7 @@ private:
void Process_In_Fragments(); void Process_In_Fragments();
void Process_In_Packets(); void Process_In_Packets();
void Process_Command_Responses(); 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::SerialDevice serial_device_;
Mist::Xbee::PacketsHandler packets_handler_; Mist::Xbee::PacketsHandler packets_handler_;
@ -99,10 +99,9 @@ private:
ros::NodeHandle node_handle_; ros::NodeHandle node_handle_;
ros::Subscriber mavlink_subscriber_; ros::Subscriber mavlink_subscriber_;
ros::Publisher mavlink_publisher_; ros::Publisher mavlink_publisher_;
//ros::Publisher Robot_Id_Publisher_;
ros::ServiceClient mav_dji_client_; ros::ServiceClient mav_dji_client_;
ros::ServiceServer mav_dji_server_; ros::ServiceServer mav_dji_server_;
ros::ServiceServer Robot_Id_; ros::ServiceServer StatusSrv_;
std_msgs::UInt8 device_id_out; std_msgs::UInt8 device_id_out;
std::shared_ptr<std::thread> service_thread_; // TO DO delete !? std::shared_ptr<std::thread> service_thread_; // TO DO delete !?
}; };

20
src/CommunicationManager.cpp
View File

@ -143,7 +143,7 @@ void CommunicationManager::Run_In_Swarm_Mode()
std::string in_messages_topic; std::string in_messages_topic;
bool success_1 = false; bool success_1 = false;
bool success_2 = 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)) if (node_handle_.getParam("Xbee_In_From_Buzz", out_messages_topic))
{ {
mavlink_subscriber_ = node_handle_.subscribe(out_messages_topic.c_str(), 1000, 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<mavros_msgs::Mavlink>( mavlink_publisher_ = node_handle_.advertise<mavros_msgs::Mavlink>(
in_messages_topic.c_str(), 1000); in_messages_topic.c_str(), 1000);
//Robot_Id_Publisher_= node_handle_.advertise<std_msgs::UInt8>(
// "/device_id_xbee_", 1000);
//device_id_out.data = packets_handler_.get_device_id();
success_2 = true; success_2 = true;
} }
else else
@ -171,7 +168,6 @@ void CommunicationManager::Run_In_Swarm_Mode()
ros::Rate loop_rate(LOOP_RATE); ros::Rate loop_rate(LOOP_RATE);
while (ros::ok()) while (ros::ok())
{ {
//Robot_Id_Publisher_.publish(device_id_out);
Process_In_Standard_Messages(); Process_In_Standard_Messages();
Process_In_Fragments(); Process_In_Fragments();
Process_In_Acks_and_Pings(); 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){ bool CommunicationManager::Get_Param (mavros_msgs::ParamGet::Request& req, mavros_msgs::ParamGet::Response& res){
mavros_msgs::ParamValue id; mavros_msgs::ParamValue val;
id.integer=packets_handler_.get_device_id(); if(req.param_id=="id"){
res.value= 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; res.success=true;
return true; return true;
} }