From 5eb4ef78b445dcafc982e3bb67cfb70e4d32576c Mon Sep 17 00:00:00 2001 From: AymenMist Date: Tue, 10 Jan 2017 12:14:41 -0500 Subject: [PATCH] first commit --- CMakeLists.txt | 81 ++++++ Resources/XBee_Config.xml | 68 +++++ include/CommunicationManager.h | 97 +++++++ include/MultithreadingDeque.hpp | 73 ++++++ include/SerialDevice.h | 83 ++++++ include/XBeeFrame.h | 56 ++++ include/XBeeModule.h | 65 +++++ include/XBeeParameter.h | 17 ++ include/XMLConfigParser.h | 41 +++ launch/xbeemav.launch | 12 + package.xml | 19 ++ src/CommunicationManager.cpp | 442 ++++++++++++++++++++++++++++++++ src/SerialDevice.cpp | 232 +++++++++++++++++ src/TestBuzz.cpp | 96 +++++++ src/TestController.cpp | 212 +++++++++++++++ src/XBeeFrame.cpp | 147 +++++++++++ src/XBeeModule.cpp | 206 +++++++++++++++ src/XMLConfigParser.cpp | 73 ++++++ src/Xbee.cpp | 57 ++++ src/main.cpp | 114 ++++++++ 20 files changed, 2191 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 Resources/XBee_Config.xml create mode 100644 include/CommunicationManager.h create mode 100644 include/MultithreadingDeque.hpp create mode 100644 include/SerialDevice.h create mode 100644 include/XBeeFrame.h create mode 100644 include/XBeeModule.h create mode 100644 include/XBeeParameter.h create mode 100644 include/XMLConfigParser.h create mode 100644 launch/xbeemav.launch create mode 100644 package.xml create mode 100644 src/CommunicationManager.cpp create mode 100644 src/SerialDevice.cpp create mode 100644 src/TestBuzz.cpp create mode 100644 src/TestController.cpp create mode 100644 src/XBeeFrame.cpp create mode 100644 src/XBeeModule.cpp create mode 100644 src/XMLConfigParser.cpp create mode 100644 src/Xbee.cpp create mode 100644 src/main.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..a54cab7 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,81 @@ +cmake_minimum_required(VERSION 2.8.3) +project(xbee_ros_node) + +if(UNIX) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11") +endif() + +set (BOOST_INCLUDEDIR "/usr/include") + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs +) + +## System dependencies are found with CMake's conventions + find_package(Boost REQUIRED COMPONENTS + system) + +################################################ +## Declare ROS messages, services and actions ## +################################################ + + + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + + + +################################### +## catkin specific configuration ## +################################### + + +catkin_package( + INCLUDE_DIRS include +# LIBRARIES xbee_ros_node + CATKIN_DEPENDS roscpp std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + + +## Specify additional locations of header files +include_directories( + include ${xbee_ros_node_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) + + +add_executable(xbee_mav src/Xbee.cpp src/XBeeFrame.cpp src/SerialDevice.cpp src/CommunicationManager.cpp) +target_link_libraries(xbee_mav ${catkin_LIBRARIES}) + +#add_executable(config src/main.cpp src/XBeeModule.cpp src/XMLConfigParser.cpp) +#target_link_libraries(config ${catkin_LIBRARIES}) + +#add_executable(test_controller src/TestController.cpp) +#target_link_libraries(test_controller ${catkin_LIBRARIES}) + +#add_executable(test_buzz src/TestBuzz.cpp) +#target_link_libraries(test_buzz ${catkin_LIBRARIES}) + + + +############# +## Install ## +############# + + + +############# +## Testing ## +############# + + + 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/include/CommunicationManager.h b/include/CommunicationManager.h new file mode 100644 index 0000000..880eb5f --- /dev/null +++ b/include/CommunicationManager.h @@ -0,0 +1,97 @@ +/* CommunicationManager.h -- Communication Manager class for XBee: + Handles all communications with other ROS nodes + and the serial port -- */ +/* ------------------------------------------------------------------------- */ +/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ +/* (aymen.soussia@gmail.com) */ + + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#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(); + + Mist::Xbee::SerialDevice serial_device_; + Thread_Safe_Deque* in_messages_; + ros::NodeHandle node_handle_; + ros::Subscriber mavlink_subscriber_; + ros::Publisher mavlink_publisher_; + ros::ServiceClient mav_dji_client_; + ros::ServiceServer mav_dji_server_; +}; + + +} + + +} diff --git a/include/MultithreadingDeque.hpp b/include/MultithreadingDeque.hpp new file mode 100644 index 0000000..8de0acf --- /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_Pack(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/SerialDevice.h b/include/SerialDevice.h new file mode 100644 index 0000000..8d34947 --- /dev/null +++ b/include/SerialDevice.h @@ -0,0 +1,83 @@ +/* SerialDevice.h -- Serial Device class to handle serial communications + with XBee -- */ +/* ------------------------------------------------------------------------- */ +/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ +/* (aymen.soussia@gmail.com) */ + + +#pragma once + +#include +#include +#include + +#include + +#include"MultithreadingDeque.hpp" +#include"XBeeFrame.h" + + +namespace Mist +{ + + +namespace Xbee +{ + + +typedef MultithreadingDeque> Thread_Safe_Deque; + + +//***************************************************************************** +class SerialDevice +{ +public: + SerialDevice(); + ~SerialDevice(); + + bool Init(const std::string& device, const std::size_t baud_rate); + void Send_Frame(const std::string& frame); + void Run_Service(); + void Stop_Service(); + Thread_Safe_Deque* Get_In_Messages_Pointer(); + +private: + + enum FRAME_TYPE + { + AT_COMMAND = 0, + AT_COMMAND_QUEUE_REGISTER_VALUE = 1, + TRANSMIT_REQUEST = 2, + EXPLICIT_ADDRESSING_COMMAND_FRAME = 3, + REMOTE_AT_COMMAND = 4, + AT_COMMAND_RESPONSE = 5, + MODEM_STATUS = 6, + TRANSMIT_STATUS = 7, + ROUTE_INFORMATION_PACKET = 8, + AGGREGATE_ADDRESSING_UPDATE = 9, + RECEIVE_PACKET = 10, + EXPLICIT_RX_INDICATOR = 11, + IO_DATA_SAMPLE_RX_INDICATOR = 12, + NODE_IDENTIFICATION_INDICATOR = 13, + REMOTE_AT_COMMAND_RESPONSE = 14 + }; + + void Init_Frame_Type_Keys(); + void Set_Port_Options(const std::size_t baud_rate); + void Read_Frame_Header(); + void Read_Frame_Body(); + void Write_Frame(); + + boost::asio::io_service io_service_; + boost::asio::serial_port serial_port_; + std::deque out_messages_; + Thread_Safe_Deque in_messages_; + 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..5512385 --- /dev/null +++ b/include/XBeeFrame.h @@ -0,0 +1,56 @@ +/* XBeeFrame.cpp -- XBee Frame class -- */ +/* ------------------------------------------------------------------------- */ +/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ +/* (aymen.soussia@gmail.com) */ + + +#pragma once + +#include +#include +#include + + +namespace Mist +{ + + +namespace Xbee +{ + + +//***************************************************************************** +class Frame +{ +public: + Frame(); + ~Frame(); + + enum {FRAME_HEADER_LENGTH = 4}; + enum {MAX_FRAME_BODY_LENGTH = 280}; // TO DO check value + + const char* Get_Frame_Data() const; + char* Get_Frame_Data(); + std::size_t Get_Frame_Length() const; + const char* Get_Frame_Body() const; + char* Get_Frame_Body(); + std::size_t Get_Frame_Body_Length() const; + bool Decode_Frame_Header(); + std::size_t Get_Frame_Type() const; + int Get_Start_Delimiter_Position(); + void Rearrange_Corrupted_Header(); + + +private: + + char data_buffer_[FRAME_HEADER_LENGTH + MAX_FRAME_BODY_LENGTH]; + std::size_t frame_body_length_; + unsigned int frame_type_; + int start_delimiter_position_; +}; + + +} + + +} 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..b2e7c80 --- /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..7457422 --- /dev/null +++ b/src/CommunicationManager.cpp @@ -0,0 +1,442 @@ +/* CommunicationManager.cpp -- Communication Manager class for XBee: + Handles all communications with other ROS nodes + and the serial port -- */ +/* ------------------------------------------------------------------------- */ +/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ +/* (aymen.soussia@gmail.com) */ + + +#include "CommunicationManager.h" + + +namespace Mist +{ + + +namespace Xbee +{ + + +//***************************************************************************** +CommunicationManager::CommunicationManager(): + START_DLIMITER(static_cast(0x7E)), + LOOP_RATE(10) /* 10 fps */ +{ +} + + +//***************************************************************************** +CommunicationManager::~CommunicationManager() +{ +} + + +//***************************************************************************** +bool CommunicationManager::Init( + const std::string& device, const std::size_t baud_rate) +{ + if (serial_device_.Init(device, baud_rate)) + { + in_messages_ = serial_device_.Get_In_Messages_Pointer(); + return true; + } + else + { + Display_Init_Communication_Failure(); + return false; + } +} + + + +//***************************************************************************** +void CommunicationManager::Run(DRONE_TYPE drone_type, + RUNNING_MODE running_mode) +{ + std::thread thread_service(&SerialDevice::Run_Service, &serial_device_); + + Display_Drone_Type_and_Running_Mode(drone_type, running_mode); + + if (RUNNING_MODE::SWARM == running_mode) + Run_In_Swarm_Mode(); + else + Run_In_Solo_Mode(drone_type); + + serial_device_.Stop_Service(); + thread_service.join(); +} + + +//***************************************************************************** +void CommunicationManager::Run_In_Solo_Mode(DRONE_TYPE drone_type) +{ + std::string service_name; + bool success = false; + + if (DRONE_TYPE::MASTER == drone_type) + { + if (node_handle_.getParam("Xbee_In_From_Controller", service_name)) + { + mav_dji_server_ = node_handle_.advertiseService(service_name.c_str(), &CommunicationManager::Serve_Flight_Controller, this); + success = true; + } + else + std::cout << "Failed to Get Service Name: param 'Xbee_In_From_Controller' Not Found." << std::endl; + } + else + { + if (node_handle_.getParam("Xbee_Out_To_Controller", service_name)) + { + mav_dji_client_ = node_handle_.serviceClient(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()) + { + Check_In_Messages_and_Transfer_To_Topics(); + ros::spinOnce(); + loop_rate.sleep(); + } + } +} + + +//***************************************************************************** +inline bool CommunicationManager::Serve_Flight_Controller(mavros_msgs::CommandInt:: + Request& request, mavros_msgs::CommandInt::Response& response) +{ + const std::size_t MAX_BUFFER_SIZE = 255; + unsigned int command = 0; + char temporary_buffer[MAX_BUFFER_SIZE]; + std::string frame; + + switch(request.command) + { + case mavros_msgs::CommandCode::NAV_TAKEOFF: + { + command = static_cast(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() +{ + 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() +{ + 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) +{ + const unsigned short MAX_BUFFER_SIZE = 211; /* 20 (length(uint64_t)) * 10 (max int number) + 10 (spaces) + 1 */ + const unsigned short MAX_NBR_OF_INT64 = 10; + char temporary_buffer[MAX_BUFFER_SIZE]; + std::string frame; + int converted_bytes = 0; + + /* Check the payload is not too long. Otherwise ignore it. */ + if (mavlink_msg->payload64.size() <= MAX_NBR_OF_INT64) + { + for (unsigned short i = 0; i < mavlink_msg->payload64.size(); i++) + { + converted_bytes += sprintf( + temporary_buffer + converted_bytes, "%" PRIu64 " ", + (uint64_t)mavlink_msg->payload64.at(i)); + + } + + Generate_Transmit_Request_Frame(temporary_buffer, &frame); + serial_device_.Send_Frame(frame); + } +} + + +//***************************************************************************** +void CommunicationManager::Display_Drone_Type_and_Running_Mode( + DRONE_TYPE drone_type, RUNNING_MODE running_mode) +{ + if (DRONE_TYPE::MASTER == drone_type) + std::cout << "Drone: MASTER" << std::endl; + else + std::cout << "Drone: SLAVE" << std::endl; + + if (RUNNING_MODE::SOLO == running_mode) + std::cout << "XBeeMav Running in SOLO Mode..." << std::endl; + else + std::cout << "XBeeMav Running in SWARM Mode..." << std::endl; +} + + +} + + +} diff --git a/src/SerialDevice.cpp b/src/SerialDevice.cpp new file mode 100644 index 0000000..33dc7a5 --- /dev/null +++ b/src/SerialDevice.cpp @@ -0,0 +1,232 @@ +/* SerialDevice.cpp -- Serial Device class to handle serial communications + with XBee -- */ +/* ------------------------------------------------------------------------- */ +/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ +/* (aymen.soussia@gmail.com) */ + + +#include "SerialDevice.h" + + +namespace Mist +{ + + +namespace Xbee +{ + + +//***************************************************************************** +SerialDevice::SerialDevice(): + serial_port_(io_service_) +{ +} + + +//***************************************************************************** +SerialDevice::~SerialDevice() +{ +} + + +//***************************************************************************** +bool SerialDevice::Init( + const std::string & device, const std::size_t baud_rate) +{ + serial_port_.open(device); + + if (serial_port_.is_open()) + { + Set_Port_Options(baud_rate); + Init_Frame_Type_Keys(); + Read_Frame_Header(); + return true; + } + else + { + std::cout << "Port Failed To Open." << std::endl; + return false; + } +} + + +//***************************************************************************** +void SerialDevice::Set_Port_Options(const std::size_t baud_rate) +{ + serial_port_.set_option(boost::asio::serial_port::baud_rate(baud_rate)); + serial_port_.set_option(boost::asio::serial_port::character_size(8)); + serial_port_.set_option(boost::asio::serial_port::parity( + boost::asio::serial_port::serial_port_base::parity::none)); + serial_port_.set_option(boost::asio::serial_port::stop_bits( + boost::asio::serial_port::serial_port_base::stop_bits::one)); + serial_port_.set_option(boost::asio::serial_port::flow_control( + boost::asio::serial_port::serial_port_base::flow_control::none)); +} + + +//***************************************************************************** +void SerialDevice::Send_Frame(const std::string& frame) +{ + io_service_.post( + [this, frame]() + { + bool write_in_progress = !out_messages_.empty(); + out_messages_.push_back(frame); + + if (!write_in_progress) + Write_Frame(); + }); +} + + +//***************************************************************************** +void SerialDevice::Run_Service() +{ + io_service_.run(); +} + + +//***************************************************************************** +void SerialDevice::Stop_Service() +{ + io_service_.post([this]() {io_service_.stop(); }); + io_service_.post([this]() {serial_port_.close(); }); +} + + +//***************************************************************************** +Thread_Safe_Deque * SerialDevice::Get_In_Messages_Pointer() +{ + return &in_messages_; +} + + +//***************************************************************************** +void SerialDevice::Init_Frame_Type_Keys() +{ + sscanf("08", "%02X", &FRAME_TYPE_KEYS[AT_COMMAND]); + sscanf("09", "%02X", &FRAME_TYPE_KEYS[AT_COMMAND_QUEUE_REGISTER_VALUE]); + sscanf("10", "%02X", &FRAME_TYPE_KEYS[TRANSMIT_REQUEST]); + sscanf("11", "%02X", + &FRAME_TYPE_KEYS[EXPLICIT_ADDRESSING_COMMAND_FRAME]); + sscanf("17", "%02X", &FRAME_TYPE_KEYS[REMOTE_AT_COMMAND]); + sscanf("88", "%02X", &FRAME_TYPE_KEYS[AT_COMMAND_RESPONSE]); + sscanf("8A", "%02X", &FRAME_TYPE_KEYS[MODEM_STATUS]); + sscanf("8B", "%02X", &FRAME_TYPE_KEYS[TRANSMIT_STATUS]); + sscanf("8D", "%02X", &FRAME_TYPE_KEYS[ROUTE_INFORMATION_PACKET]); + sscanf("8E", "%02X", &FRAME_TYPE_KEYS[AGGREGATE_ADDRESSING_UPDATE]); + sscanf("90", "%02X", &FRAME_TYPE_KEYS[RECEIVE_PACKET]); + sscanf("91", "%02X", &FRAME_TYPE_KEYS[EXPLICIT_RX_INDICATOR]); + sscanf("92", "%02X", &FRAME_TYPE_KEYS[IO_DATA_SAMPLE_RX_INDICATOR]); + sscanf("95", "%02X", &FRAME_TYPE_KEYS[NODE_IDENTIFICATION_INDICATOR]); + sscanf("97", "%02X", &FRAME_TYPE_KEYS[REMOTE_AT_COMMAND_RESPONSE]); +} + + +//***************************************************************************** +void SerialDevice::Read_Frame_Header() +{ + boost::asio::async_read(serial_port_, + boost::asio::buffer(current_frame_.Get_Frame_Data(), + Xbee::Frame::FRAME_HEADER_LENGTH), + [this](boost::system::error_code error, std::size_t) + { + if (!error) + { + int start_delimiter_position = + current_frame_.Get_Start_Delimiter_Position(); + + if (start_delimiter_position >= 0) + { + if (0 == start_delimiter_position) + current_frame_.Decode_Frame_Header(); + else + { + /* The header is corrupted. */ + boost::asio::async_read(serial_port_, + boost::asio::buffer(current_frame_.Get_Frame_Data(), + start_delimiter_position), + [this](boost::system::error_code error, std::size_t) + { + current_frame_.Rearrange_Corrupted_Header(); + current_frame_.Decode_Frame_Header(); + }); + } + + Read_Frame_Body(); + } + else + /* The header is totally corrupted, read another header. */ + Read_Frame_Header(); + } + else + { + std::cout << "Error Occured:" << std::endl; + std::cout << error << std::endl; + std::cout << "Communication With XBee is Lost." << std::endl; + serial_port_.close(); + } + }); +} + + +//***************************************************************************** +void SerialDevice::Read_Frame_Body() +{ + boost::asio::async_read(serial_port_, + boost::asio::buffer(current_frame_.Get_Frame_Body(), + current_frame_.Get_Frame_Body_Length()), + [this](boost::system::error_code error, std::size_t) + { + if (!error) + { + if (current_frame_.Get_Frame_Type() == + FRAME_TYPE_KEYS[RECEIVE_PACKET]) + { + const unsigned short ELEVEN_BYTES = 11; + const unsigned short TWELVE_BYTES = 12; + + std::shared_ptr in_message = + std::make_shared(); + in_message->append(current_frame_.Get_Frame_Body() + + ELEVEN_BYTES, + current_frame_.Get_Frame_Body_Length() - TWELVE_BYTES); + in_messages_.Push_Pack(in_message); + } + + Read_Frame_Header(); + } + else + { + std::cout << "Error Occured:" << std::endl; + std::cout << error << std::endl; + std::cout << "Communication With XBee is Lost." << std::endl; + serial_port_.close(); + } + }); +} + + +//***************************************************************************** +void SerialDevice::Write_Frame() +{ + boost::asio::async_write(serial_port_, + boost::asio::buffer(out_messages_.front().data(), + out_messages_.front().size()), + [this](boost::system::error_code error, + std::size_t transferred_bytes) + { + if (!error) + { + out_messages_.pop_front(); + if (!out_messages_.empty()) + Write_Frame(); + } + }); +} + + +} + + +} diff --git a/src/TestBuzz.cpp b/src/TestBuzz.cpp new file mode 100644 index 0000000..1337742 --- /dev/null +++ b/src/TestBuzz.cpp @@ -0,0 +1,96 @@ +/* RosBuzz.cpp -- Basic ROS Buzz node -- */ +/* ------------------------------------------------------------------------- */ +/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ +/* (aymen.soussia@gmail.com) */ + + +#include +#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 = 1; + const unsigned int MAX_PAYLOAD_SIZE = 10; + + ros::init(argc, argv, "flight_controller"); + + ros::NodeHandle node_handle; + ros::Publisher mavlink_pub = node_handle.advertise("outMavlink", 1000); + ros::Subscriber mavlink_sub = node_handle.subscribe("inMavlink", 1000, Receive_Payload_Callback); + + ros::Rate loop_rate(0.2); + + Init_Random_Seed(); + + int count = 0; + + + while (ros::ok()) + { + mavros_msgs::Mavlink mavlink_msg_; + + unsigned int payload_size = Get_Random_Size(MIN_PAYLOAD_SIZE, MAX_PAYLOAD_SIZE); + + for (unsigned int i = 0; i < payload_size; i++) + { + mavlink_msg_.payload64.push_back(Get_Random_Int64(1, ULLONG_MAX)); + } + + std::cout << "Sent Mavlink Message " << count << std::endl; + + for (unsigned int i = 0; i < payload_size; i++) + { + std::cout << mavlink_msg_.payload64.at(i) << std::endl; + } + + std::cout << std::endl; + + mavlink_pub.publish(mavlink_msg_); + + ros::spinOnce(); + + loop_rate.sleep(); + + count++; + } + + return 0; +} 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..2e2fad1 --- /dev/null +++ b/src/XBeeFrame.cpp @@ -0,0 +1,147 @@ +/* XBeeFrame.cpp -- XBee Frame class -- */ +/* ------------------------------------------------------------------------- */ +/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ +/* (aymen.soussia@gmail.com) */ + + +#include "XBeeFrame.h" + + +namespace Mist +{ + + +namespace Xbee +{ + + +//***************************************************************************** +Frame::Frame(): + frame_body_length_(0), + frame_type_(0), + start_delimiter_position_(0) +{ +} + + +//***************************************************************************** +Frame::~Frame() +{ +} + + +//***************************************************************************** +const char * Frame::Get_Frame_Data() const +{ + return data_buffer_; +} + + +//***************************************************************************** +char * Frame::Get_Frame_Data() +{ + return data_buffer_; +} + + +//***************************************************************************** +std::size_t Frame::Get_Frame_Length() const +{ + return FRAME_HEADER_LENGTH + frame_body_length_; +} + + +//***************************************************************************** +const char * Frame::Get_Frame_Body() const +{ + return data_buffer_ + FRAME_HEADER_LENGTH; +} + + +//***************************************************************************** +char * Frame::Get_Frame_Body() +{ + return data_buffer_ + FRAME_HEADER_LENGTH; +} + + +//***************************************************************************** +std::size_t Frame::Get_Frame_Body_Length() const +{ + return frame_body_length_; +} + + +//***************************************************************************** +bool Frame::Decode_Frame_Header() +{ + const unsigned short THREE_BYTES = 3; + const unsigned short EIGHT_BYTES = 8; + const unsigned short FRAME_LENGTH_OFFSET = 1; + int frame_length_1 = 0; + int frame_length_2 = 0; + unsigned int frame_length = 0; + unsigned char temporary_buffer[THREE_BYTES]; + char header_buffer[EIGHT_BYTES]; + + memcpy(temporary_buffer, data_buffer_ + FRAME_LENGTH_OFFSET, THREE_BYTES); + + frame_length_1 = static_cast(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; + } +} + + +} + + +} 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..d71e3a6 --- /dev/null +++ b/src/Xbee.cpp @@ -0,0 +1,57 @@ +/* xbee.cpp -- XBeeMav ROS node main -- */ +/* ------------------------------------------------------------------------- */ +/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ +/* (aymen.soussia@gmail.com) */ + + +#include "CommunicationManager.h" + + +//***************************************************************************** +int main(int argc, char* argv[]) +{ + + try + { + ros::init(argc, argv, "xbee"); + + Mist::Xbee::CommunicationManager communication_manager; + const std::string& device = "/dev/ttyUSB0"; // TO DO can be introduced as command. + const std::size_t baud_rate = 230400; // TO DO Can be introduced as command. + Mist::Xbee::CommunicationManager::DRONE_TYPE drone_type = + Mist::Xbee::CommunicationManager::DRONE_TYPE::SLAVE; + Mist::Xbee::CommunicationManager::RUNNING_MODE running_mode = + Mist::Xbee::CommunicationManager::RUNNING_MODE::SOLO; + + if (argc > 1) + { + if (!strcmp(argv[1], "master")) + drone_type = Mist::Xbee::CommunicationManager::DRONE_TYPE::MASTER; + + if (argc > 2) + { + if (!strcmp(argv[2], "swarm")) + running_mode = Mist::Xbee::CommunicationManager::RUNNING_MODE::SWARM; + } + } + + + if (communication_manager.Init(device, baud_rate)) + communication_manager.Run(drone_type, running_mode); + } + catch (const std::exception& e) + { + std::cout << "Error Occured:" << std::endl; + std::cout << e.what() << std::endl; + } + catch (...) + { + std::cout << "Unexpected Fatal Error." << std::endl; + std::cout << "Communication With XBee is Lost." << std::endl; + return EXIT_FAILURE; + } + + std::cout << std::endl; + std::cout << "XBeeMav Exited." << std::endl; + return EXIT_SUCCESS; +} 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; +}