extend status srv
This commit is contained in:
parent
a96b6abda5
commit
4234f895f8
|
@ -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 !?
|
||||||
};
|
};
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue