Robotics StackExchange | Archived questions

ROS2: ros_ray sensor does not default to link as frame_name

When using the raylaserplugin and a fixed joint, the framelink of the sensor defaults to the main link of the robot **basefootprint** not the actual link laser_link as described here: https://github.com/ros-simulation/gazebo_ros_pkgs/wiki/ROS-2-Migration:-Ray-sensors

I created a minimal robot xacro example that can be spawned into gazebo. Its fairly simple to not clutter the file. The scan topic itself is correctly published but the reference frame is not set. If I leave out the frame_name element the scan is referenced in the basefootprint frame, not the laserlink frame.

I don't really have a question, since I know how to fix it but I am wondering if this is intended behavior. When using fixed link poses instead of fixed joints the default to the laser_link works.

I am using ros2 foxy (gazeborospkgs 3.5.3) with Gazebo 11.5

<?xml version="1.0" ?>
<robot name="my_robot">
  <gazebo>
    <static>true</static>
  </gazebo>
  <link name="base_footprint"/>
  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin rpy="0 0 0" xyz="0 0 0.25"/>
  </joint>
  <link name="base_link">
    <inertial>
      <mass value="48.0"/>
      <origin rpy="0 0 0" xyz="0.03 0 0.20"/>
      <inertia ixx="2.0" ixy="0.0" ixz="0.0" iyy="4.3" iyz="0.0" izz="5.5"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.03 0 0"/>
      <geometry>
        <box size= "0.5 0.5 0.5"/>
      </geometry>
    </visual>
  </link>
  <joint name="base_link_to_laser_joint" type="fixed">
    <parent link="base_link"/>
    <child link="laser_link"/>
    <origin rpy="0.0 0.0 0.5" xyz="0.25 0.25 0.25"/>
  </joint>
  <link name="laser_link">
    <visual>
      <origin rpy="0 0 0" xyz="0.03 0 0"/>
      <geometry>
        <box size= "0.1 0.1 0.1"/>
      </geometry>
    </visual>
  </link>
  <gazebo reference="laser_link">
    <sensor name="laser_link" type="ray">
      <visualize>1</visualize>
      <update_rate>50</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>500</samples>
            <resolution>1</resolution>
            <min_angle>-2.35619449615</min_angle>
            <max_angle>2.35619449615</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.05</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_ray_sensor.so" name="gazebo_ros_laser_link_controller">
        <ros>
          <remapping>~/out:=scan</remapping>
        </ros>
        <!-- when leaving away frame_name it defaults to base_footprint instead-->
        <frame_name>laser_link</frame_name>
        <output_type>sensor_msgs/LaserScan</output_type>
      </plugin>
    </sensor>
  </gazebo>
</robot>

Asked by relffok on 2021-06-09 06:44:12 UTC

Comments

Answers

Not really an answer but just 'ditto'.
It took me hours to find out why the laser output looked like the scanner (in the video, the small round object on the top of the robot, above the axel) created output like it is off-center: https://youtube.com/shorts/AhgIV4C5a3o Thank for the observation as it explains why setting frame_name solved my issue.

Asked by void_main on 2022-06-18 15:02:23 UTC

Comments