Some tips on ROS

[mintar]

Cool project! I watched your video and really enjoyed it. While watching, I realized that I've seen several other of your videos over the years and liked them a lot.

Your skills in many areas surpass my own by far. However, since I've been developing ROS software for a living for 10 years, this is one area where I figured I could help, and I couldn't resist giving you some unsolicited advice, so here goes.

Aligning depth and color images

Instead of this hackery:

ReallyUsefulRobot/Vision/camera11_depth.py

Lines 100 to 101 in 8775ac2

	detectXa = int((detectX/640*400)+100) # scale and shift for RGB/depth camera alignment
	detectYa = int((detectY/480*240)+100) # scale and shift for RGB/depth camera alignment

... you should either use pyrealsense2.rs2_project_color_pixel_to_depth_pixel, or perhaps better yet don't use the pyrealsense2 library, but instead use the realsense2_camera ROS driver something like this (adjust width + height as needed):

<include file="$(find realsense2_camera)/launch/rs_rgbd.launch">
  <arg name="camera"          value="d435" />

  <arg name="depth_width"     value="848"/>
  <arg name="depth_height"    value="480"/>
  <arg name="enable_depth"    value="true"/>

  <arg name="infra_width"     value="848"/>
  <arg name="infra_height"    value="480"/>
  <arg name="enable_infra1"   value="true"/>
  <arg name="enable_infra2"   value="true"/>

  <arg name="color_width"     value="848"/>
  <arg name="color_height"    value="480"/>
  <arg name="enable_color"    value="true"/>

  <arg name="enable_sync"     value="true"/>
  <arg name="align_depth"     value="true"/>
</include>

From the realsense-ros README:

align_depth: If set to true, will publish additional topics for the "aligned depth to color" image.: /camera/aligned_depth_to_color/image_raw, /camera/aligned_depth_to_color/camera_info.

Since we set the camera parameter to d435, this will be /d435/aligned_depth_to_color/image_raw instead.

Then you can use a TimeSynchronizer to subscribe to the /d435/aligned_depth_to_color/image_raw and /d435/color/image_raw topics, and they will be aligned pixel-by-pixel; in other words, the depth image will be transformed to look as if the depth camera was exactly at the RGB camera's position and also as if it had the same camera parameters (lens distortion etc.) as the RGB camera.

Removing lens distortion

There should also be two topics called .../image_rect_raw and .../image_rect_color. These are the rectified topics, i.e. any lens distortion has been removed. You should use those if you're going to use some trigonometry like you showed in the video.

Using TF

Regarding the trigonometry calculations, you can make your life much easier by using the tf2_ros library. There are some tutorials that are 100% worth going over. TF is one part where ROS really shines.

In short, you should:

• Remove the joint_state_publisher from your rur_bringup launch file.
• Publish the angle of the camera tilt unit as a sensor_msgs/JointState message on the topic /joint_states instead of a std_msgs/UInt16 as you're doing now. You can leave the velocity and effort fields empty, just fill in the name field with the name of the joint in the URDF and the position field with the joint angle in radians.
• Do the same for the wheel joints. The joint_states topic can have multiple independent publishers, they don't all need to be in one message.
• Add the arm to your URDF and also publish joint_states for them.

This will allow the robot_state_publisher to pick up the actual joint states from your robot, display them in RViz and (most importantly) publish correct TFs for them.

Now is where the magic happens: You can now automatically transform the center point of your cup into any frame (base_link, or your gripper frame, or whatever). Disclaimer: I haven't tested the code below, but it should give you the right ideas:

import rospy
import message_filters
import tf2_geometry_msgs
import tf2_ros

from image_geometry import PinholeCameraModel

from geometry_msgs.msg import PointStamped
from sensor_msgs.msg import Image, CameraInfo


class VisionNode(object):
    def __init__(self):
        self.camera_model = PinholeCameraModel()

        self.tf = tf2_ros.Buffer()
        self.tfl = tf2_ros.transform_listener.TransformListener(tf)

        self.color_image_sub = message_filters.Subscriber(
            "/d435/color/image_rect_color", Image, queue_size=1
        )
        self.depth_image_sub = message_filters.Subscriber(
            "/d435/aligned_depth_to_color/image_rect_raw", Image, queue_size=1
        )
        self.info_sub = message_filters.Subscriber(
            "/d435/color/camera_info", CameraInfo, queue_size=1
        )

        self.ts = message_filters.TimeSynchronizer(
            [color_image_sub, depth_image_sub, info_sub], 1
        )
        self.ts.registerCallback(callback)

    def callback(color_image, depth_image, camera_info):
        # TODOs:
        # - compute detectX, detectY as in previous code
        # - look up detectZ for those pixel coordinates from depth image

        # compute 3D point (in camera frame)
        self.camera_model.fromCameraInfo(camera_info)
        (x, y, z) = self.camera_model.projectPixelTo3dRay((detectX, detectY))
        p = PointStamped()
        p.header = color_image.header
        p.point.x = x * detectZ
        p.point.y = y * detectZ
        p.point.z = detectZ

        # automagically transform into any frame
        tf.transform(p, "base_link")


def main():
    rospy.init_node("vision")
    VisionNode()
    try:
        rospy.spin()
    except rospy.ROSInterruptException:
        pass


if __name__ == "__main__":
    main()

Controlling the arm

If you want to go in really deep, look at MoveIt. It's not going to be easy though!

Joint state publisher GUI

Bonus tip: If you use the joint_state_publisher_gui in your display.launch like this...

  <node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"/>

..., it will pop up a little GUI where you can play around with the joints in your URDF. Very useful for debugging.

[XRobots]

Thanks Martin,

I'm not using a ROS camera driver. How do I align the images with the NVIDIA Jetson Inference deep learning code camera feed?

[mintar]

I'm not using a ROS camera driver.

Yes, but why not? It's really useful. For example, you can use the camera image in other ROS nodes, display it in RViz and so on.

How do I align the images with the NVIDIA Jetson Inference deep learning code camera feed?

Looking at your code, you currently do the following:

img, width, height = camera.CaptureRGBA()
detections = net.Detect(img, width, height)

The first line retrieves the image in img, the second line passes it to the neural net. So all you need to figure out is how to convert the ROS message color_image in my code into the same format that img uses. Unfortunately I don't know what that format is; I guess an array of floats, or an array of array of floats or something. It would be helpful to put a print img, print type(img) and print len(img) into the code temporarily to figure that out.

If you're lucky, you could do it like this:

from cv_bridge import CvBridge

# [...]
class VisionNode(object):
# [...]
    def callback(color_image, depth_image, camera_info):
        cv_bridge = CvBridge()
        img_cv = cv_bridge.imgmsg_to_cv2(color_image, 'rgba8')
        detections = net.Detect(img_cv, color_image.width, color_image.height)

img_cv looks like this:

array([[[ 37,  45,  34, 255],
        [ 40,  48,  37, 255],
        [ 46,  53,  45, 255],
        ...,
        [145, 140, 148, 255],
        [135, 130, 138, 255],
        [122, 117, 125, 255]],

       [[ 37,  45,  34, 255],
        [ 39,  47,  36, 255],
        [ 44,  49,  39, 255],
        ...,
        [141, 137, 142, 255],
        [128, 123, 131, 255],
        [115, 110, 118, 255]],

       [[ 35,  40,  30, 255],
        [ 42,  47,  37, 255],
        [ 45,  50,  40, 255],
        ...,
        [137, 131, 137, 255],
        [129, 124, 132, 255],
        [108, 103, 111, 255]],

       ...,

       [[125, 135, 142, 255],
        [121, 131, 138, 255],
        [124, 131, 137, 255],
        ...,
        [ 42,  49,  53, 255],
        [ 42,  49,  53, 255],
        [ 40,  47,  51, 255]],

       [[121, 126, 135, 255],
        [124, 129, 138, 255],
        [125, 130, 137, 255],
        ...,
        [ 42,  49,  53, 255],
        [ 43,  50,  54, 255],
        [ 42,  49,  53, 255]],

       [[122, 124, 134, 255],
        [127, 129, 139, 255],
        [129, 128, 137, 255],
        ...,
        [ 40,  48,  49, 255],
        [ 42,  50,  51, 255],
        [ 40,  48,  49, 255]]], dtype=uint8)

... and its type is numpy.ndarray. If it doesn't work, perhaps try other encodings by replacing 'rgba8' with '32FC3' or any other encoding from image_encodings.h.