diff --git a/src/Arm/arm_control/CMakeLists.txt b/src/Arm/arm_control/CMakeLists.txt index d8cb834b..0284c140 100644 --- a/src/Arm/arm_control/CMakeLists.txt +++ b/src/Arm/arm_control/CMakeLists.txt @@ -2,8 +2,58 @@ cmake_minimum_required(VERSION 3.22) project(arm_control) find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(interfaces REQUIRED) -ament_package() +add_library(move_group_client SHARED + src/MoveGroupClient.cpp +) + +target_include_directories(move_group_client PUBLIC + $ + $ +) + +ament_target_dependencies(move_group_client + rclcpp + geometry_msgs + interfaces + moveit_ros_planning_interface + "tf2" + "tf2_ros" + "tf2_geometry_msgs" + "geometry_msgs" +) + +install( + TARGETS move_group_client + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + rclcpp + geometry_msgs + moveit_ros_planning_interface + tf2 + tf2_ros + tf2_geometry_msgs + interfaces +) if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch") install( @@ -14,3 +64,5 @@ endif() install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) + +ament_package() \ No newline at end of file diff --git a/src/Arm/arm_control/config/arm_config.yaml b/src/Arm/arm_control/config/arm_config.yaml index 7f6fe134..31b9f540 100644 --- a/src/Arm/arm_control/config/arm_config.yaml +++ b/src/Arm/arm_control/config/arm_config.yaml @@ -9,7 +9,7 @@ use_gazebo: false ## Properties of outgoing commands publish_period: 0.08 # 1/Nominal publish rate [seconds] -low_latency_mode: false +low_latency_mode: true command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: @@ -50,7 +50,7 @@ incoming_command_timeout: 0.2 # Stop servoing if X seconds elapse without a ne # Important because ROS may drop some messages and we need the robot to halt reliably. num_outgoing_halt_msgs_to_publish: 0 halt_all_joints_in_joint_mode: false -halt_all_joints_in_cartesian_mode: false +halt_all_joints_in_cartesian_mode: true ## Configure handling of singularities and joint limits lower_singularity_threshold: 10.0 # Start decelerating when the condition number hits this (close to singularity) diff --git a/src/Arm/arm_control/include/arm_control/MoveGroupClient.hpp b/src/Arm/arm_control/include/arm_control/MoveGroupClient.hpp new file mode 100644 index 00000000..c78d14dd --- /dev/null +++ b/src/Arm/arm_control/include/arm_control/MoveGroupClient.hpp @@ -0,0 +1,60 @@ +#pragma once + +#include +#include +#include + +#include "interfaces/msg/distance.hpp" +#include "std_msgs/msg/float32.hpp" +#include +#include +#include +#include +#include +#include + +namespace arm_control { + +class MoveGroupClient { +public: + explicit MoveGroupClient(rclcpp::Node::SharedPtr node); + ~MoveGroupClient(); + + void stop(); + bool goToSavedPose(size_t index); + bool goToPose(const geometry_msgs::msg::Pose &pose); + bool goToPose(const geometry_msgs::msg::PoseStamped &pose); + bool goToCamCoord(double u, double v); + size_t saveCurrentPose(); + +private: + std::string planning_group_; + + std::unique_ptr move_group_; + rclcpp::Subscription::SharedPtr depth_sub_; + rclcpp::Node::SharedPtr node_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_{nullptr}; + + double planning_time_; + int planning_attempts_; + double vel_scaling_; + double acc_scaling_; + interfaces::msg::Distance::SharedPtr latest_distance_; + + std::vector saved_poses_; + struct cam_intrinsics { + double fx; + double fy; + int cx; + int cy; + std::string camera_link; + }; + cam_intrinsics camera_info_; + + void declareParams(); + void loadParams(); + void configureMoveGroup(); + bool planAndExecute(); +}; +} // namespace arm_control \ No newline at end of file diff --git a/src/Arm/arm_control/launch/end_effector.launch.py b/src/Arm/arm_control/launch/end_effector.launch.py index ba23a875..9fa60f11 100644 --- a/src/Arm/arm_control/launch/end_effector.launch.py +++ b/src/Arm/arm_control/launch/end_effector.launch.py @@ -17,7 +17,7 @@ def generate_launch_description(): plugin="ros_phoenix::TalonSRX", name="end_effector", parameters=[ - {"id": 21}, + {"id": 15}, {"max_voltage": 24.0}, {"max_current": 6.0}, {"brake_mode": True}, diff --git a/src/Arm/arm_control/launch/servo.launch.py b/src/Arm/arm_control/launch/servo.launch.py index 479f5b36..c3f244f3 100644 --- a/src/Arm/arm_control/launch/servo.launch.py +++ b/src/Arm/arm_control/launch/servo.launch.py @@ -79,8 +79,8 @@ def arm_launch(moveit_config, launch_package_path=None) -> LaunchDescription: servo_parameters = [ { "moveit_servo": servo_yaml, - "publish_frequency": 15.0, - "butterworth_filter_coeff": 10.0, + "publish_frequency": 100.0, + "butterworth_filter_coeff": 2.0, }, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, diff --git a/src/Arm/arm_control/package.xml b/src/Arm/arm_control/package.xml index ba04c105..aa0823d0 100644 --- a/src/Arm/arm_control/package.xml +++ b/src/Arm/arm_control/package.xml @@ -18,6 +18,11 @@ ament_cmake + rclcpp + geometry_msgs + moveit_ros_planning_interface + interfaces + moveit_ros_move_group moveit_kinematics moveit_planners diff --git a/src/Arm/arm_control/src/MoveGroupClient.cpp b/src/Arm/arm_control/src/MoveGroupClient.cpp new file mode 100644 index 00000000..bbab97ff --- /dev/null +++ b/src/Arm/arm_control/src/MoveGroupClient.cpp @@ -0,0 +1,184 @@ +#include "arm_control/MoveGroupClient.hpp" +#include +#include + +namespace arm_control { +MoveGroupClient::MoveGroupClient(rclcpp::Node::SharedPtr node) + : planning_time_(5.0), planning_attempts_(10), vel_scaling_(0.2), + acc_scaling_(0.2), node_(node) { + declareParams(); + loadParams(); + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + move_group_ = + std::make_unique( + node_, planning_group_); + + depth_sub_ = node->create_subscription( + "/eef_distance", 10, + [this](const interfaces::msg::Distance::SharedPtr msg) { + latest_distance_ = msg; + }); + + configureMoveGroup(); + + RCLCPP_INFO(node_->get_logger(), + "MoveGroupClient ready for planning group %s", + planning_group_.c_str()); +} + +MoveGroupClient::~MoveGroupClient() = default; + +void MoveGroupClient::declareParams() { + node_->declare_parameter("planning_time", 5.0); + node_->declare_parameter("planning_group", "arm"); + node_->declare_parameter("planning_attempts", 10); + node_->declare_parameter("vel_scaling", 0.2); + node_->declare_parameter("acc_scaling", 0.2); + node_->declare_parameter("cx", 960); + node_->declare_parameter("cy", 540); + node_->declare_parameter("fx", 540); + node_->declare_parameter("fy", 540); + node_->declare_parameter("camera_link", "EndEffector"); +} + +void MoveGroupClient::loadParams() { + planning_time_ = node_->get_parameter("planning_time").as_double(); + planning_attempts_ = node_->get_parameter("planning_attempts").as_int(); + planning_group_ = node_->get_parameter("planning_group").as_string(); + vel_scaling_ = node_->get_parameter("vel_scaling").as_double(); + acc_scaling_ = node_->get_parameter("acc_scaling").as_double(); + camera_info_.fx = node_->get_parameter("fx").as_double(); + camera_info_.fy = node_->get_parameter("fy").as_double(); + camera_info_.cx = node_->get_parameter("cx").as_int(); + camera_info_.cy = node_->get_parameter("cy").as_int(); + camera_info_.camera_link = node_->get_parameter("camera_link").as_string(); +} + +void MoveGroupClient::configureMoveGroup() { + move_group_->setPlanningTime(planning_time_); + move_group_->setNumPlanningAttempts(planning_attempts_); + move_group_->setMaxVelocityScalingFactor(vel_scaling_); + move_group_->setMaxAccelerationScalingFactor(acc_scaling_); + + RCLCPP_INFO(node_->get_logger(), "Planning frame: %s", + move_group_->getPlanningFrame().c_str()); + RCLCPP_INFO(node_->get_logger(), "EEF link: %s", + move_group_->getEndEffectorLink().c_str()); +} + +void MoveGroupClient::stop() { + RCLCPP_INFO(node_->get_logger(), "MoveGroupClient stopping"); + move_group_->stop(); +} + +bool MoveGroupClient::planAndExecute() { + moveit::planning_interface::MoveGroupInterface::Plan plan; + auto result = move_group_->plan(plan); + + if (result != moveit::core::MoveItErrorCode::SUCCESS) { + RCLCPP_WARN(node_->get_logger(), "Planning failed"); + return false; + } + + result = move_group_->asyncExecute(plan); + if (result != moveit::core::MoveItErrorCode::SUCCESS) { + RCLCPP_WARN(node_->get_logger(), "Async execution start failed"); + return false; + } + + return true; +} + +bool MoveGroupClient::goToPose(const geometry_msgs::msg::Pose &pose) { + RCLCPP_INFO(node_->get_logger(), + "MoveGroupClient moving to pose x: %f y: %f z: %f", + pose.position.x, pose.position.y, pose.position.z); + move_group_->clearPoseTargets(); + move_group_->setStartStateToCurrentState(); + move_group_->setPoseTarget(pose); + return planAndExecute(); +} + +bool MoveGroupClient::goToPose(const geometry_msgs::msg::PoseStamped &pose) { + RCLCPP_INFO(node_->get_logger(), + "MoveGroupClient moving to pose x: %f y: %f z: %f in frame: %s", + pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, + pose.header.frame_id.c_str()); + move_group_->clearPoseTargets(); + move_group_->setStartStateToCurrentState(); + move_group_->setPoseTarget(pose); + return planAndExecute(); +} + +size_t MoveGroupClient::saveCurrentPose() { + geometry_msgs::msg::Pose current_pose = move_group_->getCurrentPose().pose; + saved_poses_.push_back(current_pose); + return saved_poses_.size() - 1; +} + +bool MoveGroupClient::goToSavedPose(size_t index) { + if (index >= saved_poses_.size()) { + RCLCPP_ERROR(node_->get_logger(), + "Saved pose index %zu out of range (size=%zu)", index, + saved_poses_.size()); + return false; + } + + return goToPose(saved_poses_[index]); +} + +bool MoveGroupClient::goToCamCoord(double u, double v) { + auto now = node_->get_clock()->now(); + constexpr double offset_to_fingers = 0.18; + + if (!latest_distance_ || + latest_distance_->status != interfaces::msg::Distance::STATUS_OK || + (now - latest_distance_->header.stamp).seconds() > 1.1) { + RCLCPP_ERROR(node_->get_logger(), "Waiting on depth"); + return false; + } + + double Z = latest_distance_->distance; + double X = (u - camera_info_.cx) * Z / camera_info_.fx; + double Y = (v - camera_info_.cy) * Z / camera_info_.fy; + + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = camera_info_.camera_link; + + pose.pose.position.x = Y; + pose.pose.position.y = -X; + pose.pose.position.z = 0; + // If target is at center of camera, move staight forward by half the distance + // to fingers + 1cm + if (std::abs(u - camera_info_.cx) < 1e-6 && + std::abs(v - camera_info_.cy) < 1e-6) { + pose.pose.position.z = (Z - offset_to_fingers) / 2.0 + 0.01; + } + + pose.pose.orientation.x = 0; + pose.pose.orientation.y = 0; + pose.pose.orientation.z = 0; + pose.pose.orientation.w = 1; + + geometry_msgs::msg::PoseStamped transformed; + RCLCPP_INFO(node_->get_logger(), + "Transforming pose from camera frame %s to base_link frame", + camera_info_.camera_link.c_str()); + try { + tf_buffer_->transform(pose, transformed, "base_link"); + } catch (const tf2::TransformException &ex) { + RCLCPP_WARN(node_->get_logger(), "Transform failed: %s", ex.what()); + return false; + } + RCLCPP_INFO(node_->get_logger(), + "Moving to cam coord u: %f v: %f -> local x: %f local y: %f " + "local z: %f -> x: %f, y: %f, z: %f", + u, v, pose.pose.position.x, pose.pose.position.y, + pose.pose.position.z, transformed.pose.position.x, + transformed.pose.position.y, transformed.pose.position.z); + + return goToPose(transformed); +} +} // namespace arm_control diff --git a/src/Bringup/launch/control.launch.py b/src/Bringup/launch/control.launch.py index 6f61f2f8..86d39dc8 100644 --- a/src/Bringup/launch/control.launch.py +++ b/src/Bringup/launch/control.launch.py @@ -98,6 +98,12 @@ def generate_launch_description(): output="screen", condition=IfCondition(use_arm), ) + eef_distance = Node( + package="gpio_controller", + executable="distance_sensor", + output="screen", + condition=IfCondition(use_arm), + ) arm_controller_servo_spawner = Node( package="controller_manager", @@ -152,4 +158,5 @@ def generate_launch_description(): ld.add_action(delayed_spawners) ld.add_action(arm_launch) ld.add_action(eef_launch) + ld.add_action(eef_distance) return ld diff --git a/src/Cameras/video_streaming/gst-plugins/ros2image/CMakeLists.txt b/src/Cameras/video_streaming/gst-plugins/ros2image/CMakeLists.txt index d2bb8a35..8eabd322 100644 --- a/src/Cameras/video_streaming/gst-plugins/ros2image/CMakeLists.txt +++ b/src/Cameras/video_streaming/gst-plugins/ros2image/CMakeLists.txt @@ -23,6 +23,7 @@ link_directories(${GST_LIBRARY_DIRS}) add_library(gstros2 SHARED plugin.cpp gstros2src.cpp + gstros2overlay.cpp ) target_compile_definitions(gstros2 PRIVATE PACKAGE="\\\"gstros2\\\"" diff --git a/src/Cameras/video_streaming/gst-plugins/ros2image/gstros2overlay.cpp b/src/Cameras/video_streaming/gst-plugins/ros2image/gstros2overlay.cpp new file mode 100644 index 00000000..5c81e4a0 --- /dev/null +++ b/src/Cameras/video_streaming/gst-plugins/ros2image/gstros2overlay.cpp @@ -0,0 +1,648 @@ +#include "gstros2overlay.hpp" + +#include + +#include +#include +#include +#include + +GST_DEBUG_CATEGORY_STATIC(gst_ros2_overlay_debug); +#define GST_CAT_DEFAULT gst_ros2_overlay_debug + +static std::atomic g_ros2_overlay_instance_count{0}; +static std::mutex g_ros2_overlay_init_mutex; + +enum { + PROP_0, + PROP_NODE_NAME, + PROP_TEXT_TOPIC, + PROP_DOT_TOPIC, + PROP_QOS_PROFILE, + PROP_OWNS_ROS, +}; + +#define DEFAULT_NODE_NAME "gst_ros2_overlay" +#define DEFAULT_TEXT_TOPIC "/overlay/text" +#define DEFAULT_DOT_TOPIC "/overlay/dot" +#define DEFAULT_QOS_PROFILE "default" +#define DEFAULT_OWNS_ROS TRUE + +G_DEFINE_TYPE(GstRos2Overlay, gst_ros2_overlay, GST_TYPE_VIDEO_FILTER); + +static void gst_ros2_overlay_set_property(GObject *object, guint prop_id, + const GValue *value, + GParamSpec *pspec); +static void gst_ros2_overlay_get_property(GObject *object, guint prop_id, + GValue *value, GParamSpec *pspec); +static void gst_ros2_overlay_finalize(GObject *object); + +static gboolean gst_ros2_overlay_start(GstBaseTransform *trans); +static gboolean gst_ros2_overlay_stop(GstBaseTransform *trans); +static gboolean gst_ros2_overlay_set_info(GstVideoFilter *filter, + GstCaps *incaps, GstVideoInfo *ininfo, + GstCaps *outcaps, + GstVideoInfo *outinfo); +static GstFlowReturn gst_ros2_overlay_transform_frame_ip(GstVideoFilter *filter, + GstVideoFrame *frame); + +static void ros_spin_thread_fn(GstRos2Overlay *self); + +static rclcpp::QoS parse_qos_profile(const gchar *qos_profile) { + if (!qos_profile) { + return rclcpp::QoS(rclcpp::KeepLast(10)); + } + + std::string q{qos_profile}; + if (q == "sensor_data") { + return rclcpp::SensorDataQoS(); + } + return rclcpp::QoS(rclcpp::KeepLast(10)); +} + +static void text_callback(GstRos2Overlay *self, + const std_msgs::msg::String::SharedPtr msg) { + std::lock_guard lock(self->priv->data_mutex); + self->priv->current_text = msg->data; + self->priv->have_text = true; +} + +static void dot_callback(GstRos2Overlay *self, + const geometry_msgs::msg::Vector3::SharedPtr msg) { + std::lock_guard lock(self->priv->data_mutex); + if (msg->x < 0 || msg->y < 0) { + self->priv->have_dot = false; + return; + } + self->priv->dot_x = msg->x; + self->priv->dot_y = msg->y; + self->priv->have_dot = true; +} + +static void ros_spin_thread_fn(GstRos2Overlay *self) { + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(self->priv->node); + + while (self->priv->running.load()) { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + + exec.remove_node(self->priv->node); +} + +static void put_pixel_bgr(guint8 *data, int stride, int width, int height, + int x, int y, guint8 b, guint8 g, guint8 r) { + if (x < 0 || y < 0 || x >= width || y >= height) { + return; + } + + guint8 *p = data + y * stride + x * 3; + p[0] = b; + p[1] = g; + p[2] = r; +} + +static void put_pixel_rgb(guint8 *data, int stride, int width, int height, + int x, int y, guint8 r, guint8 g, guint8 b) { + if (x < 0 || y < 0 || x >= width || y >= height) { + return; + } + + guint8 *p = data + y * stride + x * 3; + p[0] = r; + p[1] = g; + p[2] = b; +} + +static void put_pixel_rgba(guint8 *data, int stride, int width, int height, + int x, int y, guint8 r, guint8 g, guint8 b) { + if (x < 0 || y < 0 || x >= width || y >= height) { + return; + } + + guint8 *p = data + y * stride + x * 4; + p[0] = r; + p[1] = g; + p[2] = b; + // preserve existing alpha +} + +static void put_pixel_gray(guint8 *data, int stride, int width, int height, + int x, int y, guint8 v) { + if (x < 0 || y < 0 || x >= width || y >= height) { + return; + } + + guint8 *p = data + y * stride + x; + p[0] = v; +} + +static void draw_dot_bgr(guint8 *data, int stride, int width, int height, + int cx, int cy, int radius) { + for (int y = cy - radius; y <= cy + radius; ++y) { + for (int x = cx - radius; x <= cx + radius; ++x) { + int dx = x - cx; + int dy = y - cy; + if (dx * dx + dy * dy <= radius * radius) { + put_pixel_bgr(data, stride, width, height, x, y, 0, 0, 255); + } + } + } +} + +static void draw_dot_rgb(guint8 *data, int stride, int width, int height, + int cx, int cy, int radius) { + for (int y = cy - radius; y <= cy + radius; ++y) { + for (int x = cx - radius; x <= cx + radius; ++x) { + int dx = x - cx; + int dy = y - cy; + if (dx * dx + dy * dy <= radius * radius) { + put_pixel_rgb(data, stride, width, height, x, y, 255, 0, 0); + } + } + } +} +static void draw_dot_rgba(guint8 *data, int stride, int width, int height, + int cx, int cy, int radius) { + for (int y = cy - radius; y <= cy + radius; ++y) { + for (int x = cx - radius; x <= cx + radius; ++x) { + int dx = x - cx; + int dy = y - cy; + if (dx * dx + dy * dy <= radius * radius) { + put_pixel_rgba(data, stride, width, height, x, y, 255, 0, 0); + } + } + } +} + +static void draw_dot_gray(guint8 *data, int stride, int width, int height, + int cx, int cy, int radius) { + for (int y = cy - radius; y <= cy + radius; ++y) { + for (int x = cx - radius; x <= cx + radius; ++x) { + int dx = x - cx; + int dy = y - cy; + if (dx * dx + dy * dy <= radius * radius) { + put_pixel_gray(data, stride, width, height, x, y, 255); + } + } + } +} + +/* + Tiny 5x7 bitmap font for a practical subset of ASCII. + Unsupported chars render as space. +*/ +static const uint8_t FONT_5X7[][5] = { + {0, 0, 0, 0, 0}, // space 32 + {0x00, 0x00, 0x5F, 0x00, 0x00}, // ! + {0x00, 0x07, 0x00, 0x07, 0x00}, // " + {0x14, 0x7F, 0x14, 0x7F, 0x14}, // # + {0x24, 0x2A, 0x7F, 0x2A, 0x12}, // $ + {0x23, 0x13, 0x08, 0x64, 0x62}, // % + {0x36, 0x49, 0x55, 0x22, 0x50}, // & + {0x00, 0x05, 0x03, 0x00, 0x00}, // ' + {0x00, 0x1C, 0x22, 0x41, 0x00}, // ( + {0x00, 0x41, 0x22, 0x1C, 0x00}, // ) + {0x14, 0x08, 0x3E, 0x08, 0x14}, // * + {0x08, 0x08, 0x3E, 0x08, 0x08}, // + + {0x00, 0x50, 0x30, 0x00, 0x00}, // , + {0x08, 0x08, 0x08, 0x08, 0x08}, // - + {0x00, 0x60, 0x60, 0x00, 0x00}, // . + {0x20, 0x10, 0x08, 0x04, 0x02}, // / + {0x3E, 0x51, 0x49, 0x45, 0x3E}, // 0 + {0x00, 0x42, 0x7F, 0x40, 0x00}, // 1 + {0x42, 0x61, 0x51, 0x49, 0x46}, // 2 + {0x21, 0x41, 0x45, 0x4B, 0x31}, // 3 + {0x18, 0x14, 0x12, 0x7F, 0x10}, // 4 + {0x27, 0x45, 0x45, 0x45, 0x39}, // 5 + {0x3C, 0x4A, 0x49, 0x49, 0x30}, // 6 + {0x01, 0x71, 0x09, 0x05, 0x03}, // 7 + {0x36, 0x49, 0x49, 0x49, 0x36}, // 8 + {0x06, 0x49, 0x49, 0x29, 0x1E}, // 9 + {0x00, 0x36, 0x36, 0x00, 0x00}, // : + {0x00, 0x56, 0x36, 0x00, 0x00}, // ; + {0x08, 0x14, 0x22, 0x41, 0x00}, // < + {0x14, 0x14, 0x14, 0x14, 0x14}, // = + {0x00, 0x41, 0x22, 0x14, 0x08}, // > + {0x02, 0x01, 0x51, 0x09, 0x06}, // ? + {0x32, 0x49, 0x79, 0x41, 0x3E}, // @ + {0x7E, 0x11, 0x11, 0x11, 0x7E}, // A + {0x7F, 0x49, 0x49, 0x49, 0x36}, // B + {0x3E, 0x41, 0x41, 0x41, 0x22}, // C + {0x7F, 0x41, 0x41, 0x22, 0x1C}, // D + {0x7F, 0x49, 0x49, 0x49, 0x41}, // E + {0x7F, 0x09, 0x09, 0x09, 0x01}, // F + {0x3E, 0x41, 0x49, 0x49, 0x7A}, // G + {0x7F, 0x08, 0x08, 0x08, 0x7F}, // H + {0x00, 0x41, 0x7F, 0x41, 0x00}, // I + {0x20, 0x40, 0x41, 0x3F, 0x01}, // J + {0x7F, 0x08, 0x14, 0x22, 0x41}, // K + {0x7F, 0x40, 0x40, 0x40, 0x40}, // L + {0x7F, 0x02, 0x0C, 0x02, 0x7F}, // M + {0x7F, 0x04, 0x08, 0x10, 0x7F}, // N + {0x3E, 0x41, 0x41, 0x41, 0x3E}, // O + {0x7F, 0x09, 0x09, 0x09, 0x06}, // P + {0x3E, 0x41, 0x51, 0x21, 0x5E}, // Q + {0x7F, 0x09, 0x19, 0x29, 0x46}, // R + {0x46, 0x49, 0x49, 0x49, 0x31}, // S + {0x01, 0x01, 0x7F, 0x01, 0x01}, // T + {0x3F, 0x40, 0x40, 0x40, 0x3F}, // U + {0x1F, 0x20, 0x40, 0x20, 0x1F}, // V + {0x3F, 0x40, 0x38, 0x40, 0x3F}, // W + {0x63, 0x14, 0x08, 0x14, 0x63}, // X + {0x03, 0x04, 0x78, 0x04, 0x03}, // Y + {0x61, 0x51, 0x49, 0x45, 0x43}, // Z +}; + +static const uint8_t *font5x7_for_char(char c) { + if (c < 32 || c > 90) { + return FONT_5X7[0]; + } + return FONT_5X7[c - 32]; +} + +static void draw_char_bgr(guint8 *data, int stride, int width, int height, + int x, int y, char c) { + const uint8_t *glyph = font5x7_for_char(c); + for (int col = 0; col < 5; ++col) { + for (int row = 0; row < 7; ++row) { + if (glyph[col] & (1 << row)) { + put_pixel_bgr(data, stride, width, height, x + col, y + row, 0, 255, 0); + } + } + } +} + +static void draw_char_rgb(guint8 *data, int stride, int width, int height, + int x, int y, char c) { + const uint8_t *glyph = font5x7_for_char(c); + for (int col = 0; col < 5; ++col) { + for (int row = 0; row < 7; ++row) { + if (glyph[col] & (1 << row)) { + put_pixel_rgb(data, stride, width, height, x + col, y + row, 0, 255, 0); + } + } + } +} + +static void draw_char_rgba(guint8 *data, int stride, int width, int height, + int x, int y, char c) { + const uint8_t *glyph = font5x7_for_char(c); + for (int col = 0; col < 5; ++col) { + for (int row = 0; row < 7; ++row) { + if (glyph[col] & (1 << row)) { + put_pixel_rgba(data, stride, width, height, x + col, y + row, 0, 255, + 0); + } + } + } +} + +static void draw_char_gray(guint8 *data, int stride, int width, int height, + int x, int y, char c) { + const uint8_t *glyph = font5x7_for_char(c); + for (int col = 0; col < 5; ++col) { + for (int row = 0; row < 7; ++row) { + if (glyph[col] & (1 << row)) { + put_pixel_gray(data, stride, width, height, x + col, y + row, 255); + } + } + } +} + +static void draw_text_bgr(guint8 *data, int stride, int width, int height, + int x, int y, const std::string &text) { + int cursor_x = x; + for (char c : text) { + draw_char_bgr(data, stride, width, height, cursor_x, y, c); + cursor_x += 6; + } +} + +static void draw_text_rgb(guint8 *data, int stride, int width, int height, + int x, int y, const std::string &text) { + int cursor_x = x; + for (char c : text) { + draw_char_rgb(data, stride, width, height, cursor_x, y, c); + cursor_x += 6; + } +} + +static void draw_text_rgba(guint8 *data, int stride, int width, int height, + int x, int y, const std::string &text) { + int cursor_x = x; + for (char c : text) { + draw_char_rgba(data, stride, width, height, cursor_x, y, c); + cursor_x += 6; + } +} + +static void draw_text_gray(guint8 *data, int stride, int width, int height, + int x, int y, const std::string &text) { + int cursor_x = x; + for (char c : text) { + draw_char_gray(data, stride, width, height, cursor_x, y, c); + cursor_x += 6; + } +} + +static void gst_ros2_overlay_class_init(GstRos2OverlayClass *klass) { + GObjectClass *gobject_class = G_OBJECT_CLASS(klass); + GstElementClass *gstelement_class = GST_ELEMENT_CLASS(klass); + GstBaseTransformClass *base_transform_class = GST_BASE_TRANSFORM_CLASS(klass); + GstVideoFilterClass *video_filter_class = GST_VIDEO_FILTER_CLASS(klass); + + gobject_class->set_property = gst_ros2_overlay_set_property; + gobject_class->get_property = gst_ros2_overlay_get_property; + gobject_class->finalize = gst_ros2_overlay_finalize; + + g_object_class_install_property( + gobject_class, PROP_NODE_NAME, + g_param_spec_string( + "node-name", "ROS 2 node name", + "ROS 2 node name used by this element", DEFAULT_NODE_NAME, + (GParamFlags)(G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS | + G_PARAM_CONSTRUCT_ONLY))); + + g_object_class_install_property( + gobject_class, PROP_TEXT_TOPIC, + g_param_spec_string("text-topic", "Text topic", + "ROS 2 std_msgs/msg/String topic", DEFAULT_TEXT_TOPIC, + (GParamFlags)(G_PARAM_READWRITE | + G_PARAM_STATIC_STRINGS | + G_PARAM_CONSTRUCT_ONLY))); + + g_object_class_install_property( + gobject_class, PROP_DOT_TOPIC, + g_param_spec_string( + "dot-topic", "Dot topic", "ROS 2 geometry_msgs/msg/Vector3 topic", + DEFAULT_DOT_TOPIC, + (GParamFlags)(G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS | + G_PARAM_CONSTRUCT_ONLY))); + + g_object_class_install_property( + gobject_class, PROP_QOS_PROFILE, + g_param_spec_string( + "qos-profile", "QoS profile", + "QoS profile: 'default' or 'sensor_data'", DEFAULT_QOS_PROFILE, + (GParamFlags)(G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS | + G_PARAM_CONSTRUCT_ONLY))); + g_object_class_install_property( + gobject_class, PROP_OWNS_ROS, + g_param_spec_boolean( + "owns-ros", "Owns ROS", + "If true, element will call rclcpp::init/shutdown. " + "Set false when running inside an existing ROS2 process.", + DEFAULT_OWNS_ROS, + (GParamFlags)(G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS | + G_PARAM_CONSTRUCT_ONLY))); + + gst_element_class_set_static_metadata( + gstelement_class, "ROS 2 Overlay", "Filter/Effect/Video", + "Draws ROS 2 text and a ROS 2-controlled dot onto video frames", + "Connor Needham (CPRT) "); + + GstCaps *caps = gst_caps_from_string( + "video/x-raw, format=(string){ RGBA, RGB, BGR, GRAY8 }, " + "width=(int)[1,MAX], height=(int)[1,MAX]"); + + gst_element_class_add_pad_template( + gstelement_class, gst_pad_template_new("src", GST_PAD_SRC, GST_PAD_ALWAYS, + gst_caps_ref(caps))); + gst_element_class_add_pad_template( + gstelement_class, + gst_pad_template_new("sink", GST_PAD_SINK, GST_PAD_ALWAYS, caps)); + gst_caps_unref(caps); + + base_transform_class->start = GST_DEBUG_FUNCPTR(gst_ros2_overlay_start); + base_transform_class->stop = GST_DEBUG_FUNCPTR(gst_ros2_overlay_stop); + + video_filter_class->set_info = GST_DEBUG_FUNCPTR(gst_ros2_overlay_set_info); + video_filter_class->transform_frame_ip = + GST_DEBUG_FUNCPTR(gst_ros2_overlay_transform_frame_ip); + + GST_DEBUG_CATEGORY_INIT(gst_ros2_overlay_debug, "ros2overlay", 0, + "ROS2 Overlay"); +} + +static void gst_ros2_overlay_init(GstRos2Overlay *self) { + self->node_name = g_strdup(DEFAULT_NODE_NAME); + self->text_topic = g_strdup(DEFAULT_TEXT_TOPIC); + self->dot_topic = g_strdup(DEFAULT_DOT_TOPIC); + self->qos_profile = g_strdup(DEFAULT_QOS_PROFILE); + self->owns_ros = DEFAULT_OWNS_ROS; + + self->priv = std::make_unique(); + gst_video_info_init(&self->info); + + gst_base_transform_set_in_place(GST_BASE_TRANSFORM(self), TRUE); +} + +static void gst_ros2_overlay_finalize(GObject *object) { + GstRos2Overlay *self = GST_ROS2_OVERLAY(object); + + g_free(self->node_name); + g_free(self->text_topic); + g_free(self->dot_topic); + g_free(self->qos_profile); + + G_OBJECT_CLASS(gst_ros2_overlay_parent_class)->finalize(object); +} + +static void gst_ros2_overlay_set_property(GObject *object, guint prop_id, + const GValue *value, + GParamSpec *pspec) { + GstRos2Overlay *self = GST_ROS2_OVERLAY(object); + + switch (prop_id) { + case PROP_NODE_NAME: + g_free(self->node_name); + self->node_name = g_value_dup_string(value); + break; + case PROP_TEXT_TOPIC: + g_free(self->text_topic); + self->text_topic = g_value_dup_string(value); + break; + case PROP_DOT_TOPIC: + g_free(self->dot_topic); + self->dot_topic = g_value_dup_string(value); + break; + case PROP_QOS_PROFILE: + g_free(self->qos_profile); + self->qos_profile = g_value_dup_string(value); + break; + case PROP_OWNS_ROS: + self->owns_ros = g_value_get_boolean(value); + break; + default: + G_OBJECT_WARN_INVALID_PROPERTY_ID(object, prop_id, pspec); + break; + } +} + +static void gst_ros2_overlay_get_property(GObject *object, guint prop_id, + GValue *value, GParamSpec *pspec) { + GstRos2Overlay *self = GST_ROS2_OVERLAY(object); + + switch (prop_id) { + case PROP_NODE_NAME: + g_value_set_string(value, self->node_name); + break; + case PROP_TEXT_TOPIC: + g_value_set_string(value, self->text_topic); + break; + case PROP_DOT_TOPIC: + g_value_set_string(value, self->dot_topic); + break; + case PROP_QOS_PROFILE: + g_value_set_string(value, self->qos_profile); + break; + case PROP_OWNS_ROS: + g_value_set_boolean(value, self->owns_ros); + break; + default: + G_OBJECT_WARN_INVALID_PROPERTY_ID(object, prop_id, pspec); + break; + } +} + +static gboolean gst_ros2_overlay_start(GstBaseTransform *trans) { + GstRos2Overlay *self = GST_ROS2_OVERLAY(trans); + + if (self->owns_ros) { + std::lock_guard lock(g_ros2_overlay_init_mutex); + if (g_ros2_overlay_instance_count.fetch_add(1) == 0) { + int argc = 0; + char **argv = nullptr; + rclcpp::init(argc, argv); + } + } else { + // Do NOT init ROS, assume already initialized + if (!rclcpp::ok()) { + GST_ERROR_OBJECT(self, "ROS not initialized but owns-ros=false"); + return FALSE; + } + } + + rclcpp::NodeOptions opts; + self->priv->node = std::make_shared( + self->node_name ? self->node_name : DEFAULT_NODE_NAME, opts); + + auto qos = parse_qos_profile(self->qos_profile); + + self->priv->text_sub = + self->priv->node->create_subscription( + self->text_topic ? self->text_topic : DEFAULT_TEXT_TOPIC, qos, + [self](const std_msgs::msg::String::SharedPtr msg) { + text_callback(self, msg); + }); + + self->priv->dot_sub = + self->priv->node->create_subscription( + self->dot_topic ? self->dot_topic : DEFAULT_DOT_TOPIC, qos, + [self](const geometry_msgs::msg::Vector3::SharedPtr msg) { + dot_callback(self, msg); + }); + + self->priv->running.store(true); + self->priv->ros_spin_thread = std::thread(ros_spin_thread_fn, self); + + return TRUE; +} + +static gboolean gst_ros2_overlay_stop(GstBaseTransform *trans) { + GstRos2Overlay *self = GST_ROS2_OVERLAY(trans); + + self->priv->running.store(false); + + if (self->priv->ros_spin_thread.joinable()) { + self->priv->ros_spin_thread.join(); + } + + self->priv->text_sub.reset(); + self->priv->dot_sub.reset(); + self->priv->node.reset(); + + if (self->owns_ros) { + std::lock_guard lock(g_ros2_overlay_init_mutex); + if (g_ros2_overlay_instance_count.fetch_sub(1) == 1) { + rclcpp::shutdown(); + } + } + + return TRUE; +} + +static gboolean gst_ros2_overlay_set_info(GstVideoFilter *filter, + GstCaps *incaps, GstVideoInfo *ininfo, + GstCaps *outcaps, + GstVideoInfo *outinfo) { + GstRos2Overlay *self = GST_ROS2_OVERLAY(filter); + self->info = *ininfo; + return TRUE; +} + +static GstFlowReturn gst_ros2_overlay_transform_frame_ip(GstVideoFilter *filter, + GstVideoFrame *frame) { + GstRos2Overlay *self = GST_ROS2_OVERLAY(filter); + + std::string text; + bool have_text = false; + double dot_x = 0.0; + double dot_y = 0.0; + bool have_dot = false; + + { + std::lock_guard lock(self->priv->data_mutex); + text = self->priv->current_text; + have_text = self->priv->have_text; + dot_x = self->priv->dot_x; + dot_y = self->priv->dot_y; + have_dot = self->priv->have_dot; + } + + auto *data = static_cast(GST_VIDEO_FRAME_PLANE_DATA(frame, 0)); + int stride = GST_VIDEO_FRAME_PLANE_STRIDE(frame, 0); + int width = GST_VIDEO_FRAME_WIDTH(frame); + int height = GST_VIDEO_FRAME_HEIGHT(frame); + GstVideoFormat fmt = GST_VIDEO_FRAME_FORMAT(frame); + + if (fmt == GST_VIDEO_FORMAT_BGR) { + if (have_text) { + draw_text_bgr(data, stride, width, height, 20, 20, text); + } + if (have_dot) { + draw_dot_bgr(data, stride, width, height, static_cast(dot_x), + static_cast(dot_y), 6); + } + } else if (fmt == GST_VIDEO_FORMAT_RGB) { + if (have_text) { + draw_text_rgb(data, stride, width, height, 20, 20, text); + } + if (have_dot) { + draw_dot_rgb(data, stride, width, height, static_cast(dot_x), + static_cast(dot_y), 6); + } + } else if (fmt == GST_VIDEO_FORMAT_GRAY8) { + if (have_text) { + draw_text_gray(data, stride, width, height, 20, 20, text); + } + if (have_dot) { + draw_dot_gray(data, stride, width, height, static_cast(dot_x), + static_cast(dot_y), 6); + } + } else if (fmt == GST_VIDEO_FORMAT_RGBA) { + if (have_text) { + draw_text_rgba(data, stride, width, height, 20, 20, text); + } + if (have_dot) { + draw_dot_rgba(data, stride, width, height, static_cast(dot_x), + static_cast(dot_y), 6); + } + } else { + GST_WARNING_OBJECT(self, "Unsupported format for drawing"); + } + + return GST_FLOW_OK; +} \ No newline at end of file diff --git a/src/Cameras/video_streaming/gst-plugins/ros2image/gstros2overlay.hpp b/src/Cameras/video_streaming/gst-plugins/ros2image/gstros2overlay.hpp new file mode 100644 index 00000000..4b57e56d --- /dev/null +++ b/src/Cameras/video_streaming/gst-plugins/ros2image/gstros2overlay.hpp @@ -0,0 +1,69 @@ +#ifndef __GST_ROS2_OVERLAY_HPP__ +#define __GST_ROS2_OVERLAY_HPP__ + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +G_BEGIN_DECLS + +#define GST_TYPE_ROS2_OVERLAY (gst_ros2_overlay_get_type()) +#define GST_ROS2_OVERLAY(obj) \ + (G_TYPE_CHECK_INSTANCE_CAST((obj), GST_TYPE_ROS2_OVERLAY, GstRos2Overlay)) +#define GST_ROS2_OVERLAY_CLASS(klass) \ + (G_TYPE_CHECK_CLASS_CAST((klass), GST_TYPE_ROS2_OVERLAY, GstRos2OverlayClass)) +#define GST_IS_ROS2_OVERLAY(obj) \ + (G_TYPE_CHECK_INSTANCE_TYPE((obj), GST_TYPE_ROS2_OVERLAY)) +#define GST_IS_ROS2_OVERLAY_CLASS(klass) \ + (G_TYPE_CHECK_CLASS_TYPE((klass), GST_TYPE_ROS2_OVERLAY)) + +typedef struct _GstRos2Overlay GstRos2Overlay; +typedef struct _GstRos2OverlayClass GstRos2OverlayClass; + +struct cpp_ros2_overlay { + std::shared_ptr node; + rclcpp::Subscription::SharedPtr text_sub; + rclcpp::Subscription::SharedPtr dot_sub; + std::thread ros_spin_thread; + std::atomic running{false}; + + std::mutex data_mutex; + std::string current_text; + bool have_text{false}; + double dot_x{0.0}; + double dot_y{0.0}; + bool have_dot{false}; +}; + +struct _GstRos2Overlay { + GstVideoFilter parent; + + gchar *node_name; + gchar *text_topic; + gchar *dot_topic; + gchar *qos_profile; + gboolean owns_ros; + + GstVideoInfo info; + std::unique_ptr priv; +}; + +struct _GstRos2OverlayClass { + GstVideoFilterClass parent_class; +}; + +GType gst_ros2_overlay_get_type(void); + +G_END_DECLS + +#endif \ No newline at end of file diff --git a/src/Cameras/video_streaming/gst-plugins/ros2image/plugin.cpp b/src/Cameras/video_streaming/gst-plugins/ros2image/plugin.cpp index 4bce2c05..44475bb2 100644 --- a/src/Cameras/video_streaming/gst-plugins/ros2image/plugin.cpp +++ b/src/Cameras/video_streaming/gst-plugins/ros2image/plugin.cpp @@ -1,3 +1,4 @@ +#include "gstros2overlay.hpp" #include "gstros2src.hpp" #include @@ -6,6 +7,10 @@ static gboolean plugin_init(GstPlugin *plugin) { GST_TYPE_ROS2_IMAGE_SRC)) { return FALSE; } + if (!gst_element_register(plugin, "ros2overlay", GST_RANK_NONE, + GST_TYPE_ROS2_OVERLAY)) { + return FALSE; + } return TRUE; } diff --git a/src/Cameras/video_streaming/include/video_streaming/rtp_client_node.hpp b/src/Cameras/video_streaming/include/video_streaming/rtp_client_node.hpp index aaa7eb2b..4bc913d2 100644 --- a/src/Cameras/video_streaming/include/video_streaming/rtp_client_node.hpp +++ b/src/Cameras/video_streaming/include/video_streaming/rtp_client_node.hpp @@ -2,8 +2,11 @@ #include "base_video_node.hpp" +#include #include +#include #include +#include #include #include #include @@ -27,6 +30,10 @@ class RtpClientNode : public BaseVideoNode { int latency_ms_; rclcpp::TimerBase::SharedPtr rtp_stats_timer_; rclcpp::Publisher::SharedPtr rtp_stats_pub_; + void draw_circle(NvDsBatchMeta *batch_meta, NvDsFrameMeta *frame_meta); + int circle_x_; + int circle_y_; + rclcpp::Subscription::SharedPtr dot_sub_; }; } // namespace video_streaming \ No newline at end of file diff --git a/src/Cameras/video_streaming/src/rtp_client_node.cpp b/src/Cameras/video_streaming/src/rtp_client_node.cpp index dd147508..277c672a 100644 --- a/src/Cameras/video_streaming/src/rtp_client_node.cpp +++ b/src/Cameras/video_streaming/src/rtp_client_node.cpp @@ -1,4 +1,6 @@ #include "rtp_client_node.hpp" +#include +#include #include namespace video_streaming { RtpClientNode::RtpClientNode(const rclcpp::NodeOptions &options) @@ -30,28 +32,72 @@ RtpClientNode::RtpClientNode(const rclcpp::NodeOptions &options) rtp_stats_pub_ = this->create_publisher("~/rtp_stats", 10); RCLCPP_INFO(get_logger(), "RtpClientNode constructed and pipeline started."); + + dot_sub_ = this->create_subscription( + "~/dot", 2, [this](const geometry_msgs::msg::Vector3::SharedPtr msg) { + circle_x_ = static_cast(msg->x); + circle_y_ = static_cast(msg->y); + }); } 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"; + return desc.str(); } +void RtpClientNode::draw_circle(NvDsBatchMeta *batch_meta, + NvDsFrameMeta *frame_meta) { + if (circle_x_ < 0 || circle_y_ < 0) { + return; + } + NvDsDisplayMeta *display_meta = + nvds_acquire_display_meta_from_pool(batch_meta); + + if (!display_meta) { + RCLCPP_ERROR(get_logger(), "Failed to acquire display meta."); + return; + } + + if (display_meta->num_circles >= MAX_ELEMENTS_IN_DISPLAY_META) { + RCLCPP_WARN(get_logger(), "Display meta circle limit reached."); + return; + } + + NvOSD_CircleParams &circle_params = + display_meta->circle_params[display_meta->num_circles]; + + circle_params.xc = circle_x_; + circle_params.yc = circle_y_; + circle_params.radius = 10; + circle_params.circle_color.red = 1.0; + circle_params.circle_color.green = 0.0; + circle_params.circle_color.blue = 1.0; + circle_params.circle_color.alpha = 1.0; + + circle_params.bg_color.red = 0.0; + circle_params.bg_color.green = 1.0; + circle_params.bg_color.blue = 0.0; + circle_params.bg_color.alpha = 0.3; + circle_params.has_bg_color = 1; + + display_meta->num_circles += 1; + nvds_add_display_meta_to_frame(frame_meta, display_meta); + RCLCPP_DEBUG(this->get_logger(), "Drew circle at (%d, %d)", circle_x_, + circle_y_); +} + bool RtpClientNode::create_pipeline() { std::string desc = get_pipeline_description(); RCLCPP_INFO(this->get_logger(), "Pipeline description: %s", desc.c_str()); @@ -74,6 +120,52 @@ bool RtpClientNode::create_pipeline() { } pipeline_ = p; + GstElement *render = get_element("render"); + if (!render) { + RCLCPP_ERROR(get_logger(), "Failed to get nvosd element."); + return false; + } + + GstPad *sink_pad = gst_element_get_static_pad(render, "sink"); + gst_object_unref(render); + + if (!sink_pad) { + RCLCPP_ERROR(get_logger(), "Failed to get nvosd sink pad."); + return false; + } + + gst_pad_add_probe( + sink_pad, GST_PAD_PROBE_TYPE_BUFFER, + [](GstPad *, GstPadProbeInfo *info, + gpointer user_data) -> GstPadProbeReturn { + auto *self = static_cast(user_data); + + GstBuffer *buf = GST_PAD_PROBE_INFO_BUFFER(info); + if (!buf) { + RCLCPP_ERROR(self->get_logger(), + "Failed to get buffer from probe info."); + return GST_PAD_PROBE_OK; + } + + NvDsBatchMeta *batch_meta = gst_buffer_get_nvds_batch_meta(buf); + if (!batch_meta) { + RCLCPP_ERROR(self->get_logger(), + "Failed to get batch meta from buffer."); + return GST_PAD_PROBE_OK; + } + + for (NvDsMetaList *l_frame = batch_meta->frame_meta_list; + l_frame != nullptr; l_frame = l_frame->next) { + auto *frame_meta = static_cast(l_frame->data); + self->draw_circle(batch_meta, frame_meta); + } + + return GST_PAD_PROBE_OK; + }, + this, nullptr); + + gst_object_unref(sink_pad); + RCLCPP_INFO(this->get_logger(), "Pipeline parsed successfully."); return true; } @@ -105,18 +197,6 @@ void RtpClientNode::rtp_stats_cb() { gst_structure_get_uint64(stats_struct, "avg-jitter", &avg_jitter); gst_structure_free(stats_struct); - // Get data from fec decoder - GstElement *fec_dec = get_element("fec_dec"); - if (!fec_dec) { - RCLCPP_WARN(this->get_logger(), "Could not get fec_dec"); - return; - } - guint recovered = 0; - guint unrecovered = 0; - g_object_get(fec_dec, "recovered", &recovered, "unrecovered", &unrecovered, - NULL); - g_object_unref(fec_dec); - auto msg = interfaces::msg::RtpStats(); msg.header.stamp = this->now(); msg.num_pushed = num_pushed; @@ -124,8 +204,6 @@ void RtpClientNode::rtp_stats_cb() { msg.num_late = num_late; msg.num_duplicates = num_duplicates; msg.avg_jitter = avg_jitter; - msg.recovered = recovered; - msg.unrecovered = unrecovered; rtp_stats_pub_->publish(msg); } diff --git a/src/Cameras/video_streaming/src/rtp_node.cpp b/src/Cameras/video_streaming/src/rtp_node.cpp index d4d3ac0f..7bc7855c 100644 --- a/src/Cameras/video_streaming/src/rtp_node.cpp +++ b/src/Cameras/video_streaming/src/rtp_node.cpp @@ -15,7 +15,6 @@ RtpNode::RtpNode(const rclcpp::NodeOptions &options) this->declare_parameter("dest_port", 7001); this->declare_parameter("target_framerate", 30); this->declare_parameter("bitrate", 2000000); - this->declare_parameter("fec_percent", 20); this->declare_parameter("default_width", 1280); this->declare_parameter("default_height", 720); @@ -27,7 +26,6 @@ RtpNode::RtpNode(const rclcpp::NodeOptions &options) throw std::runtime_error("Invalid initial target_framerate"); } bitrate_ = this->get_parameter("bitrate").as_int(); - fec_ = this->get_parameter("fec_percent").as_int(); dest_ip_ = this->get_parameter("dest_ip").as_string(); dest_port_ = this->get_parameter("dest_port").as_int(); width_ = this->get_parameter("default_width").as_int(); @@ -58,19 +56,16 @@ RtpNode::RtpNode(const rclcpp::NodeOptions &options) RtpNode::~RtpNode() { BaseVideoNode::safe_gst_unref(h265_encoder_); } std::string RtpNode::get_pipeline_description() { - int video_bitrate = bitrate_ / (1 + fec_ / 100.0); std::stringstream desc; desc << "interpipesrc format=3 listen-to=detect is-live=true ! " << "nvvidconv ! videorate ! " << "capsfilter caps=\"video/x-raw,framerate=" << target_framerate_ << "/1\" ! nvvidconv ! " << "capsfilter caps=\"video/x-raw(memory:NVMM),height=" << height_ - << ",width=" << width_ << "\" ! nvv4l2h265enc bitrate=" << video_bitrate - << " name=h265_enc bit-packetization=true EnableTwopassCBR=true " - "slice-header-spacing=900 control-rate=1 " + << ",width=" << width_ << "\" ! nvv4l2h265enc bitrate=" << bitrate_ + << " name=h265_enc EnableTwopassCBR=true control-rate=1 " "maxperf-enable=true ! queue ! rtph265pay pt=96 config-interval=-1 ! " - "rtpulpfecenc pt=122 percentage=" - << fec_ << " ! udpsink host=" << dest_ip_ << " port=" << dest_port_; + << "udpsink host=" << dest_ip_ << " port=" << dest_port_; return desc.str(); } @@ -119,15 +114,6 @@ RtpNode::on_parameter_change(const std::vector ¶meters) { for (const auto ¶m : parameters) { const std::string &name = param.get_name(); - - // Forward error correction - if (name == "fec_percent") { - fec_ = param.as_int(); - RCLCPP_INFO(this->get_logger(), - "Param Update: Forward Error Correction set to %d%%", fec_); - needs_restart = true; - continue; - } // Framerate if (name == "target_framerate") { target_framerate_ = param.as_int(); @@ -179,17 +165,16 @@ RtpNode::on_parameter_change(const std::vector ¶meters) { void RtpNode::set_bitrate(const int32_t bitrate) { bitrate_ = bitrate; - int video_bitrate = bitrate / (1 + fec_ / 100.0); if (h265_encoder_) { pause_pipeline(); - g_object_set(h265_encoder_, "bitrate", video_bitrate, NULL); + g_object_set(h265_encoder_, "bitrate", bitrate_, NULL); resume_pipeline(); - RCLCPP_DEBUG(this->get_logger(), "Bitrate set to %d", video_bitrate); + RCLCPP_DEBUG(this->get_logger(), "Bitrate set to %d", bitrate_); } // Adjust resolution based on bitrate thresholds // TODO: These thresholds are arbitrary and may need tuning based on actual // performance and quality requirements - int bits_per_frame = video_bitrate / target_framerate_; + int bits_per_frame = bitrate_ / target_framerate_; if (bits_per_frame < 50000) { set_resolution(640, 360); } else if (bits_per_frame < 100000) { diff --git a/src/HW-Devices/gpio_controller/gpio_controller/esp_distance_sensor.py b/src/HW-Devices/gpio_controller/gpio_controller/esp_distance_sensor.py new file mode 100644 index 00000000..85a26399 --- /dev/null +++ b/src/HW-Devices/gpio_controller/gpio_controller/esp_distance_sensor.py @@ -0,0 +1,38 @@ +import rclpy +from rclpy.node import Node +from interfaces.msg import Distance +import serial + +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) + self.pub = self.create_publisher(Distance, "eef_distance", 10) + self.create_timer(0.001, self.loop) + self.get_logger().info("Time of Flight Sensor node started") + + def loop(self): + if self.ser.in_waiting: + line = self.ser.readline().decode().strip() + try: + reading = int(line) + msg = Distance() + msg.header.stamp = self.get_clock().now().to_msg() + if (reading == -2): + mm = 0 + msg.status = Distance.STATUS_ERROR + elif (reading == -1): + mm = 0 + msg.status = Distance.STATUS_INVALID + else: + msg.status = Distance.STATUS_OK + mm = reading + msg.distance = mm / 1000.0 + self.get_logger().debug(f"Distance: {msg.distance:.3f} m") + self.pub.publish(msg) + except ValueError: + pass + +rclpy.init() +node = TimeOfFlightSensor() +rclpy.spin(node) \ No newline at end of file diff --git a/src/HW-Devices/gpio_controller/setup.py b/src/HW-Devices/gpio_controller/setup.py index 8f0b8fa9..d030fe5f 100644 --- a/src/HW-Devices/gpio_controller/setup.py +++ b/src/HW-Devices/gpio_controller/setup.py @@ -24,6 +24,9 @@ license="Apache-2.0", tests_require=["pytest"], entry_points={ - "console_scripts": ["lights = gpio_controller.lights:main"], + "console_scripts": [ + "lights = gpio_controller.lights:main", + "distance_sensor = gpio_controller.esp_distance_sensor:main", + ], }, ) diff --git a/src/HW-Devices/hardware/include/BaseWrapper.hpp b/src/HW-Devices/hardware/include/BaseWrapper.hpp index 0674a9da..77949f77 100644 --- a/src/HW-Devices/hardware/include/BaseWrapper.hpp +++ b/src/HW-Devices/hardware/include/BaseWrapper.hpp @@ -74,6 +74,12 @@ class BaseWrapper { * implementation, then apply additional configuration as required. */ virtual void configure(); + /** + * @brief Activate motor controllers. + * + * Optional for derived classes. + */ + virtual void activate(){}; protected: /** diff --git a/src/HW-Devices/hardware/include/TalonSRXWrapper.hpp b/src/HW-Devices/hardware/include/TalonSRXWrapper.hpp index 51016ca9..ebff13d3 100644 --- a/src/HW-Devices/hardware/include/TalonSRXWrapper.hpp +++ b/src/HW-Devices/hardware/include/TalonSRXWrapper.hpp @@ -39,6 +39,8 @@ class TalonSRXWrapper : public BaseWrapper { void configure() override; + void activate() override; + // Optional: type-level setup for Talons if you need it static void setup(); @@ -63,6 +65,7 @@ class TalonSRXWrapper : public BaseWrapper { bool inverted_; bool invert_sensor_; double gravity_const_; + double friction_const_; std::string target_frame_; std::string cur_frame_; bool initialized_; diff --git a/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp b/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp index 533594d4..bf46fb68 100644 --- a/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp +++ b/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp @@ -170,6 +170,15 @@ void TalonSRXWrapper::write() { } return; } + if (!std::isfinite(command_)) { + talon_controller_->Set(motors::ControlMode::Disabled, 0.0); + RCLCPP_WARN_THROTTLE( + debug_node_->get_logger(), *debug_node_->get_clock(), 1000, + "%s: Command is not finite for id %d, disabling motor to prevent " + "potential damage. Msg throttled to 1 Hz.", + __FUNCTION__, id_); + return; + } if (std::abs(command_ - position_) > M_PI && control_type_ == motors::ControlMode::Position) { RCLCPP_WARN_THROTTLE( @@ -242,25 +251,6 @@ int TalonSRXWrapper::get_load_enc() const { void TalonSRXWrapper::configure() { BaseWrapper::configure(); - // Start gravity ff timer - if (gravity_ff_freq_ > 0 && !target_frame_.empty() && !cur_frame_.empty()) { - gravity_ff_timer_ = debug_node_->create_wall_timer( - std::chrono::milliseconds(1000 / gravity_ff_freq_), - std::bind(&TalonSRXWrapper::update_gravity_ff, this)); - RCLCPP_INFO( - debug_node_->get_logger(), - "%s: Gravity FF enabled with frequency %d Hz, target frame '%s', " - "current frame '%s', and gravity constant %.2f", - __FUNCTION__, gravity_ff_freq_, target_frame_.c_str(), - cur_frame_.c_str(), gravity_const_); - } else { - RCLCPP_INFO( - debug_node_->get_logger(), - "%s: Gravity FF disabled (frequency %d Hz, target frame '%s', current " - "frame '%s')", - __FUNCTION__, gravity_ff_freq_, target_frame_.c_str(), - cur_frame_.c_str()); - } while (true) { if (talon_controller_->GetFirmwareVersion() == -1) { @@ -283,6 +273,7 @@ void TalonSRXWrapper::configure() { config.continuousCurrentLimit = 10.0; config.peakCurrentLimit = 10.0; config.peakCurrentDuration = 100; + config.neutralDeadband = 0.001; talon_controller_->EnableCurrentLimit(true); ErrorCode error = talon_controller_->ConfigAllSettings(config, 50); @@ -316,7 +307,6 @@ void TalonSRXWrapper::configure() { talon_controller_->SelectProfileSlot(0, 0); talon_controller_->SetStatusFramePeriod( StatusFrameEnhanced::Status_2_Feedback0, 20); - talon_controller_->SetIntegralAccumulator(0); if (load_sensor_ != SensorType::NONE) { talon_controller_->SetSelectedSensorPosition(get_load_enc()); } @@ -330,4 +320,27 @@ void TalonSRXWrapper::configure() { "%s: Successfully configured Motor Controller %d", __FUNCTION__, id_); start_time_ = debug_node_->now(); +} + +void TalonSRXWrapper::activate() { + // Start gravity ff timer + if (gravity_ff_freq_ > 0 && !target_frame_.empty() && !cur_frame_.empty()) { + gravity_ff_timer_ = debug_node_->create_wall_timer( + std::chrono::milliseconds(1000 / gravity_ff_freq_), + std::bind(&TalonSRXWrapper::update_gravity_ff, this)); + RCLCPP_INFO( + debug_node_->get_logger(), + "%s: Gravity FF enabled with frequency %d Hz, target frame '%s', " + "current frame '%s', and gravity constant %.2f", + __FUNCTION__, gravity_ff_freq_, target_frame_.c_str(), + cur_frame_.c_str(), gravity_const_); + } else { + RCLCPP_INFO( + debug_node_->get_logger(), + "%s: Gravity FF disabled (frequency %d Hz, target frame '%s', current " + "frame '%s')", + __FUNCTION__, gravity_ff_freq_, target_frame_.c_str(), + cur_frame_.c_str()); + } + talon_controller_->SetIntegralAccumulator(0); } \ No newline at end of file diff --git a/src/HW-Devices/hardware/src/rover_arm.cpp b/src/HW-Devices/hardware/src/rover_arm.cpp index e1410aec..ba9d1ff6 100644 --- a/src/HW-Devices/hardware/src/rover_arm.cpp +++ b/src/HW-Devices/hardware/src/rover_arm.cpp @@ -79,6 +79,9 @@ hardware_interface::CallbackReturn RoverArmHardwareInterface::on_activate( executor_ = rclcpp::executors::SingleThreadedExecutor::make_shared(); executor_->add_node(debug_node_); spin_thread_ = std::thread([this]() { this->executor_->spin(); }); + for (auto &controller : controllers_) { + controller->activate(); + } RCLCPP_INFO(rclcpp::get_logger("RoverArmHardwareInterface"), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; diff --git a/src/HW-Devices/ros_odrive/odrive_ros2_control/CMakeLists.txt b/src/HW-Devices/ros_odrive/odrive_ros2_control/CMakeLists.txt index 6fb31d62..27fe2b66 100644 --- a/src/HW-Devices/ros_odrive/odrive_ros2_control/CMakeLists.txt +++ b/src/HW-Devices/ros_odrive/odrive_ros2_control/CMakeLists.txt @@ -10,6 +10,9 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(ament_cmake_auto REQUIRED) find_package(ros_phoenix REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) ament_auto_find_build_dependencies() ament_export_dependencies(rosidl_default_runtime) @@ -25,6 +28,9 @@ ament_auto_add_library( ) ament_target_dependencies(odrive_ros2_control_plugin + tf2 + tf2_ros + tf2_geometry_msgs ros_phoenix rclcpp ) diff --git a/src/HW-Devices/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp b/src/HW-Devices/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp index 323659ef..aa97de6b 100644 --- a/src/HW-Devices/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp +++ b/src/HW-Devices/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp @@ -8,10 +8,15 @@ #include "rclcpp/rclcpp.hpp" #include "ros_phoenix/msg/motor_status.hpp" #include "socket_can.hpp" +#include +#include +#include +#include namespace odrive_ros2_control { class Axis; +class GravityFFManager; class ODriveHardwareInterface final : public hardware_interface::SystemInterface { @@ -57,9 +62,11 @@ class ODriveHardwareInterface final struct Axis { Axis(SocketCanIntf *can_intf, uint32_t node_id, double multiplier, - ODriveInputMode input_mode) + ODriveInputMode input_mode, + std::shared_ptr gravity_ff_manager = nullptr) : can_intf_(can_intf), node_id_(node_id), multiplier_(multiplier), - input_mode_(input_mode) {} + input_mode_(input_mode), + gravity_ff_manager_(std::move(gravity_ff_manager)) {} void on_can_msg(const rclcpp::Time ×tamp, const can_frame &frame); @@ -105,6 +112,7 @@ struct Axis { ODriveInputMode input_mode_; rclcpp::Publisher::SharedPtr debug_pub_; + std::shared_ptr gravity_ff_manager_; template bool send(const T &msg) const { struct can_frame frame; @@ -120,6 +128,69 @@ struct Axis { } }; +class GravityFFManager { +public: + GravityFFManager(rclcpp::Node::SharedPtr node, double gravity_ff_freq, + std::string link_frame, std::string gravity_frame, + double gravity_const) + : node_(node), gravity_ff_freq_(gravity_ff_freq), + gravity_frame_(gravity_frame), link_frame_(link_frame), + gravity_const_(gravity_const) { + + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + } + double get_ff_value() { return gravity_ff_.load(std::memory_order_relaxed); } + void start() { + if (gravity_ff_freq_ <= 0 || gravity_frame_.empty() || + link_frame_.empty()) { + return; + } + gravity_ff_timer_ = node_->create_wall_timer( + std::chrono::milliseconds(static_cast(1000.0 / gravity_ff_freq_)), + std::bind(&GravityFFManager::update_gravity_ff, this)); + } + +private: + void update_gravity_ff() { + if (!tf_buffer_) { + RCLCPP_ERROR(node_->get_logger(), + "%s: TF buffer not initialized, cannot update gravity FF", + __FUNCTION__); + return; + } + try { + auto tf = tf_buffer_->lookupTransform( + gravity_frame_.c_str(), link_frame_.c_str(), tf2::TimePointZero); + + tf2::Quaternion q; + tf2::fromMsg(tf.transform.rotation, q); + + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + double ff = gravity_const_ * std::sin(pitch); + gravity_ff_.store(ff, std::memory_order_relaxed); + RCLCPP_DEBUG( + node_->get_logger(), + "%s: Updated gravity FF to %.2f based on pitch %.2f degrees ", + __FUNCTION__, ff, pitch * 180.0 / M_PI); + } catch (const tf2::TransformException &err) { + RCLCPP_ERROR_THROTTLE(node_->get_logger(), *node_->get_clock(), 500, + "%s: Caught exception %s", __FUNCTION__, + err.what()); + } + } + rclcpp::Node::SharedPtr node_; + double gravity_ff_freq_; + std::string gravity_frame_; + std::string link_frame_; + double gravity_const_; + std::atomic gravity_ff_{0.0}; + rclcpp::TimerBase::SharedPtr gravity_ff_timer_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_{nullptr}; +}; + } // namespace odrive_ros2_control using namespace odrive_ros2_control; @@ -133,6 +204,10 @@ ODriveHardwareInterface::on_init(const hardware_interface::HardwareInfo &info) { CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } + debug_node_ = std::make_shared("odrive_system_debug_node"); + executor_ = rclcpp::executors::SingleThreadedExecutor::make_shared(); + executor_->add_node(debug_node_); + spin_thread_ = std::thread([this]() { this->executor_->spin(); }); can_intf_name_ = info_.hardware_parameters["can"]; debug_frequency_ = 0; @@ -174,8 +249,43 @@ ODriveHardwareInterface::on_init(const hardware_interface::HardwareInfo &info) { joint.name.c_str()); } } + double gravity_ff_freq = 0.0; + if (joint.parameters.find("gravity_ff_freq") != joint.parameters.end()) { + gravity_ff_freq = std::stod(joint.parameters.at("gravity_ff_freq")); + RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), + "Setting Joint %s gravity_ff_freq to %f'", joint.name.c_str(), + gravity_ff_freq); + } + double gravity_const = 0.0; + if (joint.parameters.find("gravity_const") != joint.parameters.end()) { + gravity_const = std::stod(joint.parameters.at("gravity_const")); + RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), + "Setting Joint %s gravity_const to %f'", joint.name.c_str(), + gravity_const); + } + std::string gravity_frame = "base_link"; + if (joint.parameters.find("gravity_frame") != joint.parameters.end()) { + gravity_frame = std::string(joint.parameters.at("gravity_frame")); + RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), + "Setting Joint %s gravity_frame to %s'", joint.name.c_str(), + gravity_frame.c_str()); + } + std::string link_frame; + if (joint.parameters.find("current_frame") != joint.parameters.end()) { + link_frame = std::string(joint.parameters.at("current_frame")); + RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), + "Setting Joint %s current_frame to %s'", joint.name.c_str(), + link_frame.c_str()); + } + std::shared_ptr gravity_ff_manager = nullptr; + if (gravity_ff_freq > 0.0 && !gravity_frame.empty() && + !link_frame.empty()) { + gravity_ff_manager = std::make_shared( + debug_node_, gravity_ff_freq, link_frame, gravity_frame, + gravity_const); + } axes_.emplace_back(&can_intf_, std::stoi(joint.parameters.at("node_id")), - multiplier, input_mode); + multiplier, input_mode, gravity_ff_manager); } return CallbackReturn::SUCCESS; @@ -212,10 +322,6 @@ CallbackReturn ODriveHardwareInterface::on_activate(const State &) { set_axis_command_mode(axis); } if (debug_frequency_ > 0) { - debug_node_ = std::make_shared("odrive_system_debug_node"); - executor_ = rclcpp::executors::SingleThreadedExecutor::make_shared(); - executor_->add_node(debug_node_); - spin_thread_ = std::thread([this]() { this->executor_->spin(); }); debug_timer_ = debug_node_->create_wall_timer( std::chrono::milliseconds(1000 / debug_frequency_), std::bind(&ODriveHardwareInterface::pub_status, this)); @@ -224,6 +330,9 @@ CallbackReturn ODriveHardwareInterface::on_activate(const State &) { debug_node_->template create_publisher( info_.joints[&axis - &axes_[0]].name + "/status", rclcpp::SystemDefaultsQoS()); + if (axis.gravity_ff_manager_) { + axis.gravity_ff_manager_->start(); + } } } @@ -348,12 +457,23 @@ return_type ODriveHardwareInterface::write(const rclcpp::Time &, for (auto &axis : axes_) { // Send the CAN message that fits the set of enabled setpoints if (axis.pos_input_enabled_) { + auto target = axis.pos_setpoint_; + if (std::isnan(axis.pos_setpoint_)) { + target = axis.pos_estimate_; + } Set_Input_Pos_msg_t msg; - msg.Input_Pos = axis.pos_setpoint_ / (2 * M_PI * axis.multiplier_); + msg.Input_Pos = target / (2 * M_PI * axis.multiplier_); msg.Vel_FF = axis.vel_input_enabled_ ? (axis.vel_setpoint_ / (2 * M_PI * axis.multiplier_)) : 0.0f; - msg.Torque_FF = axis.torque_input_enabled_ ? axis.torque_setpoint_ : 0.0f; + if (axis.torque_input_enabled_) { + msg.Torque_FF = axis.torque_setpoint_; + } else if (axis.gravity_ff_manager_) { + msg.Torque_FF = axis.gravity_ff_manager_->get_ff_value(); + } else { + msg.Torque_FF = 0.0f; + } + axis.send(msg); } else if (axis.vel_input_enabled_) { Set_Input_Vel_msg_t msg; diff --git a/src/Teleop-Control/joystick_control/CMakeLists.txt b/src/Teleop-Control/joystick_control/CMakeLists.txt index 9f6b74ba..73acbf1e 100644 --- a/src/Teleop-Control/joystick_control/CMakeLists.txt +++ b/src/Teleop-Control/joystick_control/CMakeLists.txt @@ -16,6 +16,10 @@ find_package(control_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(controller_manager_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(arm_control REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) set(CMAKE_CXX_STANDARD 14) @@ -34,6 +38,10 @@ add_executable(science src/science.cpp ) +target_link_libraries(arm + arm_control::move_group_client +) + ament_target_dependencies(drive rclcpp geometry_msgs diff --git a/src/Teleop-Control/joystick_control/config/pxn.yaml b/src/Teleop-Control/joystick_control/config/pxn.yaml index 4d2f3efc..af75ad8d 100644 --- a/src/Teleop-Control/joystick_control/config/pxn.yaml +++ b/src/Teleop-Control/joystick_control/config/pxn.yaml @@ -25,6 +25,7 @@ arm_teleop_node: wrist_yaw_positive: 3 wrist_yaw_negative: 5 joint_6_axis: 4 + dot_inc: 4 science_teleop_node: ros__parameters: diff --git a/src/Teleop-Control/joystick_control/include/arm.hpp b/src/Teleop-Control/joystick_control/include/arm.hpp index 0d0df25f..e0380c0f 100644 --- a/src/Teleop-Control/joystick_control/include/arm.hpp +++ b/src/Teleop-Control/joystick_control/include/arm.hpp @@ -1,8 +1,11 @@ #ifndef ARM_HPP #define ARM_HPP +#include "arm_control/MoveGroupClient.hpp" #include "control_msgs/msg/joint_jog.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" +#include "geometry_msgs/msg/vector3.hpp" #include "rclcpp/rclcpp.hpp" #include "ros_phoenix/msg/motor_control.hpp" #include "sensor_msgs/msg/joy.hpp" @@ -16,18 +19,24 @@ class arm : public rclcpp::Node { void arm_control(std::shared_ptr joystickMsg); void manual_arm_control(std::shared_ptr joystickMsg); void ik_arm_control(std::shared_ptr joystickMsg); + void ik_pose_control(std::shared_ptr joystickMsg); void endeffector_control(std::shared_ptr joystickMsg); + void clipboards_control(std::shared_ptr joystickMsg); bool moveit_servo_state(bool enable); + void clear_dot(); rclcpp::Subscription::SharedPtr joy_sub_; rclcpp::Publisher::SharedPtr joint_pub_; rclcpp::Publisher::SharedPtr ik_pub_; rclcpp::Publisher::SharedPtr eef_pub_; - enum ArmState { NONE = 0, MANUAL, IK }; + rclcpp::Publisher::SharedPtr dot_pub_; + std::shared_ptr moveit_client_; + enum ArmState { NONE = 0, MANUAL, IK, POS }; ArmState current_state_; void declareParameters(); void loadParameters(); + geometry_msgs::msg::PoseStamped getPoseOfPtr(); bool initialized_ = false; int kThrottleAxis; @@ -41,8 +50,22 @@ class arm : public rclcpp::Node { int kDisableButton; int kIkButton; int kManualButton; + int kPositionButton; int kClawOpen; int kClawClose; + int kCamWidth; + int kCamHeight; + int kDotInc; + int kClipboard1SaveButton; + int kClipboard1ExecuteButton; + int kClipboard2SaveButton; + int kClipboard2ExecuteButton; + + double targetPositionX; + double targetPositionY; + int clipboard1_pose_index_ = -1; + int clipboard2_pose_index_ = -1; + std::shared_ptr last_msg_; }; #endif \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/launch/controller.launch.py b/src/Teleop-Control/joystick_control/launch/controller.launch.py index bd6ec135..78505622 100644 --- a/src/Teleop-Control/joystick_control/launch/controller.launch.py +++ b/src/Teleop-Control/joystick_control/launch/controller.launch.py @@ -24,8 +24,7 @@ def find_ps5(): except Exception as e: print(f"Error reading devices: {e}") - print("ERROR: PS5 controller not found. Defaulting to /dev/input/js0") - return "/dev/input/js0" + print("ERROR: PS5 controller not found.") def generate_launch_description(): diff --git a/src/Teleop-Control/joystick_control/package.xml b/src/Teleop-Control/joystick_control/package.xml index 767ac9e5..72e26ede 100644 --- a/src/Teleop-Control/joystick_control/package.xml +++ b/src/Teleop-Control/joystick_control/package.xml @@ -16,6 +16,7 @@ geometry_msgs sensor_msgs ros_phoenix + arm_control interfaces joy joy_linux diff --git a/src/Teleop-Control/joystick_control/src/arm.cpp b/src/Teleop-Control/joystick_control/src/arm.cpp index 0d0fc6cb..71b374f6 100644 --- a/src/Teleop-Control/joystick_control/src/arm.cpp +++ b/src/Teleop-Control/joystick_control/src/arm.cpp @@ -1,6 +1,8 @@ #include "arm.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" #include "std_srvs/srv/trigger.hpp" +#include +#include arm::arm() : Node("arm_node") { @@ -16,9 +18,13 @@ arm::arm() : Node("arm_node") { "/servo_node/delta_twist_cmds", 10); eef_pub_ = this->create_publisher( "/end_effector/set", 10); + dot_pub_ = this->create_publisher( + "/rtp_client_node/dot", 10); + clear_dot(); RCLCPP_INFO(this->get_logger(), "Arm controller started"); } + void arm::endeffector_control( std::shared_ptr joystickMsg) { auto &buttons = joystickMsg->buttons; @@ -93,6 +99,15 @@ bool arm::moveit_servo_state(bool enable) { return true; } +void arm::clear_dot() { + geometry_msgs::msg::Vector3 msg; + msg.x = -1; + msg.y = -1; + dot_pub_->publish(msg); + targetPositionX = kCamWidth / 2; + targetPositionY = kCamHeight / 2; +} + void arm::arm_control(std::shared_ptr joystickMsg) { auto &buttons = joystickMsg->buttons; if (!initialized_) { @@ -102,28 +117,40 @@ void arm::arm_control(std::shared_ptr joystickMsg) { std::abs(joystickMsg->axes[kJoint4Axis]) < 0.01 && std::abs(joystickMsg->axes[kJoint6Axis]) < 0.01) { initialized_ = true; + } else { + RCLCPP_WARN_THROTTLE(this->get_logger(), *(this->get_clock()), 1, + "Arm Controller not reading zeros correctly"); } return; } - + bool state_changed = false; if (buttons[kDisableButton] && current_state_ != NONE) { current_state_ = NONE; + state_changed = true; moveit_servo_state(false); RCLCPP_INFO(this->get_logger(), "Arm disabled"); - return; } else if (buttons[kIkButton] && current_state_ != IK) { current_state_ = IK; + state_changed = true; moveit_servo_state(true); RCLCPP_INFO(this->get_logger(), "Switched to IK control"); } else if (buttons[kManualButton] && current_state_ != MANUAL) { current_state_ = MANUAL; + state_changed = true; moveit_servo_state(true); RCLCPP_INFO(this->get_logger(), "Switched to manual control"); + } else if (buttons[kPositionButton] && current_state_ != POS) { + current_state_ = POS; + state_changed = true; + moveit_servo_state(false); + RCLCPP_INFO(this->get_logger(), "Switched to position tracking control"); } - if (current_state_ != NONE) { + if (current_state_ == IK || current_state_ == MANUAL) { endeffector_control(joystickMsg); } - + if (state_changed && current_state_ != POS) { + clear_dot(); + } switch (current_state_) { case MANUAL: manual_arm_control(joystickMsg); @@ -131,9 +158,12 @@ void arm::arm_control(std::shared_ptr joystickMsg) { case IK: ik_arm_control(joystickMsg); break; + case POS: + ik_pose_control(joystickMsg); default: break; } + last_msg_ = joystickMsg; } void arm::manual_arm_control( @@ -161,7 +191,7 @@ void arm::manual_arm_control( void arm::ik_arm_control(std::shared_ptr joystickMsg) { auto twist_msg = geometry_msgs::msg::TwistStamped(); twist_msg.header.stamp = joystickMsg->header.stamp; - twist_msg.header.frame_id = "Link_6"; + twist_msg.header.frame_id = "EndEffector"; auto &axes = joystickMsg->axes; auto &buttons = joystickMsg->buttons; @@ -179,6 +209,81 @@ void arm::ik_arm_control(std::shared_ptr joystickMsg) { ik_pub_->publish(twist_msg); } +void arm::ik_pose_control(std::shared_ptr joystickMsg) { + if (!moveit_client_) { + moveit_client_ = + std::make_shared(shared_from_this()); + } + + if (joystickMsg->buttons[kClawOpen]) { + moveit_client_->stop(); + return; + } + + if (!last_msg_) { + return; + } + targetPositionX -= joystickMsg->axes[kJoint6Axis] * kDotInc; + targetPositionY -= joystickMsg->axes[kJoint3Axis] * kDotInc; + targetPositionX = + std::clamp(targetPositionX, 0.0, static_cast(kCamWidth)); + targetPositionY = + std::clamp(targetPositionY, 0.0, static_cast(kCamHeight)); + + bool wasPressed = last_msg_->buttons[kClawClose]; + bool isPressed = joystickMsg->buttons[kClawClose]; + if (wasPressed && !isPressed) { + geometry_msgs::msg::Vector3 msg; + msg.x = -1; + msg.y = -1; + dot_pub_->publish(msg); + moveit_client_->goToCamCoord(targetPositionX, targetPositionY); + targetPositionX = kCamWidth / 2; + targetPositionY = kCamHeight / 2; + } + clipboards_control(joystickMsg); + geometry_msgs::msg::Vector3 msg; + msg.x = targetPositionX; + msg.y = targetPositionY; + dot_pub_->publish(msg); +} + +void arm::clipboards_control( + std::shared_ptr joystickMsg) { + bool isPressed = joystickMsg->buttons[kClipboard1SaveButton]; + bool wasPressed = last_msg_->buttons[kClipboard1SaveButton]; + if (wasPressed && !isPressed) { + clipboard1_pose_index_ = moveit_client_->saveCurrentPose(); + RCLCPP_INFO(this->get_logger(), + "Saved current pose to clipboard 1 at index %d", + clipboard1_pose_index_); + } + isPressed = joystickMsg->buttons[kClipboard1ExecuteButton]; + wasPressed = last_msg_->buttons[kClipboard1ExecuteButton]; + if (wasPressed && !isPressed && clipboard1_pose_index_ != -1) { + moveit_client_->goToSavedPose(clipboard1_pose_index_); + RCLCPP_INFO(this->get_logger(), + "Executed pose from clipboard 1 at index %d", + clipboard1_pose_index_); + } + isPressed = joystickMsg->buttons[kClipboard2SaveButton]; + wasPressed = last_msg_->buttons[kClipboard2SaveButton]; + if (wasPressed && !isPressed) { + clipboard2_pose_index_ = moveit_client_->saveCurrentPose(); + RCLCPP_INFO(this->get_logger(), + "Saved current pose to clipboard 2 at index %d", + clipboard2_pose_index_); + } + isPressed = joystickMsg->buttons[kClipboard2ExecuteButton]; + wasPressed = last_msg_->buttons[kClipboard2ExecuteButton]; + if (wasPressed && !isPressed && clipboard2_pose_index_ != -1) { + moveit_client_->goToSavedPose(clipboard2_pose_index_); + RCLCPP_INFO(this->get_logger(), + "Executed pose from clipboard 2 at index %d", + clipboard2_pose_index_); + } +} + void arm::declareParameters() { this->declare_parameter("throttle.axis", 2); this->declare_parameter("joint_1_axis", 0); @@ -190,9 +295,17 @@ void arm::declareParameters() { this->declare_parameter("wrist_yaw_negative", 5); this->declare_parameter("ik_button", 10); this->declare_parameter("manual_button", 11); + this->declare_parameter("position_button", 8); this->declare_parameter("disable_button", 9); this->declare_parameter("claw_close_button", 0); this->declare_parameter("claw_open_button", 1); + this->declare_parameter("cam_width", 1920); + this->declare_parameter("cam_height", 1080); + this->declare_parameter("dot_inc", 2); + this->declare_parameter("clipboard1.save_button", 2); + this->declare_parameter("clipboard1.execute_button", 4); + this->declare_parameter("clipboard2.save_button", 3); + this->declare_parameter("clipboard2.execute_button", 5); } void arm::loadParameters() { this->get_parameter("throttle.axis", kThrottleAxis); @@ -205,9 +318,17 @@ void arm::loadParameters() { this->get_parameter("wrist_yaw_negative", kWristYaw_negative); this->get_parameter("ik_button", kIkButton); this->get_parameter("manual_button", kManualButton); + this->get_parameter("position_button", kPositionButton); this->get_parameter("disable_button", kDisableButton); - this->get_parameter("claw_close_button", kClawOpen); - this->get_parameter("claw_open_button", kClawClose); + this->get_parameter("claw_close_button", kClawClose); + this->get_parameter("claw_open_button", kClawOpen); + this->get_parameter("cam_width", kCamWidth); + this->get_parameter("cam_height", kCamHeight); + this->get_parameter("dot_inc", kDotInc); + this->get_parameter("clipboard1.save_button", kClipboard1SaveButton); + this->get_parameter("clipboard1.execute_button", kClipboard1ExecuteButton); + this->get_parameter("clipboard2.save_button", kClipboard2SaveButton); + this->get_parameter("clipboard2.execute_button", kClipboard2ExecuteButton); } int main(int argc, char **argv) { diff --git a/src/URDF/rover_urdf/meshes/Link_3.STL b/src/URDF/rover_urdf/meshes/Link_3.STL index 34910815..3fb269f3 100644 Binary files a/src/URDF/rover_urdf/meshes/Link_3.STL and b/src/URDF/rover_urdf/meshes/Link_3.STL differ diff --git a/src/URDF/rover_urdf/meshes/Link_4.STL b/src/URDF/rover_urdf/meshes/Link_4.STL index 5013c976..ebbdd6a7 100644 Binary files a/src/URDF/rover_urdf/meshes/Link_4.STL and b/src/URDF/rover_urdf/meshes/Link_4.STL differ diff --git a/src/URDF/rover_urdf/meshes/Link_5.STL b/src/URDF/rover_urdf/meshes/Link_5.STL index 874bb55e..223cc07c 100644 Binary files a/src/URDF/rover_urdf/meshes/Link_5.STL and b/src/URDF/rover_urdf/meshes/Link_5.STL differ diff --git a/src/URDF/rover_urdf/meshes/Link_6.STL b/src/URDF/rover_urdf/meshes/Link_6.STL index 66a87cb6..6a8df855 100644 Binary files a/src/URDF/rover_urdf/meshes/Link_6.STL and b/src/URDF/rover_urdf/meshes/Link_6.STL differ diff --git a/src/URDF/rover_urdf/meshes/Link_7.STL b/src/URDF/rover_urdf/meshes/Link_7.STL new file mode 100644 index 00000000..38b5e4ed Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Link_7.STL differ diff --git a/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro b/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro index 5d923197..b267e009 100644 --- a/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro +++ b/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro @@ -6,10 +6,11 @@ ros2_control_rover_arm/RoverArmHardwareInterface + 200 TalonSRX - 14 + 9 11.0 0.0005 1.0 @@ -17,7 +18,7 @@ quadrature absolute 4096 - 4.99 + 4.88 false false false @@ -28,21 +29,21 @@ TalonSRX 11 - 14.0 - 0.0005 - 0.3 + 11.0 + 0.0 + 0.0 0.0 quadrature absolute 4096 - 1.9 + -1.2 false false - 10 - Link_4 - -0.03 1 false + 10 + Link_4 + -0.023 @@ -50,14 +51,14 @@ TalonSRX 12 - 4.0 + 6.0 0.001 0.0 0.0 quadrature absolute 4096 - 4.69494 + 3.5 false true true @@ -65,28 +66,10 @@ - - TalonSRX - 19 - 8.5 - 0.001 - 0.001 - 0.0 - quadrature - absolute - 4096 - 3.26377 - false - false - false - - - - TalonSRX - 15 - 0.01 + 14 + 0.015 0.000001 0.0 0.0 @@ -111,6 +94,16 @@ 10 -1.0 + 30 + Link_3 + -0.035 + + + + + + 13 + 1.0 diff --git a/src/URDF/rover_urdf/urdf/arm_urdf.xacro b/src/URDF/rover_urdf/urdf/arm_urdf.xacro index 3592b557..5d104b31 100644 --- a/src/URDF/rover_urdf/urdf/arm_urdf.xacro +++ b/src/URDF/rover_urdf/urdf/arm_urdf.xacro @@ -118,7 +118,7 @@ link="Link_2" /> - + @@ -151,41 +151,29 @@ - + - + - + - + - - - - - - - - - - - - @@ -223,7 +211,7 @@ link="Link_3" /> - + @@ -278,7 +266,7 @@ name="Joint_3" type="revolute"> @@ -286,7 +274,7 @@ link="Link_4" /> - + @@ -321,7 +309,7 @@ - + @@ -331,19 +319,13 @@ - - - - - - - + - + - + @@ -353,7 +335,7 @@ name="Joint_4" type="revolute"> @@ -361,7 +343,7 @@ link="Link_5" /> - + @@ -394,13 +376,13 @@ - + - + @@ -422,7 +404,7 @@ name="Joint_5" type="revolute"> @@ -491,7 +473,7 @@ name="Joint_6" type="continuous"> diff --git a/src/interfaces/CMakeLists.txt b/src/interfaces/CMakeLists.txt index a8b04d90..b9d418f5 100644 --- a/src/interfaces/CMakeLists.txt +++ b/src/interfaces/CMakeLists.txt @@ -27,6 +27,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/GasSensorReading.msg" "msg/SrtStats.msg" "msg/RtpStats.msg" + "msg/Distance.msg" "srv/NavToGPSGeopose.srv" "srv/VideoOut.srv" "srv/VideoCapture.srv" diff --git a/src/interfaces/msg/Distance.msg b/src/interfaces/msg/Distance.msg new file mode 100644 index 00000000..5b7ab266 --- /dev/null +++ b/src/interfaces/msg/Distance.msg @@ -0,0 +1,11 @@ +# Distance.msg + +uint8 STATUS_OK = 0 +uint8 STATUS_ERROR = 1 +uint8 STATUS_INVALID = 2 + +std_msgs/Header header + +uint8 status + +float32 distance \ No newline at end of file diff --git a/src/interfaces/msg/RtpStats.msg b/src/interfaces/msg/RtpStats.msg index 29e36638..120d31cf 100644 --- a/src/interfaces/msg/RtpStats.msg +++ b/src/interfaces/msg/RtpStats.msg @@ -4,5 +4,3 @@ uint64 num_lost uint64 num_late uint64 num_duplicates uint32 avg_jitter -uint64 recovered -uint64 unrecovered