In this section we are going to create a ROS2 node that will
receive data (lidar) from the robot (implemented in
diff_drive_controller_bot) and send it commands to
move around without bumping into the walls.
Precice navigation is not something we are after in this
section, so the algorithm I am going to use is extremely
primitive: we only focus on sending and receiving data.
The code is available here.
Note that, using Gazebo's built-in editor, I have created a primitive labyrinth "world" called maze.sdf. As this world is going to be used by more than one package, I have placed it in src/worlds.
Let's start our simulation and see what topics are active. Open a new terminal, and type:
# Before we started it:
# In Terminal 1:
$ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r /cmd_vel:=/diff_cont/cmd_vel_unstamped
# In Terminal 2:
$ ros2 topic list -t /diff_cont/cmd_vel_unstamped [geometry_msgs/msg/Twist]
# After we started it:
# In Terminal 1:
$ ros2 launch diff_drive_controller_bot launch_sim.launch world:=src/worlds/shapes.sdf
# In Terminal 2:
$ ros2 topic list -t
/camera/camera_info [sensor_msgs/msg/CameraInfo]
/camera/image_raw [sensor_msgs/msg/Image]
/clock [rosgraph_msgs/msg/Clock]
/diff_cont/cmd_vel_unstamped [geometry_msgs/msg/Twist]
/dynamic_joint_states [control_msgs/msg/DynamicJointState]
/joint_states [sensor_msgs/msg/JointState]
/laser_controller/out [sensor_msgs/msg/LaserScan]
/odom [nav_msgs/msg/Odometry]
/performance_metrics [gazebo_msgs/msg/PerformanceMetrics]
/robot_description [std_msgs/msg/String]
/tf [tf2_msgs/msg/TFMessage]
/tf_static [tf2_msgs/msg/TFMessage]
Try sending commands:
ros2 topic pub /demo/cmd_demo geometry_msgs/Twist '{linear: {x: 1.0}}' -1
ros2 topic pub /demo/cmd_demo geometry_msgs/Twist '{angular: {z: 0.1}}' -1
Try listening to odometry:
ros2 topic echo /demo/odom_demo
Try listening to TF:
ros2 run tf2_ros tf2_echo odom_demo chassis
ros2 run tf2_ros tf2_echo chassis right_wheel
ros2 run tf2_ros tf2_echo chassis left_wheel
Loading new map:
ros2 service call /map_server/load_map nav2_msgs/srv/LoadMap "{map_url:
$HOME/SnowCron/ros_projects/harsh/src/maps/map_01_binary.yaml}"
Odometry message:
ros2 topic pub /robot1/odom nav_msgs/Odometry "{header: {stamp: {sec: 28, nanosec: 786000000}, frame_id: 'robot1/odom'}, child_frame_id: 'robot1/base_link', pose: {pose: {position: {x: 0.006435876230684812, y: 0.5018193951070563, z: 0.1500022532872065}, orientation: {x: -1.202497725575462e-06, y: 4.545254229127168e-06, z: -0.009871243297678176, w: 0.9999512780799137}}, covariance: [1.0e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]}, twist: {twist: {linear: {x: -4.151254536741296e-05, y: -0.0003711630071032284, z: 0.0}, angular: {x: 0.0, y: 0.0, z: -0.00035124248507245457}}, covariance: [1.0e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]}}"
Imu message:
ros2 topic pub /imu sensor_msgs/Imu "{header: {stamp: {sec: 99, nanosec: 1000000}, frame_id: 'base_link'}, orientation: {x: -3.2484036403009136e-07, y: -1.233682290372918e-07, z: -5.640383185432385e-06, w: 0.9999999999840328}, orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], angular_velocity: {x: -0.00017136447475541198, y: 0.00013966822040950878, z: 4.4918826200876715e-06}, angular_velocity_covariance: [4.0e-08, 0.0, 0.0, 0.0, 4.0e-08, 0.0, 0.0, 0.0, 4.0e-08], linear_acceleration: {x: -0.020305982380950664, y: -0.005017642374176924, z: 9.765243743038877}, linear_acceleration_covariance: [0.00028900000000000003, 0.0, 0.0, 0.0, 0.00028900000000000003, 0.0, 0.0, 0.0, 0.00028900000000000003]}"
Removing environment variable (colcon sets it, and sometimes we want to clean)
unset VARIABLE_NAME
Open up a new terminal window, and type the following command to make the robot move forward at a speed of 1.0 meters per second:
$ ros2 topic pub /demo/cmd_demo geometry_msgs/Twist '{linear: {x: 1.0}}' -1
A robot moves, as expected.
Let's start gazebo again:
$ gazebo
... and insert our (not Foxy sample) robot in it. Now we can open a terminal and issue a command:
$ ros2 topic pub /demo/cmd_vel geometry_msgs/Twist '{linear: {x: 1.0}}' -1
... to make our robot move.
$ cd ~/SnowCron/ros_projects/harsh
$ tree --dirsfirst src/ros2_teleop/
src/ros2_teleop/
├── launch
│ └── ros2_teleop.launch.py
├── resource
│ └── ros2_teleop # Empty
├── ros2_teleop
│ ├── __init__.py # Empty
│ └── robot_controller.py
├── package.xml
├── setup.cfg
└── setup.py
As you can see, it is just a Python ROS2 node, with no URDF involved. It will run from Terminal, and exchange data with diff_drive_controller_bot.
import os
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(package='ros2_teleop', executable='robot_controller', output='screen')
])
Our package is called ros2_teleop because it works under ROS2 and does things similar to what teleop_twist_keyboard does.
We instruct the system to start a node and look for a "main" function in robot_controller.py
Nothing unexpected in this file: human-readable info, like (C) and e.mail and dependencies.
[develop]
script-dir=$base/lib/ros2_teleop
[install]
install-scripts=$base/lib/ros2_teleop
An automatically (if you use ROS2 utility to create a package) file.
import os
from setuptools import setup
from glob import glob
package_name = 'ros2_teleop'
setup(
name=package_name,
version='0.0.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
(os.path.join('share', package_name), glob('launch/*.launch.py'))
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='harsh',
maintainer_email='contact@mail.com',
description='TODO: Package description',
license='TODO: License declaration',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'robot_controller = ros2_teleop.robot_controller:main'
],
},
)
The only interesting string here is
'robot_controller = ros2_teleop.robot_controller:main'
If you (like myself) prefer copying projects rather than creating them from
scratch, you need to pay attention to package names in places like
this: ROS2 does not produce meaningful error messages if package name
is wrong, and your code will "just not work".
This is the file containing most important code, it subscribes to info and publishes commands.
Note that in the listing below, I only provided whatever is used for a demo, while in an archive (above) there are some commented code that can be used in some cases (like odometry).