0
$\begingroup$

I have a robot system defined in an SDF with the following:

<sdf version='1.9'>
    <model name='simple_ddr' canonical_link='base_link'>

        <plugin filename="gz-sim-diff-drive-system" name="gz::sim::systems::DiffDrive">
            <left_joint>joint_wheelL</left_joint>
            <right_joint>joint_wheelR</right_joint>
            <wheel_separation>0.4</wheel_separation>
            <wheel_radius>0.1</wheel_radius>
            <odom_publish_frequency>0.1</odom_publish_frequency>
        </plugin>

        <plugin
            filename="gz-sim-sensors-system"
            name="gz::sim::systems::Sensors">
            <render_engine>ogre2</render_engine>
        </plugin>

        
        <link name='base_link'>
          <pose>0 0 0 0 0 0</pose>

            <inertial>
                <mass>1.0</mass>
                <inertia>
                    <ixx>0.011041666666666665</ixx>
                    <ixy>0.0</ixy>
                    <ixz>0.0</ixz>
                    <iyy>0.02604166666666667</iyy>
                    <iyz>0.0</iyz>
                    <izz>0.03541666666666667</izz>
                </inertia>
            </inertial>

            <collision name='base_link'>
              <pose>0 0 0 0 0 0</pose>
              <geometry>
                <box>
                  <size>0.55 0.35 0.1</size>
                </box>
              </geometry>
            </collision>

            <visual name='base_link'>
              <pose>0 0 0 0 0 0</pose>
              <geometry>
                <box>
                  <size>0.55 0.35 0.1</size>
                </box>
              </geometry>

              <material>
                  <ambient>0.0 0.6 0.6 1</ambient>
                  <diffuse>0.0 0.6 0.6 1</diffuse>
                  <specular>0.0 0.6 0.6 1</specular>
              </material>
            </visual>

            <sensor name="imu_sensor" type="imu">
                <always_on>1</always_on>
                <update_rate>1</update_rate>
                <visualize>true</visualize>
                <topic>imu</topic>
            </sensor>

            <sensor name='gpu_lidar' type='gpu_lidar'>"
                <pose relative_to='lidar_frame'>0 0 0 0 0 0</pose>
                <topic>lidar</topic>
                <update_rate>10</update_rate>
                <ray>
                    <scan>
                        <horizontal>
                            <samples>640</samples>
                            <resolution>0.01</resolution>
                            <min_angle>-1.396263</min_angle>
                            <max_angle>1.396263</max_angle>
                        </horizontal>
                        <vertical>
                            <samples>1</samples>
                            <resolution>0.01</resolution>
                            <min_angle>0</min_angle>
                            <max_angle>0</max_angle>
                        </vertical>
                    </scan>
                    <range>
                        <min>0.08</min>
                        <max>10.0</max>
                        <resolution>0.01</resolution>
                    </range>
                </ray>
                <always_on>1</always_on>
                <visualize>1</visualize>
            </sensor>
        </link>
      
        <joint name='joint_wheelL' type='revolute'>
            <pose relative_to='base_link'>-0.2 -0.2 0.0 0 0 0</pose>
            <parent>base_link</parent>
            <child>wheelL</child>
            <axis>
            <xyz>0 1 0</xyz>
            <limit>
                <lower>-inf</lower>
                <upper>inf</upper>
            </limit>
            <dynamics>
                <damping>10</damping>
                <friction>1</friction>
                <spring_reference>0</spring_reference>
                <spring_stiffness>0</spring_stiffness>
            </dynamics>
            </axis>
        </joint>
        
        <link name='wheelL'>
            <pose relative_to='joint_wheelL'>0 0 0 0 0 0</pose>
            <inertial>
            <pose>0 0 0 0 0 0</pose>
            <mass>0.050000000000000003</mass>
            <inertia>
                <ixx>0.001625000000000001</ixx>
                <ixy>0</ixy>
                <ixz>0</ixz>
                <iyy>0.00025000000000000011</iyy>
                <iyz>0</iyz>
                <izz>0.001625000000000001</izz>
            </inertia>
            </inertial>
            <collision name='wheelL_collision'>
            <pose>0 0 0 -1.5707963300000001 0 0</pose>
            <geometry>
                <cylinder>
                <length>0.050000000000000003</length>
                <radius>0.10000000000000001</radius>
                </cylinder>
            </geometry>
            </collision>
            <visual name='wheelL_visual'>
            <pose>0 0 0 -1.5707963300000001 0 0</pose>
            <geometry>
                <cylinder>
                <length>0.050000000000000003</length>
                <radius>0.10000000000000001</radius>
                </cylinder>
            </geometry>

                <material>
                    <ambient>1.0 0.0 0.0 1</ambient>
                    <diffuse>1.0 0.0 0.0 1</diffuse>
                    <specular>1.0 0.0 0.0 1</specular>
                </material>
            </visual>
        </link>

        <joint name='joint_wheelR' type='revolute'>
            <pose relative_to='base_link'>-0.2 0.2 0.0 0 0 0</pose>
            <parent>base_link</parent>
            <child>wheelR</child>
            <axis>
            <xyz>0 1 0</xyz>
            <limit>
                <lower>-inf</lower>
                <upper>inf</upper>
            </limit>
            <dynamics>
                <damping>10</damping>
                <friction>1</friction>
                <spring_reference>0</spring_reference>
                <spring_stiffness>0</spring_stiffness>
            </dynamics>
            </axis>
        </joint>

        <link name='wheelR'>
            <pose relative_to='joint_wheelR'>0 0 0 0 0 0</pose>
            <inertial>
            <pose>0 0 0 0 0 0</pose>
            <mass>0.050000000000000003</mass>
            <inertia>
                <ixx>0.001625000000000001</ixx>
                <ixy>0</ixy>
                <ixz>0</ixz>
                <iyy>0.00025000000000000011</iyy>
                <iyz>0</iyz>
                <izz>0.001625000000000001</izz>
            </inertia>
            </inertial>
            <collision name='wheelR_collision'>
            <pose>0 0 0 -1.5707963300000001 0 0</pose>
            <geometry>
                <cylinder>
                <length>0.050000000000000003</length>
                <radius>0.10000000000000001</radius>
                </cylinder>
            </geometry>
            </collision>
            <visual name='wheelR_visual'>
            <pose>0 0 0 -1.5707963300000001 0 0</pose>
            <geometry>
                <cylinder>
                <length>0.050000000000000003</length>
                <radius>0.10000000000000001</radius>
                </cylinder>
            </geometry>
                <material>
                    <ambient>1.0 0.0 0.0 1</ambient>
                    <diffuse>1.0 0.0 0.0 1</diffuse>
                    <specular>1.0 0.0 0.0 1</specular>
                </material>
            </visual>
        </link>

        <link name='caster'>
            <pose relative_to='joint_caster'>0 0 0.0 0 0 0</pose>
            <inertial>
                <pose>0 0 0 0 0 0</pose>
                <mass>0.005</mass>
                <inertia>
                    <ixx>0.001625000000000001</ixx>
                    <ixy>0</ixy>
                    <ixz>0</ixz>
                    <iyy>0.00025000000000000011</iyy>
                    <iyz>0</iyz>
                    <izz>0.001625000000000001</izz>
                </inertia>
            </inertial>
            <collision name='caster_collision'>
                <geometry>
                    <sphere>
                        <radius>0.025</radius>
                    </sphere>
                </geometry>
                <surface>
                    <friction>
                        <ode>
                            <mu> 0.0 </mu>
                            <mu2> 0.0 </mu2>
                        </ode>
                    </friction>
                </surface>
            </collision>
            <visual name='caster_visual'>
                <geometry>
                    <sphere>
                        <radius>0.025</radius>
                    </sphere>
                </geometry>
            </visual>
        </link>

        <joint name='joint_caster' type='fixed'>
            <pose relative_to='base_link'>0.2 0 -0.075 0 0 0</pose>
            <parent>base_link</parent>
            <child>caster</child>
        </joint>
      
        <frame name='joint_body' attached_to='base_link'>
            <pose>0 0 0 0 0 0</pose>
        </frame>


        <frame name="lidar_frame" attached_to='base_link'>
            <pose>0.8 0 0.5 0 0 0</pose>
        </frame>

    </model>
</sdf>

I put everything into 2 launch files, a start_gazebo launch file and a spawn_robot launch file:

def generate_launch_description():

    pkg_robot_models = get_package_share_directory('robot_models')
    #######################################################################################################################################
    # Start gazebo

    world_file = os.path.join(pkg_robot_models, 'worlds', 'test_world.sdf')

    world_launch = IncludeLaunchDescription(
            PythonLaunchDescriptionSource([
                PathJoinSubstitution([
                    FindPackageShare('ros_gz_sim'),
                    'launch',
                    'gz_sim.launch.py'
                ])
            ]),
            launch_arguments={
                'gz_args': world_file + ' -r -v 4'
            }.items()
        )

    #######################################################################################################################################
    # Run the node
    return LaunchDescription([
        world_launch
    ])
def generate_launch_description():

    pkg_robot_models = get_package_share_directory('robot_models')
    #######################################################################################################################################
    # Spawn robot system

    robot_filepath = os.path.join(pkg_robot_models, 'models', 'simple_DDR', 'model.sdf')

    ROBOT_MODEL = 'simple_DDR'

    start_gazebo_ros_spawner_cmd = Node(
        package='ros_gz_sim',
        executable='create',
        arguments=[
            '-name', ROBOT_MODEL ,
            '-file', robot_filepath,
            '-x', '0.0',
            '-y', '0.0',
            '-z', '0.3'
        ],
        output='screen',
    )

    #######################################################################################################################################
    # Specify the name of the package and path to xacro file within the package
    os.environ["GAZEBO_MODEL_PATH"] = os.path.join(pkg_robot_models, "models", 'simple_DDR', 'model.sdf')
    os.environ["LIBGL_ALWAYS_SOFTWARE"] = "1"

    bridge_laser = Node(
            package='ros_gz_bridge',
            namespace='',
            executable='parameter_bridge',
            name='lidar',
            arguments=["/lidar/points@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan", "--ros-args", "-r", "/lidar/points:=/laser_scan_cloud"]
        )
    
    bridge_clock = Node(
            package='ros_gz_bridge',
            namespace='',
            executable='parameter_bridge',
            name='lidar',
            arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"]
        )


    bridge_tf = Node(
            package='ros_gz_bridge',
            namespace='',
            executable='parameter_bridge',
            name='lidar',
            arguments=["/model/simple_DDR/tf@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V"]
        )
    
    bridge_odom = Node(
            package='ros_gz_bridge',
            namespace='',
            executable='parameter_bridge',
            name='lidar',
            arguments=["/model/simple_DDR/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry"]
        )

    #######################################################################################################################################
    # Run the node
    return LaunchDescription([
        start_gazebo_ros_spawner_cmd,
        bridge_laser,
        bridge_clock,
        bridge_odom,
        bridge_tf
    ])

Result: communication does not work. Point cloud in RVIZ says Showing [0] messages of [0] received, no transform data or frame are sent, ...

Edit: Some additional information. ROS 2 Iron, Gazebo Harmonic, build of ros_gz was done with GZ_VERSION=harmonic

$\endgroup$
11
  • $\begingroup$ Are you getting data from the LiDAR at all? Echo it and verify. $\endgroup$
    – Nikolai
    Feb 27 at 10:33
  • $\begingroup$ @Nikolai when I do gz topic -e -t /lidar and gz topic -e -t /lidar/points I get messages $\endgroup$ Feb 27 at 11:06
  • $\begingroup$ OK, but what about the ROS topic? Are you receiving anything there? RVIZ listens to ros topics not gazebo topics $\endgroup$
    – Nikolai
    Feb 27 at 11:15
  • $\begingroup$ @Nikolai ROS topic receives nothing, that is the issue. I do ros2 topic echo /laser_scan_cloud $\endgroup$ Feb 27 at 11:23
  • $\begingroup$ Is there a reason why you built ros_gz from source instead of using the official binary (with apt)? also, you don't need to have many ros_gz_bridge, one is enough, and you can pass a yaml file containing all the bridge config. Finally, I see that the three bridge nodes have the same name, this might cause conflicts. Another thing you can check, is make sure that your ros_gz bridge executable links with the correct gz-harmonic libs (use ldd) $\endgroup$
    – ejalaa12
    Feb 27 at 13:22

1 Answer 1

1
$\begingroup$

Have you looked at this launch file? Might be something with how you run the bridge.

$\endgroup$
1
  • 1
    $\begingroup$ I run it and works properly. I will take a look at the launch file and the sdf files associated and update in a comment to this answer with problem source. $\endgroup$ Feb 27 at 11:57

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.