From db37ca34d1d1fa3811bf1d394f9444cefea8fa51 Mon Sep 17 00:00:00 2001
From: vivek-shankar <robovivek@gmail.com>
Date: Sun, 19 Feb 2017 23:31:42 -0500
Subject: [PATCH] addition of Robot id publisher

---
 include/CommunicationManager.h | 4 +++-
 include/PacketsHandler.h       | 3 ++-
 src/CommunicationManager.cpp   | 5 ++++-
 src/PacketsHandler.cpp         | 3 +++
 4 files changed, 12 insertions(+), 3 deletions(-)

diff --git a/include/CommunicationManager.h b/include/CommunicationManager.h
index 1bbf6bb..8284344 100644
--- a/include/CommunicationManager.h
+++ b/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 !?
 };
 
diff --git a/include/PacketsHandler.h b/include/PacketsHandler.h
index a450350..178cea9 100644
--- a/include/PacketsHandler.h
+++ b/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_;
diff --git a/src/CommunicationManager.cpp b/src/CommunicationManager.cpp
index bda82f4..1959db8 100644
--- a/src/CommunicationManager.cpp
+++ b/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();
diff --git a/src/PacketsHandler.cpp b/src/PacketsHandler.cpp
index a71eb24..bbbd91f 100644
--- a/src/PacketsHandler.cpp
+++ b/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_;
+}
 
 }