Arm urdf changes

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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
+
+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()

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)

src/Arm/arm_control/include/arm_control/MoveGroupClient.hpp

@@ -0,0 +1,60 @@
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "interfaces/msg/distance.hpp"
+#include "std_msgs/msg/float32.hpp"
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+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<moveit::planning_interface::MoveGroupInterface> move_group_;
+  rclcpp::Subscription<interfaces::msg::Distance>::SharedPtr depth_sub_;
+  rclcpp::Node::SharedPtr node_;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
+
+  double planning_time_;
+  int planning_attempts_;
+  double vel_scaling_;
+  double acc_scaling_;
+  interfaces::msg::Distance::SharedPtr latest_distance_;
+
+  std::vector<geometry_msgs::msg::Pose> 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

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},

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,

src/Arm/arm_control/package.xml

@@ -18,6 +18,11 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>rclcpp</depend>
+  <depend>geometry_msgs</depend>
+  <depend>moveit_ros_planning_interface</depend>
+  <depend>interfaces</depend>
+
   <exec_depend>moveit_ros_move_group</exec_depend>
   <exec_depend>moveit_kinematics</exec_depend>
   <exec_depend>moveit_planners</exec_depend>

src/Arm/arm_control/src/MoveGroupClient.cpp

@@ -0,0 +1,184 @@
+#include "arm_control/MoveGroupClient.hpp"
+#include <memory>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+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<tf2_ros::Buffer>(node_->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+  move_group_ =
+      std::make_unique<moveit::planning_interface::MoveGroupInterface>(
+          node_, planning_group_);
+
+  depth_sub_ = node->create_subscription<interfaces::msg::Distance>(
+      "/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<double>("planning_time", 5.0);
+  node_->declare_parameter<std::string>("planning_group", "arm");
+  node_->declare_parameter<int>("planning_attempts", 10);
+  node_->declare_parameter<double>("vel_scaling", 0.2);
+  node_->declare_parameter<double>("acc_scaling", 0.2);
+  node_->declare_parameter<int>("cx", 960);
+  node_->declare_parameter<int>("cy", 540);
+  node_->declare_parameter<double>("fx", 540);
+  node_->declare_parameter<double>("fy", 540);
+  node_->declare_parameter<std::string>("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

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

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\\\""