Initially I would like to write a simple controller for an skid steer robot with the possibility of later use with slam_tooolbox. I'm trying to write my odometry publisher, for later use with slam_toolbox. Took the example from the ROS2 tf2 tutorial to create the publisher, but the publisher written this way doesn't work because when I select odom as the "Fixed frame" and Rviz gives an error about no transformation from odom -> base_link. After that I linked my publisher to the simulation time, but now I get an error:
Message Filter dropping message: frame 'base_link' at time 47.226 for reason 'discarding message because the queue is full'
I tried to make an analogy with diff_driver_controller, but I couldn't make sense of it
My state_publisher:
from math import sin, cos, pi
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from geometry_msgs.msg import Quaternion
from rosgraph_msgs.msg import Clock
from sensor_msgs.msg import JointState
from builtin_interfaces.msg import Time
from nav_msgs.msg import Odometry
from tf2_ros import TransformBroadcaster, TransformStamped
class StatePublisher(Node):
def __init__(self):
rclpy.init()
super().__init__('state_publisher')
qos_profile = QoSProfile(depth=10)
self.odomPub = self.create_publisher(Odometry, 'odom', qos_profile)
# self.timeSub = self.create_subscription(Clock, 'clock', self.get_clock_time, 10)
self.broadcaster = TransformBroadcaster(self)
self.nodeName = self.get_name()
self.get_logger().info("{0} started".format(self.nodeName))
# self.clock_time = Time()
# self.clock_time.sec = 0
# self.clock_time.nanosec = 0
degree = pi / 180.0
loop_rate = self.create_rate(30)
# robot state
tilt = 0.
tinc = degree
swivel = 0.
angle = 0.
height = 0.
hinc = 0.005
try:
while rclpy.ok():
rclpy.spin_once(self)
self.update()
loop_rate.sleep()
except KeyboardInterrupt:
pass
def get_clock_time(self,msg):
self.clock_time = msg.clock
def update(self):
# message declarations
odom_trans = TransformStamped()
odom_trans.header.frame_id = 'odom'
odom_trans.child_frame_id = 'base_link'
now = self.get_clock().now()
# update transform
odom_trans.header.stamp = now.to_msg()
odom_trans.transform.translation.x = 0.
odom_trans.transform.translation.y = 0.0
odom_trans.transform.translation.z = 0.0
odom_trans.transform.rotation = euler_to_quaternion(0, 0, 0) # roll,pitch,ya
# send the joint state and transfor
self.broadcaster.sendTransform(odom_trans)
odom = Odometry()
odom.header.stamp = now.to_msg()
odom.header.frame_id = 'odom'
odom.pose.pose.position.x = 0.0
odom.pose.pose.position.y = 0.0
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation = euler_to_quaternion(0, 0, 0)
odom.child_frame_id = 'base_link'
odom.twist.twist.linear.x = 0.0
odom.twist.twist.linear.y = 0.0
odom.twist.twist.angular.z = 0.0
# This will adjust as needed per iteration
self.odomPub.publish(odom)
#self.get_logger().info("UPDATE_END")
def spin(self):
loop_rate = self.create_rate(30)
self.get_logger().info("SPIN")
while rclpy.ok():
self.get_logger().info("IN")
self.update()
loop_rate.sleep()
self.get_logger().info("AFTER_SLEEP")
def euler_to_quaternion(roll, pitch, yaw):
qx = sin(roll/2) * cos(pitch/2) * cos(yaw/2) - cos(roll/2) * sin(pitch/2) * sin(yaw/2)
qy = cos(roll/2) * sin(pitch/2) * cos(yaw/2) + sin(roll/2) * cos(pitch/2) * sin(yaw/2)
qz = cos(roll/2) * cos(pitch/2) * sin(yaw/2) - sin(roll/2) * sin(pitch/2) * cos(yaw/2)
qw = cos(roll/2) * cos(pitch/2) * cos(yaw/2) + sin(roll/2) * sin(pitch/2) * sin(yaw/2)
return Quaternion(x=qx, y=qy, z=qz, w=qw)
def main():
node = StatePublisher()
# loop_rate = node.create_rate(30)
# try:
# while rclpy.ok():
# rclpy.spin_once(node)
# node.update()
# loop_rate.sleep()
# except KeyboardInterrupt:
# node.get_logger().info("LOX")
if __name__ == '__main__':
main()
My launch file:
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription, TimerAction
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch.substitutions import ThisLaunchFileDir
from launch_ros.actions import Node
import xacro
def generate_launch_description():
# Specify the name of the package and path to xacro file within the package
pkg_name = 'beginning'
file_subpath = 'urdf/rover_1.xacro'
urdf_tutorial_path = get_package_share_directory(pkg_name)
default_rviz_config_path = os.path.join(urdf_tutorial_path, 'rviz', 'urdf1.rviz')
# Use xacro to process the file
xacro_file = os.path.join(get_package_share_directory(pkg_name),file_subpath)
robot_description_raw = xacro.process_file(xacro_file).toxml()
world_path = LaunchConfiguration('world')
world_path_declare = DeclareLaunchArgument('world', default_value='',description='World to open')
# Configure the node
node_robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[{'robot_description': robot_description_raw,
'use_sim_time': True}] # add other parameters here if required
)
gui = DeclareLaunchArgument('gui', default_value='true',
description='Set to "false" to run headless.')
server = DeclareLaunchArgument('server', default_value='true',
description='Set to "false" not to run gzserver.')
delay = DeclareLaunchArgument('delay', default_value='0')
# TF
node_odom_state_publisher = Node(
package='beginning',
executable='state_publisher',
name='state_publisher',
output='screen',
parameters=[{'use_sim_time': True}]
)
# GAZEBO
gazebo_params_path = os.path.join(get_package_share_directory(pkg_name),'config','gazebo_params.yaml')
include_server = IncludeLaunchDescription(PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/gzserver.launch.py']),
condition=IfCondition(LaunchConfiguration('server')),
launch_arguments={'world': world_path, 'extra_gazebo_args': '--verbose'}.items()) #'extra_gazebo_args': '--ros-args --params-file ' + gazebo_params_path
include_client = IncludeLaunchDescription(PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/gzclient.launch.py']),#'params_file': gazebo_params_path,
condition=IfCondition(LaunchConfiguration('gui'))) #,
# launch_arguments={'extra_gazebo_args': '--ros-args --params-file ' + gazebo_params_path }.items())
print('CHECKPOINT1')
spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
arguments=['-topic', 'robot_description',
'-entity', 'my_bot',
'-z','5'],
output='screen')
joint_state_publisher = Node(
package='joint_state_publisher',
executable='joint_state_publisher',
)
# RVIZ
rviz = Node(
package='rviz2',
executable='rviz2',
output='screen',
arguments=['-d', default_rviz_config_path],
)
timer_spawn = TimerAction(period = '0', actions=[spawn_entity])
# CONTROL
left_wheel_1_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=["left_wheel_1"],
)
diff_drive_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=["diff_cont"],
)
joint_broad_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=["joint_state_broadcaster"],
)
# Run the node
return LaunchDescription([
world_path_declare,
gui,
server,
include_server,
include_client,
node_robot_state_publisher,
node_odom_state_publisher,
left_wheel_1_spawner,
# diff_drive_spawner,
joint_broad_spawner,
timer_spawn,
rviz,
# joint_state_publisher
])
I haven't found any good guides on writing my odometry publisher or map publisher using tf. Maybe there is another way to do it, please guide me.