Perception Configuration

View Package on GitHub

Overview

This package contains the necessary config and launch files to get any of the many Interbotix X-Series arms working with any of the Interbotix perception pipelines. The end result allows for an arm to pick up any small, non-reflective object from a tabletop-type surface that is within a RealSense color/depth camera’s field of view. While any Intel RealSense color/depth camera can be used, this package was mainly tested with the D415 camera. See more details on how the pipeline works in the interbotix_perception_modules ROS package. To purchase our vision kit (which includes a D415 camera and stand), visit its store page.

Structure

../_images/xsarm_perception_flowchart_ros2.png

As shown above, this package builds on top of the interbotix_xsarm_control and interbotix_perception_modules packages. To get familiar with those packages, please refer to their documentation. The other nodes are described below:

  • realsense2_camera_manager - nodelet manager to manage the camera nodelet below
  • RealSenseNodeFactory - nodelet to create a ROS interface for the RealSense camera

Usage

To work with this package, first build your stand and secure the RealSense camera on the 1/4 inch screw at the top. Then place the stand in your work area and adjust the goose-neck or ball/socket joint such that the camera is pointed towards your tabletop. Next, place some small, non-reflective objects on the tabletop such that they are clearly visible to the camera and are reachable by the arm. The objects should be small enough such that they can easily fit between the gripper fingers on the robot arm no matter the gripper’s orientation. They can not be too reflective though as that will interfere with the depth camera’s ability to locate them through its use of infrared light. Similarly, the workspace should not be in direct sunlight as that also interferes with the camera’s depth sensing abilities. Otherwise, the arm, camera, and small objects can be arbitrarily placed. For the pick and place demo, you should setup your workspace area as shown below.

../_images/pick_place_demo_setup.png

Before the arm picks up any object, two things must be done. First, the camera must know where the arm is relative to itself. One way of doing this is by manually measuring the offset from the camera’s color optical frame to the robot’s base_link frame and publishing it as a static transform. However, this is time consuming and prone to error. Instead, the apriltag_ros ROS2 package is used to find the transform of the AprilTag visual fiducial marker on the arm’s end-effector relative to the camera’s color optical frame. Following this, the transform from the camera’s color optical frame to the arm’s base_link frame is calculated and published as a static transform.

To get that transform, run the following launch command in a terminal (assuming a WidowX-200 arm is being used)…

$ ros2 launch interbotix_xsarm_perception xsarm_perception.launch.py robot_model:=wx200 use_pointcloud_tuner_gui:=true use_armtag_tuner_gui:=true

RViz should pop up along with two standalone GUIs. One of those GUIs will look like the picture below.

../_images/armtag_tuner_gui.png

Depending on how you setup your arm and camera in your workspace, the AprilTag on the arm may not be visible to the camera. To make it visible, first torque off all the arm joints by opening a terminal and typing…

$ ros2 service call /wx200/torque_enable interbotix_xs_msgs/srv/TorqueEnable "{cmd_type: 'group', name: 'arm', enable: false}"

Next, manually manipulate the arm such that the AprilTag is clearly visible to the camera (the live video stream in the bottom left of the RViz display should help with that). Then in the same terminal as before, torque the arm back on as follows…

$ ros2 service call /wx200/torque_enable interbotix_xs_msgs/srv/TorqueEnable "{cmd_type: 'group', name: 'arm', enable: true}"

Now, in the Armtag Tuner GUI, click the ‘Snap Pose’ button. Feel free to toggle up/down the number of snapshots that should be taken. The poses calculated from the snapshots will then be averaged to come up with a more accurate pose of where the arm is relative to the camera. One way to check the accuracy of the calculated pose is to toggle the RawPointCloud display in RViz. Hopefully, the pointcloud version of the AprilTag should be located on (possibly a couple millimeters below) the AR tag link of the virtual robot model. If it’s not, feel free to keep pressing the ‘Snap Pose’ button until it looks alright.

At this point, you should see a pointcloud version of your tabletop with the objects on it. If your arm is in the way, just torque it off and move it to its Sleep pose (make sure to hold the arm before torquing it off). Then, using the PointCloud Tuner GUI, tune the pointcloud parameters for your specific use case. See this guide for how to go about doing this. Don’t forget to save your configs after tuning them!

Now you should be able to successfully run the pick and place demo script. First make sure to edit the robot name in the script to your robot model (if it’s not the WidowX-200 arm). You can also comment out line 76 that deals with getting the pose of the arm relative to the camera. As you’ve already done this above, there’s no need to do it again. However, the code is there just in case you’d rather snap the AR tag pose in a script rather than with the GUI.

While running the script, you should see a TF marker appear close to the top of each object’s cluster (see the image below for clarification). This is where the camera believes the top of each cluster to be, and is the position returned to the user from the get_cluster_positions function. These TFs are temporary and will fade from RViz after a minute is up. The arm will then line up its ee_gripper_link to be in the same spot as each of these cluster positions and hopefully pick up the objects.

../_images/object_cluster_tf.png

After running the demo, Ctrl-C from the launch file. The ‘camera to arm base_link’ transform will automatically be saved in a file called ‘static_transforms.yaml’ in the config directory. Now, you can run the demo script headless - first by running the command below.

$ ros2 launch interbotix_xsarm_perception xsarm_perception.launch.py robot_model:=wx200 use_rviz:=false

Then head over to the demos directory and running the pick and place demo script.

For more info, check out the Armtag or Pointcloud Python APIs to reference the fully documented functions.

Other launch file arguments for further customization can be seen below…

Argument Description Default Choices
robot_model One of: (‘px100’, ‘px150’, ‘rx150’, ‘rx200’, ‘wx200’, ‘wx250’, ‘wx250s’, ‘vx250’, ‘vx300’, ‘vx300s’, ‘mobile_px100’, ‘mobile_wx200’, ‘mobile_wx250s’)    
robot_name name of the robot (typically equal to robot_model, but could be anything) LaunchConfig(robot_model)  
base_link_frame name of the ‘root’ link on the arm; typically base_link, but can be changed if attaching the arm to a mobile base that already has a base_link frame. base_link  
use_gripper if true, the default gripper is included in the robot_description parameter; if false, it is left out; set to false if not using the default gripper. true true, false
show_ar_tag if true, the AR tag mount is included in the robot_description parameter; if false, it is left out; set to true if using the AR tag mount in your project. true true, false
show_gripper_bar if true, the gripper_bar link is included in the robot_description parameter; if false, the gripper_bar and finger links are not loaded. Set to false if you have a custom gripper attachment. true true, false
show_gripper_fingers if true, the gripper fingers are included in the robot_description parameter; if false, the gripper finger links are not loaded. Set to false if you have custom gripper fingers. true true, false
use_world_frame set this to true if you would like to load a ‘world’ frame to the robot_description parameter which is located exactly at the ‘base_link’ frame of the robot; if using multiple robots or if you would like to attach the ‘base_link’ frame of the robot to a different frame, set this to false. false true, false
external_urdf_loc the file path to the custom urdf.xacro file that you would like to include in the Interbotix robot’s urdf.xacro file. ‘’  
hardware_type configures the robot_description parameter to use the actual hardware, fake hardware, or hardware simulated in Gazebo. actual actual, fake, gz_classic
robot_description URDF of the robot; this is typically generated by the xacro command. Command(FindExec(xacro) + ‘ ‘ + LocalVar(‘FindPackageShare(pkg= interbotix_xsarm_descriptions) + ‘urdf’ + LaunchConfig(robot_model)’) + ‘.urdf.xacro ‘ + ‘robot_name:=’ + LaunchConfig(robot_name) + ‘ ‘ + ‘base_link_frame:=’ + LaunchConfig(base_link_frame) + ‘ ‘ + ‘use_gripper:=’ + LaunchConfig(use_gripper) + ‘ ‘ + ‘show_ar_tag:=’ + LaunchConfig(show_ar_tag) + ‘ ‘ + ‘show_gripper_bar:=’ + LaunchConfig(show_gripper_bar) + ‘ ‘ + ‘show_gripper_fingers:=’ + LaunchConfig(show_gripper_fingers) + ‘ ‘ + ‘use_world_frame:=’ + LaunchConfig(use_world_frame) + ‘ ‘ + ‘external_urdf_loc:=’ + LaunchConfig(external_urdf_loc) + ‘ ‘ + ‘hardware_type:=’ + LaunchConfig(hardware_type) + ‘ ‘)  
load_configs a boolean that specifies whether or not the initial register values (under the ‘motors’ heading) in a Motor Config file should be written to the motors; as the values being written are stored in each motor’s EEPROM (which means the values are retained even after a power cycle), this can be set to false after the first time using the robot. Setting to false also shortens the node startup time by a few seconds and preserves the life of the EEPROM true true, false
rs_camera_pointcloud_enable enables the RealSense camera’s pointcloud. true true, false
rbg_camera_profile profile for the rbg camera image stream, in <width>x<height>x<fps> 640x480x30  
depth_module_profile profile for the depth module stream, in <width>x<height>x<fps> 640x480x30  
rs_camera_logging_level set the logging level for the realsense2_camera launch include info debug, info, warn, error, fatal
rs_camera_output_location set the logging location for the realsense2_camera launch include screen screen, log
rs_camera_initial_reset On occasions the RealSense camera is not closed properly and due to firmware issues needs to reset. If set to true, the device will reset prior to usage. false true, false
filter_ns namespace where the pointcloud related nodes and parameters are located pc_filter  
filter_params file location of the parameters used to tune the perception pipeline filters LocalVar(‘FindPackageShare(pkg= interbotix_xsarm_perception) + ‘config’ + ‘filter_params.yaml’’)  
use_pointcloud_tuner_gui whether to show a GUI that a user can use to tune filter parameters false true, false
enable_pipeline whether to enable the perception pipeline filters to run continuously; to save computer processing power, this should be set to false unless you are actively trying to tune the filter parameters; if false, the pipeline will only run if the get_cluster_positions ROS service is called. LaunchConfig(use_pointcloud_tuner_gui) true, false
cloud_topic the absolute ROS topic name to subscribe to raw pointcloud data ‘/camera/depth/color/points’  
tags_config parameter file location for the AprilTag configuration LocalVar(‘FindPackageShare(pkg= interbotix_perception_modules) + ‘config’ + ‘tags.yaml’’)  
camera_frame the camera frame in which the AprilTag will be detected camera_color_optical_frame  
apriltag_ns namespace where the AprilTag related nodes and parameters are located apriltag  
camera_color_topic the absolute ROS topic name to subscribe to color images ‘camera/color/image_raw’  
camera_info_topic the absolute ROS topic name to subscribe to the camera color info ‘camera/color/camera_info’  
armtag_ns name-space where the Armtag related nodes and parameters are located armtag  
ref_frame the reference frame that the armtag node should use when publishing a static transform for where the arm is relative to the camera LaunchConfig(camera_frame)  
arm_base_frame the child frame that the armtag node should use when publishing a static transform for where the arm is relative to the camera LaunchConfig(robot_name) + ‘/’ + LaunchConfig(base_link_frame)  
arm_tag_frame name of the frame on the arm where the AprilTag is located (typically defined in the URDF) LaunchConfig(robot_name) + ‘/’ + ‘ar_tag_link’  
use_armtag_tuner_gui whether to show a GUI that a user can use to publish the ref_frame to arm_base_frame transform false true, false
position_only whether only the position component of the detected AprilTag pose should be used when calculating the ref_frame to arm_base_frame transform; this should only be set to true if a TF chain already exists connecting the camera and arm base_link frame, and you just want to use the AprilTag to refine the pose further false true, false
load_transforms whether or not the static_trans_pub node should publish any poses stored in the static_transforms.yaml file at startup; this should only be set to false if a TF chain already exists connecting the camera and arm base_link frame (typically defined in a URDF), and you would rather use that TF chain as opposed to the one specified in the static_transforms.yaml file true true, false
transform_filepath filepath to the static_transforms.yaml file used by the static_trans_pub node; if the file does not exist yet, this is where you wouldd like the file to be generated LocalVar(‘FindPackageShare(pkg= interbotix_xsarm_perception) + ‘config’ + ‘static_transforms.yaml’’)  
use_rviz launches RViz if set to true true true, false
rviz_frame desired fixed frame in RViz LaunchConfig(robot_name) + ‘/’ + LaunchConfig(base_link_frame)  
rvizconfig filepath to the RViz config file LocalVar(‘FindPackageShare(pkg= interbotix_xsarm_perception) + ‘rviz’ + ‘xsarm_perception.rviz’’)  

Troubleshooting

Below are some common error messages, warnings, or issues you might see, and how to go about fixing them.

ArmTag Issues

Could not find AR Tag. Returning a ‘zero’ Pose…

This warning occurs if the camera cannot see the AprilTag or if the wrong AprilTag is present. To fix, make sure that the AprilTag is clearly visible to the camera when you try to ‘snap’ its pose.

PointCloud Filter Tuning Issues

Incomplete bulk usb transfer!

This is just a onetime error message that appears at launch when using the RealSense depth camera camera. It’s nothing to be worried about and can be safely ignored.

No stream match for pointcloud chosen texture Process - Color

This is a warning that can appear occasionally (once every 5 minutes or so) when using a RealSense camera. It just means that a frame was dropped during serial communication, but it’s nothing to worry about and can be safely ignored.

No clusters found…

This warning is outputted by the get_cluster_positions function in the InterbotixPointCloudInterface module if the algorithm could not find any clusters. Verify that you have non-reflective objects within the field of view of the camera and that the CropBox filter is not cutting them out. To check this, toggle on the ‘FilteredPointCloud’ display in RViz and see if the pointcloud representation of your objects are showing up. If they are, it’s possible that you need to lower the minimum cluster size threshold; turn on the ‘ObjectPointCloud’ and ‘ObjectMarkers’ displays and lower the Min Cluster Size parameter until you see small spheres at the centroid of each of your clusters.

Found ‘x’ clusters instead of ‘y’ clusters…

This warning is outputted by the get_cluster_positions function in the InterbotixPointCloudInterface module if the algorithm found a different number of clusters over ‘num_samples’ iterations (when compared to the first set of cluster positions received). Similar to the ‘No clusters found…’ issue, this can be resolved by tuning the Min Cluster Size parameter until the spherical object markers are steady and not flickering. This issue could also arise if the spherical object markers are flickering due to two clusters being very near each other (sometimes above or below the Cluster Tolerance threshold). To fix this, lower the cluster tolerance threshold or physically move the two objects such that they are further away from each other.

Could not match the cluster. Please tune the filter parameters such that all spherical ‘object markers’ are constant in their respective clusters and do not flicker

Most likely, you’ll never run into this issue; but if you do, the fixes suggested in the ‘Found x clusters instead of y clusters…’ issue should resolve the problem. The issue essentially means that the detected position of a cluster in a later iteration is vastly different than the detected position of the cluster in an earlier iteration (over ‘num_samples’ iterations). It could arise if the Object Markers are flickering, and it just happens that the same number number of clusters are found, but the clusters are in different places. If working with an arm on a Locobot, another fix is to give time (half a second or so) for the arm to settle before capturing the pointcloud data. This is because the motion of the arm can cause the Kobuki base to wobble a bit - making the camera move as well.