[Clearpath Husky A200] Integration of sensors such as Zed2i, Phidget IMU and Bpearl Lidar

Hi,

we’re looking to integrate several sensors with our Husky [S/N: a200_0930] running on a intel PC with ROS2 Humble and 22.04 Ubuntu as the main robot PC. We are still quite inexperienced as far as this is concerned. The sensors we want to integrate are:

- Zed2i
- PhidgetSpatial Precision 3/3/3 IMU
- RS-Bpearl 3D LiDAR

For the Zed2i we use a Nvidia Jetson AGX Orin in addition. The sensors have already been tested with their ROS2 drivers and work independently of the Husky. How would we integrate these sensors correctly into the Husky? Our first attempts to integrate these somehow via the Husky’s robot.yaml were not successful following these instructions here: Cameras | Clearpath Robotics Documentation
The zed2i cam for example is not included in the tf tree of the husky (it should, if working correctly, right?).
Is there possibly another way to do this? Phidget and Bpearl are also not officially supported by Clearpath according to Inertial Measurment Units | Clearpath Robotics Documentation and 3D Lidar | Clearpath Robotics Documentation but it seems that they can still be integrated somehow if you look at the great projects here in the forum!
Any help would be greatly appreciated!

Dear @TKL,

You just need to add a custom URDF to the clearpath → robot.yaml containing the camera and additional sensors. Once done it should be linked.

Yes, you just need to disable the clearpath_control localization file (ekf_localization) and launch a copy with phidgets configuration in your own ros2 launch file.

Hi Sohail,

thank you for your response!
I still have a few questions since we’ve never done this before. So I think we should start with the Zed2i first.

You just need to add a custom URDF to the clearpath → robot.yaml containing the camera and additional sensors. Once done it should be linked.

With that you mean we should add, for example, the Zed2i URDF to the “extras” section instead of the “camera” section (Cameras | Clearpath Robotics Documentation) in the robot.yaml according to this: Extras | Clearpath Robotics Documentation ?

Assuming we get this to work on the robot PC with the robot.yaml. How does the linking work if we launch our zed_wrapper from the Jetson?

Yes, you just need to disable the clearpath_control localization file (ekf_localization) and launch a copy with phidgets configuration in your own ros2 launch file.

I’m not sure how to do this. How do we disable the clearpath localization file i.e. where to find where it launches?

Yes the extras.

It should be no issue, just make sure the tf and tf_static etc. are namespaced.

It is opt/ros/humble/share/clearpath_control

Hallo Sohail,

since I have already written a program for object recognition with the ZED2i and now want to integrate it with our Husky, I have now taken over the topic from TKL. Currently I am only using the “Live Configuration” to view the Husky in Rviz. To integrate a ZED 2i camera I have adjusted the robot.yaml file as you described and as explained on

Extras | Clearpath Robotics Documentation’.

However, the camera is unfortunately not visible.

Here is the corresponding excerpt from the robot.yaml file:

.
.
.
platform:
battery:
model: ES20_12C
configuration: S2P1
controller: logitech
attachments:
- name: front_bumper
type: a200.bumper
parent: front_bumper_mount
- name: rear_bumper
type: a200.bumper
parent: rear_bumper_mount
- name: top_plate
type: a200.top_plate
- name: sensor_arch
type: a200.sensor_arch
extras:
urdf:
path: /home/ubuntu/ros2_ws/src/zed-ros2-wrapper/zed_wrapper/urdf/zed_macro.urdf.xacro
.
.
.

I have unfortunately not worked with Husky yet and can’t find a solution to the problem. I appreciate any help.

Dear @SAL,

In the zed_macro.urdf.xacro, have you made a joint with any of the links in the Husky? If not create a joint say to the base_link or to the top_mount link. You can find the link names when you see the robot urdf in rviz.

Dear Sohail,
Thank you for your support. We have included the corresponding Husky Xacro file in zed_macro.urdf.xacro and connected Husky’s base_link (as a child) to the zed_base_link. After that, we started the zed_wrapper and the robot.yaml. However, we encountered an error related to transformation, which we believe may be connected to the tf and tf_static issue you mentioned earlier. Is that correct?

Yes that is correct. To help you can use this as a template:

# Copyright 2024 Stereolabs
#
# Licensed under the Apache License, Version 2.0 (the 'License');
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an 'AS IS' BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import os
import sys

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    OpaqueFunction,
    SetEnvironmentVariable,
    LogInfo
)
from launch.conditions import IfCondition
from launch.substitutions import (
    LaunchConfiguration,
    Command,
    TextSubstitution
)
from launch_ros.actions import Node

# ZED Configurations to be loaded by ZED Node
default_config_common = os.path.join(
    get_package_share_directory('nv_sensors'),
    'config',
    'zed2i_humble.yaml'
)

# URDF/xacro file to be loaded by the Robot State Publisher node
default_xacro_path = os.path.join(
    get_package_share_directory('zed_wrapper'),
    'urdf',
    'zed_descr.urdf.xacro'
)


def parse_array_param(param):
    str = param.replace('[', '')
    str = str.replace(']', '')
    arr = str.split(',')

    return arr


def launch_setup(context, *args, **kwargs):
    wrapper_dir = get_package_share_directory('zed_wrapper')

    # Launch configuration variables
    svo_path = LaunchConfiguration('svo_path')

    use_sim_time = LaunchConfiguration('use_sim_time')
    sim_mode = LaunchConfiguration('sim_mode')
    sim_address = LaunchConfiguration('sim_address')
    sim_port = LaunchConfiguration('sim_port')

    stream_address = LaunchConfiguration('stream_address')
    stream_port = LaunchConfiguration('stream_port')

    camera_name = LaunchConfiguration('camera_name')
    camera_model = LaunchConfiguration('camera_model')

    node_name = LaunchConfiguration('node_name')

    config_common_path = LaunchConfiguration('config_path')

    serial_number = LaunchConfiguration('serial_number')

    publish_urdf = LaunchConfiguration('publish_urdf')
    publish_tf = LaunchConfiguration('publish_tf')
    publish_map_tf = LaunchConfiguration('publish_map_tf')
    publish_imu_tf = LaunchConfiguration('publish_imu_tf')
    xacro_path = LaunchConfiguration('xacro_path')

    custom_baseline = LaunchConfiguration('custom_baseline')

    ros_params_override_path = LaunchConfiguration('ros_params_override_path')

    enable_gnss = LaunchConfiguration('enable_gnss')
    gnss_antenna_offset = LaunchConfiguration('gnss_antenna_offset')

    camera_name_val = camera_name.perform(context)
    camera_model_val = camera_model.perform(context)
    enable_gnss_val = enable_gnss.perform(context)
    gnss_coords = parse_array_param(gnss_antenna_offset.perform(context))
    custom_baseline_val = custom_baseline.perform(context)

    if (camera_name_val == ''):
        camera_name_val = 'zed'

    if (camera_model_val == 'virtual' and float(custom_baseline_val) <= 0):
        return [
            LogInfo(msg="Please set a positive value for the 'custom_baseline' argument when using a 'virtual' Stereo Camera with two ZED X One devices."),
        ]

    config_camera_path = os.path.join(
        get_package_share_directory('zed_wrapper'),
        'config',
        camera_model_val + '.yaml'
    )

    # Xacro command with options
    xacro_command = []
    xacro_command.append('xacro')
    xacro_command.append(' ')
    xacro_command.append(xacro_path.perform(context))
    xacro_command.append(' ')
    xacro_command.append('camera_name:=')
    xacro_command.append(camera_name_val)
    xacro_command.append(' ')
    xacro_command.append('camera_model:=')
    xacro_command.append(camera_model_val)
    xacro_command.append(' ')
    xacro_command.append('custom_baseline:=')
    xacro_command.append(custom_baseline_val)   
    if(enable_gnss_val=='true'):
        xacro_command.append(' ')
        xacro_command.append('enable_gnss:=true')
        xacro_command.append(' ')
        if(len(gnss_coords)==3):
            xacro_command.append('gnss_x:=')
            xacro_command.append(gnss_coords[0])
            xacro_command.append(' ')
            xacro_command.append('gnss_y:=')
            xacro_command.append(gnss_coords[1])
            xacro_command.append(' ')
            xacro_command.append('gnss_z:=')
            xacro_command.append(gnss_coords[2])
            xacro_command.append(' ')

    # Robot State Publisher node
    rsp_node = Node(
        condition=IfCondition(publish_urdf),
        package='robot_state_publisher',
        namespace=camera_name_val,
        executable='robot_state_publisher',
        name='zed_state_publisher',
        output='screen',
        parameters=[{
            'robot_description': Command(xacro_command)
        }],
        remappings=[
              ('~/robot_description', '/a200_1058/robot_description'),
              ('joint_states', '/a200_1058/platform/joint_states'),
              ('dynamic_joint_states', '/a200_1058/platform/dynamic_joint_states'),
              ('/diagnostics', '/a200_1058/diagnostics'),
              ('/tf', 'tf'),
              ('/tf_static', 'tf_static'),
            ],
    )

    node_parameters = [
            # YAML files
            config_common_path,  # Common parameters
            config_camera_path,  # Camera related parameters
            # Overriding
            {
                'use_sim_time': use_sim_time,
                'simulation.sim_enabled': sim_mode,
                'simulation.sim_address': sim_address,
                'simulation.sim_port': sim_port,
                'stream.stream_address': stream_address,
                'stream.stream_port': stream_port,
                'general.camera_name': camera_name_val,
                'general.camera_model': camera_model_val,
                'svo.svo_path': svo_path,
                'general.serial_number': serial_number,
                'pos_tracking.publish_tf': publish_tf,
                'pos_tracking.publish_map_tf': publish_map_tf,
                'sensors.publish_imu_tf': publish_imu_tf,
                'gnss_fusion.gnss_fusion_enabled': enable_gnss
            }
        ]
    
    if( ros_params_override_path.perform(context) != ''):
        node_parameters.append(ros_params_override_path)

    # ZED Wrapper node
    zed_wrapper_node = Node(
        package='zed_wrapper',
        namespace=camera_name_val,
        executable='zed_wrapper',
        name=node_name,
        output='screen',
        # prefix=['xterm -e valgrind --tools=callgrind'],
        # prefix=['xterm -e gdb -ex run --args'],
        #prefix=['gdbserver localhost:3000'],
        parameters=node_parameters,
        remappings=[
              ('~/robot_description', '/a200_1058/robot_description'),
              ('joint_states', '/a200_1058/platform/joint_states'),
              ('dynamic_joint_states', '/a200_1058/platform/dynamic_joint_states'),
              ('/diagnostics', '/a200_1058/diagnostics'),
              ('/tf', 'tf'),
              ('/tf_static', 'tf_static'),
            ],
    )

    return [
        rsp_node,
        zed_wrapper_node
    ]


def generate_launch_description():
    return LaunchDescription(
        [
            SetEnvironmentVariable(name='RCUTILS_COLORIZED_OUTPUT', value='1'),
            DeclareLaunchArgument(
                'camera_name',
                default_value=TextSubstitution(text='zed2i'),
                description='The name of the camera. It can be different from the camera model and it will be used as node `namespace`.'),
            DeclareLaunchArgument(
                'camera_model',
                default_value='zed2i',
                description='[REQUIRED] The model of the camera. Using a wrong camera model can disable camera features.',
                choices=['zed', 'zedm', 'zed2', 'zed2i', 'zedx', 'zedxm', 'virtual']),
            DeclareLaunchArgument(
                'node_name',
                default_value='zed_node',
                description='The name of the zed_wrapper node. All the topic will have the same prefix: `/<camera_name>/<node_name>/`'),
            DeclareLaunchArgument(
                'config_path',
                default_value=TextSubstitution(text=default_config_common),
                description='Path to the YAML configuration file for the camera.'),
            DeclareLaunchArgument(
                'serial_number',
                default_value='0',
                description='The serial number of the camera to be opened. It is mandatory to use this parameter in multi-camera rigs to distinguish between different cameras.'),
            DeclareLaunchArgument(
                'publish_urdf',
                default_value='false',
                description='Enable URDF processing and starts Robot State Published to propagate static TF.',
                choices=['true', 'false']),
            DeclareLaunchArgument(
                'publish_tf',
                default_value='false',
                description='Enable publication of the `odom -> camera_link` TF.',
                choices=['true', 'false']),
            DeclareLaunchArgument(
                'publish_map_tf',
                default_value='false',
                description='Enable publication of the `map -> odom` TF. Note: Ignored if `publish_tf` is False.',
                choices=['true', 'false']),
            DeclareLaunchArgument(
                'publish_imu_tf',
                default_value='false',
                description='Enable publication of the IMU TF. Note: Ignored if `publish_tf` is False.',
                choices=['true', 'false']),
            DeclareLaunchArgument(
                'xacro_path',
                default_value=TextSubstitution(text=default_xacro_path),
                description='Path to the camera URDF file as a xacro file.'),
            DeclareLaunchArgument(
                'ros_params_override_path',
                default_value='',
                description='The path to an additional parameters file to override the defaults'),
            DeclareLaunchArgument(
                'svo_path',
                default_value=TextSubstitution(text='live'),
                description='Path to an input SVO file.'),
            DeclareLaunchArgument(
                'enable_gnss',
                default_value='false',
                description='Enable GNSS fusion to fix positional tracking pose with GNSS data from messages of type `sensor_msgs::msg::NavSatFix`. The fix topic can be customized in `common.yaml`.',
                choices=['true', 'false']),
            DeclareLaunchArgument(
                'gnss_antenna_offset',
                default_value='[]',
                description='Position of the GNSS antenna with respect to the mounting point of the ZED camera. Format: [x,y,z]'),
            DeclareLaunchArgument(
                'use_sim_time',
                default_value='false',
                description='If set to `true` the node will wait for messages on the `/clock` topic to start and will use this information as the timestamp reference',
                choices=['true', 'false']),
            DeclareLaunchArgument(
                'sim_mode',
                default_value='false',
                description='Enable simulation mode. Set `sim_address` and `sim_port` to configure the simulator input.',
                choices=['true', 'false']),
            DeclareLaunchArgument(
                'sim_address',
                default_value='127.0.0.1',
                description='The connection address of the simulation server. See the documentation of the supported simulation plugins for more information.'),
            DeclareLaunchArgument(
                'sim_port',
                default_value='30000',
                description='The connection port of the simulation server. See the documentation of the supported simulation plugins for more information.'),
            DeclareLaunchArgument(
                'stream_address',
                default_value='',
                description='The connection address of the input streaming server.'),
            DeclareLaunchArgument(
                'stream_port',
                default_value='30000',
                description='The connection port of the input streaming server.'),
            DeclareLaunchArgument(
                'custom_baseline',
                default_value='0.0',
                description='Distance between the center of ZED X One cameras in a custom stereo rig.'),
            OpaqueFunction(function=launch_setup)
        ]
    )

Thank you, Sohail. This is a great help in sending me this template. The template seems to be the zed_camera.launch.py file, which is different from my version. It seems to be adjusted for Clearpath, correct? Anyway, I suppose the namespace-related parts are as follows:

.
.
.
# Robot State Publisher node
rsp_node = Node(
condition=IfCondition(publish_urdf),
package=‘robot_state_publisher’,
namespace=camera_name_val,
executable=‘robot_state_publisher’,
name=‘zed_state_publisher’,
output=‘screen’,
parameters=[{
‘robot_description’: Command(xacro_command)
}],
remappings=[
(‘~/robot_description’, ‘/a200_1058/robot_description’),
(‘joint_states’, ‘/a200_1058/platform/joint_states’),
(‘dynamic_joint_states’, ‘/a200_1058/platform/dynamic_joint_states’),
(‘/diagnostics’, ‘/a200_1058/diagnostics’),
(‘/tf’, ‘tf’),
(‘/tf_static’, ‘tf_static’),
],
)
.
.
.
and
.
.
.
# ZED Wrapper node
zed_wrapper_node = Node(
package=‘zed_wrapper’,
namespace=camera_name_val,
executable=‘zed_wrapper’,
name=node_name,
output=‘screen’,
# prefix=[‘xterm -e valgrind --tools=callgrind’],
# prefix=[‘xterm -e gdb -ex run --args’],
#prefix=[‘gdbserver localhost:3000’],
parameters=node_parameters,
remappings=[
(‘~/robot_description’, ‘/a200_1058/robot_description’),
(‘joint_states’, ‘/a200_1058/platform/joint_states’),
(‘dynamic_joint_states’, ‘/a200_1058/platform/dynamic_joint_states’),
(‘/diagnostics’, ‘/a200_1058/diagnostics’),
(‘/tf’, ‘tf’),
(‘/tf_static’, ‘tf_static’),
],
)
.
.
.

We only have to adjust the Husky ID in a200_1058. We will try this and you a feedback.

2 Likes

Dear Sohail,

I have successfully adjusted the corresponding files to integrate the ZED camera and Husky models. Additionally, the transformations within RViz are fully integrated. However, there are still a few points that remain unclear to me.

a) When I run the ZED wrapper without integrating it with the Husky, the frames /odom and /map are visible in RViz and can be selected as the “Fixed Frame.” However, after integrating the two systems, these frames are no longer visible and cannot be selected in RViz.

Is this expected behavior? If so, will this cause any issues when I start using Nav2 for navigation later on?

b) Currently, the integration is done in a live configuration. This means that when both the Robot Computer and the Offboard Computer are running the live configuration node, the topics and frames from the ZED wrapper and Husky are properly integrated and visible on both machines. Would the integration also work if I would stop using the “Live Configuration”?

Here is a brief summary of the changes I made to the configuration files and integration process:

1) I added lines to the zed_descr.urdf.xacro file and changed the default camera model to zed2i. This was necessary because specifying camera_model:=zed2i in the command
ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zed2i did not override the default value of zed.

	.
	.
	.

<xacro:arg name=“camera_model” default=“zed2i” />
.
.
.

. . .

2) Lines added and adjusted in zed_descr.urdf.xacro:

	.
	.
	.

robot_description = Command(xacro_command)

#Robot State Publisher node
rsp_node = Node(
condition=IfCondition(publish_urdf),
package=‘robot_state_publisher’,
namespace=camera_name_val,
executable=‘robot_state_publisher’,
name=‘zed_state_publisher’,
output=‘screen’,
parameters=[{
# ‘robot_description’: Command(xacro_command)
‘robot_description’: ParameterValue(robot_description, value_type=str)
}],
remappings=[
(‘/tf’, ‘tf’), # Remap TF topic for ZED camera
(‘/tf_static’, ‘tf_static’) # Remap TF_STATIC topic for ZED camera
]
)

	.
	.
	.

# ZED Wrapper component
if( camera_model_val=='zed' or
    camera_model_val=='zedm' or
    camera_model_val=='zed2' or
    camera_model_val=='zed2i' or
    camera_model_val=='zedx' or
    camera_model_val=='zedxm' or
    camera_model_val=='virtual'):
    zed_wrapper_component = ComposableNode(
        package='zed_components',
        namespace=camera_name_val,
        plugin='stereolabs::ZedCamera',
        name=node_name,
        parameters=node_parameters,
        extra_arguments=[{'use_intra_process_comms': True}],
        remappings=[
            ('/tf', 'tf'),         # Remap TF topic for ZED camera
            ('/tf_static', 'tf_static')  # Remap TF_STATIC topic for ZED camera
        ]
    )

	.
	.
	.

3) Lines added to robot.yaml:

	.
	.
	.

extras:
urdf:
path: /home/ubuntu/ros2_ws/src/zed-ros2-wrapper/zed_wrapper/urdf/zed_descr.urdf.xacro

I hope I’ve clarified the issues I’m currently facing and appreciate any help or support.
If you’d like, I can send you the complete files for more details.

Thank you,

SAL

Sorry Sohail, there is a mismatch between points 1) and 2).

Regarding point 1, it seems that the editor doesn’t display the tags, so I write them without the tags instead.

.
.
.
joint name=“base_to_zed2i” type=“fixed”
parent link=“base_link”
child link=“zed_camera_link”
origin xyz=“0.0 0.0 0.526” rpy=“0 0 0”/>
joint
.
.
.

Regarding point 2, the necessary changes have been made in the zed_camera.launch.py file.

No this is not the expected behavior something might be disrupting the system.

Im not quite sure what you mean.

This is incomplete. You want to remap /tf to /a200_0000/tf and same for static. The prefix will be the husky’s number if you do not want to add it directly to remap then /tf tf is fine but in that case the namespace should contain the Huskys prefix. This will fix the frames as well.

Hi Sohail,

thank you for the helpful reply.

We have husky running on Mini ITX PC and the zed_wrapper on Nvidia Jetson Orin (Offboard Computer) as shown on following image:

Since husky and zed_wrapper run on separate machines the integration of xacro files, joining the base_link and zed_camera_link and adding the urdf path in robot.yaml file doesn’t work. Can you please let me know how to make the communication between the two machines possible, so that the integration works? Do you use SSHFS or NFS? Thank you.

Hi if it is in a separate pc, all you need to do is that the zed2i driver has the same namespace as mentioned (including tf’s) before and that it connected to the base_link and or robot frame.

You don’t add it to xacro file in this case.

Hi Sohail,
Thank you for your quick response.
Here’s my understanding based on your explanation:

  1. I must adjust the namespace in the zed_wrapper launch file (zed_camera.launch.py) as follows:

namespace=”/a200_0930”

  1. I must remap the tf topics and the robot_description in the zed_wrapper launch file (zed_camera.launch.py) like this:
remappings=[
    ('/tf', '/a200_0930/tf'),         # Remap TF topic for ZED camera
    ('/tf_static', '/a200_0930/tf_static'),  # Remap TF_STATIC topic for ZED camera
    ('~/robot_description', '/a200_0930/robot_description')
]
  1. I must not include the zed_wrapper xacro file (zed_macro.urdf.xacro) in husky a200.urdf.xacro.

  2. I must not do any changes in robot.yaml!

  3. I only must add the joint tags to a200.urdf.xacro on husky, as shown below:

<joint name="husky_zed_joint" type="fixed">
  <origin xyz="0 0 0.5" rpy="0 0 0" />
  <parent link="base_link" />
  <child link="zed_camera_link" />
</joint>
  1. Finally, on jetson i only need to run zed_wrapper manually?

Are these steps correct or some thing missing?
Should i do any thing else on husky (ITX PC)?

  1. Correct
  2. Correct
  3. You can include if the robot state publisher is not being run in the ZED2i
  4. You have to include if the robot state publisher is not being run in the ZED2i
  5. This should be sufficient to display the visual point cloud, if you want to add the camera ensure the state publisher is running in ZED2i otherwise you have to add in step 3 or 4 just for visuals
  6. Yes