diff --git a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp index c8e92b08..9cc1be27 100644 --- a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp +++ b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -69,7 +70,7 @@ class RadioBridgeNode : public rclcpp::Node public: RadioBridgeNode(const rclcpp::NodeOptions & options) : rclcpp::Node("radio_bridge", options), - sustain_timeout_threshold_(declare_parameter("sustain_timeout_ms", 250)), + sustain_timeout_threshold_(declare_parameter("sustain_timeout_ms", 500)), connect_timeout_threshold_(declare_parameter("connect_timeout_ms", 750)), vision_state_staleness_threshold_(declare_parameter("vision_state_staleness_ms", 100)), command_timeout_threshold_(declare_parameter("command_timeout_ms", 100)), @@ -95,6 +96,11 @@ class RadioBridgeNode : public rclcpp::Node {"wheel_torque", true} }); + joy_status_sub_ = create_subscription( + std::string(Topics::kJoystickControlStatus), + rclcpp::QoS(1).transient_local(), + std::bind(&RadioBridgeNode::JoyStatusCallback, this, std::placeholders::_1)); + ateam_common::indexed_topic_helpers::create_indexed_subscribers( motion_command_subscriptions_, "~/robot_motion_commands/robot", @@ -165,7 +171,10 @@ class RadioBridgeNode : public rclcpp::Node std::array shutdown_requested_; std::array reboot_requested_; std::chrono::steady_clock::time_point last_side_change_timestamp_; + ateam_msgs::msg::JoystickControlStatus joy_status_; + ateam_common::GameControllerListener game_controller_listener_; + rclcpp::Subscription::SharedPtr joy_status_sub_; std::array::SharedPtr, 16> motion_command_subscriptions_; std::array::SharedPtr, @@ -208,6 +217,11 @@ class RadioBridgeNode : public rclcpp::Node vision_state_timestamps_[robot_id] = std::chrono::steady_clock::now(); } + void JoyStatusCallback(const ateam_msgs::msg::JoystickControlStatus::SharedPtr msg) + { + joy_status_ = *msg; + } + void CloseConnection(const std::size_t & connection_index, bool send_goodbye = true) { std::unique_ptr connection; @@ -285,25 +299,34 @@ class RadioBridgeNode : public rclcpp::Node motion_commands_[id] = ateam_msgs::msg::RobotMotionCommand(); motion_commands_[id].body_control_mode = ateam_msgs::msg::RobotMotionCommand::BCM_OFF; motion_commands_[id].kick_request = ateam_msgs::msg::RobotMotionCommand::KR_DISABLE; - motion_commands_[id].dribbler_setpoint = 0.0; + motion_commands_[id].dribbler_mode = ateam_msgs::msg::RobotMotionCommand::DC_DISABLE; } BasicControl control_msg{}; control_msg.request_shutdown = shutdown_requested_[id]; control_msg.reboot_robot = reboot_requested_[id]; - control_msg.game_state_in_stop = game_controller_listener_.GetGameCommand() == - ateam_common::GameCommand::Stop; + + if(joy_status_.is_active && joy_status_.active_id == id) { + control_msg.game_state_in_stop = false; + control_msg.game_state_in_halt = false; + } else { + control_msg.game_state_in_stop = game_controller_listener_.GetGameCommand() == + ateam_common::GameCommand::Stop; + control_msg.game_state_in_halt = game_controller_listener_.GetGameCommand() == + ateam_common::GameCommand::Halt; + } + control_msg.emergency_stop = false; 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.reset_controller = (last_side_change_timestamp_ + sustain_timeout_threshold_) >= now; control_msg.reserved1 = 0; - FillVisionUpdate(control_msg, vision_states_[id], vision_state_timestamps_[id]); + FillVisionUpdate(control_msg, id); control_msg.kick_request = static_cast(motion_commands_[id].kick_request); control_msg.play_song = 0; - control_msg.dribbler_mode = static_cast(motion_commands_[id].dribbler_mode); control_msg.kick_vel = motion_commands_[id].kick_speed; control_msg.dribbler_setpoint = motion_commands_[id].dribbler_setpoint; + control_msg.dribbler_mode = static_cast(motion_commands_[id].dribbler_mode); FillBodyControl(control_msg, motion_commands_[id]); const auto control_packet = CreatePacket(CC_CONTROL, control_msg); @@ -318,6 +341,9 @@ class RadioBridgeNode : public rclcpp::Node case ateam_msgs::msg::RobotMotionCommand::BCM_OFF: control_msg.body_control_mode = BCM_OFF; break; + case ateam_msgs::msg::RobotMotionCommand::BCM_ESTOP_BRAKE: + control_msg.body_control_mode = BCM_ESTOP_BRAKE; + break; case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: control_msg.body_control_mode = BCM_GLOBAL_POSITION; control_msg.cmd.global_pos = { @@ -369,9 +395,9 @@ class RadioBridgeNode : public rclcpp::Node case ateam_msgs::msg::RobotMotionCommand::BCM_HEADING_PIVOT: control_msg.body_control_mode = BCM_HEADING_PIVOT; control_msg.cmd.heading_pivot = { - static_cast(command.pose.theta), - static_cast(command.limit_vel_angular), - static_cast(command.limit_acc_angular), + static_cast(command.pivot_global_theta), + static_cast(command.pivot_max_angular_vel), + static_cast(command.pivot_max_angular_acc), static_cast(command.pivot_orbit_radius), static_cast(command.pivot_inset_angle), static_cast(command.pivot_direction), @@ -382,10 +408,10 @@ class RadioBridgeNode : public rclcpp::Node case ateam_msgs::msg::RobotMotionCommand::BCM_POINT_PIVOT: control_msg.body_control_mode = BCM_POINT_PIVOT; control_msg.cmd.point_pivot = { - static_cast(command.pose.x), - static_cast(command.pose.y), - static_cast(command.limit_vel_angular), - static_cast(command.limit_acc_angular), + static_cast(command.pivot_target_x), + static_cast(command.pivot_target_y), + static_cast(command.pivot_max_angular_vel), + static_cast(command.pivot_max_angular_acc), static_cast(command.pivot_orbit_radius), static_cast(command.pivot_inset_angle), static_cast(command.pivot_direction), @@ -393,6 +419,43 @@ class RadioBridgeNode : public rclcpp::Node {0, 0} }; break; + case ateam_msgs::msg::RobotMotionCommand::BCM_HEADING_LINE: + control_msg.body_control_mode = BCM_HEADING_LINE; + control_msg.cmd.heading_line = { + static_cast(command.line_start_x), + static_cast(command.line_start_y), + static_cast(command.line_dir_x), + static_cast(command.line_dir_y), + static_cast(command.line_velocity), + static_cast(command.line_global_theta), + static_cast(command.line_max_vel_colinear), + static_cast(command.line_max_vel_perp), + static_cast(command.line_max_vel_angular), + static_cast(command.line_max_accel_colinear), + static_cast(command.line_max_accel_perp), + static_cast(command.line_max_accel_angular), + static_cast(command.line_colinear_start_thresh) + }; + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_POINT_LINE: + control_msg.body_control_mode = BCM_POINT_LINE; + control_msg.cmd.point_line = { + static_cast(command.line_start_x), + static_cast(command.line_start_y), + static_cast(command.line_dir_x), + static_cast(command.line_dir_y), + static_cast(command.line_velocity), + static_cast(command.line_target_x), + static_cast(command.line_target_y), + static_cast(command.line_max_vel_colinear), + static_cast(command.line_max_vel_perp), + static_cast(command.line_max_vel_angular), + static_cast(command.line_max_accel_colinear), + static_cast(command.line_max_accel_perp), + static_cast(command.line_max_accel_angular), + static_cast(command.line_colinear_start_thresh) + }; + break; default: RCLCPP_WARN(get_logger(), "Unknown body control mode: %d", command.body_control_mode); control_msg.body_control_mode = BCM_OFF; @@ -400,7 +463,9 @@ class RadioBridgeNode : public rclcpp::Node } } - void FillVisionUpdate(BasicControl & control_msg, const ateam_msgs::msg::VisionStateRobot & vision_state, const std::chrono::steady_clock::time_point & timestamp) { + void FillVisionUpdate(BasicControl & control_msg, const int id) { + const auto & vision_state = vision_states_[id]; + const auto timestamp = vision_state_timestamps_[id]; const auto now = std::chrono::steady_clock::now(); if (now - timestamp > vision_state_staleness_threshold_ || !vision_state.visible) { control_msg.vision_update = 0; @@ -409,6 +474,14 @@ class RadioBridgeNode : public rclcpp::Node control_msg.vision_position_update[2] = 0; return; } + if(joy_status_.is_active && joy_status_.active_id == id) { + // Do not send vision updates to robots under joystick control + control_msg.vision_update = 0; + control_msg.vision_position_update[0] = 0; + control_msg.vision_position_update[1] = 0; + control_msg.vision_position_update[2] = 0; + return; + } control_msg.vision_update = 1; control_msg.vision_position_update[0] = static_cast(vision_state.pose.position.x); control_msg.vision_position_update[1] = static_cast(vision_state.pose.position.y); diff --git a/radio/ateam_radio_bridge/test/launch_tests/CMakeLists.txt b/radio/ateam_radio_bridge/test/launch_tests/CMakeLists.txt index e4028ff6..cbb72e20 100644 --- a/radio/ateam_radio_bridge/test/launch_tests/CMakeLists.txt +++ b/radio/ateam_radio_bridge/test/launch_tests/CMakeLists.txt @@ -1,22 +1,26 @@ -find_package(launch_testing_ament_cmake REQUIRED) -add_launch_test(bridge_discovery_test.py - APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR} - TIMEOUT 10 -) -add_launch_test(bridge_feedback_test.py - APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR} - TIMEOUT 10 -) -add_launch_test(bridge_command_test.py - APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR} - TIMEOUT 10 -) -add_launch_test(bridge_goodbye_test.py - APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR} - TIMEOUT 10 -) +# TODO(barulicm): These tests are too annoying to maintain when packets update +# Restore these when we switch to protobufs or can auto-generate Python definitions for the packet +# structs automatically. -install(PROGRAMS - mock_robot.py - DESTINATION lib/${PROJECT_NAME} -) +# find_package(launch_testing_ament_cmake REQUIRED) +# add_launch_test(bridge_discovery_test.py +# APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR} +# TIMEOUT 10 +# ) +# add_launch_test(bridge_feedback_test.py +# APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR} +# TIMEOUT 10 +# ) +# add_launch_test(bridge_command_test.py +# APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR} +# TIMEOUT 10 +# ) +# add_launch_test(bridge_goodbye_test.py +# APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR} +# TIMEOUT 10 +# ) + +# install(PROGRAMS +# mock_robot.py +# DESTINATION lib/${PROJECT_NAME} +# ) diff --git a/radio/ateam_radio_msgs/CMakeLists.txt b/radio/ateam_radio_msgs/CMakeLists.txt index 91bb1b66..81182ccf 100644 --- a/radio/ateam_radio_msgs/CMakeLists.txt +++ b/radio/ateam_radio_msgs/CMakeLists.txt @@ -25,6 +25,8 @@ set(RADIO_STRUCTS_TO_GENERATE LocalAccelerationCommand PointPivotCommand HeadingPivotCommand + PointLineCommand + HeadingLineCommand BasicTelemetry ErrorTelemetry ExtendedTelemetry diff --git a/radio/ateam_radio_msgs/scripts/generate_conversion_code.py b/radio/ateam_radio_msgs/scripts/generate_conversion_code.py index f1a10586..e9d7932d 100644 --- a/radio/ateam_radio_msgs/scripts/generate_conversion_code.py +++ b/radio/ateam_radio_msgs/scripts/generate_conversion_code.py @@ -202,16 +202,15 @@ def generate_union_switch_copy_lines(field_node, param_name, struct_names, selec and m.type.spelling in struct_names ] enum_details = next(e for e in enums if e['type_name'] == selector_field.type.spelling) - # Convention: enum value 0 means "none/off" (no active union member). - # Remaining values sorted ascending map positionally to union members in - # declaration order, so BCM_GLOBAL_POSITION(1)->global_pos, - # BCM_GLOBAL_VELOCITY(2)->global_vel, etc. - non_zero_cases = sorted( - [(name, val) for name, val in enum_details['values'] if val != 0], + enum_values = sorted( + [(name, val) for name, val in enum_details['values']], key=lambda x: x[1], ) + # TODO(barulicm): This is not sustainable long term + if field_name == 'control_telem' or 'maneuver': + del enum_values[:2] result = f' switch ({param_name}.{selector_field.spelling}) {{\n' - for (case_name, _), member in zip(non_zero_cases, members): + for (case_name, _), member in zip(enum_values, members): msg_field = f'{field_name}_{member.spelling}' result += f' case {case_name}:\n' result += (