Humble fiducial pose (#40)

* feat: add pose service class * fix: add image callback * fix: add img callback * fix: aruco detection * fix: marker detection error * fix: add move_to_pose.cpp * refact: comments + readme * fix: lint errors * fix: ros2 build error * fix: lint errors * fix: lint error ros * fix: more ROS errors * fix: add visual for grasp point * fix: grasping point * fix: gripper point * fix: gripper point * fix: gripper tests * fix: hande xacro from old * fix: remove xacro comment * fix: remove : in xacro * fix: remove xacro comment * fix: remove visual for grasp point * fix: weired error * fix: temp remove pose service * fix: remove visual * fix: remove space * fix: add back pose service * fix: name change to aruco_pose * refac: add aruco pose launch * refac: add back move scrip tmp * fix: fix pose_build * fix: rotation matrix * fix: rotation_matrix_ * feat: gripper open and close * feat: new pickup approach * fix: change vscode base img * fix: aruco param issue * fix: docker issue * fix: display params * fix: pose params * fix: double declare param cx * fix: param naming * fix: d to f * fix: param undeclared issue * fix: param issue * fix: ur_moveit_launch break issue * fix: lint errors * fix: amnet_cppcheck * fix: can rotation correct params * fix: remove test ros shutdown * fix: correct name of yaml file * fix: name correct ur.srdf * fix: adjustments for PR * fix: remove move to pose * fix: copyright year * fix: expand readme * fix: PR changes * fix: PR fixes * fix: aruco pose errors * refact: change repo for testing * fix: cam_ prefix add to params * refact: remove useless files * refact: remove unused libs * Update docker/erobs-common-img/Dockerfile --------- Co-authored-by: Dr. Phil Maffettone <43007690+maffettone@users.noreply.github.com>
maffettone · Jul 26, 2024 · 86764da · 86764da
1 parent b5da5f9
commit 86764da
Show file tree

Hide file tree

Showing 19 changed files with 721 additions and 43 deletions.
diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile
@@ -1,7 +1,6 @@
-FROM althack/ros2:humble-dev 
+FROM althack/ros2:humble-full
 
 # ** [Optional] Uncomment this section to install additional packages. **
-#
 ENV DEBIAN_FRONTEND=noninteractive
 RUN apt-get update \
         && apt-get -y install --no-install-recommends \
@@ -12,7 +11,8 @@ RUN apt-get update \
         ros-${ROS_DISTRO}-moveit\
         build-essential \
         python3-colcon-common-extensions \
-        #
+        libopencv-dev \
+        python3-opencv \
         # Clean up
         && apt-get autoremove -y \
         && apt-get clean -y \

diff --git a/docker/erobs-common-img/Dockerfile b/docker/erobs-common-img/Dockerfile
@@ -10,6 +10,7 @@ RUN apt-get update \
         ros-${ROS_DISTRO}-ur \
         build-essential \
         python3-colcon-common-extensions
+
 RUN apt-get install -y x11vnc xvfb 
 # Clean up
 RUN apt-get autoremove -y \

diff --git a/docker/erobs-common-img/README.md b/docker/erobs-common-img/README.md
@@ -10,7 +10,7 @@ Several containers need to be running for our application. Once running, clients
 **TODO (ChandimaFernando)**: Change the repo link to nsls-II repo, and add CI/CD for build on version tag.
 
 ```bash
-export ROBOT_IP=10.67.218.141
+export ROBOT_IP=192.168.0.100
 export UR_TYPE="ur3e"
 export LAUNCH_RVIZ="false"
 export DESCRIPTION_PKG="ur3e_hande_robot_description"
@@ -23,27 +23,39 @@ export GHCR_POINTER=ghcr.io/chandimafernando/erobs-common-img:latest
 
 # Enable the connection between the UR robot and the VM
 ```bash
-podman run -it --network host --ipc=host --pid=host \
+podman run -it --rm --network host --ipc=host --pid=host \
     --env ROBOT_IP=$ROBOT_IP \
     --env UR_TYPE=$UR_TYPE \
     --env ROS_DISTRO=$ROS_DISTRO \
     --env DESCRIPTION_PKG=$DESCRIPTION_PKG \
     --env DESCRIPTION_FILE=$DESCRIPTION_FILE \
     ${GHCR_POINTER} \
-    /bin/sh -c "printenv && . /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 launch ur_robot_driver ur_control.launch.py ur_type:=${UR_TYPE} robot_ip:=${ROBOT_IP} description_package:=${DESCRIPTION_PKG} description_file:=${DESCRIPTION_FILE} launch_rviz:=false tool_voltage:=24"
+    /bin/sh -c "printenv && . /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 launch ur_robot_driver ur_control.launch.py ur_type:=${UR_TYPE} robot_ip:=${ROBOT_IP} description_package:=${DESCRIPTION_PKG} description_file:=${DESCRIPTION_FILE} launch_rviz:=${LAUNCH_RVIZ} tool_voltage:=24"
 ```
 
 # Run gripper service to enable the gripper 
 ```bash
-podman run -it --network host --ipc=host --pid=host \
+podman run -it --rm --network host --ipc=host --pid=host \
+    --env ROBOT_IP=$ROBOT_IP \
+    --env ROS_DISTRO=$ROS_DISTRO  \
     ${GHCR_POINTER} \
     /bin/bash -c ". /root/ws/install/setup.sh && \
+    . /opt/ros/${ROS_DISTRO}/setup.sh && \
+    ros2 run ur_robot_driver tool_communication.py --ros-args -p robot_ip:=${ROBOT_IP} & \
+    sleep 5 && \
+    . /root/ws/install/setup.sh && \
     ros2 run gripper_service gripper_service"
 ```
 
+**NOTE**
+
+`tool_communication.py` should not be called when `ur_control.launch.py` is called, but separately when the gripper is initiated.  The 5s delay allows socat creation before running the gripper service. 
+The gripper_service has two convenience scripts, `gripper_open.cpp` and `gripper_close.cpp` that will start up client nodes to send a request to the gripper service then promptly shutdown the client nodes.
+
+
 # Launch move_group
 ```bash
-podman run -it --network host --ipc=host --pid=host \
+podman run -it --rm --network host --ipc=host --pid=host \
     --env ROBOT_IP=$ROBOT_IP \
     --env UR_TYPE=$UR_TYPE \
     --env ROS_DISTRO=$ROS_DISTRO \
@@ -54,6 +66,16 @@ podman run -it --network host --ipc=host --pid=host \
     ${GHCR_POINTER} \
     /bin/sh -c "printenv && . /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=${UR_TYPE} launch_rviz:=${LAUNCH_RVIZ} description_package:=${DESCRIPTION_PKG}  launch_servo:=false description_file:=${DESCRIPTION_FILE} moveit_config_package:=${CONFIG_PKG} moveit_config_file:=${CONFIG_FILE}"
 ```
+# Launch aruco_pose 
+```bash
+podman run -it --network host --ipc=host --pid=host \
+    --env ROS_DISTRO=$ROS_DISTRO \
+    ${GHCR_POINTER} \
+    /bin/sh -c "printenv && \
+    . /root/ws/install/setup.sh && \
+    . /opt/ros/${ROS_DISTRO}/setup.sh && \
+    ros2 launch aruco_pose aruco_pose.launch.py"
+```
 
 # Launch the pdf_beamtime_server
 ```bash

diff --git a/src/aruco_pose/CMakeLists.txt b/src/aruco_pose/CMakeLists.txt
@@ -0,0 +1,74 @@
+cmake_minimum_required(VERSION 3.8)
+project(aruco_pose)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(message_filters REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(filters REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+set(dependencies
+  rclcpp
+  rclcpp_components
+  sensor_msgs
+  image_transport
+  message_filters
+  cv_bridge
+  OpenCV
+  eigen3_cmake_module
+  filters
+  tf2_ros
+  tf2
+  geometry_msgs
+)
+
+include_directories(include)  # This makes sure the libraries are visible
+
+add_executable(aruco_pose src/aruco_pose.cpp)
+ament_target_dependencies(aruco_pose ${dependencies})
+
+install(TARGETS
+aruco_pose
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(
+  DIRECTORY config/
+  DESTINATION config
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/src/aruco_pose/README.md b/src/aruco_pose/README.md
@@ -0,0 +1,28 @@
+# Detecting Aruco Markers with Azure Kinect
+
+## Camera Parameters
+
+In OpenCV, the distortion coefficients are usually represented as a 1x8 matrix:
+
+- distCoeffs_= [k1, k2, p1, p2, k3, k4, k5, k6] where
+- k1, k2, k3, k3, k4, k5, k6 = radial distortion coefficients
+- p1, p2 = tangential distortion coefficients
+
+For Azure kinect, they can be found when running the ROS2 camera node and explained in [this link](https://microsoft.github.io/Azure-Kinect-Sensor-SDK/master/structk4a__calibration__intrinsic__parameters__t_1_1__param.html).
+
+## ArUco markers
+
+Complete guide to ArUco markers are in [this link](https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html).
+
+## Tag detection
+When there are multiple tags present, function `estimatePoseSingleMarkers()` estimates pose of all the detected markers seperatley. At present, we only estimate the pose when only one marker is present, ignoring the cases where no-marker or more than one marker are present.
+
+## Detection rate
+Detection rate is tied to the callback of the ros topic `image_topic` (topic is defined in the fiducial_marker_param.yam file).
+
+In the current implementation, this topic's publish rate is defined in the Azure_Kinect_ROS_Driver package. In Azure_Kinect_ROS_Driver package, where the topic 'rgb/image_raw' is published, message publish rate is same as the camera's fps rate. Default fps is 5.
+
+## Median estimation
+Median pose estimation is a multi-value estimation, where it estimates the median of the 6 DoF pose returned by the camera. 
+
+For each DoF, it maintains a moving window of size 10 (can be changed via the parameter `number_of_observations` ) to calculate the median. When the detection rate is 5Hz and upon moving the sample or the detected object to a new pose, it takes a minimum of 1.2 seconds to return the correct pose for the updated pose. As a good practice, it is better to wait for the whole 2 seconds. 
diff --git a/src/aruco_pose/config/camera_param.yaml b/src/aruco_pose/config/camera_param.yaml
@@ -0,0 +1,16 @@
+aruco_pose:
+  ros__parameters:
+    intrinsics: # Uses to construct K matrix
+      fx: 974.724
+      fy: 974.456
+      cx: 1024.82
+      cy: 773.291
+    dist_coeffs: # Distrotion coefficients
+      k1: 0.602113
+      k2: -2.92716
+      k3: 1.63343
+      k4: 0.468872
+      k5: -2.72304
+      k6: 1.54828
+      p1: 0.000402061
+      p2: -0.000392198
diff --git a/src/aruco_pose/config/fiducial_marker_param.yaml b/src/aruco_pose/config/fiducial_marker_param.yaml
@@ -0,0 +1,23 @@
+aruco_pose:
+  ros__parameters:
+    cam_translation:
+      x: 0.549 # robot location w.r.t to camera in 4
+      y: -0.392 
+      z: 0.08 
+    cam_rotation: # These are expressed in RPY angles
+      cam_alpha: 270.0
+      cam_beta: 0.0
+      cam_gamma: 90.0
+    camera_tf_frame: 'camera_base'
+    sample_name: 'sample_1'
+    pre_pickup_location: # Define x, y, z adjustments with respect to tag's coordinate reference frame for pre-pickup
+      name: 'pre_pickup'
+      x_adj: 0.0
+      y_adj: 0.0
+      z_adj: 0.08
+    offset_on_marker_x: -0.005
+    offset_on_marker_y: 0.0
+    fiducial_marker_family: 'DICT_APRILTAG_36h11'
+    physical_marker_size: 0.02665 #
+    image_topic: "/rgb/image_raw"
+    number_of_observations: 10 # size of the moving window for the median filter
diff --git a/src/aruco_pose/include/aruco_pose/aruco_pose.hpp b/src/aruco_pose/include/aruco_pose/aruco_pose.hpp
@@ -0,0 +1,110 @@
+/*Copyright 2024 Brookhaven National Laboratory
+BSD 3 Clause License. See LICENSE.txt for details.*/
+#pragma once
+
+#include <cv_bridge/cv_bridge.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+
+#include <iostream>
+#include <thread>
+#include <memory>
+#include <string>
+#include <map>
+#include <cmath>
+#include <fstream>
+#include <vector>
+
+#include <rclcpp/rclcpp.hpp>
+#include <opencv2/opencv.hpp>
+#include <opencv2/aruco.hpp>
+
+#include <image_transport/image_transport.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <filters/median.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
+
+class ArucoPose : public rclcpp::Node
+{
+private:
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr camera_subscription_;
+
+  tf2_ros::StaticTransformBroadcaster static_broadcaster_;
+  tf2_ros::TransformBroadcaster tf_broadcaster_;
+
+  rclcpp::Logger LOGGER;
+
+  /// @brief Callback function to
+  /// @param rgb_msg
+  void image_raw_callback(
+    const sensor_msgs::msg::Image::ConstSharedPtr & rgb_msg);
+
+  cv::Mat cameraMatrix_;
+  cv::Mat distCoeffs_;
+
+  tf2::Quaternion camera_quaternion_;
+
+  // Parameters for Aruco maker detection
+  std::vector<int> markerIds_;
+  std::vector<std::vector<cv::Point2f>> markerCorners_, rejectedCandidates_;
+  cv::Ptr<cv::aruco::DetectorParameters> parameters_;  // Params for ArUco detetcion
+  cv::Ptr<cv::aruco::Dictionary> dictionary_;
+
+  // This is the actual length/width of the printed tag
+  double physical_marker_size_;   // height/width in meters
+
+  // Median filter related vars
+  std::shared_ptr<filters::MultiChannelFilterBase<double>> median_filter_ =
+    std::make_shared<filters::MultiChannelMedianFilter<double>>();
+
+  std::vector<double> median_filtered_rpyxyz;
+
+  /// @brief converts a rpy to a quaternion
+  geometry_msgs::msg::Quaternion toQuaternion(double roll, double pitch, double yaw);
+
+  std::vector<std::string> double_params_ = {
+    "intrinsics.fx", "intrinsics.fy", "intrinsics.cx", "intrinsics.cy",
+    "dist_coeffs.k1", "dist_coeffs.k2", "dist_coeffs.k3", "dist_coeffs.k4",
+    "dist_coeffs.k5", "dist_coeffs.k6", "dist_coeffs.p1", "dist_coeffs.p2",
+    "cam_translation.x", "cam_translation.y", "cam_translation.z",
+    "cam_rotation.cam_alpha", "cam_rotation.cam_beta", "cam_rotation.cam_gamma",
+    "pre_pickup_location.x_adj", "pre_pickup_location.y_adj", "pre_pickup_location.z_adj",
+    "offset_on_marker_x", "offset_on_marker_y", "physical_marker_size"
+  };
+
+  std::vector<std::string> string_params_ = {
+    "camera_tf_frame", "sample_name", "pre_pickup_location.name", "fiducial_marker_family",
+    "image_topic"
+  };
+
+  // Map string values to corresponding ArUco dictionary enums
+  std::map<std::string, cv::aruco::PREDEFINED_DICTIONARY_NAME> dictionary_map_ = {
+    {"DICT_4X4_50", cv::aruco::DICT_4X4_50},
+    {"DICT_4X4_100", cv::aruco::DICT_4X4_100},
+    {"DICT_4X4_250", cv::aruco::DICT_4X4_250},
+    {"DICT_4X4_1000", cv::aruco::DICT_4X4_1000},
+    {"DICT_5X5_50", cv::aruco::DICT_5X5_50},
+    {"DICT_5X5_100", cv::aruco::DICT_5X5_100},
+    {"DICT_5X5_250", cv::aruco::DICT_5X5_250},
+    {"DICT_5X5_1000", cv::aruco::DICT_5X5_1000},
+    {"DICT_6X6_50", cv::aruco::DICT_6X6_50},
+    {"DICT_6X6_100", cv::aruco::DICT_6X6_100},
+    {"DICT_6X6_250", cv::aruco::DICT_6X6_250},
+    {"DICT_6X6_1000", cv::aruco::DICT_6X6_1000},
+    {"DICT_7X7_50", cv::aruco::DICT_7X7_50},
+    {"DICT_7X7_100", cv::aruco::DICT_7X7_100},
+    {"DICT_7X7_250", cv::aruco::DICT_7X7_250},
+    {"DICT_7X7_1000", cv::aruco::DICT_7X7_1000},
+    {"DICT_ARUCO_ORIGINAL", cv::aruco::DICT_ARUCO_ORIGINAL},
+    {"DICT_APRILTAG_16h5", cv::aruco::DICT_APRILTAG_16h5},
+    {"DICT_APRILTAG_25h9", cv::aruco::DICT_APRILTAG_25h9},
+    {"DICT_APRILTAG_36h10", cv::aruco::DICT_APRILTAG_36h10},
+    {"DICT_APRILTAG_36h11", cv::aruco::DICT_APRILTAG_36h11}
+  };
+
+public:
+  ArucoPose();
+};
diff --git a/src/aruco_pose/launch/aruco_pose.launch.py b/src/aruco_pose/launch/aruco_pose.launch.py
@@ -0,0 +1,22 @@
+from launch import LaunchDescription
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    """Launch the node aruco_pose with parameter files."""
+    return LaunchDescription(
+        [
+            Node(
+                package="aruco_pose",
+                executable="aruco_pose",
+                name="aruco_pose",
+                output="screen",
+                parameters=[
+                    PathJoinSubstitution([FindPackageShare("aruco_pose"), "config", "camera_param.yaml"]),
+                    PathJoinSubstitution([FindPackageShare("aruco_pose"), "config", "fiducial_marker_param.yaml"]),
+                ],
+            )
+        ]
+    )