How to introspect ROS 2 executables

Written by Bayode Aderinola

14/12/2022

In this post, you will learn how to introspect ROS 2 executables by identifying publishers and subscribers in the executables. This answers this question posted on ROS Answers.

Step 1: Copy sample packages containing ROS 2 executables

“Hey, do I have to install ros2 first?” Absolutely not! We will be using The Construct to get access to virtual machines pre-installed with ROS.

Click here to copy the ROS2 TurtleBot3 sandbox packages. Once copied, click the red RUN button to launch the packages in a virtual machine. Please be patient while the environment loads.

PS: You will need to login or create an account to copy the packages.


You might also want to try this on a local PC if you have ros2 and some executables installed. However, please note that we cannot support local PCs and you will have to fix any errors you run into on your own. The rest of the instruction assumes that you are working on The Construct; please adapt them to your local PC and ros2 installation.

Step 2: Explore the source code of an ament_python package

You can know what topics are being used by an executable (that is, introspect it) without running it by checking its source code, looking for publishers and subscribers.

Now head over to the Code Editor to make to explore the source code of the packages you just copied.

Open the Code Editor

First, we look at the turtlebot3_teleop package.

Turtblebot3 Teleop package
turtlebot3_teleop package

Let’s look for the executable file for this package. We can find that in the setup.py file.

turtlebot3_teleop/setup.py

from setuptools import find_packages
from setuptools import setup

package_name = 'turtlebot3_teleop'

setup(
    name=package_name,
    version='2.1.2',
    packages=find_packages(exclude=[]),
    data_files=[
        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=[
        'setuptools',
    ],
    zip_safe=True,
    author='Darby Lim',
    author_email='thlim@robotis.com',
    maintainer='Will Son',
    maintainer_email='willson@robotis.com',
    keywords=['ROS'],
    classifiers=[
        'Intended Audience :: Developers',
        'License :: OSI Approved :: Apache Software License',
        'Programming Language :: Python',
        'Topic :: Software Development',
    ],
    description=(
        'Teleoperation node using keyboard for TurtleBot3.'
    ),
    license='Apache License, Version 2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'teleop_keyboard = turtlebot3_teleop.script.teleop_keyboard:main'
        ],
    },
)

Looking at the entry_points part of the file, and then the console_scripts, we find the executable file is in the turtlebot3_teleop/script/teleop_keyboard.py file. Let’s find the publishers and/subscribers for this executable.

turtlebot3_teleop/script/teleop_keyboard.py

def main():
    settings = None
    if os.name != 'nt':
        settings = termios.tcgetattr(sys.stdin)

    rclpy.init()

    qos = QoSProfile(depth=10)
    node = rclpy.create_node('teleop_keyboard')
    pub = node.create_publisher(Twist, 'cmd_vel', qos)

    status = 0
    target_linear_velocity = 0.0
    target_angular_velocity = 0.0
    control_linear_velocity = 0.0
    control_angular_velocity = 0.0

    try:
        print(msg)
        while(1):
            key = get_key(settings)
            if key == 'w':
                target_linear_velocity =\
                    check_linear_limit_velocity(target_linear_velocity + LIN_VEL_STEP_SIZE)
                status = status + 1
                print_vels(target_linear_velocity, target_angular_velocity)
            elif key == 'x':
                target_linear_velocity =\
                    check_linear_limit_velocity(target_linear_velocity - LIN_VEL_STEP_SIZE)
                status = status + 1
                print_vels(target_linear_velocity, target_angular_velocity)
            elif key == 'a':
                target_angular_velocity =\
                    check_angular_limit_velocity(target_angular_velocity + ANG_VEL_STEP_SIZE)
                status = status + 1
                print_vels(target_linear_velocity, target_angular_velocity)
            elif key == 'd':
                target_angular_velocity =\
                    check_angular_limit_velocity(target_angular_velocity - ANG_VEL_STEP_SIZE)
                status = status + 1
                print_vels(target_linear_velocity, target_angular_velocity)
            elif key == ' ' or key == 's':
                target_linear_velocity = 0.0
                control_linear_velocity = 0.0
                target_angular_velocity = 0.0
                control_angular_velocity = 0.0
                print_vels(target_linear_velocity, target_angular_velocity)
            else:
                if (key == '\x03'):
                    break

            if status == 20:
                print(msg)
                status = 0

            twist = Twist()

            control_linear_velocity = make_simple_profile(
                control_linear_velocity,
                target_linear_velocity,
                (LIN_VEL_STEP_SIZE / 2.0))

            twist.linear.x = control_linear_velocity
            twist.linear.y = 0.0
            twist.linear.z = 0.0

            control_angular_velocity = make_simple_profile(
                control_angular_velocity,
                target_angular_velocity,
                (ANG_VEL_STEP_SIZE / 2.0))

            twist.angular.x = 0.0
            twist.angular.y = 0.0
            twist.angular.z = control_angular_velocity

            pub.publish(twist)

    except Exception as e:
        print(e)

    finally:
        twist = Twist()
        twist.linear.x = 0.0
        twist.linear.y = 0.0
        twist.linear.z = 0.0

        twist.angular.x = 0.0
        twist.angular.y = 0.0
        twist.angular.z = 0.0

        pub.publish(twist)

        if os.name != 'nt':
            termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)


if __name__ == '__main__':
    main()

Success! On line 148 we can find that the executable creates a publisher to the /cmd_vel topic, so we know this is a topic used by the executable.

Are there other topics used by this executable? Find out!

Step 3: Explore the source code of an ament_cmake package

Let explore the turtlebot3_node package.

Turtlebot3_node package
turtlebot3_node package

We can find the main executable in the CMakeLists.txt file in the add_executable line (line 75).

turtlebot3_node/CMakeLists.txt

################################################################################
# Set minimum required version of cmake, project name and compile options
################################################################################
cmake_minimum_required(VERSION 3.5)
project(turtlebot3_node)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

################################################################################
# Find ament packages and libraries for ament and system dependencies
################################################################################
find_package(ament_cmake REQUIRED)
find_package(dynamixel_sdk REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(message_filters REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rcutils REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(turtlebot3_msgs REQUIRED)

################################################################################
# Build
################################################################################
include_directories(
  include
)

add_library(${PROJECT_NAME}_lib
  "src/devices/motor_power.cpp"
  "src/devices/sound.cpp"
  "src/devices/reset.cpp"

  "src/diff_drive_controller.cpp"
  "src/dynamixel_sdk_wrapper.cpp"
  "src/odometry.cpp"
  "src/turtlebot3.cpp"

  "src/sensors/battery_state.cpp"
  "src/sensors/imu.cpp"
  "src/sensors/joint_state.cpp"
  "src/sensors/sensor_state.cpp"
)

set(DEPENDENCIES
  "dynamixel_sdk"
  "geometry_msgs"
  "message_filters"
  "nav_msgs"
  "rclcpp"
  "rcutils"
  "sensor_msgs"
  "std_msgs"
  "std_srvs"
  "tf2"
  "tf2_ros"
  "turtlebot3_msgs"
)

target_link_libraries(${PROJECT_NAME}_lib)
ament_target_dependencies(${PROJECT_NAME}_lib ${DEPENDENCIES})

set(EXECUTABLE_NAME "turtlebot3_ros")

add_executable(${EXECUTABLE_NAME} src/node_main.cpp)
target_link_libraries(${EXECUTABLE_NAME} ${PROJECT_NAME}_lib)
ament_target_dependencies(${EXECUTABLE_NAME} ${DEPENDENCIES})

################################################################################
# Install
################################################################################
install(DIRECTORY param
  DESTINATION share/${PROJECT_NAME}
)

install(TARGETS ${EXECUTABLE_NAME}
  DESTINATION lib/${PROJECT_NAME}
)

################################################################################
# Macro for ament package
################################################################################
ament_export_include_directories(include)
ament_package()

So we see that the main executable is src/node_main.cpp. Let’s examine it.

turtlebot3_node/src/node_main.cpp

// Copyright 2019 ROBOTIS CO., LTD.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Author: Darby Lim

#include <rcutils/cmdline_parser.h>
#include <rclcpp/rclcpp.hpp>

#include <chrono>
#include <memory>
#include <string>

#include "turtlebot3_node/diff_drive_controller.hpp"
#include "turtlebot3_node/turtlebot3.hpp"

void help_print()
{
  printf("For turtlebot3 node : \n");
  printf("turtlebot3_node [-i usb_port] [-h]\n");
  printf("options:\n");
  printf("-h : Print this help function.\n");
  printf("-i usb_port: Connected USB port with OpenCR.");
}

int main(int argc, char * argv[])
{
  setvbuf(stdout, NULL, _IONBF, BUFSIZ);

  if (rcutils_cli_option_exist(argv, argv + argc, "-h")) {
    help_print();
    return 0;
  }

  rclcpp::init(argc, argv);

  std::string usb_port = "/dev/ttyACM0";
  char * cli_options;
  cli_options = rcutils_cli_get_option(argv, argv + argc, "-i");
  if (nullptr != cli_options) {
    usb_port = std::string(cli_options);
  }

  rclcpp::executors::SingleThreadedExecutor executor;

  auto turtlebot3 = std::make_shared<robotis::turtlebot3::TurtleBot3>(usb_port);
  auto diff_drive_controller =
    std::make_shared<robotis::turtlebot3::DiffDriveController>(
    turtlebot3->get_wheels()->separation,
    turtlebot3->get_wheels()->radius);

  executor.add_node(turtlebot3);
  executor.add_node(diff_drive_controller);
  executor.spin();

  rclcpp::shutdown();

  return 0;
}

There’s no reference to a publisher or subscriber in this file, but it references some other files. Let’s look at the turtlebot3_node/turtlebot3.cpp file referenced on line 25.

turtlebot3_node/src/turtlebot3.cpp

void TurtleBot3::cmd_vel_callback()
{
  auto qos = rclcpp::QoS(rclcpp::KeepLast(10));
  cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
    "cmd_vel",
    qos,
    [this](const geometry_msgs::msg::Twist::SharedPtr msg) -> void
    {
      std::string sdk_msg;

      union Data {
        int32_t dword[6];
        uint8_t byte[4 * 6];
      } data;

      data.dword[0] = static_cast<int32_t>(msg->linear.x * 100);
      data.dword[1] = 0;
      data.dword[2] = 0;
      data.dword[3] = 0;
      data.dword[4] = 0;
      data.dword[5] = static_cast<int32_t>(msg->angular.z * 100);

      uint16_t start_addr = extern_control_table.cmd_velocity_linear_x.addr;
      uint16_t addr_length =
      (extern_control_table.cmd_velocity_angular_z.addr -
      extern_control_table.cmd_velocity_linear_x.addr) +
      extern_control_table.cmd_velocity_angular_z.length;

      uint8_t * p_data = &amp;data.byte[0];

      dxl_sdk_wrapper_->set_data_to_device(start_addr, addr_length, p_data, &amp;sdk_msg);

      RCLCPP_DEBUG(
        this->get_logger(),
        "lin_vel: %f ang_vel: %f msg : %s", msg->linear.x, msg->angular.z, sdk_msg.c_str());
    }
  );
}

Success! We see the /cmd_vel is also referenced on line 313. Can you find other files linked to the main file and it’s linked files, where other topics are referenced?

Step 4: Check your learning

Do you understand how to introspect ROS 2 executables? If you don’t know it yet, please go over the post again, more carefully this time.

(Extra) Step 5: Watch the video to understand how to introspect ROS 2 executables

Here you go:

Feedback

Did you like this post? Do you have any questions about how to introspect ROS 2 executables? Please leave a comment in the comments section below, so we can interact and learn from each other.

If you want to learn about other ROS2 topics, please let us know in the comments area and we will do a video or post about it.

Masterclass 2023 batch2 blog banner

Check Out These Related Posts

0 Comments

Submit a Comment

Your email address will not be published.

This site uses Akismet to reduce spam. Learn how your comment data is processed.

Pin It on Pinterest

Share This