commit a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 Author: vivek-shankar Date: Fri Feb 17 14:20:52 2017 -0500 First commit of new Xbee with multi packets diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..1b5cb7a --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,78 @@ +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 src/PacketsHandler) +target_link_libraries(xbee_mav ${catkin_LIBRARIES}) + +#add_executable(config src/main.cpp src/XBeeModule.cpp src/XMLConfigParser.cpp) +#target_link_libraries(config ${catkin_LIBRARIES}) + +#add_executable(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 ## +############# diff --git a/README.md b/README.md new file mode 100644 index 0000000..f5d1e80 --- /dev/null +++ b/README.md @@ -0,0 +1,197 @@ +##Description + +The "xbee_ros_node" package provides many tools (ROS nodes) to configure, test and communicate Xbee devices. +Four nodes are provided in the package: + * config + * xbee_mav + * test_controller + * test_buzz + +The "test_controller" and the "test_buzz" are dummy nodes. They are used only for testing purposes. The "config" node is used to configure the Xbees. The "xbee_mav" is used to communicates between Xbee devices in different modes (e.g. SOLO and SWARM). + +##Prerequisites + + * Linux OS + * ROS + * Serial device drivers. For FTDI, the Virtual COM port (VCP) drivers are mandatory (http://www.ftdichip.com/Drivers/VCP.htm). + +##Configuration of Xbee devices + +To configure an Xbee device, the "config" node is required. To build the "config" node: + + 1. Uncomment the following lines in "xbee_ros_node/CMakeLists.txt": + + add_executable(config src/main.cpp src/XbeeModule.cpp src/XMLConfigParser.cpp) + target_link_libraries(config ${catkin_LIBRARIES}) + + 2. Insert the correct path to the config file “Resources/Xbee_Config.xml” in “XMLConfigParser.cpp” (line 26): + + FILE_NAME = “/home/mistlab/catkin_ws/src/xbee_ros_node/Resources/Xbee_Config.xml” + + 3. Build the package: + + $ cd ~/catkin_ws + $ catkin_make + +To configure an Xbee device run the "config" node. By default, the VCP and the baud rate are respectively "/dev/ttyUSB0" and "9600". Before running the “config” node, please make sure: + + 1. The Xbee device is plugged. + 2. The serial adapter driver is installed. + 3. You have full access to the correspondent serial port (e.g. ttyUSB0): + + $ cd ~/../../dev/ + $ sudo chmod 666 ttyUSB0 + +Now you can run the “config” node (after running roscore): + + $ cd ~/catkin_ws + $ rosrun xbee_ros_node config /dev/ttyUSB0 9600 + +If this command generates an error: + + * Error: open: No such file or directory: + 1. Make sure the Xbee device is plugged. + 2. Make sure the appropriate driver is installed correctly. + 3. Make sure the path to the VCP (e.g. /dev/ttyUSB0) is correct. + 4. Install the newest Xbee firmware with “XCTU” and Write the default config. + + * Error: open: Permission denied: + + You do not have full access to the serial port. Please execute these commands: + + $ cd ~/../../dev/ + $ sudo chmod 666 ttyUSB0 + + * Time Out: The Xbee Module is Not Responding: + + Baud rate mismatch. Please make sure the introduced baud rate (e.g. 9600) matches the baud rate used by the Xbee (by default is 9600). If the Xbee was previously configured with the “config” node the baud rate will be 230400. + + * Error: Config File Not Found: + + The path to the config file is incorrect. Change it in “XMLConfigParser.cpp” (line 26): + + FILE_NAME = “/home/mistlab/catkin_ws/src/xbee_ros_node/Resources/Xbee_Config.xml” + +The configuration will be loaded from "Resources/Xbee_Config.xml". Most of the existing values in the config file are optimized for Digi-Mesh. Some parameters are default values. However, all parameters can be edited. Please refer to **Table.1** before editing any parameter. More details can be found in “Xbee_Manual.pdf”. The commands table is in Chapter 3 (page 28). + +**Table.1:** Most relevant Xbee Parameters. + +| Key | Name | Chosen Parameter | Justification for chosen setting | +|:---:|:-------------:|:-----------------:|:---------------------------------:| +|CM | Channel Mask | 7FFF | The channels should be allowed to jump on any available channel (Unless a particular channel is very bad).| +|HP | Preamble ID | 1 |This parameter shouldn't be left as is as new not configured radios transmit through this default preamble setting and cause interference by default. Any value from 1-7 should be fine.| +|ID | Network ID | 5FFF | As with HP, this parameter should be changed from its default value because it is a factory setting that will introduce new not yet configured radios to the network. Any other value lower than 7FFF should be fine.| +|MT | Broadcast Multi-Transmits | 3 | MAC-level re-transmits. No reason for change until thoroughly tested.| +|PL | TX Power Level | 4 | Should always be Highest [4] unless very high interference is noted (unlikely in our use case). Should only be decreased for range tests.| +|RR | Unicast Retries | A | MAC-level retries. No reason for change until thoroughly tested.| +| CE | Routing/Messaging Mode | 0 |Should be standard for DigiMesh. (Needs validating)| +|BH | Broadcast Hops | 0 | When set to 0, the broadcast hops will occur until NH is attained.| +| NH | Network Hops | 7 | The maximum number of hops throughout the network. We could potentially link this parameter to the minimum necessary data bandwitdth (more hops = less bandwitdh).| +| MR | Mesh Unicast Retries | 1 | No reason for change until thoroughly tested.| +|NN | Network Delay Slots | 3 | Random delay to alleviate network congestion, especially when broadcasting. Needs to be tested in a very dense mesh.| +|DH | Destination Addr.High | 0 | Should be left to default value which will broadcast a message if entering AT mode (even though we will stay in API mode).| +|DL | Destination Addr. Low | FFFF | Should be left to default value which will broadcast a message if entering AT mode (even though we will stay in API mode).| +|NI | Node Identifier | ‘Node 1’ | This string could be set to a useful identifier for us. It allows up to a 20 characters long ASCII string. Its use needs to be evaluated with the team.| +|NT | Network Discov. Back-off| 82 | This value represents a timeout for network discovery, the value is multiplied by 100ms. Network discovery is basically transparent to us (the user).| +|NO | Network Discov. Options | 0 | Bit 2 of this bitfield adds RSSI to signal, could be useful for us.| +|CI | Cluster ID | 11 | Parameter to be studied. May be useful to identify swarms or camps.| +|EE | Encryption Enable | 0 | Enabling encryption slightly impacts the performance (2% according to data from manual) and should be activated. This option almost completely disables the existing possibilities for accidental or voluntary (hacking) interference from other radios.| +|KY | AES Encryption Key | empty | 128-bit AES Key. To be generated and shared across all radios.| +|BD | Baud Rate | 230400 | To take advantage of the full transmission rate (theoretically 200 kbps), the serial communication baud rate should be adjusted according to the bandwidth.| +|NB | Parity | 0 | Default. | +|SB | Stop Bits | 0 | Default. | +|RO | Packetization Timeout | 3 |Default.| +|FT | Flow Control Threshold | 13F | Since we are using the highest baud rate, we might need to increase the flow control threshold. Testing will tell us if the integrity of the data is fine.| +|AP | API Enable | 1 | The XBee has to be set to API Mode to receive API frames. This allows recovering useful information in the headers of the frames such as source and destination address. We shouldn't ever need to escape API mode so we chose option [1]. Otherwise, the escape bytes would have to be escaped each time they occur. | +|AO | API Options | 0 | Will need to change to [1] (Explicit Rx Indicator) if we wand to be able to read Cluster ID. Otherwise, not very useful. | + +## Test two Xbee devices + +We consider the following setup (**Fig.1**). The block (Drone + Manifold) can be replaced by any Desktop/Laptop meeting the prerequisites. + +**Fig.1:** Experimental Setup: +![][fig1] +[fig1]: https://github.com/MISTLab/XbeeMav/tree/master/Resources/Fig1.png "Fig.1" + +One of the drones will behave as a Master while the other one will act as a Slave. The Master drone will send commands to the Slave drone. +Each drone is running a dummy flight controller node "test_controller". According to the drone type Master/Slave, the "test_controller" node will respectively send or receive and display a command. The commands in the Master drone will be introduced with the keyboard. When a command is received in the Slave drone, it will be printed on the screen. The following table (**Table.2**) depicts the keys of each command: + +**Table.2:** Keys and Commands. + +| Keys | Commands | +|:----:|:---------:| +| 7 | Take Off| +| 8 | Land | +| 9 | Return To Launch | +| 21 | New Waypoint | +| 22 | Start Mission | +| 23 | Start Mission | + + +**Fig.2:** ROS nodes running on the drone: +![][fig2] +[fig2]: https://github.com/MISTLab/XbeeMav/tree/master/Resources/Fig2.png "Fig.2" + +The communication between both drones is performed with Xbees. The “xbee_mav” node (**Fig.2**) will handle all communications with other ROS nodes (test_controller(Flight Controller) or test_buzz (ROS Buzz)) and the connected Xbee device. Therefore, both Xbees must be configured for Digi-Mesh with the maximum baud rate (230400). +We recognize two modes of communications: + * SOLO mode: The "xbee_mav" will communicate only with the "test_controller" through services. By default, one of the following services will be active according to the drone type (Master/Slave): + * “xbee_cmd”: The drone is Master (The server is implemented in the "xbee_mav" side while the client is implemented in the "test_controller" side). + * “mav_dji_cmd”: The drone is Slave (The server is implemented in the "test_controller" side while the client is implemented in the "xbee_mav" side). + * SWARM mode: The "xbee_mav" will communicate only with "test_buzz" through topics. (“inMavlink” and “outMavlink”). + +All topics and services names of the “xbee_mav” node can be edited in the launch file "launch/xbeemav.launch". For other nodes (test_controller and test_buzz), topics and services names are hardcoded. Thus, they need to be modified in the source code. + +####SOLO mode + +The "test_controller" node is needed: + 1. Uncomment the following lines in "CMakeLists.txt": + add_executable(test_controller src/TestController.cpp) + target_link_libraries(test_controller ${catkin_LIBRARIES}) + 2. Build the package: + $ cd ~/catkin_ws + $ catkin_make + 3. Run the "test controller" node in Matser/Slave (after running roscore): + $ cd ~/catkin_ws + $ rosrun xbee_ros_node test_controller master + 4. Run the "xbee_mav" node in SOLO mode. The drone type (Master/Slave) and the communication mode (SOLO/SWARM) need to be specified for the "xbee_mav". You can change these parameters in the launch file “launch/xbeemav.launch”: + $ cd ~/catkin_ws + $ roslaunch xbee_ros_node xbeemav.launch + 5. Introduce some commands to the "test_controller". If the network was configured correctly, commands sent from one side should be received and displayed in the opposite side. + +####SWARM mode + +The "test_buzz" node is required: + 1. Uncomment the following lines in "CMakeLists.txt": + add_executable(test_buzz src/TestBuzz.cpp) + target_link_libraries(test_buzz ${catkin_LIBRARIES}) + 2. Build the package + $ cd ~/catkin_ws + $ catkin_make + 3. Run the "ros_buzz" node. This node is independent of drone type: + $ cd ~/catkin_ws + $ rosrun xbee_ros_node test_buzz + 4. Run the "xbee_mav" node in SWARM mode. Run one drone as Master and the other one as Slave (Modify the launch file). + $ cd ~/catkin_ws + $ roslaunch xbee_ros_node xbeemav.launch + +Random payloads (Mavlink messages) will be created in the Master drone and transferred through the Xbees to the Slave drone for display. Payloads are arrays with random sizes of random 64 bits integers. + +## Communicate drones in a Swarm + +We consider the same setup in the testing phase (**Fig.1**). The "test_controller" and the "test_buzz" dummy nodes need to be replaced with real ones (“fligh_controller” and “ros_buzz”). +You can download the real nodes by clicking on the correspondent link: + * flight_controller : (https://git.mistlab.ca/dasto/djiros_ws) + * ros_buzz : (https://github.com/MISTLab/ROSBuzz.git) + +To run drones in SWARM mode you need to: + 1. Comment the following lines in “xbee_ros_node/CMakeLists.txt”: + + 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}) + + 2. Build the three packages. + 3. Run the launch file (this will run the flight_controller, ros_buzz and xbee_mav). The "xbee_mav" should run in SWARM mode. The drone type (Master/Slave) is not needed. diff --git a/Resources/Fig1.png b/Resources/Fig1.png new file mode 100644 index 0000000..6a6744b Binary files /dev/null and b/Resources/Fig1.png differ diff --git a/Resources/Fig2.png b/Resources/Fig2.png new file mode 100644 index 0000000..b13330c Binary files /dev/null and b/Resources/Fig2.png differ diff --git a/Resources/XBee_Config.xml b/Resources/XBee_Config.xml new file mode 100644 index 0000000..f7f4b2d --- /dev/null +++ b/Resources/XBee_Config.xml @@ -0,0 +1,68 @@ + + + + + 00FFFFFFFFFFFF7FFF + 1 + 5FFF + 3 + 4 + A + 0 + 0 + 7 + 1 + 3 + 0 + FFFF + C0 + 'Node 1' + 82 + 0 + 11 + 0 + + 8 + 0 + 0 + 3 + 13F + 1 + 0 + 1 + 0 + 0 + 0 + 0 + 1 + 0 + 1 + 1 + 1 + 1 + 0 + 0 + 1 + 1 + 7FFF + 7FFF + 0 + 0 + 0 + 28 + 1 + 0 + 1 + 0 + 0 + 2 + 1 + 12C + BB8 + 0 + 2B + 60 + 3E8 + B0000 + + diff --git a/Resources/Xbee_Manual.pdf b/Resources/Xbee_Manual.pdf new file mode 100644 index 0000000..dce9b03 Binary files /dev/null and b/Resources/Xbee_Manual.pdf differ diff --git a/Resources/database.xml b/Resources/database.xml new file mode 100644 index 0000000..be63e56 --- /dev/null +++ b/Resources/database.xml @@ -0,0 +1,14 @@ + + + + 0013A20040D8CA1E + 0013A200415278B8 + 0013A2004103B363 + 0013A200415278AD + 0013A2004103B356 + 0013A200415278B7 + 0013A2004098A7A7 + 0013A2004098A7BA + 0013A200415A9DDD + 0013A200415A9DE4 + diff --git a/include/CommunicationManager.h b/include/CommunicationManager.h new file mode 100644 index 0000000..1bbf6bb --- /dev/null +++ b/include/CommunicationManager.h @@ -0,0 +1,109 @@ +/* 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 +#include + +#include +#include +#include +#include + +#include"PacketsHandler.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(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(); + 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_; + ros::NodeHandle node_handle_; + ros::Subscriber mavlink_subscriber_; + ros::Publisher mavlink_publisher_; + ros::ServiceClient mav_dji_client_; + ros::ServiceServer mav_dji_server_; + std::shared_ptr service_thread_; // TO DO delete !? +}; + + +} + + +} diff --git a/include/MultithreadingDeque.hpp b/include/MultithreadingDeque.hpp new file mode 100644 index 0000000..ea9aa6a --- /dev/null +++ b/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 +#include + + +namespace Mist +{ + + +namespace Xbee +{ + + +//***************************************************************************** +template +class MultithreadingDeque +{ +public: + //**************************************************************************** + MultithreadingDeque() + { + } + + + //**************************************************************************** + ~MultithreadingDeque() + { + } + + + //**************************************************************************** + void Push_Back(const _T& new_data) + { + std::lock_guard guard(mutex_); + deque_.push_back(new_data); + } + + + //**************************************************************************** + _T Pop_Front() + { + std::lock_guard guard(mutex_); + _T value = deque_.front(); + deque_.pop_front(); + return value; + } + + + //**************************************************************************** + unsigned int Get_Size() + { + std::lock_guard guard(mutex_); + return deque_.size(); + } + +private: + + std::mutex mutex_; + std::deque<_T> deque_; +}; + + +} + + +} diff --git a/include/PacketsHandler.h b/include/PacketsHandler.h new file mode 100644 index 0000000..a450350 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#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 fragment); + void Process_Ping_Or_Acknowledgement(std::shared_ptr 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 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 serialized_packet); + void Insert_Fragment_Header(bool single_fragment, + std::shared_ptr 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* frames, + std::shared_ptr>> packet); + void Transmit_Fragments(const std::vector& 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(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(0x01)); + + struct On_Line_Node_S + { + bool received_hole_packet_; + std::string address_64_bits_; + }; + + std::set fragments_indexes_to_transmit_; + SerialDevice* serial_device_; + std::atomic quit_; + Thread_Safe_Deque out_std_messages_; + Thread_Safe_Deque_Of_Vectors out_packets_; + Thread_Safe_Deque* in_packets_; + std::map connected_network_nodes_; + std::map::iterator connected_network_nodes_it_; + std::map packets_assembly_map_; + std::map::iterator assembly_map_it_; + std::map database_addresses_; + std::map::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_; +}; + + +} + + +} diff --git a/include/SerialDevice.h b/include/SerialDevice.h new file mode 100644 index 0000000..25333cf --- /dev/null +++ b/include/SerialDevice.h @@ -0,0 +1,99 @@ +/* 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 +#include +#include + +#include + +#include"MultithreadingDeque.hpp" +#include"XBeeFrame.h" + + +namespace Mist +{ + + +namespace Xbee +{ + + +struct Out_Packet_S +{ + uint8_t packet_ID_; + std::shared_ptr>> packet_buffer_; +}; + +typedef MultithreadingDeque> Thread_Safe_Deque; +typedef MultithreadingDeque Thread_Safe_Deque_Of_Vectors; + + +//***************************************************************************** +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(); + void Set_In_Messages_Pointers(Thread_Safe_Deque* in_std_messages, + Thread_Safe_Deque* in_fragments, + Thread_Safe_Deque* in_Acks_and_Pings, + Thread_Safe_Deque* command_responses); + bool Is_IO_Service_Stopped(); // TO DO delete this function + void Reset_IO_Service(); // TO DO delete this function + void Close_Serial_Port(); + +private: + + 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 out_messages_; + Thread_Safe_Deque* in_std_messages_; + Thread_Safe_Deque* in_fragments_; + Thread_Safe_Deque* in_Acks_and_Pings_; + Thread_Safe_Deque* command_responses_; + Mist::Xbee::Frame current_frame_; + unsigned int FRAME_TYPE_KEYS[REMOTE_AT_COMMAND_RESPONSE + 1]; +}; + + +} + + +} diff --git a/include/XBeeFrame.h b/include/XBeeFrame.h new file mode 100644 index 0000000..50ed2b3 --- /dev/null +++ b/include/XBeeFrame.h @@ -0,0 +1,57 @@ +/* XBeeFrame.cpp -- XBee Frame class -- */ +/* ------------------------------------------------------------------------- */ +/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ +/* (aymen.soussia@gmail.com) */ + + +#pragma once + +#include +#include +#include + + +namespace Mist +{ + + +namespace Xbee +{ + + +//***************************************************************************** +class Frame +{ +public: + Frame(); + ~Frame(); + + enum {FRAME_HEADER_LENGTH = 4}; + enum {MAX_FRAME_BODY_LENGTH = 270}; // 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(); + char Get_Message_Type(); // TO DO const !? + + +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_; +}; + + +} + + +} diff --git a/include/XBeeModule.h b/include/XBeeModule.h new file mode 100644 index 0000000..a65ee81 --- /dev/null +++ b/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 +#include +#include + +#include +#include + +#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_; +}; + diff --git a/include/XBeeParameter.h b/include/XBeeParameter.h new file mode 100644 index 0000000..667e393 --- /dev/null +++ b/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 + + +//***************************************************************************** +struct XBee_Parameter_S +{ + std::string command_; + std::string value_; +}; diff --git a/include/XMLConfigParser.h b/include/XMLConfigParser.h new file mode 100644 index 0000000..efc0f7f --- /dev/null +++ b/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 + +#include +#include +#include +#include + +#include"XBeeParameter.h" + + +//***************************************************************************** +using boost::property_tree::ptree; + + +//***************************************************************************** +class XMLConfigParser +{ +public: + XMLConfigParser(); + ~XMLConfigParser(); + + bool Load_Config(); + std::vector* Get_Loaded_Parameters(); + bool Is_Config_Loaded_Successfully(); + +private: + + bool Check_Config_File_Exists(const std::string& file_name); + + std::vector xbee_parameters_; + bool config_loaded_successfully_; +}; + diff --git a/launch/xbeemav.launch b/launch/xbeemav.launch new file mode 100644 index 0000000..060dac3 --- /dev/null +++ b/launch/xbeemav.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..d009ff0 --- /dev/null +++ b/package.xml @@ -0,0 +1,19 @@ + + + xbee_ros_node + 1.0.2 + The xbee_ros_node package + + aymen + + Boost Software License + + catkin + roscpp + std_msgs + roscpp + std_msgs + + + + diff --git a/src/CommunicationManager.cpp b/src/CommunicationManager.cpp new file mode 100644 index 0000000..3012b2f --- /dev/null +++ b/src/CommunicationManager.cpp @@ -0,0 +1,548 @@ +/* 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(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)) + { + serial_device_.Set_In_Messages_Pointers(&in_std_messages_, &in_fragments_, + &in_Acks_and_Pings_, &command_responses_); + + service_thread_ = std::make_shared(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; + } + else + { + Display_Init_Communication_Failure(); + return false; + } + + return true; +} + + + +//***************************************************************************** +void CommunicationManager::Run(DRONE_TYPE drone_type, + RUNNING_MODE running_mode) +{ + std::thread thread_packets_handler(&PacketsHandler::Run, &packets_handler_); + + 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(); + serial_device_.Close_Serial_Port(); + packets_handler_.Quit(); + service_thread_->join(); + thread_packets_handler.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(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( + 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()) + { + 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(); + } + } +} + + +//***************************************************************************** +inline bool CommunicationManager::Serve_Flight_Controller(mavros_msgs::CommandInt:: + Request& request, mavros_msgs::CommandInt::Response& response) +{ + const std::size_t MAX_BUFFER_SIZE = 256; + 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(mavros_msgs::CommandCode::NAV_TAKEOFF); + sprintf(temporary_buffer, "%u ", command); + response.success = true; + break; + } + case mavros_msgs::CommandCode::NAV_LAND: + { + command = static_cast(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(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(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(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(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(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(frame->at(i)); + + lowest_8_bits = bytes_sum & 0xFF; + checksum = 0xFF - lowest_8_bits; + checksum_byte = static_cast(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(Hex_frame_length_1); + header.push_back(temporary_char); + temporary_char = static_cast(Hex_frame_length_2); + header.push_back(temporary_char); + frame->insert(0, header); +}*/ + + +//***************************************************************************** +/*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; + + for (std::size_t j = 0; j < size_in_messages; j++) + { + std::shared_ptr 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 " ", + ¤t_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) + { + for (std::size_t j = 0; j < size_in_messages; j++) + { + std::shared_ptr 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(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) +{ + std::cout << "Received Message From Buzz" << std::endl; // TO DO delete + packets_handler_.Handle_Mavlink_Message(mavlink_msg); +} + + +//***************************************************************************** +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; +} + + +//***************************************************************************** +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 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 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 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 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 in_message = + command_responses_.Pop_Front(); + + packets_handler_.Process_Command_Response(in_message->c_str()); + } + } +} + + +} + + +} diff --git a/src/PacketsHandler.cpp b/src/PacketsHandler.cpp new file mode 100644 index 0000000..94c6525 --- /dev/null +++ b/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(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 serialized_packet = + std::make_shared(); + + Serialize_Mavlink_Message(mavlink_msg, serialized_packet); + + if (serialized_packet->size() > XBEE_NETWORK_MTU && serialized_packet->size() <= MAX_PACEKT_SIZE) + { + std::shared_ptr>> fragmented_packet = + std::make_shared>>(); + + std::size_t offset = 0; + std::size_t NBR_of_bytes = 0; + std::size_t NBR_of_fragments = std::ceil( + static_cast(serialized_packet->size()) / XBEE_NETWORK_MTU); + + for (uint8_t i = 0; i < NBR_of_fragments; i++) + { + fragmented_packet->push_back(std::make_shared()); + 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 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( + static_cast(static_cast(fragment->at(4))) << 8 | + static_cast(static_cast(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::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 frame) // TO DO change useless std::shared_ptr 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 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(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::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 empty_set; + packets_assembly_map_.insert(std::pair(new_node_address, {})); + + std::lock_guard guard(mutex_); + connected_network_nodes_.insert(std::pair(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 guard(mutex_); + + if (command_response[2] == static_cast(0)) // TO DO check this + { + new_node_address = static_cast( // TO DO correct this + static_cast(command_response[5]) << 56 | + static_cast(command_response[6]) << 48 | + static_cast(command_response[7]) << 40 | + static_cast(command_response[8]) << 32 | + static_cast(command_response[9]) << 24 | + static_cast(command_response[10]) << 16 | + static_cast(command_response[11]) << 8 | + static_cast(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(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(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 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 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 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 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(".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(address_64_bits_int, static_cast(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(static_cast(device_64_bits_address_[0])) << 56 | + static_cast(static_cast(device_64_bits_address_[1])) << 48 | + static_cast(static_cast(device_64_bits_address_[2])) << 40 | + static_cast(static_cast(device_64_bits_address_[3])) << 32 | + static_cast(static_cast(device_64_bits_address_[4])) << 24 | + static_cast(static_cast(device_64_bits_address_[5])) << 16 | + static_cast(static_cast(device_64_bits_address_[6])) << 8 | + static_cast(static_cast(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(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 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* frames, std::shared_ptr>> packet) +{ + std::lock_guard 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& frames) +{ + std::lock_guard 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 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(static_cast(bytes[i])) << 56 | + static_cast(static_cast(bytes[i + 1])) << 48 | + static_cast(static_cast(bytes[i + 2])) << 40 | + static_cast(static_cast(bytes[i + 3])) << 32 | + static_cast(static_cast(bytes[i + 4])) << 24 | + static_cast(static_cast(bytes[i + 5])) << 16 | + static_cast(static_cast(bytes[i + 6])) << 8 | + static_cast(static_cast(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(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(Hex_frame_length_1); + header.push_back(temporary_char); + temporary_char = static_cast(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(frame->at(i)); + + lowest_8_bits = bytes_sum & 0xFF; + checksum = 0xFF - lowest_8_bits; + checksum_byte = static_cast(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(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(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); +} + + +} + + +} diff --git a/src/SerialDevice.cpp b/src/SerialDevice.cpp new file mode 100644 index 0000000..5b00d0e --- /dev/null +++ b/src/SerialDevice.cpp @@ -0,0 +1,287 @@ +/* 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(); + 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() +{ + Read_Frame_Header(); + io_service_.run(); +} + + +//***************************************************************************** +void SerialDevice::Stop_Service() +{ + io_service_.post([this]() {io_service_.stop(); }); +} + + +//***************************************************************************** +void SerialDevice::Close_Serial_Port() +{ + io_service_.post([this]() {serial_port_.close(); }); +} + + +//***************************************************************************** +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; +} + + +//***************************************************************************** +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]) + { + char msg_type = current_frame_.Get_Message_Type(); + std::shared_ptr in_message = + std::make_shared(); + + 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 in_message = + std::make_shared(); + + in_message->append(current_frame_.Get_Frame_Body() + 1, + current_frame_.Get_Frame_Body_Length() - 2); + + command_responses_->Push_Back(in_message); + } + + Read_Frame_Header(); + } + 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(); + } + }); +} + + +//***************************************************************************** +bool SerialDevice::Is_IO_Service_Stopped() +{ + return io_service_.stopped(); +} + + +//***************************************************************************** +void SerialDevice::Reset_IO_Service() +{ + io_service_.reset(); +} + + +} + + +} diff --git a/src/TestBuzz.cpp b/src/TestBuzz.cpp new file mode 100644 index 0000000..5341620 --- /dev/null +++ b/src/TestBuzz.cpp @@ -0,0 +1,98 @@ +/* RosBuzz.cpp -- Basic ROS Buzz node -- */ +/* ------------------------------------------------------------------------- */ +/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ +/* (aymen.soussia@gmail.com) */ + + +#include +#include +#include + +#include +#include + + + +//***************************************************************************** +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 = 750; + const unsigned int MAX_PAYLOAD_SIZE = 752; + + ros::init(argc, argv, "test_buzz"); + + ros::NodeHandle node_handle; + ros::Publisher mavlink_pub = node_handle.advertise("outMavlink", 1000); + ros::Subscriber mavlink_sub = node_handle.subscribe("inMavlink", 1000, Receive_Payload_Callback); + + 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; + + 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(); + + count++; + std::cout << "Press Enter to Send New Mavlink Message..." << std::endl; + } + + return 0; +} diff --git a/src/TestController.cpp b/src/TestController.cpp new file mode 100644 index 0000000..9f1aeaf --- /dev/null +++ b/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 +#include +#include +#include + +#include +#include +#include +#include + + +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 (rand()) /( static_cast (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("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; +} diff --git a/src/XBeeFrame.cpp b/src/XBeeFrame.cpp new file mode 100644 index 0000000..1a99176 --- /dev/null +++ b/src/XBeeFrame.cpp @@ -0,0 +1,154 @@ +/* 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(temporary_buffer[0]); + frame_length_2 = static_cast(temporary_buffer[1]); + frame_type_ = static_cast(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(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; + } +} + + +//***************************************************************************** +char Frame::Get_Message_Type() +{ + return data_buffer_[15]; +} + + +} + + +} diff --git a/src/XBeeModule.cpp b/src/XBeeModule.cpp new file mode 100644 index 0000000..421c35c --- /dev/null +++ b/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); +} diff --git a/src/XMLConfigParser.cpp b/src/XMLConfigParser.cpp new file mode 100644 index 0000000..525e75e --- /dev/null +++ b/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(".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* 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_; +} diff --git a/src/Xbee.cpp b/src/Xbee.cpp new file mode 100644 index 0000000..496d531 --- /dev/null +++ b/src/Xbee.cpp @@ -0,0 +1,56 @@ +/* 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; +} diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..97bb2de --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,114 @@ +/* main.cpp -- */ +/* ------------------------------------------------------------------------- */ +/* November 30, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ +/* (aymen.soussia@gmail.com) */ + + +#include + +#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* 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), ¤t_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; +}