0
$\begingroup$

Rosanswers logo

Hi everyone,

TL;DR How to load ultrasonic in rviz/gazebo11 using Foxy? How to place the robot on the floor properly without flip it in Gazebo11 or crash it in rviz? UDRF code is at bottom.

Full post

I fail to see why can't I load sensor in rviz. I'm using Ros2 Foxy, rviz2, and gazebo11.

So, my ultrasonic sensor should be on joint called ultrasonic. It's attached with one link cylinder called head2 and the board link called sensor.

Somehow, the topic isn't appear on rviz.

Here is the screenshot:

image description

So basically, my URDF look like this:

<?xml version="1.0"?>
<robot name="my_robot" xmlns:xacro="http://ros.org/wiki/xacro">

    <!--INCLUDE FILES-->
<!--    <xacro:include filename="$(find description)/my_robot.gazebo" />-->

  <!-- Define robot constants -->
  <xacro:property name="base_width" value="0.2159"/>
  <xacro:property name="base_length" value="0.1524"/>
  <xacro:property name="base_height" value="0.10795"/>

  <xacro:property name="wheel_radius" value="0.0325"/>
  <xacro:property name="wheel_width" value="0.02"/>
  <xacro:property name="wheel_ygap" value="0.088"/>
  <xacro:property name="wheel_zoff" value="0.05"/>
  <xacro:property name="wheel_xoff" value="0.1"/>

  <xacro:property name="caster_xoff" value="0.14"/>

  <!-- Define some commonly used intertial properties  -->
  <xacro:macro name="box_inertia" params="m w h d">
    <inertial>
      <origin xyz="0 0 0" rpy="${pi/2} 0 ${pi/2}"/>
      <mass value="${m}"/>
      <inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}"/>
    </inertial>
  </xacro:macro>

  <xacro:macro name="cylinder_inertia" params="m r h">
    <inertial>
      <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
      <mass value="${m}"/>
      <inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy = "0" ixz = "0" iyy="${(m/12) * (3*r*r + h*h)}" iyz = "0" izz="${(m/2) * (r*r)}"/>
    </inertial>
  </xacro:macro>

 <!-- Robot Base -->
  <link name="base_link">
    <visual>
      <geometry>
    <box size="0.2159 0.1524 0.10795"/>
      </geometry>
      <material name="Cyan">
    <color rgba="0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
    <geometry>
        <box size="0.2159 0.1524 0.10795"/>
      </geometry>
    </collision>

    <xacro:box_inertia m="15" w="0.2159" d="0.1524" h="0.10795"/>
  </link>

  <!-- Robot Footprint -->
  <link name="base_footprint"/>

  <joint name="base_joint" type="fixed">
    <parent link="base_link"/>
    <child link="base_footprint"/>
    <origin xyz="0.0 0.0 ${-(wheel_radius+wheel_zoff)}" rpy="0 0 0"/>
  </joint>


    <!-- Servos -->
    <link name ="head">
    <visual>
        <geometry>
            <cylinder radius="0.01" length="0.02"/>
        </geometry>
        <material name="white"/>
    </visual>
    <collision>
    <geometry>
        <cylinder radius="0.01" length="0.02"/>
      </geometry>
    </collision>
    <xacro:cylinder_inertia m="0.5" r="0.01" h="0.02"/>
    </link>

    <link name ="head2">
    <visual>
        <geometry>
            <cylinder radius="0.01" length="0.02"/>
        </geometry>
        <material name="Gray">
            <color rgba="0.5 0.5 0.5 1.0"/>
        </material>
    </visual>
    <collision>
    <geometry>
        <cylinder radius="0.01" length="0.02"/>
    </geometry>
    </collision>
    <xacro:cylinder_inertia m="0.5" r="0.01" h="0.02"/>
    </link>

    <!-- Wheels -->
  <xacro:macro name="wheel" params="prefix x_reflect y_reflect">
    <link name="${prefix}_link">
      <visual>
    <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
    <geometry>
    <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
    </geometry>
    <material name="Gray">
      <color rgba="0.5 0.5 0.5 1.0"/>
    </material>
      </visual>
    <collision>
        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
        <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
    </collision>
    <xacro:cylinder_inertia m="0.5" r="0.01" h="0.02"/>
    </link>

      <joint name="${prefix}_joint" type="continuous">
      <parent link="base_link"/>
      <child link="${prefix}_link"/>
      <origin xyz="${x_reflect*wheel_xoff} ${y_reflect * (base_length/2 + wheel_width/2)} ${-wheel_zoff}" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>
    </joint>
  </xacro:macro>

    <!-- Sensor -->
    <link name ="sensor">
    <visual>
        <geometry>
            <box size="0.02 0.04 0.01"/>
        </geometry>
        <material name="Royal Blue">
            <color rgba="83 51 237 1"/>
        </material>
    </visual>
    <collision>
        <geometry>
            <box size="0.02 0.04 0.01"/>
        </geometry>
        <material name="Royal Blue">
            <color rgba="83 51 237 1"/>
        </material>
    </collision>
    <xacro:box_inertia m="15" w="0.02" d="0.04" h="0.01"/>

    </link>

    <!--TEST    -->
<!--        <link name="lidar_link">-->
<!--          <inertial>-->
<!--            <origin xyz="0 0 0" rpy="0 0 0"/>-->
<!--            <mass value="0.125"/>-->
<!--            <inertia ixx="0.001"  ixy="0"  ixz="0" iyy="0.001" iyz="0" izz="0.001" />-->
<!--          </inertial>-->

<!--          <collision>-->
<!--            <origin xyz="0 0 0" rpy="0 0 0"/>-->
<!--            <geometry>-->
<!--               <cylinder radius="0.0508" length="0.055"/>-->
<!--            </geometry>-->
<!--          </collision>-->

<!--          <visual>-->
<!--            <origin xyz="0 0 0" rpy="0 0 0"/>-->
<!--            <geometry>-->
<!--               <cylinder radius="0.0508" length="0.055"/>-->
<!--            </geometry>-->
<!--          </visual>-->
<!--        </link>-->



    <gazebo reference="sensor">
      <sensor name="range" type="ray">
        <always_on>true</always_on>
        <visualize>true</visualize>
        <update_rate>5</update_rate>
        <ray>
          <scan>
            <horizontal>
              <samples>360</samples>
              <resolution>1.000000</resolution>
              <min_angle>0.000000</min_angle>
              <max_angle>6.280000</max_angle>
            </horizontal>
          </scan>
          <range>
            <min>0.120000</min>
            <max>3.5</max>
            <resolution>0.015000</resolution>
          </range>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.01</stddev>
          </noise>
        </ray>
        <plugin name="gazebo_ros_range" filename="libgazebo_ros_range.so">
          <ros>
            <remapping>~/out:=scan</remapping>
          </ros>
          <output_type>sensor_msgs/LaserScan</output_type>
          <frame_name>sensor</frame_name>
        </plugin>
      </sensor>
    </gazebo>

    <joint name="head_attach" type="revolute">
    <origin xyz="0.090 0 0.064" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="head"/>
    <limit effort="10.0" lower="-1.51" upper="1.51" velocity="30"/>
    <dynamics damping="0.0" friction="0.0"/>
    <axis xyz="0 0 1"/>
    </joint>

    <joint name="head_attach2" type="revolute">
    <origin xyz="0 0 0.02" rpy="0 1.6 0"/>
    <parent link="head"/>
    <child link="head2"/>
    <limit effort="1000.0" lower="-1.51" upper="1.51" velocity="30"/>
    <dynamics damping="0.0" friction="0.0"/>
    <axis xyz="0 0 1"/>
    </joint>

<!--    <joint name="boards" type="fixed">-->
<!--        <origin xyz="0.02 0 -0.02"/>-->
<!--        <parent link="head2"/>-->
<!--        <child link="lidar_link"/>-->
<!--        <limit effort="1000.0" lower="-15" upper="15" velocity="30"/>-->
<!--        <dynamics damping="0.0" friction="0.0"/>-->
<!--    </joint>-->

     <joint name="ultrasonic" type="fixed">
    <axis xyz="0 1 0" />
    <origin rpy="0 0 0" xyz="0.02 0 0" />
    <parent link="head2"/>
    <child link="sensor"/>
     </joint>






  <xacro:wheel prefix="drivewhl_rl" x_reflect="-1" y_reflect="1" />
  <xacro:wheel prefix="drivewhl_rr" x_reflect="-1" y_reflect="-1" />
  <xacro:wheel prefix="drivewhl_fl" x_reflect="1" y_reflect="1" />
  <xacro:wheel prefix="drivewhl_fr" x_reflect="1" y_reflect="-1" />


</robot>

With this URDF file, my rviz look like this (it crashed btw)

image description

When it should look like this

image description

Not only that, Gazebo displays the lidar instead of cone plus it wouldn't place properly.

Here is the screenshot

image description

When it should look like this; (Gazebo SDF purely with no ROS plugins)

image description

I don't get it why it crashed when it worked before I add the gazebo11 launch?

CMakeLists.txt

cmake_minimum_required(VERSION 3.5)
project(my_robot_description)

# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

install(
  DIRECTORY src launch rviz config world
  DESTINATION ${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>my_robot_description</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="[email protected]">bwuk</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <exec_depend>joint_state_publisher</exec_depend>
  <exec_depend>robot_state_publisher</exec_depend>
  <exec_depend>rviz</exec_depend>
  <exec_depend>xacro</exec_depend>


  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

display.launch.py

import launch
from launch.substitutions import Command, LaunchConfiguration
import launch_ros
import os


def generate_launch_description():
    pkg_share = launch_ros.substitutions.FindPackageShare(package='my_robot_description').find('my_robot_description')
    default_model_path = os.path.join(pkg_share, 'src/description/my_robot.urdf')
    default_rviz_config_path = os.path.join(pkg_share, 'rviz/urdf_config.rviz')
    world_path = os.path.join(pkg_share, 'world/my_world.sdf'),

    robot_state_publisher_node = launch_ros.actions.Node(
    package='robot_state_publisher',
    executable='robot_state_publisher',
    parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}]
    )
    joint_state_publisher_node = launch_ros.actions.Node(
    package='joint_state_publisher',
    executable='joint_state_publisher',
    name='joint_state_publisher',
    condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui'))
    )
    rviz_node = launch_ros.actions.Node(
    package='rviz2',
    executable='rviz2',
    name='rviz2',
    output='screen',
    arguments=['-d', LaunchConfiguration('rvizconfig')],
    )
    spawn_entity = launch_ros.actions.Node(
    package='gazebo_ros',
    executable='spawn_entity.py',
    arguments=['-entity', 'my_robot', '-topic', 'robot_description'],
    output='screen'
    )

    
    return launch.LaunchDescription([
    launch.actions.DeclareLaunchArgument(name='gui', default_value='True',
                                         description='Flag to enable joint_state_publisher_gui'),
    launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path,
                                         description='Absolute path to robot urdf file'),
    launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path,
                                         description='Absolute path to rviz config file'),
    launch.actions.ExecuteProcess(cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so'], output='screen'),
    
    joint_state_publisher_node,
    robot_state_publisher_node,
    spawn_entity,
    rviz_node
    ])

This is what my tree directories look like image description

What am I missing? If you found what was wrong, please explain it to me so I can see why. As you probably already tell that I'm beginner based on my UDRF code. I started to work with Gazebo/rviz since 2 weeks ago.

Sorry for a long post, it's just that I've been working on this for almost 2 weeks now. I would be lie if I'm not mad about this lol I had to write everything from A to Z, following a lot of tutorials and found out midpoint where SDF isn't fully supported by ROS2. Had to re-write URDF one. It's just that ROS2 have very little tutorials with Gazebo11. I'm hoping to make more tutorials and help others out once I know more.

Thank you very much for your time!

Update 08/18/21

Add this in package.xml

<exec_depend>joint_state_publisher_gui</exec_depend>

Then add this in CMakeList.txt

DESTINATION share/${PROJECT_NAME}

Like this install( DIRECTORY src launch rviz config world DESTINATION share/${PROJECT_NAME} )

Add two things in display.launch.py.

  1. this under def generate_launch_description():

    joint_state_publisher_gui_node = launch_ros.actions.Node( package='joint_state_publisher_gui', executable='joint_state_publisher_gui', name='joint_state_publisher_gui', condition=launch.conditions.IfCondition(LaunchConfiguration('gui')) )

  2. This under return launch.LaunchDescription([

     joint_state_publisher_gui_node,
    

Those fixed the "crashed" model in rviz.

It doesn't fix the robot flipped due to speed velocity (I don't know where to find that one) It doesn't fix the sensor issue in rviz. There is no topic for it nor the sensor being loaded.

Still trying to figure things out.

Update #2 on the same day:

Gazebo showed me this

image description

But I don't see anything on my codes? Is it done by default? If so, how do I make it not move and only follow the command?

Update #3 on Aug 19:

The velocity issue has been fixed. It was this section:

<gazebo reference="front_left_wheel">
    <turnGravityOff>false</turnGravityOff>
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <kp value="1" />
    <kd value="1" />
    <material>Gazebo/Gold </material>
</gazebo>

On each link (wheels and servos aka heads) then it fixed the issue.

However, the sensor doesn't load in gazebo/rviz anymore now.

If anyone came to the same conclusion as velocity issue, feel free to ask me on here and ill show/tell you how to fix it.


Originally posted by kak13 on ROS Answers with karma: 113 on 2021-08-17

Post score: 1


Original comments

Comment by Darkproduct on 2021-08-24:
I'm working on something similar and would like to know how you got the gazebo view in your Update #2. I can't find this anywhere.

Comment by kak13 on 2021-08-24:
Hey! Show me your launch file

Comment by kak13 on 2021-08-24:
@Darkproduct

This is under return launch.LaunchDescription([

launch.actions.ExecuteProcess(cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so'], output='screen'),

This will opens gazebo. To respawn your model in gazebo, use this one under def generate_launch_description():

spawn_entity = launch_ros.actions.Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-entity', 'my_robot', '-topic', 'robot_description'],
output='screen'
)

then add this under return launch.LaunchDescription([

    spawn_entity,

They are in my launch file above. Use the ctrl F to find them as well!

Please let me know if you encounter any issue with this

Comment by Darkproduct on 2021-08-24:
I can open Gazebo and spawn my model just fine. The only thing I can't find is this nice view where you can see and possibly modify the speed and the PID values in Gazebo.

Comment by kak13 on 2021-08-24:
image description

Here, I made the picture for you. You click and hold then pull it to left :)

Comment by Darkproduct on 2021-08-24:
Thanks. I can't believe I never noticed this.

Comment by Darkproduct on 2021-08-24:
For you actual question: Launch gazebo in verbose mode with --verbose. This will help finding any problems with your configuration. I looked at your gazebo tag for the sensor link and it looks good. Look at the bottem launch file in Update 2 from my answer here to see an example on how to launch gazebo in verbose mode. https://answers.ros.org/question/384190/a-plugin-for-individual-wheel-on-a-4-wheel-robot-in-gazebo-ros2-foxy/?answer=384990#post-id-384990

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

answer from DarkProduct fixed this one

https://answers.ros.org/question/384190/a-plugin-for-individual-wheel-on-a-4-wheel-robot-in-gazebo-ros2-foxy/?answer=384990#post-id-384990


Originally posted by kak13 with karma: 113 on 2021-08-24

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

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.