What we are going to learn

1. How to obtain depth camera data through ROS1 simulation
2. How to use topics offered by camera to determine object distance

List of resources used in this post

1. Use the rosject: https://app.theconstructsim.com/#/l/467026ab/
2. ROS Development Studio (ROSDS) —▸ http://rosds.online
3. Robot Ignite Academy –▸ https://www.robotigniteacademy.com
4. ROS2 Tutorials –▸
   1. ROS2 Basics in 5 Days (C++): https://app.theconstructsim.com/#/Course/61
   2. ROS2 Navigation: https://app.theconstructsim.com/#/Course/50
   3. Get ROS2 Industrial Ready 3-Day Online Workshop

Creating a rosject

In order to learn how work with ros2 depth camera data, we need to have ROS installed.We already prepared a rosject ready for that: https://app.theconstructsim.com/#/l/467026ab/

If you want to create your own rosject instead of using the link we just provided, you can do it also. We are going to use The Construct (https://www.theconstruct.ai/) for this tutorial, but if you have ROS2 installed on your own computer, you should be able to do ~everything on your own computer, except this creating a rosject part.

Let’s start by opening The Construct (https://www.theconstruct.ai/) and logging in. You can easily create a free account if you still don’t have one.

Once inside, let’s create My Rosjects and then, Create a new rosject if you decided decided taking this route:

 

For the rosject, let’s select ROS2 Foxy for the ROS Distro, let’s name the rosject as you want. You can leave the rosject public. You should see the rosject you just created in your rosjects list (the name is certainly different from the example below that was added just for learning purposes)

If you mouse over the recently created rosject, you should see a Run button. Just click that button to launch the rosject.

Starting a simulation

In order to launch a simulation, let’s start by opening a new terminal:

We can actually open 3 terminals (just by clicking the plus “+” button that appears besides the number #569 in the image above).

At the moment there are not a lot of simulations that include depth cameras for ros2, therefore, we are going to use ros1 to launch a simulation, then use ros1_bridge to make the topics available in ros2.

After having the terminals open, let’s source ros1 noetic in the first terminal, and also source the workspace that contains the simulation:

source /opt/ros/noetic/setup.bash

source simulation_ws/devel/setup.bash

We can now launch the simulation with:

 roslaunch ur_e_gazebo ur3e.launch

The simulation should have been launched. You can ignore some error messages that appear for the moment. The logs should be something similar to the following:

Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
xacro: in-order processing became default in ROS Melodic. You can drop the option.
started roslaunch server http://4_xterm:32787/
SUMMARY
========
PARAMETERS
 * /gazebo/enable_ros_network: True
 * /gripper_controller/joint: egh_gripper_finge...
 * /gripper_controller/type: position_controll...
 * /joint_group_position_controller/joints: ['shoulder_pan_jo...
 * /joint_group_position_controller/type: position_controll...
 * /joint_state_controller/publish_rate: 50
 * /joint_state_controller/type: joint_state_contr...
 * /robot_description: <?xml version="1....
 * /robot_state_publisher/publish_frequency: 50.0
 * /robot_state_publisher/tf_prefix:
 * /rosdistro: noetic
 * /rosversion: 1.15.11
 * /scaled_pos_joint_traj_controller/action_monitor_rate: 10
 * /scaled_pos_joint_traj_controller/constraints/elbow_joint/goal: 0.1
 * /scaled_pos_joint_traj_controller/constraints/elbow_joint/trajectory: 0.1
 * /scaled_pos_joint_traj_controller/constraints/goal_time: 0.6
 * /scaled_pos_joint_traj_controller/constraints/shoulder_lift_joint/goal: 0.1
 * /scaled_pos_joint_traj_controller/constraints/shoulder_lift_joint/trajectory: 0.1
 * /scaled_pos_joint_traj_controller/constraints/shoulder_pan_joint/goal: 0.1
 * /scaled_pos_joint_traj_controller/constraints/shoulder_pan_joint/trajectory: 0.1
 * /scaled_pos_joint_traj_controller/constraints/stopped_velocity_tolerance: 0.05
 * /scaled_pos_joint_traj_controller/constraints/wrist_1_joint/goal: 0.1
 * /scaled_pos_joint_traj_controller/constraints/wrist_1_joint/trajectory: 0.1
 * /scaled_pos_joint_traj_controller/constraints/wrist_2_joint/goal: 0.1
 * /scaled_pos_joint_traj_controller/constraints/wrist_2_joint/trajectory: 0.1
 * /scaled_pos_joint_traj_controller/constraints/wrist_3_joint/goal: 0.1
 * /scaled_pos_joint_traj_controller/constraints/wrist_3_joint/trajectory: 0.1
 * /scaled_pos_joint_traj_controller/joints: ['shoulder_pan_jo...
 * /scaled_pos_joint_traj_controller/state_publish_rate: 25
 * /scaled_pos_joint_traj_controller/stop_trajectory_duration: 0.5
 * /scaled_pos_joint_traj_controller/type: position_controll...
 * /use_sim_time: True
NODES
  /
    arm_controller_spawner (controller_manager/controller_manager)
    controller_spawner_gripper (controller_manager/spawner)
    fake_joint_calibration (rostopic/rostopic)
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    joint_state_controller_spawner (controller_manager/controller_manager)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    ros_control_controller_manager (controller_manager/controller_manager)
    spawn_gazebo_model (gazebo_ros/spawn_model)
auto-starting new master
process[master]: started with pid [3700]
ROS_MASTER_URI=http://4_xterm:11311
setting /run_id to 0e52b568-6cd6-11ec-9dc4-0242ac1b0007
process[rosout-1]: started with pid [3719]
started core service [/rosout]
process[gazebo-2]: started with pid [3722]
process[gazebo_gui-3]: started with pid [3724]
process[spawn_gazebo_model-4]: started with pid [3729]
process[robot_state_publisher-5]: started with pid [3733]
process[fake_joint_calibration-6]: started with pid [3741]
process[joint_state_controller_spawner-7]: started with pid [3742]
process[arm_controller_spawner-8]: started with pid [3747]
process[controller_spawner_gripper-9]: started with pid [3748]
process[ros_control_controller_manager-10]: started with pid [3749]
++ ls /usr/bin/gzclient-11.5.1
+ gzclient_path=/usr/bin/gzclient-11.5.1
+ DISPLAY=:2
+ /usr/bin/gzclient-11.5.1 -g /opt/ros/noetic/lib/libgazebo_ros_paths_plugin.so -g /opt/ros/noetic/lib/libgazebo_ros_api_plugin.so __name:=gazebo_gui __log:=/home/user/.ros/log/0e52b568-6cd6-11ec-9dc4-0242ac1b0007/gazebo_gui-3.log
INFO: cannot create a symlink to latest log directory: [Errno 2] No such file or directory: '/home/user/.ros/log/latest'
[INFO] [1641242725.661746, 0.000000]: Controller Spawner: Waiting for service controller_manager/load_controller
[INFO] [1641242725.761337, 0.000000]: Loading model XML from ros parameter robot_description
[INFO] [1641242725.768781, 0.000000]: Waiting for service /gazebo/spawn_urdf_model
Gazebo multi-robot simulator, version 11.5.1
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org
[ INFO] [1641242726.627685197]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1641242726.629782039]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://172.27.0.7:11345
[Msg] Publicized address: 172.27.0.7
[ INFO] [1641242726.738771830]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1641242726.742342582]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
[Msg] Loading world file [/usr/share/gazebo-11/worlds/empty.world]
[ INFO] [1641242727.138178149]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1641242727.244679729, 0.048000000]: Physics dynamic reconfigure ready.
[INFO] [1641242727.280947, 0.088000]: Calling service /gazebo/spawn_urdf_model
[INFO] [1641242727.537885, 0.104000]: Spawn status: SpawnModel: Successfully spawned entity
[spawn_gazebo_model-4] process has finished cleanly
log file: /home/user/.ros/log/0e52b568-6cd6-11ec-9dc4-0242ac1b0007/spawn_gazebo_model-4*.log
[ INFO] [1641242729.504743470, 0.104000000]: Camera Plugin: Using the 'robotNamespace' param: '/'
[ INFO] [1641242729.513059783, 0.104000000]: Camera Plugin (ns = /)  <tf_prefix_>, set to ""
[ INFO] [1641242730.186888511, 0.104000000]: Loading gazebo_ros_control plugin
[ INFO] [1641242730.187188566, 0.104000000]: Starting gazebo_ros_control plugin in namespace: /
[ INFO] [1641242730.188738334, 0.104000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [robot_description] on the ROS param server.
[ERROR] [1641242730.480595227, 0.104000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/shoulder_pan_joint
[ERROR] [1641242730.482573352, 0.104000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/shoulder_lift_joint
[ERROR] [1641242730.483960993, 0.104000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/elbow_joint
[ERROR] [1641242730.485373065, 0.104000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/wrist_1_joint
[ERROR] [1641242730.486771098, 0.104000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/wrist_2_joint
[ERROR] [1641242730.488209294, 0.104000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/wrist_3_joint
[ERROR] [1641242730.489539274, 0.104000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/egh_gripper_finger_left_joint
[ INFO] [1641242730.503064828, 0.104000000]: Loaded gazebo_ros_control.
[ INFO] [1641242730.514695030, 0.104000000]: MimicJointPlugin loaded! Joint: "egh_gripper_finger_left_joint", Mimic joint: "egh_gripper_finger_right_joint", Multiplier: 1, Offset: 0, MaxEffort: 200, Sensitiveness: 0
[Msg] Loading grasp-fix plugin
[Msg] GazeboGraspFix: Using disable_collisions_on_attach 0
[Msg] GazeboGraspFix: Using update rate 10
[Msg] GazeboGraspFix: Using max_grip_count 10
[Msg] GazeboGraspFix: Using grip_count_threshold 3
[Msg] GazeboGraspFix: Using release_tolerance 0.001
[Msg] GazeboGraspFix: Adding collision scoped name robot::egh_gripper_left_finger::egh_gripper_left_finger_collision
[Msg] GazeboGraspFix: Adding collision scoped name robot::egh_gripper_right_finger::egh_gripper_right_finger_collision
[Msg] Subscribing contact manager to topic ~/robot/contacts
[Msg] Advertising grasping events on topic grasp_events
[Wrn] [Publisher.cc:136] Queue limit reached for topic /gazebo/default/pose/local/info, deleting message. This warning is printed only once.
[INFO] [1641242730.834305, 0.401000]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1641242730.842565, 0.415000]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1641242730.846773, 0.422000]: Loading controller: gripper_controller
Loaded 'joint_state_controller'
[ERROR] [1641242731.068522252, 0.634000000]: Could not load controller 'arm_controller' because the type was not specified. Did you load the controller configuration on the parameter server (namespace: '/arm_controller')?
Error when loading 'arm_controller'
Loaded 'joint_group_position_controller'
[INFO] [1641242731.563780, 1.066000]: Controller Spawner: Loaded controllers: gripper_controller
[ERROR] [1641242731.564298128, 1.067000000]: Could not start controller with name 'arm_controller' because no controller with this name exists
Started ['joint_state_controller'] successfully
Error when starting ['arm_controller'] and stopping []
[INFO] [1641242731.573472, 1.076000]: Started controllers: gripper_controller

 

You can now click Open Gazebo to see the simulation:

We are most interested in the depth camera that we can see inside a red box in the image above.

That camera has a normal rgb camera and a depth camera that can be used to detect distance.

Launching ros1_bridge

Now that we have the simulation running in ros1, let’s start ros1_bridge so that we can interact with the topics using ros2.

For that, let’s run the following command (in this order, first ros1, then ros2) in the second terminal to source ros:

source /opt/ros/noetic/setup.bash

source /opt/ros/foxy/setup.bash

We can now launch ros1_bridge with:

ros2 run ros1_bridge dynamic_bridge --bridge-all-topics

The output should be similar to:

created 1to2 bridge for topic '/calibrated' with ROS 1 type 'std_msgs/Bool' and ROS 2type 'std_msgs/msg/Bool'
created 1to2 bridge for topic '/clock' with ROS 1 type 'rosgraph_msgs/Clock' and ROS 2 type 'rosgraph_msgs/msg/Clock'
created 1to2 bridge for topic '/gazebo/link_states' with ROS 1 type 'gazebo_msgs/LinkStates' and ROS 2 type 'gazebo_msgs/msg/LinkStates'
created 1to2 bridge for topic '/gazebo/model_states' with ROS 1 type 'gazebo_msgs/ModelStates' and ROS 2 type 'gazebo_msgs/msg/ModelStates'
created 1to2 bridge for topic '/gripper_controller/gripper_cmd/status' with ROS 1 type 'actionlib_msgs/GoalStatusArray' and ROS 2 type 'actionlib_msgs/msg/GoalStatusArray'
created 1to2 bridge for topic '/joint_states' with ROS 1 type 'sensor_msgs/JointState' and ROS 2 type 'sensor_msgs/msg/JointState'
created 1to2 bridge for topic '/rosout' with ROS 1 type 'rosgraph_msgs/Log' and ROS 2type 'rcl_interfaces/msg/Log'
created 1to2 bridge for topic '/rosout_agg' with ROS 1 type 'rosgraph_msgs/Log' and ROS 2 type 'rcl_interfaces/msg/Log'
created 1to2 bridge for topic '/tf' with ROS 1 type 'tf2_msgs/TFMessage' and ROS 2 type 'tf2_msgs/msg/TFMessage'
created 1to2 bridge for topic '/tf_static' with ROS 1 type 'tf2_msgs/TFMessage' and ROS 2 type 'tf2_msgs/msg/TFMessage'
created 1to2 bridge for topic '/wrist_rgbd/depth/camera_info' with ROS 1 type 'sensor_msgs/CameraInfo' and ROS 2 type 'sensor_msgs/msg/CameraInfo'
created 1to2 bridge for topic '/wrist_rgbd/depth/image_raw' with ROS 1 type 'sensor_msgs/Image' and ROS 2 type 'sensor_msgs/msg/Image'
created 1to2 bridge for topic '/wrist_rgbd/depth/points' with ROS 1 type 'sensor_msgs/PointCloud2' and ROS 2 type 'sensor_msgs/msg/PointCloud2'
created 1to2 bridge for topic '/wrist_rgbd/rgb/camera_info' with ROS 1 type 'sensor_msgs/CameraInfo' and ROS 2 type 'sensor_msgs/msg/CameraInfo'
created 1to2 bridge for topic '/wrist_rgbd/rgb/image_raw' with ROS 1 type 'sensor_msgs/Image' and ROS 2 type 'sensor_msgs/msg/Image'
created 1to2 bridge for topic '/wrist_rgbd/rgb/image_raw/compressed' with ROS 1 type 'sensor_msgs/CompressedImage' and ROS 2 type 'sensor_msgs/msg/CompressedImage'
created 1to2 bridge for topic '/wrist_rgbd/rgb/image_raw/compressedDepth' with ROS 1 type 'sensor_msgs/CompressedImage' and ROS 2 type 'sensor_msgs/msg/CompressedImage'
Created 2 to 1 bridge for service /gazebo/clear_body_wrenches
Created 2 to 1 bridge for service /gazebo/clear_joint_forces
Created 2 to 1 bridge for service /gazebo/delete_light
Created 2 to 1 bridge for service /gazebo/delete_model
Created 2 to 1 bridge for service /gazebo/get_joint_properties
Created 2 to 1 bridge for service /gazebo/get_light_properties
Created 2 to 1 bridge for service /gazebo/get_link_properties
Created 2 to 1 bridge for service /gazebo/get_link_state
Created 2 to 1 bridge for service /gazebo/get_model_properties
Created 2 to 1 bridge for service /gazebo/get_model_state
Created 2 to 1 bridge for service /gazebo/get_physics_properties
Created 2 to 1 bridge for service /gazebo/get_world_properties
Created 2 to 1 bridge for service /gazebo/pause_physics
Created 2 to 1 bridge for service /gazebo/reset_simulation
Created 2 to 1 bridge for service /gazebo/reset_world
Created 2 to 1 bridge for service /gazebo/set_joint_properties
Created 2 to 1 bridge for service /gazebo/set_link_properties
Created 2 to 1 bridge for service /gazebo/set_link_state
Created 2 to 1 bridge for service /gazebo/set_model_configuration
Created 2 to 1 bridge for service /gazebo/set_model_state
Created 2 to 1 bridge for service /gazebo/set_physics_properties
Created 2 to 1 bridge for service /gazebo/spawn_sdf_model
Created 2 to 1 bridge for service /gazebo/spawn_urdf_model
Created 2 to 1 bridge for service /gazebo/unpause_physics
Created 2 to 1 bridge for service /wrist_rgbd/set_camera_info
[INFO] [1641243318.565086017] [ros_bridge]: Passing message from ROS 1 std_msgs/Bool to ROS 2 std_msgs/msg/Bool (showing msg only once per type)
[INFO] [1641243318.565407501] [ros_bridge]: Passing message from ROS 1 rosgraph_msgs/Clock to ROS 2 rosgraph_msgs/msg/Clock (showing msg only once per type)
[INFO] [1641243318.634712568] [ros_bridge]: Passing message from ROS 1 gazebo_msgs/LinkStates to ROS 2 gazebo_msgs/msg/LinkStates (showing msg only once per type)
[INFO] [1641243318.636128359] [ros_bridge]: Passing message from ROS 1 gazebo_msgs/ModelStates to ROS 2 gazebo_msgs/msg/ModelStates (showing msg only once per type)
[INFO] [1641243318.636843101] [ros_bridge]: Passing message from ROS 1 actionlib_msgs/GoalStatusArray to ROS 2 actionlib_msgs/msg/GoalStatusArray (showing msg only once per type)
[INFO] [1641243318.637282514] [ros_bridge]: Passing message from ROS 1 sensor_msgs/JointState to ROS 2 sensor_msgs/msg/JointState (showing msg only once per type)
[INFO] [1641243318.637751048] [ros_bridge]: Passing message from ROS 1 rosgraph_msgs/Log to ROS 2 rcl_interfaces/msg/Log (showing msg only once per type)
[INFO] [1641243318.638771467] [ros_bridge]: Passing message from ROS 1 tf2_msgs/TFMessage to ROS 2 tf2_msgs/msg/TFMessage (showing msg only once per type)
created 2to1 bridge for topic '/calibrated' with ROS 2 type 'std_msgs/msg/Bool' and ROS 1 type 'std_msgs/Bool'
created 2to1 bridge for topic '/rosout' with ROS 2 type 'rcl_interfaces/msg/Log' and ROS 1 type 'rosgraph_msgs/Log'
removed 2to1 bridge for topic '/calibrated'
[INFO] [1641243320.970126882] [ros_bridge]: Passing message from ROS 1 sensor_msgs/Image to ROS 2 sensor_msgs/msg/Image (showing msg only once per type)
[INFO] [1641243320.971249839] [ros_bridge]: Passing message from ROS 1 sensor_msgs/CameraInfo to ROS 2 sensor_msgs/msg/CameraInfo (showing msg only once per type)
[INFO] [1641243321.032322768] [ros_bridge]: Passing message from ROS 2 rcl_interfaces/msg/Log to ROS 1 rosgraph_msgs/Log (showing msg only once per type)
[INFO] [1641243321.057171155] [ros_bridge]: Passing message from ROS 1 sensor_msgs/PointCloud2 to ROS 2 sensor_msgs/msg/PointCloud2 (showing msg only once per type)
[INFO] [1641243321.136791384] [ros_bridge]: Passing message from ROS 1 sensor_msgs/CompressedImage to ROS 2 sensor_msgs/msg/CompressedImage (showing msg only once per type)

If we now go to the third terminal, we can list the topics in ros2 with:

ros2 topic list

which would show something like this:

/calibrated
/clock
/gazebo/link_states
/gazebo/model_states
/gripper_controller/gripper_cmd/status
/joint_states
/parameter_events
/rosout
/rosout_agg
/tf
/tf_static
/wrist_rgbd/depth/camera_info
/wrist_rgbd/depth/image_raw
/wrist_rgbd/depth/points
/wrist_rgbd/rgb/camera_info
/wrist_rgbd/rgb/image_raw
/wrist_rgbd/rgb/image_raw/compressed
/wrist_rgbd/rgb/image_raw/compressedDepth

The topics we are most interested in are the ones that start with /wrist_rgbd:

/wrist_rgbd/depth/camera_info
/wrist_rgbd/depth/image_raw
/wrist_rgbd/depth/points
/wrist_rgbd/rgb/camera_info
/wrist_rgbd/rgb/image_raw
/wrist_rgbd/rgb/image_raw/compressed
/wrist_rgbd/rgb/image_raw/compressedDepth

Some important notes about the topics above.

• The /wrist_rgbd/depth/camera_info topic does not show any distance of any object. Is just shows the configurations of the camera.
• The /wrist_rgbd/rgb/image_raw just shows an array of pixels that just represent the colors that we can see in an image. It does not contain any distance information either.
• The topic we are really interested in this one: /wrist_rgbd/depth/points

Let’s open rviz2 in a fourth terminal so that we can understand things better by visualizing data like the depth information of the camera:

rviz2

Rviz is a graphical tool, so, in order to see it, you have to click the Graphical tools button.

Now that rviz is open, let’s change the Fixed Frame to wrist_rgbd_camera_link

After that, let’s click the Add button that appears on the bottom left, and click the By Topic tab, and select /wrist_rgbd/depth/points

You should now be able to see the depth info visually:

The points you see in the last image above actually tell you the distance of the arm.

By checking the values that are sent in the /wrist_rgbd/depth/points topic you can easily determine how far an object is.

That is pretty much how you would use a depth camera to determine the distance of an object.

Youtube video

So this is the post for today. Remember that we have the live version of this post on YouTube. If you liked the content, please consider subscribing to our youtube channel. We are publishing new content ~every day.

Keep pushing your ROS Learning.