diff --git a/.gitmodules b/.gitmodules index 8f57dde8..fcdc3529 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,6 @@ [submodule "rosflight_io/include/rosflight_io/mavlink/v1.0"] path = rosflight_io/include/rosflight_io/mavlink/v1.0 url = https://github.com/rosflight/mavlink_c_library.git -[submodule "rosflight_firmware/rosflight_firmware"] - path = rosflight_firmware/rosflight_firmware +[submodule "rosflight_sim/include/rosflight_sim/rosflight_firmware"] + path = rosflight_sim/include/rosflight_sim/rosflight_firmware url = https://github.com/rosflight/rosflight_firmware.git diff --git a/rosflight_firmware/CHANGELOG.rst b/rosflight_firmware/CHANGELOG.rst deleted file mode 100644 index 89820e1b..00000000 --- a/rosflight_firmware/CHANGELOG.rst +++ /dev/null @@ -1,32 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rosflight_firmware -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.0.0 (pre-release) ------------------- -* Updated package to ROS2 -* Eliminated all compiler warnings and all applicable clang-tidy warnings -* Reformatted code to match ROS2 style guide -* Re-implemented linter build checking using github actions -* Contributors: Brandon Sutherland - -1.4.0 (2020-10-06) ------------------- -* GNSS improvements -* Contributors: BillThePlatypus - -1.3.1 (2020-03-27) ------------------- -* Hotfix for buildfarm failure with git version commands -* Contributors: Daniel Koch - -1.3.0 (2020-03-19) ------------------- -* Update firmware to v1.3.0 -* Contributors: BillThePlatypus, Cameron McQuinn, Daniel Koch, Jacob Olson, Jacob Willis, James Jackson, Parker Lusk, Trey Henrichsen - -1.0.0 (2018-03-13) ------------------- -* Update firmware to v1.1.0 -* added the firmware library. I think it's right, at least until the firmware changes -* Contributors: Daniel Koch, Gary Ellingson, James Jackson diff --git a/rosflight_firmware/CMakeLists.txt b/rosflight_firmware/CMakeLists.txt deleted file mode 100644 index 0c3ff067..00000000 --- a/rosflight_firmware/CMakeLists.txt +++ /dev/null @@ -1,144 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(rosflight_firmware) - -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -add_compile_options(-Wall -fPIC) - -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE "Release") -endif(NOT CMAKE_BUILD_TYPE) - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(Boost REQUIRED COMPONENTS system thread) - -# clone rosflight_firmware submodule if it is missing -set(FIRMWARE_SUBMODULE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/rosflight_firmware") -if(NOT EXISTS "${FIRMWARE_SUBMODULE_DIR}/.git") - message(STATUS "rosflight_firmware submodule not found at ${FIRMWARE_SUBMODULE_DIR}") - execute_process( - COMMAND git submodule update --init --recursive - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} - ) -endif() - -# Git information -execute_process(COMMAND git rev-parse --short=8 HEAD - OUTPUT_VARIABLE GIT_VERSION_HASH - OUTPUT_STRIP_TRAILING_WHITESPACE - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/rosflight_firmware) - -execute_process(COMMAND git describe --tags --abbrev=8 --always --dirty --long - OUTPUT_VARIABLE GIT_VERSION_STRING - OUTPUT_STRIP_TRAILING_WHITESPACE - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/rosflight_firmware) - -if("${GIT_VERSION_STRING}" STREQUAL "") - set(GIT_VERSION_STRING "undefined") -endif() - -if("${GIT_VERSION_HASH}" STREQUAL "") - set(GIT_VERSION_HASH "0") -endif() - - -########### -## Build ## -########### - -set(FIRMWARE_INCLUDE_DIRS - rosflight_firmware/include/ - rosflight_firmware/lib/ - rosflight_firmware/comms/ - rosflight_firmware/test/ - rosflight_firmware/boards/udp/ - ) - -include_directories(include - ${FIRMWARE_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ) - -# rosflight_firmware -add_library(rosflight_firmware - rosflight_firmware/src/rosflight.cpp - rosflight_firmware/src/nanoprintf.cpp - rosflight_firmware/src/estimator.cpp - rosflight_firmware/src/mixer.cpp - rosflight_firmware/src/controller.cpp - rosflight_firmware/src/param.cpp - rosflight_firmware/src/state_manager.cpp - rosflight_firmware/src/rc.cpp - rosflight_firmware/src/command_manager.cpp - rosflight_firmware/src/sensors.cpp - rosflight_firmware/src/comm_manager.cpp - - rosflight_firmware/comms/mavlink/mavlink.cpp - - rosflight_firmware/lib/turbomath/turbomath.cpp - ) -target_compile_definitions(rosflight_firmware PUBLIC - GIT_VERSION_HASH=0x${GIT_VERSION_HASH} - GIT_VERSION_STRING=\"${GIT_VERSION_STRING}\" - ) -ament_export_targets(rosflight_firmwareTargets HAS_LIBRARY_TARGET) -install( - TARGETS rosflight_firmware - EXPORT rosflight_firmwareTargets - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include -) - -# rosflight_udp_board -add_library(rosflight_udp_board - rosflight_firmware/boards/udp/udp_board.cpp - ) -target_link_libraries(rosflight_udp_board - rosflight_firmware - ${rclcpp_LIBRARIES} - ${ament_LIBRARIES} - ${Boost_LIBRARIES} - ) -ament_export_targets(rosflight_udp_boardTargets HAS_LIBRARY_TARGET) -install( - TARGETS rosflight_udp_board - EXPORT rosflight_udp_boardTargets - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include -) - -# rosflight_test -add_library(rosflight_test - rosflight_firmware/test/test_board.cpp - ) -target_link_libraries(rosflight_test - rosflight_firmware - ) -ament_export_targets(rosflight_testTargets HAS_LIBRARY_TARGET) -install( - TARGETS rosflight_test - EXPORT rosflight_testTargets - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include -) - - -install( - DIRECTORY ${PROJECT_NAME}/ ${FIRMWARE_INCLUDE_DIRS} - DESTINATION include -) - - -ament_package() diff --git a/rosflight_firmware/package.xml b/rosflight_firmware/package.xml deleted file mode 100644 index 8aa00840..00000000 --- a/rosflight_firmware/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - rosflight_firmware - 2.0.0 - Firmware library for software-in-the-loop of the ROSflight ROS stack - - Brandon Sutherland - - Daniel Koch - James Jackson - - BSD - - https://rosflight.org - https://github.com/rosflight/rosflight - https://github.com/rosflight/rosflight/issues - - ament_cmake - - rclcpp - boost - - - ament_cmake - - diff --git a/rosflight_firmware/rosflight_firmware b/rosflight_firmware/rosflight_firmware deleted file mode 160000 index 5df075de..00000000 --- a/rosflight_firmware/rosflight_firmware +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 5df075de758f02ddfe8d316e7999c0679b2a1016 diff --git a/rosflight_pkgs/package.xml b/rosflight_pkgs/package.xml index d5b250df..9fcc615a 100644 --- a/rosflight_pkgs/package.xml +++ b/rosflight_pkgs/package.xml @@ -18,7 +18,6 @@ ament_cmake rosflight_io - rosflight_firmware rosflight_msgs rosflight_sim rosflight_utils diff --git a/rosflight_sim/CMakeLists.txt b/rosflight_sim/CMakeLists.txt index bef7d43b..efa0f9b9 100644 --- a/rosflight_sim/CMakeLists.txt +++ b/rosflight_sim/CMakeLists.txt @@ -8,7 +8,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() -add_compile_options(-Wall) +add_compile_options(-Wall -fPIC) if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE "Release") @@ -21,9 +21,91 @@ find_package(gazebo_plugins REQUIRED) find_package(gazebo_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) -find_package(rosflight_firmware REQUIRED) find_package(rosflight_msgs REQUIRED) find_package(Eigen3 REQUIRED) +find_package(Boost REQUIRED COMPONENTS system thread) + + +############## +## Firmware ## +############## + +set(FIRMWARE_INCLUDE_DIRS + include/rosflight_sim/rosflight_firmware/include/ + include/rosflight_sim/rosflight_firmware/lib/ + include/rosflight_sim/rosflight_firmware/comms/ + include/rosflight_sim/rosflight_firmware/test/ +) + +include_directories( + ${FIRMWARE_INCLUDE_DIRS} +) + +# clone firmware submodule if it is missing +set(FIRMWARE_SUBMODULE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/include/rosflight_sim/rosflight_firmware") +if(NOT EXISTS "${FIRMWARE_SUBMODULE_DIR}/.git") + message(STATUS "Firmware submodule not found at ${FIRMWARE_SUBMODULE_DIR}") + execute_process( + COMMAND git submodule update --init --recursive + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + ) +endif() + +# Git information +execute_process(COMMAND git rev-parse --short=8 HEAD + OUTPUT_VARIABLE GIT_VERSION_HASH + OUTPUT_STRIP_TRAILING_WHITESPACE + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/rosflight_firmware) + +execute_process(COMMAND git describe --tags --abbrev=8 --always --dirty --long + OUTPUT_VARIABLE GIT_VERSION_STRING + OUTPUT_STRIP_TRAILING_WHITESPACE + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/rosflight_firmware) + +if("${GIT_VERSION_STRING}" STREQUAL "") + set(GIT_VERSION_STRING "undefined") +endif() + +if("${GIT_VERSION_HASH}" STREQUAL "") + set(GIT_VERSION_HASH "0") +endif() + +# rosflight_firmware +add_library(rosflight_firmware + include/rosflight_sim/rosflight_firmware/src/rosflight.cpp + include/rosflight_sim/rosflight_firmware/src/nanoprintf.cpp + include/rosflight_sim/rosflight_firmware/src/estimator.cpp + include/rosflight_sim/rosflight_firmware/src/mixer.cpp + include/rosflight_sim/rosflight_firmware/src/controller.cpp + include/rosflight_sim/rosflight_firmware/src/param.cpp + include/rosflight_sim/rosflight_firmware/src/state_manager.cpp + include/rosflight_sim/rosflight_firmware/src/rc.cpp + include/rosflight_sim/rosflight_firmware/src/command_manager.cpp + include/rosflight_sim/rosflight_firmware/src/sensors.cpp + include/rosflight_sim/rosflight_firmware/src/comm_manager.cpp + + include/rosflight_sim/rosflight_firmware/comms/mavlink/mavlink.cpp + + include/rosflight_sim/rosflight_firmware/lib/turbomath/turbomath.cpp +) +target_compile_definitions(rosflight_firmware PUBLIC + GIT_VERSION_HASH=0x${GIT_VERSION_HASH} + GIT_VERSION_STRING=\"${GIT_VERSION_STRING}\" +) +ament_export_targets(rosflight_firmwareTargets HAS_LIBRARY_TARGET) +install( + TARGETS rosflight_firmware + EXPORT rosflight_firmwareTargets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + + +################### +## ROSflight SIL ## +################### include_directories(include ${ament_INCLUDE_DIRS} @@ -31,34 +113,36 @@ include_directories(include ${Eigen_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFormat_INCLUDE_DIRS} - ) + ${Boost_INCLUDE_DIRS} +) link_directories(${GAZEBO_LIBRARY_DIRS}) add_library(rosflight_sil_plugin SHARED src/rosflight_sil.cpp src/sil_board.cpp + src/udp_board.cpp src/multirotor_forces_and_moments.cpp src/fixedwing_forces_and_moments.cpp - ) +) target_link_libraries(rosflight_sil_plugin + rosflight_firmware ${rclcpp_INLCUDE_DIRS} ${ament_INCLUDE_DIRS} ${GAZEBO_LIBRARIES} - ) + ${Boost_LIBRARIES} +) ament_target_dependencies(rosflight_sil_plugin rclcpp geometry_msgs nav_msgs - rosflight_firmware rosflight_msgs gazebo_ros - ) +) ament_export_targets(rosflight_sil_pluginTargets HAS_LIBRARY_TARGET) ament_export_dependencies( rclcpp geometry_msgs nav_msgs - rosflight_firmware rosflight_msgs gazebo_ros ) @@ -72,12 +156,16 @@ install( INCLUDES DESTINATION include ) + +############# +## Install ## +############# + install( DIRECTORY include/${PROJECT_NAME} DESTINATION include/${PROJECT_NAME} ) -# Install launch files. install( DIRECTORY launch DESTINATION share/${PROJECT_NAME} diff --git a/rosflight_sim/include/rosflight_sim/rosflight_firmware b/rosflight_sim/include/rosflight_sim/rosflight_firmware new file mode 160000 index 00000000..1d20caaf --- /dev/null +++ b/rosflight_sim/include/rosflight_sim/rosflight_firmware @@ -0,0 +1 @@ +Subproject commit 1d20caaf27744e4238e107111b4b11f475483235 diff --git a/rosflight_sim/include/rosflight_sim/sil_board.hpp b/rosflight_sim/include/rosflight_sim/sil_board.hpp index 129cbcba..28e563ac 100644 --- a/rosflight_sim/include/rosflight_sim/sil_board.hpp +++ b/rosflight_sim/include/rosflight_sim/sil_board.hpp @@ -48,8 +48,7 @@ #include #include -#include - +#include #include namespace rosflight_sim @@ -59,7 +58,7 @@ namespace rosflight_sim * actuators, and FCU clock and memory for the firmware. It also adds a simulated serial delay. It * inherits from UDP board, which establishes a communication link over UDP. */ -class SILBoard : public rosflight_firmware::UDPBoard +class SILBoard : public UDPBoard { private: GazeboVector inertial_magnetic_field_; diff --git a/rosflight_sim/include/rosflight_sim/udp_board.hpp b/rosflight_sim/include/rosflight_sim/udp_board.hpp new file mode 100644 index 00000000..b347935c --- /dev/null +++ b/rosflight_sim/include/rosflight_sim/udp_board.hpp @@ -0,0 +1,123 @@ +/* + * Copyright (c) 2017 Daniel Koch, BYU MAGICC Lab. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** + * \file udp_board.hpp + * \author Daniel Koch + */ + +#ifndef ROSFLIGHT_SIM_UDP_BOARD_HPP +#define ROSFLIGHT_SIM_UDP_BOARD_HPP + +#include +#include + +#include +#include + +#include "board.h" +#include "mavlink/mavlink.h" + +namespace rosflight_sim +{ +class UDPBoard : public rosflight_firmware::Board +{ +public: + explicit UDPBoard(std::string bind_host = "localhost", uint16_t bind_port = 14525, + std::string remote_host = "localhost", uint16_t remote_port = 14520); + ~UDPBoard(); + + void serial_init(uint32_t baud_rate, uint32_t dev) override; + void serial_write(const uint8_t * src, size_t len) override; + uint16_t serial_bytes_available() override; + uint8_t serial_read() override; + void serial_flush() override; + + void set_ports(std::string bind_host, uint16_t bind_port, std::string remote_host, + uint16_t remote_port); + +private: + struct Buffer + { + uint8_t data[MAVLINK_MAX_PACKET_LEN] = {0}; + size_t len; + size_t pos; + + Buffer() : len(0), pos(0) {} + + Buffer(const uint8_t * src, size_t length) : len(length), pos(0) + { + assert(length <= MAVLINK_MAX_PACKET_LEN); //! \todo Do something less catastrophic here + memcpy(data, src, length); + } + + const uint8_t * dpos() const { return data + pos; } + size_t nbytes() const { return len - pos; } + void add_byte(uint8_t byte) { data[len++] = byte; } + uint8_t consume_byte() { return data[pos++]; } + bool empty() const { return pos >= len; } + bool full() const { return len >= MAVLINK_MAX_PACKET_LEN; } + }; + + typedef boost::lock_guard MutexLock; + + void async_read(); + void async_read_end(const boost::system::error_code & error, size_t bytes_transferred); + + void async_write(bool check_write_state); + void async_write_end(const boost::system::error_code & error, size_t bytes_transferred); + + std::string bind_host_; + uint16_t bind_port_; + + std::string remote_host_; + uint16_t remote_port_; + + boost::thread io_thread_; + boost::recursive_mutex write_mutex_; + boost::recursive_mutex read_mutex_; + + boost::asio::io_service io_service_; + + boost::asio::ip::udp::socket socket_; + boost::asio::ip::udp::endpoint bind_endpoint_; + boost::asio::ip::udp::endpoint remote_endpoint_; + + uint8_t read_buffer_[MAVLINK_MAX_PACKET_LEN] = {0}; + std::list read_queue_; + + std::list write_queue_; + bool write_in_progress_; +}; + +} // namespace rosflight_sim + +#endif // ROSFLIGHT_SIM_UDP_BOARD_HPP diff --git a/rosflight_sim/package.xml b/rosflight_sim/package.xml index baa10580..8d965ce3 100644 --- a/rosflight_sim/package.xml +++ b/rosflight_sim/package.xml @@ -25,7 +25,6 @@ gazebo_ros geometry_msgs nav_msgs - rosflight_firmware rosflight_msgs eigen diff --git a/rosflight_sim/src/sil_board.cpp b/rosflight_sim/src/sil_board.cpp index 8c698675..92800893 100644 --- a/rosflight_sim/src/sil_board.cpp +++ b/rosflight_sim/src/sil_board.cpp @@ -40,7 +40,7 @@ namespace rosflight_sim { SILBoard::SILBoard() - : rosflight_firmware::UDPBoard(), + : UDPBoard(), random_generator_(std::chrono::system_clock::now().time_since_epoch().count()) {} diff --git a/rosflight_sim/src/udp_board.cpp b/rosflight_sim/src/udp_board.cpp new file mode 100644 index 00000000..8adf92c9 --- /dev/null +++ b/rosflight_sim/src/udp_board.cpp @@ -0,0 +1,190 @@ +/* + * Copyright (c) 2017 Daniel Koch, BYU MAGICC Lab. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** + * \file udp_board.cpp + * \author Daniel Koch + */ + +#include +#include "rosflight_sim/udp_board.hpp" + +using boost::asio::ip::udp; + +namespace rosflight_sim +{ +UDPBoard::UDPBoard(std::string bind_host, uint16_t bind_port, std::string remote_host, + uint16_t remote_port) + : bind_host_(std::move(bind_host)), bind_port_(bind_port), remote_host_(std::move(remote_host)), + remote_port_(remote_port), io_service_(), socket_(io_service_), write_in_progress_(false) +{} + +UDPBoard::~UDPBoard() +{ + MutexLock read_lock(read_mutex_); + MutexLock write_lock(write_mutex_); + + io_service_.stop(); + socket_.close(); + + if (io_thread_.joinable()) { io_thread_.join(); } +} + +void UDPBoard::set_ports(std::string bind_host, uint16_t bind_port, std::string remote_host, + uint16_t remote_port) +{ + bind_host_ = std::move(bind_host); + bind_port_ = bind_port; + remote_host_ = std::move(remote_host); + remote_port_ = remote_port; +} + +void UDPBoard::serial_init(uint32_t baud_rate, uint32_t dev) +{ + // can throw an uncaught boost::system::system_error exception + (void) dev; + + udp::resolver resolver(io_service_); + + bind_endpoint_ = *resolver.resolve({udp::v4(), bind_host_, ""}); + bind_endpoint_.port(bind_port_); + + remote_endpoint_ = *resolver.resolve({udp::v4(), remote_host_, ""}); + remote_endpoint_.port(remote_port_); + + socket_.open(udp::v4()); + socket_.bind(bind_endpoint_); + + socket_.set_option(udp::socket::reuse_address(true)); + socket_.set_option(udp::socket::send_buffer_size(1000 * MAVLINK_MAX_PACKET_LEN)); + socket_.set_option(udp::socket::receive_buffer_size(1000 * MAVLINK_MAX_PACKET_LEN)); + + async_read(); + io_thread_ = boost::thread(boost::bind(&boost::asio::io_service::run, &io_service_)); +} + +void UDPBoard::serial_flush() {} + +void UDPBoard::serial_write(const uint8_t * src, size_t len) +{ + auto buffer = new Buffer(src, len); + + { + MutexLock lock(write_mutex_); + write_queue_.push_back(buffer); + } + + async_write(true); +} + +uint16_t UDPBoard::serial_bytes_available() +{ + MutexLock lock(read_mutex_); + return !read_queue_.empty(); //! \todo This should return a number, not a bool +} + +uint8_t UDPBoard::serial_read() +{ + MutexLock lock(read_mutex_); + + if (read_queue_.empty()) { return 0; } + + Buffer * buffer = read_queue_.front(); + uint8_t byte = buffer->consume_byte(); + + if (buffer->empty()) { + read_queue_.pop_front(); + delete buffer; + } + return byte; +} + +void UDPBoard::async_read() +{ + if (!socket_.is_open()) { return; } + + MutexLock lock(read_mutex_); + socket_.async_receive_from( + boost::asio::buffer(read_buffer_, MAVLINK_MAX_PACKET_LEN), remote_endpoint_, + boost::bind(&UDPBoard::async_read_end, this, boost::asio::placeholders::error, + boost::asio::placeholders::bytes_transferred)); +} + +void UDPBoard::async_read_end(const boost::system::error_code & error, size_t bytes_transferred) +{ + if (!error) { + MutexLock lock(read_mutex_); + read_queue_.push_back(new Buffer(read_buffer_, bytes_transferred)); + } + async_read(); +} + +void UDPBoard::async_write(bool check_write_state) +{ + if (check_write_state && write_in_progress_) { return; } + + MutexLock lock(write_mutex_); + if (write_queue_.empty()) { return; } + + write_in_progress_ = true; + Buffer * buffer = write_queue_.front(); + socket_.async_send_to(boost::asio::buffer(buffer->dpos(), buffer->nbytes()), remote_endpoint_, + boost::bind(&UDPBoard::async_write_end, this, + boost::asio::placeholders::error, + boost::asio::placeholders::bytes_transferred)); +} + +void UDPBoard::async_write_end(const boost::system::error_code & error, size_t bytes_transferred) +{ + if (!error) { + MutexLock lock(write_mutex_); + + if (write_queue_.empty()) { + write_in_progress_ = false; + return; + } + + Buffer * buffer = write_queue_.front(); + buffer->pos += bytes_transferred; + if (buffer->empty()) { + write_queue_.pop_front(); + delete buffer; + } + + if (write_queue_.empty()) { + write_in_progress_ = false; + } else { + async_write(false); + } + } +} + +} // namespace rosflight_sim diff --git a/rosflight_utils/CMakeLists.txt b/rosflight_utils/CMakeLists.txt index e8f90278..91dd0b12 100644 --- a/rosflight_utils/CMakeLists.txt +++ b/rosflight_utils/CMakeLists.txt @@ -28,14 +28,14 @@ find_package(tf2_geometry_msgs REQUIRED) include_directories(include ${ament_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS} - ) +) # viz executable add_executable(viz src/viz.cpp) target_link_libraries(viz ${rclcpp_LIBRARIES} ${ament_LIBRARIES} - ) +) ament_target_dependencies(viz geometry_msgs rosflight_msgs @@ -43,7 +43,7 @@ ament_target_dependencies(viz tf2 tf2_geometry_msgs visualization_msgs - ) +) # Install header files install( @@ -59,14 +59,13 @@ install( RUNTIME DESTINATION lib/${PROJECT_NAME} ) -# TODO: Update remaining python scripts to ROS2 # Install python scripts install(PROGRAMS src/command_joy.py src/rc_joy.py src/rc_sim.py DESTINATION lib/${PROJECT_NAME} - ) +) # Install launch files install( diff --git a/rosflight_utils/package.xml b/rosflight_utils/package.xml index 4d724bde..e48518d8 100644 --- a/rosflight_utils/package.xml +++ b/rosflight_utils/package.xml @@ -30,7 +30,6 @@ rosflight_msgs rosflight_sim - rosflight_firmware rosflight_io