/odom topic doesn't work on ROS2
Hey all! I'm having some problems with Gazebo. I'm using ROS2 foxy on UBUNTU 20.04.5 LTS with Gazebo installed via ros-foxy-gazebo. I made an URDF model on a differential drive robot, with a launch file to run gazebo and spawn the robot. Even though, when launching the file, it appears on the terminal that the diffdrive is advertising the /odom and publishing the tf between odom frame and baselink, i cannot find the frame odom in RVIZ2. Moreover, when echoing the /odom topic i have no results. Finally, in the viewframes the odom frame doesn't appear. I guess that i did the correct launch file with the diffdrive control plugin and the good parameters. Here below the files. Launch file:
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
def generate_launch_description():
# Include the robot_state_publisher launch file, provided by our own package. Force sim time to be enabled
# !!! MAKE SURE YOU SET THE PACKAGE NAME CORRECTLY !!!
package_name = 'package_test' # <--- CHANGE ME
rsp = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory(
package_name), 'launch', 'rsp.launch.py'
)]), launch_arguments={'use_sim_time': 'true'}.items()
)
# Include the Gazebo launch file, provided by the gazebo_ros package
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]),
)
# Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
arguments=['-topic', 'robot_description',
'-entity', 'my_bot'],
output='screen')
# Launch them all!
return LaunchDescription([
rsp,
gazebo,
spawn_entity,
])
Gazebo plugin:
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="robot">
<gazebo>
<plugin name="diff_drive" filename="libgazebo_ros_diff_drive.so">
<!-- Wheel info-->
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>0.35</wheel_separation>
<wheel_diameter>0.1</wheel_diameter>
<!-- Limits-->
<max_wheel_torque>200</max_wheel_torque>
<max_wheel_acceleration>10.0</max_wheel_acceleration>
<!-- Output-->
<odometry_frame>odom</odometry_frame>
<robot_base_frame>base_link</robot_base_frame>
<publish_odom>true</publish_odom>
<publish_odom_tf>true</publish_odom_tf>
<publish_wheel_tf>true</publish_wheel_tf>
</plugin>
</gazebo>
</robot>
URDF file:
<?xml version="1.0"?>
<xacro:include filename="inertial_macros.xacro"/>
<material name="white">
<color rgba="1 1 1 1" />
</material>
<material name="orange">
<color rgba="1 0.3 0.1 1"/>
</material>
<material name="blue">
<color rgba="0.2 0.2 1 1"/>
</material>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
<!-- BASE LINK -->
<link name="base_link">
</link>
<!-- CHASSIS LINK -->
<joint name="chassis_joint" type="fixed">
<parent link="base_link"/>
<child link="chassis"/>
<origin xyz="-0.1 0 0"/>
</joint>
<link name="chassis">
<visual>
<origin xyz="0.15 0 0.075"/>
<geometry>
<box size="0.3 0.3 0.15"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin xyz="0.15 0 0.075"/>
<geometry>
<box size="0.3 0.3 0.15"/>
</geometry>
</collision>
</link>
<gazebo reference="right_wheel">
<material>Gazebo/White</material>
</gazebo>
<!-- LEFT WHEEL LINK -->
<joint name="left_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="left_wheel"/>
<origin xyz="0 0.175 0" rpy="-${pi/2} 0 0" />
<axis xyz="0 0 1"/>
</joint>
<link name="left_wheel">
<visual>
<geometry>
<cylinder radius="0.05" length="0.04"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<geometry>
<cylinder radius="0.05" length="0.04"/>
</geometry>
</collision>
<xacro:inertial_cylinder mass="0.1" length="0.04" radius="0.05">
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:inertial_cylinder>
</link>
<gazebo reference="left_wheel">
<material>Gazebo/Blue</material>
</gazebo>
<!-- RIGHT WHEEL LINK -->
<joint name="right_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="right_wheel"/>
<origin xyz="0 -0.175 0" rpy="${pi/2} 0 0" />
<axis xyz="0 0 -1"/>
</joint>
<link name="right_wheel">
<visual>
<geometry>
<cylinder radius="0.05" length="0.04"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<geometry>
<cylinder radius="0.05" length="0.04"/>
</geometry>
</collision>
<xacro:inertial_cylinder mass="0.1" length="0.04" radius="0.05">
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:inertial_cylinder>
</link>
<gazebo reference="right_wheel">
<material>Gazebo/Blue</material>
</gazebo>
<!-- CASTER WHEEL LINK -->
<joint name="caster_wheel_joint" type="fixed">
<parent link="chassis"/>
<child link="caster_wheel"/>
<origin xyz="0.24 0 0"/>
</joint>
<link name="caster_wheel">
<visual>
<geometry>
<sphere radius="0.05"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<sphere radius="0.05"/>
</geometry>
</collision>
<xacro:inertial_sphere mass="0.1" radius="0.05">
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:inertial_sphere>
</link>
<gazebo reference="caster_wheel">
<material>Gazebo/Black</material>
</gazebo>
the file rsp.launch
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
import xacro
def generate_launch_description():
# Check if we're told to use sim time
use_sim_time = LaunchConfiguration('use_sim_time')
# Process the URDF file
pkg_path = '/home/andi/dev_ws/src/package_test'
xacro_file = os.path.join(pkg_path, 'description', 'robot.urdf.xacro')
robot_description_config = xacro.process_file(xacro_file)
# Create a robot_state_publisher node
params = {'robot_description': robot_description_config.toxml(),
'use_sim_time': use_sim_time}
node_robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[params]
)
# Launch!
return LaunchDescription([
DeclareLaunchArgument(
'use_sim_time',
default_value='true',
description='Use sim time if true'),
node_robot_state_publisher
])
Finally, this is the output on the terminal when running the launch file:
[robot_state_publisher-1] Parsing robot urdf xml string.
[robot_state_publisher-1] Link chassis had 1 children
[robot_state_publisher-1] Link caster_wheel had 0 children
[robot_state_publisher-1] Link left_wheel had 0 children
[robot_state_publisher-1] Link right_wheel had 0 children
[robot_state_publisher-1] [INFO] [1671286215.419064105] [robot_state_publisher]: got segment base_link
[robot_state_publisher-1] [INFO] [1671286215.419196314] [robot_state_publisher]: got segment caster_wheel
[robot_state_publisher-1] [INFO] [1671286215.419212848] [robot_state_publisher]: got segment chassis
[robot_state_publisher-1] [INFO] [1671286215.419220446] [robot_state_publisher]: got segment left_wheel
[robot_state_publisher-1] [INFO] [1671286215.419226922] [robot_state_publisher]: got segment right_wheel
[spawn_entity.py-4] [INFO] [1671286215.849066532] [spawn_entity]: Spawn Entity started
[spawn_entity.py-4] [INFO] [1671286215.849747418] [spawn_entity]: Loading entity published on topic robot_description
[spawn_entity.py-4] [INFO] [1671286215.851772580] [spawn_entity]: Waiting for entity xml on robot_description
[spawn_entity.py-4] [INFO] [1671286215.864727286] [spawn_entity]: Waiting for service /spawn_entity, timeout = 30
[spawn_entity.py-4] [INFO] [1671286215.865147088] [spawn_entity]: Waiting for service /spawn_entity
[spawn_entity.py-4] [INFO] [1671286219.387568115] [spawn_entity]: Calling service /spawn_entity
[gzserver-2] Warning [parser_urdf.cc:1130] multiple inconsistent <name> exists due to fixed joint reduction overwriting previous
value [Gazebo/White] with [Gazebo/Blue].
[spawn_entity.py-4] [INFO] [1671286219.677514717] [spawn_entity]: Spawn status: SpawnEntity: Successfully spawned entity
[my_bot]
[gzserver-2] [INFO] [1671286219.744421519] [diff_drive]: Wheel pair 1 separation set to [0.350000m]
[gzserver-2] [INFO] [1671286219.746272383] [diff_drive]: Wheel pair 1 diameter set to [0.100000m]
[gzserver-2] [INFO] [1671286219.751120486] [diff_drive]: Subscribed to [/cmd_vel]
[gzserver-2] [INFO] [1671286219.759056484] [diff_drive]: Advertise odometry on [/odom]
[gzserver-2] [INFO] [1671286219.763536717] [diff_drive]: Publishing odom transforms between [odom] and [base_link]
[gzserver-2] [INFO] [1671286219.763606642] [diff_drive]: Publishing wheel transforms between [base_link], [left_wheel_joint]
and [right_wheel_joint]
[INFO] [spawn_entity.py-4]: process has finished cleanly [pid 128488]
Asked by andi on 2022-12-17 18:02:05 UTC
Answers
I checked your code and can't find the errror, but you can try this other way to do the same, maybe it help you find your error:
1) Instead of using xacro file, you can create a URDF file from this XACRO file:
ros2 run xacro xacro {name_you_want}.xacro > {name_you_want}.urdf
2) Now you have an automatic URDF file created from XACRO file, if it fails on the process, maybe it tells you where the error is on the XACRO file.
3) From that URDF file you can create your gazebo model using this:
gz sdf -p {name_you_want}.urdf > ../{folder_you_want_to_keep_model}/{name_you_want}.sdf
4) Open gazebo from terminal using
gazebo
5) Import the model you created in step (4). Save the world
6) In your launch file, access your automated URDF file and gazebo world:
import os
from ament_index_python.packages import get_package_share_directory
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions.execute_process import ExecuteProcess
#=====================================
# VARIABLES
#=====================================
pkg_name = 'package_test' # your package name
urdf_folder = 'description' # I saw on your code your xacro file is in a folder named description
model_name = 'robot.urdf' # The urdf created automatically by xacro conversion command. This is inside description folder
world_folder = 'world_folder_name'
world_name = 'robot_world'
#=====================================
# LAUNCH CODE
#=====================================
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
urdf_file = os.path.join(get_package_share_directory(pkg_name),urdf_folder, model_name)
world_file = os.path.join(get_package_share_directory(pkg_name), world_folder, world_name)
return LaunchDescription([
DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use simulation (Gazebo) clock if true'),
# --------- ROBOT STATE PUBLISHER ----------
Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
parameters=[{' use_sim_time': use_sim_time}],
arguments=[urdf_file],
),
# ------- Gazebo --------------
ExecuteProcess(
name='START-GAZEBO-SERVER',
cmd=['gzserver',
'-s', 'libgazebo_ros_init.so',
'-s', 'libgazebo_ros_factory.so',
world_file]),
ExecuteProcess(name='START-GAZEBO-CLIENT', cmd=['gzclient'])
])
Hope it helps.
Asked by iggyrrieta on 2022-12-19 07:27:34 UTC
Comments