diff --git a/src/network_systems/.gitignore b/src/network_systems/.gitignore index ec6f8109d..38919a42c 100644 --- a/src/network_systems/.gitignore +++ b/src/network_systems/.gitignore @@ -1,5 +1,6 @@ # autogenerated files /lib/cmn_hdrs/ros_info.h +/launch/ros_info.py *.pyc # PlantUML diagram export directory diff --git a/src/network_systems/launch/main_launch.py b/src/network_systems/launch/main_launch.py index 101a9ffe3..183a6cbb4 100644 --- a/src/network_systems/launch/main_launch.py +++ b/src/network_systems/launch/main_launch.py @@ -1,6 +1,7 @@ """Launch file that runs all nodes for the network systems ROS package.""" import os +import sys from importlib.util import module_from_spec, spec_from_file_location from typing import List, Tuple @@ -12,6 +13,16 @@ from launch.some_substitutions_type import SomeSubstitutionsType from launch.substitutions import LaunchConfiguration +# Deal with Python import paths +SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__)) +sys.path.append(SCRIPT_DIR) +from ros_info import ( # noqa: E402 + CACHED_FIB_NODE, + CAN_TRANSCEIVER_NODE, + MOCK_AIS_NODE, + REMOTE_TRANSCEIVER_NODE, +) + # Local launch arguments and constants PACKAGE_NAME = "network_systems" NAMESPACE = "" @@ -87,7 +98,7 @@ def get_cached_fib_description(context: LaunchContext) -> Node: Returns: Node: The node object that launches the cached_fib_node. """ - node_name = "cached_fib_node" + node_name = CACHED_FIB_NODE ros_parameters = [ global_launch_config, {"mode": LaunchConfiguration("mode")}, @@ -119,7 +130,7 @@ def get_mock_ais_description(context: LaunchContext) -> Node: Returns: Node: The node object that launches the mock_ais_node. """ - node_name = "mock_ais_node" + node_name = MOCK_AIS_NODE ros_parameters = [ global_launch_config, {"mode": LaunchConfiguration("mode")}, @@ -151,7 +162,7 @@ def get_can_transceiver_description(context: LaunchContext) -> Node: Returns: Node: The node object that launches the can_transceiver_node. """ - node_name = "can_transceiver_node" + node_name = CAN_TRANSCEIVER_NODE ros_parameters = [ global_launch_config, {"mode": LaunchConfiguration("mode")}, @@ -183,7 +194,7 @@ def get_remote_transceiver_description(context: LaunchContext) -> Node: Returns: Node: The node object that launches the remote_transceiver_node. """ - node_name = "remote_transceiver_node" + node_name = REMOTE_TRANSCEIVER_NODE ros_parameters = [ global_launch_config, {"mode": LaunchConfiguration("mode")}, diff --git a/src/network_systems/lib/cmn_hdrs/CMakeLists.txt b/src/network_systems/lib/cmn_hdrs/CMakeLists.txt index bb66d729c..ade5a688c 100644 --- a/src/network_systems/lib/cmn_hdrs/CMakeLists.txt +++ b/src/network_systems/lib/cmn_hdrs/CMakeLists.txt @@ -1,8 +1,7 @@ # Generate ROS info header file -set(ROS_INFO_FILE ${CMAKE_SOURCE_DIR}/ros_info.txt) add_custom_command( OUTPUT ${CMAKE_SOURCE_DIR}/lib/cmn_hdrs/ros_info.h - COMMAND ${CMAKE_SOURCE_DIR}/scripts/autogen_ros_topics.sh ${ROS_INFO_FILE} + COMMAND python3 ${CMAKE_SOURCE_DIR}/scripts/gen_ros_info.py DEPENDS ${CMAKE_SOURCE_DIR} ${ROS_INFO_FILE} ) add_custom_target(ros_info_h DEPENDS ${CMAKE_CURRENT_LIST_DIR}/ros_info.h) 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 62277556e..158d6d7c3 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 @@ -27,7 +27,7 @@ using CAN_FP::CanId; class CanTransceiverIntf : public NetNode { public: - CanTransceiverIntf() : NetNode("can_transceiver_node") + CanTransceiverIntf() : NetNode(ros_nodes::CAN_TRANSCEIVER) { this->declare_parameter("enabled", true); @@ -62,8 +62,8 @@ class CanTransceiverIntf : public NetNode throw std::runtime_error(msg); } - ais_pub_ = this->create_publisher(AIS_SHIPS_TOPIC, QUEUE_SIZE); - batteries_pub_ = this->create_publisher(BATTERIES_TOPIC, QUEUE_SIZE); + ais_pub_ = this->create_publisher(ros_topics::AIS_SHIPS, QUEUE_SIZE); + batteries_pub_ = this->create_publisher(ros_topics::BATTERIES, QUEUE_SIZE); can_trns_->registerCanCbs({ std::make_pair( @@ -77,10 +77,10 @@ class CanTransceiverIntf : public NetNode if (mode == SYSTEM_MODE::DEV) { // Initialize the CAN Sim Intf mock_ais_sub_ = this->create_subscription( - MOCK_AIS_SHIPS_TOPIC, QUEUE_SIZE, + ros_topics::MOCK_AIS_SHIPS, QUEUE_SIZE, [this](msg::AISShips mock_ais_ships) { subMockAISCb(mock_ais_ships); }); mock_gps_sub_ = this->create_subscription( - MOCK_GPS_TOPIC, QUEUE_SIZE, [this](msg::GPS mock_gps) { subMockGpsCb(mock_gps); }); + ros_topics::MOCK_GPS, QUEUE_SIZE, [this](msg::GPS mock_gps) { subMockGpsCb(mock_gps); }); // TODO(lross03): register a callback for CanSimToBoatSim 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 d032a2d64..c949c033f 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 "cmn_hdrs/ros_info.h" #include "net_node.h" // Include ROS headers #include @@ -14,7 +15,7 @@ constexpr int INIT_FIB_SIZE = 5; class CachedFibNode : public NetNode { public: - explicit CachedFibNode(const std::size_t initSize) : NetNode("cached_fib_node"), c_fib_(initSize) + explicit CachedFibNode(const std::size_t initSize) : NetNode(ros_nodes::CACHED_FIB), c_fib_(initSize) { this->declare_parameter("enabled", false); bool enabled = this->get_parameter("enabled").as_bool(); 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 52db8f1f0..4476c2e32 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 @@ -25,28 +25,30 @@ class LocalTransceiverIntf : public NetNode * @param lcl_trns Local Transceiver instance */ explicit LocalTransceiverIntf(std::shared_ptr lcl_trns) - : NetNode("local_transceiver_node"), lcl_trns_(lcl_trns) + : NetNode(ros_nodes::LOCAL_TRANSCEIVER), lcl_trns_(lcl_trns) { static constexpr int ROS_Q_SIZE = 5; static constexpr auto TIMER_INTERVAL = std::chrono::milliseconds(500); timer_ = this->create_wall_timer(TIMER_INTERVAL, std::bind(&LocalTransceiverIntf::pub_cb, this)); - pub_ = this->create_publisher(GLOBAL_PATH_TOPIC, ROS_Q_SIZE); + pub_ = this->create_publisher(ros_topics::GLOBAL_PATH, ROS_Q_SIZE); // subscriber nodes sub_wind_sensor = this->create_subscription( - WIND_SENSORS_TOPIC, ROS_Q_SIZE, + ros_topics::WIND_SENSORS, 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)); + ros_topics::BATTERIES, 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, + ros_topics::DATA_SENSORS, 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)); + ros_topics::AIS_SHIPS, 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)); + ros_topics::GPS, 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, + ros_topics::LOCAL_PATH, ROS_Q_SIZE, std::bind(&LocalTransceiverIntf::sub_local_path_data_cb, this, std::placeholders::_1)); } 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 853dfd230..d2695c4fc 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 @@ -20,7 +20,7 @@ class MockAisRosIntf : public NetNode { public: - MockAisRosIntf() : NetNode("mock_ais_node") + MockAisRosIntf() : NetNode(ros_nodes::MOCK_AIS) { static constexpr int ROS_Q_SIZE = 5; this->declare_parameter("enabled", false); @@ -57,7 +57,7 @@ class MockAisRosIntf : public NetNode // TODO(): Add ROS parameters so that we can use the MockAis constructor that takes SimShipConfig // Optionally use nested parameters: https://answers.ros.org/question/325939/declare-nested-parameter/ mock_ais_ = std::make_unique(seed, num_sim_ships, polaris_start_pos); - std::string polaris_gps_topic = mode == SYSTEM_MODE::DEV ? MOCK_GPS_TOPIC : GPS_TOPIC; + std::string polaris_gps_topic = mode == SYSTEM_MODE::DEV ? ros_topics::MOCK_GPS : ros_topics::GPS; // The subscriber callback is very simple so it's just the following lambda function sub_ = this->create_subscription( @@ -65,7 +65,7 @@ class MockAisRosIntf : public NetNode mock_ais_->updatePolarisPos({mock_gps.lat_lon.latitude, mock_gps.lat_lon.longitude}); }); - pub_ = this->create_publisher(MOCK_AIS_SHIPS_TOPIC, ROS_Q_SIZE); + pub_ = this->create_publisher(ros_topics::MOCK_AIS_SHIPS, ROS_Q_SIZE); timer_ = this->create_wall_timer( std::chrono::milliseconds(publish_rate_ms), std::bind(&MockAisRosIntf::pubShipsCB, this)); } else { 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 78be9a8b7..867af364a 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 @@ -6,6 +6,7 @@ #include #include +#include "cmn_hdrs/ros_info.h" #include "cmn_hdrs/shared_constants.h" #include "net_node.h" #include "remote_transceiver.h" @@ -18,7 +19,7 @@ class RemoteTransceiverRosIntf : public NetNode { public: - RemoteTransceiverRosIntf() : NetNode("remote_transceiver_node") + RemoteTransceiverRosIntf() : NetNode(ros_nodes::REMOTE_TRANSCEIVER) { this->declare_parameter("enabled", true); enabled_ = this->get_parameter("enabled").as_bool(); diff --git a/src/network_systems/ros_info.yaml b/src/network_systems/ros_info.yaml new file mode 100644 index 000000000..5e65d5c4b --- /dev/null +++ b/src/network_systems/ros_info.yaml @@ -0,0 +1,20 @@ +ros_topics: + AIS_SHIPS: ais_ships + BATTERIES: batteries + DESIRED_HEADING: desired_heading + DATA_SENSORS: data_sensors + FILTERED_WIND_SENSOR: filtered_wind_sensor + GLOBAL_PATH: global_path + GPS: gps + LOCAL_PATH: local_path_data + MOCK_AIS_SHIPS: mock_ais_ships + MOCK_GPS: mock_gps + MOCK_WIND_SENSORS: mock_wind_sensor + WIND_SENSORS: wind_sensors + +ros_nodes: + CAN_TRANSCEIVER: can_transceiver_node + CACHED_FIB: cached_fib_node + LOCAL_TRANSCEIVER: local_transceiver_node + MOCK_AIS: mock_ais_node + REMOTE_TRANSCEIVER: remote_transceiver_node diff --git a/src/network_systems/scripts/README.md b/src/network_systems/scripts/README.md index af6b510f2..0ecce8bfb 100644 --- a/src/network_systems/scripts/README.md +++ b/src/network_systems/scripts/README.md @@ -1,12 +1,12 @@ # Scripts -## Autogen ROS Topics +## Generate ROS Info ```shell -./autogen_ros_topics.sh +python3 gen_ros_info.py ``` -Given an input text file where each line is the name of a ROS topic, generates a C++ header file matching those names. +Takes [ros_info.yaml](../ros_info.yaml) and generates a C++ header file and Python file with constants defined within. ## Sailbot DB diff --git a/src/network_systems/scripts/autogen_ros_topics.sh b/src/network_systems/scripts/autogen_ros_topics.sh deleted file mode 100755 index 87a0c33de..000000000 --- a/src/network_systems/scripts/autogen_ros_topics.sh +++ /dev/null @@ -1,31 +0,0 @@ -#!/bin/bash - -# A simple shell script that takes as input a text file of names and converts them to -# C++ constants in a header file (network_systems/lib/cmn_hdrs/ros_info.h) -# REQUIREMENTS: -# - A text document with the names of required ROS topics. -# - Example use in command line: -# ./autogen_ros_topics.sh input.txt - -SCRIPT_DIR="$(dirname "$(readlink -f "$0")")" -OUTPUT_PATH=$SCRIPT_DIR/../lib/cmn_hdrs/ros_info.h - -# Checks that requirements above are fulfilled. -if [ $# -ne 1 ]; then - echo "Usage: $0 INPUT_FILE" - exit 1 -fi - -INPUT_FILE=$1 - -echo "// AUTOGENERATED FILE - DO NOT EDIT" > $OUTPUT_PATH -echo "#pragma once" >> $OUTPUT_PATH - -# Reads each line in the input text document -# Generates the name of the constant in required format -# Writes to output a C++ constant with the generated name and line -while read -r line; do - CAPITALIZED=$(echo $line | tr '[:lower:]' '[:upper:]') - VAR_NAME="${CAPITALIZED}_TOPIC" - echo "constexpr auto $VAR_NAME = \"$line\";" >> $OUTPUT_PATH -done < "$INPUT_FILE" diff --git a/src/network_systems/scripts/gen_ros_info.py b/src/network_systems/scripts/gen_ros_info.py new file mode 100644 index 000000000..7735cbe46 --- /dev/null +++ b/src/network_systems/scripts/gen_ros_info.py @@ -0,0 +1,63 @@ +import os + +import yaml + +NET_DIR = os.path.join( + os.getenv("ROS_WORKSPACE", default="/workspaces/sailbot_workspace"), "src/network_systems" +) +ROS_INFO_FILE = os.path.join(NET_DIR, "ros_info.yaml") + +GEN_HDR_FILE = os.path.join(NET_DIR, "lib/cmn_hdrs/ros_info.h") +GEN_FILE_WARN = f"FILE GENERATED BY {__file__} using {ROS_INFO_FILE} - DO NOT EDIT" + +GEN_HDR_PREAMBLE = f""" +// {GEN_FILE_WARN} +#pragma once +""" +GEN_HDR_FILE_TOPICS_START = """ +namespace ros_topics +{ +""" +GEN_HDR_FILE_TOPICS_END = "}; // namespace ros_topics\n" +GEN_HDR_FILE_NODES_START = """ +namespace ros_nodes +{ +""" +GEN_HDR_FILE_NODES_END = "}; // namespace ros_nodes\n" + +GEN_PYTHON_FILE = os.path.join(NET_DIR, "launch/ros_info.py") +GEN_PYTHON_PREAMBLE = f"# {GEN_FILE_WARN}\n" + + +def populate_hdr(hdr_file_obj, info_target): + for k, v in info_target.items(): + hdr_file_obj.write(f'static constexpr auto {k} = "{v}";\n') + + +def populate_py_nodes(py_file_obj, info_target): + for k, v in info_target.items(): + py_file_obj.write(f'{k}_NODE = "{v}"\n') + + +def main(): + with open(ROS_INFO_FILE, "r") as f: + info = yaml.safe_load(f) + + with open(GEN_HDR_FILE, "w+") as out_hdr: + out_hdr.write(GEN_HDR_PREAMBLE) + + out_hdr.write(GEN_HDR_FILE_TOPICS_START) + populate_hdr(out_hdr, info["ros_topics"]) + out_hdr.write(GEN_HDR_FILE_TOPICS_END) + + out_hdr.write(GEN_HDR_FILE_NODES_START) + populate_hdr(out_hdr, info["ros_nodes"]) + out_hdr.write(GEN_HDR_FILE_NODES_END) + + with open(GEN_PYTHON_FILE, "w+") as out_py: + out_py.write(GEN_PYTHON_PREAMBLE) + populate_py_nodes(out_py, info["ros_nodes"]) + + +if __name__ == "__main__": + main()