Spiri
/
services-ros1-xbee_mav
12
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

addition of Robot id publisher

This commit is contained in:
vivek-shankar 2017-02-19 23:31:42 -05:00
parent 5907c34069
commit db37ca34d1
4 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
include/CommunicationManager.h
View File

@ -10,7 +10,7 @@
#include <inttypes.h> #include <inttypes.h>
#include<thread> #include<thread>
#include <std_msgs/UInt8.h>
#include<mavros_msgs/CommandCode.h> #include<mavros_msgs/CommandCode.h>
#include<mavros_msgs/CommandInt.h> #include<mavros_msgs/CommandInt.h>
#include<mavros_msgs/Mavlink.h> #include<mavros_msgs/Mavlink.h>
@ -97,8 +97,10 @@ 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_;
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 !?
}; };

3
include/PacketsHandler.h
View File

@ -57,7 +57,7 @@ public:
void Delete_Packets_With_Time_Out(); void Delete_Packets_With_Time_Out();
void Deserialize_Mavlink_Message(const char * bytes, void Deserialize_Mavlink_Message(const char * bytes,
mavros_msgs::Mavlink* mavlink_msg, const std::size_t msg_size); mavros_msgs::Mavlink* mavlink_msg, const std::size_t msg_size);
uint8_t get_device_id();
private: private:
@ -116,6 +116,7 @@ private:
const unsigned char frame_ID = const unsigned char frame_ID =
static_cast<unsigned char>(0x01)); static_cast<unsigned char>(0x01));
struct On_Line_Node_S struct On_Line_Node_S
{ {
bool received_hole_packet_; bool received_hole_packet_;

5
src/CommunicationManager.cpp
View File

@ -157,6 +157,9 @@ 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
@ -165,9 +168,9 @@ void CommunicationManager::Run_In_Swarm_Mode()
if (success_1 && success_2) if (success_1 && success_2)
{ {
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();

3
src/PacketsHandler.cpp
View File

@ -771,6 +771,9 @@ void PacketsHandler::Generate_AT_Command(const char* command,
Add_Length_and_Start_Delimiter(frame); Add_Length_and_Start_Delimiter(frame);
} }
uint8_t PacketsHandler::get_device_id(){
return device_address_;
}
} }