Skip to content

Instantly share code, notes, and snippets.

@rayvburn
Last active July 14, 2021 20:13
Show Gist options
  • Save rayvburn/b7ec0708787eb128fe9345a512557dfd to your computer and use it in GitHub Desktop.
Save rayvburn/b7ec0708787eb128fe9345a512557dfd to your computer and use it in GitHub Desktop.
TiAGo robot simulation in ROS2 Foxy

Table of contents


Ubuntu 20 installation

  • download Ubuntu 20 .iso image and prepare a startup image in your USB flash drive (at least 4 GB USB stick is required)
  • install the system

ROS 2 Installation

Note that instruction is based on official ROS documentation page and covers installation of ROS2 Foxy via Debian Packages on Ubuntu 20 machine. For those who may be interested, there is a Windows installation instruction available (not recommended though).

More extensive explanations of what each command posted below does can be found in the provided ROS documentation page.

Debian Packages installation

Locale

sudo apt update && sudo apt install locales
sudo locale-gen en_US en_US.UTF-8
sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
export LANG=en_US.UTF-8

Setup sources

sudo apt update && sudo apt install curl gnupg2 lsb-release
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key  -o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null

Install ROS 2 packages

sudo apt update
sudo apt install ros-foxy-desktop

Environment setup

source /opt/ros/foxy/setup.bash

Testing

To test ROS2 operation run the Hello World system which consists of 2 nodes:

  • talker (periodically increments a variable and publishes it to ROS2 topic, printing current status to the console),
  • listener (subscribes to the topic, which talker publishes to, and prints received data to the console).

To run the 2 node system described above:

  • in the first terminal tab run (don't forget to source)
source /opt/ros/foxy/setup.bash
ros2 run demo_nodes_cpp talker
  • in another terminal tab run (don't forget to source)
source /opt/ros/foxy/setup.bash
ros2 run demo_nodes_py listener

You should expect such logs appearing in the first console:

[INFO] [1625439732.222359294] [talker]: Publishing: 'Hello World: 38'
[INFO] [1625439733.222337534] [talker]: Publishing: 'Hello World: 39'
[INFO] [1625439734.222325740] [talker]: Publishing: 'Hello World: 40'
[INFO] [1625439735.222347314] [talker]: Publishing: 'Hello World: 41'
[INFO] [1625439736.222288798] [talker]: Publishing: 'Hello World: 42'
[INFO] [1625439737.222288015] [talker]: Publishing: 'Hello World: 43'
[INFO] [1625439738.222274067] [talker]: Publishing: 'Hello World: 44'
[INFO] [1625439739.222292692] [talker]: Publishing: 'Hello World: 45'
[INFO] [1625439740.222292784] [talker]: Publishing: 'Hello World: 46'
[INFO] [1625439741.222299658] [talker]: Publishing: 'Hello World: 47'

and such, appearing in the second console:

[INFO] [1625439732.237532966] [listener]: I heard: [Hello World: 38]
[INFO] [1625439733.224391692] [listener]: I heard: [Hello World: 39]
[INFO] [1625439734.224322398] [listener]: I heard: [Hello World: 40]
[INFO] [1625439735.224084591] [listener]: I heard: [Hello World: 41]
[INFO] [1625439736.224072430] [listener]: I heard: [Hello World: 42]
[INFO] [1625439737.224747653] [listener]: I heard: [Hello World: 43]
[INFO] [1625439738.224068149] [listener]: I heard: [Hello World: 44]
[INFO] [1625439739.224729282] [listener]: I heard: [Hello World: 45]
[INFO] [1625439740.224463957] [listener]: I heard: [Hello World: 46]
[INFO] [1625439741.224478478] [listener]: I heard: [Hello World: 47]

ROS2 workspace

We will create a ROS2 workspaces directory in the home directory and tiago workspace (tiago_ws) will be one of them. What's more, create src folder - all source files of our packages will be stored here:

mkdir -p ~/ros2_workspace/tiago_ws/src

Download .rosinstall file that contains references to packages needed to run TiAGo simulation. To do so, tiago_tutorials Git repository must be cloned (in particular foxy-devel branch):

git clone -b foxy-devel https://github.com/pal-robotics/tiago_tutorials.git ~/ros2_workspace/tiago_ws/src/tiago_tutorials

SSH Key

Prepare SSH key for GitHub authentication and easy packages acquisition. Follow steps, according to the tutorial:

  1. Check for existing SSH keys, if there aren't any - generate one (note that you don't need to pay attention to legacy systems).

  2. View your SSH key, so you can copy that into your GitHub account settings. If you used default location while generating the key, running text editor for this purpose will look like that:

    gedit ~/.ssh/id_ed25519.pub
  3. Log in to GitHub and go to SSH and GPG keys, click New SSH key, put some title and paste the key (from the *.pub file) to the provided field in the page. Click Add SSH key. The key will start with ssh and end with your email address.

    Optional: if you don't want to create a SSH key for GitHub - you must change all links in the .rosinstall file (if one is mentioned in installation instruction) to HTTPS form (unfold Code button in the main page of the GitHub repository to get HTTPS or SSH link). Although, using SSH key is more convenient and recommended.

Cloning required repositories

Gazebo simulation (which is PAL's first choice) is currently still unstable. At the moment, best option to simulate TIAGo with ROS2 interface is Webots.

Links posted below redirect to instructions related to both simulators.

colcon workspace tool installation

Install colcon based on this instruction:

sudo sh -c 'echo "deb [arch=amd64,arm64] http://repo.ros2.org/ubuntu/main `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list'
curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
sudo apt update
sudo apt install python3-colcon-common-extensions
sudo apt install python3-vcstool

Simulation preparation


Navigation preparation

Install nav2 packages:

source /opt/ros/foxy/setup.bash
sudo apt install ros-$ROS_DISTRO-nav2-amcl ros-$ROS_DISTRO-nav2-dwb-controller ros-$ROS_DISTRO-nav2-gazebo-spawner ros-$ROS_DISTRO-nav2-regulated-pure-pursuit-controller ros-$ROS_DISTRO-nav2-behavior-tree ros-$ROS_DISTRO-nav2-lifecycle-manager ros-$ROS_DISTRO-nav2-rviz-plugins ros-$ROS_DISTRO-nav2-bringup ros-$ROS_DISTRO-nav2-map-server ros-$ROS_DISTRO-nav2-bt-navigator ros-$ROS_DISTRO-nav2-system-tests ros-$ROS_DISTRO-nav2-msgs ros-$ROS_DISTRO-nav2-common ros-$ROS_DISTRO-nav2-util ros-$ROS_DISTRO-nav2-controller ros-$ROS_DISTRO-nav2-navfn-planner ros-$ROS_DISTRO-nav2-voxel-grid ros-$ROS_DISTRO-nav2-core ros-$ROS_DISTRO-nav2-planner ros-$ROS_DISTRO-nav2-costmap-2d ros-$ROS_DISTRO-nav2-waypoint-follower ros-$ROS_DISTRO-nav2-recoveries

Recompilation

If you are ready to build the workspace for the first time, then run:

source /opt/ros/foxy/setup.bash
cd ~/ros2_workspace/tiago_ws
colcon build --symlink-install
source ~/ros2_workspace/tiago_ws/install/setup.bash

Note that re-sourcing opt here is required due to packages previously installed with apt install.

If you apply some changes to the source files from your workspace,

colcon build --symlink-install

must be called.

  1. Note that you should always call it while being in the workspace main directory, i.e., ~/ros2_workspace/tiago_ws in this example.

  2. Note that if you added a new package to the workspace, then after bulding you should source workspace's setup.bash, so the newly compiled package can be used:

    source ~/ros2_workspace/tiago_ws/install/setup.bash
  3. Note that you should build with --symlink-install to create symbolic links between your src and install directories. Links allow you not to re-build workspace each time you change Python script or parameter files. Of course, changes in C++-related source files require re-building.

  4. Note that calling

    source ~/ros2_workspace/tiago_ws/install/setup.bash

    in a fresh terminal, "sources" /opt/ros/$ROS_DISTRO/setup.bash too.


Workflow tips

This section will cover some helpful tools and hints that will speed up your development work.

Visual Studio Code installation [optional]

When it comes to code editing, Visual Studio Code is a recommended way to start.

Download from here, simply save the file instead of default Open with. Open a terminal window and change directory to the place, where .deb file was saved, for example:

cd ~/Downloads

Then, install .deb file with Debian Package Management System (dpkg), for example (note that version may differ):

sudo dpkg -i code_1.57.1-1623937013_amd64.deb

Now, Visual Studio Code can be launched from terminal with code command or via search in Activities (typing code should be sufficient). Useful extensions:

  • C/C++ (ID: ms-vscode.cpptools)
  • C++ Intellisense (ID: austin.code-gnu-global)
  • CMake (ID: twxs.cmake)
  • CMake Tools (ID: ms-vscode.cmake-tools)
  • ROS (ID: ms-iot.vscode-ros)
  • Git Graph (ID: mhutchie.git-graph)
  • GitLens — Git supercharged (ID: eamodio.gitlens)
  • Trailing Spaces (ID: shardulm94.trailing-spaces)
  • Todo Tree (ID: gruntfuggly.todo-tree)

A VS code ROS2 workspace template can be found at athackst/vscode_ros2_workspace.


Sublime Text 3 installation [optional]

Sublime Text is a popular lightweight text editor.

curl -fsSL https://download.sublimetext.com/sublimehq-pub.gpg | sudo apt-key add -
sudo add-apt-repository "deb https://download.sublimetext.com/ apt/stable/"
sudo apt install sublime-text

Hints and tips

  1. Currently, in ROS2, if you try to run ROS-related command like ros2 run, but you forget to source setup.bash file in the current terminal, you must source ROS2 distro's setup.bash and then your workspace's setup.bash, like that:
source /opt/ros/foxy/setup.bash
source ~/ros2_workspace/tiago_ws/install/setup.bash
  1. If a package located in a workspace contains COLCON_IGNORE empty file in the main directory, then it will be ommited during compilation process.

  2. Append your ~/.bashrc file with:

export RCUTILS_COLORIZED_OUTPUT=1  # 0 to disable

to simply see colorized log output, see details. The file can be opened via call:

gedit ~/.bashrc

and the line specified above should be placed at the very end.

  1. Non-linux people might find these shortcuts useful:
  • CTRL+C closes current process run in terminal
  • CTRL+R in terminal allows to call command from a terminal history
  • CTRL+Shift+V pastes copied phrase into terminal
  • TAB-TAB -> autocompletion in terminal
  1. Additional resources:

TIAGo simulation with Gazebo

Use .rosinstall:

sudo apt install python3-wstool
source /opt/ros/foxy/setup.bash
wstool init -j8 ~/ros2_workspace/tiago_ws/src ~/ros2_workspace/tiago_ws/src/tiago_tutorials/tiago_public.rosinstall

You may be asked to type yes few times or type your SSH key password.

Directory tree (3 levels of a depth presented) should look like this at that moment:

├── tiago_ws
│   └── src
│       ├── gazebo_ros2_control
│       ├── hey5_description
│       ├── launch_pal
│       ├── pal_gazebo_worlds
│       ├── pal_gripper
│       ├── pmb2_navigation
│       ├── pmb2_robot
│       ├── ros2_control
│       ├── ros2_controllers
│       ├── tiago_description_calibration
│       ├── tiago_robot
│       ├── tiago_simulation
│       ├── tiago_tutorials
│       ├── twist_mux
│       └── urdf_test

TIAGo simulation with Webots

Change directory to the one that stores source files in the workspace and clone necessary packages:

cd ~/ros2_workspace/tiago_ws/src
git clone https://github.com/cyberbotics/webots_ros2.git -b foxy
git clone https://github.com/ros-teleop/teleop_tools.git -b foxy-devel
git clone https://github.com/rayvburn/tiago_webots_ros2.git -b foxy

Update submodules of the webots_ros2 package:

cd ~/ros2_workspace/tiago_ws/src/webots_ros2
git submodule update --init --recursive

Build and source the workspace in the current terminal tab:

cd ~/ros2_workspace/tiago_ws/src
colcon build --symlink-install
source ~/ros2_workspace/tiago_ws/install/setup.bash

Gazebo simulator installation

Since this tutorial covers TIAGo robot simulation and PAL's first choice to simulator selection was Gazebo, then we try to run TIAGo simulation in Gazebo. To do so, we need a few, e.g., PAL packages (some of them available via apt).

Install additional packages:

sudo apt install libssl-dev

and other ROS-related packages that were not installed by default:

source /opt/ros/foxy/setup.bash
sudo apt install ros-$ROS_DISTRO-diagnostic-updater ros-$ROS_DISTRO-xacro ros-$ROS_DISTRO-gazebo-dev ros-$ROS_DISTRO-gazebo-ros ros-$ROS_DISTRO-control-toolbox ros-$ROS_DISTRO-angles ros-$ROS_DISTRO-test-msgs ros-$ROS_DISTRO-camera-calibration ros-$ROS_DISTRO-dynamixel-hardware-interface ros-$ROS_DISTRO-dynamixel-sdk ros-$ROS_DISTRO-dynamixel-sdk-custom-interfaces ros-$ROS_DISTRO-joy ros-$ROS_DISTRO-joy-teleop

(Needs verification, if required) Clone additional packages required to run TiAGo simulation:

git clone -b extend-query-trajectory-state git@github.com:pal-robotics-forks/control_msgs.git ~/ros2_workspace/tiago_ws/src/pal_control_msgs
git clone -b fix-unitialized-variable git@github.com:pal-robotics-forks/realtime_tools.git ~/ros2_workspace/tiago_ws/src/pal_realtime_tools

Skip this step, since play_motion requires moveit_ros_planning_interface package which is not available(?) for ROS2

git clone -b foxy-devel git@github.com:pal-robotics/play_motion.git ~/ros2_workspace/tiago_ws/src/pal_play_motion

Now you are ready to build the workspace:

source /opt/ros/foxy/setup.bash
cd ~/ros2_workspace/tiago_ws
colcon build --symlink-install
source ~/ros2_workspace/tiago_ws/install/setup.bash

Note that re-sourcing opt here is required due to packages installed with apt install.

For additional notes on building/recompilation, please refer to this section.

Test of TiAGo simulation in Gazebo

Note that currently TIAGo simulation in Gazebo is not working great and also .launch files in ROS2 packages are buggy. Therefore you should open files with a text editor and apply following changes, according to the diffs listed below:

  1. tiago_simulation/tiago_gazebo/launch/tiago_gazebo.launch.py

a. Open the file

gedit ~/ros2_workspace/tiago_ws/src/tiago_simulation/tiago_gazebo/launch/tiago_gazebo.launch.py

b. Required diff is as follows:

--- a/tiago_gazebo/launch/tiago_gazebo.launch.py
+++ b/tiago_gazebo/launch/tiago_gazebo.launch.py
@@ -105,5 +105,5 @@ def generate_launch_description():
         gazebo,
         tiago_spawn,
         tiago_bringup,
-        tuck_arm
+        #tuck_arm
     ])
  1. tiago_robot/tiago_bringup/launch/tiago_bringup.launch.py

a. Open the file

gedit ~/ros2_workspace/tiago_ws/src/tiago_robot/tiago_bringup/launch/tiago_bringup.launch.py

b. Required diff is as follows:

--- a/tiago_bringup/launch/tiago_bringup.launch.py
+++ b/tiago_bringup/launch/tiago_bringup.launch.py
@@ -37,7 +37,7 @@ def generate_launch_description():

     return LaunchDescription([
         default_controllers,
-        play_motion,
+        #play_motion,
         twist_mux,
         joystick_teleop
     ])

At this point you should be able to spawn TIAGo in an empty world in Gazebo:

ros2 launch tiago_gazebo tiago_gazebo.launch.py world_name:=empty

You can also observe simulation instability running:

ros2 launch tiago_gazebo tiago_gazebo.launch.py world_name:=simple_office_with_people

after a few seconds robot will probably shoot up in the air. :)

Since the simulation is so unstable, we currently (as for today) won't do anything more in Gazebo simulation direction.

Webots simulator installation

Webots has a stable simulation of the TIAGo robot. Installation is based on this instruction. Type the commands listed below:

wget -qO- https://cyberbotics.com/Cyberbotics.asc | sudo apt-key add -
sudo apt-add-repository 'deb https://cyberbotics.com/debian/ binary-amd64/'
sudo apt-get update
sudo apt-get install webots
sudo apt-get install ros-foxy-webots-ros2

Run demo with:

ros2 launch webots_ros2_demos armed_robots.launch.py

Test of TiAGo simulation in Webots

Then run TIAGo base (PMB2) example:

ros2 launch webots_ros2_tiago tiago.launch.py rviz:=True

You can also run full TIAGo example, typing:

ros2 launch webots_ros2_core robot_launch.py executable:=webots_differential_drive_node node_parameters:=$(ros2 pkg prefix webots_ros2_tiago --share)/resource/tiago.yaml world:=$(ros2 pkg prefix webots_ros2_tiago --share)/worlds/tiago++_example.wbt

After it loads, in a second terminal start publishing commands (of type geometry_msgs/msg/Twist) to the robot's differential drive controller, which subscribes /cmd_vel topic.

source /opt/ros/foxy/setup.bash
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.2}}"

See a great wiki for details. There are also video tutorials for Webots simulator available.

@rayvburn
Copy link
Author

rayvburn commented Jul 7, 2021

Webots

TIAGo in tiago++_example world
image

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment