I'm trying to implement a LiDAR sensor in my simulation. I've added the sensor and it seems to work in Gazebo but I can't see the topic showing in ros2 topic list or in Rviz.
Here is the topic list:
~/ros2_ws$ ros2 topic list
/clicked_point
/clock
/drone/bottom/camera_info
/drone/bottom/image_raw
/drone/cmd_vel
/drone/cmd_vel/statistics
/drone/data
/drone/dronevel_mode
/drone/dronevel_mode/statistics
/drone/front/camera_info
/drone/front/image_raw
/drone/gps/vel
/drone/gt_acc
/drone/gt_pose
/drone/gt_vel
/drone/imu
/drone/imu/out
/drone/imu/statistics
/drone/land
/drone/land/statistics
/drone/posctrl
/drone/posctrl/statistics
/drone/reset
/drone/reset/statistics
/drone/sonar
/drone/takeoff
/drone/takeoff/statistics
/goal_pose
/initialpose
/joint_states
/parameter_events
/performance_metrics
/robot_description
/rosout
/tf
/tf_static
Here is my simulation that is being used.
Here is the XACRO and URDF (in this order) files that I've changed in order tot add the LiDAR:
<?xml version="1.0" ?>
<robot name="sjtu_drone"
xmlns:xacro="http://ros.org/wiki/xacro">
<link name="base_link">
<inertial>
<mass value="1.477"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.1152" ixy="0" ixz="0" iyy="0.1152" iyz="0" izz="0.218"/>
</inertial>
<collision name="sjtu_drone_collision">
<origin rpy="0 0 0" xyz="0 0 0.04"/>
<geometry>
<mesh filename="file://$(find sjtu_drone_description)/models/sjtu_drone/quadrotor_4.stl"/>
<!-- <mesh filename="package://sjtu_drone/quadrotor_4.stl"/> -->
</geometry>
</collision>
<visual name="sjtu_drone_visual">
<origin rpy="0 0 0" xyz="0 0 0.04"/>
<geometry>
<mesh filename="file://$(find sjtu_drone_description)/models/sjtu_drone/quadrotor_4.dae"/>
<!-- <mesh filename="package://sjtu_drone/quadrotor_4.dae"/> -->
</geometry>
</visual>
</link>
<joint name="sonar_joint" type="fixed">
<parent link="base_link" />
<child link="sonar_link" />
<origin rpy="0 1.570796326794897 0" xyz="0 0 0"/>
</joint>
<link name="sonar_link"/>
<joint name="front_cam_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.2 0 0"/>
<parent link="base_link" />
<child link="front_cam_link" />
</joint>
<link name="front_cam_link"/>
<joint name="bottom_cam_joint" type="fixed">
<origin rpy="0 1.570796326794897 0" xyz="0 0 0"/>
<parent link="base_link" />
<child link="bottom_cam_link" />
</joint>
<link name="bottom_cam_link"/>
<!-- Link for LiDAR sensor -->
<link name="lidar_link">
<inertial>
<mass value="0.1"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<geometry>
<cylinder length="0.1" radius="0.05"/>
</geometry>
<material>
<color rgba="0.5 0.5 0.5 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.1" radius="0.05"/>
</geometry>
</collision>
</link>
<!-- Joint to attach LiDAR to the drone -->
<joint name="lidar_joint" type="fixed">
<origin xyz="0.2 0 0.1" rpy="0 0 0"/> <!-- Aligned forward with a slight elevation -->
<parent link="base_link"/>
<child link="lidar_link"/>
</joint>
<gazebo>
<plugin name='simple_drone' filename='libplugin_drone.so'>
<bodyName>base_link</bodyName>
<rosNamespace>drone</rosNamespace>
<imuTopic>imu</imuTopic>
<rollpitchProportionalGain>10.0</rollpitchProportionalGain>
<rollpitchDifferentialGain>5.0</rollpitchDifferentialGain>
<rollpitchLimit>0.5</rollpitchLimit>
<yawProportionalGain>2.0</yawProportionalGain>
<yawDifferentialGain>1.0</yawDifferentialGain>
<yawLimit>1.5</yawLimit>
<velocityXYProportionalGain>5.0</velocityXYProportionalGain>
<velocityXYDifferentialGain>2.3</velocityXYDifferentialGain>
<velocityXYLimit>2</velocityXYLimit>
<velocityZProportionalGain>5.0</velocityZProportionalGain>
<velocityZIntegralGain>0.0</velocityZIntegralGain>
<velocityZDifferentialGain>1.0</velocityZDifferentialGain>
<velocityZLimit>-1</velocityZLimit>
<positionXYProportionalGain>1.1</positionXYProportionalGain>
<positionXYDifferentialGain>0.0</positionXYDifferentialGain>
<positionXYIntegralGain>0.0</positionXYIntegralGain>
<positionXYLimit>5</positionXYLimit>
<positionZProportionalGain>1.0</positionZProportionalGain>
<positionZDifferentialGain>0.2</positionZDifferentialGain>
<positionZIntegralGain>0.0</positionZIntegralGain>
<positionZLimit>-1</positionZLimit>
<maxForce>30</maxForce>
<motionSmallNoise>0.05</motionSmallNoise>
<motionDriftNoise>0.03</motionDriftNoise>
<motionDriftNoiseTime>5.0</motionDriftNoiseTime>
</plugin>
</gazebo>
<!-- Sensors -->
<!-- IMU sensor in 100fps -->
<gazebo reference="base_link">
<sensor name='sensor_imu' type='imu'>
<always_on> 1 </always_on>
<visualize>1</visualize>
<update_rate> 100 </update_rate>
<pose> 0 0 0 0 0 0 </pose>
<plugin name='imu' filename='libgazebo_ros_imu_sensor.so'>
<initial_orientation_as_reference>false</initial_orientation_as_reference>
</plugin>
<imu>
<noise>
<type>gaussian</type>
<rate>
<mean> 0 </mean>
<stddev> 0 </stddev>
</rate>
<accel>
<mean> 0 </mean>
<stddev> 0.00 </stddev>
</accel>
</noise>
</imu>
</sensor>
</gazebo>
<!-- Sonar sensor in 100fps -->
<gazebo reference="sonar_link">
<sensor name="sonar" type="ray">
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>5</update_rate>
<ray>
<scan>
<horizontal>
<samples>5</samples>
<resolution>1.000000</resolution>
<min_angle>-0.12</min_angle>
<max_angle>0.12</max_angle>
</horizontal>
<vertical>
<samples>5</samples>
<resolution>1.000000</resolution>
<min_angle>-0.12</min_angle>
<max_angle>0.12</max_angle>
</vertical>
</scan>
<range>
<min>0.02</min>
<max>10</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="sonar" filename="libgazebo_ros_ray_sensor.so">
<ros>
<remapping>~/out:=sonar</remapping>
</ros>
<output_type>sensor_msgs/Range</output_type>
<radiation_type>ultrasound</radiation_type>
<frame_name>sonar_link</frame_name>
</plugin>
</sensor>
</gazebo>
<gazebo reference="front_cam_link">
<sensor name="front_camera" type="camera">
<camera>
<horizontal_fov>2.09</horizontal_fov>
<image>
<width>640</width>
<height>360</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.005</stddev>
</noise>
</camera>
<always_on>1</always_on>
<update_rate>60</update_rate>
<visualize>true</visualize>
<plugin name="camera_front" filename="libgazebo_ros_camera.so">
<ros>
<remapping>image_raw:=camera_front</remapping>
<remapping>camera_info:=camera_front_info</remapping>
</ros>
<camera_name>front</camera_name>
<frame_name>front_cam_link</frame_name>
<hack_baseline>0.07</hack_baseline>
</plugin>
</sensor>
</gazebo>
<!-- downward looking camera -->
<gazebo reference="bottom_cam_link">
<sensor name="down_camera" type="camera">
<camera>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>640</width>
<height>360</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.005</stddev>
</noise>
</camera>
<always_on>1</always_on>
<update_rate>15</update_rate>
<visualize>true</visualize>
<plugin name="camera_bottom" filename="libgazebo_ros_camera.so">
<ros>
<remapping>image_raw:=camera_bottom</remapping>
<remapping>camera_info:=camera_bottom_info</remapping>
</ros>
<camera_name>bottom</camera_name>
<frame_name>bottom_cam_link</frame_name>
<hack_baseline>0.07</hack_baseline>
</plugin>
</sensor>
</gazebo>
<!-- Gazebo plugin for LiDAR -->
<gazebo reference="lidar_link">
<sensor type="ray" name="hokuyo_sensor">
<pose>0 0 0 0 0 0</pose>
<visualize>true</visualize>
<update_rate>30</update_rate>
<ray>
<scan>
<horizontal>
<samples>720</samples>
<resolution>1</resolution>
<min_angle>-1.570796</min_angle>
<max_angle>1.570796</max_angle>
</horizontal>
</scan>
<range>
<min>0.1</min>
<max>30.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_laser.so">
<topicName>/drone/lidar</topicName>
<frameName>lidar_link</frameName>
</plugin>
</sensor>
</gazebo>
<gazebo reference="base_link">
<sensor name="gps" type="gps">
<always_on>true</always_on>
<update_rate>30</update_rate>
<gps>
<position_sensing>
<horizontal>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
</noise>
</horizontal>
<vertical>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
</noise>
</vertical>
</position_sensing>
</gps>
<plugin name="gps" filename="libgazebo_ros_gps_sensor.so">
<ros>
<namespace>/gps</namespace>
<remapping>~/out:=data</remapping>
</ros>
</plugin>
</sensor>
</gazebo>
</robot>
Here is the URDF:
<?xml version="1.0"?>
<robot name="sjtu_drone" xmlns:xacro="http://ros.org/wiki/xacro">
<link name="base_link">
<inertial>
<mass value="1.477"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.1152" ixy="0" ixz="0" iyy="0.1152" iyz="0" izz="0.218"/>
</inertial>
<collision name="sjtu_drone_collision">
<origin rpy="0 0 0" xyz="0 0 0.04"/>
<geometry>
<mesh filename="file://$(find sjtu_drone_description)/models/sjtu_drone/quadrotor_4.stl"/>
<!-- <mesh filename="package://sjtu_drone/quadrotor_4.stl"/> -->
</geometry>
</collision>
<visual name="sjtu_drone_visual">
<origin rpy="0 0 0" xyz="0 0 0.04"/>
<geometry>
<mesh filename="file://$(find sjtu_drone_description)/models/sjtu_drone/quadrotor_4.dae"/>
<!-- <mesh filename="package://sjtu_drone/quadrotor_4.dae"/> -->
</geometry>
</visual>
</link>
<joint name="sonar_joint" type="fixed">
<parent link="base_link" />
<child link="sonar_link" />
<origin rpy="0 1.570796326794897 0" xyz="0 0 0"/>
</joint>
<link name="sonar_link"/>
<joint name="front_cam_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.2 0 0"/>
<parent link="base_link" />
<child link="front_cam_link" />
</joint>
<link name="front_cam_link"/>
<joint name="bottom_cam_joint" type="fixed">
<origin rpy="0 1.570796326794897 0" xyz="0 0 0"/>
<parent link="base_link" />
<child link="bottom_cam_link" />
</joint>
<link name="bottom_cam_link"/>
<!-- Link for LiDAR sensor -->
<link name="lidar_link">
<inertial>
<mass value="0.1"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
<visual name="lidar_visual"> <!-- Added name attribute -->
<geometry>
<cylinder length="0.1" radius="0.05"/>
</geometry>
<material name="lidar_material"> <!-- Added name attribute -->
<color rgba="0.5 0.5 0.5 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.1" radius="0.05"/>
</geometry>
</collision>
</link>
<!-- Joint to attach LiDAR to the drone -->
<joint name="lidar_joint" type="fixed">
<origin xyz="0 0 2" rpy="0 0 0"/> <!-- Adjust the position as needed -->
<parent link="base_link"/>
<child link="lidar_link"/>
</joint>
<gazebo>
<plugin filename="libplugin_drone.so" name="simple_drone">
<bodyName>base_link</bodyName>
<rosNamespace>drone</rosNamespace>
<imuTopic>imu</imuTopic>
<rollpitchProportionalGain>10.0</rollpitchProportionalGain>
<rollpitchDifferentialGain>5.0</rollpitchDifferentialGain>
<rollpitchLimit>0.5</rollpitchLimit>
<yawProportionalGain>2.0</yawProportionalGain>
<yawDifferentialGain>1.0</yawDifferentialGain>
<yawLimit>1.5</yawLimit>
<velocityXYProportionalGain>5.0</velocityXYProportionalGain>
<velocityXYDifferentialGain>2.3</velocityXYDifferentialGain>
<velocityXYLimit>2</velocityXYLimit>
<velocityZProportionalGain>5.0</velocityZProportionalGain>
<velocityZIntegralGain>0.0</velocityZIntegralGain>
<velocityZDifferentialGain>1.0</velocityZDifferentialGain>
<velocityZLimit>-1</velocityZLimit>
<positionXYProportionalGain>1.1</positionXYProportionalGain>
<positionXYDifferentialGain>0.0</positionXYDifferentialGain>
<positionXYIntegralGain>0.0</positionXYIntegralGain>
<positionXYLimit>5</positionXYLimit>
<positionZProportionalGain>1.0</positionZProportionalGain>
<positionZDifferentialGain>0.2</positionZDifferentialGain>
<positionZIntegralGain>0.0</positionZIntegralGain>
<positionZLimit>-1</positionZLimit>
<maxForce>30</maxForce>
<motionSmallNoise>0.05</motionSmallNoise>
<motionDriftNoise>0.03</motionDriftNoise>
<motionDriftNoiseTime>5.0</motionDriftNoiseTime>
</plugin>
</gazebo>
<!-- Sensors -->
<!-- IMU sensor in 100fps -->
<gazebo reference="base_link">
<sensor name="sensor_imu" type="imu">
<always_on> 1 </always_on>
<visualize>1</visualize>
<update_rate> 100 </update_rate>
<pose> 0 0 0 0 0 0 </pose>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu">
<initial_orientation_as_reference>false</initial_orientation_as_reference>
</plugin>
<imu>
<noise>
<type>gaussian</type>
<rate>
<mean> 0 </mean>
<stddev> 0 </stddev>
</rate>
<accel>
<mean> 0 </mean>
<stddev> 0.00 </stddev>
</accel>
</noise>
</imu>
</sensor>
</gazebo>
<!-- Sonar sensor in 100fps -->
<gazebo reference="sonar_link">
<sensor name="sonar" type="ray">
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>5</update_rate>
<ray>
<scan>
<horizontal>
<samples>5</samples>
<resolution>1.000000</resolution>
<min_angle>-0.12</min_angle>
<max_angle>0.12</max_angle>
</horizontal>
<vertical>
<samples>5</samples>
<resolution>1.000000</resolution>
<min_angle>-0.12</min_angle>
<max_angle>0.12</max_angle>
</vertical>
</scan>
<range>
<min>0.02</min>
<max>10</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin filename="libgazebo_ros_ray_sensor.so" name="sonar">
<ros>
<remapping>~/out:=sonar</remapping>
</ros>
<output_type>sensor_msgs/Range</output_type>
<radiation_type>ultrasound</radiation_type>
<frame_name>sonar_link</frame_name>
</plugin>
</sensor>
</gazebo>
<gazebo reference="front_cam_link">
<sensor name="front_camera" type="camera">
<camera>
<horizontal_fov>2.09</horizontal_fov>
<image>
<width>640</width>
<height>360</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.005</stddev>
</noise>
</camera>
<always_on>1</always_on>
<update_rate>60</update_rate>
<visualize>true</visualize>
<plugin filename="libgazebo_ros_camera.so" name="camera_front">
<ros>
<remapping>image_raw:=camera_front</remapping>
<remapping>camera_info:=camera_front_info</remapping>
</ros>
<camera_name>front</camera_name>
<frame_name>front_cam_link</frame_name>
<hack_baseline>0.07</hack_baseline>
</plugin>
</sensor>
</gazebo>
<!-- downward looking camera -->
<gazebo reference="bottom_cam_link">
<sensor name="down_camera" type="camera">
<camera>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>640</width>
<height>360</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.005</stddev>
</noise>
</camera>
<always_on>1</always_on>
<update_rate>15</update_rate>
<visualize>true</visualize>
<plugin filename="libgazebo_ros_camera.so" name="camera_bottom">
<ros>
<remapping>image_raw:=camera_bottom</remapping>
<remapping>camera_info:=camera_bottom_info</remapping>
</ros>
<camera_name>bottom</camera_name>
<frame_name>bottom_cam_link</frame_name>
<hack_baseline>0.07</hack_baseline>
</plugin>
</sensor>
</gazebo>
<!-- Gazebo plugin for LiDAR -->
<gazebo reference="lidar_link">
<sensor name="hokuyo_sensor" type="ray">
<pose>0 0 0 0 0 0</pose>
<visualize>true</visualize>
<update_rate>30</update_rate>
<ray>
<scan>
<horizontal>
<samples>720</samples>
<resolution>1</resolution>
<min_angle>-1.570796</min_angle>
<max_angle>1.570796</max_angle>
</horizontal>
</scan>
<range>
<min>0.1</min>
<max>30.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin filename="libgazebo_ros_laser.so" name="gazebo_ros_laser_controller">
<topicName>/drone/lidar</topicName>
<frameName>lidar_link</frameName>
</plugin>
</sensor>
</gazebo>
<gazebo reference="base_link">
<sensor name="gps" type="gps">
<always_on>true</always_on>
<update_rate>30</update_rate>
<gps>
<position_sensing>
<horizontal>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
</noise>
</horizontal>
<vertical>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
</noise>
</vertical>
</position_sensing>
</gps>
<plugin filename="libgazebo_ros_gps_sensor.so" name="gps">
<ros>
<namespace>/gps</namespace>
<remapping>~/out:=data</remapping>
</ros>
</plugin>
</sensor>
</gazebo>
</robot>
