Hi,

I am using Ubuntu 20.04 focal with ROS noetic on amd64 (tried with arm64 as well) architecture. I have xacro file of husky robot by clearpath robotics from their official package and I am trying to add a ZED2 camera to the robot. I found ZED2's urdf from this package which works perfectly in their setup. I studied how the clearpath robotics has implemented arguments in xacro file to enable/ disable various sensors.

<!-- Intel Realsense camera related parameters or code-->
<xacro:arg name="realsense_enabled" default="$(optenv HUSKY_REALSENSE_ENABLED 0)" />
<xacro:arg name="realsense_xyz" default="$(optenv HUSKY_REALSENSE_XYZ 0 0.14 0)" />
<xacro:arg name="realsense_rpy" default="$(optenv HUSKY_REALSENSE_RPY 0 0 0)" />
<xacro:arg name="realsense_mount" default="$(optenv HUSKY_REALSENSE_MOUNT_FRAME sensor_arch_mount_link)" />

<xacro:include filename="$(find husky_description)/urdf/accessories/intel_realsense.urdf.xacro"/>

<!-- add the intel realsense to the sensor arch if needed -->
    <xacro:if value="$(arg realsense_enabled)">
        <link name="realsense_mountpoint"/>
        <joint name="realsense_mountpoint_joint" type="fixed">
        <origin xyz="$(arg realsense_xyz)" rpy="$(arg realsense_rpy)" />
        <parent link="$(arg realsense_mount)"/>
        <child link="realsense_mountpoint" />
        </joint>
        <xacro:intel_realsense_mount prefix="camera" topic="realsense" parent_link="realsense_mountpoint"/>
    </xacro:if>

I did something similar to add a ZED2 camera in the "husky.urdf.xacro" file

  <!-- Custom addition to xacro file to include ZED2 camera ; by Dev Vaibhav-->
  <xacro:arg name="zed2_enabled" default="$(optenv HUSKY_ZED2_ENABLED 1)" />
  <xacro:arg name="zed2_xyz" default="$(optenv HUSKY_ZED2_XYZ 0 0.14 0)" />
  <xacro:arg name="zed2_rpy" default="$(optenv HUSKY_ZED2_RPY 0 0 0)" />
  <xacro:arg name="zed2_mount" default="$(optenv HUSKY_ZED2_MOUNT_FRAME sensor_arch_mount_link)" />

  <xacro:include filename="$(find husky_description)/urdf/accessories/zed2.xacro"/>

  <xacro:property name="sensorbar_needed_zed2" value="$(arg zed2_enabled)" />

  <xacro:if value="${sensorbar_needed_realsense or sensorbar_user_enabled or sensorbar_needed_lidar or sensorbar_needed_zed2}">
    <xacro:sensor_arch prefix="" parent="top_plate_link" size="$(arg sensor_arch_height)">
        <origin xyz="$(optenv HUSKY_SENSOR_ARCH_OFFSET 0 0 0)" rpy="$(optenv HUSKY_SENSOR_ARCH_RPY 0 0 0)"/>
      </xacro:sensor_arch>
  </xacro:if>

  <!-- Custom addition to xacro file to include ZED2 camera ; by Dev Vaibhav-->
  <!-- add the zed2 to the sensor arch if needed -->
  <xacro:if value="$(arg zed2_enabled)">
    <link name="zed2_mountpoint"/>
    <joint name="zed2_mountpoint_joint" type="fixed">
      <origin xyz="$(arg zed2_xyz)" rpy="$(arg zed2_rpy)" />
      <parent link="$(arg zed2_mount)"/>
      <child link="zed2_mountpoint" />
    </joint>
    <xacro:zed2_mount prefix="" topic="zed2" parent_link="zed2_mountpoint"/>
  </xacro:if>

My "zed2.xacro" macro's look something like this. First is the parent macro <xacro:macro name="zed2_mount" params="prefix topic parent_link"> which I am calling through the line no. 256 in husky.urdf.xacro with no prefix; and inside that is the child macro <xacro:macro name="zed2" params="link_prefix:=${prefix} joint_prefix:=${prefix}"> With this code, I am trying to connect the ZED2 camera to robot with parent link as zed2_mountpoint (present on sensor arch on robot) and child link as zed2cameracenter (present on the sensor itself) but it is not working. I tried to visualize it in gazebo but I didn't see any trace showing error related to the sensor.

Below is the gazebo verbose log output on terminal.

ubuntu@ubuntu2004:~$ roslaunch husky_gazebo husky_empty_world.launch 
... logging to /home/ubuntu/.ros/log/4c9e033c-f650-11ec-b74f-0f173c7b7596/roslaunch-ubuntu2004-42395.log
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.

started roslaunch server http://ubuntu2004:40315/



SUMMARY
========

PARAMETERS
 * /ekf_localization/base_link_frame: base_link
 * /ekf_localization/frequency: 50
 * /ekf_localization/imu0: imu/data
 * /ekf_localization/imu0_config: [False, False, Fa...
 * /ekf_localization/imu0_differential: True
 * /ekf_localization/imu0_queue_size: 10
 * /ekf_localization/imu0_remove_gravitational_acceleration: True
 * /ekf_localization/odom0: husky_velocity_co...
 * /ekf_localization/odom0_config: [False, False, Fa...
 * /ekf_localization/odom0_differential: False
 * /ekf_localization/odom0_queue_size: 10
 * /ekf_localization/odom_frame: odom
 * /ekf_localization/predict_to_current_time: True
 * /ekf_localization/two_d_mode: True
 * /ekf_localization/world_frame: odom
 * /gazebo/enable_ros_network: True
 * /husky_joint_publisher/publish_rate: 50
 * /husky_joint_publisher/type: joint_state_contr...
 * /husky_velocity_controller/angular/z/has_acceleration_limits: True
 * /husky_velocity_controller/angular/z/has_velocity_limits: True
 * /husky_velocity_controller/angular/z/max_acceleration: 6.0
 * /husky_velocity_controller/angular/z/max_velocity: 2.0
 * /husky_velocity_controller/base_frame_id: base_link
 * /husky_velocity_controller/cmd_vel_timeout: 0.25
 * /husky_velocity_controller/enable_odom_tf: False
 * /husky_velocity_controller/estimate_velocity_from_position: False
 * /husky_velocity_controller/left_wheel: ['front_left_whee...
 * /husky_velocity_controller/linear/x/has_acceleration_limits: True
 * /husky_velocity_controller/linear/x/has_velocity_limits: True
 * /husky_velocity_controller/linear/x/max_acceleration: 3.0
 * /husky_velocity_controller/linear/x/max_velocity: 1.0
 * /husky_velocity_controller/pose_covariance_diagonal: [0.001, 0.001, 0....
 * /husky_velocity_controller/publish_rate: 50
 * /husky_velocity_controller/right_wheel: ['front_right_whe...
 * /husky_velocity_controller/twist_covariance_diagonal: [0.001, 0.001, 0....
 * /husky_velocity_controller/type: diff_drive_contro...
 * /husky_velocity_controller/velocity_rolling_window_size: 2
 * /husky_velocity_controller/wheel_radius_multiplier: 1.0
 * /husky_velocity_controller/wheel_separation_multiplier: 1.875
 * /joy_teleop/joy_node/autorepeat_rate: 20
 * /joy_teleop/joy_node/deadzone: 0.1
 * /joy_teleop/joy_node/dev: /dev/input/ps4
 * /joy_teleop/teleop_twist_joy/axis_angular: 0
 * /joy_teleop/teleop_twist_joy/axis_linear: 1
 * /joy_teleop/teleop_twist_joy/enable_button: 4
 * /joy_teleop/teleop_twist_joy/enable_turbo_button: 5
 * /joy_teleop/teleop_twist_joy/scale_angular: 1.4
 * /joy_teleop/teleop_twist_joy/scale_linear: 0.4
 * /joy_teleop/teleop_twist_joy/scale_linear_turbo: 2.0
 * /robot_description: <?xml version="1....
 * /rosdistro: noetic
 * /rosversion: 1.15.14
 * /twist_marker_server/link_name: base_link
 * /twist_mux/locks: [{'name': 'e_stop...
 * /twist_mux/topics: [{'name': 'joy', ...
 * /use_sim_time: True

NODES
  /
    base_controller_spawner (controller_manager/spawner)
    ekf_localization (robot_localization/ekf_localization_node)
    gazebo (gazebo_ros/debug)
    gazebo_gui (gazebo_ros/gzclient)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    spawn_husky_model (gazebo_ros/spawn_model)
    twist_marker_server (interactive_marker_twist_server/marker_server)
    twist_mux (twist_mux/twist_mux)
  /joy_teleop/
    joy_node (joy/joy_node)
    teleop_twist_joy (teleop_twist_joy/teleop_node)

ROS_MASTER_URI=http://localhost:11311

process[gazebo-1]: started with pid [42411]
process[gazebo_gui-2]: started with pid [42415]
process[base_controller_spawner-3]: started with pid [42420]
process[ekf_localization-4]: started with pid [42421]
process[twist_marker_server-5]: started with pid [42422]
process[robot_state_publisher-6]: started with pid [42424]
process[twist_mux-7]: started with pid [42430]
[ INFO] [1656437476.129751906]: [twist_marker_server] Initialized.
process[joy_teleop/joy_node-8]: started with pid [42432]
process[joy_teleop/teleop_twist_joy-9]: started with pid [42440]
process[spawn_husky_model-10]: started with pid [42445]
[ERROR] [1656437476.198143143]: Couldn't open joystick /dev/input/ps4. Will retry every second.
GNU gdb (Ubuntu 9.2-0ubuntu1~20.04.1) 9.2
Copyright (C) 2020 Free Software Foundation, Inc.
License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html>
This is free software: you are free to change and redistribute it.
There is NO WARRANTY, to the extent permitted by law.
Type "show copying" and "show warranty" for details.
This GDB was configured as "aarch64-linux-gnu".
Type "show configuration" for configuration details.
For bug reporting instructions, please see:
<http://www.gnu.org/software/gdb/bugs/>.
Find the GDB manual and other documentation resources online at:
    <http://www.gnu.org/software/gdb/documentation/>.

For help, type "help".
Type "apropos word" to search for commands related to "word"...
Reading symbols from gzserver...
(No debugging symbols found in gzserver)
Gazebo multi-robot simulator, version 11.10.2
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

[Wrn] [GuiIface.cc:200] g/gui-plugin is really loading a SystemPlugin. To load a GUI plugin please use --gui-client-plugin 
[ INFO] [1656437476.989546949]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1656437476.991081997]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
[Msg] Waiting for master.
[Thread debugging using libthread_db enabled]
Using host libthread_db library "/lib/aarch64-linux-gnu/libthread_db.so.1".
[New Thread 0xffffe2a9c990 (LWP 42541)]
Gazebo multi-robot simulator, version 11.10.2
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

[New Thread 0xffffe1520990 (LWP 42542)]
[New Thread 0xffffe0d1f990 (LWP 42543)]
[New Thread 0xffffdfd1d990 (LWP 42545)]
[New Thread 0xffffdf51c990 (LWP 42546)]
[New Thread 0xffffded1b990 (LWP 42547)]
[New Thread 0xffffde51a990 (LWP 42548)]
[New Thread 0xffffddd19990 (LWP 42549)]
[New Thread 0xffffdd518990 (LWP 42550)]
[New Thread 0xffffdcd17990 (LWP 42551)]
[New Thread 0xffffbfffd990 (LWP 42552)]
[New Thread 0xffffbf7fc990 (LWP 42553)]
[New Thread 0xffffbeffb990 (LWP 42554)]
[ INFO] [1656437478.130075057]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1656437478.134023357]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 10.0.0.97
[New Thread 0xffffbe7fa990 (LWP 42556)]
[New Thread 0xffffa2f72990 (LWP 42557)]
[New Thread 0xffffa2771990 (LWP 42558)]
[New Thread 0xffffa1f70990 (LWP 42559)]
[New Thread 0xffffa176f990 (LWP 42560)]
[New Thread 0xffffa0f6e990 (LWP 42561)]
[New Thread 0xffff8bffd990 (LWP 42562)]
[New Thread 0xffff8b7fc990 (LWP 42563)]
[New Thread 0xffff8affb990 (LWP 42564)]
[New Thread 0xffff8a7fa990 (LWP 42565)]
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 10.0.0.97
[New Thread 0xffff89966990 (LWP 42573)]
[New Thread 0xffff89165990 (LWP 42574)]
[New Thread 0xffff88964990 (LWP 42575)]
[New Thread 0xffff6bffd990 (LWP 42576)]
[Msg] Loading world file [/usr/share/gazebo-11/worlds/empty.world]
[New Thread 0xffff6b6fc990 (LWP 42586)]
[New Thread 0xffff6aefb990 (LWP 42587)]
[New Thread 0xffff6a6fa990 (LWP 42588)]
[New Thread 0xffff69ef9990 (LWP 42589)]
[New Thread 0xffff696f8990 (LWP 42590)]
[New Thread 0xffff68ef7990 (LWP 42591)]
[New Thread 0xffff53ffd990 (LWP 42592)]
[New Thread 0xffff537fb990 (LWP 42594)]
[New Thread 0xffff53bfc990 (LWP 42593)]
[ INFO] [1656437479.286699888]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[New Thread 0xffff533fa990 (LWP 42604)]
[New Thread 0xffff52bf9990 (LWP 42605)]
[New Thread 0xffff523f8990 (LWP 42606)]
[New Thread 0xffff51bf7990 (LWP 42607)]
[ INFO] [1656437479.307933359, 0.008000000]: Physics dynamic reconfigure ready.
[Thread 0xffffbeffb990 (LWP 42554) exited]
[ INFO] [1656437489.571615492, 0.134000000]: Velodyne laser plugin missing <min_intensity>, defaults to no clipping
[New Thread 0xffff512c1990 (LWP 43022)]
[ INFO] [1656437489.580050616, 0.134000000]: Velodyne laser plugin ready, 16 lasers
[Wrn] [ColladaLoader.cc:1928] Triangle input semantic: 'COLOR' is currently not supported
[ INFO] [1656437489.683502435, 0.134000000]: Loading gazebo_ros_control plugin
[ INFO] [1656437489.683754357, 0.134000000]: Starting gazebo_ros_control plugin in namespace: /
[ INFO] [1656437489.684994363, 0.134000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [robot_description] on the ROS param server.
[ERROR] [1656437489.825452496, 0.134000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/front_left_wheel
[ERROR] [1656437489.828111510, 0.134000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/front_right_wheel
[ERROR] [1656437489.829531957, 0.134000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/rear_left_wheel
[ERROR] [1656437489.831338007, 0.134000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/rear_right_wheel
[ INFO] [1656437489.844680036, 0.134000000]: Loaded gazebo_ros_control.
[ INFO] [1656437489.868172959, 0.134000000]: imu plugin missing <xyzOffset>, defaults to 0s
[ INFO] [1656437489.868331960, 0.134000000]: imu plugin missing <rpyOffset>, defaults to 0s
[ WARN] [1656437489.969413567, 0.135000000]: Failed to meet update rate! Took 0.13500000000000000888
[ WARN] [1656437489.971484378, 0.136000000]: Failed to meet update rate! Took 0.11600000000000000588
[New Thread 0xfffe9fffd990 (LWP 43026)]
[ INFO] [1656437490.145201684, 0.299000000]: Controller state will be published at 50Hz.
[ INFO] [1656437490.147289815, 0.301000000]: Wheel separation will be multiplied by 1.875.
[ INFO] [1656437490.148328060, 0.302000000]: Left wheel radius will be multiplied by 1.
[ INFO] [1656437490.148373460, 0.302000000]: Right wheel radius will be multiplied by 1.
[ INFO] [1656437490.149393266, 0.303000000]: Velocity rolling window size of 2.
[ INFO] [1656437490.150157150, 0.304000000]: Velocity commands will be considered old if they are older than 0.25s.
[ INFO] [1656437490.150618552, 0.305000000]: Allow mutiple cmd_vel publishers is enabled
[ INFO] [1656437490.151363956, 0.306000000]: Base frame_id set to base_link
[ INFO] [1656437490.152553242, 0.307000000]: Odometry frame_id set to odom
[ INFO] [1656437490.153345766, 0.308000000]: Publishing to tf is disabled
[ INFO] [1656437490.166495355, 0.321000000]: left wheel to origin: 0.256,0.2854, 0.03282
[ INFO] [1656437490.166794037, 0.322000000]: right wheel to origin: 0.256,-0.2854, 0.03282
[ INFO] [1656437490.166987438, 0.322000000]: Odometry params : wheel separation 1.07025, left wheel radius 0.1651, right wheel radius 0.1651
[New Thread 0xfffe9f7fc990 (LWP 43028)]
[New Thread 0xfffe9effb990 (LWP 43029)]
[ INFO] [1656437490.177338452, 0.333000000]: Adding left wheel with joint name: front_left_wheel and right wheel with joint name: front_right_wheel
[ INFO] [1656437490.179685384, 0.336000000]: Adding left wheel with joint name: rear_left_wheel and right wheel with joint name: rear_right_wheel
[ INFO] [1656437490.202079381, 0.354000000]: Dynamic Reconfigure:
DynamicParams:
    Odometry parameters:
        left wheel radius multiplier: 1
        right wheel radius multiplier: 1
        wheel separation multiplier: 1.875
    Publication parameters:
        Publish executed velocity command: disabled
        Publication rate: 50
        Publish frame odom on tf: disabled
[spawn_husky_model-10] process has finished cleanly
log file: /home/ubuntu/.ros/log/4c9e033c-f650-11ec-b74f-0f173c7b7596/spawn_husky_model-10*.log

I tried to investigate it further by checking what urdf is actually getting parsed by gazebo. Is the macro zed2_mount really getting expanded/ substituted when line # 256 is ran from husky.urdf.xacro? I did so using rosrun xacro xacro husky.urdf.xacro > /home/ubuntu/Documents/debug.txt and it turned out the xacro's macro functionality is not working properly (or I might be doing something incorrect).

Though, I made it work by copy-pasting the code from zed2.xacro to husky.urdf.xacro as I had to move forward but I feel this is not the best way to implement a sensor over a robot.

Here are the xacro files: husky.urdf.xacro, husky.urdfCOPYPASTE.xacro (working code using copy-paste, intel_realsense.urdf.xacro (no modification, provided with the package itself) and zed2.xacro ,and the debug.txt file

Please suggest what am I doing wrong over here.

Thanks,

Dev

use zed2_mount: <xacro.Macro object at 0xffffbc5bcb80> (husky.urdf.xacro)
 set prefix:  (husky.urdf.xacro)
 set topic: zed2 (husky.urdf.xacro)
 set parent_link: zed2_mountpoint (husky.urdf.xacro)
 set baseline: 0.12 (husky.urdf.xacro)
 set zed2: <xacro.Macro object at 0xffffba8717f0> (husky.urdf.xacro)
 set zed2_gazebo: <xacro.Macro object at 0xffffba871820> (husky.urdf.xacro