first commit

CMakeLists.txt

@@ -0,0 +1,81 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(xbee_ros_node)
+
+if(UNIX)
+    SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11")
+endif()
+
+set (BOOST_INCLUDEDIR "/usr/include")
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+ find_package(Boost REQUIRED COMPONENTS
+ system)
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+
+
+###################################
+## catkin specific configuration ##
+###################################
+
+
+catkin_package(
+  INCLUDE_DIRS include
+#  LIBRARIES xbee_ros_node
+  CATKIN_DEPENDS roscpp std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+
+## Specify additional locations of header files
+include_directories(
+  include ${xbee_ros_node_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+)
+
+
+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(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})
+
+
+
+#############
+## Install ##
+#############
+
+
+
+#############
+## Testing ##
+#############
+
+
+

Resources/XBee_Config.xml

@@ -0,0 +1,68 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<XBeeConfig>
+    <Settings>
+		<Parameter Command="CM" Description="Channel Mask">00FFFFFFFFFFFF7FFF</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>
+		<Parameter Command="PL" Description="TX Power Level">4</Parameter>
+		<Parameter Command="RR" Description="Unicast Retries">A</Parameter>
+		<Parameter Command="CE" Description="Routing/Messaging Mode">0</Parameter>
+		<Parameter Command="BH" Description="Broadcast Hops">0</Parameter>
+		<Parameter Command="NH" Description="Network Hops">7</Parameter>
+		<Parameter Command="MR" Description="Mesh Unicast Retries">1</Parameter>
+		<Parameter Command="NN" Description="Network Delay Slots">3</Parameter>
+		<Parameter Command="DH" Description="Destination Address High">0</Parameter>
+		<Parameter Command="DL" Description="Destination Address Low">FFFF</Parameter>
+		<Parameter Command="TO" Description="Transmit Options">C0</Parameter>
+		<Parameter Command="NI" Description="Node Identifier">'Node 1'</Parameter>
+		<Parameter Command="NT" Description="Network Dsicovery Back-Off">82</Parameter>
+		<Parameter Command="NO" Description="Network Dsicovery Options">0</Parameter>
+		<Parameter Command="CI" Description="Cluster ID">11</Parameter>
+		<Parameter Command="EE" Description="Encryption Enbale">0</Parameter>
+		<Parameter Command="KY" Description="Encryption Key"></Parameter>
+		<Parameter Command="BD" Description="Baud Rate">8</Parameter>
+		<Parameter Command="NB" Description="Parity">0</Parameter>
+		<Parameter Command="SB" Description="Stop Bits">0</Parameter>
+		<Parameter Command="RO" Description="Packetization Timeout">3</Parameter>
+		<Parameter Command="FT" Description="Flow Control Threshold">13F</Parameter>
+		<Parameter Command="AP" Description="API Enable">1</Parameter>
+		<Parameter Command="AO" Description="API Options">0</Parameter>
+		<Parameter Command="D0" Description="DIO0/AD0">1</Parameter>
+		<Parameter Command="D1" Description="DIO1/AD1/SPI_ATTN">0</Parameter>
+		<Parameter Command="D2" Description="DIO2/AD2/SPI_SCLK">0</Parameter>
+		<Parameter Command="D3" Description="DIO3/AD3/SPI_SSEL">0</Parameter>
+		<Parameter Command="D4" Description="DIO4">0</Parameter>
+		<Parameter Command="D5" Description="DIO5/ASSOCIATED_INDICATOR">1</Parameter>
+		<Parameter Command="D6" Description="DIO6/RTS">0</Parameter>
+		<Parameter Command="D7" Description="DIO7/CTS">1</Parameter>
+		<Parameter Command="D8" Description="DIO8/SLEEP_REQUEST">1</Parameter>
+		<Parameter Command="D9" Description="DIO9/ON_SLEEP">1</Parameter>
+		<Parameter Command="P0" Description="DIO10/RSSI/PWM0">1</Parameter>
+		<Parameter Command="P1" Description="DIO11/PWM1">0</Parameter>
+		<Parameter Command="P2" Description="DIO12">0</Parameter>
+		<Parameter Command="P3" Description="DIO13/DOUT">1</Parameter>
+		<Parameter Command="P4" Description="DIO14/DIN">1</Parameter>
+		<Parameter Command="PD" Description="Pull Direction">7FFF</Parameter>
+		<Parameter Command="PR" Description="Pull-Up/Down Resistor">7FFF</Parameter>
+		<Parameter Command="M0" Description=" PWM0 Duty Cycle">0</Parameter>
+		<Parameter Command="M1" Description="PWM1 Duty Cycle">0</Parameter>
+		<Parameter Command="LT" Description="Associate LED Blink Time">0</Parameter>
+		<Parameter Command="RP" Description="RSSI PWM Timer">28</Parameter>
+		<Parameter Command="AV" Description="Analog Voltage Reference">1</Parameter>
+		<Parameter Command="IC" Description="DIO Change Detect">0</Parameter>
+		<Parameter Command="IF" Description="Sleep Sample Rate">1</Parameter>
+		<Parameter Command="IR" Description="Sample Rate">0</Parameter>
+		<Parameter Command="SM" Description="Sleep Mode">0</Parameter>
+		<Parameter Command="SO" Description="Sleep Options">2</Parameter>
+		<Parameter Command="SN" Description="Nu,ber of Cycles between On/Sleep">1</Parameter>
+		<Parameter Command="SP" Description="Sleep Time">12C</Parameter>
+		<Parameter Command="ST" Description="Wake Time">BB8</Parameter>
+		<Parameter Command="WH" Description="Wake Host Delay">0</Parameter>
+		<Parameter Command="CC" Description="Commande Sequence Character">2B</Parameter>
+		<Parameter Command="CT" Description="Command Mode Time Out">60</Parameter>
+		<Parameter Command="GT" Description="Guard Time">3E8</Parameter>
+		<Parameter Command="DD" Description="Device Type Identifier">B0000</Parameter>
+    </Settings>
+</XBeeConfig>

include/CommunicationManager.h

@@ -0,0 +1,97 @@
+/* CommunicationManager.h -- Communication Manager class for XBee:
+			     Handles all communications with other ROS nodes
+			     and the serial port --     	             */
+/* ------------------------------------------------------------------------- */
+/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved.      */
+/*                                  (aymen.soussia@gmail.com)                */
+
+
+#pragma once
+
+#include <inttypes.h>
+#include<thread>
+
+#include<mavros_msgs/CommandCode.h>
+#include<mavros_msgs/CommandInt.h>
+#include<mavros_msgs/Mavlink.h>
+#include <ros/ros.h>
+
+#include"SerialDevice.h"
+
+
+namespace Mist
+{
+
+
+namespace Xbee
+{
+
+
+//*****************************************************************************
+struct Waypoint_S
+{
+	unsigned int latitude;
+	unsigned int longitude;
+	double altitude;
+	unsigned int staytime;
+	unsigned int heading;
+};
+
+
+//*****************************************************************************
+class CommunicationManager
+{
+public:
+	CommunicationManager();
+	~CommunicationManager();
+
+	enum class DRONE_TYPE {MASTER, SLAVE};
+	enum class RUNNING_MODE {SWARM, SOLO};
+
+	bool Init(const std::string& device, const std::size_t baud_rate);
+	void Run(DRONE_TYPE drone_type, RUNNING_MODE running_mode);
+
+private:
+
+	const unsigned char START_DLIMITER;
+	const std::size_t LOOP_RATE;
+
+	void Run_In_Solo_Mode(DRONE_TYPE drone_type);
+	void Run_In_Swarm_Mode();
+	void Generate_Transmit_Request_Frame(
+			const char* const message,
+			std::string* frame,
+			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 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);
+	void Send_Mavlink_Message_Callback(
+			const mavros_msgs::Mavlink::ConstPtr& mavlink_msg);
+	void Display_Drone_Type_and_Running_Mode(DRONE_TYPE drone_type,
+			RUNNING_MODE running_mode);
+	bool Serve_Flight_Controller(mavros_msgs::CommandInt::
+		Request& request, mavros_msgs::CommandInt::Response& response);
+	void Check_In_Messages_and_Transfer_To_Server();
+
+	Mist::Xbee::SerialDevice serial_device_;
+	Thread_Safe_Deque* in_messages_;
+	ros::NodeHandle node_handle_;
+	ros::Subscriber mavlink_subscriber_;	
+	ros::Publisher mavlink_publisher_;
+	ros::ServiceClient mav_dji_client_;
+	ros::ServiceServer mav_dji_server_;
+};
+
+
+}
+
+
+}

include/MultithreadingDeque.hpp

@@ -0,0 +1,73 @@
+/* MultithreadingDeque.hpp -- Safe Multithreading Deque class --             */
+/* ------------------------------------------------------------------------- */
+/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved.      */
+/*                                  (aymen.soussia@gmail.com)                */
+
+
+#pragma once
+
+#include<deque>
+#include<mutex>
+
+
+namespace Mist
+{
+
+
+namespace Xbee
+{
+
+
+//*****************************************************************************
+template<typename _T>
+class MultithreadingDeque
+{
+public:
+	//****************************************************************************
+	MultithreadingDeque()
+	{
+	}
+
+
+	//****************************************************************************
+	~MultithreadingDeque()
+	{
+	}
+
+
+	//****************************************************************************
+	void Push_Pack(const _T& new_data)
+	{
+		std::lock_guard<std::mutex> guard(mutex_);
+		deque_.push_back(new_data);
+	}
+
+
+	//****************************************************************************
+	_T Pop_Front()
+	{
+		std::lock_guard<std::mutex> guard(mutex_);
+		_T value = deque_.front();
+		deque_.pop_front();
+		return value;
+	}
+
+
+	//****************************************************************************
+	unsigned int Get_Size()
+	{
+		std::lock_guard<std::mutex> guard(mutex_);
+		return deque_.size();
+	}
+
+private:
+	
+	std::mutex mutex_;
+	std::deque<_T> deque_;
+};
+
+
+}
+
+
+}

include/SerialDevice.h

@@ -0,0 +1,83 @@
+/* SerialDevice.h -- Serial Device class to handle serial communications
+                     with XBee --                                            */
+/* ------------------------------------------------------------------------- */
+/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved.      */
+/*                                  (aymen.soussia@gmail.com)                */
+
+
+#pragma once
+
+#include<deque>
+#include<iostream>
+#include<memory> 
+
+#include<boost/asio.hpp>
+
+#include"MultithreadingDeque.hpp"
+#include"XBeeFrame.h"
+
+
+namespace Mist
+{
+
+
+namespace Xbee
+{
+
+
+typedef MultithreadingDeque<std::shared_ptr<std::string>> Thread_Safe_Deque;
+
+
+//*****************************************************************************
+class SerialDevice
+{
+public:
+	SerialDevice();
+	~SerialDevice();
+
+	bool Init(const std::string& device, const std::size_t baud_rate);
+	void Send_Frame(const std::string& frame);
+	void Run_Service();
+	void Stop_Service();
+	Thread_Safe_Deque* Get_In_Messages_Pointer();
+
+private:
+
+	enum FRAME_TYPE
+	{
+		AT_COMMAND = 0,
+		AT_COMMAND_QUEUE_REGISTER_VALUE = 1,
+		TRANSMIT_REQUEST = 2,
+		EXPLICIT_ADDRESSING_COMMAND_FRAME = 3,
+		REMOTE_AT_COMMAND = 4,
+		AT_COMMAND_RESPONSE = 5,
+		MODEM_STATUS = 6,
+		TRANSMIT_STATUS = 7,
+		ROUTE_INFORMATION_PACKET = 8,
+		AGGREGATE_ADDRESSING_UPDATE = 9,
+		RECEIVE_PACKET = 10,
+		EXPLICIT_RX_INDICATOR = 11,
+		IO_DATA_SAMPLE_RX_INDICATOR = 12,
+		NODE_IDENTIFICATION_INDICATOR = 13,
+		REMOTE_AT_COMMAND_RESPONSE = 14
+	};
+
+	void Init_Frame_Type_Keys();
+	void Set_Port_Options(const std::size_t baud_rate);
+	void Read_Frame_Header();
+	void Read_Frame_Body();
+	void Write_Frame();
+
+	boost::asio::io_service io_service_;
+	boost::asio::serial_port serial_port_;
+	std::deque<std::string> out_messages_;
+	Thread_Safe_Deque in_messages_;
+	Mist::Xbee::Frame current_frame_;
+	unsigned int FRAME_TYPE_KEYS[REMOTE_AT_COMMAND_RESPONSE + 1];
+};
+
+
+}
+
+
+}

include/XBeeFrame.h

@@ -0,0 +1,56 @@
+/* XBeeFrame.cpp -- XBee Frame class --                                      */
+/* ------------------------------------------------------------------------- */
+/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved.      */
+/*                                  (aymen.soussia@gmail.com)                */
+
+
+#pragma once
+
+#include<cstdio>
+#include<cstdlib>
+#include<cstring>
+
+
+namespace Mist
+{
+
+
+namespace Xbee
+{
+
+
+//*****************************************************************************
+class Frame
+{
+public:
+	Frame();
+	~Frame();
+
+	enum {FRAME_HEADER_LENGTH = 4};
+	enum {MAX_FRAME_BODY_LENGTH = 280}; // TO DO check value
+
+	const char* Get_Frame_Data() const;
+	char* Get_Frame_Data();
+	std::size_t Get_Frame_Length() const;
+	const char* Get_Frame_Body() const;
+	char* Get_Frame_Body();
+	std::size_t Get_Frame_Body_Length() const;
+	bool Decode_Frame_Header();
+	std::size_t Get_Frame_Type() const;
+	int Get_Start_Delimiter_Position();
+	void Rearrange_Corrupted_Header();
+
+
+private:
+
+	char data_buffer_[FRAME_HEADER_LENGTH + MAX_FRAME_BODY_LENGTH];
+	std::size_t frame_body_length_;
+	unsigned int frame_type_;
+	int start_delimiter_position_;
+};
+
+
+}
+
+
+}

include/XBeeModule.h

@@ -0,0 +1,65 @@
+/* XBeeModule.h -- Xbee Module class provides functions to communicate 
+                   with Xbee  --                                             */
+/* ------------------------------------------------------------------------- */
+/* November 30, 2016 -- @Copyright Aymen Soussia. All rights reserved.       */
+/*                                  (aymen.soussia@gmail.com)                */
+
+
+#pragma once
+
+#include<deque>
+#include<iostream>
+#include<unistd.h>
+
+#include<boost/asio.hpp>
+#include<boost/bind.hpp>
+
+#include"XBeeParameter.h"
+
+
+//*****************************************************************************
+using namespace boost;
+
+
+//*****************************************************************************
+typedef std::deque < std::string > commands_queue;
+
+
+//*****************************************************************************
+class XBeeModule
+{
+public:
+	XBeeModule();
+	~XBeeModule();
+
+	enum {MAX_MSG_LENGTH = 8};
+
+	bool Init_Port(const std::string& device_port, const unsigned int baud_rate);
+	void Run_Service();
+	void Send_Data(const std::string& command);
+	void Exit_AT_Command_Mode();
+	bool Is_Connected();
+	bool Check_Time_Out_Exceeded();
+	void Format_AT_Command(const XBee_Parameter_S& parameter, std::string* command);
+
+private:
+
+	const unsigned short TIME_OUT;
+
+	bool Open_Port(const std::string& device_port);
+	void Start_Receive();
+	void Handle_Receive(const system::error_code& error, size_t bytes_transferred);
+	void Set_AT_Command_Mode();
+	void Check_Time_Out(const system::error_code& error);
+	void Do_Write();
+	void Handle_Write(const system::error_code& error);
+
+	asio::io_service io_service_;
+	asio::serial_port serial_port_;
+	asio::deadline_timer timer_;
+	char receive_buffer_[MAX_MSG_LENGTH];
+	bool connected_to_XBee_;
+	bool time_out_exceeded_;
+	commands_queue commands_sequence_;
+};
+

include/XBeeParameter.h

@@ -0,0 +1,17 @@
+/* XBee_Parameter.h -- Xbee command struct  --                               */
+/* ------------------------------------------------------------------------- */
+/* November 30, 2016 -- @Copyright Aymen Soussia. All rights reserved.       */
+/*                                  (aymen.soussia@gmail.com)                */
+
+
+#pragma once
+
+#include<string>
+
+
+//*****************************************************************************
+struct XBee_Parameter_S
+{
+	std::string command_;
+	std::string value_;
+};

include/XMLConfigParser.h

@@ -0,0 +1,41 @@
+/* XMLConfigParser.h -- XML Config Parser class  --                          */
+/* ------------------------------------------------------------------------- */
+/* November 30, 2016 -- @Copyright Aymen Soussia. All rights reserved.       */
+/*                                  (aymen.soussia@gmail.com)                */
+
+
+#pragma once
+
+#include<iostream>
+
+#include<boost/property_tree/ptree.hpp>
+#include<boost/property_tree/xml_parser.hpp>
+#include<boost/foreach.hpp>
+#include<boost/filesystem.hpp>
+
+#include"XBeeParameter.h"
+
+
+//*****************************************************************************
+using boost::property_tree::ptree;
+
+
+//*****************************************************************************
+class XMLConfigParser
+{
+public:
+	XMLConfigParser();
+	~XMLConfigParser();
+
+	bool Load_Config();
+	std::vector<XBee_Parameter_S>* Get_Loaded_Parameters();
+	bool Is_Config_Loaded_Successfully();
+
+private:
+
+	bool Check_Config_File_Exists(const std::string& file_name);
+
+	std::vector<XBee_Parameter_S> xbee_parameters_;
+	bool config_loaded_successfully_;
+};
+

launch/xbeemav.launch

@@ -0,0 +1,12 @@
+<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="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" />
+</launch>

package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<package>
+  <name>xbee_ros_node</name>
+  <version>1.0.2</version>
+  <description>The xbee_ros_node package</description>
+
+  <maintainer email="aymen.soussia@gmail.com">aymen</maintainer>
+
+  <license>Boost Software License</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+  <export>
+  </export>
+</package>

src/CommunicationManager.cpp

@@ -0,0 +1,442 @@
+/* CommunicationManager.cpp -- Communication Manager class for XBee:
+			       Handles all communications with other ROS nodes
+			       and the serial port --     	                             */
+/* ------------------------------------------------------------------------- */
+/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved.      */
+/*                                  (aymen.soussia@gmail.com)                */
+
+
+#include "CommunicationManager.h"
+
+
+namespace Mist
+{
+
+
+namespace Xbee
+{
+
+
+//*****************************************************************************
+CommunicationManager::CommunicationManager():
+	START_DLIMITER(static_cast<unsigned char>(0x7E)),
+	LOOP_RATE(10) /* 10 fps */
+{
+}
+
+
+//*****************************************************************************
+CommunicationManager::~CommunicationManager()
+{
+}
+
+
+//*****************************************************************************
+bool CommunicationManager::Init(
+		const std::string& device, const std::size_t baud_rate)
+{
+	if (serial_device_.Init(device, baud_rate))
+	{
+		in_messages_ = serial_device_.Get_In_Messages_Pointer();
+		return true;
+	}
+	else
+	{
+		Display_Init_Communication_Failure();
+		return false;
+	}
+}
+
+
+
+//*****************************************************************************
+void CommunicationManager::Run(DRONE_TYPE drone_type,
+		RUNNING_MODE running_mode)
+{
+	std::thread thread_service(&SerialDevice::Run_Service, &serial_device_);
+
+	Display_Drone_Type_and_Running_Mode(drone_type, running_mode);
+
+	if (RUNNING_MODE::SWARM == running_mode)
+		Run_In_Swarm_Mode();
+	else
+		Run_In_Solo_Mode(drone_type);
+
+	serial_device_.Stop_Service();
+  	thread_service.join();
+}
+
+
+//*****************************************************************************
+void CommunicationManager::Run_In_Solo_Mode(DRONE_TYPE drone_type)
+{
+	std::string service_name;
+	bool success = false;
+
+	if (DRONE_TYPE::MASTER == drone_type)
+	{
+		if (node_handle_.getParam("Xbee_In_From_Controller", service_name))
+		{
+			mav_dji_server_ = node_handle_.advertiseService(service_name.c_str(), 		&CommunicationManager::Serve_Flight_Controller, this);
+			success = true;
+		}
+		else
+			std::cout << "Failed to Get Service Name: param 'Xbee_In_From_Controller' Not Found." << std::endl;
+	}
+	else
+ 	{
+		if (node_handle_.getParam("Xbee_Out_To_Controller", service_name))
+		{
+			mav_dji_client_ = node_handle_.serviceClient<mavros_msgs::CommandInt>(service_name.c_str());
+			success = true;
+		}
+		else
+			std::cout << "Failed to Get Service Name: param 'Xbee_Out_To_Controller' Not Found." << std::endl;
+	}
+
+	if (success)
+	{
+		ros::Rate loop_rate(LOOP_RATE);
+
+		while (ros::ok())
+		{
+			Check_In_Messages_and_Transfer_To_Server();
+			ros::spinOnce();
+	    		loop_rate.sleep();
+		}
+	}
+
+}
+
+
+//*****************************************************************************
+void CommunicationManager::Run_In_Swarm_Mode()
+{
+	std::string out_messages_topic;
+	std::string in_messages_topic;
+	bool success_1 = false;
+	bool success_2 = false;
+
+	if (node_handle_.getParam("Xbee_In_From_Buzz", out_messages_topic))
+	{
+		mavlink_subscriber_ = node_handle_.subscribe(out_messages_topic.c_str(), 1000,
+			&CommunicationManager::Send_Mavlink_Message_Callback, this);
+		success_1 = true;
+	}
+	else
+		std::cout << "Failed to Get Topic Name: param 'Xbee_In_From_Buzz' Not Found." << std::endl;
+
+	if (node_handle_.getParam("Xbee_Out_To_Buzz", in_messages_topic))
+	{
+		mavlink_publisher_ = node_handle_.advertise<mavros_msgs::Mavlink>(
+			in_messages_topic.c_str(), 1000);
+		success_2 = true;
+	}
+	else
+		std::cout << "Failed to Get Topic Name: param 'Xbee_Out_To_Buzz' Not Found." << std::endl;
+
+	if (success_1 && success_2)
+	{
+	
+		ros::Rate loop_rate(LOOP_RATE);
+
+		while (ros::ok())
+		{
+			Check_In_Messages_and_Transfer_To_Topics();
+			ros::spinOnce();
+	    		loop_rate.sleep();
+		}
+	}	
+}
+
+
+//*****************************************************************************
+inline bool CommunicationManager::Serve_Flight_Controller(mavros_msgs::CommandInt::
+		Request& request, mavros_msgs::CommandInt::Response& response)
+{
+	const std::size_t MAX_BUFFER_SIZE = 255;
+	unsigned int command = 0;
+	char temporary_buffer[MAX_BUFFER_SIZE];
+	std::string frame;
+
+	switch(request.command)
+	{
+		case mavros_msgs::CommandCode::NAV_TAKEOFF:
+		{
+			command = static_cast<unsigned int>(mavros_msgs::CommandCode::NAV_TAKEOFF);
+			sprintf(temporary_buffer, "%u ", command);
+			response.success = true;
+			break;
+		}
+		case mavros_msgs::CommandCode::NAV_LAND:
+		{
+			command = static_cast<unsigned int>(mavros_msgs::CommandCode::NAV_LAND);
+			sprintf(temporary_buffer, "%u ", command);
+			response.success = true;
+			break;	
+		}
+		case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
+		{
+			command = static_cast<unsigned int>(mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH);
+			sprintf(temporary_buffer, "%u ", command);
+			response.success = true;
+			break;	
+		}
+		case mavros_msgs::CommandCode::NAV_WAYPOINT:
+		{
+			command = static_cast<unsigned int>(mavros_msgs::CommandCode::NAV_WAYPOINT);
+			sprintf(temporary_buffer, "%u %u %u %lf %u %u ",
+					command, request.x, request.y, request.z, 0, 1); // TO DO change 0 and 1
+			response.success = true;
+			break;	
+		}
+		case mavros_msgs::CommandCode::CMD_MISSION_START:
+		{
+			command = static_cast<unsigned int>(mavros_msgs::CommandCode::CMD_MISSION_START);
+			sprintf(temporary_buffer, "%u ", command);
+			response.success = true;
+			break;
+		}
+		default:
+			response.success = false;		
+	}
+
+	if (command != 0)
+	{
+		Generate_Transmit_Request_Frame(temporary_buffer, &frame);
+		serial_device_.Send_Frame(frame);
+	}
+	
+	return true;
+}
+
+
+//*****************************************************************************
+void CommunicationManager::Display_Init_Communication_Failure()
+{
+	std::cout << "Failed to Init Communication With XBee." << std::endl;
+	std::cout << "Please Check The Following Parameters:" << std::endl;
+	std::cout << "   1) Device (e.g. /dev/ttyUSB0 for Linux. " << std::endl;
+	std::cout << "   2) Baud Rate." << std::endl;
+	std::cout << "   3) Press Reset Button on The XBee." << std::endl;
+	std::cout << "   4) Connect and Disconnect The XBee." << std::endl;
+	std::cout << "   5) Update The XBee Firmware." << std::endl;
+	std::cout << "   6) Reinstall The FTDI Driver." << std::endl;
+}
+
+
+//*****************************************************************************
+inline void CommunicationManager::Generate_Transmit_Request_Frame(
+		const char* const message,
+		std::string * frame,
+		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, std::strlen(message));
+
+	Calculate_and_Append_Checksum(frame);
+	Add_Length_and_Start_Delimiter(frame);
+}
+
+
+//*****************************************************************************
+inline void CommunicationManager::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);
+	}
+}
+
+
+//*****************************************************************************
+inline void CommunicationManager::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 CommunicationManager::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 CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
+{
+	std::size_t size_in_messages = in_messages_->Get_Size();
+	
+	if (size_in_messages > 0)
+	{
+		uint64_t current_int64 = 0;
+
+		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;
+			
+			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()
+{
+	std::size_t size_in_messages = in_messages_->Get_Size();
+	
+	if (size_in_messages > 0)
+	{
+		for (std::size_t j = 0; j < size_in_messages; j++)
+		{
+			std::shared_ptr<std::string> in_message =
+					in_messages_->Pop_Front();
+			mavros_msgs:: CommandInt command_msg;
+			unsigned int command = 0;
+
+			sscanf(in_message->c_str(), "%u", &command);
+
+			if (static_cast<unsigned int>(mavros_msgs::CommandCode::NAV_WAYPOINT) == command)
+			{
+				Waypoint_S new_waypoint;
+				sscanf(in_message->c_str(), "%u %u %u %lf %u %u ",
+					&command, &new_waypoint.latitude,
+					&new_waypoint.longitude, &new_waypoint.altitude,
+					&new_waypoint.staytime, &new_waypoint.heading);
+				command_msg.request.x = new_waypoint.latitude;
+				command_msg.request.y = new_waypoint.longitude;
+				command_msg.request.z = new_waypoint.altitude;
+				// TO DO add staytime and heading;
+			}
+
+			command_msg.request.command = command;
+			
+			if (mav_dji_client_.call(command_msg))
+				ROS_INFO("XBeeMav: Command Successfully Tranferred to Dji Mav.");
+			else
+				ROS_INFO("XBeeMav: Faild to Tranfer Command.");
+			
+		}
+	}
+}
+
+
+//*****************************************************************************
+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 */
+	const unsigned short MAX_NBR_OF_INT64 = 10;
+	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);
+	}
+}
+
+
+//*****************************************************************************
+void CommunicationManager::Display_Drone_Type_and_Running_Mode(
+		DRONE_TYPE drone_type, RUNNING_MODE running_mode)
+{
+	if (DRONE_TYPE::MASTER == drone_type)
+		std::cout << "Drone: MASTER" << std::endl;
+	else
+		std::cout << "Drone: SLAVE" << std::endl;
+
+	if (RUNNING_MODE::SOLO == running_mode)
+		std::cout << "XBeeMav Running in SOLO Mode..." << std::endl;
+	else
+		std::cout << "XBeeMav Running in SWARM Mode..." << std::endl;
+}
+
+
+}
+
+
+}

src/SerialDevice.cpp

@@ -0,0 +1,232 @@
+/* SerialDevice.cpp -- Serial Device class to handle serial communications
+                       with XBee --                                          */
+/* ------------------------------------------------------------------------- */
+/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved.      */
+/*                                  (aymen.soussia@gmail.com)                */
+
+
+#include "SerialDevice.h"
+
+
+namespace Mist
+{
+
+
+namespace Xbee
+{
+
+
+//*****************************************************************************
+SerialDevice::SerialDevice():
+	serial_port_(io_service_)
+{
+}
+
+
+//*****************************************************************************
+SerialDevice::~SerialDevice()
+{
+}
+
+
+//*****************************************************************************
+bool SerialDevice::Init(
+		const std::string & device, const std::size_t baud_rate)
+{
+	serial_port_.open(device);
+	
+	if (serial_port_.is_open())
+	{
+		Set_Port_Options(baud_rate);
+		Init_Frame_Type_Keys();
+		Read_Frame_Header();
+		return true;
+	}
+	else
+	{
+		std::cout << "Port Failed To Open." << std::endl;
+		return false;
+	}
+}
+
+
+//*****************************************************************************
+void SerialDevice::Set_Port_Options(const std::size_t baud_rate)
+{
+	serial_port_.set_option(boost::asio::serial_port::baud_rate(baud_rate));
+	serial_port_.set_option(boost::asio::serial_port::character_size(8));
+	serial_port_.set_option(boost::asio::serial_port::parity(
+			boost::asio::serial_port::serial_port_base::parity::none));
+	serial_port_.set_option(boost::asio::serial_port::stop_bits(
+			boost::asio::serial_port::serial_port_base::stop_bits::one));
+	serial_port_.set_option(boost::asio::serial_port::flow_control(
+			boost::asio::serial_port::serial_port_base::flow_control::none));
+}
+
+
+//*****************************************************************************
+void SerialDevice::Send_Frame(const std::string& frame)
+{
+	io_service_.post(
+		[this, frame]()
+	{
+		bool write_in_progress = !out_messages_.empty();
+		out_messages_.push_back(frame);
+		
+		if (!write_in_progress)
+			Write_Frame();
+	});
+}
+
+
+//*****************************************************************************
+void SerialDevice::Run_Service()
+{
+	io_service_.run();
+}
+
+
+//*****************************************************************************
+void SerialDevice::Stop_Service()
+{
+	io_service_.post([this]() {io_service_.stop(); });
+	io_service_.post([this]() {serial_port_.close(); });
+}
+
+
+//*****************************************************************************
+Thread_Safe_Deque * SerialDevice::Get_In_Messages_Pointer()
+{
+	return &in_messages_;
+}
+
+
+//*****************************************************************************
+void SerialDevice::Init_Frame_Type_Keys()
+{
+	sscanf("08", "%02X", &FRAME_TYPE_KEYS[AT_COMMAND]);
+	sscanf("09", "%02X", &FRAME_TYPE_KEYS[AT_COMMAND_QUEUE_REGISTER_VALUE]);
+	sscanf("10", "%02X", &FRAME_TYPE_KEYS[TRANSMIT_REQUEST]);
+	sscanf("11", "%02X",
+			&FRAME_TYPE_KEYS[EXPLICIT_ADDRESSING_COMMAND_FRAME]);
+	sscanf("17", "%02X", &FRAME_TYPE_KEYS[REMOTE_AT_COMMAND]);
+	sscanf("88", "%02X", &FRAME_TYPE_KEYS[AT_COMMAND_RESPONSE]);
+	sscanf("8A", "%02X", &FRAME_TYPE_KEYS[MODEM_STATUS]);
+	sscanf("8B", "%02X", &FRAME_TYPE_KEYS[TRANSMIT_STATUS]);
+	sscanf("8D", "%02X", &FRAME_TYPE_KEYS[ROUTE_INFORMATION_PACKET]);
+	sscanf("8E", "%02X", &FRAME_TYPE_KEYS[AGGREGATE_ADDRESSING_UPDATE]);
+	sscanf("90", "%02X", &FRAME_TYPE_KEYS[RECEIVE_PACKET]);
+	sscanf("91", "%02X", &FRAME_TYPE_KEYS[EXPLICIT_RX_INDICATOR]);
+	sscanf("92", "%02X", &FRAME_TYPE_KEYS[IO_DATA_SAMPLE_RX_INDICATOR]);
+	sscanf("95", "%02X", &FRAME_TYPE_KEYS[NODE_IDENTIFICATION_INDICATOR]);
+	sscanf("97", "%02X", &FRAME_TYPE_KEYS[REMOTE_AT_COMMAND_RESPONSE]);
+}
+
+
+//*****************************************************************************
+void SerialDevice::Read_Frame_Header()
+{
+	boost::asio::async_read(serial_port_,
+		boost::asio::buffer(current_frame_.Get_Frame_Data(),
+			Xbee::Frame::FRAME_HEADER_LENGTH),
+		[this](boost::system::error_code error, std::size_t)
+	{
+		if (!error)
+		{
+			int start_delimiter_position = 
+				current_frame_.Get_Start_Delimiter_Position();
+			
+			if (start_delimiter_position >= 0)
+			{
+				if (0 == start_delimiter_position)
+					current_frame_.Decode_Frame_Header();
+				else
+				{
+					/* The header is corrupted. */
+					boost::asio::async_read(serial_port_,
+						boost::asio::buffer(current_frame_.Get_Frame_Data(),
+						start_delimiter_position),
+						[this](boost::system::error_code error, std::size_t)
+					{
+						current_frame_.Rearrange_Corrupted_Header();
+						current_frame_.Decode_Frame_Header();
+					});
+				}
+
+				Read_Frame_Body();
+			}
+			else
+				/* The header is totally corrupted, read another header. */
+				Read_Frame_Header();
+		}
+		else
+		{
+			std::cout << "Error Occured:" << std::endl;
+			std::cout << error << std::endl;
+			std::cout << "Communication With XBee is Lost." << std::endl;
+			serial_port_.close();
+		}
+	});
+}
+
+
+//*****************************************************************************
+void SerialDevice::Read_Frame_Body()
+{
+	boost::asio::async_read(serial_port_,
+		boost::asio::buffer(current_frame_.Get_Frame_Body(),
+			current_frame_.Get_Frame_Body_Length()),
+		[this](boost::system::error_code error, std::size_t)
+	{
+		if (!error)
+		{
+			if (current_frame_.Get_Frame_Type() == 
+					FRAME_TYPE_KEYS[RECEIVE_PACKET])
+			{
+				const unsigned short ELEVEN_BYTES = 11;
+				const unsigned short TWELVE_BYTES = 12;
+
+				std::shared_ptr<std::string> in_message =
+					std::make_shared<std::string>();
+				in_message->append(current_frame_.Get_Frame_Body()
+					+ ELEVEN_BYTES,
+					current_frame_.Get_Frame_Body_Length() - TWELVE_BYTES);
+				in_messages_.Push_Pack(in_message);
+			}
+
+			Read_Frame_Header();
+		}
+		else
+		{
+			std::cout << "Error Occured:" << std::endl;
+			std::cout << error << std::endl;
+			std::cout << "Communication With XBee is Lost." << std::endl;
+			serial_port_.close();
+		}
+	});
+}
+
+
+//*****************************************************************************
+void SerialDevice::Write_Frame()
+{
+	boost::asio::async_write(serial_port_,
+		boost::asio::buffer(out_messages_.front().data(),
+			out_messages_.front().size()),
+		[this](boost::system::error_code error,
+			std::size_t transferred_bytes)
+	{
+		if (!error)
+		{
+			out_messages_.pop_front();
+			if (!out_messages_.empty())
+				Write_Frame();
+		}
+	});
+}
+
+
+}
+
+
+}

src/TestBuzz.cpp

@@ -0,0 +1,96 @@
+/* RosBuzz.cpp -- Basic ROS Buzz node --               */
+/* ------------------------------------------------------------------------- */
+/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved.      */
+/*                                  (aymen.soussia@gmail.com)                */
+
+
+#include <climits>
+#include <stdlib.h> 
+#include <time.h> 
+
+#include<mavros_msgs/Mavlink.h>
+#include <ros/ros.h>
+
+
+
+//*****************************************************************************
+void Receive_Payload_Callback(const mavros_msgs::Mavlink::ConstPtr& mavlink_msg)
+{
+	std::cout << "Received Mavlink Message:" << std::endl;
+	for (unsigned short i = 0; i < mavlink_msg->payload64.size(); i++)
+		std::cout << mavlink_msg->payload64.at(i) << std::endl;
+	std::cout << std::endl;
+}
+
+
+//*****************************************************************************
+unsigned int Get_Random_Size(unsigned int min, unsigned int max)
+{
+	return rand() % (max - min) + min;
+}
+
+
+//*****************************************************************************
+unsigned long long Get_Random_Int64(unsigned long long min, unsigned long long max)
+{
+	return rand() % (max - min) + min;
+}
+
+
+//*****************************************************************************
+void Init_Random_Seed()
+{
+	srand (time(NULL));
+}
+
+
+//*****************************************************************************
+int main(int argc, char **argv)
+{
+  const unsigned int MIN_PAYLOAD_SIZE = 1;
+  const unsigned int MAX_PAYLOAD_SIZE = 10;
+  
+  ros::init(argc, argv, "flight_controller");
+
+  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;
+  
+
+  while (ros::ok())
+  {
+    mavros_msgs::Mavlink mavlink_msg_;
+
+    unsigned int payload_size = Get_Random_Size(MIN_PAYLOAD_SIZE, MAX_PAYLOAD_SIZE);
+
+    for (unsigned int i = 0; i < payload_size; i++)
+    {
+      mavlink_msg_.payload64.push_back(Get_Random_Int64(1, ULLONG_MAX));
+    }
+
+    std::cout << "Sent Mavlink Message  " << count << std::endl;
+
+    for (unsigned int i = 0; i < payload_size; i++)
+    {
+      std::cout << mavlink_msg_.payload64.at(i) << std::endl;
+    }
+
+    std::cout << std::endl;
+
+    mavlink_pub.publish(mavlink_msg_);
+
+    ros::spinOnce();
+
+    loop_rate.sleep();
+
+    count++;
+  }
+
+  return 0;
+}

src/TestController.cpp

@@ -0,0 +1,212 @@
+/* FlightController.cpp -- Basic Flight Controller ROS node --               */
+/* ------------------------------------------------------------------------- */
+/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved.      */
+/*                                  (aymen.soussia@gmail.com)                */
+
+
+#include<climits>
+#include<cstdlib> 
+#include<ctime> 
+#include<iostream>
+
+#include<mavros_msgs/CommandCode.h>
+#include<mavros_msgs/CommandInt.h>
+#include<mavros_msgs/Mavlink.h>
+#include <ros/ros.h>
+
+
+enum DRONE_TYPE {MASTER, SLAVE};
+const std::size_t LOOP_RATE = 10; /* (10 fps) */
+
+
+//*****************************************************************************
+float Get_Random_Float(float min, float max)
+{
+	return min + static_cast <float> (rand()) /( static_cast <float> (RAND_MAX/(max - min)));
+}
+
+
+//*****************************************************************************
+unsigned long int Get_Random_Unsigned_Int(unsigned long int min, unsigned long int max)
+{
+	return rand() % (max - min) + min;
+}
+
+
+//*****************************************************************************
+bool Fill_Command_Message(mavros_msgs::CommandInt* cmd_msg, const unsigned int command_ID)
+{
+	switch (command_ID)
+	{
+		case 7:
+			cmd_msg->request.command = mavros_msgs::CommandCode::NAV_TAKEOFF;
+			return true;
+		case 8:
+			cmd_msg->request.command = mavros_msgs::CommandCode::NAV_LAND;
+			return true;
+		case 9:
+			cmd_msg->request.command = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
+			return true;
+		case 21:
+			cmd_msg->request.command = mavros_msgs::CommandCode::NAV_WAYPOINT;
+			cmd_msg->request.x = Get_Random_Unsigned_Int(0, ULONG_MAX);
+			cmd_msg->request.y = Get_Random_Unsigned_Int(0, ULONG_MAX);
+			cmd_msg->request.z = Get_Random_Float(0, FLT_MAX);
+			return true;
+		case 22:
+			cmd_msg->request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
+			return true;
+		case 23:
+			cmd_msg->request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
+			cmd_msg->request.param1 = 666;
+			return true;
+		default:
+			return false;
+	}
+}
+
+
+//*****************************************************************************
+void Init_Random_Seed()
+{
+	srand (time(NULL));
+}
+
+
+//*****************************************************************************
+bool Serve_XBee(mavros_msgs::CommandInt::Request& request,
+		 mavros_msgs::CommandInt::Response& response)
+{
+	switch(request.command)
+	{
+		case mavros_msgs::CommandCode::NAV_TAKEOFF:
+			response.success = true;
+			std::cout << "Received TakeOff Command" << std::endl;
+			return true;
+		case mavros_msgs::CommandCode::NAV_LAND:
+			response.success = true;
+			std::cout << "Received Land Command" << std::endl;
+			return true;	
+		case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
+			response.success = true;
+			std::cout << "Received Return To Launch Command" << std::endl;
+			return true;	
+		case mavros_msgs::CommandCode::NAV_WAYPOINT:
+			std::cout << "Received New Waypoint Command" << std::endl;
+			std::cout << "	Latitude  " << request.x << std::endl;
+			std::cout << "	Longitude  " <<  request.y << std::endl;
+			std::cout << "	Altitude  " <<  request.z << std::endl;
+			std::cout << "	Staytime  " <<  std::endl;
+			std::cout << "	Heading  " <<  std::endl;
+			response.success = true;
+			return true;	
+		case mavros_msgs::CommandCode::CMD_MISSION_START:
+			std::cout << "Received Mission Start Command" << std::endl;
+			response.success = true;
+			return true;
+		default:
+			std::cout << "Unknown Command" << std::endl;
+			response.success = false;
+			return false;		
+	}
+
+}
+
+
+//*****************************************************************************
+void Run_As_Master_Drone()
+{
+  
+
+  Init_Random_Seed();
+  
+  const std::size_t MAX_BUFFER_SIZE = 64;
+  char line[MAX_BUFFER_SIZE];
+  unsigned int command_ID = 0;
+  const unsigned int MIN_COMMAND_ID = 1;
+  const unsigned int MAX_COMMAND_ID = 32;
+  ros::NodeHandle node_handle;
+  ros::ServiceClient mav_dji_client_ = node_handle.serviceClient<mavros_msgs::CommandInt>("xbee_cmd");
+
+  std::cout << "Enter a Command ID Between 1 and 32" << std::endl;
+
+  ros::Rate loop_rate(LOOP_RATE);
+
+  while (ros::ok() && std::cin.getline(line, MAX_BUFFER_SIZE))
+  {
+    sscanf(line, "%u", &command_ID);
+
+    if (command_ID >= MIN_COMMAND_ID && command_ID <= MAX_COMMAND_ID)
+    {
+	mavros_msgs::CommandInt new_command;
+	Fill_Command_Message(&new_command, command_ID);
+
+	if (mav_dji_client_.call(new_command))
+		std::cout << "Sent Command." << std::endl;
+	else
+		std::cout << "Failed to Send Command." << std::endl;
+    }
+    else
+    {
+    	if (command_ID < MIN_COMMAND_ID)
+    		std::cout << "Introduced Command is Smaller Than 1" << std::endl;
+        else if (command_ID > MAX_COMMAND_ID)
+		std::cout << "Introduced Command is Greater Than 32" << std::endl;
+    }
+  
+
+    ros::spinOnce();
+    loop_rate.sleep();
+    std::cout << "Enter a Command ID Between 1 and 32" << std::endl;
+  }
+}
+
+
+
+
+//*****************************************************************************
+void Run_As_Slave_Drone()
+{
+  ros::NodeHandle node_handle;
+  ros::ServiceServer service = node_handle.advertiseService("mav_dji_cmd", Serve_XBee);
+
+	ros::Rate loop_rate(LOOP_RATE);
+	while (ros::ok())
+	{
+		ros::spinOnce();
+		loop_rate.sleep();
+	}
+}
+
+
+//*****************************************************************************
+int main(int argc, char** argv)
+{
+  DRONE_TYPE drone_type = SLAVE;
+
+  if (argc > 1)
+  {
+	if (!strcmp(argv[1], "master"))
+		drone_type = MASTER;
+  }
+
+  if (SLAVE == drone_type)
+	std::cout << "Drone: Slave" << std::endl;
+  else
+	std::cout << "Drone: Master" << std::endl;
+
+
+  std::cout << "Flight Controller Running in SOLO Mode..." << std::endl;
+  
+  ros::init(argc, argv, "flight_controller");
+
+  if (MASTER == drone_type)
+	Run_As_Master_Drone();
+  else
+	Run_As_Slave_Drone();
+
+
+  std::cout << "Flight Controller Exited." << std::endl;
+
+  return 0;
+}

src/XBeeFrame.cpp

@@ -0,0 +1,147 @@
+/* XBeeFrame.cpp -- XBee Frame class --                                      */
+/* ------------------------------------------------------------------------- */
+/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved.      */
+/*                                  (aymen.soussia@gmail.com)                */
+
+
+#include "XBeeFrame.h"
+
+
+namespace Mist
+{
+
+
+namespace Xbee
+{
+
+
+//*****************************************************************************
+Frame::Frame():
+	frame_body_length_(0),
+	frame_type_(0),
+	start_delimiter_position_(0)
+{
+}
+
+
+//*****************************************************************************
+Frame::~Frame()
+{
+}
+
+
+//*****************************************************************************
+const char * Frame::Get_Frame_Data() const
+{
+	return data_buffer_;
+}
+
+
+//*****************************************************************************
+char * Frame::Get_Frame_Data()
+{
+	return data_buffer_;
+}
+
+
+//*****************************************************************************
+std::size_t Frame::Get_Frame_Length() const
+{
+	return FRAME_HEADER_LENGTH + frame_body_length_;
+}
+
+
+//*****************************************************************************
+const char * Frame::Get_Frame_Body() const
+{
+	return data_buffer_ + FRAME_HEADER_LENGTH;
+}
+
+
+//*****************************************************************************
+char * Frame::Get_Frame_Body()
+{
+	return data_buffer_ + FRAME_HEADER_LENGTH;
+}
+
+
+//*****************************************************************************
+std::size_t Frame::Get_Frame_Body_Length() const
+{
+	return frame_body_length_;
+}
+
+
+//*****************************************************************************
+bool Frame::Decode_Frame_Header()
+{
+	const unsigned short THREE_BYTES = 3;
+	const unsigned short EIGHT_BYTES = 8;
+	const unsigned short FRAME_LENGTH_OFFSET = 1;
+	int frame_length_1 = 0;
+	int frame_length_2 = 0;
+	unsigned int frame_length = 0;
+	unsigned char temporary_buffer[THREE_BYTES];
+	char header_buffer[EIGHT_BYTES];
+
+	memcpy(temporary_buffer, data_buffer_ + FRAME_LENGTH_OFFSET, THREE_BYTES);
+
+	frame_length_1 = static_cast<int>(temporary_buffer[0]);
+	frame_length_2 = static_cast<int>(temporary_buffer[1]);
+	frame_type_ = static_cast<int>(temporary_buffer[2]);
+
+	sprintf(header_buffer, "%02X%02X%02X", frame_length_1,
+		frame_length_2, frame_type_);
+	sscanf(header_buffer, "%04X%02X", &frame_length, &frame_type_);
+	frame_body_length_ = frame_length;
+
+	if (frame_body_length_ > MAX_FRAME_BODY_LENGTH)
+		frame_body_length_ = 0; /* The message header is corrupted. Ignore the total frame */
+
+	return true;
+}
+
+
+//*****************************************************************************
+std::size_t Frame::Get_Frame_Type() const
+{
+	return frame_type_;
+}
+
+
+//*****************************************************************************
+int Frame::Get_Start_Delimiter_Position()
+{
+	const unsigned char START_DELIMITER = static_cast<unsigned char>(0x7E);
+
+	for (int i = 0; i < FRAME_HEADER_LENGTH; i++)
+	{
+		if (START_DELIMITER == data_buffer_[i])
+		{
+			start_delimiter_position_ = i;
+			return i;
+		}
+	}
+
+	return -1;
+}
+
+
+//*****************************************************************************
+void Frame::Rearrange_Corrupted_Header()
+{
+	char temporary_char;
+	
+	for (unsigned short i = 0; i  < start_delimiter_position_; i++)
+	{
+		temporary_char = data_buffer_[i];
+		data_buffer_[i] = data_buffer_[start_delimiter_position_ + i];
+		data_buffer_[start_delimiter_position_ + i] = temporary_char;
+	}
+}
+
+
+}
+
+
+}

src/XBeeModule.cpp

@@ -0,0 +1,206 @@
+/* XBeeModule.cpp -- Xbee Module class provides functions to communicate 
+                     with Xbee	--                                           */
+/* ------------------------------------------------------------------------- */
+/* November 30, 2016 -- @Copyright Aymen Soussia. All rights reserved.       */
+/*                                  (aymen.soussia@gmail.com)                */
+
+
+#include "XBeeModule.h"
+
+
+//*****************************************************************************
+XBeeModule::XBeeModule():
+	TIME_OUT(7),
+	serial_port_(io_service_),
+	timer_(serial_port_.get_io_service()),
+	connected_to_XBee_(false),
+	time_out_exceeded_(false)
+{
+}
+
+
+//*****************************************************************************
+XBeeModule::~XBeeModule()
+{
+}
+
+
+//*****************************************************************************
+bool XBeeModule::Init_Port(const std::string& device_port, const unsigned int baud_rate)
+{
+	if (Open_Port(device_port))
+	{
+		serial_port_.set_option(boost::asio::serial_port::baud_rate(baud_rate));
+		Start_Receive();
+		Set_AT_Command_Mode();
+		timer_.expires_from_now(posix_time::seconds(TIME_OUT));
+		timer_.async_wait(bind(&XBeeModule::Check_Time_Out, this,
+			boost::asio::placeholders::error));
+		return true;
+	}
+	else
+		return false;
+}
+
+
+//*****************************************************************************
+bool XBeeModule::Open_Port(const std::string& device_port)
+{
+	serial_port_.open(device_port);
+
+	if (serial_port_.is_open())
+	{
+		std::cout << "Serial Port Open..." << std::endl;
+		return true;
+	}
+	else
+	{
+		std::cout << "Failed to Open The Serial Port." << std::endl;
+		std::cout << "Please Check The Introduced Serial Port is Correct." << std::endl;
+		return false;
+	}
+}
+
+
+//*****************************************************************************
+void XBeeModule::Start_Receive()
+{
+	const std::size_t THREE_BYTES = 3;
+
+	boost::asio::async_read(serial_port_,
+		boost::asio::buffer(receive_buffer_, THREE_BYTES),
+		bind(&XBeeModule::Handle_Receive, this,
+			asio::placeholders::error,
+			asio::placeholders::bytes_transferred));
+}
+
+
+//*****************************************************************************
+void XBeeModule::Handle_Receive(const boost::system::error_code& error, size_t bytes_transferred)
+{
+	if (error)
+	{
+		std::cout << "An Error Occured: " << error << std::endl;
+	}
+
+	if (0 == strncmp(receive_buffer_, "OK\r", bytes_transferred) && !connected_to_XBee_)
+	{
+		connected_to_XBee_ = true;
+	}
+}
+
+
+//*****************************************************************************
+void XBeeModule::Set_AT_Command_Mode()
+{
+	const useconds_t ONE_SECOND = 1*1000*1000; /* 1s = 1 * 10⁶ microseconds. */
+	char AT_command_sequence[] = "+++";
+	const unsigned short THREE_BYTES = 3;
+
+	usleep(ONE_SECOND);
+	serial_port_.write_some(asio::buffer(AT_command_sequence, THREE_BYTES));
+	usleep(ONE_SECOND);
+}
+
+
+//*****************************************************************************
+void XBeeModule::Check_Time_Out(const system::error_code& error)
+{
+	if (error)
+		return;
+	else
+	{
+		if (!connected_to_XBee_)
+		{
+			std::cout << "Time Out: The XBee Module is Not Responding." << std::endl;
+			std::cout << "Please Try One of The Following Options:" << std::endl;
+			std::cout << "  1) Change The Baud Rate. Make Sure It Matches The Baud Rate Used by The XBee." << std::endl;
+			std::cout << "  2) Disconnect and Connect The XBee." << std::endl;
+			std::cout << "  3) Press The Reset Button on The XBee." << std::endl;
+			std::cout << "  4) Update The Firmware With XCTU." << std::endl;
+
+			serial_port_.close();
+			time_out_exceeded_ = true;
+		}
+	}
+}
+
+
+//*****************************************************************************
+void XBeeModule::Run_Service()
+{
+	io_service_.run();
+}
+
+
+//*****************************************************************************
+void XBeeModule::Send_Data(const std::string& command)
+{
+	io_service_.post(
+		[this, command]()
+	{
+		bool write_in_progress = !commands_sequence_.empty();
+		commands_sequence_.push_back(command);
+		if (!write_in_progress)
+			Do_Write();
+	});
+}
+
+
+//*****************************************************************************
+void XBeeModule::Exit_AT_Command_Mode()
+{
+	const unsigned short FIVE_BYTES = 5;
+	serial_port_.write_some(boost::asio::buffer("ATCN\r", FIVE_BYTES));
+
+	io_service_.post([this]() {io_service_.stop(); });
+	io_service_.post([this]() {serial_port_.close(); });
+}
+
+
+//*****************************************************************************
+bool XBeeModule::Is_Connected()
+{
+	return connected_to_XBee_;
+}
+
+
+//*****************************************************************************
+bool XBeeModule::Check_Time_Out_Exceeded()
+{
+	return time_out_exceeded_;
+}
+
+
+//*****************************************************************************
+void XBeeModule::Do_Write()
+{
+	boost::asio::async_write(serial_port_,
+		boost::asio::buffer(commands_sequence_.front().data(), commands_sequence_.front().size()),
+		boost::bind(&XBeeModule::Handle_Write, this,
+			asio::placeholders::error));
+}
+
+
+//*****************************************************************************
+void XBeeModule::Handle_Write(const system::error_code& error)
+{
+	if (!error)
+	{
+		commands_sequence_.pop_front();
+		if (!commands_sequence_.empty())
+			Do_Write();
+	}
+}
+
+
+//*****************************************************************************
+void XBeeModule::Format_AT_Command(const XBee_Parameter_S& parameter, std::string* command)
+{
+	std::string current_command = "AT";
+	current_command.append(parameter.command_);
+	current_command.append(" ");
+	current_command.append(parameter.value_);
+	current_command.append("\r");
+	command->append(current_command);
+}

src/XMLConfigParser.cpp

@@ -0,0 +1,73 @@
+/* XMLConfigParser.cpp -- XML Config Parser class  --                        */
+/* ------------------------------------------------------------------------- */
+/* November 30, 2016 -- @Copyright Aymen Soussia. All rights reserved.       */
+/*                                  (aymen.soussia@gmail.com)                */
+
+
+#include "XMLConfigParser.h"
+
+
+//*****************************************************************************
+XMLConfigParser::XMLConfigParser():
+	config_loaded_successfully_(false)
+{
+}
+
+
+//*****************************************************************************
+XMLConfigParser::~XMLConfigParser()
+{
+}
+
+
+//*****************************************************************************
+bool XMLConfigParser::Load_Config()
+{
+	const std::string FILE_NAME = "/home/mistlab/catkin_ws/src/xbee_ros_node/Resources/XBee_Config.xml";
+
+	if (Check_Config_File_Exists(FILE_NAME))
+	{
+		ptree pt;
+		boost::property_tree::read_xml(FILE_NAME, pt);
+
+		config_loaded_successfully_ = true;
+
+		BOOST_FOREACH(ptree::value_type const&v, pt.get_child("XBeeConfig.Settings"))
+		{
+			if (v.first == "Parameter")
+			{
+				XBee_Parameter_S new_xbee_parameter;
+				new_xbee_parameter.command_ = v.second.get<std::string>("<xmlattr>.Command");
+				new_xbee_parameter.value_ = v.second.data();
+				xbee_parameters_.push_back(new_xbee_parameter);
+			}
+		}
+
+		return true;
+	}
+	else
+	{
+		std::cout << "Error: Config File Not Found." << std::endl;
+		return false;
+	}
+}
+
+
+//*****************************************************************************
+std::vector<XBee_Parameter_S>* XMLConfigParser::Get_Loaded_Parameters()
+{
+	return &xbee_parameters_;
+}
+
+
+//*****************************************************************************
+bool XMLConfigParser::Check_Config_File_Exists(const std::string& file_name)
+{
+	return boost::filesystem::exists(file_name);
+}
+
+
+bool XMLConfigParser::Is_Config_Loaded_Successfully()
+{
+	return config_loaded_successfully_;
+}

src/Xbee.cpp

@@ -0,0 +1,57 @@
+/* xbee.cpp -- XBeeMav ROS node main --                                      */
+/* ------------------------------------------------------------------------- */
+/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved.      */
+/*                                  (aymen.soussia@gmail.com)                */
+
+
+#include "CommunicationManager.h"
+
+
+//*****************************************************************************
+int main(int argc, char* argv[])
+{
+
+  try
+  {
+	ros::init(argc, argv, "xbee");
+
+	Mist::Xbee::CommunicationManager communication_manager;
+	const std::string& device = "/dev/ttyUSB0"; // TO DO can be introduced as command.
+	const std::size_t baud_rate = 230400; // TO DO Can be introduced as command.
+	Mist::Xbee::CommunicationManager::DRONE_TYPE drone_type = 
+			Mist::Xbee::CommunicationManager::DRONE_TYPE::SLAVE;
+	Mist::Xbee::CommunicationManager::RUNNING_MODE running_mode =
+			Mist::Xbee::CommunicationManager::RUNNING_MODE::SOLO;
+
+	if (argc > 1)
+	{
+		if (!strcmp(argv[1], "master"))
+			drone_type = Mist::Xbee::CommunicationManager::DRONE_TYPE::MASTER;
+
+		if (argc > 2)
+		{
+			if (!strcmp(argv[2], "swarm"))
+				running_mode = Mist::Xbee::CommunicationManager::RUNNING_MODE::SWARM;
+		}
+	}
+		
+
+	if (communication_manager.Init(device, baud_rate))
+		communication_manager.Run(drone_type, running_mode);
+  }
+  catch (const std::exception& e)
+  {
+	std::cout << "Error Occured:" << std::endl;
+	std::cout << e.what() << std::endl;
+  }
+  catch (...)
+  {
+	std::cout << "Unexpected Fatal Error." << std::endl;
+	std::cout << "Communication With XBee is Lost." << std::endl;
+	return EXIT_FAILURE;
+  }
+
+  std::cout << std::endl;
+  std::cout << "XBeeMav Exited." << std::endl;
+  return EXIT_SUCCESS;
+}

src/main.cpp

@@ -0,0 +1,114 @@
+/* main.cpp --                 				                     */
+/* ------------------------------------------------------------------------- */
+/* November 30, 2016 -- @Copyright Aymen Soussia. All rights reserved.       */
+/*                                  (aymen.soussia@gmail.com)                */
+
+
+#include<thread>
+
+#include"XBeeModule.h"
+#include"XMLConfigParser.h"
+
+
+//*****************************************************************************
+void Setup_XBee(const std::string& device_port, const unsigned int baud_rate)
+{
+	XBeeModule xbee_module;
+	XMLConfigParser config_parser;
+
+	if (xbee_module.Init_Port(device_port, baud_rate))
+	{
+		std::thread th_service(&XBeeModule::Run_Service, &xbee_module);
+
+		while (!xbee_module.Is_Connected() && !xbee_module.Check_Time_Out_Exceeded())
+		{
+			continue;
+		}
+
+		if (xbee_module.Is_Connected())
+		{
+			std::cout << "Connected to XBee." << std::endl;
+			std::cout << "Loading Config File..." << std::endl;
+
+			if (config_parser.Load_Config())
+			{
+				std::cout << "Config Loaded Successfully." << std::endl;
+				std::cout << "Transferring Data..." << std::endl;
+				std::vector<XBee_Parameter_S>* config_parameters = config_parser.Get_Loaded_Parameters();
+
+				for (std::size_t i = 0; i < config_parameters->size(); i++)
+				{
+					std::string current_command;
+					xbee_module.Format_AT_Command(config_parameters->at(i), &current_command);
+					xbee_module.Send_Data(current_command);
+				}
+
+				std::string write_command = "ATWR \r";
+				xbee_module.Send_Data(write_command);
+			}
+		}
+			
+		th_service.join();
+
+		std::cout << "Exiting AT Command Mode..." << std::endl;
+
+		if (xbee_module.Is_Connected())
+		{
+			xbee_module.Exit_AT_Command_Mode();
+
+			if (config_parser.Is_Config_Loaded_Successfully())
+				std::cout << "XBee Configured Successfully." << std::endl;
+			else
+				std::cout << "XBee Configuration Failed." << std::endl;
+		}
+		else
+		{
+			std::cout << "XBee Configuration Failed." << std::endl;
+		}
+	}
+}
+
+
+//*****************************************************************************
+int main(int argc, char*argv[])
+{
+	try
+	{
+		std::string device_port;
+		unsigned int baud_rate = 0;
+		const unsigned int DEFAULT_BAUD_RATE = 9600;
+		const std::string DEFAULT_DEVICE_PORT = "/dev/ttyUSB0";
+
+		if (argc  < 1)
+			device_port = DEFAULT_DEVICE_PORT;
+		else
+			device_port.append(argv[1]);
+
+		if (argc < 2)
+			baud_rate = DEFAULT_BAUD_RATE;
+		else
+			sscanf(argv[2], "%u", &baud_rate);
+
+		Setup_XBee(device_port, baud_rate);
+	}
+	catch (const std::exception& e)
+	{
+		std::cout << "Error: " <<  e.what() << std::endl;
+		std::cout << "Please Try One of The Following Options:" << std::endl;
+		std::cout << "  1) Change The Baud Rate. Make Sure It Matches The Current Baud Rate Used by The XBee (By default BD = 9600 bps)." << std::endl;
+		std::cout << "  2) Change The Specified Device (e.g. COM1 for Windows or /dev/tty/USB0 for Linux)." << std::endl;
+		std::cout << "  3) Disconnect and Connect The XBee." << std::endl;
+		std::cout << "  4) Press The Reset Button on The XBee." << std::endl;
+		std::cout << "  5) Update The Firmware With XCTU." << std::endl;
+		std::cout << "XBee Configuration Failed." << std::endl;
+
+	}
+	catch (...)
+	{
+		std::cout << " Unexpected Error." << std::endl;
+		std::cout << "XBee Configuration Failed." << std::endl;
+		return EXIT_FAILURE;
+	}
+
+	return EXIT_SUCCESS;
+}