Merge branch 'test' of https://github.com/MISTLab/XbeeMav into test

Conflicts:
	CMakeLists.txt
	Resources/XBee_Config.xml
	include/CommunicationManager.h
	include/MultithreadingDeque.hpp
	include/SerialDevice.h
	include/XBeeFrame.h
	launch/xbeemav.launch
	src/CommunicationManager.cpp
	src/SerialDevice.cpp
	src/TestBuzz.cpp
	src/XBeeFrame.cpp
	src/XMLConfigParser.cpp
	src/Xbee.cpp

CMakeLists.txt

@@ -53,17 +53,30 @@ include_directories(
 )
 
 
+<<<<<<< HEAD
 add_executable(xbee_mav src/Xbee.cpp src/XBeeFrame.cpp src/SerialDevice.cpp  src/CommunicationManager.cpp)
 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(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})
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 
 #add_executable(test_controller src/TestController.cpp)
 #target_link_libraries(test_controller ${catkin_LIBRARIES})
 
+<<<<<<< HEAD
 #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})
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 
 
 
@@ -76,6 +89,9 @@ target_link_libraries(config ${catkin_LIBRARIES})
 #############
 ## Testing ##
 #############
+<<<<<<< HEAD
 
 
 
+=======
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1

Resources/XBee_Config.xml

@@ -22,7 +22,11 @@
 		<Parameter Command="CI" Description="Cluster ID">11</Parameter>
 		<Parameter Command="EE" Description="Encryption Enbale">0</Parameter>
 		<Parameter Command="KY" Description="Encryption Key"></Parameter>
+<<<<<<< HEAD
 		<Parameter Command="BD" Description="Baud Rate">7</Parameter>
+=======
+		<Parameter Command="BD" Description="Baud Rate">8</Parameter>
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 		<Parameter Command="NB" Description="Parity">0</Parameter>
 		<Parameter Command="SB" Description="Stop Bits">0</Parameter>
 		<Parameter Command="RO" Description="Packetization Timeout">3</Parameter>

Resources/database.xml

@@ -0,0 +1,14 @@
+<?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>
+</Addresses>

include/CommunicationManager.h

@@ -1,6 +1,10 @@
 /* CommunicationManager.h -- Communication Manager class for XBee:
 			     Handles all communications with other ROS nodes
+<<<<<<< HEAD
 			     and the serial port --     	             */
+=======
+			     and the serial port --     	                             */
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 /* ------------------------------------------------------------------------- */
 /* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved.      */
 /*                                  (aymen.soussia@gmail.com)                */
@@ -16,12 +20,18 @@
 #include<mavros_msgs/Mavlink.h>
 #include <ros/ros.h>
 
+<<<<<<< HEAD
 #include"SerialDevice.h"
 
 #define MESSAGE_CONSTANT 238
 #define ACK_MESSAGE_CONSTANT 911
 #define XBEE_MESSAGE_CONSTANT 586782343
 #define XBEE_STOP_TRANSMISSION 4355356352
+=======
+#include"PacketsHandler.h"
+#include"SerialDevice.h"
+
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 
 namespace Mist
 {
@@ -62,15 +72,22 @@ private:
 
 	void Run_In_Solo_Mode(DRONE_TYPE drone_type);
 	void Run_In_Swarm_Mode();
+<<<<<<< HEAD
 	void Generate_Transmit_Request_Frame(
 			const char* const message,
 			std::string* frame,
 			int tot,
+=======
+	/*void Generate_Transmit_Request_Frame(
+			const char* const message,
+			std::string* frame,
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 			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",
+<<<<<<< HEAD
 			const std::string& options = "00");
 	void Check_In_Messages_and_Transfer_To_Topics();
 	void Display_Init_Communication_Failure();
@@ -78,6 +95,15 @@ private:
 			std::string* converted_data);
 	void Calculate_and_Append_Checksum(std::string* frame);
 	void Add_Length_and_Start_Delimiter(std::string* frame);
+=======
+			const std::string& options = "00");*/
+	//void Check_In_Messages_and_Transfer_To_Topics();
+	void Display_Init_Communication_Failure();
+	//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);
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 	void Send_Mavlink_Message_Callback(
 			const mavros_msgs::Mavlink::ConstPtr& mavlink_msg);
 	void Display_Drone_Type_and_Running_Mode(DRONE_TYPE drone_type,
@@ -85,15 +111,32 @@ private:
 	bool Serve_Flight_Controller(mavros_msgs::CommandInt::
 		Request& request, mavros_msgs::CommandInt::Response& response);
 	void Check_In_Messages_and_Transfer_To_Server();
+<<<<<<< HEAD
 	unsigned short Caculate_Checksum(std::string* frame);
 	void Send_multi_msg();
 	Mist::Xbee::SerialDevice serial_device_;
 	Thread_Safe_Deque* in_messages_;
+=======
+	void Process_In_Standard_Messages();
+	void Process_In_Acks_and_Pings();
+	void Process_In_Fragments();
+	void Process_In_Packets();
+	void Process_Command_Responses();
+
+	Mist::Xbee::SerialDevice serial_device_;
+	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_;
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 	ros::NodeHandle node_handle_;
 	ros::Subscriber mavlink_subscriber_;	
 	ros::Publisher mavlink_publisher_;
 	ros::ServiceClient mav_dji_client_;
 	ros::ServiceServer mav_dji_server_;
+<<<<<<< HEAD
 	/*No of robots*/
 	int no_of_dev;
 	int device_id;
@@ -114,6 +157,9 @@ private:
 	uint64_t message_obtmulti[600];
 	//int test_1;
 	//struct timeval t1, t2;
+=======
+	std::shared_ptr<std::thread> service_thread_; // TO DO delete !?
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 };
 
 

include/MultithreadingDeque.hpp

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

include/PacketsHandler.h

@@ -0,0 +1,152 @@
+/* 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);
+	
+	
+	
+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,18 @@ namespace Xbee
 {
 
 
+<<<<<<< HEAD
 typedef MultithreadingDeque<std::shared_ptr<std::string>> Thread_Safe_Deque;
+=======
+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;
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 
 
 //*****************************************************************************
@@ -39,7 +50,17 @@ public:
 	void Send_Frame(const std::string& frame);
 	void Run_Service();
 	void Stop_Service();
+<<<<<<< HEAD
 	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();
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 
 private:
 
@@ -71,7 +92,14 @@ private:
 	boost::asio::io_service io_service_;
 	boost::asio::serial_port serial_port_;
 	std::deque<std::string> out_messages_;
+<<<<<<< HEAD
 	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_;
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 	Mist::Xbee::Frame current_frame_;
 	unsigned int FRAME_TYPE_KEYS[REMOTE_AT_COMMAND_RESPONSE + 1];
 };

include/XBeeFrame.h

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

launch/xbeemav.launch

@@ -7,7 +7,10 @@
 	<!--  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" />
+<<<<<<< HEAD
 	<param name="No_of_dev" type="int" value="3" />
+=======
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 	<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
 	<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
 </launch>

src/CommunicationManager.cpp

@@ -9,6 +9,7 @@
 #include "CommunicationManager.h"
 
 
+<<<<<<< HEAD
 uint16_t* u64_cvt_u16(uint64_t u64){
    uint16_t* out = new uint16_t[4];
    uint32_t int32_1 = u64 & 0xFFFFFFFF;
@@ -33,6 +34,8 @@ uint16_t get_deviceid(){
 return (uint16_t)id;
 }
 
+=======
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 namespace Mist
 {
 
@@ -44,7 +47,11 @@ namespace Xbee
 //*****************************************************************************
 CommunicationManager::CommunicationManager():
 	START_DLIMITER(static_cast<unsigned char>(0x7E)),
+<<<<<<< HEAD
 	LOOP_RATE(50) /* 10 fps */
+=======
+	LOOP_RATE(10) /* 10 fps */
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 {
 }
 
@@ -61,14 +68,46 @@ bool CommunicationManager::Init(
 {
 	if (serial_device_.Init(device, baud_rate))
 	{
+<<<<<<< HEAD
 		in_messages_ = serial_device_.Get_In_Messages_Pointer();
 		return true;
+=======
+		serial_device_.Set_In_Messages_Pointers(&in_std_messages_, &in_fragments_,
+		&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 <= 30000)
+		{
+			Process_Command_Responses();
+			
+			if (packets_handler_.Init_Device_ID())
+			{
+				device_ID_loaded = true;
+				break;
+			}
+		}
+		
+		if (!device_ID_loaded)
+			return false;
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 	}
 	else
 	{
 		Display_Init_Communication_Failure();
 		return false;
 	}
+<<<<<<< HEAD
+=======
+	
+	return true;
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 }
 
 
@@ -77,7 +116,11 @@ bool CommunicationManager::Init(
 void CommunicationManager::Run(DRONE_TYPE drone_type,
 		RUNNING_MODE running_mode)
 {
+<<<<<<< HEAD
 	std::thread thread_service(&SerialDevice::Run_Service, &serial_device_);
+=======
+	std::thread thread_packets_handler(&PacketsHandler::Run, &packets_handler_);
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 
 	Display_Drone_Type_and_Running_Mode(drone_type, running_mode);
 
@@ -87,7 +130,14 @@ void CommunicationManager::Run(DRONE_TYPE drone_type,
 		Run_In_Solo_Mode(drone_type);
 
 	serial_device_.Stop_Service();
+<<<<<<< HEAD
   	thread_service.join();
+=======
+	serial_device_.Close_Serial_Port();
+	packets_handler_.Quit();
+  	service_thread_->join();
+  	thread_packets_handler.join();
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 }
 
 
@@ -101,7 +151,11 @@ void CommunicationManager::Run_In_Solo_Mode(DRONE_TYPE drone_type)
 	{
 		if (node_handle_.getParam("Xbee_In_From_Controller", service_name))
 		{
+<<<<<<< HEAD
 			mav_dji_server_ = node_handle_.advertiseService(service_name.c_str(), 		&CommunicationManager::Serve_Flight_Controller, this);
+=======
+			mav_dji_server_ = node_handle_.advertiseService(service_name.c_str(), &CommunicationManager::Serve_Flight_Controller, this);
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 			success = true;
 		}
 		else
@@ -159,6 +213,7 @@ void CommunicationManager::Run_In_Swarm_Mode()
 	else
 		std::cout << "Failed to Get Topic Name: param 'Xbee_Out_To_Buzz' Not Found." << std::endl;
 
+<<<<<<< HEAD
 	/*Get no of devices*/
 	node_handle_.getParam("No_of_dev", no_of_dev);
 	/*Get device Id feom host name*/
@@ -176,6 +231,21 @@ void CommunicationManager::Run_In_Swarm_Mode()
 			Send_multi_msg();
 			ros::spinOnce();
 	    		loop_rate.sleep();
+=======
+	if (success_1 && success_2)
+	{
+		ros::Rate loop_rate(LOOP_RATE);
+
+		while (ros::ok())
+		{
+			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();
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 		}
 	}	
 }
@@ -185,12 +255,20 @@ void CommunicationManager::Run_In_Swarm_Mode()
 inline bool CommunicationManager::Serve_Flight_Controller(mavros_msgs::CommandInt::
 		Request& request, mavros_msgs::CommandInt::Response& response)
 {
+<<<<<<< HEAD
 	const std::size_t MAX_BUFFER_SIZE = 255;
+=======
+	const std::size_t MAX_BUFFER_SIZE = 256;
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 	unsigned int command = 0;
 	char temporary_buffer[MAX_BUFFER_SIZE];
 	std::string frame;
 
+<<<<<<< HEAD
 	switch(request.command)
+=======
+	/*switch(request.command)
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 	{
 		case mavros_msgs::CommandCode::NAV_TAKEOFF:
 		{
@@ -234,9 +312,15 @@ inline bool CommunicationManager::Serve_Flight_Controller(mavros_msgs::CommandIn
 
 	if (command != 0)
 	{
+<<<<<<< HEAD
 		//Generate_Transmit_Request_Frame(temporary_buffer, &frame);
 		serial_device_.Send_Frame(frame);
 	}
+=======
+		Generate_Transmit_Request_Frame(temporary_buffer, &frame);
+		serial_device_.Send_Frame(frame);
+	}*/
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 	
 	return true;
 }
@@ -257,10 +341,16 @@ void CommunicationManager::Display_Init_Communication_Failure()
 
 
 //*****************************************************************************
+<<<<<<< HEAD
 inline void CommunicationManager::Generate_Transmit_Request_Frame(
 		const char* const message,
 		std::string * frame,
 		int tot,
+=======
+/*inline void CommunicationManager::Generate_Transmit_Request_Frame(
+		const char* const message,
+		std::string * frame,
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 		const unsigned char frame_ID,
 		const std::string& destination_adssress,
 		const std::string& short_destination_adress,
@@ -268,7 +358,11 @@ inline void CommunicationManager::Generate_Transmit_Request_Frame(
 		const std::string& options)
 {
 	const unsigned char FAME_TYPE = static_cast<unsigned char>(0x10); /* Transmit Request Frame */
+<<<<<<< HEAD
 	std::string frame_parameters;
+=======
+	/*std::string frame_parameters;
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 	std::string bytes_frame_parameters;
 
 	frame_parameters.append(destination_adssress);
@@ -281,6 +375,7 @@ inline void CommunicationManager::Generate_Transmit_Request_Frame(
 	frame->push_back(FAME_TYPE);
 	frame->push_back(frame_ID);
 	frame->append(bytes_frame_parameters);
+<<<<<<< HEAD
 	frame->append(message, tot);
 
 	Calculate_and_Append_Checksum(frame);
@@ -290,6 +385,17 @@ inline void CommunicationManager::Generate_Transmit_Request_Frame(
 
 //*****************************************************************************
 inline void CommunicationManager::Convert_HEX_To_Bytes(
+=======
+	frame->append(message, std::strlen(message));
+
+	Calculate_and_Append_Checksum(frame);
+	Add_Length_and_Start_Delimiter(frame);
+}*/
+
+
+//*****************************************************************************
+/*inline void CommunicationManager::Convert_HEX_To_Bytes(
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 		const std::string& HEX_data, std::string* converted_data)
 {
 	const unsigned short TWO_BYTES = 2;
@@ -305,11 +411,19 @@ inline void CommunicationManager::Convert_HEX_To_Bytes(
 		temporary_char = static_cast<unsigned char>(temporary_HEX);
 		converted_data->push_back(temporary_char);
 	}
+<<<<<<< HEAD
 }
 
 
 //*****************************************************************************
 inline void CommunicationManager::Calculate_and_Append_Checksum(
+=======
+}*/
+
+
+//*****************************************************************************
+/*inline void CommunicationManager::Calculate_and_Append_Checksum(
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 		std::string* frame)
 {
 	unsigned short bytes_sum = 0;
@@ -324,6 +438,7 @@ inline void CommunicationManager::Calculate_and_Append_Checksum(
 	checksum = 0xFF - lowest_8_bits;
 	checksum_byte = static_cast<unsigned char>(checksum);
 	frame->push_back(checksum_byte);
+<<<<<<< HEAD
 }
 
 unsigned short CommunicationManager::Caculate_Checksum(std::string* frame)
@@ -345,6 +460,13 @@ return checksum;
 
 //*****************************************************************************
 inline void CommunicationManager::Add_Length_and_Start_Delimiter(
+=======
+}*/
+
+
+//*****************************************************************************
+/*inline void CommunicationManager::Add_Length_and_Start_Delimiter(
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 		std::string* frame)
 {
 	const unsigned short FIVE_BYTES = 5;
@@ -355,7 +477,11 @@ inline void CommunicationManager::Add_Length_and_Start_Delimiter(
 	std::string header;
 	int frame_length = frame->size() - 1; /* frame_length = frame_size - checksum byte */
 
+<<<<<<< HEAD
 	header.push_back(START_DLIMITER);
+=======
+	/*header.push_back(START_DLIMITER);
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 	sprintf(temporary_buffer, "%04X", frame_length);
 	sscanf(temporary_buffer, "%02X%02X", &Hex_frame_length_1, 
 			&Hex_frame_length_2);
@@ -364,6 +490,7 @@ 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);
+<<<<<<< HEAD
 }
 
 
@@ -386,11 +513,26 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
 		 counter--;
 		}
 		uint64_t current_int64 = 0;
+=======
+}*/
+
+
+//*****************************************************************************
+/*inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics() // TO DO delete
+{
+	std::size_t size_in_messages = in_messages_->Get_Size();
+	
+	if (size_in_messages > 0)
+	{
+		uint64_t current_int64 = 0;
+
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 		for (std::size_t j = 0; j < size_in_messages; j++)
 		{
 			std::shared_ptr<std::string> in_message =
 					in_messages_->Pop_Front();
 			mavros_msgs::Mavlink mavlink_msg;
+<<<<<<< HEAD
 			int tot = 0;
 			//uint64_t header=0;
 			/*Copy the header*/
@@ -565,6 +707,33 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server()
 	std::size_t size_in_messages = in_messages_->Get_Size();
 	
 	if (size_in_messages > 0)
+=======
+			
+			for (std::size_t i = 0; i < in_message->size(); i++)
+			{
+				if (' ' == in_message->at(i) || 0 == i)
+				{
+					sscanf(in_message->c_str() + i, "%" PRIu64 " ",
+							&current_int64);
+					mavlink_msg.payload64.push_back(current_int64);
+				}
+		
+			}
+			
+			mavlink_publisher_.publish(mavlink_msg);
+		}
+		
+	}
+}*/
+
+
+//*****************************************************************************
+inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server() // TO DO change it
+{
+	//std::size_t size_in_messages = in_messages_->Get_Size();
+	
+	/*if (size_in_messages > 0)
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 	{
 		for (std::size_t j = 0; j < size_in_messages; j++)
 		{
@@ -596,7 +765,11 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server()
 				ROS_INFO("XBeeMav: Faild to Tranfer Command.");
 			
 		}
+<<<<<<< HEAD
 	}
+=======
+	}*/
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 }
 
 
@@ -604,6 +777,7 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server()
 inline void CommunicationManager::Send_Mavlink_Message_Callback(
 		const mavros_msgs::Mavlink::ConstPtr& mavlink_msg)
 {
+<<<<<<< HEAD
 	const unsigned short MAX_BUFFER_SIZE = 211; /* 20 (length(uint64_t)) * 10 (max int number) + 10 (spaces) + 1 */
 	const unsigned short MAX_NBR_OF_INT64 = 24;
 	char temporary_buffer[MAX_BUFFER_SIZE];
@@ -820,6 +994,12 @@ void CommunicationManager::Send_multi_msg(){
 	}
 
 }
+=======
+	std::cout << "Received Message From Buzz" << std::endl; // TO DO delete
+	packets_handler_.Handle_Mavlink_Message(mavlink_msg);
+}
+
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 
 //*****************************************************************************
 void CommunicationManager::Display_Drone_Type_and_Running_Mode(
@@ -837,9 +1017,110 @@ void CommunicationManager::Display_Drone_Type_and_Running_Mode(
 }
 
 
+<<<<<<< HEAD
 }
 
 
 }
 
 
+=======
+//*****************************************************************************
+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());
+		}
+	}
+}
+
+
+}
+
+
+}
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1

src/PacketsHandler.cpp

@@ -0,0 +1,778 @@
+/* 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;
+		
+	// TO DO node dicov
+		
+	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) // TO DO delete 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) // TO DO change useless std::shared_ptr<std::string> frame args to ->c_str()
+{
+	uint8_t packet_ID = frame->at(12);
+	uint8_t node_8_bits_address = frame->at(13);
+	
+	if (frame->at(11) == 'A')
+	{
+		/*std::lock_guard<std::mutex> guard(mutex_);
+		
+		connected_network_nodes_it_ = connected_network_nodes_.find(node_8_bits_address);
+	
+		if (connected_network_nodes_it_ == connected_network_nodes_.end())
+		{
+			Add_New_Node_To_Network(node_8_bits_address);
+			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));
+			}
+		}*/
+	}
+	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()); // TO Do send as unicast
+			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)) // TO DO check this
+		{
+			new_node_address = static_cast<uint64_t>( // TO DO correct this
+			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)) // TO DO check this
+		{
+			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)) // TO DO check this
+		{
+			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_);
+	
+	std::clock_t elapsed_time = std::clock();
+		
+	//while (NBR_of_transmission <= optimum_MT_NBR_ && !Check_Packet_Transmitted_To_All_Nodes())
+	//{
+		//NBR_of_transmission++;
+		
+		Transmit_Fragments(frames);
+		
+		// TO DO usleep(flow_control_time)
+		// TO DO clear fragments_IDs_to_transmit_set
+		Send_End_Of_Packet_Ping(packet.packet_ID_, packet.packet_buffer_->size());
+		// TO DO sleep after ping
+	//}
+	
+	elapsed_time = std::clock() - elapsed_time;
+	Adjust_Optimum_MT_Number(elapsed_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 = "/home/aymen/catkin_ws/src/xbee_ros_node/Resources/database.xml";
+	
+	if (!boost::filesystem::exists(FILE_PATH))
+	{	
+		std::cout << "database.xml Not Found." << 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_);
+	}
+			
+}
+		
+
+//*****************************************************************************
+void PacketsHandler::Adjust_Optimum_MT_Number(const std::clock_t elapsed_time,
+		const std::size_t NBR_of_transmission) 
+{
+	if (NBR_of_transmission < optimum_MT_NBR_)
+		optimum_MT_NBR_ = NBR_of_transmission;
+	else if (NBR_of_transmission == optimum_MT_NBR_)
+	{
+		std::lock_guard<std::mutex> guard(mutex_);
+		
+		if (elapsed_time <= MAX_TIME_TO_SEND_PACKET && fragments_indexes_to_transmit_.size() > 0)
+			optimum_MT_NBR_++;
+	}
+}
+
+
+//*****************************************************************************
+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);
+}
+
+
+}
+
+
+}

src/SerialDevice.cpp

@@ -39,7 +39,10 @@ bool SerialDevice::Init(
 	{
 		Set_Port_Options(baud_rate);
 		Init_Frame_Type_Keys();
+<<<<<<< HEAD
 		Read_Frame_Header();
+=======
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 		return true;
 	}
 	else
@@ -82,6 +85,10 @@ void SerialDevice::Send_Frame(const std::string& frame)
 //*****************************************************************************
 void SerialDevice::Run_Service()
 {
+<<<<<<< HEAD
+=======
+	Read_Frame_Header();
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 	io_service_.run();
 }
 
@@ -90,14 +97,35 @@ void SerialDevice::Run_Service()
 void SerialDevice::Stop_Service()
 {
 	io_service_.post([this]() {io_service_.stop(); });
+<<<<<<< HEAD
+=======
+}
+
+
+//*****************************************************************************
+void SerialDevice::Close_Serial_Port()
+{
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 	io_service_.post([this]() {serial_port_.close(); });
 }
 
 
 //*****************************************************************************
+<<<<<<< HEAD
 Thread_Safe_Deque * SerialDevice::Get_In_Messages_Pointer()
 {
 	return &in_messages_;
+=======
+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)
+{
+	in_std_messages_ = in_std_messages;
+	in_fragments_ = in_fragments;
+	in_Acks_and_Pings_ = in_Acks_and_Pings;
+	command_responses_ = command_responses;
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 }
 
 
@@ -156,8 +184,15 @@ void SerialDevice::Read_Frame_Header()
 				Read_Frame_Body();
 			}
 			else
+<<<<<<< HEAD
 				/* The header is totally corrupted, read another header. */
 				Read_Frame_Header();
+=======
+			{
+				/* The header is totally corrupted, read another header. */
+				Read_Frame_Header();
+			}
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 		}
 		else
 		{
@@ -183,6 +218,7 @@ void SerialDevice::Read_Frame_Body()
 			if (current_frame_.Get_Frame_Type() == 
 					FRAME_TYPE_KEYS[RECEIVE_PACKET])
 			{
+<<<<<<< HEAD
 				const unsigned short ELEVEN_BYTES = 11;
 				const unsigned short TWELVE_BYTES = 12;
 
@@ -192,6 +228,44 @@ void SerialDevice::Read_Frame_Body()
 					+ ELEVEN_BYTES,
 					current_frame_.Get_Frame_Body_Length() - TWELVE_BYTES);
 				in_messages_.Push_Pack(in_message);
+=======
+				char msg_type = current_frame_.Get_Message_Type();
+				std::shared_ptr<std::string> in_message =
+					std::make_shared<std::string>();
+				
+				if (msg_type == 'F')
+				{
+					in_message->append(current_frame_.Get_Frame_Body()
+					+ 11,
+					current_frame_.Get_Frame_Body_Length() - 12);
+					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);
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 			}
 
 			Read_Frame_Header();
@@ -226,6 +300,23 @@ void SerialDevice::Write_Frame()
 }
 
 
+<<<<<<< HEAD
+=======
+//*****************************************************************************
+bool SerialDevice::Is_IO_Service_Stopped()
+{
+	return io_service_.stopped();
+}
+
+
+//*****************************************************************************
+void SerialDevice::Reset_IO_Service()
+{
+	io_service_.reset();
+}
+
+
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 }
 
 

src/TestBuzz.cpp

@@ -47,15 +47,23 @@ void Init_Random_Seed()
 //*****************************************************************************
 int main(int argc, char **argv)
 {
+<<<<<<< HEAD
   const unsigned int MIN_PAYLOAD_SIZE = 1;
   const unsigned int MAX_PAYLOAD_SIZE = 10;
   
   ros::init(argc, argv, "flight_controller");
+=======
+  const unsigned int MIN_PAYLOAD_SIZE = 750;
+  const unsigned int MAX_PAYLOAD_SIZE = 752;
+  
+  ros::init(argc, argv, "test_buzz");
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 
   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);
 
+<<<<<<< HEAD
   ros::Rate loop_rate(0.2);
 
   Init_Random_Seed();
@@ -66,6 +74,21 @@ int main(int argc, char **argv)
   while (ros::ok())
   {
     mavros_msgs::Mavlink mavlink_msg_;
+=======
+  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() && std::cin.getline(line, MAX_BUFFER_SIZE))
+  {
+    mavros_msgs::Mavlink mavlink_msg_;
+    mavlink_msg_.sysid = 2;
+    mavlink_msg_.msgid = 1;
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 
     unsigned int payload_size = Get_Random_Size(MIN_PAYLOAD_SIZE, MAX_PAYLOAD_SIZE);
 
@@ -87,9 +110,14 @@ int main(int argc, char **argv)
 
     ros::spinOnce();
 
+<<<<<<< HEAD
     loop_rate.sleep();
 
     count++;
+=======
+    count++;
+    std::cout << "Press Enter to Send New Mavlink Message..." << std::endl;
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
   }
 
   return 0;

src/XBeeFrame.cpp

@@ -141,6 +141,16 @@ void Frame::Rearrange_Corrupted_Header()
 }
 
 
+<<<<<<< HEAD
+=======
+//*****************************************************************************
+char Frame::Get_Message_Type()
+{
+	return data_buffer_[15];
+}
+
+
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 }
 
 

src/XMLConfigParser.cpp

@@ -23,7 +23,11 @@ XMLConfigParser::~XMLConfigParser()
 //*****************************************************************************
 bool XMLConfigParser::Load_Config()
 {
+<<<<<<< HEAD
 	const std::string FILE_NAME = "/home/ubuntu/ROS_WS/src/xbeemav/Resources/XBee_Config.xml";
+=======
+	const std::string FILE_NAME = "/home/mistlab/catkin_ws/src/xbee_ros_node/Resources/XBee_Config.xml";
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 
 	if (Check_Config_File_Exists(FILE_NAME))
 	{

src/Xbee.cpp

@@ -17,7 +17,11 @@ int main(int argc, char* argv[])
 
 	Mist::Xbee::CommunicationManager communication_manager;
 	const std::string& device = "/dev/ttyUSB0"; // TO DO can be introduced as command.
+<<<<<<< HEAD
 	const std::size_t baud_rate = 115200; // TO DO Can be introduced as command.
+=======
+	const std::size_t baud_rate = 230400; // TO DO Can be introduced as command.
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 	Mist::Xbee::CommunicationManager::DRONE_TYPE drone_type = 
 			Mist::Xbee::CommunicationManager::DRONE_TYPE::SLAVE;
 	Mist::Xbee::CommunicationManager::RUNNING_MODE running_mode =
@@ -34,8 +38,12 @@ int main(int argc, char* argv[])
 				running_mode = Mist::Xbee::CommunicationManager::RUNNING_MODE::SWARM;
 		}
 	}
+<<<<<<< HEAD
 		
 
+=======
+	
+>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
 	if (communication_manager.Init(device, baud_rate))
 		communication_manager.Run(drone_type, running_mode);
   }