From 7d15338d31fe82b7712a33e73f3a55be26da3538 Mon Sep 17 00:00:00 2001 From: hhenry01 Date: Sat, 9 Mar 2024 14:03:11 -0800 Subject: [PATCH] latest changes --- src/network_systems/functions.cmake | 4 +- src/network_systems/lib/CMakeLists.txt | 1 + .../lib/net_node/CMakeLists.txt | 15 ++ .../lib/net_node/inc/net_node.h | 53 ++++++ .../lib/net_node/src/net_node.cpp | 42 +++++ .../lib/sailbot_db/src/sailbot_db.cpp | 2 +- .../src/can_transceiver_ros_intf.cpp | 21 ++- .../example/src/cached_fib_ros_intf.cpp | 21 ++- .../projects/local_transceiver/CMakeLists.txt | 1 + .../projects/local_transceiver/inc/at_cmds.h | 81 ++++++-- .../local_transceiver/inc/local_transceiver.h | 67 ++++--- .../src/local_transceiver.cpp | 177 ++++++++++++++---- .../src/local_transceiver_ros_intf.cpp | 111 +++++++++-- .../test/test_local_transceiver.cpp | 147 +++++++++++++-- .../mock_ais/src/mock_ais_ros_intf.cpp | 21 ++- .../src/remote_transceiver_ros_intf.cpp | 20 +- src/network_systems/ros_info.txt | 1 + src/network_systems/scripts/README.md | 2 +- .../scripts/http_echo_server.py | 31 +++ 19 files changed, 690 insertions(+), 128 deletions(-) create mode 100644 src/network_systems/lib/net_node/CMakeLists.txt create mode 100644 src/network_systems/lib/net_node/inc/net_node.h create mode 100644 src/network_systems/lib/net_node/src/net_node.cpp create mode 100644 src/network_systems/scripts/http_echo_server.py diff --git a/src/network_systems/functions.cmake b/src/network_systems/functions.cmake index 92d040848..5f989e8cf 100644 --- a/src/network_systems/functions.cmake +++ b/src/network_systems/functions.cmake @@ -20,12 +20,13 @@ function(make_exe module srcs link_libs inc_dirs compile_defs) add_executable(${bin_module} ${srcs}) target_compile_definitions(${bin_module} PUBLIC ${compile_defs}) ament_target_dependencies(${bin_module} PUBLIC ${ROS_DEPS}) - target_link_libraries(${bin_module} PUBLIC ${link_libs} boost_program_options) + target_link_libraries(${bin_module} PUBLIC ${link_libs} boost_program_options net_node) target_include_directories( ${bin_module} PUBLIC ${CMAKE_CURRENT_LIST_DIR}/inc ${CMAKE_SOURCE_DIR}/lib ${inc_dirs} + ${net_node_inc_dir} ) add_dependencies(${bin_module} ${AUTOGEN_TARGETS}) install(TARGETS ${bin_module} DESTINATION lib/${PROJECT_NAME}) @@ -50,5 +51,6 @@ function(make_unit_test module srcs link_libs inc_dirs compile_defs) add_dependencies(${test_module} ${AUTOGEN_TARGETS}) # Make the unit test runnable with CTest (invoked via test.sh) add_test(NAME ${test_module} COMMAND ${test_module}) + set_tests_properties(${test_module} PROPERTIES TIMEOUT 60) # 1 minute per test timeout endif() endfunction() diff --git a/src/network_systems/lib/CMakeLists.txt b/src/network_systems/lib/CMakeLists.txt index d05824d0c..7e291a67c 100755 --- a/src/network_systems/lib/CMakeLists.txt +++ b/src/network_systems/lib/CMakeLists.txt @@ -1,4 +1,5 @@ add_subdirectory(cmn_hdrs) +add_subdirectory(net_node) add_subdirectory(protofiles) add_subdirectory(sailbot_db) diff --git a/src/network_systems/lib/net_node/CMakeLists.txt b/src/network_systems/lib/net_node/CMakeLists.txt new file mode 100644 index 000000000..ef4adf278 --- /dev/null +++ b/src/network_systems/lib/net_node/CMakeLists.txt @@ -0,0 +1,15 @@ +set(module net_node) + +set(link_libs +) + +set(inc_dirs +) + +set(compile_defs +) + +set(srcs + ${CMAKE_CURRENT_LIST_DIR}/src/net_node.cpp +) +make_lib(${module} "${srcs}" "${link_libs}" "${inc_dirs}" "${compile_defs}") diff --git a/src/network_systems/lib/net_node/inc/net_node.h b/src/network_systems/lib/net_node/inc/net_node.h new file mode 100644 index 000000000..c53cd16e9 --- /dev/null +++ b/src/network_systems/lib/net_node/inc/net_node.h @@ -0,0 +1,53 @@ +#pragma once +#include + +/** + * Network Systems custom ROS Node. + * Redirects std::cout and std::cerr streams to ROS logging macros. + * @note An extra newline character is added to ROS logs from redirected streams. Not the prettiest, but it's harmless. + * Errors are extra ugly, with multiple empty newlines. It does make them extra obvious though. + */ +class NetNode : public rclcpp::Node +{ +public: + /** + * @brief Initialize a NetNode's buffers and stream redirections + * + * @param node_name Name of the node to pass to the base rclcpp::Node + */ + explicit NetNode(const std::string & node_name); + ~NetNode(); + +private: + /** + * @brief Custom string buffer that flushes its contents to ROS logging macros. + * https://stackoverflow.com/a/14232652 + */ + class LogBuf : public std::stringbuf + { + public: + /** + * @brief Initialize a LogBuf for the NetNode + * + * @param fd File descriptor. Must be either STDOUT_FILENO or STDERR_FILENO + * @param logger Logger object of from the encompassing NetNode + */ + LogBuf(int fd, rclcpp::Logger logger); + + /** + * @brief Called when the buffer is flushed. Invokes ROS logging macros and clears buffer. + * + * @return 0 + */ + virtual int sync(); + + private: + const int fd_; // File descriptor to redirect (either STDOUT_FILENO or STDERR_FILENO) + const rclcpp::Logger logger_; // Logger instance from the encompassing NetNode + }; + + std::streambuf * og_stdout_buf_; // Original buffer for stdout + std::streambuf * og_stderr_buf_; // Original buffer for stderr + LogBuf new_stdout_buf_; // LogBuf to redirect stdout to + LogBuf new_stderr_buf_; // LogBuf to redirect stderr to +}; diff --git a/src/network_systems/lib/net_node/src/net_node.cpp b/src/network_systems/lib/net_node/src/net_node.cpp new file mode 100644 index 000000000..405742ab6 --- /dev/null +++ b/src/network_systems/lib/net_node/src/net_node.cpp @@ -0,0 +1,42 @@ +#include "net_node.h" + +#include + +NetNode::NetNode(const std::string & node_name) +: rclcpp::Node(node_name), + // When constructing the new buffers, create a new rclcpp::Logger with ":net_node" appended to the name so that + // we can identify that redirected messages are output from this file, rather than the _ros_intf.cpp file. + // Hopefully this will help reduce confusion with respect to the printed line numbers. + new_stdout_buf_(STDOUT_FILENO, rclcpp::get_logger(std::string(this->get_logger().get_name()) + ":net_node")), + new_stderr_buf_(STDERR_FILENO, rclcpp::get_logger(std::string(this->get_logger().get_name()) + ":net_node")) +{ + og_stdout_buf_ = std::cout.rdbuf(); + og_stderr_buf_ = std::cerr.rdbuf(); + std::cout.rdbuf(&new_stdout_buf_); + std::cerr.rdbuf(&new_stderr_buf_); +} + +NetNode::~NetNode() +{ + std::cout.rdbuf(og_stdout_buf_); + std::cerr.rdbuf(og_stderr_buf_); +} + +NetNode::LogBuf::LogBuf(int fd, rclcpp::Logger logger) : fd_(fd), logger_(logger) {} + +int NetNode::LogBuf::sync() +{ + switch (fd_) { + case STDOUT_FILENO: + RCLCPP_INFO(logger_, "%s", this->str().c_str()); + break; + case STDERR_FILENO: + RCLCPP_ERROR(logger_, "%s", this->str().c_str()); + break; + default: + // unreachable + throw std::runtime_error("Invalid file descriptor! " + std::to_string(fd_)); + } + this->str(""); + return 0; +} diff --git a/src/network_systems/lib/sailbot_db/src/sailbot_db.cpp b/src/network_systems/lib/sailbot_db/src/sailbot_db.cpp index 588b5a090..8589cb31e 100644 --- a/src/network_systems/lib/sailbot_db/src/sailbot_db.cpp +++ b/src/network_systems/lib/sailbot_db/src/sailbot_db.cpp @@ -63,7 +63,7 @@ bool SailbotDB::testConnection() db.run_command(ping_cmd.view()); return true; } catch (const std::exception & e) { - std::cout << "Exception: " << e.what() << std::endl; + std::cerr << "Exception: " << e.what() << std::endl; return false; } } diff --git a/src/network_systems/projects/can_transceiver/src/can_transceiver_ros_intf.cpp b/src/network_systems/projects/can_transceiver/src/can_transceiver_ros_intf.cpp index 35b96d6a5..62277556e 100644 --- a/src/network_systems/projects/can_transceiver/src/can_transceiver_ros_intf.cpp +++ b/src/network_systems/projects/can_transceiver/src/can_transceiver_ros_intf.cpp @@ -15,6 +15,7 @@ #include "can_transceiver.h" #include "cmn_hdrs/ros_info.h" #include "cmn_hdrs/shared_constants.h" +#include "net_node.h" constexpr int QUEUE_SIZE = 10; // Arbitrary number constexpr auto TIMER_INTERVAL = std::chrono::milliseconds(500); @@ -23,10 +24,10 @@ namespace msg = custom_interfaces::msg; using CAN_FP::CanFrame; using CAN_FP::CanId; -class CanTransceiverIntf : public rclcpp::Node +class CanTransceiverIntf : public NetNode { public: - CanTransceiverIntf() : Node("can_transceiver_node") + CanTransceiverIntf() : NetNode("can_transceiver_node") { this->declare_parameter("enabled", true); @@ -188,8 +189,20 @@ class CanTransceiverIntf : public rclcpp::Node int main(int argc, char * argv[]) { + bool err = false; rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + try { + std::shared_ptr node = std::make_shared(); + try { + rclcpp::spin(node); + } catch (std::exception & e) { + RCLCPP_ERROR(node->get_logger(), "%s", e.what()); + throw e; + } + } catch (std::exception & e) { + std::cerr << e.what() << std::endl; + err = true; + } rclcpp::shutdown(); - return 0; + return err ? -1 : 0; } diff --git a/src/network_systems/projects/example/src/cached_fib_ros_intf.cpp b/src/network_systems/projects/example/src/cached_fib_ros_intf.cpp index 4b90e6a39..d032a2d64 100644 --- a/src/network_systems/projects/example/src/cached_fib_ros_intf.cpp +++ b/src/network_systems/projects/example/src/cached_fib_ros_intf.cpp @@ -1,5 +1,6 @@ // Include this module #include "cached_fib.h" +#include "net_node.h" // Include ROS headers #include #include @@ -10,10 +11,10 @@ constexpr int ROS_Q_SIZE = 10; constexpr int INIT_FIB_SIZE = 5; } // namespace -class CachedFibNode : public rclcpp::Node +class CachedFibNode : public NetNode { public: - explicit CachedFibNode(const std::size_t initSize) : Node("cached_fib_node"), c_fib_(initSize) + explicit CachedFibNode(const std::size_t initSize) : NetNode("cached_fib_node"), c_fib_(initSize) { this->declare_parameter("enabled", false); bool enabled = this->get_parameter("enabled").as_bool(); @@ -50,8 +51,20 @@ class CachedFibNode : public rclcpp::Node int main(int argc, char * argv[]) { + bool err = false; rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared(INIT_FIB_SIZE)); + try { + std::shared_ptr node = std::make_shared(INIT_FIB_SIZE); + try { + rclcpp::spin(node); + } catch (std::exception & e) { + RCLCPP_ERROR(node->get_logger(), "%s", e.what()); + throw e; + } + } catch (std::exception & e) { + std::cerr << e.what() << std::endl; + err = true; + } rclcpp::shutdown(); - return 0; + return err ? -1 : 0; } diff --git a/src/network_systems/projects/local_transceiver/CMakeLists.txt b/src/network_systems/projects/local_transceiver/CMakeLists.txt index be94351de..ee423d8cf 100644 --- a/src/network_systems/projects/local_transceiver/CMakeLists.txt +++ b/src/network_systems/projects/local_transceiver/CMakeLists.txt @@ -11,6 +11,7 @@ set(inc_dirs set(compile_defs LOCAL_TRANSCEIVER_TEST_PORT="$ENV{LOCAL_TRANSCEIVER_TEST_PORT}" RUN_VIRTUAL_IRIDIUM_SCRIPT_PATH="$ENV{ROS_WORKSPACE}/src/network_systems/scripts/run_virtual_iridium.sh" + RUN_HTTP_ECHO_SERVER_CMD="python3 $ENV{ROS_WORKSPACE}/src/network_systems/scripts/http_echo_server.py" ) set(srcs diff --git a/src/network_systems/projects/local_transceiver/inc/at_cmds.h b/src/network_systems/projects/local_transceiver/inc/at_cmds.h index 1e1c18999..b3a4d64e3 100644 --- a/src/network_systems/projects/local_transceiver/inc/at_cmds.h +++ b/src/network_systems/projects/local_transceiver/inc/at_cmds.h @@ -4,26 +4,61 @@ // Section numbers in this header file refer to this document #include +#include #include +#include namespace AT { -const std::string DELIMITER = "\r"; + +const std::string DELIMITER = "\r\n"; const std::string STATUS_OK = "OK"; +const std::string RSP_READY = "READY"; + +const std::string CHECK_CONN = "AT"; +const std::string SBD_SESSION = "AT+SBDIX"; // 5.144 + +namespace write_bin // 5.154 +{ +const std::string CMD = "AT+SBDWB="; -const std::string CHECK_CONN = "AT" + DELIMITER; -const std::string SBD_SESSION = "AT+SBDIX" + DELIMITER; // 5.144 +namespace rsp +{ +const std::string SUCCESS = "0"; +const std::string TIMEOUT = "1"; +const std::string BAD_CHECKSUM = "2"; +const std::string WRONG_SIZE = "3"; +} // namespace rsp +} // namespace write_bin + +/** + * @brief Simple Line struct to help enforce DRY when dealing with strings while performing reads and writes + * + */ +struct Line +{ + /** + * @param str valid AT command or response string + */ + inline explicit Line(const std::string & str) + : str_((str == AT::DELIMITER || str == "\n" || str == "\r") ? str : (str + "\r")) + { + } + // In most cases, str_ will just be the input str to the constructor + "\r" + // AT::DELIMITER, \n, and \r are exceptions, and remain the same + const std::string str_; +}; /** * Struct representing the response to the CHECK_STATUS command * 5.144 */ -struct SBDStatusResponse // TODO(Jng468): Implement this class +struct SBDStatusRsp // TODO(Jng468): Implement this class { static constexpr uint8_t MO_SUCCESS_START = 0; - static constexpr uint8_t MO_SUCCESS_END = 5; + static constexpr uint8_t MO_SUCCESS_END = 4; - uint8_t MO_status_; + uint8_t MO_status_; // indicates if MO message is transferred successfully [1, 4] uint16_t MOMSN_; uint8_t MT_status_; uint16_t MTMSN_; @@ -33,17 +68,31 @@ struct SBDStatusResponse // TODO(Jng468): Implement this class /** * @brief Construct a new Status Response object * - * @param rsp_string string of format "+SBDIX:,,,,,"" + * @param rsp_string string of format "+SBDIX:,,,,," */ - explicit SBDStatusResponse(const std::string & rsp_string) + explicit SBDStatusRsp(const std::string & rsp_string) { - (void)rsp_string; - MO_status_ = 0; - MOMSN_ = 0; - MT_status_ = 0; - MTMSN_ = 0; - MT_len_ = 0; - MT_queued_ = 0; + size_t begin_point = rsp_string.find(':'); + std::string data = rsp_string.substr(begin_point + 1); + std::vector tokens; + + size_t start = 0; + size_t end; + while ((end = data.find(',', start)) != std::string::npos) { + tokens.push_back(data.substr(start, end - start)); + start = end + 1; + } + tokens.push_back(data.substr(start)); + + // assign index numbers + enum { MO_STATUS_INDEX, MOMSN_INDEX, MT_STATUS_INDEX, MTMSN_INDEX, MT_LEN_INDEX, MT_QUEUED_INDEX }; + + MO_status_ = std::stoi(tokens[MO_STATUS_INDEX]); + MOMSN_ = std::stoi(tokens[MOMSN_INDEX]); + MT_status_ = std::stoi(tokens[MT_STATUS_INDEX]); + MTMSN_ = std::stoi(tokens[MTMSN_INDEX]); + MT_len_ = std::stoi(tokens[MT_LEN_INDEX]); + MT_queued_ = std::stoi(tokens[MT_QUEUED_INDEX]); }; /** @@ -52,7 +101,7 @@ struct SBDStatusResponse // TODO(Jng468): Implement this class * @return true on success * @return false on failure */ - bool MOSuccess() const { return MO_status_ < MO_SUCCESS_END; } + bool MOSuccess() const { return MO_status_ <= MO_SUCCESS_END; } }; } // namespace AT diff --git a/src/network_systems/projects/local_transceiver/inc/local_transceiver.h b/src/network_systems/projects/local_transceiver/inc/local_transceiver.h index ac269e829..7bcf66947 100644 --- a/src/network_systems/projects/local_transceiver/inc/local_transceiver.h +++ b/src/network_systems/projects/local_transceiver/inc/local_transceiver.h @@ -1,16 +1,18 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include #include +#include "at_cmds.h" +#include "boost/asio/io_service.hpp" +#include "boost/asio/serial_port.hpp" +#include "custom_interfaces/msg/ais_ships.hpp" +#include "custom_interfaces/msg/batteries.hpp" +#include "custom_interfaces/msg/generic_sensors.hpp" +#include "custom_interfaces/msg/gps.hpp" +#include "custom_interfaces/msg/l_path_data.hpp" +#include "custom_interfaces/msg/wind_sensors.hpp" +#include "rclcpp/node.hpp" #include "sensors.pb.h" namespace msg = custom_interfaces::msg; @@ -109,9 +111,9 @@ class LocalTransceiver * @brief Send a debug command and return the output * * @param cmd string to send to the serial port - * @return output of the sent cmd + * @return response to the sent command as a string if successful, std::nullopt on failure */ - std::string debugSend(const std::string & cmd); + std::optional debugSend(const std::string & cmd); /** * @brief Retrieve the latest message from the remote server via the serial port @@ -121,6 +123,12 @@ class LocalTransceiver std::string receive(); private: + // Serial port read/write timeout + constexpr static const struct timeval TIMEOUT + { + 0, // seconds + 200000 // microseconds + }; // boost io service - required for boost::asio operations boost::asio::io_service io_; // serial port data where is sent and received @@ -133,36 +141,45 @@ class LocalTransceiver * * @param cmd command to send */ - void send(const std::string & cmd); + bool send(const AT::Line & cmd); /** - * @brief Parse the message received from the remote server + * @brief Read responses from serial * - * @param msg message received from the remote server - * @return the data byte string payload from the message + * @param expected_rsps expected responses + * @return true if all expected responses are read successfully, false otherwise */ - static std::string parseInMsg(const std::string & msg); + bool rcvRsps(std::initializer_list expected_rsps); + + bool rcvRsp(const AT::Line & expected_rsp); + + std::optional readRsp(); /** - * @brief Read a line from serial + * @brief Parse the message received from the remote server * - * @return line + * @param msg message received from the remote server + * @return the data byte string payload from the message */ - std::string readLine(); + static std::string parseInMsg(const std::string & msg); /** - * @brief Check that the last command sent to serial was valid + * @brief Convert a boost::asio::streambuf into a string + * @warning Flushes the streambuf object * - * @return true if valid - * @return false if invalid + * @param buf streambuf to convert + * @return buf contents as a string */ - bool checkOK(); + static std::string streambufToStr(boost::asio::streambuf & buf); /** - * @brief Compute a checksum + * @brief Compute the checksum of a binary data string. + * The checksum is the least significant 2 bytes of the + * sum of all bytes in the data string, where each character + * is one byte. * * @param data data string - * @return checksum as a string + * @return checksum value */ static std::string checksum(const std::string & data); }; diff --git a/src/network_systems/projects/local_transceiver/src/local_transceiver.cpp b/src/network_systems/projects/local_transceiver/src/local_transceiver.cpp index c24cc5ec9..943bb0de6 100644 --- a/src/network_systems/projects/local_transceiver/src/local_transceiver.cpp +++ b/src/network_systems/projects/local_transceiver/src/local_transceiver.cpp @@ -1,5 +1,7 @@ #include "local_transceiver.h" +#include +#include #include #include #include @@ -22,6 +24,7 @@ #include "sensors.pb.h" #include "waypoint.pb.h" +using boost::system::error_code; using Polaris::Sensors; namespace bio = boost::asio; @@ -82,9 +85,9 @@ void LocalTransceiver::updateSensor(msg::GenericSensors msg) void LocalTransceiver::updateSensor(msg::LPathData localData) { sensors_.clear_local_path_data(); + Sensors::Path * new_local = sensors_.mutable_local_path_data(); for (const msg::HelperLatLon & local_data : localData.local_path.waypoints) { - Sensors::Path * new_local = sensors_.mutable_local_path_data(); - Polaris::Waypoint * waypoint = new_local->add_waypoints(); + Polaris::Waypoint * waypoint = new_local->add_waypoints(); waypoint->set_latitude(local_data.latitude); waypoint->set_longitude(local_data.longitude); } @@ -95,6 +98,8 @@ Sensors LocalTransceiver::sensors() { return sensors_; } LocalTransceiver::LocalTransceiver(const std::string & port_name, const uint32_t baud_rate) : serial_(io_, port_name) { serial_.set_option(bio::serial_port_base::baud_rate(baud_rate)); + // Set a timeout for read/write operations on the serial port + setsockopt(serial_.native_handle(), SOL_SOCKET, SO_RCVTIMEO, &TIMEOUT, sizeof(TIMEOUT)); }; LocalTransceiver::~LocalTransceiver() @@ -130,48 +135,108 @@ bool LocalTransceiver::send() throw std::length_error(err_string); } - static constexpr int MAX_NUM_RETRIES = 20; + std::string write_bin_cmd_str = AT::write_bin::CMD + std::to_string(data.size()); //according to specs + AT::Line at_write_cmd(write_bin_cmd_str); + + static constexpr int MAX_NUM_RETRIES = 20; // allow retries because the connection is imperfect for (int i = 0; i < MAX_NUM_RETRIES; i++) { - std::string sbdwbCommand = "AT+SBDWB=" + std::to_string(data.size()) + "\r"; - send(sbdwbCommand + data + "\r"); + if (!send(at_write_cmd)) { + continue; + } + + if (!rcvRsps({ + at_write_cmd, + AT::Line(AT::DELIMITER), + AT::Line(AT::RSP_READY), + AT::Line("\n"), + })) { + continue; + } + + std::string msg_str = data + checksum(data); + AT::Line msg(msg_str); + if (!send(msg)) { + continue; + } - std::string checksumCommand = std::to_string(data.size()) + checksum(data) + "\r"; - send(data + "+" + checksumCommand + "\r"); + if (!rcvRsps({ + AT::Line(AT::DELIMITER), + AT::Line(AT::write_bin::rsp::SUCCESS), + AT::Line("\n"), + AT::Line(AT::DELIMITER), + AT::Line(AT::STATUS_OK), + AT::Line("\n"), + })) { + continue; + } // Check SBD Session status to see if data was sent successfully - send(AT::SBD_SESSION); - std::string rsp_str = readLine(); - readLine(); // empty line after response - if (checkOK()) { - try { - AT::SBDStatusResponse rsp(rsp_str); - if (rsp.MOSuccess()) { - return true; - } - } catch (std::invalid_argument & e) { - /* Catch response parsing exceptions */ - } + // NEEDS AN ACTIVE SERVER ON $WEBHOOK_SERVER_ENDPOINT OR VIRTUAL IRIDIUM WILL CRASH + static const AT::Line sbdix_cmd = AT::Line(AT::SBD_SESSION); + if (!send(sbdix_cmd)) { + continue; + } + + if (!rcvRsps({ + AT::Line("\r"), + sbdix_cmd, + AT::Line(AT::DELIMITER), + })) { + continue; + } + + auto opt_rsp = readRsp(); + if (!opt_rsp) { + continue; + } + + // This string will look something like: + // "+SBDIX:,,,,,\r\n\r\nOK\r" + // on success + // Don't bother to check for OK\r as MO status will tell us if it succeeded or not + std::string opt_rsp_val = opt_rsp.value(); + std::vector sbd_status_vec; + boost::algorithm::split(sbd_status_vec, opt_rsp_val, boost::is_any_of(AT::DELIMITER)); + + AT::SBDStatusRsp rsp(sbd_status_vec[0]); + if (rsp.MOSuccess()) { + return true; } } + std::cerr << "Failed to transmit data to satellite!" << std::endl; + std::cerr << sensors.DebugString() << std::endl; return false; } -std::string LocalTransceiver::debugSend(const std::string & cmd) +std::optional LocalTransceiver::debugSend(const std::string & cmd) { - send(cmd); + AT::Line at_cmd(cmd); + std::string sent_cmd; - std::string response = readLine(); // Read and capture the response - readLine(); // Check if there is an empty line after respones - return response; + if (!send(at_cmd)) { + return std::nullopt; + } + + return readRsp(); } std::string LocalTransceiver::receive() { - std::string receivedData = readLine(); + // TODO(hhenry01) + std::string receivedData = readRsp().value(); return receivedData; } -void LocalTransceiver::send(const std::string & cmd) { bio::write(serial_, bio::buffer(cmd, cmd.size())); } +bool LocalTransceiver::send(const AT::Line & cmd) +{ + boost::system::error_code ec; + bio::write(serial_, bio::buffer(cmd.str_, cmd.str_.size()), ec); + if (ec) { + std::cerr << "Write failed with error: " << ec.message() << std::endl; + return false; + } + return true; +} std::string LocalTransceiver::parseInMsg(const std::string & msg) { @@ -180,30 +245,64 @@ std::string LocalTransceiver::parseInMsg(const std::string & msg) return "placeholder"; } -std::string LocalTransceiver::readLine() +bool LocalTransceiver::rcvRsp(const AT::Line & expected_rsp) { bio::streambuf buf; + error_code ec; + // Caution: will hang if another proccess is reading from the same serial port + bio::read(serial_, buf, bio::transfer_exactly(expected_rsp.str_.size()), ec); + if (ec) { + std::cerr << "Failed to read with error: " << ec.message() << std::endl; + return false; + } + std::string outstr = streambufToStr(buf); + if (outstr != expected_rsp.str_) { + std::cerr << "Expected to read: \"" << expected_rsp.str_ << "\"\nbut read: \"" << outstr << "\"" << std::endl; + return false; + } + return true; +} - // Caution: will hang if another proccess is reading from serial port - bio::read_until(serial_, buf, AT::DELIMITER); - return std::string( - bio::buffers_begin(buf.data()), bio::buffers_begin(buf.data()) + static_cast(buf.data().size())); +bool LocalTransceiver::rcvRsps(std::initializer_list expected_rsps) +{ + // All responses must match the expected responses + return std::all_of( + expected_rsps.begin(), expected_rsps.end(), [this](const AT::Line & e_rsp) { return rcvRsp(e_rsp); }); } -bool LocalTransceiver::checkOK() +std::optional LocalTransceiver::readRsp() { - std::string status = readLine(); - return status == AT::STATUS_OK; + bio::streambuf buf; + error_code ec; + + // Caution: will hang if another proccess is reading from serial port + bio::read_until(serial_, buf, AT::DELIMITER, ec); + if (ec) { + return std::nullopt; + } + + std::string rsp_str = streambufToStr(buf); + rsp_str.pop_back(); // Remove the "\n" + return rsp_str; } std::string LocalTransceiver::checksum(const std::string & data) { - uint16_t counter = 0; + uint16_t sum = 0; for (char c : data) { - counter += static_cast(c); + sum += static_cast(c); } - std::stringstream ss; - ss << std::hex << std::setw(4) << std::setfill('0') << counter; - return ss.str(); + char checksum_low = static_cast(sum & 0xff); // NOLINT(readability-magic-numbers) + char checksum_high = static_cast((sum & 0xff00) >> 8); // NOLINT(readability-magic-numbers) + + return std::string{checksum_high, checksum_low}; +} + +std::string LocalTransceiver::streambufToStr(bio::streambuf & buf) +{ + std::string str = std::string( + bio::buffers_begin(buf.data()), bio::buffers_begin(buf.data()) + static_cast(buf.data().size())); + buf.consume(buf.size()); + return str; } diff --git a/src/network_systems/projects/local_transceiver/src/local_transceiver_ros_intf.cpp b/src/network_systems/projects/local_transceiver/src/local_transceiver_ros_intf.cpp index e5cb5bc7d..52db8f1f0 100644 --- a/src/network_systems/projects/local_transceiver/src/local_transceiver_ros_intf.cpp +++ b/src/network_systems/projects/local_transceiver/src/local_transceiver_ros_intf.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -9,12 +10,13 @@ #include "cmn_hdrs/ros_info.h" #include "cmn_hdrs/shared_constants.h" #include "local_transceiver.h" +#include "net_node.h" /** * Local Transceiver Interface Node * */ -class LocalTransceiverIntf : public rclcpp::Node +class LocalTransceiverIntf : public NetNode { public: /** @@ -23,14 +25,29 @@ class LocalTransceiverIntf : public rclcpp::Node * @param lcl_trns Local Transceiver instance */ explicit LocalTransceiverIntf(std::shared_ptr lcl_trns) - : Node("local_transceiver_node"), lcl_trns_(lcl_trns) + : NetNode("local_transceiver_node"), lcl_trns_(lcl_trns) { static constexpr int ROS_Q_SIZE = 5; static constexpr auto TIMER_INTERVAL = std::chrono::milliseconds(500); - pub_ = this->create_publisher(PLACEHOLDER_TOPIC_0_TOPIC, ROS_Q_SIZE); timer_ = this->create_wall_timer(TIMER_INTERVAL, std::bind(&LocalTransceiverIntf::pub_cb, this)); - sub_ = this->create_subscription( - PLACEHOLDER_TOPIC_1_TOPIC, ROS_Q_SIZE, std::bind(&LocalTransceiverIntf::sub_cb, this, std::placeholders::_1)); + pub_ = this->create_publisher(GLOBAL_PATH_TOPIC, ROS_Q_SIZE); + + // subscriber nodes + sub_wind_sensor = this->create_subscription( + WIND_SENSORS_TOPIC, ROS_Q_SIZE, + std::bind(&LocalTransceiverIntf::sub_wind_sensor_cb, this, std::placeholders::_1)); + sub_batteries = this->create_subscription( + BATTERIES_TOPIC, ROS_Q_SIZE, std::bind(&LocalTransceiverIntf::sub_batteries_cb, this, std::placeholders::_1)); + sub_data_sensors = this->create_subscription( + DATA_SENSORS_TOPIC, ROS_Q_SIZE, + std::bind(&LocalTransceiverIntf::sub_data_sensors_cb, this, std::placeholders::_1)); + sub_ais_ships = this->create_subscription( + AIS_SHIPS_TOPIC, ROS_Q_SIZE, std::bind(&LocalTransceiverIntf::sub_ais_ships_cb, this, std::placeholders::_1)); + sub_gps = this->create_subscription( + GPS_TOPIC, ROS_Q_SIZE, std::bind(&LocalTransceiverIntf::sub_gps_cb, this, std::placeholders::_1)); + sub_local_path_data = this->create_subscription( + LOCAL_PATH_DATA_TOPIC, ROS_Q_SIZE, + std::bind(&LocalTransceiverIntf::sub_local_path_data_cb, this, std::placeholders::_1)); } private: @@ -39,34 +56,96 @@ class LocalTransceiverIntf : public rclcpp::Node // Publishing timer rclcpp::TimerBase::SharedPtr timer_; // String is a placeholder pub and sub msg type - we will definitely define custom message types - rclcpp::Publisher::SharedPtr pub_; - // Placeholder subscriber object - rclcpp::Subscription::SharedPtr sub_; + rclcpp::Publisher::SharedPtr pub_; + + rclcpp::Subscription::SharedPtr sub_wind_sensor; + rclcpp::Subscription::SharedPtr sub_batteries; + rclcpp::Subscription::SharedPtr sub_data_sensors; + rclcpp::Subscription::SharedPtr sub_ais_ships; + rclcpp::Subscription::SharedPtr sub_gps; + rclcpp::Subscription::SharedPtr sub_local_path_data; /** * @brief Callback function to publish to onboard ROS network * */ - void pub_cb(/* placeholder */) + void pub_cb(/*place*/) { - //TODO(jng468) + // TODO(Jng468): complete, after receive is done + // std::string recent_data = lcl_trns_->receive(); //receives most recent data from remote server + // auto msg = custom_interfaces::msg::Path(); + // pub_->publish(msg); } /** - * @brief Callback function to subscribe to the onboard ROS network - * + * @brief Callback function to subscribe to the onboard ROS network for wind sensors + */ + void sub_wind_sensor_cb(custom_interfaces::msg::WindSensors in_msg) + { + custom_interfaces::msg::WindSensors data = in_msg; + lcl_trns_->updateSensor(data); + } + + /** + * @brief Callback function to subscribe to the onboard ROS network for batteries */ - void sub_cb(std_msgs::msg::String /* placeholder */) + void sub_batteries_cb(custom_interfaces::msg::Batteries in_msg) { - //TODO(jng468) + custom_interfaces::msg::Batteries data = in_msg; + lcl_trns_->updateSensor(data); + } + + /** + * @brief Callback function to subscribe to the onboard ROS network for generic sensors + */ + void sub_data_sensors_cb(custom_interfaces::msg::GenericSensors in_msg) + { + custom_interfaces::msg::GenericSensors data = in_msg; + lcl_trns_->updateSensor(data); + } + + /** + * @brief Callback function to subscribe to the onboard ROS network for ais ships + */ + void sub_ais_ships_cb(custom_interfaces::msg::AISShips in_msg) + { + custom_interfaces::msg::AISShips data = in_msg; + lcl_trns_->updateSensor(data); + } + + /** + * @brief Callback function to subscribe to the onboard ROS network for GPS + */ + void sub_gps_cb(custom_interfaces::msg::GPS in_msg) + { + custom_interfaces::msg::GPS data = in_msg; + lcl_trns_->updateSensor(data); + } + + void sub_local_path_data_cb(custom_interfaces::msg::LPathData in_msg) + { + custom_interfaces::msg::LPathData data = in_msg; + lcl_trns_->updateSensor(data); } }; int main(int argc, char * argv[]) { + bool err = false; rclcpp::init(argc, argv); std::shared_ptr lcl_trns = std::make_shared("PLACEHOLDER", SATELLITE_BAUD_RATE); - rclcpp::spin(std::make_shared(lcl_trns)); + try { + std::shared_ptr node = std::make_shared(lcl_trns); + try { + rclcpp::spin(node); + } catch (std::exception & e) { + RCLCPP_ERROR(node->get_logger(), "%s", e.what()); + throw e; + } + } catch (std::exception & e) { + std::cerr << e.what() << std::endl; + err = true; + } rclcpp::shutdown(); - return 0; + return err ? -1 : 0; } diff --git a/src/network_systems/projects/local_transceiver/test/test_local_transceiver.cpp b/src/network_systems/projects/local_transceiver/test/test_local_transceiver.cpp index b845f7bfd..b9f9c31bf 100644 --- a/src/network_systems/projects/local_transceiver/test/test_local_transceiver.cpp +++ b/src/network_systems/projects/local_transceiver/test/test_local_transceiver.cpp @@ -2,22 +2,60 @@ #include +#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "at_cmds.h" #include "cmn_hdrs/shared_constants.h" #include "local_transceiver.h" #include "sensors.pb.h" +namespace bp = boost::process; + +/* >>>>>README<<<<<< +Local Transceiver unit tests rely on two other programs: Virtual Iridium and HTTP Echo Server +1. Spawning a separate process for RUN_VIRTUAL_IRIDIUM_SCRIPT_PATH doesn't work very well because the script + spawns it's own subprocess for mock serial ports. You can spawn it ezpz, but cleaning up mock serial port + subprocesses is a lot harder than you'd think. Hence, it's not done in the test code. +2. Virtual Iridium needs a valid HTTP POST endpoint for certain commands. RUN_HTTP_ECHO_SERVER_CMD runs a simple + server that just echos whatever it receives. + ***IMPORTANT***: Make sure the echo server is running on the host and port specified in the virtual iridium + --webhook_server_endpoint argument (default: 127.0.0.1:8081) +*/ class TestLocalTransceiver : public ::testing::Test { protected: + static void SetUpTestSuite() + { + bp::system("pkill -f http_echo_server"); // kill any currently running http_echo_server processes + http_echo_server_proc_ = bp::child(RUN_HTTP_ECHO_SERVER_CMD, bp::std_out > stdout, bp::std_err > stderr); + std::error_code e; + if (!http_echo_server_proc_.running(e)) { + throw std::runtime_error("Failed to start http echo server process! " + e.message()); + } + } + + static void TearDownTestSuite() { http_echo_server_proc_.terminate(); } + TestLocalTransceiver() { try { lcl_trns_ = new LocalTransceiver(LOCAL_TRANSCEIVER_TEST_PORT, SATELLITE_BAUD_RATE); - } catch (boost::system::system_error & /**/) { - std::cerr << "Failed to create Local Transceiver for tests, is only one instance of: \"" - << RUN_VIRTUAL_IRIDIUM_SCRIPT_PATH << "\" running?" << std::endl; + } catch (boost::system::system_error & e) { + std::stringstream ss; + ss << "Failed to create Local Transceiver for tests, is only one instance of: \"" + << RUN_VIRTUAL_IRIDIUM_SCRIPT_PATH << "\" running?" << std::endl; + ss << e.what() << std::endl; + throw std::runtime_error(ss.str()); } } ~TestLocalTransceiver() override @@ -27,30 +65,117 @@ class TestLocalTransceiver : public ::testing::Test } LocalTransceiver * lcl_trns_; + +private: + static bp::child http_echo_server_proc_; }; +bp::child TestLocalTransceiver::http_echo_server_proc_ = {}; /** - * @brief Verify debugSendTest sends something to the terminal + * @brief Verify debugSend */ TEST_F(TestLocalTransceiver, debugSendTest) { - //std::string testDebug = "showsUp"; - //lcl_trns_->debugSend(testDebug); + auto opt_result = lcl_trns_->debugSend(AT::CHECK_CONN); + EXPECT_TRUE(opt_result); + std::string result = opt_result.value(); + EXPECT_TRUE(boost::algorithm::contains(result, AT::Line(AT::STATUS_OK).str_)); } /** * @brief Send a binary string to virtual_iridium and verify it is received - * Uses gps custom interface + * Using gps, ais, wind, batteries, generic sensors, local path data */ -TEST_F(TestLocalTransceiver, sendGpsTest) +TEST_F(TestLocalTransceiver, sendData) { - constexpr float holder = 14.3; + constexpr float holder = 14.3; + constexpr int32_t holder_int = 11; + + // custom inferfaces used + custom_interfaces::msg::GPS gps; + custom_interfaces::msg::AISShips ais; + custom_interfaces::msg::WindSensors wind; + custom_interfaces::msg::Batteries batteries; + custom_interfaces::msg::GenericSensors sensors; + custom_interfaces::msg::LPathData local_paths; - custom_interfaces::msg::GPS gps; + // assign gps data gps.heading.set__heading(holder); gps.lat_lon.set__latitude(holder); gps.lat_lon.set__longitude(holder); gps.speed.set__speed(holder); + + // assign ais data + custom_interfaces::msg::HelperAISShip ship_one; + custom_interfaces::msg::HelperHeading heading_one; + custom_interfaces::msg::HelperLatLon lat_lon_one; + custom_interfaces::msg::HelperSpeed speed_one; + custom_interfaces::msg::HelperROT rotation_one; + custom_interfaces::msg::HelperDimension width_one; + custom_interfaces::msg::HelperDimension length_one; + + heading_one.set__heading(holder); + lat_lon_one.set__latitude(holder); + lat_lon_one.set__longitude(holder); + speed_one.set__speed(holder); + rotation_one.set__rot(holder_int); + width_one.set__dimension(holder); + length_one.set__dimension(holder); + + ship_one.set__id(holder_int); + ship_one.set__cog(heading_one); + ship_one.set__lat_lon(lat_lon_one); + ship_one.set__sog(speed_one); + ship_one.set__rot(rotation_one); + ship_one.set__width(width_one); + ship_one.set__length(length_one); + + ais.set__ships({ship_one}); + + // assign wind data + custom_interfaces::msg::WindSensor wind_data_one; + custom_interfaces::msg::WindSensor wind_data_two; + custom_interfaces::msg::HelperSpeed wind_speed; + + wind_data_one.set__direction(holder_int); + wind_speed.set__speed(holder); + wind_data_one.set__speed(wind_speed); + wind_data_two.set__direction(holder_int); + wind_data_two.set__speed(wind_speed); + wind.set__wind_sensors({wind_data_one, wind_data_two}); + + // assign batteries data + custom_interfaces::msg::HelperBattery battery_one; + custom_interfaces::msg::HelperBattery battery_two; + + battery_one.set__current(holder); + battery_one.set__voltage(holder); + battery_two.set__current(holder); + battery_two.set__voltage(holder); + batteries.set__batteries({battery_one, battery_two}); + + // assign generic sensors data + custom_interfaces::msg::HelperGenericSensor sensor; + + sensor.set__data(holder_int); + sensor.set__id(holder_int); + sensors.set__generic_sensors({sensor}); + + // assign local path data + custom_interfaces::msg::Path local_path; + custom_interfaces::msg::HelperLatLon lat_lon; + lat_lon.set__latitude(holder); + lat_lon.set__longitude(holder); + local_path.set__waypoints({lat_lon}); + local_paths.set__local_path({local_path}); + + // update sensors and send + lcl_trns_->updateSensor(wind); lcl_trns_->updateSensor(gps); - lcl_trns_->send(); + lcl_trns_->updateSensor(ais); + lcl_trns_->updateSensor(batteries); + lcl_trns_->updateSensor(sensors); + lcl_trns_->updateSensor(local_paths); + + EXPECT_TRUE(lcl_trns_->send()); } diff --git a/src/network_systems/projects/mock_ais/src/mock_ais_ros_intf.cpp b/src/network_systems/projects/mock_ais/src/mock_ais_ros_intf.cpp index e19ed9342..853dfd230 100644 --- a/src/network_systems/projects/mock_ais/src/mock_ais_ros_intf.cpp +++ b/src/network_systems/projects/mock_ais/src/mock_ais_ros_intf.cpp @@ -12,14 +12,15 @@ #include "cmn_hdrs/ros_info.h" #include "cmn_hdrs/shared_constants.h" #include "mock_ais.h" +#include "net_node.h" /** * Connect the Mock AIS to the onbaord ROS network */ -class MockAisRosIntf : public rclcpp::Node +class MockAisRosIntf : public NetNode { public: - MockAisRosIntf() : Node("mock_ais_node") + MockAisRosIntf() : NetNode("mock_ais_node") { static constexpr int ROS_Q_SIZE = 5; this->declare_parameter("enabled", false); @@ -118,8 +119,20 @@ class MockAisRosIntf : public rclcpp::Node int main(int argc, char * argv[]) { + bool err = false; rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + try { + std::shared_ptr node = std::make_shared(); + try { + rclcpp::spin(node); + } catch (std::exception & e) { + RCLCPP_ERROR(node->get_logger(), "%s", e.what()); + throw e; + } + } catch (std::exception & e) { + std::cerr << e.what() << std::endl; + err = true; + } rclcpp::shutdown(); - return 0; + return err ? -1 : 0; } diff --git a/src/network_systems/projects/remote_transceiver/src/remote_transceiver_ros_intf.cpp b/src/network_systems/projects/remote_transceiver/src/remote_transceiver_ros_intf.cpp index 5c74235ce..78be9a8b7 100644 --- a/src/network_systems/projects/remote_transceiver/src/remote_transceiver_ros_intf.cpp +++ b/src/network_systems/projects/remote_transceiver/src/remote_transceiver_ros_intf.cpp @@ -7,6 +7,7 @@ #include #include "cmn_hdrs/shared_constants.h" +#include "net_node.h" #include "remote_transceiver.h" #include "sailbot_db.h" @@ -14,10 +15,10 @@ * @brief Connect the Remote Transceiver to the onboard ROS network * */ -class RemoteTransceiverRosIntf : public rclcpp::Node +class RemoteTransceiverRosIntf : public NetNode { public: - RemoteTransceiverRosIntf() : Node("remote_transceiver_node") + RemoteTransceiverRosIntf() : NetNode("remote_transceiver_node") { this->declare_parameter("enabled", true); enabled_ = this->get_parameter("enabled").as_bool(); @@ -117,14 +118,21 @@ class RemoteTransceiverRosIntf : public rclcpp::Node int main(int argc, char ** argv) { + bool err = false; rclcpp::init(argc, argv); try { - rclcpp::spin(std::make_shared()); - } catch (std::runtime_error & e) { + std::shared_ptr node = std::make_shared(); + try { + rclcpp::spin(node); + } catch (std::exception & e) { + RCLCPP_ERROR(node->get_logger(), "%s", e.what()); + throw e; + } + } catch (std::exception & e) { std::cerr << e.what() << std::endl; - return -1; + err = true; } rclcpp::shutdown(); - return 0; + return err ? -1 : 0; } diff --git a/src/network_systems/ros_info.txt b/src/network_systems/ros_info.txt index 765654d87..5c3ce2925 100644 --- a/src/network_systems/ros_info.txt +++ b/src/network_systems/ros_info.txt @@ -13,3 +13,4 @@ mock_gps filtered_wind_sensor mock_wind_sensors wind_sensors +local_path_data diff --git a/src/network_systems/scripts/README.md b/src/network_systems/scripts/README.md index 5dc3c5a61..af6b510f2 100644 --- a/src/network_systems/scripts/README.md +++ b/src/network_systems/scripts/README.md @@ -34,7 +34,7 @@ Allows testing of satellite code without needing physical hardware. Optional argument - webhook server url: - Specify where the URL where the Remote Transceiver or whatever other HTTP server is running. -- Default is 127.0.0.1:8081, which assumes fully local testing. +- Default is `http://127.0.0.1:8081`, which assumes fully local testing. Optional argument - virtual iridium server port diff --git a/src/network_systems/scripts/http_echo_server.py b/src/network_systems/scripts/http_echo_server.py new file mode 100644 index 000000000..4df34de65 --- /dev/null +++ b/src/network_systems/scripts/http_echo_server.py @@ -0,0 +1,31 @@ +import argparse +from http.server import BaseHTTPRequestHandler, HTTPServer + + +class HTTPRequestHandler(BaseHTTPRequestHandler): + def do_GET(self): + self.send_response(200) + self.end_headers() + + def do_POST(self): + self.send_response(200) + self.end_headers() + content_len = int(self.headers["Content-Length"]) + body = self.rfile.read(content_len) + print(body) + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--host", type=str, default="127.0.0.1") + parser.add_argument("-p", "--port", type=int, default=8081) + args = parser.parse_args() + + print(f"Running HTTP Echo Server at http://{args.host}:{args.port}") + + with HTTPServer((args.host, args.port), HTTPRequestHandler) as server: + server.serve_forever() + + +if __name__ == "__main__": + main()