merge with dev

CMakeLists.txt

@@ -11,6 +11,7 @@ set (BOOST_INCLUDEDIR "/usr/include")
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   std_msgs
+  mavros_msgs
 )
 
 ## System dependencies are found with CMake's conventions
@@ -37,14 +38,18 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
   INCLUDE_DIRS include
 #  LIBRARIES xbee_ros_node
-  CATKIN_DEPENDS roscpp std_msgs
+  CATKIN_DEPENDS roscpp std_msgs mavros_msgs
 #  DEPENDS system_lib
 )
 
 ###########
 ## Build ##
 ###########
-
+## Add definitions
+add_definitions(
+  -DDATABASE_PATH="${CMAKE_CURRENT_SOURCE_DIR}/Resources/database.xml"
+  -DXBEE_CONFIG_PATH="${CMAKE_CURRENT_SOURCE_DIR}/Resources/XBee_Config.xml"
+)
 
 ## Specify additional locations of header files
 include_directories(
@@ -53,19 +58,19 @@ include_directories(
 )
 
 
-add_executable(xbee_mav src/Xbee.cpp src/XBeeFrame.cpp src/SerialDevice.cpp  src/CommunicationManager.cpp)
+
+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)
+add_executable(xbee_config src/main.cpp src/XBeeModule.cpp src/XMLConfigParser.cpp)
-target_link_libraries(config ${catkin_LIBRARIES})
+target_link_libraries(xbee_config ${catkin_LIBRARIES})
 
 #add_executable(test_controller src/TestController.cpp)
 #target_link_libraries(test_controller ${catkin_LIBRARIES})
 
-#add_executable(test_buzz src/TestBuzz.cpp)
-#target_link_libraries(test_buzz ${catkin_LIBRARIES})
-
 
+add_executable(test_buzz src/TestBuzz.cpp)
+target_link_libraries(test_buzz ${catkin_LIBRARIES})
 
 #############
 ## Install ##
@@ -77,5 +82,3 @@ target_link_libraries(config ${catkin_LIBRARIES})
 ## Testing ##
 #############
 
-
-

Resources/XBee_Config.xml

@@ -2,7 +2,7 @@
 
 <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">3</Parameter>

Resources/database.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<Addresses>
+		<Device Address= "1"  >0013A20040D8CA1E</Device>
+		<Device Address= "2"  >0013A200415278B8</Device>
+		<Device Address= "3"  >0013A2004103B363</Device>
+		<Device Address= "4"  >0013A200415278AD</Device>
+		<Device Address= "5"  >0013A2004103B356</Device>
+		<Device Address= "6"  >0013A200415278B7</Device>
+		<Device Address= "7"  >0013A2004098A7A7</Device>
+		<Device Address= "8"  >0013A2004098A7BA</Device>
+		<Device Address= "9"  >0013A200415A9DDD</Device>
+		<Device Address= "10" >0013A200415A9DE4</Device>
+		<Device Address= "11" >0013A200415A9DDF</Device>
+		<Device Address= "12" >0013A200415A9DDE</Device>
+		<Device Address= "13" >0013A200415A9DE8</Device>
+
+</Addresses>

include/CommunicationManager.h

@@ -10,12 +10,14 @@
 
 #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>
 #include <ros/ros.h>
-
+#include <mavros_msgs/ParamGet.h>
+#include <mavros_msgs/ParamValue.h>
+#include"PacketsHandler.h"
 #include"SerialDevice.h"
 
 
@@ -58,7 +60,7 @@ private:
 
 	void Run_In_Solo_Mode(DRONE_TYPE drone_type);
 	void Run_In_Swarm_Mode();
-	void Generate_Transmit_Request_Frame(
+	/*void Generate_Transmit_Request_Frame(
 			const char* const message,
 			std::string* frame,
 			const unsigned char frame_ID = 
@@ -66,13 +68,13 @@ private:
 			const std::string& destination_adssress = "000000000000FFFF",
 			const std::string& short_destination_adress = "FFFF",
 			const std::string& broadcast_radius = "00",
-			const std::string& options = "00");
+			const std::string& options = "00");*/
-	void Check_In_Messages_and_Transfer_To_Topics();
+	//void Check_In_Messages_and_Transfer_To_Topics();
 	void Display_Init_Communication_Failure();
-	void Convert_HEX_To_Bytes(const std::string& HEX_data,
+	//void Convert_HEX_To_Bytes(const std::string& HEX_data,
-			std::string* converted_data);
+			//std::string* converted_data);
-	void Calculate_and_Append_Checksum(std::string* frame);
+	//void Calculate_and_Append_Checksum(std::string* frame);
-	void Add_Length_and_Start_Delimiter(std::string* frame);
+	//void Add_Length_and_Start_Delimiter(std::string* frame);
 	void Send_Mavlink_Message_Callback(
 			const mavros_msgs::Mavlink::ConstPtr& mavlink_msg);
 	void Display_Drone_Type_and_Running_Mode(DRONE_TYPE drone_type,
@@ -80,14 +82,28 @@ private:
 	bool Serve_Flight_Controller(mavros_msgs::CommandInt::
 		Request& request, mavros_msgs::CommandInt::Response& response);
 	void Check_In_Messages_and_Transfer_To_Server();
+	void Process_In_Standard_Messages();
+	void Process_In_Acks_and_Pings();
+	void Process_In_Fragments();
+	void Process_In_Packets();
+	void Process_Command_Responses();
+	bool Get_Param(mavros_msgs::ParamGet::Request& req, mavros_msgs::ParamGet::Response& res);
 
 	Mist::Xbee::SerialDevice serial_device_;
-	Thread_Safe_Deque* in_messages_;
+	Mist::Xbee::PacketsHandler packets_handler_;
+	Thread_Safe_Deque in_std_messages_;
+	Thread_Safe_Deque in_fragments_;
+	Thread_Safe_Deque in_Acks_and_Pings_;
+	Thread_Safe_Deque command_responses_;
+	Thread_Safe_Deque in_packets_;
 	ros::NodeHandle node_handle_;
 	ros::Subscriber mavlink_subscriber_;	
 	ros::Publisher mavlink_publisher_;
 	ros::ServiceClient mav_dji_client_;
 	ros::ServiceServer mav_dji_server_;
+	ros::ServiceServer StatusSrv_;
+	std_msgs::UInt8 device_id_out;
+	std::shared_ptr<std::thread> service_thread_; // TO DO delete !?
 };
 
 

include/MultithreadingDeque.hpp

@@ -36,7 +36,7 @@ public:
 
 
 	//****************************************************************************
-	void Push_Pack(const _T& new_data)
+	void Push_Back(const _T& new_data)
 	{
 		std::lock_guard<std::mutex> guard(mutex_);
 		deque_.push_back(new_data);

include/PacketsHandler.h

@@ -0,0 +1,153 @@
+/* PacketsHandler.h-- Packets Handler class for XBee:
+			     Serialize, deserialize, fragment and reassemly mavlink
+			     messages                             --                     */
+/* ------------------------------------------------------------------------- */
+/* February 06, 2017 -- @Copyright Aymen Soussia. All rights reserved.       */
+/*                                  (aymen.soussia@gmail.com)                */
+
+
+#pragma once
+
+#include<atomic>
+#include<inttypes.h>
+#include<mutex>
+#include<map>
+#include<set>
+#include<string>
+#include<unistd.h>
+#include<vector>
+
+#include<boost/property_tree/ptree.hpp>
+#include<boost/property_tree/xml_parser.hpp>
+#include<boost/foreach.hpp>
+#include<boost/filesystem.hpp>
+#include<mavros_msgs/Mavlink.h>
+
+#include"MultithreadingDeque.hpp"
+#include"SerialDevice.h"
+
+
+//*****************************************************************************
+using boost::property_tree::ptree;
+
+
+namespace Mist
+{
+
+
+namespace Xbee
+{
+
+
+//*****************************************************************************
+class PacketsHandler
+{
+public:
+	PacketsHandler();
+	~PacketsHandler();
+	
+	bool Init(SerialDevice* serial_device, Thread_Safe_Deque* in_packets);
+	bool Init_Device_ID();
+	void Run();
+	void Handle_Mavlink_Message(const mavros_msgs::Mavlink::ConstPtr& mavlink_msg);
+	void Process_Fragment(std::shared_ptr<std::string> fragment);
+	void Process_Ping_Or_Acknowledgement(std::shared_ptr<std::string> frame);
+	void Process_Command_Response(const char* command_response);
+	void Quit();
+	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:
+	const std::size_t MAX_PACEKT_SIZE; /* MAX packet size in bytes = 63750 bytes */
+	const std::size_t XBEE_NETWORK_MTU; /* Maximum Transmission Unit of Xbee netwrok = 256 bytes (max payload) - 6 bytes (Header size of each fragment) = 250 bytes */
+	const std::size_t FRAGMENT_HEADER_SIZE; /* Header size of each fragment = 6 bytes */
+	const std::clock_t MAX_TIME_TO_SEND_PACKET; /* Maximum time before dropping a packet = 30 seconds*/
+	const unsigned char START_DLIMITER;
+	
+	struct Reassembly_Packet_S
+	{
+		uint8_t packet_ID_;
+		std::string packet_buffer_; // TO DO make it shared ptr
+		std::set<uint8_t> received_fragments_IDs_;
+		std::clock_t time_since_creation_; // TO DO use it to delete packets with time out
+	};
+	
+	void Insert_Fragment_In_Packet_Buffer(std::string* buffer,
+			const char* fragment, const uint16_t offset, const std::size_t length);
+	void Add_New_Node_To_Network(const uint8_t new_node_address);
+	void Serialize_Mavlink_Message(const mavros_msgs::Mavlink::ConstPtr&
+		mavlink_msg, std::shared_ptr<std::string> serialized_packet);
+	void Insert_Fragment_Header(bool single_fragment,
+		std::shared_ptr<std::string> fragment, const uint8_t packet_ID,
+		const uint8_t fragment_ID, const uint16_t offset);
+	void Process_Out_Standard_Messages();
+	void Process_Out_Packets();
+	void Send_Packet(const Out_Packet_S& packet);
+	void Send_End_Of_Packet_Ping(const uint8_t packet_ID, const uint8_t total_NBR_of_fragments);
+	bool Load_Database_Addresses();
+	bool Get_Local_Address();
+	bool Check_Packet_Transmitted_To_All_Nodes();
+	void Init_Network_Nodes_For_New_Transmission(const uint8_t packet_ID,
+			std::vector<std::string>* frames,
+			std::shared_ptr<std::vector<std::shared_ptr<std::string>>> packet);
+	void Transmit_Fragments(const std::vector<std::string>& frames);
+	void Adjust_Optimum_MT_Number(const std::clock_t elapsed_time,
+		const std::size_t NBR_of_transmission);
+	void Send_SL_and_SH_Commands();
+	void Generate_Transmit_Request_Frame(
+			const char* message,
+			std::string* frame,
+			const std::size_t message_size,
+			const unsigned char frame_ID = 
+			static_cast<unsigned char>(0x01),
+			const std::string& destination_adssress = "000000000000FFFF",
+			const std::string& short_destination_adress = "FFFF",
+			const std::string& broadcast_radius = "00",
+			const std::string& options = "00");
+	void Convert_HEX_To_Bytes(const std::string& HEX_data,
+			std::string* converted_data);
+	void Calculate_and_Append_Checksum(std::string* frame);
+	void Add_Length_and_Start_Delimiter(std::string* frame);
+	void Generate_AT_Command(const char* command,
+			std::string* frame,
+			const unsigned char frame_ID = 
+			static_cast<unsigned char>(0x01));
+	
+	
+	struct On_Line_Node_S
+	{
+		bool received_hole_packet_;
+		std::string address_64_bits_;
+	};
+	
+	std::set<uint8_t> fragments_indexes_to_transmit_;
+	SerialDevice* serial_device_;
+	std::atomic<bool> quit_;
+	Thread_Safe_Deque out_std_messages_;
+	Thread_Safe_Deque_Of_Vectors out_packets_;
+	Thread_Safe_Deque* in_packets_;
+	std::map<uint8_t, bool> connected_network_nodes_;
+	std::map<uint8_t, bool>::iterator connected_network_nodes_it_;
+	std::map<uint8_t, Reassembly_Packet_S> packets_assembly_map_;
+	std::map<uint8_t, Reassembly_Packet_S>::iterator assembly_map_it_;
+	std::map<uint64_t, uint8_t> database_addresses_;
+	std::map<uint64_t, uint8_t>::iterator database_addresses_it_;
+	std::mutex mutex_;
+	uint8_t device_address_;
+	std::string device_64_bits_address_;
+	bool loaded_SL_;
+	bool loaded_SH_;
+	uint8_t current_processed_packet_ID_;
+	std::size_t optimum_MT_NBR_;
+	// TO DO & after auto !?
+	useconds_t delay_interframes_;
+};
+
+
+}
+
+
+}

include/SerialDevice.h

@@ -25,7 +25,14 @@ namespace Xbee
 {
 
 
+struct Out_Packet_S
+{
+		uint8_t packet_ID_;
+		std::shared_ptr<std::vector<std::shared_ptr<std::string>>> packet_buffer_;
+};
+
 typedef MultithreadingDeque<std::shared_ptr<std::string>> Thread_Safe_Deque;
+typedef MultithreadingDeque<Out_Packet_S> Thread_Safe_Deque_Of_Vectors;
 
 
 //*****************************************************************************
@@ -39,7 +46,13 @@ public:
 	void Send_Frame(const std::string& frame);
 	void Run_Service();
 	void Stop_Service();
-	Thread_Safe_Deque* Get_In_Messages_Pointer();
+	void Set_In_Messages_Pointers(Thread_Safe_Deque* in_std_messages,
+			Thread_Safe_Deque* in_fragments,
+			Thread_Safe_Deque* in_Acks_and_Pings,
+			Thread_Safe_Deque* command_responses);
+	bool Is_IO_Service_Stopped(); // TO DO delete this function
+	void Reset_IO_Service(); // TO DO delete this function
+	void Close_Serial_Port();
 
 private:
 
@@ -71,7 +84,10 @@ private:
 	boost::asio::io_service io_service_;
 	boost::asio::serial_port serial_port_;
 	std::deque<std::string> out_messages_;
-	Thread_Safe_Deque in_messages_;
+	Thread_Safe_Deque* in_std_messages_;
+	Thread_Safe_Deque* in_fragments_;
+	Thread_Safe_Deque* in_Acks_and_Pings_;
+	Thread_Safe_Deque* command_responses_;
 	Mist::Xbee::Frame current_frame_;
 	unsigned int FRAME_TYPE_KEYS[REMOTE_AT_COMMAND_RESPONSE + 1];
 };

include/XBeeFrame.h

@@ -27,7 +27,7 @@ public:
 	~Frame();
 
 	enum {FRAME_HEADER_LENGTH = 4};
-	enum {MAX_FRAME_BODY_LENGTH = 280}; // TO DO check value
+	enum {MAX_FRAME_BODY_LENGTH = 270}; // TO DO check value
 
 	const char* Get_Frame_Data() const;
 	char* Get_Frame_Data();
@@ -39,6 +39,7 @@ public:
 	std::size_t Get_Frame_Type() const;
 	int Get_Start_Delimiter_Position();
 	void Rearrange_Corrupted_Header();
+	char Get_Message_Type(); // TO DO const !?
 
 
 private:

launch/xbeemav.launch

@@ -1,12 +1,14 @@
 <launch>
 	<!--  xmee_mav Drone type and Commununication Mode -->
 	<!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="master solo" output="screen" -->
-	<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="master swarm" output="screen"/>
+	<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="master swarm" output="screen" />
-	<!--node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave solo" output="screen" -->
+	<!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave solo" output="screen" -->
 	<!--node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" -->
 	<!--  xmee_mav Topics and Services Names -->
 	<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
 	<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
 	<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
 	<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
+	<param name="USB_port" type="str" value="/dev/ttyUSB0" />
+	<param name="Baud_rate" type="double" value="230400" />
 </launch>

package.xml

@@ -14,6 +14,9 @@
   <run_depend>roscpp</run_depend>
   <run_depend>std_msgs</run_depend>
   
+  <build_depend>mavros_msgs</build_depend>
+  <run_depend>mavros_msgs</run_depend>
+
   <export>
   </export>
 </package>

src/CommunicationManager.cpp

@@ -37,14 +37,38 @@ bool CommunicationManager::Init(
 {
 	if (serial_device_.Init(device, baud_rate))
 	{
-		in_messages_ = serial_device_.Get_In_Messages_Pointer();
+		serial_device_.Set_In_Messages_Pointers(&in_std_messages_, &in_fragments_,
-		return true;
+		&in_Acks_and_Pings_, &command_responses_);
+		
+		service_thread_ = std::make_shared<std::thread>(std::thread(&SerialDevice::Run_Service, &serial_device_));
+		
+		if (!packets_handler_.Init(&serial_device_, &in_packets_))
+			return false;
+		
+		std::clock_t elapsed_time = std::clock();
+		bool device_ID_loaded = false;
+		
+		while (std::clock() - elapsed_time <= 300000)
+		{
+			Process_Command_Responses();
+			
+			if (packets_handler_.Init_Device_ID())
+			{
+				device_ID_loaded = true;
+				break;
+			}
+		}
+		
+		if (!device_ID_loaded)
+			return false;
 	}
 	else
 	{
 		Display_Init_Communication_Failure();
 		return false;
 	}
+	
+	return true;
 }
 
 
@@ -53,7 +77,7 @@ bool CommunicationManager::Init(
 void CommunicationManager::Run(DRONE_TYPE drone_type,
 		RUNNING_MODE running_mode)
 {
-	std::thread thread_service(&SerialDevice::Run_Service, &serial_device_);
+	std::thread thread_packets_handler(&PacketsHandler::Run, &packets_handler_);
 
 	Display_Drone_Type_and_Running_Mode(drone_type, running_mode);
 
@@ -63,7 +87,10 @@ void CommunicationManager::Run(DRONE_TYPE drone_type,
 		Run_In_Solo_Mode(drone_type);
 
 	serial_device_.Stop_Service();
-  	thread_service.join();
+	serial_device_.Close_Serial_Port();
+	packets_handler_.Quit();
+  	service_thread_->join();
+  	thread_packets_handler.join();
 }
 
 
@@ -116,7 +143,7 @@ void CommunicationManager::Run_In_Swarm_Mode()
 	std::string in_messages_topic;
 	bool success_1 = false;
 	bool success_2 = false;
-
+	StatusSrv_ = node_handle_.advertiseService("/xbee_status", &CommunicationManager::Get_Param, this);
 	if (node_handle_.getParam("Xbee_In_From_Buzz", out_messages_topic))
 	{
 		mavlink_subscriber_ = node_handle_.subscribe(out_messages_topic.c_str(), 1000,
@@ -139,10 +166,13 @@ void CommunicationManager::Run_In_Swarm_Mode()
 	{
 
 		ros::Rate loop_rate(LOOP_RATE);
-
 		while (ros::ok())
 		{
-			Check_In_Messages_and_Transfer_To_Topics();
+			Process_In_Standard_Messages();
+			Process_In_Fragments();
+			Process_In_Acks_and_Pings();
+			Process_In_Packets();
+			packets_handler_.Delete_Packets_With_Time_Out();
 			ros::spinOnce();
 	    	loop_rate.sleep();
 		}
@@ -154,12 +184,12 @@ void CommunicationManager::Run_In_Swarm_Mode()
 inline bool CommunicationManager::Serve_Flight_Controller(mavros_msgs::CommandInt::
 		Request& request, mavros_msgs::CommandInt::Response& response)
 {
-	const std::size_t MAX_BUFFER_SIZE = 255;
+	const std::size_t MAX_BUFFER_SIZE = 256;
 	unsigned int command = 0;
 	char temporary_buffer[MAX_BUFFER_SIZE];
 	std::string frame;
 
-	switch(request.command)
+	/*switch(request.command)
 	{
 		case mavros_msgs::CommandCode::NAV_TAKEOFF:
 		{
@@ -205,7 +235,7 @@ inline bool CommunicationManager::Serve_Flight_Controller(mavros_msgs::CommandIn
 	{
 		Generate_Transmit_Request_Frame(temporary_buffer, &frame);
 		serial_device_.Send_Frame(frame);
-	}
+	}*/
 	
 	return true;
 }
@@ -226,7 +256,7 @@ void CommunicationManager::Display_Init_Communication_Failure()
 
 
 //*****************************************************************************
-inline void CommunicationManager::Generate_Transmit_Request_Frame(
+/*inline void CommunicationManager::Generate_Transmit_Request_Frame(
 		const char* const message,
 		std::string * frame,
 		const unsigned char frame_ID,
@@ -236,7 +266,7 @@ inline void CommunicationManager::Generate_Transmit_Request_Frame(
 		const std::string& options)
 {
 	const unsigned char FAME_TYPE = static_cast<unsigned char>(0x10); /* Transmit Request Frame */
-	std::string frame_parameters;
+	/*std::string frame_parameters;
 	std::string bytes_frame_parameters;
 
 	frame_parameters.append(destination_adssress);
@@ -253,11 +283,11 @@ inline void CommunicationManager::Generate_Transmit_Request_Frame(
 
 	Calculate_and_Append_Checksum(frame);
 	Add_Length_and_Start_Delimiter(frame);
-}
+}*/
 
 
 //*****************************************************************************
-inline void CommunicationManager::Convert_HEX_To_Bytes(
+/*inline void CommunicationManager::Convert_HEX_To_Bytes(
 		const std::string& HEX_data, std::string* converted_data)
 {
 	const unsigned short TWO_BYTES = 2;
@@ -273,11 +303,11 @@ inline void CommunicationManager::Convert_HEX_To_Bytes(
 		temporary_char = static_cast<unsigned char>(temporary_HEX);
 		converted_data->push_back(temporary_char);
 	}
-}
+}*/
 
 
 //*****************************************************************************
-inline void CommunicationManager::Calculate_and_Append_Checksum(
+/*inline void CommunicationManager::Calculate_and_Append_Checksum(
 		std::string* frame)
 {
 	unsigned short bytes_sum = 0;
@@ -292,11 +322,11 @@ inline void CommunicationManager::Calculate_and_Append_Checksum(
 	checksum = 0xFF - lowest_8_bits;
 	checksum_byte = static_cast<unsigned char>(checksum);
 	frame->push_back(checksum_byte);
-}
+}*/
 
 
 //*****************************************************************************
-inline void CommunicationManager::Add_Length_and_Start_Delimiter(
+/*inline void CommunicationManager::Add_Length_and_Start_Delimiter(
 		std::string* frame)
 {
 	const unsigned short FIVE_BYTES = 5;
@@ -307,7 +337,7 @@ inline void CommunicationManager::Add_Length_and_Start_Delimiter(
 	std::string header;
 	int frame_length = frame->size() - 1; /* frame_length = frame_size - checksum byte */
 
-	header.push_back(START_DLIMITER);
+	/*header.push_back(START_DLIMITER);
 	sprintf(temporary_buffer, "%04X", frame_length);
 	sscanf(temporary_buffer, "%02X%02X", &Hex_frame_length_1, 
 			&Hex_frame_length_2);
@@ -316,11 +346,11 @@ inline void CommunicationManager::Add_Length_and_Start_Delimiter(
 	temporary_char = static_cast<unsigned char>(Hex_frame_length_2);
 	header.push_back(temporary_char);
 	frame->insert(0, header);
-}
+}*/
 
 
 //*****************************************************************************
-inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
+/*inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics() // TO DO delete
 {
 	std::size_t size_in_messages = in_messages_->Get_Size();
 	
@@ -349,15 +379,15 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
 		}
 		
 	}
-}
+}*/
 
 
 //*****************************************************************************
-inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server()
+inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server() // TO DO change it
 {
-	std::size_t size_in_messages = in_messages_->Get_Size();
+	//std::size_t size_in_messages = in_messages_->Get_Size();
 	
-	if (size_in_messages > 0)
+	/*if (size_in_messages > 0)
 	{
 		for (std::size_t j = 0; j < size_in_messages; j++)
 		{
@@ -389,7 +419,7 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server()
 				ROS_INFO("XBeeMav: Faild to Tranfer Command.");
 			
 		}
-	}
+	}*/
 }
 
 
@@ -397,26 +427,8 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server()
 inline void CommunicationManager::Send_Mavlink_Message_Callback(
 		const mavros_msgs::Mavlink::ConstPtr& mavlink_msg)
 {
-	const unsigned short MAX_BUFFER_SIZE = 211; /* 20 (length(uint64_t)) * 10 (max int number) + 10 (spaces) + 1 */
+	//std::cout << "Received Message From Buzz" << std::endl; // TO DO delete
-	const unsigned short MAX_NBR_OF_INT64 = 10;
+	packets_handler_.Handle_Mavlink_Message(mavlink_msg);
-	char temporary_buffer[MAX_BUFFER_SIZE];
-	std::string frame;
-	int converted_bytes = 0;
-
-	/* Check the payload is not too long. Otherwise ignore it. */
-	if (mavlink_msg->payload64.size() <= MAX_NBR_OF_INT64)
-	{
-		for (unsigned short i = 0; i < mavlink_msg->payload64.size(); i++)
-		{
-			converted_bytes += sprintf(
-				temporary_buffer + converted_bytes, "%" PRIu64 " ",
-				(uint64_t)mavlink_msg->payload64.at(i));
-		
-		}
-	
-		Generate_Transmit_Request_Frame(temporary_buffer, &frame);
-		serial_device_.Send_Frame(frame);
-	}
 }
 
 
@@ -436,6 +448,113 @@ void CommunicationManager::Display_Drone_Type_and_Running_Mode(
 }
 
 
+//*****************************************************************************
+void CommunicationManager::Process_In_Standard_Messages()
+{
+	std::size_t in_messages_size = in_std_messages_.Get_Size();
+	
+	if (in_messages_size > 0)
+	{
+		for (std::size_t j = 0; j < in_messages_size; j++)
+		{
+			std::shared_ptr<std::string> in_message =
+					in_std_messages_.Pop_Front();
+			mavros_msgs::Mavlink mavlink_msg;
+					
+			packets_handler_.Deserialize_Mavlink_Message(in_message->c_str(), &mavlink_msg, in_message->size());
+			mavlink_publisher_.publish(mavlink_msg);
+		}
+	}
+}
+
+
+//*****************************************************************************
+void CommunicationManager::Process_In_Acks_and_Pings()
+{
+	std::size_t in_messages_size = in_Acks_and_Pings_.Get_Size();
+	
+	if (in_messages_size > 0)
+	{
+		for (std::size_t j = 0; j < in_messages_size; j++)
+		{
+			std::shared_ptr<std::string> in_message =
+					in_Acks_and_Pings_.Pop_Front();
+					
+			packets_handler_.Process_Ping_Or_Acknowledgement(in_message);
+		}
+	}
+}
+
+
+//*****************************************************************************
+void CommunicationManager::Process_In_Fragments()
+{
+	std::size_t in_messages_size = in_fragments_.Get_Size();
+	
+	if (in_messages_size > 0)
+	{
+		for (std::size_t j = 0; j < in_messages_size; j++)
+		{
+			std::shared_ptr<std::string> in_message =
+					in_fragments_.Pop_Front();
+					
+			packets_handler_.Process_Fragment(in_message);
+		}
+	}
+}
+	
+	
+//*****************************************************************************
+void CommunicationManager::Process_In_Packets()
+{
+	std::size_t in_messages_size = in_packets_.Get_Size(); 
+	
+	if (in_messages_size > 0)
+	{
+		for (std::size_t j = 0; j < in_messages_size; j++)
+		{
+			std::shared_ptr<std::string> in_message =
+					in_packets_.Pop_Front(); 
+			mavros_msgs::Mavlink mavlink_msg;
+					
+			packets_handler_.Deserialize_Mavlink_Message(in_message->c_str(), &mavlink_msg, in_message->size());
+			mavlink_publisher_.publish(mavlink_msg);
+		}
+	}
+}
+
+
+//*****************************************************************************
+void CommunicationManager::Process_Command_Responses()
+{		
+	std::size_t in_messages_size = command_responses_.Get_Size();
+	
+	if (in_messages_size > 0)
+	{
+		for (std::size_t j = 0; j < in_messages_size; j++)
+		{
+			std::shared_ptr<std::string> in_message =
+					command_responses_.Pop_Front();
+					
+			packets_handler_.Process_Command_Response(in_message->c_str());
+		}
+	}
+}
+
+bool CommunicationManager::Get_Param (mavros_msgs::ParamGet::Request& req, mavros_msgs::ParamGet::Response& res){
+mavros_msgs::ParamValue val;
+if(req.param_id=="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;
+return true;
+}
+
 }
 
 

src/PacketsHandler.cpp

@@ -0,0 +1,775 @@
+/* PacketsHandler.cpp -- Packets Handler class for XBee:
+			     Serialize, deserialize, fragment and reassemly mavlink
+			     messages (packets and std messages)  --                     */
+/* ------------------------------------------------------------------------- */
+/* February 06, 2017 -- @Copyright Aymen Soussia. All rights reserved.       */
+/*                                  (aymen.soussia@gmail.com)                */
+
+
+#include"PacketsHandler.h"
+
+
+namespace Mist
+{
+
+
+namespace Xbee
+{
+
+
+//*****************************************************************************
+PacketsHandler::PacketsHandler():
+	MAX_PACEKT_SIZE(64000),
+	XBEE_NETWORK_MTU(250),
+	FRAGMENT_HEADER_SIZE(6),
+	MAX_TIME_TO_SEND_PACKET(30000),
+	START_DLIMITER(static_cast<unsigned char>(0x7E)),
+	device_64_bits_address_("12345678"),
+	loaded_SL_(false),
+	loaded_SH_(false),
+	optimum_MT_NBR_(3),
+	delay_interframes_(100 * 1000)
+{
+}
+
+
+//*****************************************************************************
+PacketsHandler::~PacketsHandler()
+{
+}
+
+
+//*****************************************************************************
+bool PacketsHandler::Init(SerialDevice* serial_device,
+		Thread_Safe_Deque* in_packets)
+{
+	serial_device_ = serial_device;
+	
+	if (!Load_Database_Addresses())
+		return false;
+		
+	Send_SL_and_SH_Commands();
+	in_packets_ = in_packets;
+	
+	return true;
+}
+
+
+//*****************************************************************************
+bool PacketsHandler::Init_Device_ID()
+{
+	if (Get_Local_Address())
+		return true;
+	else
+		return false;
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Run()
+{	
+	quit_.store(false);
+	
+	while (!quit_.load())
+	{
+		Process_Out_Standard_Messages();
+		Process_Out_Packets();
+	}
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Handle_Mavlink_Message(const mavros_msgs::Mavlink::ConstPtr& mavlink_msg)
+{
+	std::shared_ptr<std::string> serialized_packet =
+			std::make_shared<std::string>();
+	
+	Serialize_Mavlink_Message(mavlink_msg, serialized_packet);
+	
+	if (serialized_packet->size() > XBEE_NETWORK_MTU && serialized_packet->size() <= MAX_PACEKT_SIZE)
+	{
+		std::shared_ptr<std::vector<std::shared_ptr<std::string>>> fragmented_packet =
+		std::make_shared<std::vector<std::shared_ptr<std::string>>>();
+			
+		std::size_t offset = 0;
+		std::size_t NBR_of_bytes = 0;
+		std::size_t NBR_of_fragments = std::ceil(
+			static_cast<float>(serialized_packet->size()) / XBEE_NETWORK_MTU);
+		
+		for (uint8_t i = 0; i < NBR_of_fragments; i++)
+		{
+			fragmented_packet->push_back(std::make_shared<std::string>());
+			NBR_of_bytes = std::min(XBEE_NETWORK_MTU, serialized_packet->size() - offset);
+			Insert_Fragment_Header(false, fragmented_packet->at(i), mavlink_msg->msgid, i, offset);
+			fragmented_packet->at(i)->append(serialized_packet->c_str() + offset, NBR_of_bytes);
+			offset += NBR_of_bytes;
+		}
+		
+		out_packets_.Push_Back({mavlink_msg->msgid, fragmented_packet});
+	}
+	else if (serialized_packet->size() < XBEE_NETWORK_MTU)
+	{
+		serialized_packet->insert(0, 1, 'S');
+		out_std_messages_.Push_Back(serialized_packet);
+	}
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Process_Fragment(std::shared_ptr<std::string> fragment)
+{
+	uint8_t packet_ID = fragment->at(1);
+	uint8_t node_8_bits_address = fragment->at(2);
+	uint8_t fragment_ID = fragment->at(3);
+	uint16_t offset = static_cast<uint16_t>(
+			static_cast<uint16_t>(static_cast<unsigned char>(fragment->at(4))) << 8 |
+			static_cast<uint16_t>(static_cast<unsigned char>(fragment->at(5))));
+	
+	assembly_map_it_ = packets_assembly_map_.find(node_8_bits_address);
+	
+	if (assembly_map_it_ != packets_assembly_map_.end())
+	{
+		if (assembly_map_it_->second.packet_ID_ == packet_ID)
+		{
+			std::set<uint8_t>::iterator it = assembly_map_it_->second.received_fragments_IDs_.find(fragment_ID);
+			
+			if (it == assembly_map_it_->second.received_fragments_IDs_.end())
+			{
+				if (assembly_map_it_->second.received_fragments_IDs_.size() == 0)
+					assembly_map_it_->second.time_since_creation_ = std::clock();
+				
+				Insert_Fragment_In_Packet_Buffer(&assembly_map_it_->second.packet_buffer_, fragment->c_str(), offset, fragment->size());
+				assembly_map_it_->second.received_fragments_IDs_.insert(fragment_ID);
+			}
+		}
+		else
+		{
+			assembly_map_it_->second = {};
+			assembly_map_it_->second.packet_ID_ = packet_ID;
+			Insert_Fragment_In_Packet_Buffer(&assembly_map_it_->second.packet_buffer_, fragment->c_str(), offset, fragment->size());
+			assembly_map_it_->second.received_fragments_IDs_.insert(fragment_ID);
+			assembly_map_it_->second.time_since_creation_ = std::clock();
+		}
+	}
+	else
+	{
+		Add_New_Node_To_Network(node_8_bits_address);
+		assembly_map_it_ = packets_assembly_map_.find(node_8_bits_address);
+		assembly_map_it_->second.packet_ID_ = packet_ID;
+		Insert_Fragment_In_Packet_Buffer(&assembly_map_it_->second.packet_buffer_, fragment->c_str(), offset, fragment->size());
+		assembly_map_it_->second.received_fragments_IDs_.insert(fragment_ID);
+		assembly_map_it_->second.time_since_creation_ = std::clock();
+	}
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Insert_Fragment_In_Packet_Buffer(std::string* buffer, const char* fragment, const uint16_t offset, const std::size_t length)
+{
+	if (offset >= buffer->size())
+		buffer->append(fragment + FRAGMENT_HEADER_SIZE, length - FRAGMENT_HEADER_SIZE);
+	else
+		buffer->insert(offset, fragment + FRAGMENT_HEADER_SIZE, length - FRAGMENT_HEADER_SIZE);
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Process_Ping_Or_Acknowledgement(std::shared_ptr<std::string> frame)
+{
+	uint8_t packet_ID = frame->at(12);
+	uint8_t node_8_bits_address = frame->at(13);
+	
+	if (frame->at(11) == 'A')
+	{
+		mutex_.lock();
+		
+		connected_network_nodes_it_ = connected_network_nodes_.find(node_8_bits_address);
+	
+		if (connected_network_nodes_it_ == connected_network_nodes_.end())
+		{
+			mutex_.unlock();
+			Add_New_Node_To_Network(node_8_bits_address);
+			mutex_.lock();
+			connected_network_nodes_it_ = connected_network_nodes_.find(node_8_bits_address);
+		}
+
+		if (packet_ID == current_processed_packet_ID_)
+		{
+			if (frame->size() < 15)
+  				connected_network_nodes_it_->second = true;
+			else
+			{
+				for (uint8_t i = 14; i < frame->size(); i++)
+					fragments_indexes_to_transmit_.insert(frame->at(i));
+			}
+		}
+		
+		mutex_.unlock();
+	}
+	else  if (frame->at(11) == 'P')
+	{
+		assembly_map_it_ = packets_assembly_map_.find(node_8_bits_address);
+		
+		if (assembly_map_it_ == packets_assembly_map_.end())
+		{
+			Add_New_Node_To_Network(node_8_bits_address);
+			assembly_map_it_ = packets_assembly_map_.find(node_8_bits_address);
+			assembly_map_it_->second.packet_ID_ = packet_ID;
+			assembly_map_it_->second.time_since_creation_ = std::clock();
+		}
+		
+		if (assembly_map_it_->second.packet_ID_ == packet_ID)
+		{
+			std::string Acknowledgement = "A";
+			Acknowledgement.push_back(packet_ID);
+			Acknowledgement.push_back(device_address_);
+			uint8_t packet_size = frame->at(14);
+			
+			if (assembly_map_it_->second.received_fragments_IDs_.size() == packet_size)
+			{
+				in_packets_->Push_Back(std::make_shared<std::string>(assembly_map_it_->second.packet_buffer_));
+				assembly_map_it_->second.packet_buffer_.clear();
+				assembly_map_it_->second.received_fragments_IDs_.clear();
+				assembly_map_it_->second.time_since_creation_ = 0;
+			}
+			else
+			{
+				std::set<uint8_t>::iterator it = assembly_map_it_->second.received_fragments_IDs_.begin();
+				uint8_t j = 0;
+				
+				while (j <= packet_size - 1)
+				{
+					if (j != *it)
+						Acknowledgement.push_back(j);
+					else if (it != std::prev(assembly_map_it_->second.received_fragments_IDs_.end()))
+						it++;
+
+					j++;
+				}
+			}
+			
+			std::string Ack_frame;
+			Generate_Transmit_Request_Frame(Acknowledgement.c_str(), &Ack_frame, Acknowledgement.size());
+			serial_device_->Send_Frame(Ack_frame);
+			usleep(delay_interframes_);
+			
+		}
+		else
+		{
+			assembly_map_it_->second = {};
+			assembly_map_it_->second.packet_ID_ = packet_ID;
+		}
+	}
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Add_New_Node_To_Network(const uint8_t new_node_address)
+{
+	std::set<uint8_t> empty_set;
+	packets_assembly_map_.insert(std::pair<uint8_t, Reassembly_Packet_S>(new_node_address, {}));
+	
+	std::lock_guard<std::mutex> guard(mutex_);
+	connected_network_nodes_.insert(std::pair<uint8_t, bool>(new_node_address, false));
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Process_Command_Response(const char* command_response)
+{
+	if (command_response[0] == 'N' && command_response[1] == 'D')
+	{
+		uint64_t new_node_address = 0;
+		std::lock_guard<std::mutex> guard(mutex_);
+		
+		if (command_response[2] == static_cast<unsigned char>(0))
+		{
+			new_node_address = static_cast<uint64_t>(
+			static_cast<unsigned char>(command_response[5]) << 56 |
+			static_cast<unsigned char>(command_response[6]) << 48 |
+			static_cast<unsigned char>(command_response[7]) << 40 |
+			static_cast<unsigned char>(command_response[8]) << 32 |
+			static_cast<unsigned char>(command_response[9]) << 24 |
+			static_cast<unsigned char>(command_response[10]) << 16 |
+			static_cast<unsigned char>(command_response[11]) << 8 |
+			static_cast<unsigned char>(command_response[12]));
+		}
+		
+		database_addresses_it_ = database_addresses_.find(new_node_address);
+		
+		if (database_addresses_it_ != database_addresses_.end())
+			device_address_ = database_addresses_it_->second;
+		else
+			std::cout << "Remote Node Not in Database" << std::endl; 
+	}
+	else if (command_response[0] == 'S' && command_response[1] == 'H')
+	{
+		if (command_response[2] == static_cast<unsigned char>(0))
+		{
+			loaded_SH_ = true;
+			
+			for (std::size_t i = 0; i < 4; i++)
+				device_64_bits_address_[i] = command_response[3 + i];
+		}
+	}
+	else if (command_response[0] == 'S' && command_response[1] == 'L')
+	{
+		if (command_response[2] == static_cast<unsigned char>(0))
+		{
+			loaded_SL_ = true;
+			
+			for (std::size_t i = 0; i < 4; i++)
+				device_64_bits_address_[4 + i] = command_response[3 + i];
+		}
+	}
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Quit()
+{
+	quit_.store(true);
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Serialize_Mavlink_Message(const mavros_msgs::Mavlink::ConstPtr&
+		mavlink_msg, std::shared_ptr<std::string> serialized_packet)
+{
+	serialized_packet->push_back(mavlink_msg->sysid);
+	serialized_packet->push_back(mavlink_msg->msgid);
+	
+	std::string bytes="12345678";
+	
+	for (std::size_t j = 0; j < mavlink_msg->payload64.size(); j++)
+	{
+		for (std::size_t i = 0; i < 8; i++)
+			bytes[7 - i] = (mavlink_msg->payload64.at(j) >> (i * 8));
+			
+	 	serialized_packet->append(bytes);
+	}
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Insert_Fragment_Header(bool single_fragment,
+		std::shared_ptr<std::string> fragment, const uint8_t packet_ID,
+		const uint8_t fragment_ID, const uint16_t offset) 
+{	
+	if (!single_fragment)
+	{
+		fragment->push_back('F');
+		fragment->push_back(packet_ID);
+		fragment->push_back(device_address_);
+		fragment->push_back(fragment_ID);
+		fragment->push_back(offset >> (1 * 8));
+		fragment->push_back(offset >> (0 * 8));
+	}
+	else
+		fragment->push_back('S');
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Delete_Packets_With_Time_Out()
+{	
+	for(auto& iterator: packets_assembly_map_)
+	{
+		if (std::clock_t() - iterator.second.time_since_creation_ > MAX_TIME_TO_SEND_PACKET && iterator.second.time_since_creation_ != 0)
+			iterator.second = {};
+	}
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Process_Out_Standard_Messages()
+{
+	std::size_t deque_size = out_std_messages_.Get_Size();
+	
+	if (deque_size > 0)
+	{
+		std::string frame;
+		std::shared_ptr<std::string> out_message;
+		
+		for (std::size_t i = 0; i < deque_size; i++)
+		{
+			frame.clear();
+			out_message = out_std_messages_.Pop_Front();
+			
+			Generate_Transmit_Request_Frame(out_message->c_str(), &frame, out_message->size());
+			serial_device_->Send_Frame(frame);
+			usleep(delay_interframes_);
+		}
+	}
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Process_Out_Packets()
+{
+	std::size_t deque_size = out_packets_.Get_Size();
+	
+	if (deque_size > 0)
+	{
+		Out_Packet_S out_packet;
+		
+		for (std::size_t i = 0; i < deque_size; i++)
+		{
+			Process_Out_Standard_Messages();
+			out_packet = out_packets_.Pop_Front();
+
+			Send_Packet(out_packet);
+		}
+	}
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Send_Packet(const Out_Packet_S& packet)
+{
+	std::size_t NBR_of_transmission = 0; 
+	std::vector<std::string> frames;
+	
+	Init_Network_Nodes_For_New_Transmission(packet.packet_ID_, &frames, packet.packet_buffer_);
+	current_processed_packet_ID_ = packet.packet_ID_;
+	
+	std::clock_t start_time = std::clock();
+		
+	while (std::clock() - start_time <= MAX_TIME_TO_SEND_PACKET && !Check_Packet_Transmitted_To_All_Nodes())
+	{
+		NBR_of_transmission++;
+		Transmit_Fragments(frames);
+		Send_End_Of_Packet_Ping(packet.packet_ID_, packet.packet_buffer_->size());
+		usleep(500 * 1000);
+	}
+	
+	Adjust_Optimum_MT_Number(std::clock() - start_time, NBR_of_transmission);
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Send_End_Of_Packet_Ping(const uint8_t packet_ID, const uint8_t total_NBR_of_fragments)
+{
+	std::string ping_message = "P";
+	std::string ping_frame;
+	
+	ping_message.push_back(packet_ID);
+	ping_message.push_back(device_address_);
+	ping_message.push_back(total_NBR_of_fragments);
+	
+	Generate_Transmit_Request_Frame(ping_message.c_str(), &ping_frame, ping_message.size());
+	serial_device_->Send_Frame(ping_frame);
+	usleep(delay_interframes_);
+}
+
+
+//*****************************************************************************
+bool PacketsHandler::Load_Database_Addresses()
+{
+	const std::string FILE_PATH = DATABASE_PATH;
+
+	if (!boost::filesystem::exists(FILE_PATH))
+	{	
+		std::cout << "database.xml Not Found with path: " << FILE_PATH << std::endl;
+		return false;
+	}
+	
+	ptree pt;
+	boost::property_tree::read_xml(FILE_PATH, pt);
+	std::string short_address;
+	std::string address_64_bits;
+	unsigned int short_address_int;
+	uint64_t address_64_bits_int;
+	
+	BOOST_FOREACH(ptree::value_type const&v, pt.get_child("Addresses"))
+	{
+		if (v.first == "Device")
+		{
+			short_address = v.second.get<std::string>("<xmlattr>.Address");
+			address_64_bits = v.second.data();
+			
+			if (sscanf(short_address.c_str(), "%3u", &short_address_int) < 0)
+			{
+				std::cout << "Short Address Error. Please Check database.xml For Possible Errors." << std::endl;
+				return false;
+			}
+			
+			if (sscanf(address_64_bits.c_str(), "%" SCNx64, &address_64_bits_int) < 0)
+			{
+				std::cout << "64 bits Address Error. Please Check database.xml For Possible Errors." << std::endl;
+				return false;
+			}
+			
+			database_addresses_.insert(std::pair<uint64_t, uint8_t>(address_64_bits_int, static_cast<uint8_t>(short_address_int)));
+		}
+	}
+	
+	return true;
+}
+
+
+//*****************************************************************************
+bool PacketsHandler::Get_Local_Address()
+{
+	const useconds_t ONE_SECOND = 1*1000*1000; /* 1s = 1 * 10⁶ microseconds. */
+	usleep(ONE_SECOND);
+	
+	if (loaded_SH_ && loaded_SL_)
+	{
+		uint64_t local_64_bits_address = (
+		static_cast<uint64_t>(static_cast<unsigned char>(device_64_bits_address_[0])) << 56 |
+		static_cast<uint64_t>(static_cast<unsigned char>(device_64_bits_address_[1])) << 48 |
+		static_cast<uint64_t>(static_cast<unsigned char>(device_64_bits_address_[2])) << 40 |
+		static_cast<uint64_t>(static_cast<unsigned char>(device_64_bits_address_[3])) << 32 |
+		static_cast<uint64_t>(static_cast<unsigned char>(device_64_bits_address_[4])) << 24 |
+		static_cast<uint64_t>(static_cast<unsigned char>(device_64_bits_address_[5])) << 16 |
+		static_cast<uint64_t>(static_cast<unsigned char>(device_64_bits_address_[6])) << 8 |
+		static_cast<uint64_t>(static_cast<unsigned char>(device_64_bits_address_[7])));
+		
+		database_addresses_it_ = database_addresses_.find(local_64_bits_address);
+		
+		if (database_addresses_it_ != database_addresses_.end())
+		{
+			device_address_ = database_addresses_it_->second;
+			std::cout << "Loaded Short Device Address :  " << static_cast<int>(device_address_) << std::endl;
+			return true;
+		}
+		else
+		{
+			std::cout << "Local Address Not Found In Database. Please Add The Xbee Address to database.xml." << std::endl;
+			return false;
+		}
+	}
+	else
+	{
+		Send_SL_and_SH_Commands();
+		return false;
+	}	
+}
+
+
+//*****************************************************************************
+bool PacketsHandler::Check_Packet_Transmitted_To_All_Nodes()
+{
+	std::lock_guard<std::mutex> guard(mutex_);
+	
+	if (connected_network_nodes_.size() == 0)
+		return false;
+	
+	for (auto it : connected_network_nodes_)
+	{
+		if (!it.second)
+			return false;
+	}
+	
+	return true;
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Init_Network_Nodes_For_New_Transmission(const uint8_t packet_ID, std::vector<std::string>* frames, std::shared_ptr<std::vector<std::shared_ptr<std::string>>> packet)
+{
+	std::lock_guard<std::mutex> guard(mutex_);
+	fragments_indexes_to_transmit_.clear();
+	
+	for (auto& it : connected_network_nodes_)
+		it.second = false;
+	
+	current_processed_packet_ID_ = packet_ID;
+	
+	for (uint8_t i = 0; i < packet->size(); i++)
+	{
+		frames->push_back("");
+		fragments_indexes_to_transmit_.insert(i);
+		Generate_Transmit_Request_Frame(packet->at(i)->c_str(), &frames->at(i), packet->at(i)->size());
+	}
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Transmit_Fragments(const std::vector<std::string>& frames)
+{
+	std::lock_guard<std::mutex> guard(mutex_);
+	
+	for (auto index: fragments_indexes_to_transmit_)
+	{
+		serial_device_->Send_Frame(frames.at(index));
+		usleep(delay_interframes_);
+	}
+	
+	fragments_indexes_to_transmit_.clear();		
+}
+		
+
+//*****************************************************************************
+void PacketsHandler::Adjust_Optimum_MT_Number(const std::clock_t elapsed_time,
+		const std::size_t NBR_of_transmission)
+{	
+	if (NBR_of_transmission > 1 && elapsed_time < MAX_TIME_TO_SEND_PACKET)
+		delay_interframes_ += 5000;
+	else if (NBR_of_transmission == 1 && delay_interframes_ >= 5000)
+		delay_interframes_ -= 5000;
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Send_SL_and_SH_Commands()
+{
+	std::string command;
+	std::string frame;
+	
+	command = "SL";
+	Generate_AT_Command(command.c_str(), &frame);
+	serial_device_->Send_Frame(frame);
+	
+	command = "SH";
+	frame = "";
+	Generate_AT_Command(command.c_str(), &frame);
+	serial_device_->Send_Frame(frame);
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Deserialize_Mavlink_Message(const char * bytes,
+			mavros_msgs::Mavlink* mavlink_msg, const std::size_t msg_size)
+{
+	mavlink_msg->sysid = bytes[0];
+	mavlink_msg->msgid = bytes[1];
+	uint64_t current_int64 = 0;
+	
+	for (std::size_t i = 2; i < msg_size; i += 8)
+	{
+		current_int64 = (
+		static_cast<uint64_t>(static_cast<unsigned char>(bytes[i])) << 56 |
+		static_cast<uint64_t>(static_cast<unsigned char>(bytes[i + 1])) << 48 |
+		static_cast<uint64_t>(static_cast<unsigned char>(bytes[i + 2])) << 40 |
+		static_cast<uint64_t>(static_cast<unsigned char>(bytes[i + 3])) << 32 |
+		static_cast<uint64_t>(static_cast<unsigned char>(bytes[i + 4])) << 24 |
+		static_cast<uint64_t>(static_cast<unsigned char>(bytes[i + 5])) << 16 |
+		static_cast<uint64_t>(static_cast<unsigned char>(bytes[i + 6])) << 8 |
+		static_cast<uint64_t>(static_cast<unsigned char>(bytes[i + 7])));
+		
+		mavlink_msg->payload64.push_back(current_int64);
+	}
+}
+
+
+//*****************************************************************************
+inline void PacketsHandler::Generate_Transmit_Request_Frame(
+		const char* message,
+		std::string * frame,
+		const std::size_t message_size,
+		const unsigned char frame_ID,
+		const std::string& destination_adssress,
+		const std::string& short_destination_adress,
+		const std::string& broadcast_radius,
+		const std::string& options)
+{
+	const unsigned char FAME_TYPE = static_cast<unsigned char>(0x10); /* Transmit Request Frame */
+	std::string frame_parameters;
+	std::string bytes_frame_parameters;
+
+	frame_parameters.append(destination_adssress);
+	frame_parameters.append(short_destination_adress);
+	frame_parameters.append(broadcast_radius);
+	frame_parameters.append(options);
+
+	Convert_HEX_To_Bytes(frame_parameters, &bytes_frame_parameters);
+
+	frame->push_back(FAME_TYPE);
+	frame->push_back(frame_ID);
+	frame->append(bytes_frame_parameters);
+	frame->append(message, message_size);
+
+	Calculate_and_Append_Checksum(frame);
+	Add_Length_and_Start_Delimiter(frame);
+}
+
+
+//*****************************************************************************
+inline void PacketsHandler::Add_Length_and_Start_Delimiter(
+		std::string* frame)
+{
+	const unsigned short FIVE_BYTES = 5;
+	char temporary_buffer[FIVE_BYTES];
+	unsigned char temporary_char;
+	int Hex_frame_length_1;
+	int Hex_frame_length_2;
+	std::string header;
+	int frame_length = frame->size() - 1; /* frame_length = frame_size - checksum byte */
+
+	header.push_back(START_DLIMITER);
+	sprintf(temporary_buffer, "%04X", frame_length);
+	sscanf(temporary_buffer, "%02X%02X", &Hex_frame_length_1, 
+			&Hex_frame_length_2);
+	temporary_char = static_cast<unsigned char>(Hex_frame_length_1);
+	header.push_back(temporary_char);
+	temporary_char = static_cast<unsigned char>(Hex_frame_length_2);
+	header.push_back(temporary_char);
+	frame->insert(0, header);
+}
+
+
+//*****************************************************************************
+inline void PacketsHandler::Calculate_and_Append_Checksum(
+		std::string* frame)
+{
+	unsigned short bytes_sum = 0;
+	unsigned lowest_8_bits = 0;
+	unsigned short checksum = 0;
+	unsigned char checksum_byte;
+
+	for (unsigned short i = 0; i < frame->size(); i++)
+		bytes_sum += static_cast<unsigned short>(frame->at(i));
+
+	lowest_8_bits = bytes_sum & 0xFF;
+	checksum = 0xFF - lowest_8_bits;
+	checksum_byte = static_cast<unsigned char>(checksum);
+	frame->push_back(checksum_byte);
+}
+
+
+//*****************************************************************************
+inline void PacketsHandler::Convert_HEX_To_Bytes(
+		const std::string& HEX_data, std::string* converted_data)
+{
+	const unsigned short TWO_BYTES = 2;
+	char temporary_buffer[TWO_BYTES];
+	unsigned char temporary_char;
+	int temporary_HEX;
+
+	for (unsigned short i = 0; i <= HEX_data.size() - TWO_BYTES;
+			i += TWO_BYTES)
+	{
+		memcpy(temporary_buffer, HEX_data.c_str() + i, TWO_BYTES);
+		sscanf(temporary_buffer, "%02X", &temporary_HEX);
+		temporary_char = static_cast<unsigned char>(temporary_HEX);
+		converted_data->push_back(temporary_char);
+	}
+}
+
+
+//*****************************************************************************
+void PacketsHandler::Generate_AT_Command(const char* command,
+			std::string* frame,
+			const unsigned char frame_ID)
+{
+	const unsigned char FAME_TYPE = static_cast<unsigned char>(0x09);/* AT Command Frame */
+	std::string temporary_parameter_value;
+
+	frame->push_back(FAME_TYPE);
+	frame->push_back(frame_ID);
+	frame->append(command);
+	
+	Calculate_and_Append_Checksum(frame);
+	Add_Length_and_Start_Delimiter(frame);
+}
+
+uint8_t PacketsHandler::get_device_id(){
+return device_address_;
+}
+
+}
+
+
+}

src/SerialDevice.cpp

@@ -39,7 +39,6 @@ bool SerialDevice::Init(
 	{
 		Set_Port_Options(baud_rate);
 		Init_Frame_Type_Keys();
-		Read_Frame_Header();
 		return true;
 	}
 	else
@@ -82,6 +81,7 @@ void SerialDevice::Send_Frame(const std::string& frame)
 //*****************************************************************************
 void SerialDevice::Run_Service()
 {
+	Read_Frame_Header();
 	io_service_.run();
 }
 
@@ -90,14 +90,26 @@ void SerialDevice::Run_Service()
 void SerialDevice::Stop_Service()
 {
 	io_service_.post([this]() {io_service_.stop(); });
+}
+
+
+//*****************************************************************************
+void SerialDevice::Close_Serial_Port()
+{
 	io_service_.post([this]() {serial_port_.close(); });
 }
 
 
 //*****************************************************************************
-Thread_Safe_Deque * SerialDevice::Get_In_Messages_Pointer()
+void SerialDevice::Set_In_Messages_Pointers(Thread_Safe_Deque* in_std_messages,
+			Thread_Safe_Deque* in_fragments,
+			Thread_Safe_Deque* in_Acks_and_Pings,
+			Thread_Safe_Deque* command_responses)
 {
-	return &in_messages_;
+	in_std_messages_ = in_std_messages;
+	in_fragments_ = in_fragments;
+	in_Acks_and_Pings_ = in_Acks_and_Pings;
+	command_responses_ = command_responses;
 }
 
 
@@ -156,9 +168,11 @@ void SerialDevice::Read_Frame_Header()
 				Read_Frame_Body();
 			}
 			else
+			{
 				/* The header is totally corrupted, read another header. */
 				Read_Frame_Header();
 			}
+		}
 		else
 		{
 			std::cout << "Error Occured:" << std::endl;
@@ -183,15 +197,42 @@ void SerialDevice::Read_Frame_Body()
 			if (current_frame_.Get_Frame_Type() == 
 					FRAME_TYPE_KEYS[RECEIVE_PACKET])
 			{
-				const unsigned short ELEVEN_BYTES = 11;
+				char msg_type = current_frame_.Get_Message_Type();
-				const unsigned short TWELVE_BYTES = 12;
-
 				std::shared_ptr<std::string> in_message =
 					std::make_shared<std::string>();
+				
+				if (msg_type == 'F')
+				{
 					in_message->append(current_frame_.Get_Frame_Body()
-					+ ELEVEN_BYTES,
+					+ 11,
-					current_frame_.Get_Frame_Body_Length() - TWELVE_BYTES);
+					current_frame_.Get_Frame_Body_Length() - 12);
-				in_messages_.Push_Pack(in_message);
+					in_fragments_->Push_Back(in_message);
+				}
+					
+				else if (msg_type == 'A' || msg_type == 'P')
+				{
+					in_message->append(current_frame_.Get_Frame_Body(),
+					current_frame_.Get_Frame_Body_Length() - 1);
+					in_Acks_and_Pings_->Push_Back(in_message);
+				}
+					
+				else if (msg_type == 'S')
+				{
+					in_message->append(current_frame_.Get_Frame_Body() + 12,
+					current_frame_.Get_Frame_Body_Length() - 13);
+					in_std_messages_->Push_Back(in_message);
+				}
+			}
+			else if (current_frame_.Get_Frame_Type() == 
+					FRAME_TYPE_KEYS[AT_COMMAND_RESPONSE])
+			{	
+				std::shared_ptr<std::string> in_message =
+					std::make_shared<std::string>();
+				
+				in_message->append(current_frame_.Get_Frame_Body() + 1,
+					current_frame_.Get_Frame_Body_Length() - 2);
+				
+				command_responses_->Push_Back(in_message);
 			}
 
 			Read_Frame_Header();
@@ -226,6 +267,20 @@ void SerialDevice::Write_Frame()
 }
 
 
+//*****************************************************************************
+bool SerialDevice::Is_IO_Service_Stopped()
+{
+	return io_service_.stopped();
+}
+
+
+//*****************************************************************************
+void SerialDevice::Reset_IO_Service()
+{
+	io_service_.reset();
+}
+
+
 }
 
 

src/TestBuzz.cpp

@@ -8,7 +8,7 @@
 #include <stdlib.h> 
 #include <time.h> 
 
-#include<mavros_msgs/Mavlink.h>
+#include <mavros_msgs/Mavlink.h>
 #include <ros/ros.h>
 
 
@@ -47,25 +47,28 @@ void Init_Random_Seed()
 //*****************************************************************************
 int main(int argc, char **argv)
 {
-  const unsigned int MIN_PAYLOAD_SIZE = 1;
+  const unsigned int MIN_PAYLOAD_SIZE = 750;
-  const unsigned int MAX_PAYLOAD_SIZE = 10;
+  const unsigned int MAX_PAYLOAD_SIZE = 752;
   
-  ros::init(argc, argv, "flight_controller");
+  ros::init(argc, argv, "test_buzz");
 
   ros::NodeHandle node_handle;
   ros::Publisher mavlink_pub = node_handle.advertise<mavros_msgs::Mavlink>("outMavlink", 1000);
   ros::Subscriber mavlink_sub = node_handle.subscribe("inMavlink", 1000, Receive_Payload_Callback);
 
-  ros::Rate loop_rate(0.2);
-
   Init_Random_Seed();
 
   int count = 0;
+  const std::size_t MAX_BUFFER_SIZE = 64;
+  char line[MAX_BUFFER_SIZE];
   
+  std::cout << "Press Enter to Send New Mavlink Message..." << std::endl;
   
-  while (ros::ok())
+  while (ros::ok() && std::cin.getline(line, MAX_BUFFER_SIZE))
   {
     mavros_msgs::Mavlink mavlink_msg_;
+    mavlink_msg_.sysid = 2;
+    mavlink_msg_.msgid = 1;
 
     unsigned int payload_size = Get_Random_Size(MIN_PAYLOAD_SIZE, MAX_PAYLOAD_SIZE);
 
@@ -87,9 +90,8 @@ int main(int argc, char **argv)
 
     ros::spinOnce();
 
-    loop_rate.sleep();
-
     count++;
+    std::cout << "Press Enter to Send New Mavlink Message..." << std::endl;
   }
 
   return 0;

src/XBeeFrame.cpp

@@ -141,6 +141,13 @@ void Frame::Rearrange_Corrupted_Header()
 }
 
 
+//*****************************************************************************
+char Frame::Get_Message_Type()
+{
+	return data_buffer_[15];
+}
+
+
 }
 
 

src/XMLConfigParser.cpp

@@ -7,6 +7,7 @@
 #include "XMLConfigParser.h"
 
 
+
 //*****************************************************************************
 XMLConfigParser::XMLConfigParser():
 	config_loaded_successfully_(false)
@@ -23,7 +24,7 @@ XMLConfigParser::~XMLConfigParser()
 //*****************************************************************************
 bool XMLConfigParser::Load_Config()
 {
-	const std::string FILE_NAME = "/home/ubuntu/ROS_WS/src/xbeemav/Resources/XBee_Config.xml";
+	const std::string FILE_NAME = XBEE_CONFIG_PATH;
 
 	if (Check_Config_File_Exists(FILE_NAME))
 	{

src/Xbee.cpp

@@ -14,10 +14,11 @@ int main(int argc, char* argv[])
   try
   {
 	ros::init(argc, argv, "xbee");
+	ros::NodeHandle node_handle;
 
 	Mist::Xbee::CommunicationManager communication_manager;
-	const std::string& device = "/dev/ttyUSB0"; // TO DO can be introduced as command.
+	std::string device;
-	const std::size_t baud_rate = 230400; // TO DO Can be introduced as command.
+	double baud_rate;
 	Mist::Xbee::CommunicationManager::DRONE_TYPE drone_type = 
 			Mist::Xbee::CommunicationManager::DRONE_TYPE::SLAVE;
 	Mist::Xbee::CommunicationManager::RUNNING_MODE running_mode =
@@ -34,9 +35,18 @@ int main(int argc, char* argv[])
 				running_mode = Mist::Xbee::CommunicationManager::RUNNING_MODE::SWARM;
 		}
 	}
+	if (!node_handle.getParam("USB_port", device))
+     	{
+       		std::cout << "Failed to Get Topic Name: param 'USB_port' Not Found." << std::endl;
+       		return EXIT_FAILURE;
+     	}
 
-
+     	if (!node_handle.getParam("Baud_rate", baud_rate))
-	if (communication_manager.Init(device, baud_rate))
+     	{
+       		std::cout << "Failed to Get Topic Name: param 'Baud_rate' Not Found." << std::endl;
+       		return EXIT_FAILURE;
+     	}
+	if (communication_manager.Init(device, static_cast<std::size_t>(baud_rate)))
 		communication_manager.Run(drone_type, running_mode);
   }
   catch (const std::exception& e)