diff --git a/.github/workflows/full_ci.yml b/.github/workflows/full_ci.yml index 3a0972427..66b66661b 100644 --- a/.github/workflows/full_ci.yml +++ b/.github/workflows/full_ci.yml @@ -24,6 +24,7 @@ jobs: rosdep update --rosdistro=jazzy rosdep install --from-paths . --ignore-src -y ./src/ateam_software/ateam_ui/install_deps.sh + sudo apt install -y python3-clang - name: Build shell: bash @@ -35,5 +36,5 @@ jobs: shell: bash run: | source /opt/ros/jazzy/setup.bash - colcon test --packages-skip ateam_ui ateam_spatial + colcon test --packages-skip ateam_ui ateam_spatial ateam_radio_bridge colcon test-result --verbose diff --git a/.github/workflows/pr_validation.yml b/.github/workflows/pr_validation.yml index f7ab31f2b..f58641bab 100644 --- a/.github/workflows/pr_validation.yml +++ b/.github/workflows/pr_validation.yml @@ -23,6 +23,7 @@ jobs: rosdep update --rosdistro=jazzy rosdep install --from-paths . --ignore-src -y ./src/ateam_software/ateam_ui/install_deps.sh + sudo apt install -y python3-clang - name: Get Changed Packages id: get-changed-packages @@ -49,5 +50,5 @@ jobs: shell: bash run: | source /opt/ros/jazzy/setup.bash - colcon test --packages-skip ateam_ui ateam_spatial --packages-select ${{ steps.get-changed-packages.outputs.changed-packages }} + colcon test --packages-skip ateam_ui ateam_spatial ateam_radio_bridge --packages-select ${{ steps.get-changed-packages.outputs.changed-packages }} colcon test-result --verbose diff --git a/.gitmodules b/.gitmodules index 9210ecb1c..08ea60349 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,6 @@ -[submodule "ateam_radio_bridge/software-communication"] - path = ateam_radio_bridge/software-communication - url = https://github.com/SSL-A-Team/software-communication.git [submodule "ssl_ros_bridge"] path = ssl_ros_bridge url = https://github.com/SSL-A-Team/ssl_ros_bridge.git +[submodule "radio/ateam_radio_msgs/software-communication"] + path = radio/ateam_radio_msgs/software-communication + url = https://github.com/SSL-A-Team/software-communication.git diff --git a/README.md b/README.md index 4fa240a31..827fd5412 100644 --- a/README.md +++ b/README.md @@ -41,7 +41,10 @@ source /opt/ros/jazzy/setup.bash # In the ateam_ws directory ./src/software/ateam_ui/install_deps.sh + + sudo apt install python3-clang ``` + 1. Build the code diff --git a/ateam_bringup/launch/bringup_physical.launch.py b/ateam_bringup/launch/bringup_physical.launch.py index 0d63f5b41..db8cee8b8 100644 --- a/ateam_bringup/launch/bringup_physical.launch.py +++ b/ateam_bringup/launch/bringup_physical.launch.py @@ -96,8 +96,9 @@ def generate_launch_description(): respawn=True, remappings=remap_indexed_topics([ ('~/robot_motion_commands/robot', '/robot_motion_commands/robot'), - ('~/robot_feedback/status/robot', '/robot_feedback/status/robot'), - ('~/robot_feedback/motion/robot', '/robot_feedback/motion/robot') + ('~/robot_feedback/basic/robot', '/robot_feedback/basic/robot'), + ('~/robot_feedback/extended/robot', '/robot_feedback/extended/robot'), + ('~/robot_feedback/connection/robot', '/robot_feedback/connection/robot') ]), # prefix=['xterm -bg black -fg white -e gdb -ex run --args'] ), diff --git a/ateam_common/include/ateam_common/topic_names.hpp b/ateam_common/include/ateam_common/topic_names.hpp index 2ee1539b3..af7a71a2f 100644 --- a/ateam_common/include/ateam_common/topic_names.hpp +++ b/ateam_common/include/ateam_common/topic_names.hpp @@ -30,8 +30,9 @@ constexpr std::string_view kVisionMessages = "/vision_messages"; // Raw vision constexpr std::string_view kRefereeMessages = "/referee_messages"; // Raw ref protobufs // Input from robots -constexpr std::string_view kRobotFeedbackPrefix = "/robot_feedback/status/robot"; -constexpr std::string_view kRobotMotionFeedbackPrefix = "/robot_feedback/motion/robot"; +constexpr std::string_view kRobotFeedbackPrefix = "/robot_feedback/basic/robot"; +constexpr std::string_view kRobotMotionFeedbackPrefix = "/robot_feedback/extended/robot"; +constexpr std::string_view kRobotConnectionStatusPrefix = "/robot_feedback/connection/robot"; // Output from vision filter constexpr std::string_view kBall = "/ball"; diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index a79e771d2..f7f8d5fcf 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(ateam_msgs REQUIRED) +find_package(ateam_radio_msgs REQUIRED) find_package(ateam_common REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) @@ -117,6 +118,7 @@ ament_target_dependencies( kenobi_node_component "rclcpp" "rclcpp_components" + "ateam_radio_msgs" "ateam_msgs" "ateam_common" "tf2" diff --git a/ateam_kenobi/package.xml b/ateam_kenobi/package.xml index 0a9957863..12a24d576 100644 --- a/ateam_kenobi/package.xml +++ b/ateam_kenobi/package.xml @@ -12,6 +12,7 @@ rclcpp rclcpp_components ateam_msgs + ateam_radio_msgs ateam_common tf2 tf2_geometry_msgs diff --git a/ateam_kenobi/src/kenobi_node.cpp b/ateam_kenobi/src/kenobi_node.cpp index 47f3166e8..f7a1b1712 100644 --- a/ateam_kenobi/src/kenobi_node.cpp +++ b/ateam_kenobi/src/kenobi_node.cpp @@ -26,8 +26,9 @@ #include #include #include +#include +#include #include -#include #include #include #include @@ -99,13 +100,20 @@ class KenobiNode : public rclcpp::Node &KenobiNode::yellow_robot_state_callback, this); - create_indexed_subscribers( + create_indexed_subscribers( robot_feedback_subscriptions_, Topics::kRobotFeedbackPrefix, 10, &KenobiNode::robot_feedback_callback, this); + create_indexed_subscribers( + robot_connection_status_subscriptions_, + Topics::kRobotConnectionStatusPrefix, + 10, + &KenobiNode::robot_connection_callback, + this); + create_indexed_publishers( robot_commands_publishers_, Topics::kRobotMotionCommandPrefix, rclcpp::SystemDefaultsQoS(), this); @@ -177,8 +185,10 @@ class KenobiNode : public rclcpp::Node 16> blue_robots_subscriptions_; std::array::SharedPtr, 16> yellow_robots_subscriptions_; - std::array::SharedPtr, + std::array::SharedPtr, 16> robot_feedback_subscriptions_; + std::array::SharedPtr, + 16> robot_connection_status_subscriptions_; rclcpp::Subscription::SharedPtr field_subscription_; std::array::SharedPtr, @@ -251,15 +261,21 @@ class KenobiNode : public rclcpp::Node } void robot_feedback_callback( - const ateam_msgs::msg::RobotFeedback::SharedPtr robot_feedback_msg, + const ateam_radio_msgs::msg::BasicTelemetry::SharedPtr robot_feedback_msg, int id) { - world_.our_robots.at(id).radio_connected = robot_feedback_msg->radio_connected; world_.our_robots.at(id).breakbeam_ball_detected = robot_feedback_msg->breakbeam_ball_detected; world_.our_robots.at(id).kicker_available = robot_feedback_msg->kicker_available; world_.our_robots.at(id).chipper_available = robot_feedback_msg->chipper_available; } + void robot_connection_callback( + const ateam_radio_msgs::msg::ConnectionStatus::SharedPtr robot_connection_msg, + int id) + { + world_.our_robots.at(id).radio_connected = robot_connection_msg->radio_connected; + } + void ball_state_callback(const ateam_msgs::msg::BallState::SharedPtr ball_state_msg) { world_.ball.visible = ball_state_msg->visible; diff --git a/ateam_msgs/CMakeLists.txt b/ateam_msgs/CMakeLists.txt index 9c656ebf6..b9da1e8d9 100755 --- a/ateam_msgs/CMakeLists.txt +++ b/ateam_msgs/CMakeLists.txt @@ -16,10 +16,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} msg/Overlay.msg msg/OverlayArray.msg msg/RefereeInfo.msg - msg/RobotFeedback.msg msg/RobotMotionCommand.msg - msg/RobotMotionFeedback.msg - msg/RobotMotorFeedback.msg msg/RobotState.msg msg/Sample3d.msg msg/TeamClientConnectionStatus.msg diff --git a/ateam_msgs/msg/RobotFeedback.msg b/ateam_msgs/msg/RobotFeedback.msg deleted file mode 100644 index 78ff97de5..000000000 --- a/ateam_msgs/msg/RobotFeedback.msg +++ /dev/null @@ -1,32 +0,0 @@ -bool radio_connected -uint16 sequence_number -uint8 robot_revision_major -uint8 robot_revision_minor -float32 battery_level # volts -float32 battery_temperature # deg C -bool power_error -bool tipped_error -bool breakbeam_error -bool breakbeam_ball_detected -bool accelerometer_0_error -bool accelerometer_1_error -bool gyroscope_0_error -bool gyroscope_1_error -bool motor_0_general_error -bool motor_0_hall_error -bool motor_1_general_error -bool motor_1_hall_error -bool motor_2_general_error -bool motor_2_hall_error -bool motor_3_general_error -bool motor_3_hall_error -bool motor_4_general_error -bool motor_4_hall_error -bool chipper_available -bool kicker_available -float32 motor_0_temperature # deg C -float32 motor_1_temperature # deg C -float32 motor_2_temperature # deg C -float32 motor_3_temperature # deg C -float32 motor_4_temperature # deg C -float32 kicker_charge_level # volts diff --git a/ateam_msgs/msg/RobotMotionFeedback.msg b/ateam_msgs/msg/RobotMotionFeedback.msg deleted file mode 100644 index f0a2b13d9..000000000 --- a/ateam_msgs/msg/RobotMotionFeedback.msg +++ /dev/null @@ -1,16 +0,0 @@ -uint8 FRONT_LEFT_MOTOR = 0 -uint8 BACK_LEFT_MOTOR = 1 -uint8 BACK_RIGHT_MOTOR = 2 -uint8 FRONT_RIGHT_MOTOR = 3 - -ateam_msgs/RobotMotorFeedback[4] motors - -sensor_msgs/Imu imu - -geometry_msgs/Twist body_velocity_setpoint -geometry_msgs/Twist clamped_body_velocity_setpoint -geometry_msgs/Twist body_velocity_state_estimate -geometry_msgs/Twist body_velocity_control_variable - -float32[4] wheel_velocity_control_variable -float32[4] clamped_wheel_velocity_control_variable \ No newline at end of file diff --git a/ateam_msgs/msg/RobotMotorFeedback.msg b/ateam_msgs/msg/RobotMotorFeedback.msg deleted file mode 100644 index 26ee5b93b..000000000 --- a/ateam_msgs/msg/RobotMotorFeedback.msg +++ /dev/null @@ -1,31 +0,0 @@ -bool master_error -bool hall_power_error -bool hall_disconnected_error -bool bldc_transition_error -bool bldc_commutation_watchdog_error -bool enc_disconnected_error -bool enc_decoding_error -bool hall_enc_vel_disagreement_error -bool overcurrent_error -bool undervoltage_error -bool overvoltage_error -bool torque_limited -bool control_loop_time_error -bool reset_watchdog_independent -bool reset_watchdog_window -bool reset_low_power -bool reset_software -bool reset_pin - -float32 vel_setpoint -float32 vel_setpoint_clamped -int32 encoder_delta -float32 vel_enc_estimate -float32 vel_computed_error -float32 vel_computed_setpoint - -float32 torque_setpoint -float32 current_estimate -float32 torque_estimate -float32 torque_computed_error -float32 torque_computed_setpoint diff --git a/ateam_radio_bridge/software-communication b/ateam_radio_bridge/software-communication deleted file mode 160000 index 5c06b8ac3..000000000 --- a/ateam_radio_bridge/software-communication +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 5c06b8ac380e254188fa2cb25310c4b09e1d262c diff --git a/ateam_radio_bridge/src/conversion.cpp b/ateam_radio_bridge/src/conversion.cpp deleted file mode 100644 index f90cbda21..000000000 --- a/ateam_radio_bridge/src/conversion.cpp +++ /dev/null @@ -1,158 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - - -#include "conversion.hpp" - -#include - -namespace ateam_radio_bridge -{ - -ateam_msgs::msg::RobotFeedback Convert(const BasicTelemetry & basic_telemetry) -{ - ateam_msgs::msg::RobotFeedback robot_feedback; - - robot_feedback.sequence_number = basic_telemetry.sequence_number; - robot_feedback.robot_revision_major = basic_telemetry.robot_revision_major; - robot_feedback.robot_revision_minor = basic_telemetry.robot_revision_minor; - robot_feedback.battery_level = basic_telemetry.battery_level; - robot_feedback.battery_temperature = basic_telemetry.battery_temperature; - robot_feedback.power_error = basic_telemetry.power_error; - robot_feedback.tipped_error = basic_telemetry.tipped_error; - robot_feedback.breakbeam_error = basic_telemetry.breakbeam_error; - robot_feedback.breakbeam_ball_detected = basic_telemetry.breakbeam_ball_detected; - robot_feedback.accelerometer_0_error = basic_telemetry.accelerometer_0_error; - robot_feedback.accelerometer_1_error = basic_telemetry.accelerometer_1_error; - robot_feedback.gyroscope_0_error = basic_telemetry.gyroscope_0_error; - robot_feedback.gyroscope_1_error = basic_telemetry.gyroscope_1_error; - robot_feedback.motor_0_general_error = basic_telemetry.motor_0_general_error; - robot_feedback.motor_0_hall_error = basic_telemetry.motor_0_hall_error; - robot_feedback.motor_1_general_error = basic_telemetry.motor_1_general_error; - robot_feedback.motor_1_hall_error = basic_telemetry.motor_1_hall_error; - robot_feedback.motor_2_general_error = basic_telemetry.motor_2_general_error; - robot_feedback.motor_2_hall_error = basic_telemetry.motor_2_hall_error; - robot_feedback.motor_3_general_error = basic_telemetry.motor_3_general_error; - robot_feedback.motor_3_hall_error = basic_telemetry.motor_3_hall_error; - robot_feedback.motor_4_general_error = basic_telemetry.motor_4_general_error; - robot_feedback.motor_4_hall_error = basic_telemetry.motor_4_hall_error; - robot_feedback.chipper_available = basic_telemetry.chipper_available; - robot_feedback.kicker_available = basic_telemetry.kicker_available; - robot_feedback.motor_0_temperature = basic_telemetry.motor_0_temperature; - robot_feedback.motor_1_temperature = basic_telemetry.motor_1_temperature; - robot_feedback.motor_2_temperature = basic_telemetry.motor_2_temperature; - robot_feedback.motor_3_temperature = basic_telemetry.motor_3_temperature; - robot_feedback.motor_4_temperature = basic_telemetry.motor_4_temperature; - robot_feedback.kicker_charge_level = basic_telemetry.kicker_charge_level; - - - return robot_feedback; -} - -geometry_msgs::msg::Vector3 ConvertFloatArrayToVec3(const float (&float_arr_3)[3]) { - geometry_msgs::msg::Vector3 vec; - - vec.x = float_arr_3[0]; - vec.y = float_arr_3[1]; - vec.z = float_arr_3[2]; - - return vec; -} - -geometry_msgs::msg::Twist ConvertFloatArrayToTwist(const float (&fw_state_space_array)[3]) { - geometry_msgs::msg::Twist twist; - - twist.linear.x = fw_state_space_array[0]; - twist.linear.y = fw_state_space_array[1]; - twist.linear.z = 0.0; - twist.angular.x = 0.0; - twist.angular.y = 0.0; - twist.angular.z = fw_state_space_array[2]; - - return twist; -} - -ateam_msgs::msg::RobotMotorFeedback Convert(const MotorResponse_Motion_Packet & motor_debug_telemetry) { - ateam_msgs::msg::RobotMotorFeedback robot_motor_feedback; - - robot_motor_feedback.master_error = motor_debug_telemetry.master_error; - robot_motor_feedback.hall_power_error = motor_debug_telemetry.hall_power_error; - robot_motor_feedback.hall_disconnected_error = motor_debug_telemetry.hall_disconnected_error; - robot_motor_feedback.bldc_transition_error = motor_debug_telemetry.bldc_transition_error; - robot_motor_feedback.bldc_commutation_watchdog_error = motor_debug_telemetry.bldc_commutation_watchdog_error; - robot_motor_feedback.enc_disconnected_error = motor_debug_telemetry.enc_disconnected_error; - robot_motor_feedback.enc_decoding_error = motor_debug_telemetry.enc_decoding_error; - robot_motor_feedback.hall_enc_vel_disagreement_error = motor_debug_telemetry.hall_enc_vel_disagreement_error; - robot_motor_feedback.overcurrent_error = motor_debug_telemetry.overcurrent_error; - robot_motor_feedback.undervoltage_error = motor_debug_telemetry.undervoltage_error; - robot_motor_feedback.overvoltage_error = motor_debug_telemetry.overvoltage_error; - robot_motor_feedback.torque_limited = motor_debug_telemetry.torque_limited; - robot_motor_feedback.control_loop_time_error = motor_debug_telemetry.control_loop_time_error; - robot_motor_feedback.reset_watchdog_independent = motor_debug_telemetry.reset_watchdog_independent; - robot_motor_feedback.reset_watchdog_window = motor_debug_telemetry.reset_watchdog_window; - robot_motor_feedback.reset_low_power = motor_debug_telemetry.reset_low_power; - robot_motor_feedback.reset_software = motor_debug_telemetry.reset_software; - robot_motor_feedback.reset_pin = motor_debug_telemetry.reset_pin; - - robot_motor_feedback.vel_setpoint = motor_debug_telemetry.vel_setpoint; - robot_motor_feedback.vel_setpoint_clamped = motor_debug_telemetry.vel_setpoint_clamped; - robot_motor_feedback.encoder_delta = motor_debug_telemetry.encoder_delta; - robot_motor_feedback.vel_enc_estimate = motor_debug_telemetry.vel_enc_estimate; - robot_motor_feedback.vel_computed_error = motor_debug_telemetry.vel_computed_error; - robot_motor_feedback.vel_computed_setpoint = motor_debug_telemetry.vel_computed_setpoint; - - robot_motor_feedback.torque_setpoint = motor_debug_telemetry.torque_setpoint; - robot_motor_feedback.current_estimate = motor_debug_telemetry.current_estimate; - robot_motor_feedback.torque_estimate = motor_debug_telemetry.torque_estimate; - robot_motor_feedback.torque_computed_error = motor_debug_telemetry.torque_computed_error; - robot_motor_feedback.torque_computed_setpoint = motor_debug_telemetry.torque_computed_setpoint; - - return robot_motor_feedback; -} - -ateam_msgs::msg::RobotMotionFeedback Convert(const ControlDebugTelemetry & control_debug_telemetry) { - ateam_msgs::msg::RobotMotionFeedback robot_motion_feedback; - - robot_motion_feedback.motors[robot_motion_feedback.FRONT_LEFT_MOTOR] = Convert(control_debug_telemetry.motor_fl); - robot_motion_feedback.motors[robot_motion_feedback.BACK_LEFT_MOTOR] = Convert(control_debug_telemetry.motor_bl); - robot_motion_feedback.motors[robot_motion_feedback.BACK_RIGHT_MOTOR] = Convert(control_debug_telemetry.motor_br); - robot_motion_feedback.motors[robot_motion_feedback.FRONT_RIGHT_MOTOR] = Convert(control_debug_telemetry.motor_fr); - - robot_motion_feedback.imu.orientation_covariance[0] = -1.0; // ROS2 docs say if a sensor doesn't provide a data point, then set element '0' of it's covariance to -1 - robot_motion_feedback.imu.angular_velocity = ConvertFloatArrayToVec3(control_debug_telemetry.imu_gyro); - robot_motion_feedback.imu.linear_acceleration = ConvertFloatArrayToVec3(control_debug_telemetry.imu_accel); - - robot_motion_feedback.body_velocity_setpoint = ConvertFloatArrayToTwist(control_debug_telemetry.commanded_body_velocity); - robot_motion_feedback.clamped_body_velocity_setpoint = ConvertFloatArrayToTwist(control_debug_telemetry.clamped_commanded_body_velocity); - robot_motion_feedback.body_velocity_state_estimate = ConvertFloatArrayToTwist(control_debug_telemetry.cgkf_body_velocity_state_estimate); - robot_motion_feedback.body_velocity_control_variable = ConvertFloatArrayToTwist(control_debug_telemetry.body_velocity_u); - - std::span wheel_velocity_u_span{control_debug_telemetry.wheel_velocity_u}; - static_assert(wheel_velocity_u_span.size() == robot_motion_feedback.wheel_velocity_control_variable.size()); - std::ranges::copy(wheel_velocity_u_span, robot_motion_feedback.wheel_velocity_control_variable.begin()); - - std::span wheel_velocity_clamped_u_span{control_debug_telemetry.wheel_velocity_clamped_u}; - static_assert(wheel_velocity_clamped_u_span.size() == robot_motion_feedback.clamped_wheel_velocity_control_variable.size()); - std::ranges::copy(wheel_velocity_clamped_u_span, robot_motion_feedback.clamped_wheel_velocity_control_variable.begin()); - - return robot_motion_feedback; -} - -} // namespace ateam_radio_bridge diff --git a/ateam_radio_bridge/src/conversion.hpp b/ateam_radio_bridge/src/conversion.hpp deleted file mode 100644 index dca521e3e..000000000 --- a/ateam_radio_bridge/src/conversion.hpp +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - - -#ifndef CONVERSION_HPP_ -#define CONVERSION_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -namespace ateam_radio_bridge -{ - -ateam_msgs::msg::RobotFeedback Convert(const BasicTelemetry & basic_telemetry); -ateam_msgs::msg::RobotMotorFeedback Convert(const MotorResponse_Motion_Packet & motor_debug_telemetry); -ateam_msgs::msg::RobotMotionFeedback Convert(const ControlDebugTelemetry & control_debug_telemetry); - -} - -#endif // CONVERSION_HPP_ diff --git a/ateam_radio_bridge/test/unit_tests/conversion_tests.cpp b/ateam_radio_bridge/test/unit_tests/conversion_tests.cpp deleted file mode 100644 index 41da6a35e..000000000 --- a/ateam_radio_bridge/test/unit_tests/conversion_tests.cpp +++ /dev/null @@ -1,219 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - - -#include -#include "conversion.hpp" - -TEST(ConvertBasicTelemmetry, PacketConversions) -{ - BasicTelemetry telemetry { - 0, - 1, - 2, - 3.4, - 5.6, - 0, - 1, - 0, - 1, - 0, - 0, - 1, - 1, - 0, - 1, - 1, - 0, - 1, - 1, - 0, - 1, - 0, - 0, - 1, - 0, - 0, - 1.2, - 3.4, - 5.6, - 7.8, - 9.0, - 1.2 - }; - const auto feedback_msg = ateam_radio_bridge::Convert(telemetry); - EXPECT_EQ(feedback_msg.sequence_number, 0); - EXPECT_EQ(feedback_msg.robot_revision_major, 1); - EXPECT_EQ(feedback_msg.robot_revision_minor, 2); - EXPECT_FLOAT_EQ(feedback_msg.battery_level, 3.4); - EXPECT_FLOAT_EQ(feedback_msg.battery_temperature, 5.6); - EXPECT_EQ(feedback_msg.power_error, 0); - EXPECT_EQ(feedback_msg.tipped_error, 1); - EXPECT_EQ(feedback_msg.breakbeam_error, 0); - EXPECT_EQ(feedback_msg.breakbeam_ball_detected, 1); - EXPECT_EQ(feedback_msg.accelerometer_0_error, 0); - EXPECT_EQ(feedback_msg.accelerometer_1_error, 0); - EXPECT_EQ(feedback_msg.gyroscope_0_error, 1); - EXPECT_EQ(feedback_msg.gyroscope_1_error, 1); - EXPECT_EQ(feedback_msg.motor_0_general_error, 0); - EXPECT_EQ(feedback_msg.motor_0_hall_error, 1); - EXPECT_EQ(feedback_msg.motor_1_general_error, 1); - EXPECT_EQ(feedback_msg.motor_1_hall_error, 0); - EXPECT_EQ(feedback_msg.motor_2_general_error, 1); - EXPECT_EQ(feedback_msg.motor_2_hall_error, 1); - EXPECT_EQ(feedback_msg.motor_3_general_error, 0); - EXPECT_EQ(feedback_msg.motor_3_hall_error, 1); - EXPECT_EQ(feedback_msg.motor_4_general_error, 0); - EXPECT_EQ(feedback_msg.motor_4_hall_error, 0); - EXPECT_EQ(feedback_msg.chipper_available, 1); - EXPECT_EQ(feedback_msg.kicker_available, 0); - EXPECT_FLOAT_EQ(feedback_msg.motor_0_temperature, 1.2); - EXPECT_FLOAT_EQ(feedback_msg.motor_1_temperature, 3.4); - EXPECT_FLOAT_EQ(feedback_msg.motor_2_temperature, 5.6); - EXPECT_FLOAT_EQ(feedback_msg.motor_3_temperature, 7.8); - EXPECT_FLOAT_EQ(feedback_msg.motor_4_temperature, 9.0); - EXPECT_FLOAT_EQ(feedback_msg.kicker_charge_level, 1.2); -} - -TEST(ConvertControlDebugTelemetry, PacketConversions) { - MotorResponse_Motion_Packet front_left_motor_packet; - front_left_motor_packet.vel_setpoint = 1.0f; - MotorResponse_Motion_Packet back_left_motor_packet; - back_left_motor_packet.vel_setpoint = 2.0f; - MotorResponse_Motion_Packet back_right_motor_packet; - back_right_motor_packet.vel_setpoint = 3.0f; - MotorResponse_Motion_Packet front_right_motor_packet; - front_right_motor_packet.vel_setpoint = 4.0f; - - ControlDebugTelemetry control_debug_telemetry { - front_left_motor_packet, - back_left_motor_packet, - back_right_motor_packet, - front_right_motor_packet, - - {10.0, 20.0, 30.0}, - {100.0, 200.0, 300.0}, - - {1.6, 1.7, 6.28}, - {1.5, 1.4, 6.00}, - {1.2, 1.1, 5.00}, - {1.8, 1.9, 7.00}, - - {4.0, 5.0, 6.0, 7.0}, - {1.0, 2.0, 3.0, 4.0} - }; - - const auto motion_feedback_msg = ateam_radio_bridge::Convert(control_debug_telemetry); - - // This just checks that the motor ordering is correct - EXPECT_FLOAT_EQ(motion_feedback_msg.motors[motion_feedback_msg.FRONT_LEFT_MOTOR].vel_setpoint, 1.0); - EXPECT_FLOAT_EQ(motion_feedback_msg.motors[motion_feedback_msg.BACK_LEFT_MOTOR].vel_setpoint, 2.0); - EXPECT_FLOAT_EQ(motion_feedback_msg.motors[motion_feedback_msg.BACK_RIGHT_MOTOR].vel_setpoint, 3.0); - EXPECT_FLOAT_EQ(motion_feedback_msg.motors[motion_feedback_msg.FRONT_RIGHT_MOTOR].vel_setpoint, 4.0); - - EXPECT_FLOAT_EQ(motion_feedback_msg.imu.orientation_covariance[0], -1.0); - - EXPECT_FLOAT_EQ(motion_feedback_msg.imu.angular_velocity.x, 10.0); - EXPECT_FLOAT_EQ(motion_feedback_msg.imu.angular_velocity.y, 20.0); - EXPECT_FLOAT_EQ(motion_feedback_msg.imu.angular_velocity.z, 30.0); - - EXPECT_FLOAT_EQ(motion_feedback_msg.imu.linear_acceleration.x, 100.0); - EXPECT_FLOAT_EQ(motion_feedback_msg.imu.linear_acceleration.y, 200.0); - EXPECT_FLOAT_EQ(motion_feedback_msg.imu.linear_acceleration.z, 300.0); - - EXPECT_FLOAT_EQ(motion_feedback_msg.body_velocity_setpoint.linear.x, 1.6); - EXPECT_FLOAT_EQ(motion_feedback_msg.body_velocity_setpoint.linear.y, 1.7); - EXPECT_FLOAT_EQ(motion_feedback_msg.body_velocity_setpoint.angular.z, 6.28); - - EXPECT_FLOAT_EQ(motion_feedback_msg.clamped_body_velocity_setpoint.linear.x, 1.5); - EXPECT_FLOAT_EQ(motion_feedback_msg.clamped_body_velocity_setpoint.linear.y, 1.4); - EXPECT_FLOAT_EQ(motion_feedback_msg.clamped_body_velocity_setpoint.angular.z, 6.0); - - EXPECT_FLOAT_EQ(motion_feedback_msg.body_velocity_state_estimate.linear.x, 1.2); - EXPECT_FLOAT_EQ(motion_feedback_msg.body_velocity_state_estimate.linear.y, 1.1); - EXPECT_FLOAT_EQ(motion_feedback_msg.body_velocity_state_estimate.angular.z, 5.0); - - EXPECT_FLOAT_EQ(motion_feedback_msg.body_velocity_control_variable.linear.x, 1.8); - EXPECT_FLOAT_EQ(motion_feedback_msg.body_velocity_control_variable.linear.y, 1.9); - EXPECT_FLOAT_EQ(motion_feedback_msg.body_velocity_control_variable.angular.z, 7.0); - - EXPECT_FLOAT_EQ(motion_feedback_msg.wheel_velocity_control_variable[motion_feedback_msg.FRONT_LEFT_MOTOR], 4.0); - EXPECT_FLOAT_EQ(motion_feedback_msg.wheel_velocity_control_variable[motion_feedback_msg.BACK_LEFT_MOTOR], 5.0); - EXPECT_FLOAT_EQ(motion_feedback_msg.wheel_velocity_control_variable[motion_feedback_msg.BACK_RIGHT_MOTOR], 6.0); - EXPECT_FLOAT_EQ(motion_feedback_msg.wheel_velocity_control_variable[motion_feedback_msg.FRONT_RIGHT_MOTOR], 7.0); - - EXPECT_FLOAT_EQ(motion_feedback_msg.clamped_wheel_velocity_control_variable[motion_feedback_msg.FRONT_LEFT_MOTOR], 1.0); - EXPECT_FLOAT_EQ(motion_feedback_msg.clamped_wheel_velocity_control_variable[motion_feedback_msg.BACK_LEFT_MOTOR], 2.0); - EXPECT_FLOAT_EQ(motion_feedback_msg.clamped_wheel_velocity_control_variable[motion_feedback_msg.BACK_RIGHT_MOTOR], 3.0); - EXPECT_FLOAT_EQ(motion_feedback_msg.clamped_wheel_velocity_control_variable[motion_feedback_msg.FRONT_RIGHT_MOTOR], 4.0); -} - - -TEST(ConvertMotorFeedback, PacketConversions) { - const MotorResponse_Motion_Packet src { - 0, 1, 0, 1, 0, 0, 1, 1, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, - 0, - 1.0f, - 2.0f, - 3, - 4.0f, - 5.0f, - 6.0f, - 7.0f, - 8.0f, - 9.0f, - 10.0f, - 11.0f - }; - - const auto dst = ateam_radio_bridge::Convert(src); - - EXPECT_FALSE(dst.master_error); - EXPECT_TRUE(dst.hall_power_error); - EXPECT_FALSE(dst.hall_disconnected_error); - EXPECT_TRUE(dst.bldc_transition_error); - EXPECT_FALSE(dst.bldc_commutation_watchdog_error); - EXPECT_FALSE(dst.enc_disconnected_error); - EXPECT_TRUE(dst.enc_decoding_error); - EXPECT_TRUE(dst.hall_enc_vel_disagreement_error); - EXPECT_FALSE(dst.overcurrent_error); - EXPECT_TRUE(dst.undervoltage_error); - EXPECT_TRUE(dst.overvoltage_error); - EXPECT_FALSE(dst.torque_limited); - EXPECT_TRUE(dst.control_loop_time_error); - EXPECT_FALSE(dst.reset_watchdog_independent); - EXPECT_FALSE(dst.reset_watchdog_window); - EXPECT_TRUE(dst.reset_low_power); - EXPECT_FALSE(dst.reset_software); - EXPECT_TRUE(dst.reset_pin); - - EXPECT_FLOAT_EQ(dst.vel_setpoint, 1.0f); - EXPECT_FLOAT_EQ(dst.vel_setpoint_clamped, 2.0f); - EXPECT_EQ(dst.encoder_delta, 3); - EXPECT_FLOAT_EQ(dst.vel_enc_estimate, 4.0f); - EXPECT_FLOAT_EQ(dst.vel_computed_error, 5.0f); - EXPECT_FLOAT_EQ(dst.vel_computed_setpoint, 6.0f); - - EXPECT_FLOAT_EQ(dst.torque_setpoint, 7.0f); - EXPECT_FLOAT_EQ(dst.current_estimate, 8.0f); - EXPECT_FLOAT_EQ(dst.torque_estimate, 9.0f); - EXPECT_FLOAT_EQ(dst.torque_computed_error, 10.0f); - EXPECT_FLOAT_EQ(dst.torque_computed_setpoint, 11.0f); -} diff --git a/ateam_radio_bridge/CMakeLists.txt b/radio/ateam_radio_bridge/CMakeLists.txt similarity index 95% rename from ateam_radio_bridge/CMakeLists.txt rename to radio/ateam_radio_bridge/CMakeLists.txt index 65606a182..cdffff551 100644 --- a/ateam_radio_bridge/CMakeLists.txt +++ b/radio/ateam_radio_bridge/CMakeLists.txt @@ -8,11 +8,11 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) +find_package(ateam_radio_msgs REQUIRED) find_package(ateam_msgs REQUIRED) find_package(ateam_common REQUIRED) add_library(${PROJECT_NAME} SHARED - src/conversion.cpp src/firmware_parameter_server.cpp src/ip_address_helpers.cpp src/radio_bridge_node.cpp @@ -24,6 +24,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC src) ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_components + ateam_radio_msgs ateam_msgs ateam_common ) diff --git a/ateam_radio_bridge/package.xml b/radio/ateam_radio_bridge/package.xml similarity index 95% rename from ateam_radio_bridge/package.xml rename to radio/ateam_radio_bridge/package.xml index 432c06fec..4e57f0676 100644 --- a/ateam_radio_bridge/package.xml +++ b/radio/ateam_radio_bridge/package.xml @@ -12,6 +12,7 @@ rclcpp rclcpp_components sensor_msgs + ateam_radio_msgs ateam_msgs ateam_common diff --git a/ateam_radio_bridge/src/firmware_parameter_server.cpp b/radio/ateam_radio_bridge/src/firmware_parameter_server.cpp similarity index 100% rename from ateam_radio_bridge/src/firmware_parameter_server.cpp rename to radio/ateam_radio_bridge/src/firmware_parameter_server.cpp diff --git a/ateam_radio_bridge/src/firmware_parameter_server.hpp b/radio/ateam_radio_bridge/src/firmware_parameter_server.hpp similarity index 97% rename from ateam_radio_bridge/src/firmware_parameter_server.hpp rename to radio/ateam_radio_bridge/src/firmware_parameter_server.hpp index 5e86dc5ef..94ba13c2b 100644 --- a/ateam_radio_bridge/src/firmware_parameter_server.hpp +++ b/radio/ateam_radio_bridge/src/firmware_parameter_server.hpp @@ -24,7 +24,7 @@ #include #include -#include +#include #include #include #include diff --git a/ateam_radio_bridge/src/ip_address_helpers.cpp b/radio/ateam_radio_bridge/src/ip_address_helpers.cpp similarity index 100% rename from ateam_radio_bridge/src/ip_address_helpers.cpp rename to radio/ateam_radio_bridge/src/ip_address_helpers.cpp diff --git a/ateam_radio_bridge/src/ip_address_helpers.hpp b/radio/ateam_radio_bridge/src/ip_address_helpers.hpp similarity index 100% rename from ateam_radio_bridge/src/ip_address_helpers.hpp rename to radio/ateam_radio_bridge/src/ip_address_helpers.hpp diff --git a/ateam_radio_bridge/src/radio_bridge_node.cpp b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp similarity index 82% rename from ateam_radio_bridge/src/radio_bridge_node.cpp rename to radio/ateam_radio_bridge/src/radio_bridge_node.cpp index 75806f6cb..247235fbc 100644 --- a/ateam_radio_bridge/src/radio_bridge_node.cpp +++ b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp @@ -26,17 +26,19 @@ #include #include #include +#include +#include +#include +#include +#include +#include #include -#include -#include -#include #include #include #include #include #include "rnp_packet_helpers.hpp" -#include "conversion.hpp" #include "ip_address_helpers.hpp" #include "firmware_parameter_server.hpp" @@ -54,7 +56,8 @@ class RadioBridgeNode : public rclcpp::Node : rclcpp::Node("radio_bridge", options), timeout_threshold_(declare_parameter("timeout_ms", 250)), command_timeout_threshold_(declare_parameter("command_timeout_ms", 100)), - game_controller_listener_(*this, std::bind_front(&RadioBridgeNode::TeamColorChangeCallback, this)), + game_controller_listener_(*this, + std::bind_front(&RadioBridgeNode::TeamColorChangeCallback, this)), discovery_receiver_(declare_parameter("discovery_address", "224.4.20.69"), declare_parameter("discovery_port", 42069), std::bind(&RadioBridgeNode::DiscoveryMessageCallback, this, std::placeholders::_1, @@ -62,6 +65,12 @@ class RadioBridgeNode : public rclcpp::Node declare_parameter("net_interface_address", "")), firmware_parameter_server_(*this, connections_) { + declare_parameters("controls_enabled", { + {"body_vel", true}, + {"wheel_vel", true}, + {"wheel_torque", false} + }); + ateam_common::indexed_topic_helpers::create_indexed_subscribers( motion_command_subscriptions_, "~/robot_motion_commands/robot", @@ -69,15 +78,21 @@ class RadioBridgeNode : public rclcpp::Node &RadioBridgeNode::MotionCommandCallback, this); - ateam_common::indexed_topic_helpers::create_indexed_publishers( + ateam_common::indexed_topic_helpers::create_indexed_publishers( + connection_publishers_, + "~/robot_feedback/connection/robot", + rclcpp::SystemDefaultsQoS(), + this); + + ateam_common::indexed_topic_helpers::create_indexed_publishers( feedback_publishers_, - "~/robot_feedback/status/robot", + "~/robot_feedback/basic/robot", rclcpp::SystemDefaultsQoS(), this); - ateam_common::indexed_topic_helpers::create_indexed_publishers( + ateam_common::indexed_topic_helpers::create_indexed_publishers( motion_feedback_publishers_, - "~/robot_feedback/motion/robot", + "~/robot_feedback/extended/robot", rclcpp::SystemDefaultsQoS(), this); @@ -105,8 +120,12 @@ class RadioBridgeNode : public rclcpp::Node ateam_common::GameControllerListener game_controller_listener_; std::array::SharedPtr, 16> motion_command_subscriptions_; - std::array::SharedPtr, 16> feedback_publishers_; - std::array::SharedPtr, 16> motion_feedback_publishers_; + std::array::SharedPtr, + 16> connection_publishers_; + std::array::SharedPtr, + 16> feedback_publishers_; + std::array::SharedPtr, + 16> motion_feedback_publishers_; ateam_common::MulticastReceiver discovery_receiver_; FirmwareParameterServer firmware_parameter_server_; std::array, 16> connections_; @@ -155,9 +174,9 @@ class RadioBridgeNode : public rclcpp::Node for (auto i = 0ul; i < connections_.size(); ++i) { std::unique_lock lock(mutex_); if (!connections_[i]) { - ateam_msgs::msg::RobotFeedback feedback_message; - feedback_message.radio_connected = false; - feedback_publishers_[i]->publish(feedback_message); + ateam_radio_msgs::msg::ConnectionStatus connection_message; + connection_message.radio_connected = false; + connection_publishers_[i]->publish(connection_message); continue; } const auto & last_heartbeat_time = last_heartbeat_timestamp_[i]; @@ -187,6 +206,13 @@ class RadioBridgeNode : public rclcpp::Node motion_commands_[id].kick_request = ateam_msgs::msg::RobotMotionCommand::KR_DISABLE; } BasicControl control_msg; + control_msg.request_shutdown = false; + control_msg.game_state_in_stop = game_controller_listener_.GetGameCommand() == ateam_common::GameCommand::Stop; + control_msg.emergency_stop = false; + control_msg.body_vel_controls_enabled = get_parameter("controls_enabled.body_vel").as_bool(); + control_msg.wheel_vel_control_enabled = get_parameter("controls_enabled.wheel_vel").as_bool(); + control_msg.wheel_torque_control_enabled = get_parameter("controls_enabled.wheel_torque").as_bool(); + control_msg.play_song = 0; control_msg.vel_x_linear = motion_commands_[id].twist.linear.x; control_msg.vel_y_linear = motion_commands_[id].twist.linear.y; control_msg.vel_z_angular = motion_commands_[id].twist.angular.z; @@ -249,6 +275,7 @@ class RadioBridgeNode : public rclcpp::Node discovery_receiver_.SendTo( sender_address, sender_port, reinterpret_cast(&reply_packet), GetPacketSize(reply_packet.command_code)); + RCLCPP_WARN(get_logger(), "Rejecting discovery packet. Invalid robot ID: %d", robot_id); return; } @@ -262,6 +289,7 @@ class RadioBridgeNode : public rclcpp::Node discovery_receiver_.SendTo( sender_address, sender_port, reinterpret_cast(&reply_packet), GetPacketSize(reply_packet.command_code)); + RCLCPP_WARN(get_logger(), "Rejecting discovery packet. Robot ID already connected: %d", robot_id); return; } @@ -317,9 +345,11 @@ class RadioBridgeNode : public rclcpp::Node return; } if (std::holds_alternative(data_var)) { - auto msg = Convert(std::get(data_var)); - msg.radio_connected = true; - feedback_publishers_[robot_id]->publish(msg); + feedback_publishers_[robot_id]->publish(ateam_radio_msgs::Convert( + std::get(data_var))); + ateam_radio_msgs::msg::ConnectionStatus connection_message; + connection_message.radio_connected = true; + connection_publishers_[robot_id]->publish(connection_message); } break; } @@ -331,9 +361,9 @@ class RadioBridgeNode : public rclcpp::Node return; } - if (std::holds_alternative(data_var)) { - auto msg = Convert(std::get(data_var)); - motion_feedback_publishers_[robot_id]->publish(msg); + if (std::holds_alternative(data_var)) { + motion_feedback_publishers_[robot_id]->publish(ateam_radio_msgs::Convert( + std::get(data_var))); } break; } @@ -341,11 +371,13 @@ class RadioBridgeNode : public rclcpp::Node { const auto data_var = ExtractData(packet, error); if (!error.empty()) { - RCLCPP_WARN(get_logger(), "Ignoring parameter command response message. %s", error.c_str()); + RCLCPP_WARN(get_logger(), "Ignoring parameter command response message. %s", + error.c_str()); return; } if(std::holds_alternative(data_var)) { - firmware_parameter_server_.HandleIncomingParameterPacket(robot_id, std::get(data_var)); + firmware_parameter_server_.HandleIncomingParameterPacket(robot_id, + std::get(data_var)); } break; } diff --git a/ateam_radio_bridge/src/rnp_packet_helpers.cpp b/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp similarity index 98% rename from ateam_radio_bridge/src/rnp_packet_helpers.cpp rename to radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp index 21a82efd2..191538dbc 100644 --- a/ateam_radio_bridge/src/rnp_packet_helpers.cpp +++ b/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp @@ -68,7 +68,7 @@ std::size_t GetPacketSize(const CommandCode & command_code) return kPacketHeaderSize + sizeof(HelloResponse); break; case CC_CONTROL_DEBUG_TELEMETRY: - return kPacketHeaderSize + sizeof(ControlDebugTelemetry); + return kPacketHeaderSize + sizeof(ExtendedTelemetry); break; case CC_ROBOT_PARAMETER_COMMAND: return kPacketHeaderSize + sizeof(ParameterCommand); @@ -231,8 +231,8 @@ PacketDataVariant ExtractData(const RadioPacket & packet, std::string & error) } case CC_CONTROL_DEBUG_TELEMETRY: { - if (packet.data_length != sizeof(ControlDebugTelemetry)) { - error = "Incorrect data length for ControlDebugTelemetry type."; + if (packet.data_length != sizeof(ExtendedTelemetry)) { + error = "Incorrect data length for ExtendedTelemetry type."; break; } var = packet.data.control_debug_telemetry; diff --git a/ateam_radio_bridge/src/rnp_packet_helpers.hpp b/radio/ateam_radio_bridge/src/rnp_packet_helpers.hpp similarity index 90% rename from ateam_radio_bridge/src/rnp_packet_helpers.hpp rename to radio/ateam_radio_bridge/src/rnp_packet_helpers.hpp index 4534937ca..f773bfa55 100644 --- a/ateam_radio_bridge/src/rnp_packet_helpers.hpp +++ b/radio/ateam_radio_bridge/src/rnp_packet_helpers.hpp @@ -28,11 +28,11 @@ #include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include namespace ateam_radio_bridge { @@ -77,7 +77,7 @@ RadioPacket CreateEmptyPacket(const CommandCode command_code); RadioPacket ParsePacket(const uint8_t * data, const std::size_t data_length, std::string & error); using PacketDataVariant = std::variant; + BasicControl, ExtendedTelemetry, ParameterCommand>; PacketDataVariant ExtractData(const RadioPacket & packet, std::string & error); diff --git a/ateam_radio_bridge/test/CMakeLists.txt b/radio/ateam_radio_bridge/test/CMakeLists.txt similarity index 100% rename from ateam_radio_bridge/test/CMakeLists.txt rename to radio/ateam_radio_bridge/test/CMakeLists.txt diff --git a/ateam_radio_bridge/test/launch_tests/CMakeLists.txt b/radio/ateam_radio_bridge/test/launch_tests/CMakeLists.txt similarity index 100% rename from ateam_radio_bridge/test/launch_tests/CMakeLists.txt rename to radio/ateam_radio_bridge/test/launch_tests/CMakeLists.txt diff --git a/ateam_radio_bridge/test/launch_tests/bridge_command_test.py b/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py similarity index 96% rename from ateam_radio_bridge/test/launch_tests/bridge_command_test.py rename to radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py index 4d9dfaee6..3b98a39f5 100644 --- a/ateam_radio_bridge/test/launch_tests/bridge_command_test.py +++ b/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py @@ -1,6 +1,5 @@ """Tests the bridge's ability to pass command messages to robots.""" -import socket import struct import time import unittest @@ -85,10 +84,10 @@ def test_commands(self): break self.cmd_pub.publish(cmd_msg) last_packet = self.robot.getLastCmdMessage() - if len(last_packet) != 36: + if len(last_packet) != 40: continue # Extract BasicControl.vel_x_linear - vel_x_linear = struct.unpack(" + $ + $ +) +set_target_properties(${LIB_TARGET} PROPERTIES CXX_STANDARD 20) +rosidl_get_typesupport_target(cpp_typesupport_target ${MSG_TARGET} rosidl_typesupport_cpp) +target_link_libraries(${LIB_TARGET} PUBLIC ${cpp_typesupport_target}) + +ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) + +install( + DIRECTORY include/ + DESTINATION include + PATTERN packets EXCLUDE +) +install( + DIRECTORY ${GENERATED_DIR}/include/ + DESTINATION include/ateam_radio_msgs/ateam_radio_msgs +) +file(GLOB SYMLINKED_INCLUDES include/ateam_radio_msgs/packets/*) +install( + FILES ${SYMLINKED_INCLUDES} + DESTINATION include/ateam_radio_msgs/ateam_radio_msgs/packets +) +install( + TARGETS ${LIB_TARGET} + EXPORT ${PROJECT_NAME}Targets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +if(BUILD_TESTING) + file(GLOB_RECURSE submodule_files software-communication/*) + file(GLOB symlinked_headers include/ateam_radio_msgs/packets/*) + set(_linter_excludes + software-communication + ${submodule_files} + ${symlinked_headers} + ) + + find_package(ament_cmake_lint_cmake REQUIRED) + ament_lint_cmake() + + find_package(ament_cmake_flake8 REQUIRED) + ament_flake8(EXCLUDE ${_linter_excludes}) + + find_package(ament_cmake_pep257 REQUIRED) + ament_pep257(EXCLUDE ${_linter_excludes}) +endif() + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/radio/ateam_radio_msgs/LICENSE b/radio/ateam_radio_msgs/LICENSE new file mode 100644 index 000000000..30e8e2ece --- /dev/null +++ b/radio/ateam_radio_msgs/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/radio/ateam_radio_msgs/cmake/generate_conversion_code.cmake b/radio/ateam_radio_msgs/cmake/generate_conversion_code.cmake new file mode 100644 index 000000000..7fe37feab --- /dev/null +++ b/radio/ateam_radio_msgs/cmake/generate_conversion_code.cmake @@ -0,0 +1,56 @@ +# Copyright 2025 A Team +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +function(generate_conversion_code) + set(oneValueArgs SOURCE DESTINATION) + set(multiValueArgs STRUCTS) + cmake_parse_arguments(PARSE_ARGV 0 arg "" "${oneValueArgs}" "${multiValueArgs}") + if(NOT arg_SOURCE) + message(FATAL_ERROR "Source header must be specified.") + endif() + if(NOT arg_DESTINATION) + message(FATAL_ERROR "Destination directory must be specified.") + endif() + if(NOT arg_STRUCTS) + message(FATAL_ERROR "At least one struct must be specified.") + endif() + + file(MAKE_DIRECTORY "${arg_DESTINATION}") + + set(_generation_script "${CMAKE_CURRENT_SOURCE_DIR}/scripts/generate_conversion_code.py") + if(NOT EXISTS "${_generation_script}") + message(FATAL_ERROR "Script ${_generation_script} does not exist.") + endif() + + set(${generated_msgs_files} "") + foreach(struct ${arg_STRUCTS}) + list(APPEND generated_msgs_files "${arg_DESTINATION}/${struct}.msg") + endforeach() + + execute_process( + COMMAND python3 ${_generation_script} ${arg_DESTINATION} ${arg_SOURCE} ${arg_STRUCTS} + RESULT_VARIABLE result + OUTPUT_VARIABLE output + ERROR_VARIABLE error + ) + if(result) + message(FATAL_ERROR "Failed to generate conversion code: ${error}") + endif() +endfunction() diff --git a/radio/ateam_radio_msgs/cmake/generate_msgs.cmake b/radio/ateam_radio_msgs/cmake/generate_msgs.cmake new file mode 100644 index 000000000..294bf44f8 --- /dev/null +++ b/radio/ateam_radio_msgs/cmake/generate_msgs.cmake @@ -0,0 +1,56 @@ +# Copyright 2025 A Team +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +function(generate_msgs) + set(oneValueArgs SOURCE DESTINATION) + set(multiValueArgs STRUCTS) + cmake_parse_arguments(PARSE_ARGV 0 arg "" "${oneValueArgs}" "${multiValueArgs}") + if(NOT arg_SOURCE) + message(FATAL_ERROR "Source header must be specified.") + endif() + if(NOT arg_DESTINATION) + message(FATAL_ERROR "Destination directory must be specified.") + endif() + if(NOT arg_STRUCTS) + message(FATAL_ERROR "At least one struct must be specified.") + endif() + + file(MAKE_DIRECTORY "${arg_DESTINATION}") + + set(_generate_msgs_script "${CMAKE_CURRENT_SOURCE_DIR}/scripts/generate_msgs.py") + if(NOT EXISTS "${_generate_msgs_script}") + message(FATAL_ERROR "Script ${_generate_msgs_script} does not exist.") + endif() + + set(${generated_msgs_files} "") + foreach(struct ${arg_STRUCTS}) + list(APPEND generated_msgs_files "${arg_DESTINATION}/${struct}.msg") + endforeach() + + execute_process( + COMMAND python3 ${_generate_msgs_script} ${arg_DESTINATION} ${arg_SOURCE} ${arg_STRUCTS} + RESULT_VARIABLE result + OUTPUT_VARIABLE output + ERROR_VARIABLE error + ) + if(result) + message(FATAL_ERROR "Failed to generate messages: ${error}") + endif() +endfunction() diff --git a/radio/ateam_radio_msgs/include/ateam_radio_msgs/packets b/radio/ateam_radio_msgs/include/ateam_radio_msgs/packets new file mode 120000 index 000000000..9bb25cf94 --- /dev/null +++ b/radio/ateam_radio_msgs/include/ateam_radio_msgs/packets @@ -0,0 +1 @@ +../../software-communication/ateam-common-packets/include/ \ No newline at end of file diff --git a/radio/ateam_radio_msgs/msg/ConnectionStatus.msg b/radio/ateam_radio_msgs/msg/ConnectionStatus.msg new file mode 100644 index 000000000..f3d337426 --- /dev/null +++ b/radio/ateam_radio_msgs/msg/ConnectionStatus.msg @@ -0,0 +1 @@ +bool radio_connected diff --git a/radio/ateam_radio_msgs/package.xml b/radio/ateam_radio_msgs/package.xml new file mode 100644 index 000000000..8470fe0a9 --- /dev/null +++ b/radio/ateam_radio_msgs/package.xml @@ -0,0 +1,24 @@ + + + + ateam_radio_msgs + 0.0.0 + Generates message files from shared radio headers. + Matthew Barulic + MIT + + ament_cmake + + rosidl_default_generators + + + ament_cmake_flake8 + ament_cmake_lint_cmake + ament_cmake_pep257 + + rosidl_interface_packages + + + ament_cmake + + diff --git a/radio/ateam_radio_msgs/scripts/generate_conversion_code.py b/radio/ateam_radio_msgs/scripts/generate_conversion_code.py new file mode 100644 index 000000000..a1c907826 --- /dev/null +++ b/radio/ateam_radio_msgs/scripts/generate_conversion_code.py @@ -0,0 +1,221 @@ +# Copyright 2025 A Team +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +"""Generates C++ conversion code for ROS2 messages from C structs.""" + +import os +import pathlib +import re +import sys + +import clang.cindex + + +def generate_conversion_code(output_directory, header_file, struct_names): + """Generate conversion functions.""" + index = clang.cindex.Index.create() + translation_unit = index.parse(header_file) + generate_header_file(output_directory, translation_unit, struct_names) + generate_implementation_file(output_directory, translation_unit, struct_names) + + +def generate_header_file(output_directory, translation_unit, struct_names): + """Generate header file for conversion functions.""" + header_text = ( + '// Auto-generated conversion functions for ROS2 messages\n' + '#ifndef CONVERSION_HPP_\n' + '#define CONVERSION_HPP_\n\n' + ) + for struct_name in struct_names: + header_text += '#include \n' + for node in translation_unit.cursor.get_children(): + if node.kind == clang.cindex.CursorKind.STRUCT_DECL: + msg_name = node.spelling + if msg_name not in struct_names: + continue + declaration_file = pathlib.Path( + node.get_definition().location.file.name + ).name + header_text += f'#include \n' + header_text += 'namespace ateam_radio_msgs {\n' + for node in translation_unit.cursor.get_children(): + if node.kind == clang.cindex.CursorKind.STRUCT_DECL: + msg_name = node.spelling + if msg_name not in struct_names: + continue + header_text += generate_conversion_function_declaration(node) + '\n' + header_text += '} // namespace ateam_radio_msgs\n\n' + header_text += '#endif // CONVERSION_HPP_\n' + os.makedirs(f'{output_directory}/include', exist_ok=True) + file_path = f'{output_directory}/include/conversion.hpp' + with open(file_path, 'w') as f: + f.write(header_text) + + +def generate_implementation_file(output_directory, translation_unit, struct_names): + """Generate implementation file for conversion functions.""" + enums = [] + impl_text = '#include "conversion.hpp"\n\n' 'namespace ateam_radio_msgs {\n\n' + for node in translation_unit.cursor.get_children(): + match node.kind: + case clang.cindex.CursorKind.ENUM_DECL: + enums.append(collect_enum_details(node)) + case clang.cindex.CursorKind.STRUCT_DECL: + msg_name = node.spelling + if msg_name not in struct_names: + continue + impl_text += generate_conversion_function_implementation(node, enums) + case _: + continue + impl_text += '} // namespace ateam_radio_bridge\n' + os.makedirs(f'{output_directory}/src', exist_ok=True) + file_path = f'{output_directory}/src/conversion.cpp' + with open(file_path, 'w') as f: + f.write(impl_text) + + +def generate_conversion_function_declaration(struct_node): + """Generate a conversion function declaration for the given struct.""" + param_name = re.sub(r'(? ' + ' [ ...]' + ) + sys.exit(1) + generate_conversion_code(sys.argv[1], sys.argv[2], sys.argv[3:]) diff --git a/radio/ateam_radio_msgs/scripts/generate_msgs.py b/radio/ateam_radio_msgs/scripts/generate_msgs.py new file mode 100644 index 000000000..2ad51cb0b --- /dev/null +++ b/radio/ateam_radio_msgs/scripts/generate_msgs.py @@ -0,0 +1,195 @@ +# Copyright 2025 A Team +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +"""Generates ROS2 message definitions from a C header file containing struct definitions.""" + +import pathlib +import sys + +import clang.cindex + + +def generate_msgs_for_file(output_dir, file_path, struct_names): + """Generate ROS2 message definitions from a C header file.""" + index = clang.cindex.Index.create() + translation_unit = index.parse(file_path) + + enums = [] + + for node in translation_unit.cursor.get_children(): + match node.kind: + case clang.cindex.CursorKind.ENUM_DECL: + enums.append(collect_enum_details(node)) + case clang.cindex.CursorKind.STRUCT_DECL: + msg_name = node.spelling + if msg_name not in struct_names: + continue + declaration_file = pathlib.Path( + node.get_definition().location.file.name + ).name + msg = generate_msg_for_struct(node, enums) + write_msg_to_file(output_dir, msg, declaration_file) + case _: + continue + + +def generate_msg_for_struct(struct_ast_node, enums): + """Generate a ROS2 message definition for a single struct AST node.""" + type_name = struct_ast_node.spelling + declarations = [] + for field in struct_ast_node.get_children(): + if field.kind != clang.cindex.CursorKind.FIELD_DECL: + continue + if field.spelling.startswith('_'): + continue + add_field_declarations(declarations, field, enums) + return {'type_name': type_name, 'declarations': declarations} + + +def write_msg_to_file(output_dir, msg, input_file_path): + """Write the generated ROS2 message definition to a file.""" + type_name = msg['type_name'] + declarations = msg['declarations'] + file_path = f'{output_dir}/{type_name}.msg' + + with open(file_path, 'w') as f: + f.write(f'# Auto-generated from {input_file_path}\n') + for declaration in declarations: + f.write(f'{declaration}\n') + + +def add_field_declarations(declarations, field_node, enums): + """Add declarations for a field node to the list of declarations.""" + if field_node.is_bitfield(): + add_bitfield_declaration(declarations, field_node) + elif field_node.type.kind == clang.cindex.TypeKind.CONSTANTARRAY: + add_array_declaration(declarations, field_node) + elif field_node.type.spelling in [e['type_name'] for e in enums]: + add_enum_declaration(declarations, field_node, enums) + else: + ros2_type = get_ros2_basic_type(field_node.type) + declarations.append(f'{ros2_type} {field_node.spelling}') + + +def add_bitfield_declaration(declarations, field_node): + """Add a declaration for a bitfield field node.""" + type_name = '' + bitfield_width = field_node.get_bitfield_width() + if bitfield_width == 1: + type_name = 'bool' + elif bitfield_width <= 8: + type_name = 'uint8' + elif bitfield_width <= 16: + type_name = 'uint16' + elif bitfield_width <= 32: + type_name = 'uint32' + elif bitfield_width <= 64: + type_name = 'uint64' + else: + raise ValueError( + f'Unsupported bitfield width: {bitfield_width} for field {field_node.spelling}' + ) + declarations.append(f'{type_name} {field_node.spelling}') + + +def add_array_declaration(declarations, field_node): + """Add a declaration for an array field node.""" + element_type = get_ros2_basic_type(field_node.type.get_array_element_type()) + declarations.append(f'{element_type}[] {field_node.spelling}') + + +def add_enum_declaration(declarations, field_node, enums): + """Add a declaration for an enum field node.""" + enum_details = [e for e in enums if e['type_name'] == field_node.type.spelling][0] + declarations.append(f'{enum_details["underlying_type"]} {field_node.spelling}') + for value_name, value in enum_details['values']: + declarations.append( + f'{enum_details["underlying_type"]} {value_name.upper()} = {value}' + ) + + +def collect_enum_details(enum_node): + """Collect details of an enum AST node.""" + type_name = enum_node.spelling + underlying_type = get_ros2_basic_type(enum_node.enum_type) + values = [] + for child in enum_node.get_children(): + if child.kind == clang.cindex.CursorKind.ENUM_CONSTANT_DECL: + values.append((child.spelling, child.enum_value)) + return { + 'type_name': type_name, + 'underlying_type': underlying_type, + 'values': values, + } + + +def get_ros2_basic_type(field_type): + """Map a basic C type to a ROS2 message type.""" + match field_type.kind: + case clang.cindex.TypeKind.INT: + return 'int32' + case clang.cindex.TypeKind.UINT: + return 'uint32' + case clang.cindex.TypeKind.SHORT: + return 'int16' + case clang.cindex.TypeKind.USHORT: + return 'uint16' + case clang.cindex.TypeKind.UCHAR: + return 'uint8' + case clang.cindex.TypeKind.FLOAT: + return 'float32' + case clang.cindex.TypeKind.DOUBLE: + return 'float64' + case clang.cindex.TypeKind.CHAR_S: + return 'string' + case clang.cindex.TypeKind.BOOL: + return 'bool' + case clang.cindex.TypeKind.ELABORATED: + match field_type.spelling: + case 'uint8_t': + return 'uint8' + case 'uint16_t': + return 'uint16' + case 'uint32_t': + return 'uint32' + case 'uint64_t': + return 'uint64' + case 'int8_t': + return 'int8' + case 'int16_t': + return 'int16' + case 'int32_t': + return 'int32' + case 'int64_t': + return 'int64' + case _: + return 'ateam_radio_msgs/' + field_type.spelling + case _: + raise ValueError(f'Unsupported basic type: {field_type.spelling}') + + +if __name__ == '__main__': + if len(sys.argv) < 4: + print( + 'Usage: python generate_msgs.py ' + ' [ ...]' + ) + sys.exit(1) + generate_msgs_for_file(sys.argv[1], sys.argv[2], sys.argv[3:]) diff --git a/radio/ateam_radio_msgs/software-communication b/radio/ateam_radio_msgs/software-communication new file mode 160000 index 000000000..0f08a44ae --- /dev/null +++ b/radio/ateam_radio_msgs/software-communication @@ -0,0 +1 @@ +Subproject commit 0f08a44ae1092fe5200767c6319b7ccca9dd58e9 diff --git a/radio/ateam_radio_msgs/srv/GetFirmwareParameter.srv b/radio/ateam_radio_msgs/srv/GetFirmwareParameter.srv new file mode 100644 index 000000000..deace5fd9 --- /dev/null +++ b/radio/ateam_radio_msgs/srv/GetFirmwareParameter.srv @@ -0,0 +1,21 @@ +# Request +int32 robot_id +int8 parameter_id + +int8 PARAM_ID_VEL_PID_X=0 +int8 PARAM_ID_VEL_PID_Y=1 +int8 PARAM_ID_ANGULAR_VEL_PID_Z=2 +int8 PARAM_ID_VEL_CGKF_ENCODER_NOISE=3 +int8 PARAM_ID_VEL_CGKF_GYRO_NOISE=4 +int8 PARAM_ID_VEL_CGKF_PROCESS_NOISE=5 +int8 PARAM_ID_VEL_CGFK_INITIAL_COVARIANCE=6 +int8 PARAM_ID_VEL_CGKF_K_MATRIX=7 +int8 PARAM_ID_RC_BODY_VEL_LIMIT=8 +int8 PARAM_ID_RC_BODY_ACC_LIMIT=9 +int8 PARAM_ID_RC_WHEEL_ACC_LIMIT=10 + +--- +# Response +float32[] data +bool success +string reason diff --git a/radio/ateam_radio_msgs/srv/SetFirmwareParameter.srv b/radio/ateam_radio_msgs/srv/SetFirmwareParameter.srv new file mode 100644 index 000000000..c175777c2 --- /dev/null +++ b/radio/ateam_radio_msgs/srv/SetFirmwareParameter.srv @@ -0,0 +1,21 @@ +# Request +int32 robot_id +int8 parameter_id +float32[] data + +int8 PARAM_ID_VEL_PID_X=0 +int8 PARAM_ID_VEL_PID_Y=1 +int8 PARAM_ID_ANGULAR_VEL_PID_Z=2 +int8 PARAM_ID_VEL_CGKF_ENCODER_NOISE=3 +int8 PARAM_ID_VEL_CGKF_GYRO_NOISE=4 +int8 PARAM_ID_VEL_CGKF_PROCESS_NOISE=5 +int8 PARAM_ID_VEL_CGFK_INITIAL_COVARIANCE=6 +int8 PARAM_ID_VEL_CGKF_K_MATRIX=7 +int8 PARAM_ID_RC_BODY_VEL_LIMIT=8 +int8 PARAM_ID_RC_BODY_ACC_LIMIT=9 +int8 PARAM_ID_RC_WHEEL_ACC_LIMIT=10 + +--- +# Response +bool success +string reason diff --git a/ateam_ssl_simulation_radio_bridge/CMakeLists.txt b/radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt similarity index 95% rename from ateam_ssl_simulation_radio_bridge/CMakeLists.txt rename to radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt index 3c54db218..b68d98e9c 100644 --- a/ateam_ssl_simulation_radio_bridge/CMakeLists.txt +++ b/radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(ateam_common REQUIRED) find_package(ateam_msgs REQUIRED) +find_package(ateam_radio_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(ssl_league_protobufs REQUIRED) @@ -24,6 +25,7 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_components ateam_common ateam_msgs + ateam_radio_msgs tf2 tf2_geometry_msgs ssl_league_protobufs diff --git a/ateam_ssl_simulation_radio_bridge/package.xml b/radio/ateam_ssl_simulation_radio_bridge/package.xml similarity index 96% rename from ateam_ssl_simulation_radio_bridge/package.xml rename to radio/ateam_ssl_simulation_radio_bridge/package.xml index 45f9068b2..12ebcc556 100644 --- a/ateam_ssl_simulation_radio_bridge/package.xml +++ b/radio/ateam_ssl_simulation_radio_bridge/package.xml @@ -13,6 +13,7 @@ rclcpp_components ateam_common ateam_msgs + ateam_radio_msgs tf2 tf2_geometry_msgs ssl_league_protobufs diff --git a/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp similarity index 95% rename from ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp rename to radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp index dc218a994..a7a7bb6e8 100644 --- a/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp @@ -28,14 +28,16 @@ namespace ateam_ssl_simulation_radio_bridge::message_conversions { -ateam_msgs::msg::RobotFeedback fromProto(const RobotFeedback & proto_msg) +ateam_radio_msgs::msg::BasicTelemetry fromProto(const RobotFeedback & proto_msg) { - ateam_msgs::msg::RobotFeedback robot_feedback; + ateam_radio_msgs::msg::BasicTelemetry robot_feedback; - robot_feedback.radio_connected = true; if(proto_msg.has_dribbler_ball_contact()) { robot_feedback.breakbeam_ball_detected = proto_msg.dribbler_ball_contact(); } + robot_feedback.kicker_available = true; + robot_feedback.chipper_available = true; + robot_feedback.battery_percent = 100; return robot_feedback; } diff --git a/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp similarity index 93% rename from ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp rename to radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp index 0bfa44c64..fc1dc8aa0 100644 --- a/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp @@ -27,13 +27,13 @@ #include -#include +#include #include namespace ateam_ssl_simulation_radio_bridge::message_conversions { -ateam_msgs::msg::RobotFeedback fromProto(const RobotFeedback & proto_msg); +ateam_radio_msgs::msg::BasicTelemetry fromProto(const RobotFeedback & proto_msg); RobotControl fromMsg(const ateam_msgs::msg::RobotMotionCommand & ros_msg, int robot_id); diff --git a/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp similarity index 89% rename from ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp rename to radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp index d6619fea2..9f6836c97 100644 --- a/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp @@ -35,7 +35,8 @@ #include #include #include -#include +#include +#include #include #include @@ -52,6 +53,9 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node gc_listener_(*this, std::bind_front(&SSLSimulationRadioBridgeNode::team_color_change_callback, this)) { + using ateam_common::indexed_topic_helpers::create_indexed_publishers; + using ateam_common::indexed_topic_helpers::create_indexed_subscribers; + SET_ROS_PROTOBUF_LOG_HANDLER("ateam_ssl_simulation_radio_bridge.protobuf"); declare_parameter("ssl_sim_radio_ip", "127.0.0.1"); @@ -61,7 +65,7 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node team_color_change_callback(ateam_common::TeamColor::Blue); - ateam_common::indexed_topic_helpers::create_indexed_subscribers + create_indexed_subscribers ( command_subscriptions_, Topics::kRobotMotionCommandPrefix, @@ -69,12 +73,18 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node &SSLSimulationRadioBridgeNode::message_callback, this); - ateam_common::indexed_topic_helpers::create_indexed_publishers( + create_indexed_publishers( feedback_publishers_, Topics::kRobotFeedbackPrefix, rclcpp::SystemDefaultsQoS(), this); + create_indexed_publishers( + connection_publishers_, + Topics::kRobotConnectionStatusPrefix, + rclcpp::SystemDefaultsQoS(), + this); + send_simulator_control_service_ = create_service("~/send_simulator_control_packet", std::bind(&SSLSimulationRadioBridgeNode::handle_send_simulator_control, this, @@ -187,6 +197,9 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node for (const auto & single_feedback : feedback_proto.feedback()) { int robot_id = single_feedback.id(); feedback_publishers_.at(robot_id)->publish(message_conversions::fromProto(single_feedback)); + ateam_radio_msgs::msg::ConnectionStatus connection_msg; + connection_msg.radio_connected = true; + connection_publishers_.at(robot_id)->publish(connection_msg); } } @@ -219,7 +232,10 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node std::unique_ptr udp_sim_control_; std::array::SharedPtr, 16> command_subscriptions_; - std::array::SharedPtr, 16> feedback_publishers_; + std::array::SharedPtr, + 16> feedback_publishers_; + std::array::SharedPtr, + 16> connection_publishers_; rclcpp::Service::SharedPtr send_simulator_control_service_; rclcpp::TimerBase::SharedPtr zero_command_timer_;