# Introduction I always find robotics a fascinating subject but hard to learn on my own. There are many reasons why learning robotics is hard. For one thing, robotics systems are complex. To control even a single part of a robotics system demands tremendous domain knowledge. That's why in robotics companies, we typically see teams dedicated to a very specific domain. The very nature of the field of robotics makes self-learning challenging. To understand how to control a robotic arm, we need to understand coordinate systems, linear applications, control theories, and of course physics. For someone who has left the school and no longer deals with these things on a daily basis, even reviewing these concepts can be a time-consuming endeavor. Perhaps the greatest obstacle is the constant friction caused by context switches when learning robotics. For example, to understand how a robotic arm is controlled, one has to go back to refresh the memory on linear algebra, angular momentum, PID controller, and more. While it's fun to review these concepts, it often becomes distracting and sometimes a little annoying. After a while, I finally realized that the real pain point is that this knowledge is scattered all over the place. Yes, there are tons of videos explaining the PID controller, but it takes a good amount of time to find one that explains the concept in a way that anyone can understand. Yes, there are books on linear algebra and probabilities, but no one can afford to read a whole probability book just to understand how SLAM works. What if there is a book that covers the fundamentals and offers a high-level overview of the subject? This book starts with introducing ROS2 and Gazebo, a set of software libraries and tools that are useful for building robot applications and simulations. It then progresses to a chapter dedicated to robotics programming as solid programming skills is key to the success of robotic projects. In the second part, the book shifts its focus to the theoretical aspect of robotics. Reviews of relevant Mathematics and Physics are provided. Additionally, the book features in-depth chapters on control theory and probabilistic robotics, essential to any robotic applications. # ROS2 # Index * [ROS2 rclcpp Github Repo](https://github.com/ros2/rclcpp/tree/humble) * [ROS2 Message Field Types and Services](https://docs.ros.org/en/humble/Concepts/Basic/About-Interfaces.html) * [Common Message and Service Types - Common Interface](https://github.com/ros2/common_interfaces/tree/humble) * Message Filters * [ROS documentation of Message Filters](http://wiki.ros.org/message_filters) * [Github repo of message\_filter package](https://github.com/ros2/message_filters/tree/humble) # IDE and CMake Setup ROS2 has its own build system and tools. For example, ROS2 humble uses ament cmake. It's convenient to use the build tools in terminal but this can cause some issues with IDE. In this article, we address some common issues. {% hint style="info" %} OS: Ubuntu 22.04, ROS: Humble, IDE: CLion {% endhint %} ## CMake Project Setup Let's first talk about the issues. In CLion, it only support single-project configuration. Here, project roughly means ROS packages. Recall that ROS2 development is centered around workspace and each workspace contains multiple packages. The structure below is a typical layout of a ROS2 workspace ``` user:~/workspace/ros-projects/ros2_ws_tutorial_creating_ws$ tree -L 2 . ├── build ├── cmake-build-debug ├── install ├── log ├── src    ├── cpp_pubsub    ├── cpp_srvcli    └── tutorial_interfaces ``` In the workspace `ros2_ws_tutorial_creating_ws`, there are three pakcages and each of them has a CMakeLists.txt file: ``` user:~/workspace/ros-projects/ros2_ws_tutorial_creating_ws/src$ tree -L 2 . ├── cpp_pubsub │   ├── cmake-build-debug │   ├── CMakeLists.txt │   ├── include │   ├── package.xml │   └── src ├── cpp_srvcli │   ├── cmake-build-debug │   ├── CMakeLists.txt │   ├── include │   ├── package.xml │   └── src └── tutorial_interfaces ├── CMakeLists.txt ├── include ├── msg ├── package.xml ├── src └── srv ``` Here comes our first problem. We cannot just open the `ros2_ws_tutorial_creating_ws` as the root directory because CLion does not support multiple projects. To make it work, we need to combine all ROS2 packages/projects as one single CMake project. This means we need to have a top-level CMakeLists.txt file. We cannot have one directly in the `ros2_ws_tutorial_creating_ws` directory because it will interfere withe ROS2 build tools. What we can do is to create a `tmp` directory under `ros2_ws_tutorial_creating_ws` and put a CMakeLists.txt in it. For instance: ``` # Location of this file is ~/workspace/ros-projects/ros2_ws_tutorial_creating_ws/tmp/CMakeLists.txt cmake_minimum_required(VERSION 3.8) project(ros2_ws_tutorial_creating_ws) add_subdirectory("../src/cpp_pubsub" "../src/cpp_pubsub") add_subdirectory("../src/cpp_srvcli" "../src/cpp_srvcli") add_subdirectory("../src/tutorial_interfaces" "../src/tutorial_interfaces") ``` Remember to reload the CMakeLists.txt file after the change. ## Resolve Symbols You may notice that jump to definition does not work for ROS2 defined symbols. The problem is that CLion does not know where to search for the symbols. In fact, when you reloade the CMake project in CLion, it may complain that it cannot find the ament\_cmake. The solution to this problem is to add environment variables to the cmake configuration in CLion.
How do we know which variables to add? Recall that when we launch a new terminal, we need to source the ROS2 setup.bash file (i.e. /opt/ros/humble/setup.bash). To determine the environment variables, you could following the steps below: * Update \~/.bashrc file and make sure the terminal does not automatically source the ROS2 setup file. * Launch a new terminal and save the current environment variables using the command `printenv` * Source the ROS2 setup file and save the environment variables again to a different file * Compare the two files using the command `diff --color -u old-env-vars-file new-env-vars-file` After the change, don't forget to reload the CMakeLists.txt files. TODO: Set up IDE for python files: /opt/ros/humble/lib/python3.10 # How to add additional include search path When working on ROS2 projects in an IDE (e.g. Clion), it is important to understand that there are two build systems involved. * **IDE's Build System**: The IDE uses `CmakeLists.txt` file in each ROS2 project to resolve paths and variables. * **ROS2's Build System**: This is the build system used by ROS2 when you build the ROS2 workspace in the terminal. For instance, in ROS2 Humble, `colcon` command is used to build the package and `ament` commands are used in the CmakeLists.txt file. Successfully building a ROS2 workspace in the terminal does not guarantee that the IDE will resolve all paths and variables. This issue becomes more apparent with custom message and service files, where the IDE might fail to locate certain header files. To address this issue, we can add additional include search paths. However, it's crucial not to modify the `CMakeLists.txt` file directly to avoid introducing changes that could affect the ROS2 build system and lead to confusion. Instead, we should adjust the CMake configuration within the IDE by adding the following CMake option: ``` -DCMAKE_CXX_FLAGS="-I " ``` For example, in Clion, we can add this in Settings > Build, Execution, Deployment > Cmake > CMake options.
# ROS2 Building Blocks # ROS Workspace and Package Layout The layout below is a typical ROS2 workspace: ``` user:~/workspace/ros-projects/ros_ws_demo$ tree . ├── build ├── install ├── log └── src ├── package_A │   ├── CMakeLists.txt │   ├── launch │   ├── msg │   ├── package.xml │   └── src │   ├── program_1.cpp │   ├── program_2.cpp │   └── program_3.cpp └── package_B ├── CMakeLists.txt ├── launch ├── package.xml └── src ├── program_x.cpp └── program_y.cpp ``` **Terminology** * \~/workspace/ros-projects/ros\_ws\_demo is the root of the workspace * \~/workspace/ros-projects/ros\_ws\_demo/src/package\_A is the root of the package A # Launch File Launch files are used to launch a large system with many executables where an executable may contain multiple nodes. From a computing perspective, nodes or executables form a graph because the philosophy of ROS2 is to have independent modules that focus on a single responsibility. The communication between nodes are supported through message passing. From a launch process perspective, the executables and launch files form a tree structure.
Suppose we have a main launch file for our project `learn-launch-file`. This launch file can launch nodes/executables of the project learn-launch-file and run launch files in other projects. A more concrete example is presented below:
The syntax are different for these two types of tasks. To launch a node, we use the `Node` class. For example: ```python from launch_ros.actions import Node robot_state_publisher = Node( package="robot_state_publisher", executable="robot_state_publisher", name="my_robot_state_publisher", output="both", parameters=[ {"use_sim_time": True}, {"robot_description": robot_model_description}, ] ) rviz = Node( package="rviz2", executable="rviz2", condition=IfCondition(LaunchConfiguration("rviz")), arguments=["-d", path.join(bringup_package, "config", "slam_and_nav_v1.rviz")] ) ``` To run another launch file inside our current launch file, we use `IncludeLaunchDescription` and `PythonLaunchDescriptionSource`. For example: ``` from launch.actions import IncludeLaunchDescription from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource slam_toolbox = IncludeLaunchDescription( PythonLaunchDescriptionSource(path.join(get_package_share_directory("slam_toolbox"), "launch", "online_async_launch.py")), launch_arguments={ 'use_sim_time': "True", 'slam_params_file': PathJoinSubstitution([bringup_package, "config", "mapper_params_online_async.yaml"]) }.items(), ) ``` {% hint style="warning" %} The parameters for specifying the arguments are different. Use arguments for a node and launch\_arguments for a launch file. {% endhint %} # tf2 # Quality of Service ## Related Readings * [ROS Design: QoS - Deadline, Liveliness, and Lifespan](https://design.ros2.org/articles/qos_deadline_liveliness_lifespan.html) * [Quality of Service Settings](https://docs.ros.org/en/humble/Concepts/Intermediate/About-Quality-of-Service-Settings.html) ## Default QoS Setting ROS2 provies default QoS setting. For example, By default, publishers and subscriptions in ROS 2 have “keep last” for history with a queue size of 10, “reliable” for reliability, **“volatile” for durability**, and “system default” for liveliness. Deadline, lifespan, and lease durations are also all set to “default”. The volatile durability on the publishers side means the message is not stored in publishers for late-joining subscribers. This is one of the reasons why the bringup order matters during the system start-up. ## QoS Service Event Callbacks The callback is part of the subscription options. (see [code](https://github.com/ros2/rclcpp/blob/d9b2744057b8fd230f2c71c739fcdf7e27219d85/rclcpp/include/rclcpp/subscription_options.hpp#L42)) Currently in the Humble distribution, the following callback types are defined (see [code](https://github.com/ros2/rclcpp/blob/d9b2744057b8fd230f2c71c739fcdf7e27219d85/rclcpp/include/rclcpp/event_handler.hpp#L66-L73)): ``` QOSDeadlineOfferedCallbackType deadline_callback; QOSLivelinessLostCallbackType liveliness_callback; QOSOfferedIncompatibleQoSCallbackType incompatible_qos_callback; IncompatibleTypeCallbackType incompatible_type_callback; PublisherMatchedCallbackType matched_callback; ``` **Example: Usage of deadline callback** See [original post](https://answers.ros.org/question/352472/ros2-qos-deadline-usage/). ``` // create publisher rclcpp::QoS qos_profile(10); rclcpp::PublisherOptions publisher_options; qos_profile.deadline(deadline_duration); publisher_options.event_callbacks.deadline_callback = [](rclcpp::QOSDeadlineOfferedInfo & event) -> void { // handle missing deadlines }; publisher_ = this->create_publisher("joy", qos_profile, publisher_options); // create subscription rclcpp::SubscriptionOptions subscription_options; subscription_options.event_callbacks.deadline_callback = [](rclcpp::QOSDeadlineRequestedInfo & event) -> void { // handle missing deadlines }; subscription_ = this->create_subscription("joy", qos_profile, std::bind(&JoyToVel::topic_callback, this, _1), subscription_options); ``` # Configurations # Rviz Configuration Manually configuring the Rviz every time a new instance is launched can be inconvenient. We can actually provide a configuration file when launching rviz. Here is [an example of the rviz config](https://github.com/gazebosim/ros_gz_project_template/blob/main/ros_gz_example_bringup/config/diff_drive.rviz) file. Now the question is where to find the documentation about these parameters. The good news is we don't need to. After the rviz is configured, we can save the configuration.
To specify the rviz config file in a python launch file, we can use the syntax below: ```python rviz = Node( package="rviz2", executable="rviz2", condition=IfCondition(LaunchConfiguration("rviz")), arguments=["-d", path.join(bringup_package, "config", "slam_and_nav_v1.rviz")] ) ``` # Built-in Types # Built-in Message Type The built-in message types are defined in the [common\_interface](https://github.com/ros2/common_interfaces/tree/humble) package. This page collect all the built-in message types in the Humble distribution. ``` ------------------------------------------------------------ Message Type: stereo_msgs/DisparityImage.msg ------------------------------------------------------------ # Separate header for compatibility with current TimeSynchronizer. # Likely to be removed in a later release, use image.header instead. std_msgs/Header header # Floating point disparity image. The disparities are pre-adjusted for any # x-offset between the principal points of the two cameras (in the case # that they are verged). That is: d = x_l - x_r - (cx_l - cx_r) sensor_msgs/Image image # Stereo geometry. For disparity d, the depth from the camera is Z = fT/d. float32 f # Focal length, pixels float32 t # Baseline, world units # Subwindow of (potentially) valid disparity values. sensor_msgs/RegionOfInterest valid_window # The range of disparities searched. # In the disparity image, any disparity less than min_disparity is invalid. # The disparity search range defines the horopter, or 3D volume that the # stereo algorithm can "see". Points with Z outside of: # Z_min = fT / max_disparity # Z_max = fT / min_disparity # could not be found. float32 min_disparity float32 max_disparity # Smallest allowed disparity increment. The smallest achievable depth range # resolution is delta_Z = (Z^2/fT)*delta_d. float32 delta_d ------------------------------------------------------------ Message Type: nav_msgs/MapMetaData.msg ------------------------------------------------------------ # This hold basic information about the characteristics of the OccupancyGrid # The time at which the map was loaded builtin_interfaces/Time map_load_time # The map resolution [m/cell] float32 resolution # Map width [cells] uint32 width # Map height [cells] uint32 height # The origin of the map [m, m, rad]. This is the real-world pose of the # bottom left corner of cell (0,0) in the map. geometry_msgs/Pose origin ------------------------------------------------------------ Message Type: nav_msgs/Path.msg ------------------------------------------------------------ # An array of poses that represents a Path for a robot to follow. # Indicates the frame_id of the path. std_msgs/Header header # Array of poses to follow. geometry_msgs/PoseStamped[] poses ------------------------------------------------------------ Message Type: nav_msgs/Odometry.msg ------------------------------------------------------------ # This represents an estimate of a position and velocity in free space. # The pose in this message should be specified in the coordinate frame given by header.frame_id # The twist in this message should be specified in the coordinate frame given by the child_frame_id # Includes the frame id of the pose parent. std_msgs/Header header # Frame id the pose points to. The twist is in this coordinate frame. string child_frame_id # Estimated pose that is typically relative to a fixed world frame. geometry_msgs/PoseWithCovariance pose # Estimated linear and angular velocity relative to child_frame_id. geometry_msgs/TwistWithCovariance twist ------------------------------------------------------------ Message Type: nav_msgs/OccupancyGrid.msg ------------------------------------------------------------ # This represents a 2-D grid map std_msgs/Header header # MetaData for the map MapMetaData info # The map data, in row-major order, starting with (0,0). # Cell (1, 0) will be listed second, representing the next cell in the x direction. # Cell (0, 1) will be at the index equal to info.width, followed by (1, 1). # The values inside are application dependent, but frequently, # 0 represents unoccupied, 1 represents definitely occupied, and # -1 represents unknown. int8[] data ------------------------------------------------------------ Message Type: nav_msgs/GridCells.msg ------------------------------------------------------------ # An array of cells in a 2D grid std_msgs/Header header # Width of each cell float32 cell_width # Height of each cell float32 cell_height # Each cell is represented by the Point at the center of the cell geometry_msgs/Point[] cells ------------------------------------------------------------ Message Type: visualization_msgs/InteractiveMarker.msg ------------------------------------------------------------ # Time/frame info. # If header.time is set to 0, the marker will be retransformed into # its frame on each timestep. You will receive the pose feedback # in the same frame. # Otherwise, you might receive feedback in a different frame. # For rviz, this will be the current 'fixed frame' set by the user. std_msgs/Header header # Initial pose. Also, defines the pivot point for rotations. geometry_msgs/Pose pose # Identifying string. Must be globally unique in # the topic that this message is sent through. string name # Short description (< 40 characters). string description # Scale to be used for default controls (default=1). float32 scale # All menu and submenu entries associated with this marker. MenuEntry[] menu_entries # List of controls displayed for this marker. InteractiveMarkerControl[] controls ------------------------------------------------------------ Message Type: visualization_msgs/MarkerArray.msg ------------------------------------------------------------ Marker[] markers ------------------------------------------------------------ Message Type: visualization_msgs/MeshFile.msg ------------------------------------------------------------ # Used to send raw mesh files. # The filename is used for both debug purposes and to provide a file extension # for whatever parser is used. string filename # This stores the raw text of the mesh file. uint8[] data ------------------------------------------------------------ Message Type: visualization_msgs/UVCoordinate.msg ------------------------------------------------------------ # Location of the pixel as a ratio of the width of a 2D texture. # Values should be in range: [0.0-1.0]. float32 u float32 v ------------------------------------------------------------ Message Type: visualization_msgs/MenuEntry.msg ------------------------------------------------------------ # MenuEntry message. # # Each InteractiveMarker message has an array of MenuEntry messages. # A collection of MenuEntries together describe a # menu/submenu/subsubmenu/etc tree, though they are stored in a flat # array. The tree structure is represented by giving each menu entry # an ID number and a "parent_id" field. Top-level entries are the # ones with parent_id = 0. Menu entries are ordered within their # level the same way they are ordered in the containing array. Parent # entries must appear before their children. # # Example: # - id = 3 # parent_id = 0 # title = "fun" # - id = 2 # parent_id = 0 # title = "robot" # - id = 4 # parent_id = 2 # title = "pr2" # - id = 5 # parent_id = 2 # title = "turtle" # # Gives a menu tree like this: # - fun # - robot # - pr2 # - turtle # ID is a number for each menu entry. Must be unique within the # control, and should never be 0. uint32 id # ID of the parent of this menu entry, if it is a submenu. If this # menu entry is a top-level entry, set parent_id to 0. uint32 parent_id # menu / entry title string title # Arguments to command indicated by command_type (below) string command # Command_type stores the type of response desired when this menu # entry is clicked. # FEEDBACK: send an InteractiveMarkerFeedback message with menu_entry_id set to this entry's id. # ROSRUN: execute "rosrun" with arguments given in the command field (above). # ROSLAUNCH: execute "roslaunch" with arguments given in the command field (above). uint8 FEEDBACK=0 uint8 ROSRUN=1 uint8 ROSLAUNCH=2 uint8 command_type ------------------------------------------------------------ Message Type: visualization_msgs/InteractiveMarkerControl.msg ------------------------------------------------------------ # Represents a control that is to be displayed together with an interactive marker # Identifying string for this control. # You need to assign a unique value to this to receive feedback from the GUI # on what actions the user performs on this control (e.g. a button click). string name # Defines the local coordinate frame (relative to the pose of the parent # interactive marker) in which is being rotated and translated. # Default: Identity geometry_msgs/Quaternion orientation # Orientation mode: controls how orientation changes. # INHERIT: Follow orientation of interactive marker # FIXED: Keep orientation fixed at initial state # VIEW_FACING: Align y-z plane with screen (x: forward, y:left, z:up). uint8 INHERIT = 0 uint8 FIXED = 1 uint8 VIEW_FACING = 2 uint8 orientation_mode # Interaction mode for this control # # NONE: This control is only meant for visualization; no context menu. # MENU: Like NONE, but right-click menu is active. # BUTTON: Element can be left-clicked. # MOVE_AXIS: Translate along local x-axis. # MOVE_PLANE: Translate in local y-z plane. # ROTATE_AXIS: Rotate around local x-axis. # MOVE_ROTATE: Combines MOVE_PLANE and ROTATE_AXIS. uint8 NONE = 0 uint8 MENU = 1 uint8 BUTTON = 2 uint8 MOVE_AXIS = 3 uint8 MOVE_PLANE = 4 uint8 ROTATE_AXIS = 5 uint8 MOVE_ROTATE = 6 # "3D" interaction modes work with the mouse+SHIFT+CTRL or with 3D cursors. # MOVE_3D: Translate freely in 3D space. # ROTATE_3D: Rotate freely in 3D space about the origin of parent frame. # MOVE_ROTATE_3D: Full 6-DOF freedom of translation and rotation about the cursor origin. uint8 MOVE_3D = 7 uint8 ROTATE_3D = 8 uint8 MOVE_ROTATE_3D = 9 uint8 interaction_mode # If true, the contained markers will also be visible # when the gui is not in interactive mode. bool always_visible # Markers to be displayed as custom visual representation. # Leave this empty to use the default control handles. # # Note: # - The markers can be defined in an arbitrary coordinate frame, # but will be transformed into the local frame of the interactive marker. # - If the header of a marker is empty, its pose will be interpreted as # relative to the pose of the parent interactive marker. Marker[] markers # In VIEW_FACING mode, set this to true if you don't want the markers # to be aligned with the camera view point. The markers will show up # as in INHERIT mode. bool independent_marker_orientation # Short description (< 40 characters) of what this control does, # e.g. "Move the robot". # Default: A generic description based on the interaction mode string description ------------------------------------------------------------ Message Type: visualization_msgs/InteractiveMarkerPose.msg ------------------------------------------------------------ # Time/frame info. std_msgs/Header header # Initial pose. Also, defines the pivot point for rotations. geometry_msgs/Pose pose # Identifying string. Must be globally unique in # the topic that this message is sent through. string name ------------------------------------------------------------ Message Type: visualization_msgs/Marker.msg ------------------------------------------------------------ # See: # - http://www.ros.org/wiki/rviz/DisplayTypes/Marker # - http://www.ros.org/wiki/rviz/Tutorials/Markers%3A%20Basic%20Shapes # # for more information on using this message with rviz. int32 ARROW=0 int32 CUBE=1 int32 SPHERE=2 int32 CYLINDER=3 int32 LINE_STRIP=4 int32 LINE_LIST=5 int32 CUBE_LIST=6 int32 SPHERE_LIST=7 int32 POINTS=8 int32 TEXT_VIEW_FACING=9 int32 MESH_RESOURCE=10 int32 TRIANGLE_LIST=11 int32 ADD=0 int32 MODIFY=0 int32 DELETE=2 int32 DELETEALL=3 # Header for timestamp and frame id. std_msgs/Header header # Namespace in which to place the object. # Used in conjunction with id to create a unique name for the object. string ns # Object ID used in conjunction with the namespace for manipulating and deleting the object later. int32 id # Type of object. int32 type # Action to take; one of: # - 0 add/modify an object # - 1 (deprecated) # - 2 deletes an object (with the given ns and id) # - 3 deletes all objects (or those with the given ns if any) int32 action # Pose of the object with respect the frame_id specified in the header. geometry_msgs/Pose pose # Scale of the object; 1,1,1 means default (usually 1 meter square). geometry_msgs/Vector3 scale # Color of the object; in the range: [0.0-1.0] std_msgs/ColorRGBA color # How long the object should last before being automatically deleted. # 0 indicates forever. builtin_interfaces/Duration lifetime # If this marker should be frame-locked, i.e. retransformed into its frame every timestep. bool frame_locked # Only used if the type specified has some use for them (eg. POINTS, LINE_STRIP, etc.) geometry_msgs/Point[] points # Only used if the type specified has some use for them (eg. POINTS, LINE_STRIP, etc.) # The number of colors provided must either be 0 or equal to the number of points provided. # NOTE: alpha is not yet used std_msgs/ColorRGBA[] colors # Texture resource is a special URI that can either reference a texture file in # a format acceptable to (resource retriever)[https://index.ros.org/p/resource_retriever/] # or an embedded texture via a string matching the format: # "embedded://texture_name" string texture_resource # An image to be loaded into the rendering engine as the texture for this marker. # This will be used iff texture_resource is set to embedded. sensor_msgs/CompressedImage texture # Location of each vertex within the texture; in the range: [0.0-1.0] UVCoordinate[] uv_coordinates # Only used for text markers string text # Only used for MESH_RESOURCE markers. # Similar to texture_resource, mesh_resource uses resource retriever to load a mesh. # Optionally, a mesh file can be sent in-message via the mesh_file field. If doing so, # use the following format for mesh_resource: # "embedded://mesh_name" string mesh_resource MeshFile mesh_file bool mesh_use_embedded_materials ------------------------------------------------------------ Message Type: visualization_msgs/InteractiveMarkerUpdate.msg ------------------------------------------------------------ # Identifying string. Must be unique in the topic namespace # that this server works on. string server_id # Sequence number. # The client will use this to detect if it has missed an update. uint64 seq_num # Type holds the purpose of this message. It must be one of UPDATE or KEEP_ALIVE. # UPDATE: Incremental update to previous state. # The sequence number must be 1 higher than for # the previous update. # KEEP_ALIVE: Indicates the that the server is still living. # The sequence number does not increase. # No payload data should be filled out (markers, poses, or erases). uint8 KEEP_ALIVE = 0 uint8 UPDATE = 1 uint8 type # Note: No guarantees on the order of processing. # Contents must be kept consistent by sender. # Markers to be added or updated InteractiveMarker[] markers # Poses of markers that should be moved InteractiveMarkerPose[] poses # Names of markers to be erased string[] erases ------------------------------------------------------------ Message Type: visualization_msgs/ImageMarker.msg ------------------------------------------------------------ int32 CIRCLE=0 int32 LINE_STRIP=1 int32 LINE_LIST=2 int32 POLYGON=3 int32 POINTS=4 int32 ADD=0 int32 REMOVE=1 std_msgs/Header header # Namespace which is used with the id to form a unique id. string ns # Unique id within the namespace. int32 id # One of the above types, e.g. CIRCLE, LINE_STRIP, etc. int32 type # Either ADD or REMOVE. int32 action # Two-dimensional coordinate position, in pixel-coordinates. geometry_msgs/Point position # The scale of the object, e.g. the diameter for a CIRCLE. float32 scale # The outline color of the marker. std_msgs/ColorRGBA outline_color # Whether or not to fill in the shape with color. uint8 filled # Fill color; in the range: [0.0-1.0] std_msgs/ColorRGBA fill_color # How long the object should last before being automatically deleted. # 0 indicates forever. builtin_interfaces/Duration lifetime # Coordinates in 2D in pixel coords. Used for LINE_STRIP, LINE_LIST, POINTS, etc. geometry_msgs/Point[] points # The color for each line, point, etc. in the points field. std_msgs/ColorRGBA[] outline_colors ------------------------------------------------------------ Message Type: visualization_msgs/InteractiveMarkerInit.msg ------------------------------------------------------------ # Identifying string. Must be unique in the topic namespace # that this server works on. string server_id # Sequence number. # The client will use this to detect if it has missed a subsequent # update. Every update message will have the same sequence number as # an init message. Clients will likely want to unsubscribe from the # init topic after a successful initialization to avoid receiving # duplicate data. uint64 seq_num # All markers. InteractiveMarker[] markers ------------------------------------------------------------ Message Type: visualization_msgs/InteractiveMarkerFeedback.msg ------------------------------------------------------------ # Time/frame info. std_msgs/Header header # Identifying string. Must be unique in the topic namespace. string client_id # Feedback message sent back from the GUI, e.g. # when the status of an interactive marker was modified by the user. # Specifies which interactive marker and control this message refers to string marker_name string control_name # Type of the event # KEEP_ALIVE: sent while dragging to keep up control of the marker # MENU_SELECT: a menu entry has been selected # BUTTON_CLICK: a button control has been clicked # POSE_UPDATE: the pose has been changed using one of the controls uint8 KEEP_ALIVE = 0 uint8 POSE_UPDATE = 1 uint8 MENU_SELECT = 2 uint8 BUTTON_CLICK = 3 uint8 MOUSE_DOWN = 4 uint8 MOUSE_UP = 5 uint8 event_type # Current pose of the marker # Note: Has to be valid for all feedback types. geometry_msgs/Pose pose # Contains the ID of the selected menu entry # Only valid for MENU_SELECT events. uint32 menu_entry_id # If event_type is BUTTON_CLICK, MOUSE_DOWN, or MOUSE_UP, mouse_point # may contain the 3 dimensional position of the event on the # control. If it does, mouse_point_valid will be true. mouse_point # will be relative to the frame listed in the header. geometry_msgs/Point mouse_point bool mouse_point_valid ------------------------------------------------------------ Message Type: sensor_msgs/Image.msg ------------------------------------------------------------ # This message contains an uncompressed image # (0, 0) is at top-left corner of image std_msgs/Header header # Header timestamp should be acquisition time of image # Header frame_id should be optical frame of camera # origin of frame should be optical center of cameara # +x should point to the right in the image # +y should point down in the image # +z should point into to plane of the image # If the frame_id here and the frame_id of the CameraInfo # message associated with the image conflict # the behavior is undefined uint32 height # image height, that is, number of rows uint32 width # image width, that is, number of columns # The legal values for encoding are in file src/image_encodings.cpp # If you want to standardize a new string format, join # ros-users@lists.ros.org and send an email proposing a new encoding. string encoding # Encoding of pixels -- channel meaning, ordering, size # taken from the list of strings in include/sensor_msgs/image_encodings.hpp uint8 is_bigendian # is this data bigendian? uint32 step # Full row length in bytes uint8[] data # actual matrix data, size is (step * rows) ------------------------------------------------------------ Message Type: sensor_msgs/CameraInfo.msg ------------------------------------------------------------ # This message defines meta information for a camera. It should be in a # camera namespace on topic "camera_info" and accompanied by up to five # image topics named: # # image_raw - raw data from the camera driver, possibly Bayer encoded # image - monochrome, distorted # image_color - color, distorted # image_rect - monochrome, rectified # image_rect_color - color, rectified # # The image_pipeline contains packages (image_proc, stereo_image_proc) # for producing the four processed image topics from image_raw and # camera_info. The meaning of the camera parameters are described in # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo. # # The image_geometry package provides a user-friendly interface to # common operations using this meta information. If you want to, e.g., # project a 3d point into image coordinates, we strongly recommend # using image_geometry. # # If the camera is uncalibrated, the matrices D, K, R, P should be left # zeroed out. In particular, clients may assume that K[0] == 0.0 # indicates an uncalibrated camera. ####################################################################### # Image acquisition info # ####################################################################### # Time of image acquisition, camera coordinate frame ID std_msgs/Header header # Header timestamp should be acquisition time of image # Header frame_id should be optical frame of camera # origin of frame should be optical center of camera # +x should point to the right in the image # +y should point down in the image # +z should point into the plane of the image ####################################################################### # Calibration Parameters # ####################################################################### # These are fixed during camera calibration. Their values will be the # # same in all messages until the camera is recalibrated. Note that # # self-calibrating systems may "recalibrate" frequently. # # # # The internal parameters can be used to warp a raw (distorted) image # # to: # # 1. An undistorted image (requires D and K) # # 2. A rectified image (requires D, K, R) # # The projection matrix P projects 3D points into the rectified image.# ####################################################################### # The image dimensions with which the camera was calibrated. # Normally this will be the full camera resolution in pixels. uint32 height uint32 width # The distortion model used. Supported models are listed in # sensor_msgs/distortion_models.hpp. For most cameras, "plumb_bob" - a # simple model of radial and tangential distortion - is sufficent. string distortion_model # The distortion parameters, size depending on the distortion model. # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). float64[] d # Intrinsic camera matrix for the raw (distorted) images. # [fx 0 cx] # K = [ 0 fy cy] # [ 0 0 1] # Projects 3D points in the camera coordinate frame to 2D pixel # coordinates using the focal lengths (fx, fy) and principal point # (cx, cy). float64[9] k # 3x3 row-major matrix # Rectification matrix (stereo cameras only) # A rotation matrix aligning the camera coordinate system to the ideal # stereo image plane so that epipolar lines in both stereo images are # parallel. float64[9] r # 3x3 row-major matrix # Projection/camera matrix # [fx' 0 cx' Tx] # P = [ 0 fy' cy' Ty] # [ 0 0 1 0] # By convention, this matrix specifies the intrinsic (camera) matrix # of the processed (rectified) image. That is, the left 3x3 portion # is the normal camera intrinsic matrix for the rectified image. # It projects 3D points in the camera coordinate frame to 2D pixel # coordinates using the focal lengths (fx', fy') and principal point # (cx', cy') - these may differ from the values in K. # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will # also have R = the identity and P[1:3,1:3] = K. # For a stereo pair, the fourth column [Tx Ty 0]' is related to the # position of the optical center of the second camera in the first # camera's frame. We assume Tz = 0 so both cameras are in the same # stereo image plane. The first camera always has Tx = Ty = 0. For # the right (second) camera of a horizontal stereo pair, Ty = 0 and # Tx = -fx' * B, where B is the baseline between the cameras. # Given a 3D point [X Y Z]', the projection (x, y) of the point onto # the rectified image is given by: # [u v w]' = P * [X Y Z 1]' # x = u / w # y = v / w # This holds for both images of a stereo pair. float64[12] p # 3x4 row-major matrix ####################################################################### # Operational Parameters # ####################################################################### # These define the image region actually captured by the camera # # driver. Although they affect the geometry of the output image, they # # may be changed freely without recalibrating the camera. # ####################################################################### # Binning refers here to any camera setting which combines rectangular # neighborhoods of pixels into larger "super-pixels." It reduces the # resolution of the output image to # (width / binning_x) x (height / binning_y). # The default values binning_x = binning_y = 0 is considered the same # as binning_x = binning_y = 1 (no subsampling). uint32 binning_x uint32 binning_y # Region of interest (subwindow of full camera resolution), given in # full resolution (unbinned) image coordinates. A particular ROI # always denotes the same window of pixels on the camera sensor, # regardless of binning settings. # The default setting of roi (all values 0) is considered the same as # full resolution (roi.width = width, roi.height = height). RegionOfInterest roi ------------------------------------------------------------ Message Type: sensor_msgs/Range.msg ------------------------------------------------------------ # Single range reading from an active ranger that emits energy and reports # one range reading that is valid along an arc at the distance measured. # This message is not appropriate for laser scanners. See the LaserScan # message if you are working with a laser scanner. # # This message also can represent a fixed-distance (binary) ranger. This # sensor will have min_range===max_range===distance of detection. # These sensors follow REP 117 and will output -Inf if the object is detected # and +Inf if the object is outside of the detection range. std_msgs/Header header # timestamp in the header is the time the ranger # returned the distance reading # Radiation type enums # If you want a value added to this list, send an email to the ros-users list uint8 ULTRASOUND=0 uint8 INFRARED=1 uint8 radiation_type # the type of radiation used by the sensor # (sound, IR, etc) [enum] float32 field_of_view # the size of the arc that the distance reading is # valid for [rad] # the object causing the range reading may have # been anywhere within -field_of_view/2 and # field_of_view/2 at the measured range. # 0 angle corresponds to the x-axis of the sensor. float32 min_range # minimum range value [m] float32 max_range # maximum range value [m] # Fixed distance rangers require min_range==max_range float32 range # range data [m] # (Note: values < range_min or > range_max should be discarded) # Fixed distance rangers only output -Inf or +Inf. # -Inf represents a detection within fixed distance. # (Detection too close to the sensor to quantify) # +Inf represents no detection within the fixed distance. # (Object out of range) ------------------------------------------------------------ Message Type: sensor_msgs/LaserEcho.msg ------------------------------------------------------------ # This message is a submessage of MultiEchoLaserScan and is not intended # to be used separately. float32[] echoes # Multiple values of ranges or intensities. # Each array represents data from the same angle increment. ------------------------------------------------------------ Message Type: sensor_msgs/PointCloud.msg ------------------------------------------------------------ ## THIS MESSAGE IS DEPRECATED AS OF FOXY ## Please use sensor_msgs/PointCloud2 # This message holds a collection of 3d points, plus optional additional # information about each point. # Time of sensor data acquisition, coordinate frame ID. std_msgs/Header header # Array of 3d points. Each Point32 should be interpreted as a 3d point # in the frame given in the header. geometry_msgs/Point32[] points # Each channel should have the same number of elements as points array, # and the data in each channel should correspond 1:1 with each point. # Channel names in common practice are listed in ChannelFloat32.msg. ChannelFloat32[] channels ------------------------------------------------------------ Message Type: sensor_msgs/JoyFeedback.msg ------------------------------------------------------------ # Declare of the type of feedback uint8 TYPE_LED = 0 uint8 TYPE_RUMBLE = 1 uint8 TYPE_BUZZER = 2 uint8 type # This will hold an id number for each type of each feedback. # Example, the first led would be id=0, the second would be id=1 uint8 id # Intensity of the feedback, from 0.0 to 1.0, inclusive. If device is # actually binary, driver should treat 0<=x<0.5 as off, 0.5<=x<=1 as on. float32 intensity ------------------------------------------------------------ Message Type: sensor_msgs/MultiEchoLaserScan.msg ------------------------------------------------------------ # Single scan from a multi-echo planar laser range-finder # # If you have another ranging device with different behavior (e.g. a sonar # array), please find or create a different message, since applications # will make fairly laser-specific assumptions about this data std_msgs/Header header # timestamp in the header is the acquisition time of # the first ray in the scan. # # in frame frame_id, angles are measured around # the positive Z axis (counterclockwise, if Z is up) # with zero angle being forward along the x axis float32 angle_min # start angle of the scan [rad] float32 angle_max # end angle of the scan [rad] float32 angle_increment # angular distance between measurements [rad] float32 time_increment # time between measurements [seconds] - if your scanner # is moving, this will be used in interpolating position # of 3d points float32 scan_time # time between scans [seconds] float32 range_min # minimum range value [m] float32 range_max # maximum range value [m] LaserEcho[] ranges # range data [m] # (Note: NaNs, values < range_min or > range_max should be discarded) # +Inf measurements are out of range # -Inf measurements are too close to determine exact distance. LaserEcho[] intensities # intensity data [device-specific units]. If your # device does not provide intensities, please leave # the array empty. ------------------------------------------------------------ Message Type: sensor_msgs/Illuminance.msg ------------------------------------------------------------ # Single photometric illuminance measurement. Light should be assumed to be # measured along the sensor's x-axis (the area of detection is the y-z plane). # The illuminance should have a 0 or positive value and be received with # the sensor's +X axis pointing toward the light source. # # Photometric illuminance is the measure of the human eye's sensitivity of the # intensity of light encountering or passing through a surface. # # All other Photometric and Radiometric measurements should not use this message. # This message cannot represent: # - Luminous intensity (candela/light source output) # - Luminance (nits/light output per area) # - Irradiance (watt/area), etc. std_msgs/Header header # timestamp is the time the illuminance was measured # frame_id is the location and direction of the reading float64 illuminance # Measurement of the Photometric Illuminance in Lux. float64 variance # 0 is interpreted as variance unknown ------------------------------------------------------------ Message Type: sensor_msgs/CompressedImage.msg ------------------------------------------------------------ # This message contains a compressed image. std_msgs/Header header # Header timestamp should be acquisition time of image # Header frame_id should be optical frame of camera # origin of frame should be optical center of cameara # +x should point to the right in the image # +y should point down in the image # +z should point into to plane of the image string format # Specifies the format of the data # Acceptable values: # jpeg, png, tiff uint8[] data # Compressed image buffer ------------------------------------------------------------ Message Type: sensor_msgs/TimeReference.msg ------------------------------------------------------------ # Measurement from an external time source not actively synchronized with the system clock. std_msgs/Header header # stamp is system time for which measurement was valid # frame_id is not used builtin_interfaces/Time time_ref # corresponding time from this external source string source # (optional) name of time source ------------------------------------------------------------ Message Type: sensor_msgs/PointCloud2.msg ------------------------------------------------------------ # This message holds a collection of N-dimensional points, which may # contain additional information such as normals, intensity, etc. The # point data is stored as a binary blob, its layout described by the # contents of the "fields" array. # # The point cloud data may be organized 2d (image-like) or 1d (unordered). # Point clouds organized as 2d images may be produced by camera depth sensors # such as stereo or time-of-flight. # Time of sensor data acquisition, and the coordinate frame ID (for 3d points). std_msgs/Header header # 2D structure of the point cloud. If the cloud is unordered, height is # 1 and width is the length of the point cloud. uint32 height uint32 width # Describes the channels and their layout in the binary data blob. PointField[] fields bool is_bigendian # Is this data bigendian? uint32 point_step # Length of a point in bytes uint32 row_step # Length of a row in bytes uint8[] data # Actual point data, size is (row_step*height) bool is_dense # True if there are no invalid points ------------------------------------------------------------ Message Type: sensor_msgs/ChannelFloat32.msg ------------------------------------------------------------ # This message is used by the PointCloud message to hold optional data # associated with each point in the cloud. The length of the values # array should be the same as the length of the points array in the # PointCloud, and each value should be associated with the corresponding # point. # # Channel names in existing practice include: # "u", "v" - row and column (respectively) in the left stereo image. # This is opposite to usual conventions but remains for # historical reasons. The newer PointCloud2 message has no # such problem. # "rgb" - For point clouds produced by color stereo cameras. uint8 # (R,G,B) values packed into the least significant 24 bits, # in order. # "intensity" - laser or pixel intensity. # "distance" # The channel name should give semantics of the channel (e.g. # "intensity" instead of "value"). string name # The values array should be 1-1 with the elements of the associated # PointCloud. float32[] values ------------------------------------------------------------ Message Type: sensor_msgs/RelativeHumidity.msg ------------------------------------------------------------ # Single reading from a relative humidity sensor. # Defines the ratio of partial pressure of water vapor to the saturated vapor # pressure at a temperature. std_msgs/Header header # timestamp of the measurement # frame_id is the location of the humidity sensor float64 relative_humidity # Expression of the relative humidity # from 0.0 to 1.0. # 0.0 is no partial pressure of water vapor # 1.0 represents partial pressure of saturation float64 variance # 0 is interpreted as variance unknown ------------------------------------------------------------ Message Type: sensor_msgs/RegionOfInterest.msg ------------------------------------------------------------ # This message is used to specify a region of interest within an image. # # When used to specify the ROI setting of the camera when the image was # taken, the height and width fields should either match the height and # width fields for the associated image; or height = width = 0 # indicates that the full resolution image was captured. uint32 x_offset # Leftmost pixel of the ROI # (0 if the ROI includes the left edge of the image) uint32 y_offset # Topmost pixel of the ROI # (0 if the ROI includes the top edge of the image) uint32 height # Height of ROI uint32 width # Width of ROI # True if a distinct rectified ROI should be calculated from the "raw" # ROI in this message. Typically this should be False if the full image # is captured (ROI not used), and True if a subwindow is captured (ROI # used). bool do_rectify ------------------------------------------------------------ Message Type: sensor_msgs/JointState.msg ------------------------------------------------------------ # This is a message that holds data to describe the state of a set of torque controlled joints. # # The state of each joint (revolute or prismatic) is defined by: # * the position of the joint (rad or m), # * the velocity of the joint (rad/s or m/s) and # * the effort that is applied in the joint (Nm or N). # # Each joint is uniquely identified by its name # The header specifies the time at which the joint states were recorded. All the joint states # in one message have to be recorded at the same time. # # This message consists of a multiple arrays, one for each part of the joint state. # The goal is to make each of the fields optional. When e.g. your joints have no # effort associated with them, you can leave the effort array empty. # # All arrays in this message should have the same size, or be empty. # This is the only way to uniquely associate the joint name with the correct # states. std_msgs/Header header string[] name float64[] position float64[] velocity float64[] effort ------------------------------------------------------------ Message Type: sensor_msgs/BatteryState.msg ------------------------------------------------------------ # Constants are chosen to match the enums in the linux kernel # defined in include/linux/power_supply.h as of version 3.7 # The one difference is for style reasons the constants are # all uppercase not mixed case. # Power supply status constants uint8 POWER_SUPPLY_STATUS_UNKNOWN = 0 uint8 POWER_SUPPLY_STATUS_CHARGING = 1 uint8 POWER_SUPPLY_STATUS_DISCHARGING = 2 uint8 POWER_SUPPLY_STATUS_NOT_CHARGING = 3 uint8 POWER_SUPPLY_STATUS_FULL = 4 # Power supply health constants uint8 POWER_SUPPLY_HEALTH_UNKNOWN = 0 uint8 POWER_SUPPLY_HEALTH_GOOD = 1 uint8 POWER_SUPPLY_HEALTH_OVERHEAT = 2 uint8 POWER_SUPPLY_HEALTH_DEAD = 3 uint8 POWER_SUPPLY_HEALTH_OVERVOLTAGE = 4 uint8 POWER_SUPPLY_HEALTH_UNSPEC_FAILURE = 5 uint8 POWER_SUPPLY_HEALTH_COLD = 6 uint8 POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE = 7 uint8 POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE = 8 # Power supply technology (chemistry) constants uint8 POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0 uint8 POWER_SUPPLY_TECHNOLOGY_NIMH = 1 uint8 POWER_SUPPLY_TECHNOLOGY_LION = 2 uint8 POWER_SUPPLY_TECHNOLOGY_LIPO = 3 uint8 POWER_SUPPLY_TECHNOLOGY_LIFE = 4 uint8 POWER_SUPPLY_TECHNOLOGY_NICD = 5 uint8 POWER_SUPPLY_TECHNOLOGY_LIMN = 6 std_msgs/Header header float32 voltage # Voltage in Volts (Mandatory) float32 temperature # Temperature in Degrees Celsius (If unmeasured NaN) float32 current # Negative when discharging (A) (If unmeasured NaN) float32 charge # Current charge in Ah (If unmeasured NaN) float32 capacity # Capacity in Ah (last full capacity) (If unmeasured NaN) float32 design_capacity # Capacity in Ah (design capacity) (If unmeasured NaN) float32 percentage # Charge percentage on 0 to 1 range (If unmeasured NaN) uint8 power_supply_status # The charging status as reported. Values defined above uint8 power_supply_health # The battery health metric. Values defined above uint8 power_supply_technology # The battery chemistry. Values defined above bool present # True if the battery is present float32[] cell_voltage # An array of individual cell voltages for each cell in the pack # If individual voltages unknown but number of cells known set each to NaN float32[] cell_temperature # An array of individual cell temperatures for each cell in the pack # If individual temperatures unknown but number of cells known set each to NaN string location # The location into which the battery is inserted. (slot number or plug) string serial_number # The best approximation of the battery serial number ------------------------------------------------------------ Message Type: sensor_msgs/NavSatFix.msg ------------------------------------------------------------ # Navigation Satellite fix for any Global Navigation Satellite System # # Specified using the WGS 84 reference ellipsoid # header.stamp specifies the ROS time for this measurement (the # corresponding satellite time may be reported using the # sensor_msgs/TimeReference message). # # header.frame_id is the frame of reference reported by the satellite # receiver, usually the location of the antenna. This is a # Euclidean frame relative to the vehicle, not a reference # ellipsoid. std_msgs/Header header # Satellite fix status information. NavSatStatus status # Latitude [degrees]. Positive is north of equator; negative is south. float64 latitude # Longitude [degrees]. Positive is east of prime meridian; negative is west. float64 longitude # Altitude [m]. Positive is above the WGS 84 ellipsoid # (quiet NaN if no altitude is available). float64 altitude # Position covariance [m^2] defined relative to a tangential plane # through the reported position. The components are East, North, and # Up (ENU), in row-major order. # # Beware: this coordinate system exhibits singularities at the poles. float64[9] position_covariance # If the covariance of the fix is known, fill it in completely. If the # GPS receiver provides the variance of each measurement, put them # along the diagonal. If only Dilution of Precision is available, # estimate an approximate covariance from that. uint8 COVARIANCE_TYPE_UNKNOWN = 0 uint8 COVARIANCE_TYPE_APPROXIMATED = 1 uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN = 2 uint8 COVARIANCE_TYPE_KNOWN = 3 uint8 position_covariance_type ------------------------------------------------------------ Message Type: sensor_msgs/JoyFeedbackArray.msg ------------------------------------------------------------ # This message publishes values for multiple feedback at once. JoyFeedback[] array ------------------------------------------------------------ Message Type: sensor_msgs/MagneticField.msg ------------------------------------------------------------ # Measurement of the Magnetic Field vector at a specific location. # # If the covariance of the measurement is known, it should be filled in. # If all you know is the variance of each measurement, e.g. from the datasheet, # just put those along the diagonal. # A covariance matrix of all zeros will be interpreted as "covariance unknown", # and to use the data a covariance will have to be assumed or gotten from some # other source. std_msgs/Header header # timestamp is the time the # field was measured # frame_id is the location and orientation # of the field measurement geometry_msgs/Vector3 magnetic_field # x, y, and z components of the # field vector in Tesla # If your sensor does not output 3 axes, # put NaNs in the components not reported. float64[9] magnetic_field_covariance # Row major about x, y, z axes # 0 is interpreted as variance unknown ------------------------------------------------------------ Message Type: sensor_msgs/PointField.msg ------------------------------------------------------------ # This message holds the description of one point entry in the # PointCloud2 message format. uint8 INT8 = 1 uint8 UINT8 = 2 uint8 INT16 = 3 uint8 UINT16 = 4 uint8 INT32 = 5 uint8 UINT32 = 6 uint8 FLOAT32 = 7 uint8 FLOAT64 = 8 # Common PointField names are x, y, z, intensity, rgb, rgba string name # Name of field uint32 offset # Offset from start of point struct uint8 datatype # Datatype enumeration, see above uint32 count # How many elements in the field ------------------------------------------------------------ Message Type: sensor_msgs/NavSatStatus.msg ------------------------------------------------------------ # Navigation Satellite fix status for any Global Navigation Satellite System. # # Whether to output an augmented fix is determined by both the fix # type and the last time differential corrections were received. A # fix is valid when status >= STATUS_FIX. int8 STATUS_NO_FIX = -1 # unable to fix position int8 STATUS_FIX = 0 # unaugmented fix int8 STATUS_SBAS_FIX = 1 # with satellite-based augmentation int8 STATUS_GBAS_FIX = 2 # with ground-based augmentation int8 status # Bits defining which Global Navigation Satellite System signals were # used by the receiver. uint16 SERVICE_GPS = 1 uint16 SERVICE_GLONASS = 2 uint16 SERVICE_COMPASS = 4 # includes BeiDou. uint16 SERVICE_GALILEO = 8 uint16 service ------------------------------------------------------------ Message Type: sensor_msgs/Joy.msg ------------------------------------------------------------ # Reports the state of a joystick's axes and buttons. # The timestamp is the time at which data is received from the joystick. std_msgs/Header header # The axes measurements from a joystick. float32[] axes # The buttons measurements from a joystick. int32[] buttons ------------------------------------------------------------ Message Type: sensor_msgs/Imu.msg ------------------------------------------------------------ # This is a message to hold data from an IMU (Inertial Measurement Unit) # # Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec # # If the covariance of the measurement is known, it should be filled in (if all you know is the # variance of each measurement, e.g. from the datasheet, just put those along the diagonal) # A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the # data a covariance will have to be assumed or gotten from some other source # # If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an # orientation estimate), please set element 0 of the associated covariance matrix to -1 # If you are interpreting this message, please check for a value of -1 in the first element of each # covariance matrix, and disregard the associated estimate. std_msgs/Header header geometry_msgs/Quaternion orientation float64[9] orientation_covariance # Row major about x, y, z axes geometry_msgs/Vector3 angular_velocity float64[9] angular_velocity_covariance # Row major about x, y, z axes geometry_msgs/Vector3 linear_acceleration float64[9] linear_acceleration_covariance # Row major x, y z ------------------------------------------------------------ Message Type: sensor_msgs/LaserScan.msg ------------------------------------------------------------ # Single scan from a planar laser range-finder # # If you have another ranging device with different behavior (e.g. a sonar # array), please find or create a different message, since applications # will make fairly laser-specific assumptions about this data std_msgs/Header header # timestamp in the header is the acquisition time of # the first ray in the scan. # # in frame frame_id, angles are measured around # the positive Z axis (counterclockwise, if Z is up) # with zero angle being forward along the x axis float32 angle_min # start angle of the scan [rad] float32 angle_max # end angle of the scan [rad] float32 angle_increment # angular distance between measurements [rad] float32 time_increment # time between measurements [seconds] - if your scanner # is moving, this will be used in interpolating position # of 3d points float32 scan_time # time between scans [seconds] float32 range_min # minimum range value [m] float32 range_max # maximum range value [m] float32[] ranges # range data [m] # (Note: values < range_min or > range_max should be discarded) float32[] intensities # intensity data [device-specific units]. If your # device does not provide intensities, please leave # the array empty. ------------------------------------------------------------ Message Type: sensor_msgs/FluidPressure.msg ------------------------------------------------------------ # Single pressure reading. This message is appropriate for measuring the # pressure inside of a fluid (air, water, etc). This also includes # atmospheric or barometric pressure. # # This message is not appropriate for force/pressure contact sensors. std_msgs/Header header # timestamp of the measurement # frame_id is the location of the pressure sensor float64 fluid_pressure # Absolute pressure reading in Pascals. float64 variance # 0 is interpreted as variance unknown ------------------------------------------------------------ Message Type: sensor_msgs/MultiDOFJointState.msg ------------------------------------------------------------ # Representation of state for joints with multiple degrees of freedom, # following the structure of JointState which can only represent a single degree of freedom. # # It is assumed that a joint in a system corresponds to a transform that gets applied # along the kinematic chain. For example, a planar joint (as in URDF) is 3DOF (x, y, yaw) # and those 3DOF can be expressed as a transformation matrix, and that transformation # matrix can be converted back to (x, y, yaw) # # Each joint is uniquely identified by its name # The header specifies the time at which the joint states were recorded. All the joint states # in one message have to be recorded at the same time. # # This message consists of a multiple arrays, one for each part of the joint state. # The goal is to make each of the fields optional. When e.g. your joints have no # wrench associated with them, you can leave the wrench array empty. # # All arrays in this message should have the same size, or be empty. # This is the only way to uniquely associate the joint name with the correct # states. std_msgs/Header header string[] joint_names geometry_msgs/Transform[] transforms geometry_msgs/Twist[] twist geometry_msgs/Wrench[] wrench ------------------------------------------------------------ Message Type: sensor_msgs/Temperature.msg ------------------------------------------------------------ # Single temperature reading. std_msgs/Header header # timestamp is the time the temperature was measured # frame_id is the location of the temperature reading float64 temperature # Measurement of the Temperature in Degrees Celsius. float64 variance # 0 is interpreted as variance unknown. ------------------------------------------------------------ Message Type: std_msgs/Char.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. char data ------------------------------------------------------------ Message Type: std_msgs/Bool.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. bool data ------------------------------------------------------------ Message Type: std_msgs/Float64MultiArray.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. # Please look at the MultiArrayLayout message definition for # documentation on all multiarrays. MultiArrayLayout layout # specification of data layout float64[] data # array of data ------------------------------------------------------------ Message Type: std_msgs/Float32MultiArray.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. # Please look at the MultiArrayLayout message definition for # documentation on all multiarrays. MultiArrayLayout layout # specification of data layout float32[] data # array of data ------------------------------------------------------------ Message Type: std_msgs/String.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. string data ------------------------------------------------------------ Message Type: std_msgs/Int16.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. int16 data ------------------------------------------------------------ Message Type: std_msgs/Float32.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. float32 data ------------------------------------------------------------ Message Type: std_msgs/Int16MultiArray.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. # Please look at the MultiArrayLayout message definition for # documentation on all multiarrays. MultiArrayLayout layout # specification of data layout int16[] data # array of data ------------------------------------------------------------ Message Type: std_msgs/Int8.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. int8 data ------------------------------------------------------------ Message Type: std_msgs/UInt16MultiArray.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. # Please look at the MultiArrayLayout message definition for # documentation on all multiarrays. MultiArrayLayout layout # specification of data layout uint16[] data # array of data ------------------------------------------------------------ Message Type: std_msgs/UInt64MultiArray.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. # Please look at the MultiArrayLayout message definition for # documentation on all multiarrays. MultiArrayLayout layout # specification of data layout uint64[] data # array of data ------------------------------------------------------------ Message Type: std_msgs/Float64.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. float64 data ------------------------------------------------------------ Message Type: std_msgs/Int32.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. int32 data ------------------------------------------------------------ Message Type: std_msgs/UInt16.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. uint16 data ------------------------------------------------------------ Message Type: std_msgs/MultiArrayDimension.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. string label # label of given dimension uint32 size # size of given dimension (in type units) uint32 stride # stride of given dimension ------------------------------------------------------------ Message Type: std_msgs/Int64.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. int64 data ------------------------------------------------------------ Message Type: std_msgs/UInt64.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. uint64 data ------------------------------------------------------------ Message Type: std_msgs/UInt8MultiArray.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. # Please look at the MultiArrayLayout message definition for # documentation on all multiarrays. MultiArrayLayout layout # specification of data layout uint8[] data # array of data ------------------------------------------------------------ Message Type: std_msgs/Int8MultiArray.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. # Please look at the MultiArrayLayout message definition for # documentation on all multiarrays. MultiArrayLayout layout # specification of data layout int8[] data # array of data ------------------------------------------------------------ Message Type: std_msgs/Byte.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. byte data ------------------------------------------------------------ Message Type: std_msgs/Int32MultiArray.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. # Please look at the MultiArrayLayout message definition for # documentation on all multiarrays. MultiArrayLayout layout # specification of data layout int32[] data # array of data ------------------------------------------------------------ Message Type: std_msgs/UInt8.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. uint8 data ------------------------------------------------------------ Message Type: std_msgs/Header.msg ------------------------------------------------------------ # Standard metadata for higher-level stamped data types. # This is generally used to communicate timestamped data # in a particular coordinate frame. # Two-integer timestamp that is expressed as seconds and nanoseconds. builtin_interfaces/Time stamp # Transform frame with which this data is associated. string frame_id ------------------------------------------------------------ Message Type: std_msgs/ByteMultiArray.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. # Please look at the MultiArrayLayout message definition for # documentation on all multiarrays. MultiArrayLayout layout # specification of data layout byte[] data # array of data ------------------------------------------------------------ Message Type: std_msgs/ColorRGBA.msg ------------------------------------------------------------ float32 r float32 g float32 b float32 a ------------------------------------------------------------ Message Type: std_msgs/UInt32.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. uint32 data ------------------------------------------------------------ Message Type: std_msgs/Empty.msg ------------------------------------------------------------ ------------------------------------------------------------ Message Type: std_msgs/Int64MultiArray.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. # Please look at the MultiArrayLayout message definition for # documentation on all multiarrays. MultiArrayLayout layout # specification of data layout int64[] data # array of data ------------------------------------------------------------ Message Type: std_msgs/UInt32MultiArray.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. # Please look at the MultiArrayLayout message definition for # documentation on all multiarrays. MultiArrayLayout layout # specification of data layout uint32[] data # array of data ------------------------------------------------------------ Message Type: std_msgs/MultiArrayLayout.msg ------------------------------------------------------------ # This was originally provided as an example message. # It is deprecated as of Foxy # It is recommended to create your own semantically meaningful message. # However if you would like to continue using this please use the equivalent in example_msgs. # The multiarray declares a generic multi-dimensional array of a # particular data type. Dimensions are ordered from outer most # to inner most. # # Accessors should ALWAYS be written in terms of dimension stride # and specified outer-most dimension first. # # multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k] # # A standard, 3-channel 640x480 image with interleaved color channels # would be specified as: # # dim[0].label = "height" # dim[0].size = 480 # dim[0].stride = 3*640*480 = 921600 (note dim[0] stride is just size of image) # dim[1].label = "width" # dim[1].size = 640 # dim[1].stride = 3*640 = 1920 # dim[2].label = "channel" # dim[2].size = 3 # dim[2].stride = 3 # # multiarray(i,j,k) refers to the ith row, jth column, and kth channel. MultiArrayDimension[] dim # Array of dimension properties uint32 data_offset # padding bytes at front of data ------------------------------------------------------------ Message Type: diagnostic_msgs/DiagnosticArray.msg ------------------------------------------------------------ # This message is used to send diagnostic information about the state of the robot. std_msgs/Header header # for timestamp DiagnosticStatus[] status # an array of components being reported on ------------------------------------------------------------ Message Type: diagnostic_msgs/DiagnosticStatus.msg ------------------------------------------------------------ # This message holds the status of an individual component of the robot. # Possible levels of operations. byte OK=0 byte WARN=1 byte ERROR=2 byte STALE=3 # Level of operation enumerated above. byte level # A description of the test/component reporting. string name # A description of the status. string message # A hardware unique string. string hardware_id # An array of values associated with the status. KeyValue[] values ------------------------------------------------------------ Message Type: diagnostic_msgs/KeyValue.msg ------------------------------------------------------------ # What to label this value when viewing. string key # A value to track over time. string value ------------------------------------------------------------ Message Type: trajectory_msgs/MultiDOFJointTrajectory.msg ------------------------------------------------------------ # The header is used to specify the coordinate frame and the reference time for the trajectory durations std_msgs/Header header # A representation of a multi-dof joint trajectory (each point is a transformation) # Each point along the trajectory will include an array of positions/velocities/accelerations # that has the same length as the array of joint names, and has the same order of joints as # the joint names array. string[] joint_names MultiDOFJointTrajectoryPoint[] points ------------------------------------------------------------ Message Type: trajectory_msgs/JointTrajectory.msg ------------------------------------------------------------ # The header is used to specify the coordinate frame and the reference time for # the trajectory durations std_msgs/Header header # The names of the active joints in each trajectory point. These names are # ordered and must correspond to the values in each trajectory point. string[] joint_names # Array of trajectory points, which describe the positions, velocities, # accelerations and/or efforts of the joints at each time point. JointTrajectoryPoint[] points ------------------------------------------------------------ Message Type: trajectory_msgs/JointTrajectoryPoint.msg ------------------------------------------------------------ # Each trajectory point specifies either positions[, velocities[, accelerations]] # or positions[, effort] for the trajectory to be executed. # All specified values are in the same order as the joint names in JointTrajectory.msg. # Single DOF joint positions for each joint relative to their "0" position. # The units depend on the specific joint type: radians for revolute or # continuous joints, and meters for prismatic joints. float64[] positions # The rate of change in position of each joint. Units are joint type dependent. # Radians/second for revolute or continuous joints, and meters/second for # prismatic joints. float64[] velocities # Rate of change in velocity of each joint. Units are joint type dependent. # Radians/second^2 for revolute or continuous joints, and meters/second^2 for # prismatic joints. float64[] accelerations # The torque or the force to be applied at each joint. For revolute/continuous # joints effort denotes a torque in newton-meters. For prismatic joints, effort # denotes a force in newtons. float64[] effort # Desired time from the trajectory start to arrive at this trajectory point. builtin_interfaces/Duration time_from_start ------------------------------------------------------------ Message Type: trajectory_msgs/MultiDOFJointTrajectoryPoint.msg ------------------------------------------------------------ # Each multi-dof joint can specify a transform (up to 6 DOF). geometry_msgs/Transform[] transforms # There can be a velocity specified for the origin of the joint. geometry_msgs/Twist[] velocities # There can be an acceleration specified for the origin of the joint. geometry_msgs/Twist[] accelerations # Desired time from the trajectory start to arrive at this trajectory point. builtin_interfaces/Duration time_from_start ------------------------------------------------------------ Message Type: shape_msgs/SolidPrimitive.msg ------------------------------------------------------------ # Defines box, sphere, cylinder, cone and prism. # All shapes are defined to have their bounding boxes centered around 0,0,0. uint8 BOX=1 uint8 SPHERE=2 uint8 CYLINDER=3 uint8 CONE=4 uint8 PRISM=5 # The type of the shape uint8 type # The dimensions of the shape float64[<=3] dimensions # At no point will dimensions have a length > 3. # The meaning of the shape dimensions: each constant defines the index in the 'dimensions' array. # For type BOX, the X, Y, and Z dimensions are the length of the corresponding sides of the box. uint8 BOX_X=0 uint8 BOX_Y=1 uint8 BOX_Z=2 # For the SPHERE type, only one component is used, and it gives the radius of the sphere. uint8 SPHERE_RADIUS=0 # For the CYLINDER and CONE types, the center line is oriented along the Z axis. # Therefore the CYLINDER_HEIGHT (CONE_HEIGHT) component of dimensions gives the # height of the cylinder (cone). # The CYLINDER_RADIUS (CONE_RADIUS) component of dimensions gives the radius of # the base of the cylinder (cone). # Cone and cylinder primitives are defined to be circular. The tip of the cone # is pointing up, along +Z axis. uint8 CYLINDER_HEIGHT=0 uint8 CYLINDER_RADIUS=1 uint8 CONE_HEIGHT=0 uint8 CONE_RADIUS=1 # For the type PRISM, the center line is oriented along Z axis. # The PRISM_HEIGHT component of dimensions gives the # height of the prism. # The polygon defines the Z axis centered base of the prism. # The prism is constructed by extruding the base in +Z and -Z # directions by half of the PRISM_HEIGHT # Only x and y fields of the points are used in the polygon. # Points of the polygon are ordered counter-clockwise. uint8 PRISM_HEIGHT=0 geometry_msgs/Polygon polygon ------------------------------------------------------------ Message Type: shape_msgs/Plane.msg ------------------------------------------------------------ # Representation of a plane, using the plane equation ax + by + cz + d = 0. # # a := coef[0] # b := coef[1] # c := coef[2] # d := coef[3] float64[4] coef ------------------------------------------------------------ Message Type: shape_msgs/Mesh.msg ------------------------------------------------------------ # Definition of a mesh. # List of triangles; the index values refer to positions in vertices[]. MeshTriangle[] triangles # The actual vertices that make up the mesh. geometry_msgs/Point[] vertices ------------------------------------------------------------ Message Type: shape_msgs/MeshTriangle.msg ------------------------------------------------------------ # Definition of a triangle's vertices. uint32[3] vertex_indices ------------------------------------------------------------ Message Type: geometry_msgs/PoseWithCovariance.msg ------------------------------------------------------------ # This represents a pose in free space with uncertainty. Pose pose # Row-major representation of the 6x6 covariance matrix # The orientation parameters use a fixed-axis representation. # In order, the parameters are: # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis) float64[36] covariance ------------------------------------------------------------ Message Type: geometry_msgs/WrenchStamped.msg ------------------------------------------------------------ # A wrench with reference coordinate frame and timestamp std_msgs/Header header Wrench wrench ------------------------------------------------------------ Message Type: geometry_msgs/InertiaStamped.msg ------------------------------------------------------------ # An Inertia with a time stamp and reference frame. std_msgs/Header header Inertia inertia ------------------------------------------------------------ Message Type: geometry_msgs/QuaternionStamped.msg ------------------------------------------------------------ # This represents an orientation with reference coordinate frame and timestamp. std_msgs/Header header Quaternion quaternion ------------------------------------------------------------ Message Type: geometry_msgs/Point.msg ------------------------------------------------------------ # This contains the position of a point in free space float64 x float64 y float64 z ------------------------------------------------------------ Message Type: geometry_msgs/Vector3.msg ------------------------------------------------------------ # This represents a vector in free space. # This is semantically different than a point. # A vector is always anchored at the origin. # When a transform is applied to a vector, only the rotational component is applied. float64 x float64 y float64 z ------------------------------------------------------------ Message Type: geometry_msgs/TwistWithCovarianceStamped.msg ------------------------------------------------------------ # This represents an estimated twist with reference coordinate frame and timestamp. std_msgs/Header header TwistWithCovariance twist ------------------------------------------------------------ Message Type: geometry_msgs/PointStamped.msg ------------------------------------------------------------ # This represents a Point with reference coordinate frame and timestamp std_msgs/Header header Point point ------------------------------------------------------------ Message Type: geometry_msgs/Accel.msg ------------------------------------------------------------ # This expresses acceleration in free space broken into its linear and angular parts. Vector3 linear Vector3 angular ------------------------------------------------------------ Message Type: geometry_msgs/PoseStamped.msg ------------------------------------------------------------ # A Pose with reference coordinate frame and timestamp std_msgs/Header header Pose pose ------------------------------------------------------------ Message Type: geometry_msgs/Pose.msg ------------------------------------------------------------ # A representation of pose in free space, composed of position and orientation. Point position Quaternion orientation ------------------------------------------------------------ Message Type: geometry_msgs/PoseArray.msg ------------------------------------------------------------ # An array of poses with a header for global reference. std_msgs/Header header Pose[] poses ------------------------------------------------------------ Message Type: geometry_msgs/Vector3Stamped.msg ------------------------------------------------------------ # This represents a Vector3 with reference coordinate frame and timestamp # Note that this follows vector semantics with it always anchored at the origin, # so the rotational elements of a transform are the only parts applied when transforming. std_msgs/Header header Vector3 vector ------------------------------------------------------------ Message Type: geometry_msgs/Quaternion.msg ------------------------------------------------------------ # This represents an orientation in free space in quaternion form. float64 x 0 float64 y 0 float64 z 0 float64 w 1 ------------------------------------------------------------ Message Type: geometry_msgs/AccelStamped.msg ------------------------------------------------------------ # An accel with reference coordinate frame and timestamp std_msgs/Header header Accel accel ------------------------------------------------------------ Message Type: geometry_msgs/PolygonStamped.msg ------------------------------------------------------------ # This represents a Polygon with reference coordinate frame and timestamp std_msgs/Header header Polygon polygon ------------------------------------------------------------ Message Type: geometry_msgs/TwistWithCovariance.msg ------------------------------------------------------------ # This expresses velocity in free space with uncertainty. Twist twist # Row-major representation of the 6x6 covariance matrix # The orientation parameters use a fixed-axis representation. # In order, the parameters are: # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis) float64[36] covariance ------------------------------------------------------------ Message Type: geometry_msgs/Pose2D.msg ------------------------------------------------------------ # Deprecated as of Foxy and will potentially be removed in any following release. # Please use the full 3D pose. # In general our recommendation is to use a full 3D representation of everything and for 2D specific applications make the appropriate projections into the plane for their calculations but optimally will preserve the 3D information during processing. # If we have parallel copies of 2D datatypes every UI and other pipeline will end up needing to have dual interfaces to plot everything. And you will end up with not being able to use 3D tools for 2D use cases even if they're completely valid, as you'd have to reimplement it with different inputs and outputs. It's not particularly hard to plot the 2D pose or compute the yaw error for the Pose message and there are already tools and libraries that can do this for you.# This expresses a position and orientation on a 2D manifold. float64 x float64 y float64 theta ------------------------------------------------------------ Message Type: geometry_msgs/AccelWithCovariance.msg ------------------------------------------------------------ # This expresses acceleration in free space with uncertainty. Accel accel # Row-major representation of the 6x6 covariance matrix # The orientation parameters use a fixed-axis representation. # In order, the parameters are: # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis) float64[36] covariance ------------------------------------------------------------ Message Type: geometry_msgs/AccelWithCovarianceStamped.msg ------------------------------------------------------------ # This represents an estimated accel with reference coordinate frame and timestamp. std_msgs/Header header AccelWithCovariance accel ------------------------------------------------------------ Message Type: geometry_msgs/Inertia.msg ------------------------------------------------------------ # Mass [kg] float64 m # Center of mass [m] geometry_msgs/Vector3 com # Inertia Tensor [kg-m^2] # | ixx ixy ixz | # I = | ixy iyy iyz | # | ixz iyz izz | float64 ixx float64 ixy float64 ixz float64 iyy float64 iyz float64 izz ------------------------------------------------------------ Message Type: geometry_msgs/Wrench.msg ------------------------------------------------------------ # This represents force in free space, separated into its linear and angular parts. Vector3 force Vector3 torque ------------------------------------------------------------ Message Type: geometry_msgs/TwistStamped.msg ------------------------------------------------------------ # A twist with reference coordinate frame and timestamp std_msgs/Header header Twist twist ------------------------------------------------------------ Message Type: geometry_msgs/Twist.msg ------------------------------------------------------------ # This expresses velocity in free space broken into its linear and angular parts. Vector3 linear Vector3 angular ------------------------------------------------------------ Message Type: geometry_msgs/PoseWithCovarianceStamped.msg ------------------------------------------------------------ # This expresses an estimated pose with a reference coordinate frame and timestamp std_msgs/Header header PoseWithCovariance pose ------------------------------------------------------------ Message Type: geometry_msgs/Point32.msg ------------------------------------------------------------ # This contains the position of a point in free space(with 32 bits of precision). # It is recommended to use Point wherever possible instead of Point32. # # This recommendation is to promote interoperability. # # This message is designed to take up less space when sending # lots of points at once, as in the case of a PointCloud. float32 x float32 y float32 z ------------------------------------------------------------ Message Type: geometry_msgs/TransformStamped.msg ------------------------------------------------------------ # This expresses a transform from coordinate frame header.frame_id # to the coordinate frame child_frame_id at the time of header.stamp # # This message is mostly used by the # tf2 package. # See its documentation for more information. # # The child_frame_id is necessary in addition to the frame_id # in the Header to communicate the full reference for the transform # in a self contained message. # The frame id in the header is used as the reference frame of this transform. std_msgs/Header header # The frame id of the child frame to which this transform points. string child_frame_id # Translation and rotation in 3-dimensions of child_frame_id from header.frame_id. Transform transform ------------------------------------------------------------ Message Type: geometry_msgs/Polygon.msg ------------------------------------------------------------ # A specification of a polygon where the first and last points are assumed to be connected Point32[] points ------------------------------------------------------------ Message Type: geometry_msgs/Transform.msg ------------------------------------------------------------ # This represents the transform between two coordinate frames in free space. Vector3 translation Quaternion rotation ------------------------------------------------------------ Message Type: actionlib_msgs/GoalStatusArray.msg ------------------------------------------------------------ # Stores the statuses for goals that are currently being tracked # by an action server std_msgs/Header header GoalStatus[] status_list ------------------------------------------------------------ Message Type: actionlib_msgs/GoalStatus.msg ------------------------------------------------------------ GoalID goal_id uint8 status uint8 PENDING = 0 # The goal has yet to be processed by the action server. uint8 ACTIVE = 1 # The goal is currently being processed by the action server. uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing # and has since completed its execution (Terminal State). uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server # (Terminal State). uint8 ABORTED = 4 # The goal was aborted during execution by the action server due # to some failure (Terminal State). uint8 REJECTED = 5 # The goal was rejected by the action server without being processed, # because the goal was unattainable or invalid (Terminal State). uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing # and has not yet completed execution. uint8 RECALLING = 7 # The goal received a cancel request before it started executing, but # the action server has not yet confirmed that the goal is canceled. uint8 RECALLED = 8 # The goal received a cancel request before it started executing # and was successfully cancelled (Terminal State). uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not # be sent over the wire by an action server. # Allow for the user to associate a string with GoalStatus for debugging. string text ------------------------------------------------------------ Message Type: actionlib_msgs/GoalID.msg ------------------------------------------------------------ # The stamp should store the time at which this goal was requested. # It is used by an action server when it tries to preempt all # goals that were requested before a certain time builtin_interfaces/Time stamp # The id provides a way to associate feedback and # result message with specific goal requests. The id # specified must be unique. string id ``` # ROS Architecture ## Related Readings * [Robot Operating System 2: Design, Architecture, and Uses In the Wild](https://arxiv.org/pdf/2211.07752.pdf) * [ROS Humble - Executor Explained](https://docs.ros.org/en/humble/Concepts/Intermediate/About-Executors.html) * [Executor Class Reference](https://docs.ros2.org/dashing/api/rclcpp/classrclcpp_1_1executor_1_1Executor.html#details) # Intra-process Communication ## Related Readings * [ROS2 Design - Intra-process Communication](https://design.ros2.org/articles/intraprocess_communications.html) * [C++ Smart Pointers Lecture](https://websites.umich.edu/~eecs381/handouts/C++11_smart_ptrs.pdf) The ROS2 Design - Intra-process Communication is a design doc but it captures the main ideas in the actual implementation. Note that for ROS Humble, the intra-process communication is still an experimental feature.
The key of the intra-processs is to keep the message object inside the memory and avoid copy. In this article, we will take a look at the source code and see how the code determine when to make a copy and when to use a pointer. Let's start with the publisher.publish method:
The control is then passed to another publish method with different signature. It determins if we need to perform inter-process publication. Let's focus on the intra-process communication and take a look at the do\_intra\_process\_ros\_message\_publish method.
So jump to do\_intra\_process\_ros\_message\_publish method:
Not too much info here. Move on to do\_intra\_process\_publish method:
Things getting interesting. In this function, we can see many key concepts in the design: * The sub\_ids are related to the subscriptions. And we can see there are two types of subscriptions. The ones that own the message and the ones that share (or observe) the message. * We see the word buffers in some of the method names. * Note that for subscriptions that take the ownship of the message, we pass the original unique\_ptr of the message to them; for subscriptions that share (or observe) the message, a shared pointer is constructed. Depending on the scenario, we can either convert the unique\_ptr to the shared\_ptr or we need to construct a copy. The next question is how to control the type of the subscription?
The buffer type is resolved in the `resolve_intra_process_buffer_type.hpp` file:
We can either specify the buffer type explicitly or ask the code to determine the buffer type based on the parameter type of the callback function of the subscriber.\
# Navigation and Planning # Navigation Stack and Concepts Source: | Section | Description | | ------------------ | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | | 2D Nav Goal |

| | 2D Pose Estimate | | | Static Map | | | Particle Cloud |
  • Topic: particlecloud
  • Type: geometry\_msgs/PoseArray
  • Description: Displays the particle cloud used by the robot's localization system. The spread of the cloud represents the localization system's uncertainty about the robot's pose. A cloud that is very spread out reflects high uncertainty, while a condensed cloud represents low uncertainty.
| | Robot Footprint | | | Obstacles |
  • Topic: local\_costmap/obstacles
  • Type: nav\_msgs/GridCells
  • Desctiption: Displays the obstacles that the navigation stack sees in its costmap. For the robot to avoid collision, the robot footprint should never intersect with a cell that contains an obstacle.
| | Inflated Obstacles |
  • Topic: local\_costmap/inflated\_obstacles
  • Type: nav\_msgs/GridCells
  • Description: Displays obstacles in the navigation stack's costmap inflated by the inscribed radius of the robot. For the robot to avoid collision, the center point of the robot should never overlap with a cell that contains an inflated obstacle.
| | Unknown Space | | | Global Plan |
  • Topic: TrajectoryPlannerROS/global\_plan
  • Type: nav\_msgs/Path
  • Description: Displays the portion of the global plan that the local planner is currently pursuing.
| | Local Plan |
  • Topic: TrajectoryPlannerROS/local\_plan
  • Type: nav\_msgs/Path
  • Description: Displays the trajectory associated with the velocity commands currently being commanded to the base by the local planner.
| | Planner Plan |
  • Topic: TrajectoryPlannerROS/local\_plan
  • Type: nav\_msgs/Path
  • Description: Displays the trajectory associated with the velocity commands currently being commanded to the base by the local planner.
| | Current Goal |
  • Topic: current\_goal
  • Type: geometry\_msgs/PoseStamped
  • Description: Displays the goal pose that the navigation stack is attempting to achieve.
| # Navigation2 Implementation Overview ## Input to the navigation stack. In ROS2, there are three types of input: (1) topics, (2) service calls, and (3) actions. In this section, we will focus on the topics. | Component | Topics | Message Type | | ------------------------------------ | ------ | ------------------------ | | nav2\_costmap\_2d::CostmapSubscriber | | nav2\_msgs::msg::Costmap | | | | | | | | | # Cost Map Cost maps are the backbone of the navigation stack. It encodes information in the environments. There are two key components in the implementation
# Obstacle Avoidance and DWB Controller ## Related Readings * [Github repo of nav2\_dwb\_controller](https://github.com/ros-planning/navigation2/tree/humble/nav2_dwb_controller) * [ROS Document - dwa\_local\_planner](http://wiki.ros.org/dwa_local_planner) * [The Dynamic Window Approach to Collision Avoidance](https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf) * [Planning and Control in Unstructured Terrain](https://citeseerx.ist.psu.edu/document?repid=rep1\&type=pdf\&doi=dabdbb636f02d3cff3d546bd1bdae96a058ba4bc) * [Local Path Planning: Dynamic Window Approach With Virtual Manipulators Considering Dynamic Obstacles](https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=\&arnumber=9707826) ## Introduction In this article, we will explore the implementation of the DWB controller in Navigation2, a critic-based and highly configurable variant of the Dynamic Window Approach (DWA) Algorithm. We will start with a brief overview of the DWA algorithm, followed by a detailed examination of the DWB controller's key components, including the local planner, trajectory generator, and base obstacle critic. ## DWA Overview DWA simplifies robot movement calculation by assuming constant translation and rotational velocity during short time intervals. Under this assumption, the robot's path is reduced to either a straight line or an arc during these periods, making collision detection straightforward. The algorithm searches a 2D velocity space to determine the next command. Velocity pairs leading to potential collisions form inaccessible areas, while the robot's current velocity and acceleration limits confine the reachable space to a rectangular region centered around the current velocity of the robot. An objective function determines the best velocity command. For example, the function used in the DWA paper is as follows: $$ G(v, w) = \sigma(\alpha \cdot \textrm{heading}(v, w) + \beta \cdot \textrm{dist}(v, w) + \gamma \cdot \textrm{velocity}(v, w)) $$
## How is DWB implemented? In the previous section, we outlined three essential components of a dynamic-window-based algorithm: * **The dynamic window**. This is determined by the robot's current velocity and the maximum accelerations. * **The inaccessible regions**. These regions depend on the robot's current location and the locations of the obstacles, informed by sensor data and map information. * **The objective function**. The objective function evaluates various velocity commands to identify the optimal one. ## Core Components The core component in the DWB controller is the `DWBLocalPlanner` class, instantiated in [nav2\_controller/src/controller\_server.cpp](https://github.com/ros-planning/navigation2/blob/humble/nav2_controller/src/controller_server.cpp#L47). Rather than diving into the interactions between `DWBLocalPlanner` and other components in the navigation stack, this article focuses on the code that implements the DWA algorithm. In the Nav2 codebase, the standard practice for initializing an instance is through the `configure` method. For instance, the `configure` method in the `DWBLocalPlanner` class requires three arguments: `LifecycleNode`, `tf2`, and `Costmap2DROS`. These objects provide the context and inputs for the `DWBLocalPlanner` class. Parameters are another type of input. They are adjustable settings used to fine-tune the planner's behavior. We will cover this topic in a separate article. The outputs of the `DWBLocalPlanner` are handled by the `DWBPublisher` class, which is responsible for publishing computation results to specific topics. For example: ``` pub_->publishCostGrid(costmap_ros_, critics_); pub_->publishEvaluation(results); pub_->publishGlobalPlan(global_plan_); pub_->publishLocalPlan(pose.header, best.traj); pub_->publishTransformedPlan(transformed_plan); ``` ## Public Methods of Planner The `DWBLocalPlanner` has the following public methods: ``` setPlan setSpeedLimit scoreTrajectory computeVelocityCommands ``` The `setPlan` method suggests that the planner receives a global plan, which it may then publish or modify for further processing. The role of the `setSpeedLimit` method is self-explanatory: it sets the linear speed limit of the robot. Specifically, it sets the speed limit of the trajectory generator, the component responsible for simulating the robot's trajectory. The next two methods are related. `scoreTrajectory` is used to evaluate proposed trajectories when the planner computes the velocity commands. In the context of the DWA algorithm, an objective function is required to choose the optimal velocity command, and `scoreTrajectory` implements such a function. Let's take a look at the method `computeVelocityCommands`. Setting aside variable initialization and error handling, here is a simplified overview of its implementation: ``` nav_2d_msgs::msg::Twist2DStamped DWBLocalPlanner::computeVelocityCommands( const nav_2d_msgs::msg::Pose2DStamped & pose, const nav_2d_msgs::msg::Twist2D & velocity, std::shared_ptr & results) { nav_2d_msgs::msg::Path2D transformed_plan; nav_2d_msgs::msg::Pose2DStamped goal_pose; prepareGlobalPlan(pose, transformed_plan, goal_pose); nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); std::unique_lock lock(*(costmap->getMutex())); dwb_msgs::msg::TrajectoryScore best = coreScoringAlgorithm(pose.pose, velocity, results); // Return Value nav_2d_msgs::msg::Twist2DStamped cmd_vel; cmd_vel.header.stamp = clock_->now(); cmd_vel.velocity = best.traj.velocity; lock.unlock(); pub_->publishLocalPlan(pose.header, best.traj); pub_->publishCostGrid(costmap_ros_, critics_); return cmd_vel; } ``` It first prepares a global plan based on the current pose and the goal pose. Then, it locks the cost map so that the map cannot be modified during the calculation of the trajectory score. Next, it invokes the scoring algorithm and selects the best one. Finally, it unlocks the cost map and publishes the local plan and the cost map. Move on to the `coreScoringAlgorithm`. Setting aside variable initialization and the exception handling, the method has the following structure: ``` dwb_msgs::msg::TrajectoryScore DWBLocalPlanner::coreScoringAlgorithm( const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D velocity, std::shared_ptr & results) { traj_generator_->startNewIteration(velocity); while (traj_generator_->hasMoreTwists()) { twist = traj_generator_->nextTwist(); traj = traj_generator_->generateTrajectory(pose, velocity, twist); try { dwb_msgs::msg::TrajectoryScore score = scoreTrajectory(traj, best.total); tracker.addLegalTrajectory(); if (results) { results->twists.push_back(score); } if (best.total < 0 || score.total < best.total) { best = score; if (results) { results->best_index = results->twists.size() - 1; } } if (worst.total < 0 || score.total > worst.total) { worst = score; if (results) { results->worst_index = results->twists.size() - 1; } } } return best; } ``` It iterates through the candidate velocity command and generates the correspondent trajectory. Then it calculates the score for each trajectory and returns the best one. The key step in this function is the invocation of the method `scoreTrajectory`. The method `scoreTrajector` is straightforward. It iterates through the critics and each of them produces a score. The total score is used as the score of the trajectory. Recall in the paper, the objective function is defined as follows: $$ G(v, w) = \sigma(\alpha \cdot \textrm{heading}(v, w) + \beta \cdot \textrm{dist}(v, w) + \gamma \cdot \textrm{velocity}(v, w)) $$ In the formula, heading, dist, and velocity are critics and each produces a score. For example, the score produced by the `heading` critic is $$\textrm{heading}(v, w)$$. The coefficient $$\alpha$$ corresponds to the critic scale in the implementation. For a complete reference of critics, you can check out the [official document](https://navigation.ros.org/configuration/packages/configuring-dwb-controller.html#trajectory-critics). So far, we've examined the part in the `DWBLocalPlanner` that computes the velocity commands. We've learned the scoring algorithm, which implements the objective function in the DWA pager, is based on critics and this is the reason why nav2 DWB controller is called critic-based controller. The remaining question is where is the code that determines the dynamic window and accessible regions? This is answered in the next section. ## Trajectory Generator Two trajectory generators provided in navigation 2: * Standard trajectory generator ([StandardTrajectoryGenerator](https://github.com/ros-planning/navigation2/blob/humble/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp)) * Limited acceleration trajectory generator ([LimitedAccelGenerator](https://github.com/ros-planning/navigation2/blob/humble/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/limited_accel_generator.hpp)) It's worth noting that the `LimitedAccelGenerator` is a derived class of the `StandardTrajectoryGenerator`. One of the key methods in the trajectory generator class is the method `generateTrajectory`: ``` dwb_msgs::msg::Trajectory2D generateTrajectory( const geometry_msgs::msg::Pose2D & start_pose, const nav_2d_msgs::msg::Twist2D & start_vel, const nav_2d_msgs::msg::Twist2D & cmd_vel) override; ``` This method takes the start pose, start velocity, and the velocity command as inputs and generate a `Trajectory2D` object, whose definition is given as follows: ``` # cat ./nav2_dwb_controller/dwb_msgs/msg/Trajectory2D.msg # For a given velocity command, the poses that the robot will go to in the allotted time. # Input Velocity nav_2d_msgs/Twist2D velocity # Time difference between first and last poses builtin_interfaces/Duration[] time_offsets # Poses the robot will go to, given our kinematic model geometry_msgs/Pose2D[] poses ``` The implementation of the `generateTrajectory` method is straightforward. It essentially simulates a path: ``` dwb_msgs::msg::Trajectory2D StandardTrajectoryGenerator::generateTrajectory( const geometry_msgs::msg::Pose2D & start_pose, const nav_2d_msgs::msg::Twist2D & start_vel, const nav_2d_msgs::msg::Twist2D & cmd_vel) { dwb_msgs::msg::Trajectory2D traj; traj.velocity = cmd_vel; // simulate the trajectory geometry_msgs::msg::Pose2D pose = start_pose; nav_2d_msgs::msg::Twist2D vel = start_vel; double running_time = 0.0; std::vector steps = getTimeSteps(cmd_vel); traj.poses.push_back(start_pose); for (double dt : steps) { // calculate velocities vel = computeNewVelocity(cmd_vel, vel, dt); // update the position of the robot using the velocities passed in pose = computeNewPosition(pose, vel, dt); traj.poses.push_back(pose); traj.time_offsets.push_back(rclcpp::Duration::from_seconds(running_time)); running_time += dt; } // end for simulation steps if (include_last_point_) { traj.poses.push_back(pose); traj.time_offsets.push_back(rclcpp::Duration::from_seconds(running_time)); } return traj; } ``` Regarding the dynamic window in the DWA algorithm, it's implemented in the `computeNewVelocity` method, which is overridden by the `LimitedAccelGenerator` which takes the acceleration into account. If you search for keywords such as "obstacle" or "avoidance" in the [navigation2/nav2\_dwb\_controller/dwb\_plugins](https://github.com/ros-planning/navigation2/tree/humble/nav2_dwb_controller/dwb_plugins) directory, you won't find any results. This suggests that the obstacle avoidance and the related accessible region determination are handled by other parts of the code. ## Base Obstacle Critic You may still wonder how obstacle avoidance is handled by the code. Remember, the DWA algorithm introduces the concept of inaccessible regions in the velocity space, representing velocity commands that could lead to collisions. In the nav2 implementation, given the current pose of the robot and the velocity command, the planner calculates a trajectory and submits it to the base obstacle critic for evaluation. If this trajectory intersects with any obstacle in the map, it is considered invalid and excluded from the selection of the optimal trajectory. The base obstacle critic can obtain obstacle information through the cost map. Special cost values are defined in the [costmap header file](https://github.com/ros-planning/navigation2/blob/humble/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp) to indicate the nature of a cell in the map: ``` static constexpr unsigned char NO_INFORMATION = 255; static constexpr unsigned char LETHAL_OBSTACLE = 254; static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; static constexpr unsigned char MAX_NON_OBSTACLE = 252; static constexpr unsigned char FREE_SPACE = 0; ``` Recall that when the local planner computes the velocity, it locks the cost map object. Now we can see why it needs to be locked: we want to ensure that all critics use the same cost map for their evaluation. Here, we only list the key part of the `BaseObstacleCritic` implementation. For more details, please refer to the[ source code](https://github.com/ros-planning/navigation2/blob/humble/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp): ``` double BaseObstacleCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) { double score = 0.0; for (unsigned int i = 0; i < traj.poses.size(); ++i) { double pose_score = scorePose(traj.poses[i]); // Optimized/branchless version of if (sum_scores_) score += pose_score, // else score = pose_score; score = static_cast(sum_scores_) * score + pose_score; } return score; } double BaseObstacleCritic::scorePose(const geometry_msgs::msg::Pose2D & pose) { unsigned int cell_x, cell_y; if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) { throw dwb_core:: IllegalTrajectoryException(name_, "Trajectory Goes Off Grid."); } unsigned char cost = costmap_->getCost(cell_x, cell_y); if (!isValidCost(cost)) { throw dwb_core:: IllegalTrajectoryException(name_, "Trajectory Hits Obstacle."); } return cost; } ``` ## Summary This article discusses the implementation of the DWB controller in navigation 2. It begins with an overview of the DWA algorithm and introduces key concepts such as velocity space, inaccessible regions, and the dynamic window. It then focuses on the `DWBLocalPlanner`, a key component in the DWB controller, and discusses the trajectory generator and the base obstacle critic. The article also highlights how these components are related to the steps of the DWA algorithm. *If you enjoy reading this article and wish to support my work, you can make a contribution through Stripe or Buy Me a Coffee. Your support means a lot to me and helps me to continue producing content that you like and find useful.* (っ◔◡◔)っ :heart: give support :arrow\_right: [**Stripe**](https://donate.stripe.com/8wMeYddJy7d4fK0145) :arrow\_right: {% embed url="" %} # DWB Controller ## Related Readings * [nav2\_dwb\_controller code repository](https://github.com/ros-planning/navigation2/tree/humble/nav2_dwb_controller) # Page 5 # How to launch the Nav2 stack In this article, we will demonstrate how to launch the Nav2 with default setup. To launch the stack, we have the following prerequisites: * a map: This map is used to construct the static layer of the cost map. We can build and save a map with slam\_toolbox package. Note that slam\_toolbox support different map format but nav2 uses the yaml file. * a nav2 parameter file: This is the configuration file of the nav2 stack. It might be overwhelming to create a configuration file from scratch as it contains many parameters. Instead, we can use [this file](https://github.com/ros-planning/navigation2/blob/humble/nav2_bringup/params/nav2_params.yaml) in the navigation2 package as the blueprint. | Param Name | Default Value | Description | | ---------------- | --------------------------------------------------------- | --------------------------------------------------------------------------- | | namespace | (empty) | Top-level namespace | | use\_sim\_time | false | Use simulation (Gazebo) clock if true | | params\_file | os.path.join(bringup\_dir, 'params', 'nav2\_params.yaml') | Full path to the ROS2 parameters file to use for all launched nodes | | autostart | true | Automatically startup the nav2 stack | | use\_composition | False | Use composed bringup if True | | container\_name | nav2\_container | The name of container that nodes will load in if use composition | | use\_respawn | False | Whether to respawn if a node crashes. Applied when composition is disabled. | | log\_level | info | Log level | Let's edit the nav2 stack configuration (knowns as params file). Each section in the file corresponds to a component in the nav2 stack. The details of those parameters can be found in the [Nav2 Configuration Guide](https://navigation.ros.org/configuration/index.html). We will put some emphasis on the namespae. The end goal is to have * topic with proper namespace * frame with proper namespace * node with proper namespace * service with proper namespace # ROS2 Control ## Concept Clarification
# Online Resources * [Github Repo: ros2\_control](https://github.com/ros-controls/ros2_control/tree/humble) * [Github Repo: ros2\_controllers](https://github.com/ros-controls/ros2_controllers/tree/humble) * [Document - Demos of Ros2 Control](https://control.ros.org/humble/doc/ros2_control_demos/doc/index.html) * [Github Repo: Demos of Ros2 Control](https://github.com/ros-controls/ros2_control_demos/tree/humble) * [Simple tutorial of gz\_ros\_control](https://github.com/ros-controls/gz_ros2_control/blob/humble/doc/index.rst) * # Overview of Codebase ## Introduction In this article, we will present a high-level overview of the ros2 control codebase, which involves two repositories: ros2\_control and ros2\_controllers. Collectively, we refer to these as the ros2 control library for the ease of discussion. The ros2 control library consists of following major components: * **Control Node Executable**: An executable responsible for launching the Controller Manager node * **Controller Manager**: This is the central entity that manages the life cycle of controllers. It loads, unloads, starts, and stops controllers, and also handles their configuration. * **Resource Manager**: This component manages resources like sensors and actuators, ensuring that they are properly allocated to controllers as needed. * **Hardware Interface**: This component provides an abstraction over different hardware devices. It allows controllers to communicate with hardware without needing to know the specifics of the hardware. * **Controllers**: Controllers are responsible for implementing specific control algorithms. Examples include joint trajectory controllers for arm movements, velocity controllers, position controllers, etc. They process input data and send command values to actuators. In this article, we will guide you through each component and present the interaction betwteen them. ## Control Node The first component is control node. Strictly speaking, this is just an executable called ros2\_control\_node defined in [ros2\_control/controller\_manager](https://github.com/ros-controls/ros2_control/blob/506887fb9018a0286c4dea828d33592bddb12f6e/controller_manager/CMakeLists.txt#L36). The source file of this executable is [ros2\_control\_node.cpp](https://github.com/ros-controls/ros2_control/blob/humble/controller_manager/src/ros2_control_node.cpp). The main responsibility of this executable is to launch a `ControllerManager` and start the main update loop. The snippets below shows the critical section of the script: ``` auto cm = std::make_shared(executor, manager_node_name); auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate()); // ... while (rclcpp::ok()) { // calculate measured period auto const current_time = cm->now(); auto const measured_period = current_time - previous_time; previous_time = current_time; // execute update loop cm->read(cm->now(), measured_period); cm->update(cm->now(), measured_period); cm->write(cm->now(), measured_period); // wait until we hit the end of the period next_iteration_time += period; std::this_thread::sleep_until(next_iteration_time); } ``` This loop continuously reads states from hardware, updates commands, and then writes these commands to the hardware. The loop's frequency is set by the update rate parameter. ## Controller Manager From previous section, we established that the `ControllerManager` is a critical piece in the ros2 control library. Given the extensive size of the source code, it's impractical to cover every detail here. Instead, this section will focus on the code snippets of the `read`, `update`, and `write` functions. ``` void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration & period) { resource_manager_->write(time, period); } void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & period) { resource_manager_->read(time, period); } controller_interface::return_type ControllerManager::update( const rclcpp::Time & time, const rclcpp::Duration & period) { // ... for (auto loaded_controller : rt_controller_list) { if (is_controller_active(*loaded_controller.c)) { const auto controller_update_rate = loaded_controller.c->get_update_rate(); const auto controller_update_factor = (controller_update_rate == 0) || (controller_update_rate >= update_rate_) ? 1u : update_rate_ / controller_update_rate; bool controller_go = ((update_loop_counter_ % controller_update_factor) == 0); if (controller_go) { auto controller_ret = loaded_controller.c->update( time, (controller_update_factor != 1u) ? rclcpp::Duration::from_seconds(1.0 / controller_update_rate) : period); if (controller_ret != controller_interface::return_type::OK) { ret = controller_ret; } } } } // ... return ret; } ``` We make the following observations based on the listed code above: * The `read` and `write` operations are delegated to the `ResourceManager` * `ControllerManager` has access to a list of controllers. We will see in the later section that the ResourceManager is responsible for dealing with the hardware whereas the controllers are used to apply control theory algirhtms. The following parameters are invovled in `ResourceManager`: * activate\_components\_on\_start: Determines whether components are activated at startup. * configure\_components\_on\_start: Specifies if components should be configured when starting. * robot\_description: Contains the description of the robot configuration. * update\_rate: Sets the frequency at which updates are made. * use\_sim\_time: Indicates whether to use simulated time. ## Resource Manager The first thing we want to highlight is that `ResourceManager` is defined in [ros2\_control/hardware\_interface](https://github.com/ros-controls/ros2_control/blob/humble/hardware_interface/include/hardware_interface/resource_manager.hpp), which means it's closed related to the actual hardware. Secondly, `ResourceManager` class consumes the URDF information as indicated by the constructor taking an `urdf` argument and the `load_urdf` function. Third, it has a field of type `ResoureStorage`. The `read` and `write` functions delegate the operations to the components in `ResourceStorage` as indicated by the code below: ``` // CM API: Called in "update"-thread HardwareReadWriteStatus ResourceManager::read( const rclcpp::Time & time, const rclcpp::Duration & period) { std::lock_guard guard(resources_lock_); read_write_status.ok = true; read_write_status.failed_hardware_names.clear(); auto read_components = [&](auto & components) { for (auto & component : components) { if (component.read(time, period) != return_type::OK) { read_write_status.ok = false; read_write_status.failed_hardware_names.push_back(component.get_name()); resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name()); } } }; read_components(resource_storage_->actuators_); read_components(resource_storage_->sensors_); read_components(resource_storage_->systems_); return read_write_status; } // CM API: Called in "update"-thread HardwareReadWriteStatus ResourceManager::write( const rclcpp::Time & time, const rclcpp::Duration & period) { std::lock_guard guard(resources_lock_); read_write_status.ok = true; read_write_status.failed_hardware_names.clear(); auto write_components = [&](auto & components) { for (auto & component : components) { if (component.write(time, period) != return_type::OK) { read_write_status.ok = false; read_write_status.failed_hardware_names.push_back(component.get_name()); resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name()); } } }; write_components(resource_storage_->actuators_); write_components(resource_storage_->systems_); return read_write_status; } ``` `sensor`, `actuator`, and `system` are hardware type, which is specified by the `type` attribute in the `ros2_control` element. As we can see, the `sensor` type hardware is read-only whereas `actuator` and `system` type hardware can do both read and write. We also want to highlight the classic technique dependency inversion employed in the implementation. The `ResourceStorage` class depends on the `System` class, which in turn depends on the `SystemInterface` as it's used as one of the arguments in the constructor. The user-provided plugin class needs to implements the `SystemInterface` so that it will be picked up by ros2 control automatically. It's interesting to notice the `SystemInterface` itself extends the lifecycle node interface. This suggests the hardware control code is in the form of a lifecycle node.
## Controller Now, let's focus on the update operations performed by the `ControllerManager`. We've see earlier that these operations are delegated to the `Controller` class. One of the key line in the `update` method of `ControllerManager` class is the one below: ``` loaded_controller.c->update(...) ``` Where the variable `loaded_controller` is of type `ControllerSpec`, which has the following definition: ``` struct ControllerSpec { hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; }; ``` This means the `update` action is provided in the `ControllerInterfaceBase` ([see code)](https://github.com/ros-controls/ros2_control/blob/506887fb9018a0286c4dea828d33592bddb12f6e/controller_interface/include/controller_interface/controller_interface_base.hpp#L144). This setup is similar to the system/actuator/sensor interface. The `ControllerSpec` depends on the `ControllerInterface` and the user can provide their own controller, which will implement the `ControllerInterface`. The ros2\_controllers packages provides implementations of many well-known controllers. For more details, please refer to the [Github repo of the ros2\_controllers](https://github.com/ros-controls/ros2_controllers/tree/humble) package. Let's take a closer look at how the controller work. We will study the `ForwardCommandController`, presumably the most simple one. According to the [comment](https://github.com/ros-controls/ros2_controllers/blob/e08dcb91123a511fbf8233161d4f8d5ffad854be/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp#L31), this controller just forwards the command to the interface. Our previous focus was on the `update` function in different classes, so let's take a look at the implementation of the `update` function in `ForwardCommandController` class. The complete code can be found at [this link](https://github.com/ros-controls/ros2_controllers/blob/e08dcb91123a511fbf8233161d4f8d5ffad854be/forward_command_controller/src/forward_controllers_base.cpp#L120) and here we only list the essential part of this function: ``` controller_interface::return_type ForwardControllersBase::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { auto joint_commands = rt_command_ptr_.readFromRT(); // ... for (auto index = 0ul; index < command_interfaces_.size(); ++index) { command_interfaces_[index].set_value((*joint_commands)->data[index]); } return controller_interface::return_type::OK; } ``` As we can see, the `update` function simply read the command value from `rt_command_ptr` and then set the value in the `command_interfaces_`. We are already familiar with what command interfaces do and the remaining questions are (1) what is `rt_command_ptr` variable and (2) where it gets from the data. The `rt_comamnd_ptr` can be considered a buffer and the value is provided by the subscription to the command topic: ``` controller_interface::CallbackReturn ForwardControllersBase::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { auto ret = this->read_parameters(); if (ret != controller_interface::CallbackReturn::SUCCESS) { return ret; } joints_command_subscriber_ = get_node()->create_subscription( "~/commands", rclcpp::SystemDefaultsQoS(), [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; } ``` ## Summary In this article, we examined many key components of the ros2 control library. The term interface seems overloaded and its meaning varies in different contexts. Therefore, when we come across the term "interface" in the code or documentation, it is important to understand what exactly it represents. It could refer to a system interface, a command interface, a state interface, or merely the general concept of an interface in software engineering. The root component of the ros2 control library is the ros2 control node executable, responsible for launching the `ControllerManager` node. The `ControllerManager` is the core of the library, it has access to the list of controllers and the resource manager. The resource manager is responsible for loading the URDF file and load and initialize the hardware information accordingly. These hardware information is stored in the resource storage, which also has access to the the user-implemented hardware plugin. The hardware plugin behaves like a driver from ROS2's perspective. It can read data from the hardware or send command to it. A hardware can have many components and each of them is represented by a `HardwareComponentInfo` class, which contains the command data and the hardware state. Command interface and state interface in the `ComponentInfo` class are accessible by controllers. Controllers apply control theory algorithm to determine the optimal commands that needed to be sent to the hardware component. They are the key components for the `update` step in the update loop. Their implementation involves subscribing to the command topic and updating the state (i.e. command interface and state interface) of the component. The diagram below illustrates the overall architecture:
# Cookbook ## How to set up timer for periodic tasks? `rclcpp:rclcpp::Node` class has a method called `create_wall_timer`, which can be used to create period signal. The following two c++ functions are usually used with the `create_wall_timer` method. * [std::bind](https://en.cppreference.com/w/cpp/utility/functional/bind) * [std::placeholders::\_1](https://en.cppreference.com/w/cpp/utility/functional/placeholders) ## How to get the current time? If we are inside a node, we can use the following code to get the current time. ``` this->get_clock()->now() ``` # Useful Commands ## Create Workspace A ROS2 project workspace has a standard structure. First, we create the root directory of the workspace ``` mkdir MyRosWorkspace ``` Then, we go to the workspace directory and create a build directory ``` cd MyRosWorkspace mkdir build ``` ## Create C++ Package Go to the workspace root directory and use the `pkg create` command. For instance ``` cd MyRosWorkspace ros2 pkg create MyFirstProject --build-type ament_cmake ``` ## Create Python Package Go to the workspace root directory and use the `pkg create` command. For instance ``` cd MyRosWorkspace ros2 pkg create MyPythonProject --build-type ament_python ``` ## Build Workspace To build the workspace, we use the following command: ``` rosdep install -i --from-path src --rosdistro humble -y colcon build --symlink-install ``` The first command checks missing dependencies and the second command build the whole workspace. To build a specific package, use the following command: ``` colcon build --packages-select ``` ## How to specify the namespace and node name in the command line? In ROS2, the namespace and node name can be specified in the command line through the remap. For more information, please refer to the design doc [Remapping Names](https://design.ros2.org/articles/static_remapping.html). The command below shows the syntax: ``` ros2 run --ros-arg -r __ns:=/MyNamespace -r __name:=MyNodeName ``` ## How to check URDF file To validate a URDF, you can use the following command: ``` check_urdf ``` ## How to turn on the debug info when launch the stack? Use the `-d` option. For example: ``` ros2 launch -d ``` # How to specify parameters ## Related Readings * [Passing ROS arguments to nodes via the command-line](https://docs.ros.org/en/humble/How-To-Guides/Node-arguments.html) Specify the \- ``` ros2 run turtlesim turtlesim_node --ros-args --params-file ./turtlesim.yaml ``` \- \- \- # How to build the workspace To build the code in a workspace, we can follow the following steps: * Go to the root directory of the workspace * Install missing dependencies: \ `rosdep install -i --from-path src --rosdistro humble -y` * Build the code * If you want to build all packages in the workspace:\ colcon build --symlink-install * If you want to build a specific package:\ colcon build --packages-select * Open a new terminal, go to the root directory of the workspace, and execute the following command:\ `. install/setup.bash` (or `soruce install/setup.sh`)
# How to publish message to a topic from command line? To publish a message to a topic, we can use the following command: ``` ros2 topic pub --once "" ``` To publish message to a topic at a fixed rate, use the following command: ``` ros2 topic pub -r "" ``` # How to inspect service and make a service call In this article, we will walk through the process of service inspection and present an example of making service call. The first step is to list available services on the network ``` ros2 service list ros2 service list -t ``` The second command list both service name and the request type. Suppose we have a service called `/add_entity`. The next step is to find out the reques type of this service. This can be done using the following command: ``` ros2 service type /add_entity ``` The output of this command is the service request type. Suppose the output is `entity_management/srv/AddEntity`. The next step is to find out the details of this type. This can be done using the following command: ``` ros2 interface show entity_management/srv/AddEntity ``` The `interface show` command produces the details of the type. For example, ``` AddEntity string name string model_filepath geometry_msgs/Point location float64 x float64 y float64 z float64 theta --- bool result ``` To summarize, we have collected the following information: | Item | Description | | --------------------------- | -------------------------------------- | | service name | /add\_entity | | service request type | entity\_management/srv/AddEntity | | service response type | bool | | fields in the request type | name, model\_filepath, location, theta | The remaining question is how can we make a service call? The command to use is `ros2 service call `. Here is an example: {% code overflow="wrap" %} ``` ros2 service call /add_entity entity_management/srv/AddEntity "{name: 'name with space', model_filepath: 'test_path', location: {x: 1, y: 2, z: 3}, theta: 1.3}" ``` {% endcode %} {% hint style="warning" %} The space in the command is significant, especially the one following the colon (:) that comes after the field name. {% endhint %} ©2023 - 2024 all rights reserved # How to properly terminate ROS and Gazebo ROS and Gazebo have many components running in the background. To properly terminate the program, one can use the `ps` command to collect all the related process IDs and then kill them manually. Here is a python utility code that collects the process IDs in the `ps` command outputs and execute the `kill -9` command. ```python import sys import subprocess if __name__ == '__main__': result = [] for _line in sys.stdin: line = _line.strip() if line: k = line.find(' ') while k < len(line) and line[k] == ' ': k += 1 end_index = line.find(' ', k) result.append(line[k:end_index]) command = ["kill", "-9"] command.extend(result[:-1]) subprocess.run(command) ``` We can create an alias for this utility script, say, `alias kill-all="python ".` To usage the script, we can do ```bash ps aux | grep -i ros | kill-all ps aux | grep -i gazebo | kill-all ``` # How to add and remove models in Gazebo simulation dynamically In this article, we will present how we can add and remove models in Gazebo simulation dynamically. By dynamically, we meana the ability to add and remove models after the Gazebo process is launched. Suppose we define a world called `demo` in the `sdf` file and launch a Gazebo instance with it. When the Gazebo is up and running, we should see a few services are created automatically. For instance, if we run the command: ``` ign service -l | grep "world" ``` It produces the following output: ``` /gazebo/worlds /world/demo/control /world/demo/control/state /world/demo/create /world/demo/create_multiple /world/demo/declare_parameter /world/demo/disable_collision /world/demo/enable_collision /world/demo/entity/system/add /world/demo/generate_world_sdf /world/demo/get_parameter /world/demo/gui/info /world/demo/level/set_performer /world/demo/light_config /world/demo/list_parameters /world/demo/playback/control /world/demo/remove /world/demo/scene/graph /world/demo/scene/info /world/demo/set_parameter /world/demo/set_physics /world/demo/set_pose /world/demo/set_pose_vector /world/demo/set_spherical_coordinates /world/demo/state /world/demo/state_async /world/demo/system/info /world/demo/visual_config /world/demo/wheel_slip ``` Our focus in this article is on `/world/demo/create` and `/world/demo/remove`. The command we will use is: {% code overflow="wrap" %} ``` ign service -s --reqtype --reptyp --timeout --req ``` {% endcode %} {% hint style="warning" %} It's recommended to use one-line command. You may run into issues if you break the command into multiple lines with "\\" {% endhint %} To find out the request type and response type of the service, we can use command `ign service -is`. For example: ``` ign service -is /world/demo/create ``` It produces: ``` Service providers [Address, Request Message Type, Response Message Type]: tcp://10.0.0.22:32865, ignition.msgs.EntityFactory, ignition.msgs.Boolean ``` We can check the detials of the message with command `ign msg -i`. For example: ``` ign msg -i ignition.msgs.EntityFactory ``` and it produces: ``` Name: ignition.msgs.EntityFactory File: ignition/msgs/entity_factory.proto message EntityFactory { .ignition.msgs.Header header = 1; oneof from { string sdf = 2; string sdf_filename = 3; .ignition.msgs.Model model = 4; .ignition.msgs.Light light = 5; string clone_name = 6; } .ignition.msgs.Pose pose = 7; string name = 8; bool allow_renaming = 9; string relative_to = 10; .ignition.msgs.SphericalCoordinates spherical_coordinates = 11; } ``` As expected, we need to provide the following information to spawn a new model instance: * name: Model nam. This is also the "identifier" of the model instance and needs to be unique. * model source: In this article, we use sdf\_filename option and will provide a path of the model `sdf` file in the request. * pose: The pose of the model instance. Examine the `.ignition.msgs.Pose` message type and it shows we can specify both position and the orientation of the model instance. ``` Name: ignition.msgs.Pose File: ignition/msgs/pose.proto message Pose { .ignition.msgs.Header header = 1; string name = 2; uint32 id = 3; .ignition.msgs.Vector3d position = 4; .ignition.msgs.Quaternion orientation = 5; } ``` The command below creates a new model instance in Gazebo with the following attributes: * The model name is test * The model definition `sdf` file is `diff_drive/model.sdf` * The initial pose is at (10, 10, 0) with orientation (0, 0, 0.7071, 0.7071) (i.e. theta=90 degree) {% code overflow="wrap" %} ``` ign service -s /world/demo/create --reqtype ignition.msgs.EntityFactory --reptype ignition.msgs.Boolean --timeout 5000 --req 'name: "test"; sdf_filename: "diff_drive/model.sdf"; pose: {position: {x: 10, y: 10, z: 0}, orientation: {x: 0, y: 0, z: 0.7071, w: 0.7071}}' ``` {% endcode %} To remove the model instance, we can send a request to `/world/demo/remove` service. For instance: {% code overflow="wrap" %} ``` ign service -s /world/demo/remove --reqtype ignition.msgs.Entity --reptype ignition.msgs.Boolean --timeout 5000 --req 'type: MODEL; name: "test"' ``` {% endcode %} # How to spin nodes There two ways to spin nodes: * use `rclcpp::spin` * create an executor, add nodes to the executor, and then call the `spin` method of the executor. # Tutorials # Services and Communication between ROS2 and Gazebo {% hint style="warning" %} This tutorial is based on ROS2 Humble and Gazebo Fortress on Ubuntu and the code is implemented in C++. {% endhint %} ## Introduction In this tutorial, we will guild you through the processe of seeting up a service node in ROS2. But we're adding a twist! Our service will bridge ROS2 and Gazebo. Specifically, you'll learn to implement an entity management service, which aloows you to dynamcially add entities to the simulation. ## Related Readings 1. [Writing a simple service and client in C++](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Service-And-Client.html) 2. [Creating custom msg and srv files](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Custom-ROS2-Interfaces.html) 3. [Implementing custom interface](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Single-Package-Define-And-Use-Interface.html) 4. [ros\_gz\_bridge Github repository](https://github.com/gazebosim/ros_gz/blob/humble/ros_gz_bridge/README.md) The first document offers guidelines for developing a simple service in C++. In the second document, the focus shifts to the creation of custom msg and srv files, which are designed for usage by external packages. The third document details the setup process for a project to enable the same-package usage of the custom msg and srv files. Lastly, the fourth link features code that facilitates the conversion of built-in ROS types to Gazebo formats and vice versa. ## What does this tutorial cover? * Implementation of a simple service in C++ * Send a request to Gazebo from a ROS node in the code * An example of message type conversion * An example of python launch file which lanches both the ROS2 service node and Gazebo * An example of Gazebo world and model file ## What is outside the scope of this tutorial? * The tutorial does not provide a Python implementation of the service * Service client implementation is not provided * Detailed explanation of the launch file is not provided * Detailed explanation of SDF file is not provided ## Objective and results The goal is to develop a ROS2 service that facilitates the process of introducing a new entity into Gazebo. Users can define specific details such as the entity's name, model, and initial position in their request and send it to the ROS2 service. this request is then forwarded to Gazebo. The figure below illustrates the end outcome.
## Step 1: Create custom service file The first step is to create a custom service file. The service request type is a direct translation of the use case. ``` string name string model_filepath geometry_msgs/Point location float64 theta --- bool result ``` This file can be found in `ros2-service-demo/src/entity_management/src/entity_management_service.cpp` file. Additional dependencies are needed to generate the implementation of the service request data type. Add the following code to the `ros2-service-demo/src/entity_management/CMakeLists.txt` file: ``` rosidl_generate_interfaces(${PROJECT_NAME} "srv/AddEntity.srv" DEPENDENCIES geometry_msgs ) ament_export_dependencies(rosidl_default_runtime) rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} rosidl_typesupport_cpp) ``` The configuration above enables the build-system to create code for the service request data type, which can be used outside the `entity_management` package. However, to make this code accessible within the `entity_management` package itself, we need to link additional libraries to the target: ``` target_link_libraries(entity_management_service PUBLIC "${cpp_typesupport_target}") ``` We also need to add the following code to the `ros2-service-demo/src/entity_management/package.xml` file: ``` geometry_msgs rosidl_default_generators rosidl_default_runtime rosidl_interface_packages ``` Build the package and the service request data type should become available. ## Step 2: Implement a service The service implementation has a simple structure as presented below. One thing worth noting is the name of the header file. The `srv` file we created is called `AddEntity.srv` and the header file becomes `add_entity.hpp`. However, when we access the `Request` and `Response` type, the namespace uses `srv::AddEntity`. This naming convention may cause confusion. The structure of the service implementation, as shown below, is straightforward. An important detail to note is the naming convention of the header file. For instance, our srv file is named `AddEntity.srv`, but the corresponding header file is named `add_entity.hpp`. When referring to the `Request` and `Response` types, the namespace used is `srv::AddEntity`. This difference in naming conventions between the file names and the namespace usage is a subtle point that might lead to some confusion. It's essential to be aware of these distinctions to ensure proper referencing and usage in the service implementation. ```cpp #include "entity_management/srv/add_entity.hpp" namespace entity_management_service { std::string SERVICE_NODE_NAME = "add_entity_service_node"; std::string SERVICE_NAME = "add_entity"; class EntityManagementService : public rclcpp::Node { private: void add_entity(const std::shared_ptr request, std::shared_ptr response) { } public: EntityManagementService(std::string name): rclcpp::Node(name) { this->service = this->create_service( SERVICE_NAME, std::bind(&EntityManagementService::add_entity, this, std::placeholders::_1, std::placeholders::_2)); } }; } int main(int argc, char **argv) { rclcpp::init(argc, argv); auto node = std::make_shared( entity_management_service::SERVICE_NODE_NAME); rclcpp::spin(node); rclcpp::shutdown(); } ``` The next step is to implement the `add_entity` function, which executes the following two tasks: * Convert `entity_management::srv::AddEntity::Request` into a Gazbo type * Send the request to the Gazebo service. The Gazebo service responsible for adding new models is identified as `/world/demo/create`. The naming of this service is directly dependent on the world name specified in the world SDF file. To create a new entity, the Gazebo service uses the type `gz::msgs::EntityFactory`. Consequently, the `add_entity` function is structured as follows, with the section pertaining to the conversion process omitted for brevity: ``` void add_entity(const std::shared_ptr request, std::shared_ptr response) { gz::msgs::EntityFactory gz_req; // Convert request to gz_req // ... ignition::msgs::Boolean reply; bool call_succeeded; this->gz_node->Request("/world/demo/create", gz_req, 5000, reply, call_succeeded); response->result = call_succeeded && reply.data(); } ``` ## Step 3: Prepare world and model files In this tutorial, we use `ros2-service-demo/src/bringup/worlds/empty_world.sdf` as the world file and `ros2-service-demo/src/bringup/models/diff_drive/model.sdf` for the model file. The name of the world file is specified in the launch file, while the name of the model file is referenced in the terminal command when calling the `add_entity` service. It's important to ensure that these files are correctly installed during the build process. If they are not properly set up, ROS2 may have difficulties in locating them. ## Step 4: Prepare the launch file We need two nodes in the launch file. One for the `add_entity` service and the othe for the Gazebo process. ``` # ----- Launch Entity Management Service Node ----- entity_management_service_node = Node( package="entity_management", executable="entity_management_service", parameters=[{"use_sim_time": False}], ) # ----- Launch Gazebo ----- # Locate Gazebo executable. It's provided in the ros_gz_sim package. ros_gz_sim_package = get_package_share_directory("ros_gz_sim") gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource(path.join(ros_gz_sim_package, 'launch', 'gz_sim.launch.py')), launch_arguments={'gz_args': PathJoinSubstitution([ bringup_package, 'worlds', "empty_world.sdf", ])}.items(), ) ``` ## Step 5: Launch the system {% hint style="warning" %} Before launch the system, make sure no other ROS2 and Gazebo processing are running on your machine. You can checkout [this article](https://www.learnros2.com/ros/cookbook/how-to-properly-terminate-ros-and-gazebo) to properly terminate the ROS2 and Gazego processes. {% endhint %} To launch the system, use the command below: ``` # Open a new terminal and go to the workspace directory ros2-service-demo # Run . install/setup.bash before launching the system. ros2 launch bringup entity_management_server.launch.py ``` You should see the ROS2 starts running and Gazebo is open. Now, you could add some robots to the simulation using the following commands: {% code overflow="wrap" %} ``` # Open a new terminal and go to the workspace directory ros2-service-demo # Run . install/setup.bash before executing the following commands. ros2 service call /add_entity entity_management/srv/AddEntity "{name: 'model-0', model_filepath: 'diff_drive/model.sdf', location: {x: 10, y: 0, z: 0}, theta: 0}" ros2 service call /add_entity entity_management/srv/AddEntity "{name: 'model-90', model_filepath: 'diff_drive/model.sdf', location: {x: 0, y: 10, z: 0}, theta: 1.57}" ros2 service call /add_entity entity_management/srv/AddEntity "{name: 'model-180', model_filepath: 'diff_drive/model.sdf', location: {x: -10, y: 0, z: 0}, theta: 3.14}" ros2 service call /add_entity entity_management/srv/AddEntity "{name: 'model-270', model_filepath: 'diff_drive/model.sdf', location: {x: 0, y: -10, z: 0}, theta: -1.57}" ``` {% endcode %} ## Conclusion In this tutorial, we've implement a simple ROS2 service in C++ that forwards user request to Gazego to add new entities to the simulation. We've highlighted the structure of the `add_entity` function, which converts the user request to a Gazebo type `gz::msgs::EntityFactory` and then send it to Gazebo. We have also demonstrated how to initiate the ROS2 service call from the terminal. ## Download Code The code is available for download at this [link](https://learnros2.gumroad.com/l/iuwbq). ©2023 - 2024 all rights reserved # Subscription and Message Filters Demo ## Introduction In this tutorial, we first present how to create a simple subscribers in C++ and then we'll explore message filters, an useful tool to combine multiple streams of messages. ## Related Readings * [ROS documentation of Message Filters](http://wiki.ros.org/message_filters) * [Github repo of message\_filter package](https://github.com/ros2/message_filters/tree/humble) * [Writing a simple publisher and subscriber in C++](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html) ## Create a simple subscriber Creating subscribers in ROS2 is a relatively simple process that requires specifying a topic and a callback function. Typically, this callback function is a member function of a node. To adapt a member function into a function reference suitable for the subscriber, we can employ techniques such as `std::bind` or a lambda function. For instance: {% code overflow="wrap" %} ``` subscription1_ = this->create_subscription( "/robot_1/published_message", 3, std::bind(&BasicSubscriber::callback_1, this, _1)); subscription2_ = this->create_subscription( "/robot_2/published_message", 3, [this](const std_msgs::msg::String & msg) {this->callback_2(msg);}); void callback_1(const std_msgs::msg::String & msg); void callback_2(const std_msgs::msg::String & msg) ``` {% endcode %} While CLion suggests using the lambda function format, the choice ultimately boils down to personal preference. It's important to note that the callback function can accept the message reference as its argument. ## Use message filter A message filter can be viewed as a function applied to one or more incoming message streams and generates an output message stream. The `message_filters` package in ROS2 provides tools such as the `Subscriber` class and policy-based `Synchronizer` class to streamline the process of combining message streams. The following code snippet illustrates the creation and application of a message filter using the approximate time synchronizer. An illustration of the synchronization algorithm can be found at this [link](https://wiki.ros.org/message_filters/ApproximateTime). {% code overflow="wrap" %} ``` using DataType = std_msgs::msg::String; using MySyncPolicy = message_filters::sync_policies::ApproximateTime; subscriber1_ = std::make_unique>(this, "/robot_1/published_message"); subscriber2_ = std::make_unique>(this, "/robot_2/published_message"); sync_ = std::make_unique>(MySyncPolicy(10), *subscriber1_, *subscriber2_); sync_->registerCallback(std::bind(&BasicSubscriber::callback_synchronizer, this, _1, _2)); void callback_synchronizer(const std_msgs::msg::String::ConstSharedPtr& msg1, const std_msgs::msg::String::ConstSharedPtr& msg2) ``` {% endcode %} It's important to note that the callback accepts the **pointer** of the message as its argument. ## Demo setup In the demo code, we will set up * **Two publisher nodes**. Each of these publishers emits a string message at a fixed rate.The first publisher does so every second, while the second publisher sends out its message every three seconds. * **A subscriber node**. The subscriber node subscribes to the topics using two different method. The first method involves the standard subscription created from the node. Custom callback functions are detailed in the demo code, showcasing synchronization techniques. The second method uses the `message_filters::Subscriber` class and pass the two message streams to an approximate time synchronizer. A callback function that outputs the synced message pairs is attached to the filter. * A launch file is included to launch the system. ## Result The code produces the following results: ``` [INFO] [1703180455.850472669] [minimal_subscriber]: Synced message: Hello, world! 0 | Hello, world! 0 [INFO] [1703180458.849553854] [minimal_subscriber]: Report count 5 [INFO] [1703180458.849662810] [minimal_subscriber]: Synced message: Hello, world! 1 | Hello, world! 1 [INFO] [1703180461.850195260] [minimal_subscriber]: Synced message: Hello, world! 2 | Hello, world! 2 [INFO] [1703180464.849931543] [minimal_subscriber]: Report count 6 [INFO] [1703180464.850334020] [minimal_subscriber]: Synced message: Hello, world! 3 | Hello, world! 3 [INFO] [1703180467.850195856] [minimal_subscriber]: Synced message: Hello, world! 4 | Hello, world! 4 [INFO] [1703180470.850082954] [minimal_subscriber]: Report count 6 [INFO] [1703180470.850464750] [minimal_subscriber]: Synced message: Hello, world! 7 | Hello, world! 5 [INFO] [1703180473.850209145] [minimal_subscriber]: Synced message: Hello, world! 10 | Hello, world! 6 [INFO] [1703180476.850447058] [minimal_subscriber]: Synced message: Hello, world! 13 | Hello, world! 7 [INFO] [1703180479.850429622] [minimal_subscriber]: Synced message: Hello, world! 16 | Hello, world! 8 [INFO] [1703180482.850377697] [minimal_subscriber]: Report count 12 [INFO] [1703180482.850753011] [minimal_subscriber]: Synced message: Hello, world! 19 | Hello, world! 9 [INFO] [1703180485.850230340] [minimal_subscriber]: Synced message: Hello, world! 22 | Hello, world! 10 [INFO] [1703180488.850340054] [minimal_subscriber]: Report count 6 [INFO] [1703180488.850797899] [minimal_subscriber]: Synced message: Hello, world! 25 | Hello, world! 11 [INFO] [1703180491.850552337] [minimal_subscriber]: Synced message: Hello, world! 28 | Hello, world! 12 [INFO] [1703180494.850307471] [minimal_subscriber]: Synced message: Hello, world! 31 | Hello, world! 13 [INFO] [1703180497.850698574] [minimal_subscriber]: Synced message: Hello, world! 34 | Hello, world! 14 [INFO] [1703180500.850442515] [minimal_subscriber]: Synced message: Hello, world! 37 | Hello, world! 15 [INFO] [1703180503.850877867] [minimal_subscriber]: Synced message: Hello, world! 40 | Hello, world! 16 ``` We make two observations: * There are no gaps in the messages from the publisher 2 because the messages are published at a slower rate. * Once the system stabilizes, the gaps in the printed message from the publisher 1 consistently stay at 3. ## Download code The code is available for download at this [link](https://learnros2.gumroad.com/l/qlkzqa). ©2023 - 2024 all rights reserved # Executor and Spin Explained {% hint style="warning" %} This tutorial is based on ROS2 Humble and the behavior described in this tutorial is subject to change in future releases. {% endhint %} ## Related Readings * [rclcpp::Executor class reference](https://docs.ros2.org/beta3/api/rclcpp/classrclcpp_1_1executor_1_1Executor.html) * [Functional difference between spin, spin\_once, and spin\_until\_future\_complete](https://answers.ros.org/question/296480/functional-difference-between-ros2-spin_some-spin_once-and-spin_until_future/) * [Github discussion on spin function names](https://github.com/ros2/rclcpp/issues/2311) * [Concurrency and thread safety in ROS2](https://robotics.stackexchange.com/questions/106026/ros2-multi-nodes-each-on-a-thread-in-same-process) * [ROS2 document - Callback Groups](https://docs.ros.org/en/humble/How-To-Guides/Using-callback-groups.html) * [Issue - spin\_until\_future\_complete may block forever if nothing wakes the executor after the future completes](https://github.com/ros2/rclcpp/issues/1916) ## Introduction In this tutorial, we aim to clarify the difference between spin\_once, spin\_some, spin\_until\_future\_complete, and spin. However, due to limited official documentation, the accuracy of our explanations and interpretations is not guaranteed. We've include code at the end of the tutorial for you to independently verify the behaviors. Please note, this content is specifically tailored to ROS2 Humble; other version may exhibit different behaviors. ## A Few Words on Concurrency It's highly recommended to read the discussion [ROS2: multi nodes, each on a thread in same process](https://robotics.stackexchange.com/questions/106026/ros2-multi-nodes-each-on-a-thread-in-same-process). > By **default**, ROS 2 is thread safe: > > * Each node's callbacks are called mutually exclusive (i.e. only **one** callback **per node** is called at a time, there is no concurrence within a node), > * Different nodes' callbacks can be executed concurrently: > > * If they run in different processes, or > * If they are in the same process and spun by a MultiThreadedExecutor, or > * If they are in the same process and you use multiple threads each running a SingleThreadedExecutor, > > But even in those cases, the active callbacks cannot access another nodes' data, and only one callback per node is being executed, so there are no concurrency issues. In this tutorial, we will not focus on the callback group. ## Experiment Setup The experimental system consists of * A publisher that simultaneously publishes the same messages to two different topics. * A subscriber that subscribes to these two topics. The callback function is used to simulate the task execution and the execution time can be adjusted via a parameter. * Parameters that control the start-up time of the publisher and subscriber. If the subscriber activates before the publisher, the the executor queue is empty because no messages are published yet. Conversely, if the publisher starts first, the subscriber may see a non-empty queue. * Parameters that control the message publication rate and task execution time. An execution time longer than the publication interval leads to buildup of tasks in the executor queue. Conversely, if the execution time is shorter, the queue gets drained. ## Spin Once and Spin Some `spin_once` and `spin_some` have similar behaviors. Upon invocation, the executor inspects the work queue. If the queue is empty, the function returns immediately. If tasks are present, `spin_once` executes a single task, whereas `spin_some` can handle one or more tasks. It's important to note that although new tasks may arrive during execution, the execution will not address them in the current cycle since task collection occurs only once. ### Experiment 1 In this experiment, we demonstrate the call returns immediately if there is no work in the queue. We will introduce a delay in starting the publisher. Consequently, when the subscriber is initiated, no messages have been published yet, resulting in an empty work queue. First, let's launch the system with `spin_once`: {% code overflow="wrap" %} ``` ros2 launch bringup spin_once_repeat.launch.py publisher_warmup_time:=10 publication_interval:=1 subscriber_warmup_time:=1 task_running_time:=5 ``` {% endcode %} Here is the output of the program: ``` [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [publisher-1]: process started with pid [104870] [INFO] [spin_once_repeat-2]: process started with pid [104872] [spin_once_repeat-2] [INFO] [1703721523.286740357] [subscriber-main-thread]: ----- start to create the subscriber node [spin_once_repeat-2] [INFO] [1703721524.292396398] [subscriber-main-thread]: ----- subscriber node is created [spin_once_repeat-2] [INFO] [1703721524.292595346] [subscriber-main-thread]: ----- start spin_once [spin_once_repeat-2] [INFO] [1703721524.292609020] [subscriber-main-thread]: ----- call spin_once [spin_once_repeat-2] [INFO] [1703721525.292906999] [subscriber-main-thread]: ----- call spin_once [spin_once_repeat-2] [INFO] [1703721526.293163103] [subscriber-main-thread]: ----- call spin_once [spin_once_repeat-2] [INFO] [1703721527.293488979] [subscriber-main-thread]: ----- call spin_once [spin_once_repeat-2] [INFO] [1703721528.293822772] [subscriber-main-thread]: ----- call spin_once [spin_once_repeat-2] [INFO] [1703721529.294154032] [subscriber-main-thread]: ----- call spin_once [spin_once_repeat-2] [INFO] [1703721530.294414491] [subscriber-main-thread]: ----- call spin_once [spin_once_repeat-2] [INFO] [1703721531.294735447] [subscriber-main-thread]: ----- call spin_once [spin_once_repeat-2] [INFO] [1703721532.294992828] [subscriber-main-thread]: ----- call spin_once [publisher-1] [INFO] [1703721533.290849977] [task_publisher]: creating publisher and timer [spin_once_repeat-2] [INFO] [1703721533.295210474] [subscriber-main-thread]: ----- call spin_once [publisher-1] [INFO] [1703721534.292651668] [task_publisher]: Publishing: 'task 1' [spin_once_repeat-2] [INFO] [1703721534.295465766] [subscriber-main-thread]: ----- spin_once ends [INFO] [spin_once_repeat-2]: process has finished cleanly [pid 104872] [publisher-1] [INFO] [1703721535.292633903] [task_publisher]: Publishing: 'task 2' [publisher-1] [INFO] [1703721536.292643748] [task_publisher]: Publishing: 'task 3' [publisher-1] [INFO] [1703721537.292647627] [task_publisher]: Publishing: 'task 4' [publisher-1] [INFO] [1703721538.292668753] [task_publisher]: Publishing: 'task 5' ``` The log message confirms that the subscriber is created before the first message is published. Moreover, the `spin_once` call returns immediately. Let's try `spin_some`: {% code overflow="wrap" %} ``` ros2 launch bringup spin_some_repeat.launch.py publisher_warmup_time:=10 publication_interval:=1 subscriber_warmup_time:=1 task_running_time:=5 ``` {% endcode %} Similar results are produced: ``` [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [publisher-1]: process started with pid [106002] [INFO] [spin_some_repeat-2]: process started with pid [106004] [spin_some_repeat-2] [INFO] [1703721861.285776631] [subscriber-main-thread]: ----- start to create the subscriber node [spin_some_repeat-2] [INFO] [1703721862.291873202] [subscriber-main-thread]: ----- subscriber node is created [spin_some_repeat-2] [INFO] [1703721862.292095052] [subscriber-main-thread]: ----- start spin_some [spin_some_repeat-2] [INFO] [1703721862.292110432] [subscriber-main-thread]: ----- call spin_some [spin_some_repeat-2] [INFO] [1703721863.292420633] [subscriber-main-thread]: ----- call spin_some [spin_some_repeat-2] [INFO] [1703721864.292675445] [subscriber-main-thread]: ----- call spin_some [spin_some_repeat-2] [INFO] [1703721865.292924385] [subscriber-main-thread]: ----- call spin_some [spin_some_repeat-2] [INFO] [1703721866.293178492] [subscriber-main-thread]: ----- call spin_some [spin_some_repeat-2] [INFO] [1703721867.293437536] [subscriber-main-thread]: ----- call spin_some [spin_some_repeat-2] [INFO] [1703721868.293725006] [subscriber-main-thread]: ----- call spin_some [spin_some_repeat-2] [INFO] [1703721869.293870219] [subscriber-main-thread]: ----- call spin_some [spin_some_repeat-2] [INFO] [1703721870.294132843] [subscriber-main-thread]: ----- call spin_some [publisher-1] [INFO] [1703721871.288962120] [task_publisher]: creating publisher and timer [spin_some_repeat-2] [INFO] [1703721871.294388290] [subscriber-main-thread]: ----- call spin_some [publisher-1] [INFO] [1703721872.290757235] [task_publisher]: Publishing: 'task 1' [spin_some_repeat-2] [INFO] [1703721872.294619692] [subscriber-main-thread]: ----- spin_some ends [INFO] [spin_some_repeat-2]: process has finished cleanly [pid 106004] [publisher-1] [INFO] [1703721873.290692259] [task_publisher]: Publishing: 'task 2' [publisher-1] [INFO] [1703721874.290768198] [task_publisher]: Publishing: 'task 3' ``` ### Experiment 2 In this experiment, we demonstrate the difference between `spin_once` and `spin_some`. Specifically, `spin_once` only processes a single task in the queue while `spin_some` can handle multiple tasks. To illustrate this, we start the publisher before the subscribing, ensuring that by the time the subscriber is initiated, messages have already been published to the topics. Additionally, the message publication rate is set to be slower than the rate at which tasks can be competed. Let's start with `spin_once`: {% code overflow="wrap" %} ``` ros2 launch bringup spin_once_repeat.launch.py publisher_warmup_time:=1 publication_interval:=3 subscriber_warmup_time:=10 task_running_time:=1 ``` {% endcode %} As we can see in the output below, between two calls of `spin_once`, at most one callback is executed: ``` [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [publisher-1]: process started with pid [106255] [INFO] [spin_once_repeat-2]: process started with pid [106257] [spin_once_repeat-2] [INFO] [1703722264.902515185] [subscriber-main-thread]: ----- start to create the subscriber node [publisher-1] [INFO] [1703722265.906442638] [task_publisher]: creating publisher and timer [publisher-1] [INFO] [1703722268.908613701] [task_publisher]: Publishing: 'task 1' [publisher-1] [INFO] [1703722271.908610507] [task_publisher]: Publishing: 'task 2' [spin_once_repeat-2] [INFO] [1703722274.906863820] [subscriber-main-thread]: ----- subscriber node is created [spin_once_repeat-2] [INFO] [1703722274.907071708] [subscriber-main-thread]: ----- start spin_once [spin_once_repeat-2] [INFO] [1703722274.907087350] [subscriber-main-thread]: ----- call spin_once [publisher-1] [INFO] [1703722274.908598651] [task_publisher]: Publishing: 'task 3' [spin_once_repeat-2] [INFO] [1703722275.907374781] [subscriber-main-thread]: ----- call spin_once [spin_once_repeat-2] [INFO] [1703722276.907591243] [subscriber-main-thread]: ----- call spin_once [spin_once_repeat-2] [INFO] [1703722276.907733630] [task_subscriber]: start the task task 3 [spin_once_repeat-2] [INFO] [1703722277.907985576] [task_subscriber]: task is complete task 3 [publisher-1] [INFO] [1703722277.908628763] [task_publisher]: Publishing: 'task 4' [spin_once_repeat-2] [INFO] [1703722278.908218273] [subscriber-main-thread]: ----- call spin_once [spin_once_repeat-2] [INFO] [1703722278.908302400] [task_subscriber]: start the task task 3 [spin_once_repeat-2] [INFO] [1703722279.908425731] [task_subscriber]: task is complete task 3 [publisher-1] [INFO] [1703722280.908625705] [task_publisher]: Publishing: 'task 5' [spin_once_repeat-2] [INFO] [1703722280.908680301] [subscriber-main-thread]: ----- call spin_once [spin_once_repeat-2] [INFO] [1703722281.908934264] [subscriber-main-thread]: ----- call spin_once [spin_once_repeat-2] [INFO] [1703722281.909057961] [task_subscriber]: start the task task 4 [spin_once_repeat-2] [INFO] [1703722282.909210972] [task_subscriber]: task is complete task 4 [publisher-1] [INFO] [1703722283.908633067] [task_publisher]: Publishing: 'task 6' ``` Now let's check the behavior of `spin_some`: {% code overflow="wrap" %} ``` ros2 launch bringup spin_some_repeat.launch.py publisher_warmup_time:=1 publication_interval:=3 subscriber_warmup_time:=10 task_running_time:=1 ``` {% endcode %} Here is the output: ``` [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [publisher-1]: process started with pid [106414] [INFO] [spin_some_repeat-2]: process started with pid [106416] [spin_some_repeat-2] [INFO] [1703722411.352279678] [subscriber-main-thread]: ----- start to create the subscriber node [publisher-1] [INFO] [1703722412.355485901] [task_publisher]: creating publisher and timer [publisher-1] [INFO] [1703722415.357177825] [task_publisher]: Publishing: 'task 1' [publisher-1] [INFO] [1703722418.357209205] [task_publisher]: Publishing: 'task 2' [spin_some_repeat-2] [INFO] [1703722421.357183365] [subscriber-main-thread]: ----- subscriber node is created [publisher-1] [INFO] [1703722421.357189710] [task_publisher]: Publishing: 'task 3' [spin_some_repeat-2] [INFO] [1703722421.357378437] [subscriber-main-thread]: ----- start spin_some [spin_some_repeat-2] [INFO] [1703722421.357391962] [subscriber-main-thread]: ----- call spin_some [spin_some_repeat-2] [INFO] [1703722422.357763617] [subscriber-main-thread]: ----- call spin_some [spin_some_repeat-2] [INFO] [1703722422.357974239] [task_subscriber]: start the task task 3 [spin_some_repeat-2] [INFO] [1703722423.358207789] [task_subscriber]: task is complete task 3 [spin_some_repeat-2] [INFO] [1703722423.358434326] [task_subscriber]: start the task task 3 [publisher-1] [INFO] [1703722424.357224667] [task_publisher]: Publishing: 'task 4' [spin_some_repeat-2] [INFO] [1703722424.358609446] [task_subscriber]: task is complete task 3 [spin_some_repeat-2] [INFO] [1703722425.358905006] [subscriber-main-thread]: ----- call spin_some [spin_some_repeat-2] [INFO] [1703722425.359112045] [task_subscriber]: start the task task 4 [spin_some_repeat-2] [INFO] [1703722426.359259373] [task_subscriber]: task is complete task 4 [spin_some_repeat-2] [INFO] [1703722426.359453598] [task_subscriber]: start the task task 4 ``` Let's examine the log more closely. The snippet below shows that between two `spin_some` calls, two task 3 are executed. This is expected since the same message is published to two topics. This shows `spin_some` can execute multiple tasks. Interestingly, despite using a multi-thread executor, the two tasks are executed sequentially. This behavior suggests that the system operates as though only a single thread is allocated to the node. We also notice that the interval between the two `spin_some` call is approximately 2 seconds, aligning with the combined execution time of two tasks, where each takes about 1 second. This observation highlights the "blocking" nature of the `spin_some` method. {% code overflow="wrap" %} ``` [spin_some_repeat-2] [INFO] [1703722422.357763617] [subscriber-main-thread]: ----- call spin_some [spin_some_repeat-2] [INFO] [1703722422.357974239] [task_subscriber]: start the task task 3 [spin_some_repeat-2] [INFO] [1703722423.358207789] [task_subscriber]: task is complete task 3 [spin_some_repeat-2] [INFO] [1703722423.358434326] [task_subscriber]: start the task task 3 [publisher-1] [INFO] [1703722424.357224667] [task_publisher]: Publishing: 'task 4' [spin_some_repeat-2] [INFO] [1703722424.358609446] [task_subscriber]: task is complete task 3 [spin_some_repeat-2] [INFO] [1703722425.358905006] [subscriber-main-thread]: ----- call spin_some ``` {% endcode %} ### Experiment 3 This experiment, focused on `spin_some`, demonstrates that the executor collects work only once per `spin_some` call. We configure tasks to run for 3 seconds and publish messages every 1 second. Given that work arrives faster than it can be processed, we will see accumulation of tasks in the queue. Launch the system using the following command: {% code overflow="wrap" %} ``` ros2 launch bringup spin_some_repeat.launch.py publisher_warmup_time:=1 publication_interval:=1 subscriber_warmup_time:=10 task_running_time:=3 ``` {% endcode %} It produces the following outputs: (the key is that there are multiple `call spin_some` messages) ``` [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [publisher-1]: process started with pid [107142] [INFO] [spin_some_repeat-2]: process started with pid [107144] [spin_some_repeat-2] [INFO] [1703723176.275294226] [subscriber-main-thread]: ----- start to create the subscriber node [publisher-1] [INFO] [1703723177.279028661] [task_publisher]: creating publisher and timer [publisher-1] [INFO] [1703723178.280777602] [task_publisher]: Publishing: 'task 1' [publisher-1] [INFO] [1703723179.280792555] [task_publisher]: Publishing: 'task 2' [publisher-1] [INFO] [1703723180.280780229] [task_publisher]: Publishing: 'task 3' [publisher-1] [INFO] [1703723181.280840609] [task_publisher]: Publishing: 'task 4' [publisher-1] [INFO] [1703723182.280792853] [task_publisher]: Publishing: 'task 5' [publisher-1] [INFO] [1703723183.280864991] [task_publisher]: Publishing: 'task 6' [publisher-1] [INFO] [1703723184.280812151] [task_publisher]: Publishing: 'task 7' [publisher-1] [INFO] [1703723185.280807081] [task_publisher]: Publishing: 'task 8' [spin_some_repeat-2] [INFO] [1703723186.280558357] [subscriber-main-thread]: ----- subscriber node is created [spin_some_repeat-2] [INFO] [1703723186.280754425] [subscriber-main-thread]: ----- start spin_some [spin_some_repeat-2] [INFO] [1703723186.280769249] [subscriber-main-thread]: ----- call spin_some [publisher-1] [INFO] [1703723186.280767472] [task_publisher]: Publishing: 'task 9' [publisher-1] [INFO] [1703723187.280827878] [task_publisher]: Publishing: 'task 10' [spin_some_repeat-2] [INFO] [1703723187.281134662] [subscriber-main-thread]: ----- call spin_some [spin_some_repeat-2] [INFO] [1703723187.281333990] [task_subscriber]: start the task task 9 [publisher-1] [INFO] [1703723188.280855246] [task_publisher]: Publishing: 'task 11' [publisher-1] [INFO] [1703723189.280850277] [task_publisher]: Publishing: 'task 12' [publisher-1] [INFO] [1703723190.280844620] [task_publisher]: Publishing: 'task 13' [spin_some_repeat-2] [INFO] [1703723190.281558348] [task_subscriber]: task is complete task 9 [spin_some_repeat-2] [INFO] [1703723190.281732216] [task_subscriber]: start the task task 11 [publisher-1] [INFO] [1703723191.280854109] [task_publisher]: Publishing: 'task 14' [publisher-1] [INFO] [1703723192.280859971] [task_publisher]: Publishing: 'task 15' [publisher-1] [INFO] [1703723193.280857388] [task_publisher]: Publishing: 'task 16' [spin_some_repeat-2] [INFO] [1703723193.281838492] [task_subscriber]: task is complete task 11 [publisher-1] [INFO] [1703723194.280880210] [task_publisher]: Publishing: 'task 17' [spin_some_repeat-2] [INFO] [1703723194.282059193] [subscriber-main-thread]: ----- call spin_some [spin_some_repeat-2] [INFO] [1703723194.282234788] [task_subscriber]: start the task task 15 ``` ## Spin Until Future is Complete In the previous section, we observed that both `spin_once` and `spin_some` methods return either when the queue is empty or after the collected work is done. In particular, experiment 3 shows that `spin_some` returns even when there is more work in the queue. You may wonder how can we keep the executor continuously working on new tasks in the queue? One approach involves manually wrapping the `spin_once` and `spin_some` methods within a loop. Alternatively, you can use the built-in methods `spin_until_future_complete` or `spin`. The difference between `spin_until_future_complete` and `spin` methods is that the "focus time" is bounded by the future argument in `spin_until_future_complete` whereas the "focus time" is unbounded in `spin`. ### Experiment 4 In this experiment, we run a long-running task in a future and pass it to the `spin_until_future_complete` method. Due to [a pending issue](https://github.com/ros2/rclcpp/issues/1916), additional setup is required to wake up the executor, allowing the executor to exit from the `spin_until_future_complete` method. To launch the experiment, use the command below: {% code overflow="wrap" %} ``` ros2 launch bringup spin_until_future_complete.launch.py publisher_warmup_time:=1 publication_interval:=2 subscriber_warmup_time:=5 task_running_time:=1 ``` {% endcode %} The output of the program is presented as follows: ``` [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [publisher-1]: process started with pid [112108] [INFO] [spin_until_future_complete-2]: process started with pid [112110] [spin_until_future_complete-2] [INFO] [1703727953.315024507] [subscriber-main-thread]: ----- start to create the subscriber node [publisher-1] [INFO] [1703727954.318600781] [task_publisher]: creating publisher and timer [publisher-1] [INFO] [1703727956.320320568] [task_publisher]: Publishing: 'task 1' [spin_until_future_complete-2] [INFO] [1703727958.320232632] [subscriber-main-thread]: ----- subscriber node is created [publisher-1] [INFO] [1703727958.320275752] [task_publisher]: Publishing: 'task 2' [spin_until_future_complete-2] [INFO] [1703727958.320647246] [subscriber-main-thread]: ----- start spin_some [spin_until_future_complete-2] [INFO] [1703727958.320657151] [subscriber-main-thread]: doing some work [0] [spin_until_future_complete-2] [INFO] [1703727958.320914234] [task_subscriber]: start the task task 2 [spin_until_future_complete-2] [INFO] [1703727959.321089551] [task_subscriber]: task is complete task 2 [spin_until_future_complete-2] [INFO] [1703727959.321303642] [task_subscriber]: start the task task 2 [publisher-1] [INFO] [1703727960.320299603] [task_publisher]: Publishing: 'task 3' [spin_until_future_complete-2] [INFO] [1703727960.321402142] [task_subscriber]: task is complete task 2 [spin_until_future_complete-2] [INFO] [1703727960.321625114] [task_subscriber]: start the task task 3 [spin_until_future_complete-2] [INFO] [1703727961.321797323] [task_subscriber]: task is complete task 3 [spin_until_future_complete-2] [INFO] [1703727961.321989861] [task_subscriber]: start the task task 3 [publisher-1] [INFO] [1703727962.320294331] [task_publisher]: Publishing: 'task 4' [spin_until_future_complete-2] [INFO] [1703727962.322159138] [task_subscriber]: task is complete task 3 [spin_until_future_complete-2] [INFO] [1703727962.322412436] [task_subscriber]: start the task task 4 [spin_until_future_complete-2] [INFO] [1703727963.320893505] [subscriber-main-thread]: doing some work [1] [spin_until_future_complete-2] [INFO] [1703727963.322512534] [task_subscriber]: task is complete task 4 [spin_until_future_complete-2] [INFO] [1703727963.322717466] [task_subscriber]: start the task task 4 [publisher-1] [INFO] [1703727964.320283152] [task_publisher]: Publishing: 'task 5' [spin_until_future_complete-2] [INFO] [1703727964.322817282] [task_subscriber]: task is complete task 4 [spin_until_future_complete-2] [INFO] [1703727964.323053314] [task_subscriber]: start the task task 5 [spin_until_future_complete-2] [INFO] [1703727965.323226421] [task_subscriber]: task is complete task 5 [spin_until_future_complete-2] [INFO] [1703727965.323427751] [task_subscriber]: start the task task 5 [publisher-1] [INFO] [1703727966.320294448] [task_publisher]: Publishing: 'task 6' [spin_until_future_complete-2] [INFO] [1703727966.323529903] [task_subscriber]: task is complete task 5 [spin_until_future_complete-2] [INFO] [1703727966.323760117] [task_subscriber]: start the task task 6 [spin_until_future_complete-2] [INFO] [1703727967.323863154] [task_subscriber]: task is complete task 6 [spin_until_future_complete-2] [INFO] [1703727967.324076546] [task_subscriber]: start the task task 6 [publisher-1] [INFO] [1703727968.320294840] [task_publisher]: Publishing: 'task 7' [spin_until_future_complete-2] [INFO] [1703727968.321037042] [subscriber-main-thread]: doing some work [2] [spin_until_future_complete-2] [INFO] [1703727968.324188134] [task_subscriber]: task is complete task 6 [spin_until_future_complete-2] [INFO] [1703727968.324408263] [task_subscriber]: start the task task 7 [spin_until_future_complete-2] [INFO] [1703727969.324533568] [task_subscriber]: task is complete task 7 [spin_until_future_complete-2] [INFO] [1703727969.324728485] [task_subscriber]: start the task task 7 [publisher-1] [INFO] [1703727970.320293347] [task_publisher]: Publishing: 'task 8' [spin_until_future_complete-2] [INFO] [1703727970.324830376] [task_subscriber]: task is complete task 7 [spin_until_future_complete-2] [INFO] [1703727970.325058938] [task_subscriber]: start the task task 8 [spin_until_future_complete-2] [INFO] [1703727971.325161432] [task_subscriber]: task is complete task 8 [spin_until_future_complete-2] [INFO] [1703727971.325351216] [task_subscriber]: start the task task 8 [publisher-1] [INFO] [1703727972.320277252] [task_publisher]: Publishing: 'task 9' [spin_until_future_complete-2] [INFO] [1703727972.325452600] [task_subscriber]: task is complete task 8 [spin_until_future_complete-2] [INFO] [1703727972.325691728] [task_subscriber]: start the task task 9 [spin_until_future_complete-2] [INFO] [1703727973.321177505] [subscriber-main-thread]: ----- !!! ----- all the side word is done. [spin_until_future_complete-2] [INFO] [1703727973.325863001] [task_subscriber]: task is complete task 9 [spin_until_future_complete-2] [INFO] [1703727973.326011324] [subscriber-main-thread]: ----- spin_some ends [INFO] [spin_until_future_complete-2]: process has finished cleanly [pid 112110] [publisher-1] [INFO] [1703727974.320284937] [task_publisher]: Publishing: 'task 10' [publisher-1] [INFO] [1703727976.320347662] [task_publisher]: Publishing: 'task 11' ``` The key part in the output is highlighted in the section below: {% code overflow="wrap" %} ``` [spin_until_future_complete-2] [INFO] [1703727972.325452600] [task_subscriber]: task is complete task 8 [spin_until_future_complete-2] [INFO] [1703727972.325691728] [task_subscriber]: start the task task 9 [spin_until_future_complete-2] [INFO] [1703727973.321177505] [subscriber-main-thread]: ----- !!! ----- all the side word is done. [spin_until_future_complete-2] [INFO] [1703727973.325863001] [task_subscriber]: task is complete task 9 [spin_until_future_complete-2] [INFO] [1703727973.326011324] [subscriber-main-thread]: ----- spin_some ends ``` {% endcode %} It shows that the `spin_until_future_complete` exits when the work in the future is done. ## Spin `spin` is the most common one among these variants. According to the code comments, it does work periodically as it becomes available to the executor. It's a blocking call and may block indefinitely. Unlike other method, the `spin` method does not return when the work queue is empty. It always waits for additional tasks. ## Download Code The code is available for download at this [link](https://learnros2.gumroad.com/l/wncli). ©2023 - 2024 all rights reserved # Lifecycle Node Demo {% hint style="warning" %} This tutorial is based on ROS2 Humbler. The behavior may vary in a different distro. {% endhint %} ## Related Readings * [ROS2 Design Doc - Managed Node (Lifecycle Node)](https://design.ros2.org/articles/node_lifecycle.html) * [ROS2 Lifecycle Node Officiel Demo Repo](https://github.com/ros2/demos/tree/humble/lifecycle) * [Lifecycle Message Definition](https://github.com/ros2/rcl_interfaces/tree/humble/lifecycle_msgs) * [Lifecycle Node State Definition](https://github.com/ros2/rcl_interfaces/blob/humble/lifecycle_msgs/msg/State.msg) ## Introduction In this tutorial, we will demonstrate how to trigger a state transition in a lifecycle node. We'll start with a brief introduction to the lifecycle node's states and transitions. Next, we'll clarify a few potentially confusing concepts. Lastly, we'll guide you through a practical example and demonstrate the state transition via the command line and the code. ## State and State Transition According to [ROS2 Design Doc - Managed Node (Lifecycle Node)](https://design.ros2.org/articles/node_lifecycle.html), a lifecycle node has the following states and state transitions:
Lifecycle nodes in ROS2 have two state types: primary states and transition states. Transitioning between primary states requires invoking a specific function. For instance, to move a node from inactive to active, we use the `activate` function. This transition can be initiated either from the command line or directly in the code. During the execution of the transition function, the node enters the corresponding transition states. For example, if the `activate` function takes 10 seconds to run and we query the node's state during this window, it will return `activating`. Before moving on to our demo, it's important to clarify a few key points. ## Where are the states defined? The [state class](https://github.com/ros2/rclcpp/blob/humble/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp) is implemented in the rclcpp/rclcpp\_lifecycle package. However, if you check the code, the `State` class is simply a wrapper of the id and label pair. The class does not contain any clue about the mapping between the id and the actual state name described in the design document. The mapping is defined in the [state](https://github.com/ros2/rcl_interfaces/blob/humble/lifecycle_msgs/msg/State.msg) message of the [rcl\_interfaces/lifecycle\_msgs package](https://github.com/ros2/rcl_interfaces/tree/humble/lifecycle_msgs). Therefore, the most robust way to check the current state of the lifecycle node in the code is to include `lifecycle_msgs/msg/state.hpp` and examine the `id` field of the state object. ## How to transit out of the finalized state? {% hint style="warning" %} This section is personal interpretation. {% endhint %} The design doc [ROS2 Design Doc - Managed Node (Lifecycle Node)](https://design.ros2.org/articles/node_lifecycle.html) seems to suggest that transitioning out of the `finalized` state is possible by invoking the `destroy` method. Yet, when we set the node to the `finalized` state and attempt to invoke the `destroy` method, the following results are observed: ``` learnros2@7D41:~/ros2-lifecycle-node-demo$ ros2 lifecycle get /my_demo_node finalized [4] :learnros2@7D41~/ros2-lifecycle-node-demo$ ros2 lifecycle set /my_demo_node destroy Unknown transition requested, available ones are: ``` When the node is in the `finalized` state, it appears that there are no valid transitions available. Indeed, examining the implementation of transition methods in [lifecycle\_node.hpp](https://github.com/ros2/rclcpp/blob/humble/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp#L844-L850) within the [rclcpp\_lifecycle](https://github.com/ros2/rclcpp/tree/humble/rclcpp_lifecycle) package reveals no reference to the `destroy` method. This raises the question: what exactly is happening in this scenario? There are two plausible explanations. Firstly, like the `start` state, `destroyed` is a concept state. This means although we understand the node is destroyed, querying it for its current state is not feasible as the node no longer exists. This explains why the `destroyed` state is not a primary state. Secondly, the destruction of a node involves memory deallocation by an owner. However, not all nodes are under the supervision of a node manager or similar system, making it challenging to determine the recipient of the destroy command in advance. ## Demo Code Setup The demo code will illustrate the transition from the `unconfigured` state to the `finalized` state. We will manually trigger most transitions via the command line, except for the transition from inactive to active, which happens automatically after a pre-defined delay. Components such as timer, subscriber, and publisher are created in the `configuring` state and become functional in the `inactive` state. The node will then automatically move to the `active` state, where it can receive messages and become ready for operation. The `ready` state is a user-define concept in our demo code. As suggested in the design document, complex software-level initialization logic can be implemented as a user-defined state machine that runs when the node is in the `active` state. In our demonstration, the user-defined state machine is trivial: the node transitions from active to ready upon receiving a message from the `ready_signal` topic.
## Transit from Unconfigured to Inactive Let's first launch the system: ``` ros2 launch bringup demo.launch.py ``` It gives the following results: ``` [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [demo-1]: process started with pid [280004] ``` There aren't many outputs at this step. We can check the state of the node and it should be in the `unconfigured` state: ``` learnros2@7D41:~/ros2-lifecycle-node-demo$ ros2 lifecycle get /my_demo_node unconfigured [1] ``` To transit from the `unconfigured` state to the `inactive` state, we can invoke the `configure` method using the following command: ``` ros2 lifecycle set /my_demo_node configure ``` We will see the following outputs in the terminal where we launched the system: ``` [demo-1] [INFO] [1704042797.997082968] [my_demo_node]: on_configure() is called. Previous state is unconfigured. Internal state: is_ready=false [demo-1] [INFO] [1704042797.997865606] [my_demo_node]: on_configure is complete. Thread ID is 140308162622336 [demo-1] [INFO] [1704042797.997879296] [my_demo_node]: thread ID in lambda: 140308090152512 [demo-1] [INFO] [1704042798.997785585] [my_demo_node]: Demo node is not active yet and cannot publish messages. [demo-1] [INFO] [1704042799.997786979] [my_demo_node]: Demo node is not active yet and cannot publish messages. [demo-1] [INFO] [1704042800.997802945] [my_demo_node]: Demo node is not active yet and cannot publish messages. ``` We can see the `on_configured` method is called and the node executes the `publish` method at a fixed rate. This shows that the timer is activated when the node is in the inactive state. Check the node state again by running the command below ``` ros2 lifecycle get /my_demo_node ``` It confirms that the node is in the `inactive` state. ``` inactive [2] ``` If we publish a message to the `read_signal` topic, we can see that the node is able to subscirbe to the topic when it's in the `inactive` state. To publish a message, use the following command: {% code overflow="wrap" %} ``` ros2 topic pub --once /ready_signal std_msgs/msg/String "{data: 'Hello from terminal'}" ``` {% endcode %} The output confirms the node can receive the message: ``` [demo-1] [INFO] [1704043103.047689958] [my_demo_node]: Demo node is not active yet and cannot publish messages. [demo-1] [INFO] [1704043104.047725655] [my_demo_node]: Demo node is not active yet and cannot publish messages. [demo-1] [INFO] [1704043104.744767441] [my_demo_node]: Demo node is ready for work now. [demo-1] [INFO] [1704043105.047731930] [my_demo_node]: Demo node is not active yet and cannot publish messages. [demo-1] [INFO] [1704043106.047750263] [my_demo_node]: Demo node is not active yet and cannot publish messages. [demo-1] [INFO] [1704043107.047752290] [my_demo_node]: Demo node is not active yet and cannot publish messages. ``` ## Transit from Inactive to Active We add a small code in the `on_configure` method to make the transit from inactive to active automatic: ``` std::thread async_task( [this]() { std::ostringstream ss; ss << std::this_thread::get_id(); RCLCPP_INFO(this->get_logger(), "thread ID in lambda: %s", ss.str().c_str()); std::this_thread::sleep_for(std::chrono::seconds(20)); RCLCPP_INFO(this->get_logger(), "Activate the node by itself"); this->activate(); RCLCPP_INFO(this->get_logger(), "[lambda] Activation is completed."); } ); async_task.detach(); ``` Note a few details. We use `sleep_for` to delay the invocation of the `activate` function and this code runs in a separate thread. The `detach` is required because we want to separate the execution from the thread object; otherwise, when the execution exits the `on_configure` method, the thread object will terminate the program (see [c++ documentation](https://en.cppreference.com/w/cpp/thread/thread/%7Ethread)). Once the node reaches the `inactive` state, no further commands are required. Depending on the delay specified in the code, simply wait a few seconds and you should observe the following outputs: ``` [demo-1] [INFO] [1704044107.059493913] [my_demo_node]: Demo node is not active yet and cannot publish messages. [demo-1] [INFO] [1704044108.059540732] [my_demo_node]: Demo node is not active yet and cannot publish messages. [demo-1] [INFO] [1704044108.059703175] [my_demo_node]: Activate the node by itself [demo-1] [INFO] [1704044108.059876436] [my_demo_node]: on_activate() is called. Previous state is inactive. Internal state: is_ready=false [demo-1] [INFO] [1704044108.059975961] [my_demo_node]: [lambda] Activation is completed. [demo-1] [INFO] [1704044109.059562172] [my_demo_node]: Publish message: internal_state: is_ready=false [demo-1] [INFO] [1704044110.059572780] [my_demo_node]: Publish message: internal_state: is_ready=false [demo-1] [INFO] [1704044111.059531749] [my_demo_node]: Publish message: internal_state: is_ready=false [demo-1] [INFO] [1704044112.059586519] [my_demo_node]: Publish message: internal_state: is_ready=false ``` If you haven't published a message to the `ready_signal` topic, the ready flag of the node remains false. To set the node to ready, you can manually publish a message from the command line: {% code overflow="wrap" %} ``` ros2 topic pub --once /ready_signal std_msgs/msg/String "{data: 'Hello from terminal'}" ``` {% endcode %} And you should see the following messages: ``` [demo-1] [INFO] [1704044121.059626693] [my_demo_node]: Publish message: internal_state: is_ready=false [demo-1] [INFO] [1704044122.059679773] [my_demo_node]: Publish message: internal_state: is_ready=false [demo-1] [INFO] [1704044123.059692743] [my_demo_node]: Publish message: internal_state: is_ready=false [demo-1] [INFO] [1704044123.265610986] [my_demo_node]: Demo node is ready for work now. [demo-1] [INFO] [1704044124.059697042] [my_demo_node]: Publish message: internal_state: is_ready=true [demo-1] [INFO] [1704044125.059758017] [my_demo_node]: Publish message: internal_state: is_ready=true [demo-1] [INFO] [1704044126.059715186] [my_demo_node]: Publish message: internal_state: is_ready=true ``` ## Download the code The code is available for download at this [link](https://learnros2.gumroad.com/l/qfxlg). ©2023 - 2024 all rights reserved # Robotic Arm Demo ## Introduction In this tutorial, we will walk you through setting up a robot arm in Gazebo. First, we will download the SDF model file for the UR10 robot arm and convert it into a URDF file. Next, we will explain how to configure the ros2 control and Gazebo simulation for the robot model. Finally, we demonstrate launching the system and issuing a basic command to move the robotic arm. Please note, this tutorial focuses on practical steps and does not cover the theoretical aspects of robotic arm control. ## Related Readings * [How to control Universal Robot by using ROS2](https://www.ritsumei.ac.jp/~kawamura/doc/ros2_ur.pdf) * [GitHub repo - ros2\_control](https://github.com/ros-controls/ros2_control/tree/humble) * [GitHub repo - ros2\_controllers](https://github.com/ros-controls/ros2_controllers/tree/humble) * [Examples of ros2\_control](https://control.ros.org/humble/doc/ros2_control_demos/doc/index.html) * [Simple Tutorial of gz\_ros2\_control](https://github.com/ros-controls/gz_ros2_control/blob/humble/doc/index.rst) * [gz\_ros2\_control demo](https://github.com/ros-controls/gz_ros2_control/tree/humble/ign_ros2_control_demos) * [Ros2 Control Example: 6DoF robot](https://control.ros.org/humble/doc/ros2_control_demos/example_7/doc/userdoc.html) ## Step 1: Download the Robotic Arm Model The first step is to download a robotic arm model. Many options are available online and in this tutorial, we will use the Universal Robotics UR10 robot arm. You can download the model file at this [link](https://app.gazebosim.org/OpenRobotics/fuel/models/Universal%20Robotics%20UR10%20robot%20arm). The model files assume all relevant files are store in the `ur10` directory, therefore, it's important to ensure the content in the zip file extract to a `ur10` directory. Moreover, the parent directory of the `ur10` directory needs to be included in the Gazebo resource path. More specifically, suppose we extract the UR10 robot arm model files to `~/demo/models/` directory: ``` learnros2@7D41:~/demo/models$ tree -L 2 . └── ur10 ├── meshes ├── model.config └── ur10.sdf ``` We need to ensure that the path `~/demo/models` is included in the Gazebo resource path, which can be configured with the environment variable `IGN_GAZEBO_RESOURCE_PATH` in Gazebo Fortress. We can now launch the system and verify the setup is correct. In this tutorial, both ROS2 components and Gazebo are launched using the launch file. This ensures a consistent experience with different components and minimize the separation between ROS2 and Gazebo. Through out the tutorial, we consider Gazebo a sub-system of the ROS2 ecosystem. If you download the code attached to this tutorial, you can launch the system using the following command: ``` ros2 launch bringup ur10.launch.py ``` You can also create your own launch file. The objective is to launch the the Gazebo world with the downloaded robotic arm model. The code below demonstrates how to launch a Gazebo world with a specific world sdf file: ``` def generate_launch_description(): bringup_package = get_package_share_directory("bringup") export_models(path.join(bringup_package, "models")) ros_gz_sim_package = get_package_share_directory('ros_gz_sim') gz_node = IncludeLaunchDescription( PythonLaunchDescriptionSource(path.join(ros_gz_sim_package, 'launch', 'gz_sim.launch.py')), launch_arguments={'gz_args': PathJoinSubstitution([ bringup_package, 'worlds', "simple_world_with_ur10.sdf", ])}.items(), ) return LaunchDescription([ gz_node ]) ``` Make sure the world sdf file (in the above case, it's `simple_world_with_ur10.sdf`) contains the robot model: ``` true 0 1 0 0 0 0 model://models/ur10/ur10.modified.sdf ``` If everything works correctly, you should see Gazebo launched successfully and a robot arm lying on the ground:
## Step 2: Convert SDF to URDF If we don't care about simulation, we don't need to deal with the SDF file because URDF is the native format for describing robots in ROS2. However, if we want to integrate ROS2 with a simulator such as Gazebo, additional simulation-specific information is required and it's usually specified in the SDF file. Both URDF and SDF are used to describe robots and they have overlaps. If we use two formats to describe the same robot, we need to ensure they are in-sync. For example, whenever we modify the geometry in one file, we need to make the same update in another file too.
It's important to notice that many components in the ROS2 and Gazebo ecosystem either require URDF file or is able to consume it. Therefore, a possible solution is to put geometry-related information in an URDF file, group the control-specific and simulation-specific information in a separate URDF file, and use `xacro` to combine the two files. For example: {% code overflow="wrap" %} ``` ... ... ``` {% endcode %} In this way, the robot model definition is separated from the control and simulation configurations and we only need to maintain a single source of the model. Another benefit is that it's easy to replace the control and simulation configurations. Many tools exist to convert SDF files to URDF files. You can also check out the [blog](/ros/tech-blog/difference-between-urdf-and-sdf-and-how-to-convert) on how to perform the conversion manually. It's a good practice to always verify the generate files. Unfortunately, we cannot simply include the URDF model in the SDf world file. Instead, we can create a node that will run the `create` executable in the `ros_gz_sim` package. For example: ``` ignition_spawn_entity = Node( package='ros_gz_sim', executable='create', output='screen', arguments=['-world', 'demo', '-string', urdf_content, '-name', 'MyRobot', '-allow_renaming', 'true', "-x", "0", "-y", "2"], ) ``` One more technical detail. The robot arm will be subject to the force. If we don't fix the base to the ground, it will not behave as expected. To fix the base to the ground in Gazebo, we can add a dummy link and joint in the URDF file. For example: ``` ``` ## Step 3: Configure ros2 control and Gazebo In this tutorial, we want to demonstrate how to move a robotic arm. It has two aspects: * Use the ros2 control package to control the robot hardware * Because we don't have the real robot hardware, we will use Gazebo to simulate one. The configuration of ros2 control is standard. We specify the joint types, state interfaces, and command interfaces in the URDF file inside the `ros2_control` element. The simulation part is handled by the [gz\_ros2\_control](https://github.com/ros-controls/gz_ros2_control/tree/humble) package, which is responsible for simulating the movement of the joints. Moreover, it also provides the hardware interface used by the ros2 control. As mentioned earlier, the configurations of ros2 control and the gazebo simulation are separate from the robot model. Below is a sample configuration file: ``` ign_ros2_control/IgnitionSystem {-2*pi} {2*pi} 0.0 {-2*pi} {2*pi} 0.0 {-2*pi} {2*pi} 0.0 {-2*pi} {2*pi} 0.0 {-2*pi} {2*pi} 0.0 {-2*pi} {2*pi} 0.0 $(find bringup)/configs/ur10-controller.yaml robot_description robot_state_publisher false 1 ``` We also need to configure the controllers, which are specified in a parameter file. In our case, the parameter file is simple: we will use a position controller for all the joints. ``` controller_manager: ros__parameters: update_rate: 100 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster position_controller: type: position_controllers/JointGroupPositionController position_controller: ros__parameters: joints: - shoulder_pan - shoulder_lift - elbow - forearm_wrist_1 - wrist_1_wrist_2 - writs_2_wrist_3 ``` ## Step 4: Launch the system We need to create the following tasks in the launch file: * **Process the xacro file**. Recall that the model definition and the ros2 control/simulation configurations are stored in two separate files. We need to merge them into one single file during the launch. This can be done using the `xacro` library. * **Create a robot state publisher**. This is almost always needed in any ros2 application. * **Load the position controller and the joint state broadcaster**. These two components are part of the rso2 control package. They are required because they are specified in the parameter file. * **Launch the Gazebo node.** We can specify a world SDF file in this step. * **Spawn the model**. This is the step where we instantiate the robot arm model. When we spawn a model, we need to provide the world name, model name, and the model URDF file with the ros2 control and the simulation configuration. We can also specify the entity position. Note that the sequence of the task execution is important. We spawn the model first, followed by activating the joint state broadcaster, and activate the position controller at last. The execution order can be enforced by using the `RegisterEventHandler` utility function. For example, the code blow only execute `load_joint_state_broadcaster` after the `ignition_spawn_entity` process exits. ``` RegisterEventHandler( event_handler=OnProcessExit( target_action=ignition_spawn_entity, on_exit=[load_joint_state_broadcaster], ) ) ``` ## Step 5: Send a command to move the robot Once everything is up running. We can now send a request to the controller. The request takes an array as inputs as the controller controls multiple joints. To send a request, we publish a message to the `/position_controller/commands` topic. For example: {% code overflow="wrap" %} ``` ros2 topic pub --once /position_controller/commands std_msgs/msg/Float64MultiArray "{data: [0,-1.5,0,0,0,0]}" ``` {% endcode %} ## Demo {% embed url="" %} ## Download Code The code can be downloaded at this [link](https://learnros2.gumroad.com/l/bmkhwm). ©2023 - 2024 all rights reserved # Multiple Robotic Arms Simulation Demo ## Introduction In this article, we will demonstrate how to set up multiple robotics arms in gazebo. In the article [Robotic Arm Simulation Demo,](/ros/tutorials/robotic-arm-demo) we presented the steps to set up one robotics arms in gazebo, focusing on the interaction between ROS2 and Gazebo. Controlling multiple robotics arms presents new challenges. One of the reasons why it is difficult to manage multiple robots in ROS2 is that there is no standard on "encapsulation". Here we borrow the encapsulation from software engineer, which roughly means a clear boundary of an entity. Ros2 provides two mechanisms to define scope: namespace and node names, but the usages of these two mechanisms are not consistent in the ecosystem. This article is organize as follows. We first clarify the namespace concept and explain how to leverage this feature to support multiple robots. Next, we demonstrate how to introduce the namespace to the robot model file and why it's needed. Finally, we present the step to launch multiple robots in the Gazebo simulation, which requires additional steps due to some "limits" in the gazebo ros2 control library. ## Namespace and Multiple Robots Namespace is arguably the only way to define multiple robots in ROS2. Drawing a comparison between the concepts of namespace/node name and websites can enhance our understanding: the namespace serves a role similar to that of a domain name, while the node name parallels the specific page URL within a website. Just as a website comprises numerous pages, each designed for various purposes, a robot is equipped with multiple nodes, each tasked with distinct functions. A simple way of using namespace to support multiple robots is to allocate a unique namespace for each robot. For exmpale, in this article, we will set up two robotic arms: Bob and Toby. All Bob's nodes should use "Bob" as their namespace and respectively all Toby's nodes should use the "Toby" namespace. It's important to note that the `tf2` framework does not have the namespace concept. The frame ID is just a plain string. This means to define scopes in `tf2`, we need to establish a convention on frame ID first. For example, we could say frame IDs that share the same prefix can considered in the same group. Consequently, frames of a robot named Toby should have IDs such as Toby\_frame\_A, toby\_frame\_B, etc. Frame ID convention is a major source of confusion because different libraries have different behavior. Suppose we have the following robot model definition: ``` ... ``` Some libraries add the robot name as the frame ID prefix automatically so they will publish data for the frame `Toby_arm` or `Toby.arm` and they tend to be multi-robot friendly. For those libraries that do not add prefix, it's clear that they cannot support multiple robots out of the box because Bob and Toby both publish data for the same frame `arm` and the data is overwritten constantly. The solution is simple but can be tedious to set up. We can simply change the model file to ``` ... ``` and then pass the value to the xacro tool: ``` doc = xacro.parse(open(self.xacro_file)) xacro.process_doc(doc, mappings={ 'robot_name': self.robot_name, 'control_parameter_file': RobotModelConfigManager.CONTROL_PARAMETER_FILE}) robot_description = doc.toxml() ``` ## Edit Robot Model File and Support Multiple Robots It turns out that `ign_ros2_control::IgnitionROS2ControlPlugin` does not add robot name as the prefix of the frame ID and as discussed in the previous section, we need to manually add the robot name to all the link and joint names. Alternatively, we can parse the xml file and add the prefix programmatically. Links and joints specification is only part of the Gazebo ros2 control configuration. Recall that the plugin requires additional settings. For example: ``` $(find bringup)/configs/ur10-controller.yaml robot_description robot_state_publisher ``` We need to add namespace to these parameters as well: ``` $(find bringup)/configs/$(arg robot_name)_control_parameters.yaml /$(arg robot_name)/robot_description /$(arg robot_name)/robot_state_publisher $(arg robot_name) ``` The added `` tag is used to instruct plugin to create nodes with the provided namespace. Unfortunately, the above does not work due to a limitation in the `IgnitionROS2ControlPlugin`. At least for Gazebo Ign Fortress, the control parameters file is somehow shared among the plugins. Therefore, if we launch robot Bob, followed by robot Toby, both Bob's plugin and Toby's plugin will use Toby's parameter file. This implies that we cannot have separate control parameters for different robots. Instead, we need too have one single control parameters, which contains parameters for multiple namespace. For example: ``` Toby: controller_manager: ros__parameters: update_rate: 100 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster position_controller: type: position_controllers/JointGroupPositionController position_controller: ros__parameters: joints: - Toby_shoulder_pan - Toby_shoulder_lift - Toby_elbow - Toby_forearm_wrist_1 - Toby_wrist_1_wrist_2 - Toby_writs_2_wrist_3 Bob: controller_manager: ros__parameters: update_rate: 100 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster position_controller: type: position_controllers/JointGroupPositionController position_controller: ros__parameters: joints: - Bob_shoulder_pan - Bob_shoulder_lift - Bob_elbow - Bob_forearm_wrist_1 - Bob_wrist_1_wrist_2 - Bob_writs_2_wrist_3 ``` And the plugin configuration becomes: ``` $(find bringup)/configs/shared_control_parameters.yaml /$(arg robot_name)/robot_description /$(arg robot_name)/robot_state_publisher $(arg robot_name) ``` ## Launch the Stack Let's first take a look at the composition of the stack: * Rviz * Gazebo * robots * robot sate publisher * robot instance in Gazebo * joint state broadcaster * position controller To launch the stack, we first launch Rviz, Gazebo and robot state publishers. The sequence of these components does not matter. Next, for each robot, we spawn an entity in Gazebo, load the state broadcaster and the position controller. The order of these robot specific components matters and they need to be done in the mentioned order. It's reasonable to assume that the launch process of each robot is independent and they can be done in parallel. Unfortunately, that's not the case. With Gazebo Ignirition Fortress version, a race condition may be triggered if multiple robots load the position controller at the same time. To get around of the issue, we need to launch robot stack one by one. There is another issue. Recall that to fix the robot arm on the ground, we introduced a world link and a fixed joint between the world link and the robot base link. By default, the robot base link is set at the origin of the world. When we spawn the robot entities in Gazebo, they are at different places. We can specify the entity position with the `ros_gz_sim create` command. However, it seems that the IgnitionROS2ControlPlugin only takes care of the position in Gazebo and it does not update the position in ROS2. Therefore, even if we specify different positions for different robots in Gazebo, from ROS2's perspective, they all stay at the origin of the world. To fix this issue, we need to manually update the position in ROS2 by publishing a static transform between the world link and the robot base link. This can be achieved by the following commands: ``` ros2 run tf2_ros static_transform_publisher 0 2 0 0 0 0 world Toby_base ros2 run tf2_ros static_transform_publisher 0 4 0 0 0 0 world Bob_base ``` Here comes anther limitation. The `static_transform_publisher` is a node and it doesn't exit after it publishes the data. A manual exit is required. That's why this step is not included in the launch file in the attached code. ## Troubleshooting: Rviz cannot find material files In URDF, we can specify either the relative path of the absolute path. The relative path is defined by ``` ``` and the absolute path is defined by ``` ``` I cannot find detailed documentation on how ROS2 and Gazebo resolve the search path and most likely they are inconsistent. So far, what seems to work is as follows: * In the URDF/xacro robot model file, use `package://`. This notation indicates that the provided path is relative in both ROS2 and Gazebo. * Process the xacro file with appropriate mapping (i.e. specifying the arguments) and pass the result string to Gazebo nodes or processes. Gazebo model search path can be configured via the `IGN_GAZEBO_RESOURCE_PATH` environment variable. * Replace the `package://` with `file://` in the result string in the previous step, and pass it to ROS2 nodes or processes such as Rviz. ## Download Code The code can be downloaded at this link. ©2023 - 2024 all rights reserved # Introduction to xacro The first challenge is model description. ROS2 and Ignition Gazebo use different formats for robot description. ROS2 uses URDF and Gazebo uses SDF. Many solutions exist to deal with this issue. One can keep both URDF and SDF files at cost of maintaining two different files that need to be in sync. If we modify one of the files, the other one needs to be updated accordingly. Another approach is to use conversion tools. However, this dependency on external tools can be a blocker when we run into corner cases which are not covered by the tools. In this tutorial, we will show how to leverage xacro to generate both URDF and SDF files from a single source. We will walk you through how to create xacro file to describe our robot. First, create the name space and give our robot a name. ```xml ... ``` For this source file is used to generate two different files, we need a way to distinguish the two cases. This can be achieved by introducing a `sim_gazebo` flag: ``` ... ``` Let's add a link. A typical link definition in URDF files looks similar to the one below: ```xml ``` And a typical link definition in SDF looks like the following: ```xml 4 0.00610633 0 0 0.00610633 0 0.01125 model://ur10/meshes/base.dae 0 0 0.019 0 -0 0 0.075 0.038 ``` It's unfortunate that the syntax is similar but not exactly the same. To deal with different format, we will parameterize these blocks and leverage the macro. # Page # Tech Blog # Difference between URDF and SDF and how to convert ## Introduction In this article, we discuss the differences betwee URFD and SDF, focusing on their syntax and transformation representation. Additionally, we demonstrate how to implement the conversion from SDF to URDF and provide a customizable sample code. ## Related Readings * [URDF XML Specification](https://wiki.ros.org/urdf/XML) * [link](https://wiki.ros.org/urdf/XML/link) * [joint](https://wiki.ros.org/urdf/XML/joint) * [SDF Pose Frame Semantics Tutorial](http://sdformat.org/tutorials?tut=pose_frame_semantics) * [xacro documentation](http://wiki.ros.org/xacro) * [ROS xacro tutorial](http://wiki.ros.org/urdf/Tutorials/Using%20Xacro%20to%20Clean%20Up%20a%20URDF%20File) * [gz\_ros2\_control demo](https://github.com/ros-controls/gz_ros2_control/tree/humble/ign_ros2_control_demos) * [Tutorial: Using a URDF in Gazebo](http://classic.gazebosim.org/tutorials?tut=ros_urdf\&cat=connect_ros) * [Mesh element in URDF example](http://wiki.ros.org/urdf/XML/link#Recommended_Mesh_Resolution) ## Difference in Syntax The primary distinction between URDF and SDF lies in their syntax. You can find more details in the official format specifications online. Here, we illustrate with examples to showcase the apparent differences. In URDF, attributes are typically embedded within a tag as fields, such as ``. Conversely, SDF expresses the same information through nested tags, like `...`. **Example of URDF** ``` ``` **Example of SDF** ``` 4 0.00610633 0 0 0.00610633 0 0.01125 0 0 0.019 0 -0 0 0.075 0.038 model://ur10/meshes/base.dae wrist_2 wrist_1 3.58979e-09 0 -1 -6.28319 6.28319 54 3.2 1 ``` ## Difference in transformation representation A key difference between URDF and SDF lies in how transformations are represented. In URDF, transformations are specified with in the `joint` element, whereas in SDF, they are defined separated in the parent and child link elements. For instance: ``` 0 0 0 0 0 0 some values here base_link slider_link ``` It's important to note that during the conversion from SDF to URDF, values for the `pose` tag in the child link are not directly transferred. This is because the representation of transformations differ significantly between the two formats, making the process more complex than a straightforward copy of tag values. The specification of the transformation in URDF is straightforward. According to the [specification document](https://wiki.ros.org/urdf/XML/joint): > (optional: defaults to identity if not specified) > > This is the transform from the parent link to the child link. The joint is located at the origin of the child link, as shown in the figure above. > > xyz (optional: defaults to zero vector): Represents the x, y, z offset. All positions are specified in metres. > > rpy (optional: defaults to zero vector): Represents the rotation around fixed axis: first roll around x, then pitch around y and finally yaw around z. All angles are specified in radians. It's important to note that the transformations in URDF are always relative (i.e. from the parent link frame to the child link frame), whereas in SDF, they are more nuanced. According to the [SDF document](http://sdformat.org/spec?ver=1.11\&elem=joint#joint_pose): > Element Required: 0 Type: pose Default: 0 0 0 0 0 0 Description: A pose (translation, rotation) expressed in the frame named by @relative\_to. The first three components (x, y, z) represent the position of the element's origin (in the @relative\_to frame). The rotation component represents the orientation of the element as either a sequence of Euler rotations (r, p, y), see , or as a quaternion (x, y, z, w), where w is the real component. > > relative\_to Attribute Required: 0 Type: string Default: Description:If specified, this pose is expressed in the named frame. The named frame must be declared within the same scope (world/model) as the element that has its pose specified by this tag. If missing, the pose is expressed in the frame of the parent XML element of the element that contains the pose. For exceptions to this rule and more details on the default behavior, see . Note that @relative\_to merely affects an element's initial pose and does not affect the element's dynamic movement thereafter. New in v1.8: @relative\_to may use frames of nested scopes. In this case, the frame is specified using `::` as delimiter to define the scope of the frame, e.g. `nested_model_A::nested_model_B::awesome_frame`. This implies that the transformation described in the `pose` tag of the child link may be based on the world frame (by default) or a frame specified in the `relative_to` attribute, which differ from the parent frame. When you download an SDF file, it often uses the default mode of the `pose` tag, where link transformations are expressed in the world frame. Directly copying the the child link's pose value from an SDF file to the `origin` tag of a URDF `joint` element may lead to unexpected results. The below screenshot illustrates this effect. We also replicate this in Three.js, using the `Group` concept to verify the behavior.
{% hint style="warning" %} Another tag that has similar nuance is axis. For simplicity, we will not dive into the details in this article. {% endhint %} ## Conversion from SDF to URDF Since Gazebo can use URDF files directly, conversion from URDF to SDF is typically unnecessary. However, when downloading the robot models from the internet, which are often in the SDF format, converting them to URDF becomes essential. This section will outline the steps in this conversion process. ### Parse SDF file The first step is to parse the SDF file and create an in-memory representation. An XML parser library such as ElementTree in python can do the job. ### Convert to URDF Format Follow the URDF specification and convert the in-memory representation into the URDF format. This step is a standard serialization process. ### Adjust Model Directory For SDF files that use mesh files, it's often necessary to modify the model path. This is because ROS2 and Gazebo locate model files differently. In URDF, mesh file locations are specified as follows: ``` ``` where as in SDF, we use the following form: ``` model://ur10/meshes/base.dae ``` The key distinction between the two formats lies in their path references: URDF uses an absolute path, whereas SDF employs a relative path that depends on an environment variable that configure the model search directory. To standardize the path, we can set this environment variable to the directory of the package that contains the model resources. In Gazebo Fortress, the relevant variable is `IGN_GAZEBO_RESOURCE_PATH`. This adjustment can be implemented in the python launch file. For instance, {% code overflow="wrap" %} ``` os.environ['IGN_GAZEBO_RESOURCE_PATH'] = get_package_share_directory('') ``` {% endcode %} ### Adjust Transformation As discussed earlier, converting transformation of child links from the world frame to the parent frame may be necessary. The `pose` element in SDF takes the form `x y z r p y` and in both `pose` and `origin` tags, the `xyz` represents to the translation part of the transformation and the `rpy` attribute denote rotation. Recall that the homogeneous transformation is represented as follows: $$ H=\begin{bmatrix}R & d \0 & 1 \ \end{bmatrix} $$ Suppose the `pose` of `link` represents the transformation in the world frame. Consider a joint with parent link and the child link. This scenario involves three frames: * world frame * parent frame * child frame and three transformations: * $$H\_1^0$$: transformation of parent frame expressed in the world frame * $$H\_2^0$$: transformation of child frame expressed in the world frame * $$H\_2^1$$: transformation of the child frame expressed in the parent frame The first two transformation are inputs as they are given in the SDF file; the third transformation is the output because that's the information we need to put in the `joint` tag in the URDF file.
The conversion is given the formula below: $$ H\_2^1 = (H\_1^0)^{-1}H\_2^0 $$ and the inverse of the homogeneous transformations is given as: $$ H^{-1} = \begin{bmatrix} R^T & -R^Td \ 0 & 1 \ \end{bmatrix} $$ The python code below implements the conversion using `scipy`: ``` import numpy as np from scipy.spatial.transform import Rotation as R def compute_relative_transformation(rot_parent, d_parent, rot_child, d_child): """ Computes the relative transformation for the child frame expressed in parent frame. :param rot_parent: The rotation matrix of the parent frame expressed in the world frame. :param d_parent: The translation vector of the parent frame expressed in the world frame. :param rot_child: The rotation matrix of the child frame expressed in the world frame. :param d_child: The translation vector of the child frame expressed in the world frame. :return: The relative transformation of the child frame express in the parent frame. The translation part is a 3d vector and the rotation is also a 3d vector in rpy format. """ H_p_inv = np.zeros((4,4)) H_p_inv[:3, :3] = rot_parent.T H_p_inv[:3, -1] = -np.matmul(rot_parent.T, d_parent) H_p_inv[3, 3] = 1 H_c = np.zeros((4, 4)) H_c[:3, :3] = rot_child H_c[:3, -1] = d_child H_c[-1, -1] = 1 H_relative = np.matmul(H_p_inv, H_c) translation = H_relative[:3, -1] rotation_mat = H_relative[:3, :3] rotation = R.from_matrix(rotation_mat).as_euler("xyz", degrees=False) return translation, rotation ``` ## Sample Code We created a [python package](https://github.com/mukuplanet/sdf2urdf) that illustrates the steps mentioned in this article, which is not suitable for production usage. ©2023 - 2024 all rights reserved # Gazebo # Index {% hint style="info" %} Gazebo Version: Fortress {% endhint %} Github Repo * [gz-make](https://github.com/gazebosim/gz-cmake) * [gz-plugin](https://github.com/gazebosim/gz-plugin) * [gz-sim](https://github.com/gazebosim/gz-sim/) * [Gazebo Systems](https://github.com/gazebosim/gz-sim/tree/gz-sim7/src/systems) * [Gazebo Plugins](https://github.com/gazebosim/gz-sim/tree/gz-sim7/src/gui/plugins) * [sdformat\_urdf](https://github.com/ros/sdformat_urdf): Convert SDF file to URDF. * [gazebo guid source code: gz-gui6](https://github.com/gazebosim/gz-gui/tree/2630a94c3b1c49bfa33447cd47b1d39528cfb3ec) Resources * [Finding Resources](https://gazebosim.org/api/gazebo/4.0/resources.html) # Terminology * **World:** The term used to describe a collection of robots and objects (such as buildings, tables, and lights), and global parameters including the sky, ambient light, and physics properties. * **Static:** Entities marked as static (those having the `true` element in SDF), are objects which only have collision geometry. All objects which are not meant to move should be marked as static, which is a performance enhancement. * **Dynamic:** Entities marked as dynamic (either missing the `` element or setting false in SDF), are objects which have both inertia and a collision geometry. Reference: # GUI # World Frame and Axis {% hint style="info" %} The Gazebo version is Fortress {% endhint %} We have already seen that the root node in a `sdf` file is `` and other entities are the child node of the world. The frame of this world node is the world frame. The easiest way to visualize it is to create a box entity and set the pose to (0,0,0). If we run the command `ign gazebo shapes.sdf`, it brings a default world with shapes and the box is put at the origin of the world with an offset in z-axis. To dislay the axis, we can use click the translate button and then select the red box. **The red arrow represents the x-axis, green the y-axis, and blue the z-axis.**
Strictly speaking, the axis displayed in the figure above is the axis of the local frame of the red box. It just happens that it aligns with the world frame in the shape.sdf file. To verify it's the axis of the local frame, we can rotate the box and see the effect:
Note that if we now translate the red box, it moves along the local axis. If we want to translate a rotated entity along the world frame axis, we can hold the shift key and drag the object. # Cookbook {% hint style="warning" %} We use Gazebo version Fortress in our local experiment but the linked documentation may involve a different version of Gazebo. {% endhint %} ## How are the model files resolved? For large world, it's impossible to store all the model definition in the .sdf world file. Instead, models are commonly stored in a subdirectory `models`. For example, in the aws-robomaker-bookstore-world, we have the following project structure ``` ~/workspace/gazebo-projects/aws-robomaker-bookstore-world$ tree -L 1 . ├── CMakeLists.txt ├── CODE_OF_CONDUCT.md ├── CONTRIBUTING.md ├── docs ├── launch ├── LICENSE ├── maps ├── models ├── my_note.txt ├── package.xml ├── README.md ├── routes └── worlds ``` And the models directory looks like the following: ``` ~/workspace/gazebo-projects/aws-robomaker-bookstore-world/models$ tree -L 2 | head -n 15 . ├── aws_robomaker_retail_AirConditionerC_01 │   ├── materials │   ├── meshes │   ├── model.config │   └── model.sdf ├── aws_robomaker_retail_Bicycle_01 │   ├── materials │   ├── meshes │   ├── model.config │   └── model.sdf ``` These models are reference in the world file. For instance: ```xml model://aws_robomaker_retail_AirConditionerC_01 7.458202 1.422453 3.290944 0 -0 -1.564217 ``` As we can see, the model path is a relative path. Therefore, when launching the Gazebo, we need to set the model path. Depending on the Gazebo version, there may be many environment variables available. According to the [documentation](http://classic.gazebosim.org/tutorials?tut=components\&cat=get_started), we have the following variables: * GAZEBO\_MODEL\_PATH: colon-separated set of directories where Gazebo will search for models * GAZEBO\_RESOURCE\_PATH: colon-separated set of directories where Gazebo will search for other resources such as world and media files. * GAZEBO\_MASTER\_URI: URI of the Gazebo master. This specifies the IP and port where the server will be started and tells the clients where to connect to. * GAZEBO\_PLUGIN\_PATH: colon-separated set of directories where Gazebo will search for the plugin shared libraries at runtime. * GAZEBO\_MODEL\_DATABASE\_URI: URI of the online model database where Gazebo will download models from. For Gazebo Fortress, we need to set the variable IGN\_GAZEBO\_RESOURCE\_PATH to the `models` directory. This variable is documented on this [page](https://gazebosim.org/api/gazebo/4.0/resources.html), Models, Lights, actors section. ## Where are the Gazebo plugins installed on Ubuntu? * Check the environment variable `IGN_GAZEBO_SYSTEM_PLUGIN_PATH` * Check directory similar to /usr/lib/x86\_64-linux-gnu/gazebo-7.0/plugins # Page 1 # Programming in Robotics # C++ # CMake ## Introduction ## Related Readings * [GNU gcc compiler documentation](https://gcc.gnu.org/onlinedocs/gcc/) * [CMake Key Concept](https://cmake.org/cmake/help/book/mastering-cmake/) * [The Hitchhiker’s Guide to the CMake](https://cgold.readthedocs.io/en/latest/) * [GoogleTest Documentation](http://google.github.io/googletest/) ## Key Concepts ### Target ### Subdirectory ### Libraries ### Include Directories ### Dependencies ## Useful Commands TODO # Python # Rust # Mathematics in Robotics # Linear Algebra # Matrix Properties # Probability ## ## Importance Sampling According to wiki, importance sampling is a Monte Carlo method for evaluating properties of a particular distribution, while only having samples generated from a different distribution than the distribution of interest. $$ \begin{align\*} E\_p\[f(X)] & = \int{}f(x)p(x)dx \\ & = \int{}f(x)\frac{p(x)}{q(x)}q(x)dx \\ & = E\_q\[\frac{p(X)}{q(X)}f(X)] \end{align\*} $$ The term $$\frac{p(x)}{q(x)}$$is a weight factor. The left side of the equation is the expectation of $$f(X)$$ under distribution $$p(x)$$. Suppose we have samples of $$X$$under distribution $$q(x)$$, we can reconstruct the expectation of $$f(X)$$ under distribution $$p(x)$$ by weighting the samples (i.e. multiplying $$f(x)$$by $$\frac{p(x)}{q(x)}$$. The distribution $$p(x)$$is sometimes called the target distribution and they are often not directly accessible. # Expectation-Maximization Algorithm # Multivariable Function and Derivatives # Physics in Robotics # Control of Dynamic Systems ## # Dynamic Response and Transfer Function ## Introduction ## ## Laplace Transform **Definition**: One-sided Laplace Transform $$ \mathcal{L}[f(t)](https://www.learnros2.com/control-of-dynamic-systems/s) = \int\_{0}^{\infty}f(t)e^{-st}dt $$ The inverse of the Laplace is given as follows: $$ f(t) = \frac{1}{2\pi{}j}\int\_{x\_0-j\infty}^{x\_0+j\infty}\mathcal{L}[f](https://www.learnros2.com/control-of-dynamic-systems/s)e^{st}ds $$ where $$x\_0$$is a value that lies on the right side of all the singularities of $$\mathcal{L}[f](https://www.learnros2.com/control-of-dynamic-systems/s)$$ in the s-plane. **Prop:** $$ \begin{align\*} (1);;; \mathcal{L}&\[\alpha f\_1 + \beta f\_2] = \alpha \mathcal{L}\[f\_1] + \beta \mathcal{L}\[f\_2] \\ (2);;; \mathcal{L}&[f(t-\lambda)](https://www.learnros2.com/control-of-dynamic-systems/s) = e^{-s\lambda}\mathcal{L}[f](https://www.learnros2.com/control-of-dynamic-systems/s) \\ (3);;; \mathcal{L}&[f(at)](https://www.learnros2.com/control-of-dynamic-systems/s) = \frac{1}{|a|}\mathcal{L}[f](https://www.learnros2.com/control-of-dynamic-systems/\frac{s}{a}) \\ (4);;; \mathcal{L}&[f(t)e^{-at}](https://www.learnros2.com/control-of-dynamic-systems/s) = \mathcal{L}[f](https://www.learnros2.com/control-of-dynamic-systems/s+a) \\ (5);;; \mathcal{L}&[\frac{df}{dt}](https://www.learnros2.com/control-of-dynamic-systems/s) = -f(0^{-}) + s \cdot \mathcal{L}[f](https://www.learnros2.com/control-of-dynamic-systems/s) \\ (6);;; \mathcal{L}&[\int\_{0}^{t}f(\tau)d\tau](https://www.learnros2.com/control-of-dynamic-systems/s) = \frac{1}{s}\mathcal{L}[f](https://www.learnros2.com/control-of-dynamic-systems/s) \\ (7);;; \mathcal{L}&[(f \ast g)(t)](https://www.learnros2.com/control-of-dynamic-systems/s) = \mathcal{L}[f](https://www.learnros2.com/control-of-dynamic-systems/s) \mathcal{L}[g](https://www.learnros2.com/control-of-dynamic-systems/s) \\ (8);;; \mathcal{L}&[f(t)g(t)](https://www.learnros2.com/control-of-dynamic-systems/s) = \frac{1}{2\pi{}j}(\mathcal{L}\[f] \ast \mathcal{L}\[g])(s) \\ (9);;; \mathcal{L}&\[tf(t)] = - \frac{d}{ds}(\mathcal{L}[f](https://www.learnros2.com/control-of-dynamic-systems/s)) \end{align\*} $$ Note: the equations (5) and (6) are important. They can convert terms in a differential equations into simple algebraic operations. That's one of the reasons when we analyze system behaviors, we work in the state space instead of in the time domain. ## Linear Time-Invariant System ## Dynamic Response ## Analyze System Behavior TODO partial-Fraction Expansion
# Block Diagram The block diagram is used to visualize the relationships between signals and transfer functions. Here, we present a few building blocks of the block diagram. The third example is a basic closed-loop setup.
# PID Controller For the purpose of completeness, we provide a brief description of PID Controller. In the time domain, a PID controller has the following equation: $$ u(t) = K\_pe(t) +K\_I\int\_0^{t}e(\tau)d\tau + K\_D\dot{e}(t) $$ where $$K\_p$$, $$K\_I$$, and $$K\_D$$ are parameters for proportional control, integral control, and derivative control respectively. In the state domain, a PID controller has the following transfer function: $$ H(s) = K\_P+\frac{K\_I}{s} + K\_D s $$ This is a direct application of the Laplace transform The integral control helps to remove the steady state error; the combination of integral and derivative control helps to reduce the overshoot and improve the transient response to make it faster. There is a very nice video () that explains the intuition behind these terms. The general idea is the following: suppose we want to track a variable, say a car. By tracking, it means we drive side-by-side with the tracked car. Initially, we are behind, in this case, the error is the distance between our car and the tracked car. The proportional term means if the distance is long, we should make more effort (i.e. press the gas pedal more). Imaging we are able to reduce the distance. This means our car is driving faster than the tracked car. At the moment when we catch up the tracked car, the distance is zero, which means the error is zero. At this particular moment, we are not making effort. However, during the catch-up process, our car drives faster. The speed of the car is continuouse so even if we stop making efforts anymore, our car is still faster than the tracked car, which means it will go beyond the tracked car. This is overshoot. To deal with the overshot, a derivative term is added as an offset. In our case, as the distance is reduced, we should make less efforts. The integral term is used to improve the control in steady state. For example, based on the distance, we apply the proportional control, this may make our car drive at the exact same speed as the tracked car. In this case, there is no change in the distance between the two cars so the derivative term is zero and the two-car system is in a steady state and the distance will remain the same forever. The intergral control term represents the accumulation of the error. In our example, the distance and the error remain constant and the error is accumulated. Thus the integral term will instruct the car to be faster and make it escape the steady state. # Robot Modeling and Control # Rotation and Homogeneous Transformation ## Introduction In this document, we will focus on rotation. We assume the reader is familiar with basic linear algebra. Rotations and homogeneous transformations are key in robotic control. They are mathematical tools that represent rigid-body motion. In this document, we focus on different representations of rotation. ## Resources and Related Readings Most of the content of this document is based on the following three books: * [Modern Robotics: Mechanics, Planning, and Control](https://hades.mech.northwestern.edu/index.php/Modern_Robotics) * [Robotics, Vision and Control - 3rd Edition](https://www.amazon.com/Robotics-Vision-Control-Fundamental-Algorithms/dp/3031064682/ref=sr_1_1?crid=1CUB79FMBM0FT\&keywords=robotics+vision+and+control\&qid=1707185685\&sprefix=robotics+vision+a%2Caps%2C102\&sr=8-1\&ufe=app_do%3Aamzn1.fos.006c50ae-5d4c-4777-9bc0-4513d670b6bc) * [Robot Modeling and Control - 2nd Edition](https://www.amazon.com/Robot-Modeling-Control-Mark-Spong/dp/1119523990/ref=sr_1_1?crid=33YTRCZ76P9UG\&keywords=Robot+modeling+and+control\&qid=1707185717\&sprefix=robot+modeling+and+control%2Caps%2C113\&sr=8-1\&ufe=app_do%3Aamzn1.fos.006c50ae-5d4c-4777-9bc0-4513d670b6bc) ## Notation and Convention It's important we have an intuitive notations in formula when we study robotics. In robotics, we need to deal with many different frames and coordinate systems and it's critical that we know unambiguously which frames we are working with. In this document, we adapt the notations from the book [Robotics, Vision and Control - 3rd Edition](https://www.amazon.com/Robotics-Vision-Control-Fundamental-Algorithms/dp/3031064682/ref=sr_1_1?crid=1CUB79FMBM0FT\&keywords=robotics+vision+and+control\&qid=1707185685\&sprefix=robotics+vision+a%2Caps%2C102\&sr=8-1\&ufe=app_do%3Aamzn1.fos.006c50ae-5d4c-4777-9bc0-4513d670b6bc) and [Robot Modeling and Control - 2nd Edition](https://www.amazon.com/Robot-Modeling-Control-Mark-Spong/dp/1119523990/ref=sr_1_1?crid=33YTRCZ76P9UG\&keywords=Robot+modeling+and+control\&qid=1707185717\&sprefix=robot+modeling+and+control%2Caps%2C113\&sr=8-1\&ufe=app_do%3Aamzn1.fos.006c50ae-5d4c-4777-9bc0-4513d670b6bc).
## Rotation Representations There are many representations of rotations. The most common ones are the following: * rotation matrices * Euler and Cardan angles representation * rotation axis and angle * exponential coordinates * unit quaternions In this section we will briefly go through each one of them. ### Rotation Matrices The rotation matrices representation is a direct application of the change of basis. Columns in a rotation matrix $$R$$represents the coordinates of bases of the transformed frame in the base frame. It has the following properties: $$ \begin{align\*} & R^{-1} = R^T &\\ & \textrm{det}(R) = 1 & \end{align\*} $$ #### Example: Rotate a robot on a 2D map In this case, the rotation is around the z-axis and the rotation matrix takes the following form $$ R\_z(\theta)=\begin{pmatrix} \cos{\theta} & -\sin{\theta} & 0 \\ \sin{\theta} & \cos{\theta} & 0 \\ 0 & 0 & 1 \end{pmatrix} $$ #### Interpretations of Rotation Matrix There are two interpretations/views of rotation. As we mentioned earlier, the rotation matrix represents the change of basis. It relates the coordinates of the same point in two differnet frames.
The second view of the rotation matrix is that it applies the rotation transformation on an object. In this view, the coordinates are all in the same frames (i.e. the original frame) and no other frames are involved. The effect is that it transforms the coordinates of the blue dot in the **black** frame in the figure below to the coordinates of the red dot in the **black** frame. (The orange frame is the frame after the rotation as it's rigidly related to the dot. However it's not involved in the formula)
### Three-Angle Representations and Euler Angles Euler's rotation theorem: > Any two independent orthonormal coordinate frames can be related by a sequence of rotations (not more than three) about coordinate axes, where no two successive rotations may be about the same axis (Kuipers 1999) According to the Euler's theorem, a rotation between any coordinate frames can be constructed by a sequence of rotation about a particular axis. During the process of constructing the intermediate rotations, new coordinate frames are created and the next rotation is about one of the axis in the current frame (not the original one). More specifically, suppose the original frame is denoted by $$F\_0$$ and the consecutive rotations are denoted by $$R\_1$$, $$R\_2$$, and $$R\_3$$. We use $$F\_{final}$$to denote the final frame and we have the transformation sequence below: $$ F\_0 \xrightarrow{R\_1} F\_1 \xrightarrow{R\_2} F\_2 \xrightarrow{R\_3} F\_3=F\_{final} $$ Because the rotation is about an axis in the current frame, we can simply compose the transformation: $$ R^{0}\_{final}=R\_1R\_2R\_3 $$ In the book [Robotics, Vision and Control - 3rd Edition](https://www.amazon.com/Robotics-Vision-Control-Fundamental-Algorithms/dp/3031064682/ref=sr_1_1?crid=1CUB79FMBM0FT\&keywords=robotics+vision+and+control\&qid=1707185685\&sprefix=robotics+vision+a%2Caps%2C102\&sr=8-1\&ufe=app_do%3Aamzn1.fos.006c50ae-5d4c-4777-9bc0-4513d670b6bc), the author mentions that Euler angles is an ambiguous term. What's more confusing is that the rotation sequence depends on the context. For example, when we say roll-pitch-yaw angle, it could mean either XYZ or ZYX rotation. The bottom line is that when we use the three-angle representation of rotations, we need to * Remember the rotation axis is in the "current" or "latest" intermediate frame. * Simply compose the rotation following the order. **Example: Roll-pitch-yaw with ZYX sequence** Remember that the composition order is simply the order of the rotation sequence. In this example, the rotation sequence is $$R\_z(\gamma)$$, $$R\_y(\beta)$$, and $$R\_x(\alpha)$$. Therefore, the overall rotation is: $$ R(\alpha, \beta, \gamma) = R\_z(\gamma)R\_y(\beta)R\_x(\alpha) $$ ### Exponential Coordinates and Different View of Rotation A rotation can be viewed as the result of the object being subject to a constant angular velocity $$\dot{\theta}$$during the unit of time (i.e. 1 second). Suppose the rotation axis is given by the unit vector $$\hat{\omega}$$, the angular velocity can be represented by $$ \omega = \dot{\theta} \hat {\omega} $$ We also have the formula of the velocity of a point subject to the angular velocity: $$ \dot{p}(t) = \omega \times p(t) $$ TODO: this is a bit complicated. It's related to the twist concetp. ### Rotation Axis and Angle The most natural way to describe a rotation is to specify the rotation axis and the rotation angle.
The relationship between the axis-angle pair and the rotation matrix is established by the Rodrigues' rotation formula: $$ R(\theta, \hat{v}) = I + sin(\theta)\[\hat{v}]*{\times} + (1 - \cos{\theta})\[\hat{v}]^2*{\times} $$ where $$\theta$$ is the rotation angle, $$\hat{v}$$ is the unit vector that represents the rotation axis. $$\[\hat{v}]\_{\times}$$ is a skew-symmetric matrix constructed from the rotation axis vector. This formula has a variation: $$ R(\theta, \hat{v})x = x + 2s(\omega \times x) + 2(\omega \times (\omega \times x)) $$ where $$s=\cos{\frac{\theta}{2}}$$ and $$\omega = \sin{\frac{\theta}{2}} \hat{v}$$. We can also recover the the rotation axis vector and the rotation angle from the rotation matrix. The rotation axis vector is the eigen vector with eigen value equals to 1. Note that a rotation matrix always has a real eigenvalue 1 because vectors on the rotation axis will not be changed by the rotation. The other two eigenvalues are $$\lambda = \cos{\theta} \pm j \sin{\theta}$$ where $$\theta$$ is the rotation angle. ### Unit Quaternions Quaternions is frequently used in ROS. For information about quaternion in ROS, you can refer to the article [Quaternion Fundamentals](https://docs.ros.org/en/foxy/Tutorials/Intermediate/Tf2/Quaternion-Fundamentals.html). We adopt the notation in the book [Robotics, Vision and Control - 3rd Edition](https://www.amazon.com/Robotics-Vision-Control-Fundamental-Algorithms/dp/3031064682/ref=sr_1_1?crid=1CUB79FMBM0FT\&keywords=robotics+vision+and+control\&qid=1707185685\&sprefix=robotics+vision+a%2Caps%2C102\&sr=8-1\&ufe=app_do%3Aamzn1.fos.006c50ae-5d4c-4777-9bc0-4513d670b6bc) and represent a quaternion using the following form: $$ \breve{q}(s, \bm{v}) = s\langle v\_x, v\_y, v\_z \rangle $$ and a rotation with axis $$\bm{\hat{v}}$$ and angle $$\theta$$ can be represented by a **unit** quaternion: $$ \r{q} = \cos{\frac{\theta}{2}} \langle \bm{\hat{v}} \sin{\frac{\theta}{2}} \rangle $$ We can apply the following operations to a quaternion: $$ \begin{align\*} &\breve{q}^\* = s \langle -v\_x, -v\_y, -v\_z \rangle \\ &\breve{q}*1 \cdot \breve{q}*2 = s\_1s\_2 + v*{x1}v*{x2} + v\_{y1}v\_{y2} + v\_{z1}v\_{z2} \\ &\breve{q}\_1 \circ \breve{q}\_2 = (s\_1s\_2 - \bm{v\_1}\cdot\bm{v\_2}) \langle s\_1 \bm{v\_2} + s\_2 \bm{v\_1} +\bm{v\_1} \times \bm{v\_2} \rangle \end{align\*} $$ ## Relations between Different Representations
# Probabilistic Robotics The world is complex and imperfect. For instance, surrounding environment can have disturbance and sensor data can have noise. Randomness is all over the place and the system is not deterministic. To deal with randomness, we need a framework. In this chapter, we provide a brief description of the probabilistic robotics framework. The main objective is to estimate the state of the robot while dealing with the noise. We start with Bayes Filter and introduce the basic Bayes Filter algorithm, Kalman filter and some non-parametric filters such as particle filter. TODO # Bayes Filter The core idea of the application of Bayes' rule is that we can update our belief based on the new data. Here is a classic example in many robotics text books that illustrates the idea:
This example might be too simple. An example with multiple doors is more interesting for the thought process but we will leave it as an exercise. ## Graph Model Representation The Bayes Filter algorithm consists of two steps: * prediction * belief update
TODO
## Bayes Filter Algorithm
{% code fullWidth="true" %} ```latex \begin{algorithm} \renewcommand{\thealgorithm}{} \caption{\textbf{Bayes Filter}}\label{alg:cap} \begin{algorithmic}[1] \Function{BayesFilter}{$bel(x_{t-1}), u_t, z_t$} \ForAll{$x_t$} \State $\overline{bel}(x_t) = \int{p(x_t|u_t, x_{t-1})bel(x_{t-1})dx_{t-1}}$ \State $bel(x_t) = \eta{}p(z_t|x_t)\overline{bel}(x_t)$ \EndFor \State \textbf{return} $bel(x_t)$ \EndFunction \end{algorithmic} \end{algorithm} ``` {% endcode %} $$ \ $$ # Kalman Filter
Recall that the key steps in implementing a model are: * represent the probability distribution (analytically or using sampling) * model the motion (or state transition): $$p(x\_t|u\_t, x\_{t-1})$$ * model the measurement: $$p(z\_t|x\_t)$$ ## Kalman Filter In the standard Kalman Filter algorithm, the state transition is modeled as: $$ x\_t = A\_tx\_{t-1} + B\_t u\_t + \epsilon\_t $$ where $$\epsilon\_t \sim \mathcal{N}(0, R\_t)$$. The covariance matrix $$R\_t$$ represents the uncertainty or noise. To obtain the analytic form of $$p(x\_t|u\_t,x\_{t-1})$$, we just need to observer $$x\_t - A\_tx\_{t-1} - B\_tu\_{t}$$ follows the normal distribution. Similarly, the measurement in the standard Kalman filter is modeled as: $$ z\_t = C\_tx\_t + \delta\_t $$ where $$\delta\_t \sim \mathcal{N}(0, Q\_t)$$ describing the measurement noise. It can be shown that in the standard Kalman filter, the states $$x\_t$$follows normal distribution. i.e.: $$ x\_t \sim \mathcal{N}(\mu\_t, \Sigma\_t) $$ and the algorithm is given as
``` \begin{algorithm} \renewcommand{\thealgorithm}{} \begin{algorithmic}[1] \Function{Kalman\_Filter}{$\mu_{t-1}, \Sigma_{t-1}, u_t, z_t$} \State $\bar{\mu}_t = A_t \mu_{t-1} + B_t u_t$ \State $\bar{\Sigma}_t = A_t\Sigma_{t-1}A_t^T + R_t$ \State $K_t = \bar{\Sigma}_t C_t^T (C_t \bar{\Sigma}_t C_t^T + Q_t)^{-1}$ \Comment{This is called Kalman gain} \State $\mu_t = \bar{\mu}_t + K_t(z_t - C_t \bar{\mu}_t)$ \State $\Sigma_t = (I - K_tC_t) \bar{\Sigma}_t$ \State $\textbf{return} \;\;\; \mu_t, \Sigma_t$ \EndFunction \end{algorithmic} \end{algorithm} ``` ## Extended Kalman Filter In EKF, the state transition and measurement model take more general form: $$ \begin{align\*} x\_t & = g(u\_t, x\_{t-1}) + \epsilon\_t \\ z\_t & = h(x\_t) + \delta\_t \end{align\*} $$ The problem is that for arbitrary functions, it may not be possible to obtain an analytic form of the distribution of state variable $$x\_t$$. One way to get around this problem is linearization using Taylor expansion. Before we look into the detail of the Taylor expansion, let's take one step back and review what we already have. One of the important things is not to confuse parameters with known values. Recall that the ultimate goal is to calculate $$p(x\_t|u\_t, x\_{t-1})$$and $$p(z\_t|x\_t)$$. Although they are conditional probability and $$(u\_t, x\_{t-1})$$ and $$x\_t$$ are conditions in the two expressions respectively, all of them are parameters (or function arguments). If we forget about the probability context for a moment, it's quite obvious $$p(x\_t|u\_t, x\_{t-1})$$ is a mapping from $$(x\_t, u\_t, x\_{t-1})$$to a value. We also recall that in the standard Kalman filter, the distribution of the states is tracked by $$\mathcal{N}(\mu\_t, \Sigma\_t)$$ so at time $$t$$, $$\mu\_1, \mu\_2, ..., \mu\_{t-1}$$ are known values. Now, we can get back to the Taylor expansion. For the motion model, we perform the linearization around the $$\mu\_{t-1}$$ because this is our estimate of the state at $$t-1$$ and it should be close to $$x\_{t-1}$$. Therefore, we have $$ \begin{align\*} g(u\_t, x\_{t-1}) & \approx g(u\_t, \mu\_{t-1}) + \frac{\partial g}{\partial x\_{t-1}}(u\_t, \mu\_{t-1})(x\_{t-1} - \mu\_{t-1}) \\ & = g(u\_t, \mu\_{t-1}) + G\_t(x\_{t-1} - \mu\_{t-1}) \end{align\*} $$ where $$\frac{\partial g}{\partial x\_{t-1}}(u\_t,\mu\_{t-1})$$ means the partial derivative with respect to the second variable evaluated at $$(u\_t, \mu\_{t-1})$$. Similarly, we can write $$ h(x\_t) \approx h(\bar{\mu}\_t) + H\_t(x\_t - \bar{\mu}\_t) $$ where $$\bar{\mu}*t = g(u\_t, \mu*{t-1})$$. The extended Kalman filter algorithm is given as:
``` \begin{algorithm} \renewcommand{\thealgorithm}{} \begin{algorithmic}[1] \Function{EKF}{$\mu_{t-1}, \Sigma_{t-1}, u_t, z_t$} \State $\bar{\mu}_t = g(u_t, \mu_{t-1})$ \State $\bar{\Sigma}_t = G_t\Sigma_{t-1}G_t^T + R_t$ \State $K_t = \bar{\Sigma}_t H_t^T (H_t \bar{\Sigma}_t H_t^T + Q_t)^{-1}$ \State $\mu_t = \bar{\mu}_t + K_t(z_t - h(\bar{\mu}_t))$ \State $\Sigma_t = (I - K_tH_t) \bar{\Sigma}_t$ \State $\textbf{return} \;\;\; \mu_t, \Sigma_t$ \EndFunction \end{algorithmic} \end{algorithm} ``` # Particle Filter Particle filter is a non-parametric filter. The main idea is to use "particles" to represent the distribution. The algorithm involves sampling data from a given distribution. To some extent, we can view it as a mini simulation. Before presenting the pseudo code, let's first see an example. Suppose we have a state graph where a node represents a state and an edge represents the station transition. Each edge has a weight which corresponds to the probability of the transition from the source state to the destination state.
For example, the first state is the start state, there is a 80% chance it will go to the A1 state and another 20% chance to the A2 state. Now, the question is how we can figure our the distribution of states in the third level (i.e. B1, ..., B5). If the transition graph is simple, we may be able to manually calculate the distribution of states. As things are often complicated in real world, it can be challenging to come with the closed-form of the distribution. An alternative is to simulate the state transition. At a given state, we generate a sample of next state according to the transition probability. We repeat this process until we have sufficient data to approximate the real distribution.
#### Particle Filter Algorithm

(source: Probabilistic Robotics)

# Discrete Bayes Filter The discrete Bayes filter is a special case of the general Bayes filter. Because the states are discrete, the integral in the general Bayes filter becomes a summation. The implementation is straightforward and is given as follows:

(source: Probabilistic Robotics)

The key is to define the states. If the physical states are continuous, discretization is required. The number of the states is obviously an important parameter. The larger the state number, the more accurate the estimation is. On the other hand, with large state number, it's expensive to compute. # Motion Model Recall that one of steps in Bayes Filter algorithm is to perform a prediction. The prediction is an estimate of the state transition: $$ p(x\_t|u\_{t-1},x\_{t-1}) $$ How we use the model depends on the filter algorithm. For example, with particle filter, we essentially simulate a physical event and it's a direct application of the motion model. The input is $$u\_{t-1}$$and $$x\_{t-1}$$, and the output is $$x\_t$$. With particle filter and other sampling methods, we don't need to calcualte $$p(x\_t|u\_{t-1},x\_{t-1})$$. For algorithms that require an explicit calculation, the inputs are $$x\_t$$, $$u\_{t-1}$$, and $$x\_1$$. However, $$u\_{t-1}$$ and $$x\_{t-1}$$are real values while $$x\_t$$is hypothetical. In the book [Probabilistic Robotics](http://www.probabilistic-robotics.org/), the auther presents two models: * velocity model * odometry model The mathematical derivation starts from assuming the instantenous movement of the robot follows a perfect circular path.
TODO
# Perception Model This article focus on the following calculation in the Bayes Filter algorithm: $$ p(z\_t|x\_t) $$ which is the probability of obtaining the measurement $$z\_t$$ given the current robot state $$x\_t$$. For robots are often operating in a given environment, a map is avaialble in many applications. In such cases, we can have an additional term in the formula as follows: $$ p(z\_t|x\_t, m) $$ where $$m$$ represents the information of the map. In this article, we will present a couple of perception models: * correlation-based measurement models * likelihood fields model for range finders * feature based models * beam model Disclaimer: The content of this article is based on the chapter 6 in the book Probabilistic Robotics. You may notice that we rearrange the order of different models. The reason is that it may be easier to follow if we start with simple models. One thing to notice is that the first three models use the map information directly, while the beam model extracts the map information by learning from the data. ## Correlation-Based Measurement Models Correlation-Based measurement model is essentially a pattern matching algorithm. Given a robot state (note that this is not the current state of the robot. Instead, it's any possible state) and the sensor data, the algorithm performs the following steps * construct a local map $$M\_{local}^{local frame}$$ based on the sensor data. * use the provided robot state, we can map the local map data to the global map. (denoted by $$M\_{local}^{global frame}$$) * calculate a similarity score between the constructed map (i.e. $$M\_{local}^{global frame}$$) and the real map In the correlation-based measure model, we use correlation as the similarity measure. The correlation is then bounded away from zero. The obtains positive values is interpreted as the probability of the measurement. This interpretation is actually one of the drawbacks of correlation-based measurement models because there is no corresponding physical meaning. Here we provide examples to visualize the idea. Suppose we have a global map and the robot collects some data which is used to construct a local map:
Now we ask the question: given the local map, where do you think is the robot? To an human eye, this is not a hard question. It's reasonable to make the following guesses:
So why these spots? In fact, given a pose $$(x, y, \theta)$$ of the robot, we can project the collected sensor data to the global map and if the "pattern matches", then we know the pose $$(x, y, \theta)$$ is likely to be the actual pose of the robot.
The "pattern matching" has two parts: (1) pattern and (2) matching. For pattern part roughly corresponds to the transformation from local map to global map in the correlation-based models and the matching part is the calculation of similarity. ## Likelihood Fields for Range Finders TODO ## Beam Models of Range Finders Prerequisite: [EM Algorithm](/review-of-mathematics/probability/expectation-maximization-algorithm) # Localization Localization is the process of finding out the pose of the robot. It's a robotic specific use case of state estimation because at the end of the day, we want to estimate the robot state $$x\_t$$. The localization is a direct application of Bayes filter. We have seen different filters in previous chapters and each of them, equipped with appropriate motion models and measurement models, forms a localization algorithm. The relationship is given as the table below: | Filter Algorithm | Localization Algorithm | | -------------------------------------- | ------------------------ | | Kalman Filter | Markov Localization | | Histogram Filter/Discrete Bayes Filter | Grid Localization | | Particle Filter | Monte Carlo Localization | ## Markov Localization TODO ## Grid Localization The grid localization is a direct application of the [Discrete Bayes Filter](/probabilistic-robotics/discrete-bayes-filter). The first step is to define the grid. One common approach is to divide the state space into small chunks. The figure below is a grid map of the world:

(source: Wiki: https://en.m.wikipedia.org/wiki/File:World_Map_1-2,500,000_grid.svg)

The grids above only concerns the $$(x,y)$$ coordinates and we need an additional dimension to take the robot orientation into account. The grid size can be uniform or variable depending on the need. Similar to the discussion in the Discrete Bayes Filter section, the finer the grid, the more accurate the estimation is. The grid above is called the metric representation of the world. There is another type called topological grids. The idea is that only (important) features are on the grid map. One example is the subway map:
Stations are "feature" of the city and each station represents the surrounding area. We could immediately notice that the topological grid is a coarse representation of the real world. The emphasis is on features and other details are omitted. The next step is to select the motion model and measurement model. The grid size has impact on the motion model and measurement model implementation. The problem is that most of the time, the center of the grid is used to represent the robot position. Image we apply some control $$u\_t$$to the robot, if we don't pay enough attention in the implementation, what would happen is that the motion model would think that the control $$u\_t$$is not large enough and the robot stays in the same grid. This is problematic because the robot moves in the real world but the model thinks the robot stays in the same grid (which essentially puts the robot back to the center to the grid) and get stuck in that state. One common solution to this problem is to increase the noise in the motion and measurement model so that it's more likely for the robot to move to a new grid estimated by the model. Finally, apply the Discrete Bayes Filter algorithm and we obtain the grid localization algorithm as follows:

(source: Probabilistic Robotics)

## Monte Carlo Localization TODO # SLAM In this article, we attempt to give a high-level presentation of the SLAM(Simultaneous Localization and Mapping) based on the book Probabilistic Robotics. For the purpose of simplicity, the mathematical derivation will be omitted because they are technical details and are not strictly necessary for understanding the main idea of SLAM. ## Bayes Filer Algorithm Let's start with the Bayes Filter algorithm
$$x\_t$$, $$u\_t$$ and $$z\_t$$ represent state, control and measurement respectively. The $$p(x\_t|u\_t, x\_{t-1})$$ part in line 3 is called the motion model and it describes the state transition given a specific control. The $$p(z\_t|x\_t)$$in line 3 is called the measurement model or perception model and it describes the (expected) distribution of sensor data given the (current) state of the robot. Obviously, the motion model depends on how the robot moves and the measurement model depends on the sensors. The details of these two parts are not super important in our discussion about the SLAM algorithm. The Bayes Filter algorithm listed above is such a general framework that three out of four parts of the book Probabilistic Robotics are discussing its application. It can be used to solve the localization problems and the mapping problems. One of the reasons why it's so powerful is that **state** is a general concept. If the state consists of the 2D-robot pose $$(x, y, z, \theta)$$, the Bayes Filter solves the localization problem; if the state consists of both the robot pose and mapping, the Bayes Filter solves the localization and mapping problem simultaneously (SLAM). ## Map Representation TODO ## Different Types of SLAM Algorithm A SLAM algorithm has the following dimensions: * Motion models * Measurement models * Online SLAM vs full SLAM * Feature-based map vs grid The first two dimensions are not specific to SLAM and as we've seen earlier, they are part fo the general Bayes Filter algorithm. Online SLAM means our target is the snapshot at time $$t$$: $$p(x\_t, m, c\_t|z\_{1:t}, u\_{1:t})$$ while the full SLAM means the target is the full history: $$p(x\_{1:t}, m, c\_{1:t}|z\_{1:t},u\_{1:t})$$. As a practical consideration, a full SLAM algorithm requires more memory because it needs to track the full history. Therefore, special attention is required in the implementation. The last dimension is about map representation. Most of the algorithm described in the book use feature-based map and with a feature-based map, we can further divide the problem into two categories: (1) problem with known correspondence, and (2) problem with unknown correspondence. A problem with known correspondence means when we receive the sensor data, we know it's measurement of a specific feature in the environment. This correspondence information is not always available. For example, if the robot is put in a completely unknown environment or the environment has many symmetric structure, it's not an easy task to establish the correspondence relationship between the sensor data and a location in the environment. When the correspondence is unknown, the SLAM algorithm needs to take care of two more tasks: * how to identify the correspondence * how to know if the sensor is for a new feature that we haven't see before In next section, we follow the book Probabilistic Robotics and provide a brief discussion on three SLAM algorithms. ## SLAM with Extended Kalman Filters TODO ## GraphSLAM Algorithm TODO ## FastSLAM Algorithm TODO # Miscellany Examples: Laplace Transform symbol: \mathcal{L}: $$\mathcal{L}$$ # Concept Index # Quaternions ## Related Readings * [The Quaternions with an application to Rigid Body Dynamics](https://math.unm.edu/~vageli/courses/Ma375/literature/rrr.pdf). * [Understanding Quaternions](https://www.clear.rice.edu/comp360/lectures/old/QuaternionWeb.pdf). * [Quaternions, Interpolation and Animation](https://web.mit.edu/2.998/www/QuaternionReport1.pdf). * [Quaternion Algebras.](https://math.dartmouth.edu/~jvoight/quat-book.pdf) * [Quaternions and Rotations](https://graphics.stanford.edu/courses/cs348a-17-winter/Papers/quaternion.pdf)