addition of Robot id publisher

include/CommunicationManager.h

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

include/PacketsHandler.h

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

src/CommunicationManager.cpp

@@ -157,6 +157,9 @@ void CommunicationManager::Run_In_Swarm_Mode()
 	{
 		mavlink_publisher_ = node_handle_.advertise<mavros_msgs::Mavlink>(
 			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;
 	}
 	else
@@ -165,9 +168,9 @@ void CommunicationManager::Run_In_Swarm_Mode()
 	if (success_1 && success_2)
 	{
 		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();

src/PacketsHandler.cpp

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