Controlling a CANopen-based robot with ROS2 packages

16 min readOct 26, 2023

This article presents the integration of ROS2 (Robot Operating System, version 2) packages to build up a CANopen-based system to control the movement of an anthropomorphic arm robot.

ROS2 is “a set of software libraries and tools for building robot applications” [1]. We can refer to it as an open-source middleware framework with a decentralized and modular architecture that allows different software to run as independent nodes communicating with each other with well-defined interfaces. The ROS 2 layered architecture is presented below.

ROS2 layered architecture. — ROS 2 layered architecture. Based on [2, 3].

The ROS 2 defines three communication patterns that are organized under a node:

Topics: asynchronous and anonymous messaging system based on a publisher-subscriber architecture which allows many-to-many communication.
Services: a request-response communication for ensuring that the task request was received and processed.
Actions: asynchronous and goal-oriented communication to support long-running tasks. Clients can send requests, receive periodic feedback, and finally obtain a result from the server.

A diagram to provide a graphical view of these communication patterns is presented below.

A graphical view of the ROS2 communication patterns. Based on [2].

The other main component is the CANopen protocol which defines a set of communication services and protocols on top of the CAN (Controller Area Network). The CANopen is specified by the CAN in Automation (CiA) with several technical documents. We can highlight the CiA 301, which defines the application layer and communication profile, and the CiA 402, which defines the device profile for drives and motion control. The protocol is based on communication objects (COBs) for real-time data exchange, parameter configuration, and network management.

Furthermore, for this article, I created a repository to host the software stack built to simulate the controlling system. The repository can be found at [4], and one can follow this article as a tutorial to build and run the packages on its own machine. The repository was created to run on a Linux-based OS (Operating System) and can be used to build a local Docker image with the necessary dependencies.

GitHub - lucasmluza/ros2_moveit_canopen_example

Contribute to lucasmluza/ros2_moveit_canopen_example development by creating an account on GitHub.

github.com

Project Organization

The repository [4] has the following structure:

ros2_moveit_canopen_example/
├── panda_canopen/
├── panda_description/
├── panda_moveit/
├── panda_rviz_viewer/
├── ros2_canopen/ @e0b5706
├── .gitmodules
├── Dockerfile
├── README.md
└── compose.yaml

Where:

panda_<pkg_name>/: hosts the ROS2 package for the <pkg_name> application.
ros2_canopen @e0b5706: link to the ROS2 CANopen package [5].
.gitmodules: contains the submodules configuration (path, URL, and branch).
Dockerfile and compose.yaml: Docker-related files to build the image, and start and run the containers. Dockerfile contains the instructions to build the image, and the compose.yaml contains the service configuration to start the container with the required configuration.

Docker Usage

For building the Docker Image, you need to have the Docker and Docker Compose packages installed on your machine.

The commands below shall be executed in the root path of the repository.

Build the image:

docker compose build

To create and start the service container:

docker compose up ros2_humble

To run a second instance of bash on the running container, on an other terminal:

docker compose exec ros2_humble bash

Install ROS2 Dependencies

In the running container, install the dependencies with rosdep. In ~/ws_ros:

Update the apt repositories:

sudo apt update

Initialize rosdep:

sudo rosdep init && rosdep update

Install dependencies:

rosdep install -i --from-paths src -y

Building Packages

In the running container, in ~/ws_ros, build the system with the following command:

colcon build

To be able to use the compiled packages, source the install/setup.bash, in ~/ws_ros, as follows:

source install/setup.bash

Robot Model (URDF)

For this example project, the canopen_descriptionpackage takes the robot model description of the Franka Emika Panda robot from the moveit_resources package provided by the ros-planning[6].

The package contains the meshes of each link for both the visual and the collision description of the model. The URDF describes each link and joint, and how they are placed together to build up the robot model. For more details, one can refer to the specifications of the URDF which can be found at [7].

Rviz Viewer

The package panda_rviz_viewerwas created to visualize the 3D model of the robot and control the joint's position using graphical user interfaces, such as the Rviz2 [8]. The package contains a launcher to run the required nodes, and a configuration file (basic.rviz) to load the necessary elements on the Rviz2 graphical interface.

The launcher is as follows:

from launch import LaunchDescription
from launch.substitutions import PathJoinSubstitution, Command, FindExecutable

from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():

    ld = LaunchDescription()

    xacro_file = PathJoinSubstitution(
        [FindPackageShare("moveit_resources_panda_description"), "urdf", "panda.urdf"]
    )

    robot_description = Command(
        [PathJoinSubstitution([FindExecutable(name='xacro')]), ' ', xacro_file]
    )

    rviz_file = PathJoinSubstitution(
        [FindPackageShare("panda_rviz_viewer"), "launch", "basic.rviz"]
    )

    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters=[
            {'robot_description': robot_description}
        ],
    )
    
    joint_state_publisher_node = Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        parameters=[
            {'robot_description': robot_description},
        ],
    )

    rviz2 = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', rviz_file],
        parameters=[
            {'robot_description': robot_description},
        ]
    )

    ld.add_action(robot_state_publisher)
    ld.add_action(joint_state_publisher_node)
    ld.add_action(rviz2)

    return ld

Examine the Launcher Code

The launcher will start the following nodes: joint_state_publisher_gui, robot_state_publisher,and rviz2.

Joint State Publisher GUI

This node receives as a parameter the URDF model of the robot and then publishes to the joint_states topic according to the values provided by the user through the graphical interface.

Robot State Publisher

This node receives as a parameter the URDF model of the robot and then subscribes to the joint_statestopic to get the individual joint positions. The joint states are used to update the kinematic tree model and the resulting 3D poses are then published to tf2 related topics (/tf, and /tf_static).

The tf2 is the transform library that handles the transformation of coordinates between different frames maintaining a dynamic tree structure of coordinate frames.

Rviz2

Launches the Rviz2, which is a graphical interface for visualizing the robot model, sensor data, coordinate system, and more on the same window. The node receives as a parameter a .rviz file that provides a pre-defined configuration to be loaded on the GUI.

Launch the Application

The application to visualize the robot and control the joint states (i.e., the joint positions) can be launched as follows:

ros2 launch panda_rviz_viewer view.launch.py

CANopen Interface

This repository uses the ros2_canopenpackage that is available on the ros_industrial GitHub [9] for implementing the CANopen interface. The package provides nodes to interface with CANopen devices that are built on top of the Lely CANopen library [10]. In this case, the ros2_canopen repository is added as a submodule to the example repository.

The robot arm revolute joints (1 to 7) will be configured to be controlled using the CANopen interface. To achieve this behavior, the panda_canopen package was created as follows.

Package Structure

panda_canopen/
├── config/
│   └── cia402/
│       ├── bus.yml
│       └── cia402_slave.eds
├── include/
│   └── canopen_joint_state_publisher
├── launch/
│   ├── basic.rviz
│   ├── rviz.launch.py
│   └── simulation.launch.py
├── src/
│   └── canopen_joint_state_publisher.cpp
├── CMakeLists.txt
└── package.xml

Configuration

The ros2_canopenrelies on the bus.yml configuration file that hosts the details of which devices are connected to the CANopen network, and its configurations. The file has four main components: options, master, defaults, and devices. The configuration developed for this example is presented below.

options:
  dcf_path: "@BUS_CONFIG_PATH@"

master:
  node_id: 1
  driver: "ros2_canopen::MasterDriver"
  package: "canopen_master_driver"
  sync_period: 10000 # us

defaults:
  dcf: "cia402_slave.eds"
  driver: "ros2_canopen::Cia402Driver"
  package: "canopen_402_driver"
  period: 10 # 1 ms
  heartbeat_producer: 1000 # Heartbeat every 1000 ms
  position_mode: 1 # Profile Position Mode
  sdo: # SDO executed during config
        - {index: 0x6081, sub_index: 0, value: 1000} # Set Profile Velocity
        - {index: 0x6083, sub_index: 0, value: 10000} # Set Profile Acceleration
        - {index: 0x6084, sub_index: 0, value: 10000} # Set Profile Deceleration
        - {index: 0x6085, sub_index: 0, value: 10000} # Set Quickstop Deceleration
        - {index: 0x6098, sub_index: 0, value: 0} # Set default Homing Method to 0 (No homing operation required)
        - {index: 0x60C2, sub_index: 1, value: 50} # Set Interpolation Time Period Value to 50 ms
        - {index: 0x60C2, sub_index: 2, value: -3} # Set Interpolation Time Index to 10-3s
  tpdo:
    1:
      enabled: true
      cob_id: "auto"
      transmission: 0x01
      mapping:
        - {index: 0x6041, sub_index: 0} # Statusword
        - {index: 0x6061, sub_index: 0} # Modes of Operation Display
    2:
      enabled: true
      cob_id: "auto"
      transmission: 0x01
      mapping:
        - {index: 0x6064, sub_index: 0} # Position Actual Value
        - {index: 0x606C, sub_index: 0} # Velocity Actual Value
    3:
      enabled: false
    4:
      enabled: false
  rpdo:
    1:
      enabled: true
      cob_id: "auto"
      mapping:
      - {index: 0x6040, sub_index: 0} # Controlword
      - {index: 0x6060, sub_index: 0} # Modes of Operation
    2:
      enabled: true
      cob_id: "auto"
      mapping:
      - {index: 0x607A, sub_index: 0} # Target Position
      - {index: 0x60FF, sub_index: 0} # Target Velocity
    3:
      enabled: false
    4:
      enabled: false

nodes:
  panda_joint1:
    node_id: 2
  panda_joint2:
    node_id: 3
  panda_joint3:
    node_id: 4
  panda_joint4:
    node_id: 5
  panda_joint5:
    node_id: 6
  panda_joint6:
    node_id: 7
  panda_joint7:
    node_id: 8

The options element defines the dcf_path, that is the location of the generated .bin file which contains the SDO requests used to configure the slaves during boot-up.

The master element defines the package and driver that will implement the master node, its ID on the bus, and the sync period (μs). The remaining configurations follow the default values, which are kept implicit.

The default element can be used to set default configurations that are common to the slave nodes. It defines the DCF (Device Configuration File); the driver and the package that will implement the slaves; the period for refreshing the CiA 402 state machine; the heartbeat producer time period; and, more specifically for this example, the position_mode, which is set to use the Profile Position Mode.

In more detail, the canopen_402_driver package implements the CiA 402 profile and can set the device status, operation modes, and target values. Moreover, the Profile Position Mode is a control mode that allows precise positioning of a motor by specifying a desired target position. The motor controller moves the motor shaft to the specified position, either relative to the last target position or as an absolute position based on the last reference position. During the movement, parameters such as speed, acceleration, and deceleration are controlled within defined limits, ensuring smooth and controlled motion.

The sdo element defines the objects that will be configured during the boot-up via SDO. In this example, we are defining the velocity and acceleration to be taken into account when running in the Profile Position Mode. Also, the tpdo and rpdo elements define the mapped objects for each PDO that will be used. In this case, the configured objects are listed below.

TPDOs (Transmit PDOs) by the slave:

Statusword (0x6041–0): Provides information about the device’s current operational state and any active faults or errors.
Modes of Operation Display (0x6061–0): Indicates the current operational mode of a device, e.g., if it is in the Profile Position Mode, or Velocity Mode, among other available options.
Position Actual Value (0x6064–0): The actual position, which is absolute and referenced to the system zero position.
Velocity Actual Value (0x606C-0): The actual velocity at which a motor or a moving component is currently operating.

RPDOs (Receive PDOs) by the slave:

Controlword (0x6040–0): Control the device’s operational state.
Modes of Operation(0x6060–0): Control the current operational mode of a device.
Target Position (0x607A-0): The position that the drive is supposed to move to.
Target Velocity (0x60FF-0): The speed the drive is supposed to reach.

The nodes element defines the slave’s device name and the node ID.

All the possible configurations can be seen in more detail in [11] and [12].

Launchers

The main nodes to be launched for running the CANopen bus are the master and the slaves that will be simulated by the canopen_fake_slave package provided by the ros2_canopen. This can be accomplished by the simulation.launch.py, which is presented below.

from launch import LaunchDescription
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.substitutions import FindPackageShare

import yaml

def launch_setup(context, *args, **kwargs):

    can_interface_name = LaunchConfiguration("can_interface_name")

    nodes_to_start = []

    # Launch slaves nodes

    slave_launch = PathJoinSubstitution(
        [FindPackageShare("canopen_fake_slaves"), "launch", "cia402_slave.launch.py"]
    )

    bus_config = PathJoinSubstitution(
        [FindPackageShare("panda_canopen"), "config", "cia402", "bus.yml"]
    )

    with open(bus_config.perform(context), 'r') as f:
        bus_config_dict = yaml.safe_load(f)

    joints = []
    for key in bus_config_dict.keys():
        if "joint" in key:
            joints.append(key)

    for joint in joints:
        nodes_to_start.append(
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(slave_launch),
                launch_arguments={
                    "node_id": str(bus_config_dict[joint]["node_id"]),
                    "node_name": f"slave_node_{joint}",
                    "slave_config": PathJoinSubstitution([
                         FindPackageShare("panda_canopen"), "config", "cia402", str(bus_config_dict[joint]["dcf"])
                    ]),
                    "can_interface_name": can_interface_name,
                }.items(),
            )
        )

    # Launch master node

    master_launch = PathJoinSubstitution(
        [FindPackageShare("canopen_core"), "launch", "canopen.launch.py"]
    )

    master_config = PathJoinSubstitution(
        [FindPackageShare("panda_canopen"), "config", "cia402", "master.dcf"]
    )

    bus_config = PathJoinSubstitution(
        [FindPackageShare("panda_canopen"), "config", "cia402", "bus.yml"]
    )
    
    nodes_to_start.append(
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(master_launch),
            launch_arguments={
                "master_config": master_config,
                "master_bin": "",
                "bus_config": bus_config,
                "can_interface_name": can_interface_name,
            }.items(),
        )
    )

    return nodes_to_start

def generate_launch_description():
    declared_arguments = []

    declared_arguments.append(
        DeclareLaunchArgument(
            "can_interface_name",
            default_value="vcan0",
            description="CAN interface name to run the master and, when in simulation, the fake slaves.",
        )
    )
    
    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

This launcher retrieves the DCF file that is used for the fake slaves, and the node ID and names for each joint defined in the bus.yml file. Then, it adds to the launch description the cia402_slave.launch.py for each joint with its own node ID, name, and configuration. These fake slaves mock a real driver and can generate motion by following the parameters defined in the bus.yml when in Profile Position Mode (it also supports Cyclic Mode, but is not addressed in this example).

For the master node, it retrieves the bus.yml and the master.dcf file that is automatically generated when compiling the package by the cogen_dcf function implemented with the cmake. It then adds to the launch description the canopen.launch.py, from the canopen_core package with the master parameters.

To run this application, first, it is needed to have a virtual CAN interface available. For it, in the host (out of the container), the user can set the interface with the following commands:

sudo modprobe vcan
sudo ip link add dev vcan0 type vcan
sudo ip link set vcan0 txqueuelen 1000
sudo ip link set up vcan0

Obs: If using real hardware, replace vcan with can, and set the bitrate, e.g., bitrate 500000 for 500 kbps.

Since the compose.yaml is configured to network_mode = host, the network interfaces available on the host are also available inside the container, and thus, the container has access to the vcan0.

Then, the nodes can be executed with the launcher:

ros2 launch panda_canopen simulation.launch.py

Then, in a second bash instance of the container, the joints can be controlled by the exposed services.

Examples:

Initialize a joint:

ros2 service call /panda_joint1/init std_srvs/srv/Trigger

Set the operation mode to Profile Position Mode:

ros2 service call /panda_joint1/position_mode std_srvs/srv/Trigger

Set a target position value:

ros2 service call /panda_joint1/target canopen_interfaces/srv/COTargetDouble "{ target: 10.0 }"

One can check out the CAN bus messages with the following command:

candump vcan0

The candump is part of the can-utils package that is installed in the Docker container.

Furthermore, to be able to visualize the robot responding to the target position calls, it is necessary to make available the topic /joint_states, and then use the same structure presented in the package panda_rviz_viewer. Since the ros2_canopen stack publishes the individual joint states in topics named as /<joint_name>/joint_states, this example defines a ROS2 node (canopen_joint_state_publisher) to, based on the URDF model (passed as an argument), subscribe to all available individual joint states topics, and then publish the values of all joints in the “unified” joint states topic. The code to implement this node is presented below.

#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "urdf/model.h"
#include "kdl/tree.hpp"
#include "kdl_parser/kdl_parser.hpp"

using std::placeholders::_1;

class CANopenJointStatePublisher : public rclcpp::Node
{
public:
    CANopenJointStatePublisher() : Node("canopen_joint_state_publisher")
    {

        publisher_ = this->create_publisher<sensor_msgs::msg::JointState>(
            "/joint_states",
            rclcpp::SystemDefaultsQoS());

        std::string urdf_xml = this->declare_parameter("robot_description", std::string(""));

        if (urdf_xml.empty())
        {
            throw std::runtime_error("robot_description parameter must not be empty");
        }

        urdf::Model model;

        // Initialize the model
        if (!model.initString(urdf_xml))
        {
            throw std::runtime_error("Unable to initialize urdf::model from robot description");
        }

        // Initialize the KDL tree
        KDL::Tree tree;
        if (!kdl_parser::treeFromUrdfModel(model, tree))
        {
            throw std::runtime_error("Failed to extract kdl tree from robot description");
        }

        for (auto const &joint : model.joints_)
        {
            RCLCPP_INFO(this->get_logger(), "Get joint: '%s'", joint.first.c_str());
            joint_names_.push_back(joint.first.c_str());
            joint_positions_.push_back(0.0);

            subscriptions_.push_back(this->create_subscription<sensor_msgs::msg::JointState>(
                "/" + joint.first + "/joint_states",
                rclcpp::SystemDefaultsQoS(),
                std::bind(&CANopenJointStatePublisher::topic_callback, this, _1)));
        }
    }

private:
    void topic_callback(const sensor_msgs::msg::JointState::SharedPtr msg)
    {
        // ROS2 CANopen publishes a <joint_name>/joint_states with a vector of only one element
        if (msg->name.size() == 1)
        {
            // Search index that is associated with the joint name
            auto it = std::find(joint_names_.begin(), joint_names_.end(), msg->name[0]);
            if (it != joint_names_.end())
            {
                int index = it - joint_names_.begin();
                if (joint_positions_.at(index) != msg->position[0])
                {
                    joint_positions_.at(index) = msg->position[0];
                }
            }
        }

        auto message = sensor_msgs::msg::JointState();
        message.header.stamp = now();
        message.name = joint_names_;
        message.position = joint_positions_;
        publisher_->publish(message);
    }

    std::vector<std::string> joint_names_;
    std::vector<double> joint_positions_;

    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr publisher_;
    std::vector<rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr> subscriptions_;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<CANopenJointStatePublisher>());
    rclcpp::shutdown();
    return 0;
}

Then, the rviz.launch.py starts the nodes in a similar way that is done with the panda_rviz_viewer, but in this case, the joint_state_publisher is replaced by the canopen_joint_state_publisher.

To open the Rviz2 with the robot model:

ros2 launch panda_canopen rviz.launch.py

An example of controlling the /panda_joint1 is presented below.

MoveIt2 Integration

The MoveIt2 is a platform to implement motion planning, manipulation, 3D perception, kinematics, control, and navigation. The package, based on the URDF model, employs motion planning algorithms, e.g., OMPL (Open Motion Planning Library) to compute collision-free paths (based on the joint limits, collision check, and other possible constraints) for the robot. The trajectory is generated by interpolating the waypoints along the path. Furthermore, it interfaces with the robot controllers using the ros2_control.

The package to host all MoveIt2-related configurations and launchers is the panda_moveit, and has the following structure.

panda_moveit/
├── config/
│   ├── initial_positions.yaml
│   ├── joint_limits.yaml
│   ├── kinematics.yaml
│   ├── moveit_controllers.yaml
│   ├── moveit.rviz
│   ├── panda.ros2_control.xacro
│   ├── panda.srdf
│   ├── panda.urdf.xacro
│   ├── pilz_cartesian_limits.yaml
│   └── ros2_controllers.yaml
├── launch/
│   └── simulation.launch.py
├── CMakeLists.txt
└── package.xml

The configuration created for this example is composed by:

initial_positions.yaml: This is not a standard or built-in configuration file, but it is used to store predefined initial position values for each joint for the initialization.
joint_limits.yaml: Specifies the joint's limits (position, velocity, acceleration, and effort). In this example, it does not define a limit for acceleration, and the velocity limit is redundant information that, in this case, can be also found in the URDF model.
kinematics.yaml: Configures the kinematics solver plugin, resolution, timeout, and attempts.
moveit_controllers.yaml: Describes to MoveIt2 the available controllers, their associated joints, and the specific controller interface type used (FollowJointTrajectory or GripperCommand). It will use this configuration to automatically connect to, in this example, the ROS2 action interface.
moveit.rviz: Provides a predefined configuration to be loaded on the Rviz2.
panda.ros2_control.xacro: Defines the hardware interface commands and states, and the hardware plugin to be used with ros2_control.
panda.srdf: The SRDF (Semantic Robot Description Format) complements the URDF by defining joint groups, virtual joints, collision checking details, and additional transformation information.
panda.urdf.xacro: Expands the URDF model by including the panda.ros2_control.xacro to the panda.urdf that is provided by the moveit_resources_panda_description package.
pilz_cartesian_limits.yaml: Defines the cartesian limits to be employed when using the Pilz Industrial Motion Planner that provides a trajectory generation to standard robot motions, e.g., point-to-point, linear, and circular.
ros2_controllers.yaml: Provides the controllers’ configuration to be launched when running the ROS2 Control node. In this case, it defines that the robot will be controlled by a joint trajectory controller with position commands and position and velocity states.

The simulation.launch.py, which is presented below, loads the required configurations and launches all the nodes.

from launch import LaunchDescription
from launch.actions import OpaqueFunction, DeclareLaunchArgument, IncludeLaunchDescription
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

from moveit_configs_utils import MoveItConfigsBuilder

from srdfdom.srdf import SRDF

import yaml


def launch_setup(context, *args, **kwargs):

    # Initialize arguments
    can_interface_name = LaunchConfiguration("can_interface_name")

    # List of nodes to be started
    nodes_to_start = []

    # Get panda configs (defined in urdf.xacro, srdf, ros2_control.xacro)
    moveit_config = (
        MoveItConfigsBuilder("panda", package_name="panda_moveit")
        .robot_description(mappings={"can_interface_name": str(can_interface_name.perform(context))})
        .to_moveit_configs()
    )

    # Generate static virtual joint tfs
    for _, xml_contents in moveit_config.robot_description_semantic.items():
        srdf = SRDF.from_xml_string(xml_contents)
        for vj in srdf.virtual_joints:
            nodes_to_start.append(
                Node(
                    package="tf2_ros",
                    executable="static_transform_publisher",
                    name=f"static_transform_publisher_{vj.name}",
                    output="both",
                    arguments=[
                        "--frame-id",
                        vj.parent_frame,
                        "--child-frame-id",
                        vj.child_link,
                    ],
                )
            )

    # Given the published joint states, publish tf for the robot links
    nodes_to_start.append(
        Node(
            package="robot_state_publisher",
            executable="robot_state_publisher",
            respawn=True,
            output="both",
            parameters=[
                moveit_config.robot_description
            ],
        )
    )

    # Move Group
    nodes_to_start.append(
        Node(
            package="moveit_ros_move_group",
            executable="move_group",
            output="both",
            parameters=[
                moveit_config.to_dict(),
                {
                    "publish_robot_description_semantic": True,
                    "allow_trajectory_execution": True,
                    "capabilities": "",
                    "disable_capabilities": "",
                    "publish_planning_scene": True,
                    "publish_geometry_updates": True,
                    "publish_state_updates": True,
                    "publish_transforms_updates": True,
                    "monitor_dynamics": False,
                },
            ],
        )
    )

    # Controller manager
    ros2_controllers_config = PathJoinSubstitution(
        [FindPackageShare("panda_moveit"), "config", "ros2_controllers.yaml"]
    )

    nodes_to_start.append(
        Node(
            package="controller_manager",
            executable="ros2_control_node",
            output="both",
            parameters=[
                moveit_config.robot_description,
                ros2_controllers_config
            ]
        )
    )

    # Generate spawn controllers
    controller_names = moveit_config.trajectory_execution.get(
        "moveit_simple_controller_manager", {}
    ).get("controller_names", [])
    for controller in controller_names + ["joint_state_broadcaster"]:
        nodes_to_start.append(
            Node(
                package="controller_manager",
                executable="spawner",
                arguments=[controller],
                output="screen",
            )
        )

    # Rviz
    nodes_to_start.append(
        Node(
            package="rviz2",
            executable="rviz2",
            respawn=False,
            output="both",
            arguments=[
                "-d",
                PathJoinSubstitution(
                    [
                        FindPackageShare("panda_moveit"),
                        "config",
                        "moveit.rviz",
                    ]
                ),
            ],
            parameters=[
                moveit_config.planning_pipelines,
                moveit_config.robot_description_kinematics,
            ],
        )
    )

    # Launch slaves nodes

    slave_launch = PathJoinSubstitution(
        [FindPackageShare("canopen_fake_slaves"), "launch", "cia402_slave.launch.py"]
    )

    bus_config = PathJoinSubstitution(
        [FindPackageShare("panda_canopen"), "config", "cia402", "bus.yml"]
    )

    with open(bus_config.perform(context), 'r') as f:
        bus_config_dict = yaml.safe_load(f)

    joints = []
    for key in bus_config_dict.keys():
        if "joint" in key:
            joints.append(key)

    for joint in joints:
        nodes_to_start.append(
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(slave_launch),
                launch_arguments={
                    "node_id": str(bus_config_dict[joint]["node_id"]),
                    "node_name": f"slave_node_{joint}",
                    "slave_config": PathJoinSubstitution([
                         FindPackageShare("panda_canopen"), "config", "cia402", str(bus_config_dict[joint]["dcf"])
                    ]),
                    "can_interface_name": can_interface_name,
                }.items(),
            )
        )

    return nodes_to_start


def generate_launch_description():
    declared_arguments = []

    declared_arguments.append(
        DeclareLaunchArgument(
            "can_interface_name",
            default_value="vcan0",
            description="CAN interface name to run the master and, when in simulation, the fake slaves.",
        )
    )

    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

In more detail, it imports the configurations with the MoveItConfigsBuilder class, which will easily load the specific arguments to the nodes. Then, it configures the static_transform_publisher node (from the tf2_ros package) to publish the transform from world to the panda_link0. It also launches the robot_state_publisher node.

Then, the move_group node is configured and added to the list of nodes to be launched. This node works as an integrator. More details can be seen at [13].

Furthermore, it creates the ros2_control_node, which acts as a controller manager, and the spawner, which loads, configures, and starts a controller on startup. The ros2_control_node will launch the RobotSystem from the canopen_ros2_control package to act directly on the hardware using the CANopen interface. The CANopen configuration is hosted in the panda_canopen package and it is referenced in the panda.urdf.xacro.

Also, it adds the Rviz2 to the node list, and, since we are working in a simulated environment, the fake slaves for each joint are launched.

A demonstration of the system running can be seen below.

References

Links

ROS2 Humble Documentation: https://docs.ros.org/en/humble/index.htm
Robot Operating System 2: Design, Architecture, and Uses In The Wild: https://arxiv.org/abs/2211.07752
ROS 2 Foxy Fitzroy: Setting a new standard for production robot development: https://aws.amazon.com/blogs/robotics/ros-2-foxy-fitzroy-robot-development/
Example repository: https://github.com/lucasmluza/ros2_moveit_canopen_example
ROS2 CANopen Stack: https://ros-industrial.github.io/ros2_canopen/manual/rolling/
ROS Planning — Moveit Resources: https://github.com/ros-planning/moveit_resources
URDF XML Specifications: https://wiki.ros.org/urdf/XML
ROS2 Rviz: https://github.com/ros2/rviz
ROS2 CANopen: https://github.com/ros-industrial/ros2_canopen
Lely CANopen repository: https://gitlab.com/lely_industries/lely-core
ROS2 CANopen — Bus Configuration File: https://ros-industrial.github.io/ros2_canopen/manual/rolling/user-guide/configuration.html#bus-configuration-file
ROS2 CANopen — CiA402 Driver: https://ros-industrial.github.io/ros2_canopen/manual/rolling/user-guide/cia402-driver.html
MoveIt — The move_group node: https://moveit.picknik.ai/main/doc/concepts/move_group.html

Main Packages

panda_description:
- From: https://github.com/ros-planning/moveit_resources/tree/ros2/panda_description
- Branch: humble
- Commit: #5cf047b
- License: Apache-2.0
ros2_canopen:
- From: https://github.com/ros-industrial/ros2_canopen
- Branch: humble
- Commit: #e0b5706
- License: Apache-2.0
Added as a submodule.