Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Register a depth image to a mono camera frame with Nodelets.

Prerequisites

This tutorial requires you to have a calibrated multi camera setup consisting of (at least) one stereo camera and one mono camera. For more information see the tutorial on multi camera setup.

In general the nodes of the ensenso_camera ROS package, as well as this tutorial, make extensive use of ROS actions. If you are not familiar with ROS actions yet, we recommend you do the ROS action tutorials first.

Workflow

To recieve a registered depth image from an Ensenso stereo camera linked to a mono camera we have to perform the following steps:

1. Receive a depth image from the stereo camera
2. Receive an image from the linked mono camera
3. Project the depth image into the mono camera's frame

Step 1 and 2 are done with the corresponding RequestData actions defined for stereo and mono cameras:

• ensenso_camera_msgs/RequestData and

• ensenso_camera_msgs/RequestDataMono.

Step 3 is done by using the depth_image_proc/register nodelet.

In the next section we will write a Python ROS node that will define action clients for the corresponding action servers and make calls to them via action goals. The node thus takes care of step 1 and 2. The last section presents a launch file which runs the Python node and the depth_image_proc/register nodelet (step 3).

Python Node

The following node calls the actions servers of the mono and the stereo camera respectively. It is already defined in the ensenso_camera package as request_data_linked, so you do not have to write it yourself or copy it to your workspace. Below is the complete script and after it we will go through the it in detail:

   1 #!/usr/bin/env python
   2 import rospy
   3 import sys
   4 
   5 import actionlib
   6 from actionlib_msgs.msg import GoalStatus
   7 from ensenso_camera_msgs.msg import RequestDataAction, RequestDataGoal
   8 from ensenso_camera_msgs.msg import RequestDataMonoAction, RequestDataMonoGoal
   9 
  10 
  11 def main():
  12     loop_rate = rospy.get_param("~rate", 0.25)
  13     stereo_namespace = rospy.get_param("~stereo_ns", "stereo")
  14     mono_namespace = rospy.get_param("~mono_ns", "mono")
  15     timeout = rospy.get_param("~timeout", 60)
  16 
  17     # Use different namespaces in order to prevent duplicate topic names.
  18     request_stereo_data_client_name = stereo_namespace.rstrip("/") + "/request_data"
  19     request_mono_data_client_name = mono_namespace.rstrip("/") + "/request_data"
  20 
  21     request_stereo_data_client = actionlib.SimpleActionClient(
  22         request_stereo_data_client_name, RequestDataAction)
  23     request_mono_data_client = actionlib.SimpleActionClient(
  24         request_mono_data_client_name, RequestDataMonoAction)
  25 
  26     clients_names = [request_stereo_data_client_name, request_mono_data_client_name]
  27     clients = [request_stereo_data_client, request_mono_data_client]
  28     for client_name, client in zip(clients_names, clients):
  29         rospy.loginfo("Trying to connect to: {}".format(client_name))
  30         if not client.wait_for_server(rospy.Duration(timeout)):
  31             rospy.logerr("The camera node is not running!")
  32             sys.exit()
  33         rospy.loginfo("Connected")
  34 
  35     request_stereo_data_goal = RequestDataGoal()
  36     request_stereo_data_goal.request_depth_image = True
  37 
  38     request_mono_data_goal = RequestDataMonoGoal()
  39     request_mono_data_goal.request_rectified_images = True
  40 
  41     rate = rospy.Rate(loop_rate)
  42     while not rospy.is_shutdown():
  43         request_stereo_data_client.send_goal(request_stereo_data_goal)
  44         request_mono_data_client.send_goal(request_mono_data_goal)
  45 
  46         request_stereo_data_client.wait_for_result()
  47         request_mono_data_client.wait_for_result()
  48 
  49         if request_stereo_data_client.get_state() != GoalStatus.SUCCEEDED:
  50             rospy.logwarn("Stereo action was not successful.")
  51         else:
  52             error = request_stereo_data_client.get_result().error
  53             if error.code != 0:
  54                 rospy.logerr("Stereo error {}: {}".format(error.code, error.message))
  55 
  56         if request_mono_data_client.get_state() != GoalStatus.SUCCEEDED:
  57             rospy.logwarn("Mono action was not successful.")
  58         else:
  59             error = request_mono_data_client.get_result().error
  60             if error.code != 0:
  61                 rospy.logerr("Mono error {}: {}".format(error.code, error.message))
  62 
  63         rate.sleep()
  64 
  65 
  66 if __name__ == "__main__":
  67     try:
  68         rospy.init_node("ensenso_camera_request_data_linked")
  69         main()
  70     except rospy.ROSInterruptException:
  71         pass

The script has the same structure as the color_point_cloud script of the previous tutorial, except that it does not request a textured point cloud from the camera. For a detailed description please refer to the previous tutorial.

The quick summary of the script is: It requests a depth image from the stereo camera and a rectified image from the mono camera. The results are by default published under the following topics:

• mono/rectified/camera_info (sensor_msgs/CameraInfo): The camera info of the rectified mono image.

• mono/rectified/image (sensor_msgs/Image): The actual rectified mono image.

• stereo/depth/camera_info (sensor_msgs/CameraInfo): The camera info of the depth image, which is already rectified since it is calculated from the camera's disparity map, which uses the rectified images as input.

• stereo/depth/iamge (sensor_msgs/Image): The actual depth image.

In the following section you will see how to connect these camera topics to the nodelet that registers the depth image to the mono camera's frame.

Launch File

Since we want to use the depth_image_proc/register nodelet, let us have a closer look on how to use it:

It subscribes from the following topics:

• rgb/camera_info (sensor_msgs/CameraInfo): remap to mono/rectified/camera_info

• depth/camera_info (sensor_msgs/CameraInfo): remap to stereo/depth/camera_info

• depth/image_rect (sensor_msgs/Image): remap to stereo/depth/iamge

And it publishes these topics:

• depth_registered/camera_info (sensor_msgs/CameraInfo)

• depth_registered/image_rect (sensor_msgs/Image)

Furthermore, the node has a queue_size parameter (default 5) and requires the following tf transform:

• /depth_optical_frame -> /rgb_optical_frame

This transform is automatically published by the mono camera with tf in case the multi camera setup is correctly calibrated.

The launch file shown below combines the following nodes/nodelets:

• ensenso_camera/stereo_camera_nodelet: Nodelet

• ensenso_camera/mono_camera_nodelet: Nodelet

• ensenso_camera/request_data_linked: Node (Python)

• depth_image_proc/register: Nodelet

Since we want to leverage the advantages of nodelets, all three nodelets loaded in the launch file are run in the same nodelet manager. This is why the stereo and mono camera are not launched by simply including ensenso_camera/mono_stereo_nodelets.launch as it is done in the previous tutorial, but are both exlicitly launched.

   1 <!-- Open both an Ensenso stereo camera and a mono camera within the same
   2 nodelet manager, run the register_depth_image Python node and the
   3 depth_image_proc/register node in order to register the depth image from the
   4 stereo camera to the mono camera'a frame. -->
   5 
   6 <launch>
   7   <arg name="serial_stereo" default=""
   8        doc="Serial of the stereo camera (default: first found stereo camera)" />
   9   <arg name="serial_mono" default=""
  10        doc="Serial of the mono mamera (default: first found mono camera)" />
  11   <arg name="stereo_ns" default="stereo"
  12        doc="Namespace of the stereo camera" />
  13   <arg name="mono_ns" default="mono"
  14        doc="Namespace of the mono camera" />
  15   <arg name="target_frame" default=""
  16        doc="The tf frame the data will be returned in (default: camera_frame)" />
  17 
  18   <node pkg="nodelet" type="nodelet" name="manager_" args="manager" output="screen" />
  19 
  20   <node pkg="nodelet" type="nodelet" name="Ensenso_$(arg serial_stereo)"
  21         ns="$(arg stereo_ns)"
  22         args="load ensenso_camera/stereo_camera_nodelet /manager_" output="screen">
  23     <param name="serial" type="string" value="$(arg serial_stereo)" />
  24     <param name="target_frame" type="string" value="$(arg target_frame)" />
  25   </node>
  26 
  27   <node pkg="nodelet" type="nodelet" name="Ensenso_$(arg serial_mono)"
  28         ns="$(arg mono_ns)"
  29         args="load ensenso_camera/mono_camera_nodelet /manager_" output="screen">
  30     <param name="serial" type="string" value="$(arg serial_mono)" />
  31     <param name="link_frame" value="optical_frame_$(arg serial_stereo)" />
  32   </node>
  33 
  34   <node pkg="ensenso_camera" type="request_data_linked"
  35         name="Ensenso_request_data_linked">
  36     <param name="stereo_ns" type="string" value="$(arg stereo_ns)" />
  37     <param name="mono_ns" type="string" value="$(arg mono_ns)" />
  38   </node>
  39 
  40   <node pkg="nodelet" type="nodelet" name="DIP_Register"
  41         args="load depth_image_proc/register /manager_" output="screen">
  42     <!-- Inputs.  -->
  43     <remap from="rgb/camera_info" to="mono/rectified/camera_info" />
  44     <remap from="depth/camera_info" to="stereo/depth/camera_info" />
  45     <remap from="depth/image_rect" to="stereo/depth/image" />
  46     <!-- Outputs. -->
  47     <remap from="depth_registered/camera_info" to="mono/depth/camera_info" />
  48     <remap from="depth_registered/image_rect" to="mono/depth/image" />
  49     <!-- Parameters. -->
  50     <param name="queue_size" type="int" value="5" />
  51   </node>
  52 </launch>

In lines 40 to 51 the depth_image_proc/register nodelet is loaded and the subscribed topics are provided by remapping them accordingly. Note that we can directly map from depth/image_rect to stereo/depth/image, because the depth image is alreay rectified. Also note that we map the published topics into the mono namespace.

In order to run the launch file, which does already exists in the ensenso_camera package, run the following command in a terminal and fill in your camera serials:

roslaunch ensenso_camera \
    register_depth_image.launch \
    serial_stereo:=<serial of Ensenso stereo camera> \
    serial_mono:=<serial of mono camera>

In case your stereo camera is workspace calibrated, the resulting 3D data has been transformed by the NxLib and is no longer aligned to your camera frame. Simply provide the name of the target frame as in the following example:

roslaunch ensenso_camera \
    register_depth_image.launch \
    serial_stereo:=<serial of Ensenso stereo camera> \
    serial_mono:=<serial of mono camera> \
    target_frame:=Workspace