Arm urdf changes#95
Merged
ConnorNeed merged 7 commits intomainfrom Apr 27, 2026
Merged
Conversation
Squash
There was a problem hiding this comment.
Pull request overview
This PR expands the rover’s manipulation and perception stack by adding higher-level MoveIt arm control (including camera-coordinate targeting driven by an end-effector distance sensor) and improving video streaming overlays/pipeline integration, alongside URDF/ros2_control parameter tuning for the arm.
Changes:
- Added a new
arm_control::MoveGroupClientlibrary and integrated it into joystick teleop for pose save/recall and camera-coordinate targeting. - Reworked parts of the video streaming pipeline, adding dot overlay support (DeepStream OSD) and introducing a new
ros2overlayGStreamer filter plugin. - Introduced a new
interfaces/Distancemessage + GPIO distance sensor node, and adjusted arm URDF/ros2_control and hardware-interface behaviors (activation + feedforward).
Reviewed changes
Copilot reviewed 34 out of 39 changed files in this pull request and generated 10 comments.
Show a summary per file
| File | Description |
|---|---|
| src/interfaces/msg/RtpStats.msg | Removes FEC-related fields from RTP stats message. |
| src/interfaces/msg/Distance.msg | Adds new Distance message for end-effector depth/ToF readings. |
| src/interfaces/CMakeLists.txt | Registers the new Distance message for interface generation. |
| src/URDF/rover_urdf/urdf/arm_urdf.xacro | Updates joint limits and collision geometry for the arm model. |
| src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro | Updates arm hardware params (CAN IDs, offsets, gravity FF params, rw_rate). |
| src/Teleop-Control/joystick_control/src/arm.cpp | Adds POS tracking mode, dot publishing, clipboard pose save/execute, and MoveGroupClient usage. |
| src/Teleop-Control/joystick_control/package.xml | Adds dependency on arm_control. |
| src/Teleop-Control/joystick_control/launch/controller.launch.py | Updates controller detection logic for drive joystick device selection. |
| src/Teleop-Control/joystick_control/include/arm.hpp | Adds MoveGroupClient integration and new state/parameters for POS tracking & clipboards. |
| src/Teleop-Control/joystick_control/config/pxn.yaml | Adds dot increment parameter for POS tracking UI. |
| src/Teleop-Control/joystick_control/CMakeLists.txt | Links the teleop arm node against the new MoveGroupClient library. |
| src/HW-Devices/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp | Adds Gravity feedforward manager based on TF pitch and integrates it into write(). |
| src/HW-Devices/ros_odrive/odrive_ros2_control/CMakeLists.txt | Adds tf2 dependencies required for gravity feedforward. |
| src/HW-Devices/hardware/src/rover_arm.cpp | Activates controller wrappers on hardware interface activation. |
| src/HW-Devices/hardware/src/TalonSRXWrapper.cpp | Adds non-finite command safety disable; moves gravity-FF timer start to activate(). |
| src/HW-Devices/hardware/include/TalonSRXWrapper.hpp | Declares activate() and adds a new friction_const_ member. |
| src/HW-Devices/hardware/include/BaseWrapper.hpp | Adds a (default no-op) activate() hook to wrappers. |
| src/HW-Devices/gpio_controller/setup.py | Registers new distance_sensor console script entry point. |
| src/HW-Devices/gpio_controller/gpio_controller/esp_distance_sensor.py | Adds a ROS2 node reading ToF distance over serial and publishing interfaces/Distance. |
| src/Cameras/video_streaming/src/rtp_node.cpp | Removes FEC support from RTP sender pipeline and parameters. |
| src/Cameras/video_streaming/src/rtp_client_node.cpp | Adds dot subscription and circle drawing via DeepStream OSD; changes pipeline to use nvstreammux/nvdsosd. |
| src/Cameras/video_streaming/include/video_streaming/rtp_client_node.hpp | Adds DeepStream metadata includes and dot overlay-related members. |
| src/Cameras/video_streaming/gst-plugins/ros2image/plugin.cpp | Registers new ros2overlay element in the gstros2 plugin. |
| src/Cameras/video_streaming/gst-plugins/ros2image/gstros2overlay.hpp | Declares the ros2overlay video filter element and ROS2 integration state. |
| src/Cameras/video_streaming/gst-plugins/ros2image/gstros2overlay.cpp | Implements ros2overlay drawing text/dot onto raw frames with a small bitmap font. |
| src/Cameras/video_streaming/gst-plugins/ros2image/CMakeLists.txt | Builds ros2overlay as part of the gstros2 plugin library. |
| src/Bringup/launch/control.launch.py | Launches the new end-effector distance sensor node when arm is enabled. |
| src/Arm/arm_control/src/MoveGroupClient.cpp | Implements MoveIt client wrapper incl. saved poses and camera-coordinate targeting using Distance + TF. |
| src/Arm/arm_control/package.xml | Adds dependencies required for MoveGroupClient (MoveIt planning interface, geometry msgs, interfaces). |
| src/Arm/arm_control/launch/servo.launch.py | Tunes MoveIt Servo frequency/filter coefficients. |
| src/Arm/arm_control/launch/end_effector.launch.py | Updates end-effector motor ID parameter. |
| src/Arm/arm_control/include/arm_control/MoveGroupClient.hpp | Declares MoveGroupClient public API and internal state. |
| src/Arm/arm_control/config/arm_config.yaml | Enables low-latency mode and halting behavior in cartesian mode. |
| src/Arm/arm_control/CMakeLists.txt | Builds/installs MoveGroupClient as a shared library and exports dependencies/targets. |
Comments suppressed due to low confidence (1)
src/Teleop-Control/joystick_control/launch/controller.launch.py:36
- find_ps5() no longer returns a device path when the controller is not found, so drive_dev becomes None and gets passed as the joy_linux_node "dev" parameter. This will likely fail at runtime. Consider restoring a fallback return value (e.g., /dev/input/js0) or raising/aborting launch explicitly when the controller is missing.
print("ERROR: PS5 controller not found.")
def generate_launch_description():
pkg_joystick_control = get_package_share_directory("joystick_control")
parameters_file = os.path.join(pkg_joystick_control, "pxn.yaml")
# Detect IDs dynamically
arm_dev = "/dev/input/by-id/usb-LiteStar_PXN-2113_Pro-joystick"
drive_dev = find_ps5()
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
Comment on lines
+207
to
+210
| debug_node_ = std::make_shared<rclcpp::Node>("odrive_system_debug_node"); | ||
| executor_ = rclcpp::executors::SingleThreadedExecutor::make_shared(); | ||
| executor_->add_node(debug_node_); | ||
| spin_thread_ = std::thread([this]() { this->executor_->spin(); }); |
Comment on lines
+36
to
+38
| rclpy.init() | ||
| node = TimeOfFlightSensor() | ||
| rclpy.spin(node) No newline at end of file |
Comment on lines
43
to
+55
| std::string RtpClientNode::get_pipeline_description() { | ||
| std::stringstream desc; | ||
| constexpr guint storage_tolerance_ms = 20; | ||
| guint64 storage_sz_ns = (latency_ms_ + storage_tolerance_ms) * 1000000ULL; | ||
| desc << "udpsrc port=" << dest_port_ << " caps=" | ||
| << "\"application/x-rtp, payload=96, clock-rate=90000\" ! " | ||
| << "rtpstorage size-time=" << storage_sz_ns | ||
| << " ! rtpssrcdemux ! capsfilter " | ||
| "caps=\"application/" | ||
| "x-rtp,media=video,encoding-name=H265,payload=96,clock-rate=90000\" " | ||
| "! rtpjitterbuffer name=rtp_buf mode=4 latency=" | ||
| << "\"application/x-rtp, " | ||
| "media=video,encoding-name=H265,payload=96,clock-rate=90000\" ! " | ||
| << "rtpssrcdemux ! rtpjitterbuffer name=rtp_buf mode=4 " | ||
| "drop-on-latency=true latency=" | ||
| << latency_ms_ | ||
| << " drop-on-latency=true do-lost=true post-drop-messages=true ! " | ||
| "rtpulpfecdec name=fec_dec pt=122 ! " | ||
| "rtph265depay ! h265parse ! nvv4l2decoder ! nvvidconv ! " | ||
| << "capsfilter caps=\"video/x-raw(memory:NVMM),width=1920,height=1080\"" | ||
| " ! nvvidconv ! nveglglessink sync=false"; | ||
| << " ! rtph265depay ! h265parse ! nvv4l2decoder ! nvvidconv ! " | ||
| << "capsfilter caps=\"video/x-raw(memory:NVMM),width=1920,height=1080\" " | ||
| << " ! queue ! mux.sink_0 " | ||
| << "nvstreammux batch-size=1 width=1920 height=1080 name=mux ! " | ||
| << "nvdsosd name=render ! nvvidconv ! nveglglessink sync=false"; |
| } | ||
|
|
||
| if (display_meta->num_circles >= MAX_ELEMENTS_IN_DISPLAY_META) { | ||
| RCLCPP_WARN(get_logger(), "Display meta circle limit reached."); |
Comment on lines
324
to
336
| @@ -224,6 +330,9 @@ CallbackReturn ODriveHardwareInterface::on_activate(const State &) { | |||
| debug_node_->template create_publisher<ros_phoenix::msg::MotorStatus>( | |||
| info_.joints[&axis - &axes_[0]].name + "/status", | |||
| rclcpp::SystemDefaultsQoS()); | |||
| if (axis.gravity_ff_manager_) { | |||
| axis.gravity_ff_manager_->start(); | |||
| } | |||
| } | |||
| class TimeOfFlightSensor(Node): | ||
| def __init__(self): | ||
| super().__init__("time_of_flight_sensor") | ||
| self.ser = serial.Serial('/dev/serial/by-id/usb-Espressif_USB_JTAG_serial_debug_unit_20:6E:F1:69:EE:E0-if00', 115200) |
| std::abs(joystickMsg->axes[kJoint6Axis]) < 0.01) { | ||
| initialized_ = true; | ||
| } else { | ||
| RCLCPP_WARN_THROTTLE(this->get_logger(), *(this->get_clock()), 1, |
Comment on lines
6
to
41
| @@ -30,28 +32,72 @@ RtpClientNode::RtpClientNode(const rclcpp::NodeOptions &options) | |||
| rtp_stats_pub_ = | |||
| this->create_publisher<interfaces::msg::RtpStats>("~/rtp_stats", 10); | |||
| RCLCPP_INFO(get_logger(), "RtpClientNode constructed and pipeline started."); | |||
|
|
|||
| dot_sub_ = this->create_subscription<geometry_msgs::msg::Vector3>( | |||
| "~/dot", 2, [this](const geometry_msgs::msg::Vector3::SharedPtr msg) { | |||
| circle_x_ = static_cast<int>(msg->x); | |||
| circle_y_ = static_cast<int>(msg->y); | |||
| }); | |||
| } | |||
Comment on lines
+256
to
+260
| clipboard1_pose_index_ = moveit_client_->saveCurrentPose(); | ||
| RCLCPP_INFO(this->get_logger(), | ||
| "Saved current pose to clipboard 1 at index %d", | ||
| clipboard1_pose_index_); | ||
| } |
|
|
||
| void declareParameters(); | ||
| void loadParameters(); | ||
| geometry_msgs::msg::PoseStamped getPoseOfPtr(); |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
This pull request introduces significant new functionality and configuration changes to the arm control and video streaming subsystems. The main updates include the addition of a new
MoveGroupClientclass for arm control, enhancements to video streaming overlays and pipeline, and several configuration and parameter tuning changes for the arm and end effector. Below are the most important changes grouped by subsystem:Arm Control Subsystem:
MoveGroupClientclass (with correspondingMoveGroupClient.hppandMoveGroupClient.cpp) that provides high-level methods for arm motion planning, pose saving, and camera coordinate targeting, integrating with MoveIt and ROS2 topics. [1] [2]CMakeLists.txtandpackage.xmlto include new dependencies (rclcpp,geometry_msgs,moveit_ros_planning_interface,interfaces) and to build and install the newmove_group_clientlibrary. [1] [2] [3]low_latency_mode, sethalt_all_joints_in_cartesian_modetotrue, and adjusted end effector ID and servo parameters for improved responsiveness and safety. [1] [2] [3] [4]Video Streaming Subsystem:
ros2overlaythat can overlay text and dots on video streams based on ROS2 topics, with all necessary headers and plugin registration. [1] [2] [3] [4]RtpClientNodeto subscribe to a dot position topic and draw circles on video frames using DeepStream metadata, and updated the video pipeline to use DeepStream elements for better performance and overlay support. [1] [2] [3] [4]Bringup/Launch Integration:
These changes collectively improve the flexibility, observability, and performance of the arm and camera systems, and lay the groundwork for more advanced autonomous manipulation and perception features.