Skip to content

Feat/Add PointCloud2 sensor #22

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
wants to merge 1 commit into from
Closed

Feat/Add PointCloud2 sensor #22

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 10 additions & 1 deletion nav2_minimal_tb4_description/urdf/icreate/common_properties.urdf.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,8 +48,9 @@
</xacro:macro>

<xacro:macro name="ray_sensor"
params="sensor_name gazebo update_rate visualize
params="sensor_name use_vertical_rays:=^|false gazebo update_rate visualize
h_samples h_res h_min_angle h_max_angle
v_samples:=^|0 v_res:=^|0 v_min_angle:=^|0 v_max_angle:=^|0
r_min r_max r_res *plugin">
<sensor name="${sensor_name}" type="gpu_lidar">
<update_rate>${update_rate}</update_rate>
Expand All @@ -65,6 +66,14 @@
<min_angle>${h_min_angle}</min_angle>
<max_angle>${h_max_angle}</max_angle>
</horizontal>
<xacro:if value="${use_vertical_rays}">
<vertical>
<samples>${v_samples}</samples>
<resolution>${v_res}</resolution>
<min_angle>${v_min_angle}</min_angle>
<max_angle>${v_max_angle}</max_angle>
</vertical>
</xacro:if>
</scan>
<range>
<min>${r_min}</min>
Expand Down
53 changes: 53 additions & 0 deletions nav2_minimal_tb4_description/urdf/sensors/velodyne.urdf.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,53 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">

<xacro:macro name="velodyne" params="name parent_link gazebo *origin">
<xacro:include filename="$(find nav2_minimal_tb4_description)/urdf/icreate/common_properties.urdf.xacro"/>

<xacro:property name="mass" value="0.17"/>
<xacro:property name="length_x" value="${7.1*cm2m}" />
<xacro:property name="length_y" value="${10*cm2m}" />
<xacro:property name="length_z" value="${6*cm2m}" />

<xacro:property name="collision_x_offset" value="${0*cm2m}" />
<xacro:property name="collision_y_offset" value="${1.3*cm2m}" />
<xacro:property name="collision_z_offset" value="${-1.9*cm2m}" />

<joint name="${name}_joint" type="fixed">
<parent link="${parent_link}"/>
<child link="${name}_link"/>
<xacro:insert_block name="origin"/>
</joint>

<link name="${name}_link">
<visual>
<geometry>
<mesh filename="package://nav2_minimal_tb4_description/meshes/rplidar.dae" scale="1 1 1" />
</geometry>
</visual>
<collision>
<origin xyz="${collision_x_offset} ${collision_y_offset} ${collision_z_offset}"/>
<geometry>
<box size="${length_x} ${length_y} ${length_z}"/>
</geometry>
</collision>
<xacro:inertial_cuboid mass="0.17" x="${length_x}" y="${length_y}" z="${length_z}"/>
</link>

<gazebo reference="${name}_link">
<xacro:ray_sensor sensor_name="${name}" use_vertical_rays="true" gazebo="${gazebo}"
update_rate="10.0" visualize="true"
h_samples="360" h_res="1.0" h_min_angle="0.000" h_max_angle="6.280000"
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Are these representative of a velodyne sensor?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I used arbitrary values, while these are the sensor dimensions and specs.
Just providing a template for 3D LIDARs 🥲

Copy link
Member

@SteveMacenski SteveMacenski Jan 28, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'd quickly look up the current specs for a mid-range 3D lidar (that you can actually buy; I think Velodyne brand is end of life?) and update them to be realistic / based on something in particular - especially if we want to take metrics about its performance with respect to! Then, update the name of the file to match that current 3D lidar option

v_samples="180" v_res="0.2" v_min_angle="-0.03" v_max_angle="0.215"
r_min="0.164" r_max="20.0" r_res="0.01">
<plugin name="dummy" filename="dummyfile"></plugin>
</xacro:ray_sensor>
<xacro:material_darkgray/>
</gazebo>

<gazebo reference="${name}_joint">
<preserveFixedJoint>true</preserveFixedJoint>
</gazebo>

</xacro:macro>
</robot>
20 changes: 16 additions & 4 deletions nav2_minimal_tb4_description/urdf/standard/turtlebot4.urdf.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
<!-- Base create3 model -->
<xacro:include filename="$(find nav2_minimal_tb4_description)/urdf/icreate/create3.urdf.xacro" />
<xacro:include filename="$(find nav2_minimal_tb4_description)/urdf/sensors/rplidar.urdf.xacro" />
<xacro:include filename="$(find nav2_minimal_tb4_description)/urdf/sensors/velodyne.urdf.xacro" />
<xacro:include filename="$(find nav2_minimal_tb4_description)/urdf/sensors/oakd.urdf.xacro" />
<xacro:include filename="$(find nav2_minimal_tb4_description)/urdf/sensors/camera_bracket.urdf.xacro" />
<xacro:include filename="$(find nav2_minimal_tb4_description)/urdf/standard/tower_standoff.urdf.xacro" />
Expand Down Expand Up @@ -130,10 +131,21 @@
</xacro:tower_sensor_plate>

<!-- Turtlebot4 sensor definitions -->
<xacro:rplidar name="rplidar" parent_link="shell_link" gazebo="$(arg gazebo)">
<origin xyz="${rplidar_x_offset} ${rplidar_y_offset} ${rplidar_z_offset}"
rpy="0 0 ${pi/2}"/>
</xacro:rplidar>
<xacro:arg name="use_3d_lidar" default="false" />

<xacro:unless value="$(arg use_3d_lidar)">
<xacro:rplidar name="rplidar" parent_link="shell_link" gazebo="$(arg gazebo)">
<origin xyz="${rplidar_x_offset} ${rplidar_y_offset} ${rplidar_z_offset}"
rpy="0 0 ${pi/2}"/>
</xacro:rplidar>
</xacro:unless>

<xacro:if value="$(arg use_3d_lidar)">
<xacro:velodyne name="velodyne" parent_link="shell_link" gazebo="$(arg gazebo)">
<origin xyz="${rplidar_x_offset} ${rplidar_y_offset} ${rplidar_z_offset}"
rpy="0 0 ${pi/2}"/>
</xacro:velodyne>
</xacro:if>

<xacro:camera_bracket name="oakd_camera_bracket">
<origin xyz="${camera_mount_x_offset} ${camera_mount_y_offset} ${camera_mount_z_offset}"/>
Expand Down
13 changes: 12 additions & 1 deletion nav2_minimal_tb4_sim/launch/simulation.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@ def generate_launch_description():
use_simulator = LaunchConfiguration('use_simulator')
use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
headless = LaunchConfiguration('headless')
use_3dlidar = LaunchConfiguration('use_3dlidar')
world = LaunchConfiguration('world')
pose = {
'x': LaunchConfiguration('x_pose', default='-8.00'),
Expand Down Expand Up @@ -113,6 +114,12 @@ def generate_launch_description():
'headless', default_value='False', description='Whether to execute gzclient)'
)

declare_use_3dlidar_cmd = DeclareLaunchArgument(
'use_3dlidar',
default_value='False',
description='Whether to use 3D Lidar and publish pointcloud2'
)

declare_world_cmd = DeclareLaunchArgument(
'world',
default_value=os.path.join(sim_dir, 'worlds', 'depot.sdf'),
Expand All @@ -138,7 +145,9 @@ def generate_launch_description():
output='screen',
parameters=[
{'use_sim_time': use_sim_time,
'robot_description': Command(['xacro', ' ', robot_sdf])}
'robot_description': Command(
['xacro', ' ', 'use_3d_lidar:=', use_3dlidar, ' ', robot_sdf]
)}
],
remappings=remappings,
)
Expand Down Expand Up @@ -196,6 +205,7 @@ def generate_launch_description():
launch_arguments={'namespace': namespace,
'use_simulator': use_simulator,
'use_sim_time': use_sim_time,
'use_3dlidar': use_3dlidar,
'robot_name': robot_name,
'robot_sdf': robot_sdf,
'x_pose': pose['x'],
Expand All @@ -217,6 +227,7 @@ def generate_launch_description():
ld.add_action(declare_use_simulator_cmd)
ld.add_action(declare_use_robot_state_pub_cmd)
ld.add_action(declare_simulator_cmd)
ld.add_action(declare_use_3dlidar_cmd)
ld.add_action(declare_world_cmd)
ld.add_action(declare_robot_name_cmd)
ld.add_action(declare_robot_sdf_cmd)
Expand Down
27 changes: 27 additions & 0 deletions nav2_minimal_tb4_sim/launch/spawn_tb4.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time')
namespace = LaunchConfiguration('namespace')
use_simulator = LaunchConfiguration('use_simulator')
use_3dlidar = LaunchConfiguration('use_3dlidar')
robot_name = LaunchConfiguration('robot_name')
# robot_sdf = LaunchConfiguration('robot_sdf')
pose = {'x': LaunchConfiguration('x_pose', default='-8.00'),
Expand All @@ -51,6 +52,12 @@ def generate_launch_description():
default_value='True',
description='Whether to start the simulator')

declare_use_3dlidar_cmd = DeclareLaunchArgument(
'use_3dlidar',
default_value='False',
description='Whether to use 3D Lidar and publish pointcloud2'
)

declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='true',
Expand Down Expand Up @@ -78,6 +85,24 @@ def generate_launch_description():
output='screen',
)

# replace [ros_gz_point_cloud that only works with ign-gazebo <= 2.6.1]
# https://github.com/gazebosim/ros_gz/issues/40
pcl_bridge = Node(
condition=IfCondition(use_3dlidar),
package='ros_gz_bridge',
executable='parameter_bridge',
name='bridge_ros_gz_pcl',
namespace=namespace,
output='screen',
parameters=[{
'use_sim_time': use_sim_time,
}],
remappings=[
('/scan/points', '/pointcloud')
],
arguments=['/scan/points@sensor_msgs/msg/PointCloud2[gz.msgs.PointCloudPacked'],
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Could just add here? https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation/blob/7e9b330d3b10e0617efe576b2e41c01aaa3e6781/nav2_minimal_tb4_sim/configs/tb4_bridge.yaml. Does it hurt to have topics not actually published by GZ on the bridge?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The gazebo topic /scan/points is actually available for the 2D lidar so in that case it would publish the scan points as PointCloud2 as well but I think I took it far by adding an argument to choose whether to bridge it or not.
And at some point, it could be replaced with ros_gz_point_cloud

Copy link
Member

@SteveMacenski SteveMacenski Jan 28, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Have the 3D lidar publish to a different topic? Overriding scan probably isn't good here anyway, thats usually a name for 2D lidars

)

camera_bridge_image = Node(
package='ros_gz_image',
executable='image_bridge',
Expand Down Expand Up @@ -119,9 +144,11 @@ def generate_launch_description():
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_robot_name_cmd)
ld.add_action(declare_use_simulator_cmd)
ld.add_action(declare_use_3dlidar_cmd)
ld.add_action(declare_use_sim_time_cmd)

ld.add_action(bridge)
ld.add_action(pcl_bridge)
ld.add_action(camera_bridge_image)
ld.add_action(camera_bridge_depth)
ld.add_action(spawn_model)
Expand Down
Loading