From a96b6abda55de759a81b92017f39266a11fd93ca Mon Sep 17 00:00:00 2001
From: vivek-shankar <robovivek@gmail.com>
Date: Sat, 1 Apr 2017 02:49:25 -0400
Subject: [PATCH] robot id service

---
 CMakeLists.txt                 |  4 ++--
 Resources/XBee_Config.xml      |  6 +++---
 include/CommunicationManager.h |  7 +++++--
 src/CommunicationManager.cpp   | 18 +++++++++++++-----
 src/XMLConfigParser.cpp        |  2 +-
 5 files changed, 24 insertions(+), 13 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index a4fb046..927d79a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -57,8 +57,8 @@ include_directories(
 add_executable(xbee_mav src/Xbee.cpp src/XBeeFrame.cpp src/SerialDevice.cpp  src/CommunicationManager.cpp src/PacketsHandler)
 target_link_libraries(xbee_mav ${catkin_LIBRARIES})
 
-#add_executable(config src/main.cpp src/XBeeModule.cpp src/XMLConfigParser.cpp)
-#target_link_libraries(config ${catkin_LIBRARIES})
+add_executable(config src/main.cpp src/XBeeModule.cpp src/XMLConfigParser.cpp)
+target_link_libraries(config ${catkin_LIBRARIES})
 
 #add_executable(test_controller src/TestController.cpp)
 #target_link_libraries(test_controller ${catkin_LIBRARIES})
diff --git a/Resources/XBee_Config.xml b/Resources/XBee_Config.xml
index d84901d..17284b1 100644
--- a/Resources/XBee_Config.xml
+++ b/Resources/XBee_Config.xml
@@ -2,13 +2,13 @@
 
 <XBeeConfig>
     <Settings>
-		<Parameter Command="CM" Description="Channel Mask">00FFFFFFFFFFFF7FFF</Parameter>
+		<Parameter Command="CM" Description="Channel Mask">00FFFFFFFFFFF7FFFF</Parameter>
 		<Parameter Command="HP" Description="Preamble ID">1</Parameter>
 		<Parameter Command="ID" Description="Network ID">5FFF</Parameter>
-		<Parameter Command="MT" Description="Broadcast Multi-Transmits">1</Parameter>
+		<Parameter Command="MT" Description="Broadcast Multi-Transmits">3</Parameter>
 		<Parameter Command="PL" Description="TX Power Level">4</Parameter>
 		<Parameter Command="RR" Description="Unicast Retries">A</Parameter>
-		<Parameter Command="CE" Description="Routing/Messaging Mode">2</Parameter>
+		<Parameter Command="CE" Description="Routing/Messaging Mode">0</Parameter>
 		<Parameter Command="BH" Description="Broadcast Hops">0</Parameter>
 		<Parameter Command="NH" Description="Network Hops">7</Parameter>
 		<Parameter Command="MR" Description="Mesh Unicast Retries">1</Parameter>
diff --git a/include/CommunicationManager.h b/include/CommunicationManager.h
index 8284344..2e128cc 100644
--- a/include/CommunicationManager.h
+++ b/include/CommunicationManager.h
@@ -15,7 +15,8 @@
 #include<mavros_msgs/CommandInt.h>
 #include<mavros_msgs/Mavlink.h>
 #include <ros/ros.h>
-
+#include <mavros_msgs/ParamGet.h>
+#include <mavros_msgs/ParamValue.h>
 #include"PacketsHandler.h"
 #include"SerialDevice.h"
 
@@ -86,6 +87,7 @@ private:
 	void Process_In_Fragments();
 	void Process_In_Packets();
 	void Process_Command_Responses();
+	bool Get_ID(mavros_msgs::ParamGet::Request& req, mavros_msgs::ParamGet::Response& res);
 
 	Mist::Xbee::SerialDevice serial_device_;
 	Mist::Xbee::PacketsHandler packets_handler_;
@@ -97,9 +99,10 @@ private:
 	ros::NodeHandle node_handle_;
 	ros::Subscriber mavlink_subscriber_;	
 	ros::Publisher mavlink_publisher_;
-	ros::Publisher Robot_Id_Publisher_;
+	//ros::Publisher Robot_Id_Publisher_;
 	ros::ServiceClient mav_dji_client_;
 	ros::ServiceServer mav_dji_server_;
+	ros::ServiceServer Robot_Id_;
 	std_msgs::UInt8 device_id_out;
 	std::shared_ptr<std::thread> service_thread_; // TO DO delete !?
 };
diff --git a/src/CommunicationManager.cpp b/src/CommunicationManager.cpp
index 0e27167..30bcc1d 100644
--- a/src/CommunicationManager.cpp
+++ b/src/CommunicationManager.cpp
@@ -143,7 +143,7 @@ void CommunicationManager::Run_In_Swarm_Mode()
 	std::string in_messages_topic;
 	bool success_1 = false;
 	bool success_2 = false;
-
+	Robot_Id_ = node_handle_.advertiseService("/Robot_ID_srv", &CommunicationManager::Get_ID, this);
 	if (node_handle_.getParam("Xbee_In_From_Buzz", out_messages_topic))
 	{
 		mavlink_subscriber_ = node_handle_.subscribe(out_messages_topic.c_str(), 1000,
@@ -157,9 +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();
+		//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
@@ -167,10 +167,11 @@ 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);
+			//Robot_Id_Publisher_.publish(device_id_out);
 			Process_In_Standard_Messages();
 			Process_In_Fragments();
 			Process_In_Acks_and_Pings();
@@ -544,6 +545,13 @@ void CommunicationManager::Process_Command_Responses()
 	}
 }
 
+bool CommunicationManager::Get_ID (mavros_msgs::ParamGet::Request& req, mavros_msgs::ParamGet::Response& res){
+mavros_msgs::ParamValue id;
+id.integer=packets_handler_.get_device_id();
+res.value= id;
+res.success=true;
+return true;
+}
 
 }
 
diff --git a/src/XMLConfigParser.cpp b/src/XMLConfigParser.cpp
index 9627889..aed02f8 100644
--- a/src/XMLConfigParser.cpp
+++ b/src/XMLConfigParser.cpp
@@ -24,7 +24,7 @@ XMLConfigParser::~XMLConfigParser()
 //*****************************************************************************
 bool XMLConfigParser::Load_Config()
 {
-	const std::string FILE_NAME = "/home/vivek/catkin_ws/src/xbee_ros_node/Resources/XBee_Config.xml";
+	const std::string FILE_NAME = "/home/vivek/catkin_ws/src/xbee/Resources/XBee_Config.xml";
 
 	if (Check_Config_File_Exists(FILE_NAME))
 	{