ros2_control

CI

linux:

Disclaimer

ros2_control is a proof of concept on how new features within ROS 2 can be elaborated and used in the context of robot control. We were keeping the vocabulary close the ROS 1 implemenation, however this is with no notion a migration, but rather a fresh new-write. We hope that this can be a starting point for migrating ros_control to ROS 2 at some point. Reasons for re-writing this repo from scratch rather than porting existing code to ROS 2 syntax is to leverage the new ROS 2 concepts in full. The work done in this repo (together with ros2_controllers) was presented at ROSCon 2017 and is currently not under active development.

Getting Started

In order to be able to compile these two repos, a complete ROS 2 installation is necessary. Please find instructions on how to install ROS 2 here. At the time of writing, there exist binaries for ROS 2 (Bouncy Bolson) for all three major operating systems as well as detailed installation instructions when compiling from source.

Once ROS 2 is successfully installed, an overlay workspace can be created for the ros2_control packages.

$ mkdir -p ~/ros2_control_ws/src
$ cd $_
$ git clone https://github.com/ros-controls/ros2_control.git
$ git clone https://github.com/ros-controls/ros2_controlllers.git

We can then compile the overlay workspace. For this we first have to source the ROS 2 installation. In this case, we source the setup.bash from the Bouncy binary distribution for Ubuntu. Obviously, the path to the setup file is different on each platform and thus has to be adjusted. Once ROS 2 is sourced, we can compile the ros2_control packages.

$ cd ~/ros2_control_ws
$ source /opt/ros/bouncy/setup.bash # this has to be adjusted for ROS-Distro and/or OS
$ colcon build

Controller Architecture

There are currently two controller available:

• JointStateController
• JointTrajectoryController

Both can be loaded through the controller manager from the ament_resource_index.

What’s new in ROS 2 are Managed Nodes. That means, every LifecycleNode adheres to an underlying state machine as described in the linked wiki page. We feature this for the controller interface in this work and thus every controller implicitely has means to start and stop the controller. Similar to the ROS 1 implementation, each controller has to implement two functions: init and update. We refer to the ROS 1 wiki for a general overview.

Writing a demo for your own robot

Similar as to the ROS 1 implementation, every hardware platform has to implement the robot_hardware_interface Essentially, these three functions are required for every hardware and have to be implemented in a hardware-dependent manner.

  HARDWARE_INTERFACE_PUBLIC
  virtual
  hardware_interface_ret_t init() = 0;

  HARDWARE_INTERFACE_PUBLIC
  virtual
  hardware_interface_ret_t read() = 0;

  HARDWARE_INTERFACE_PUBLIC
  virtual
  hardware_interface_ret_t write() = 0;

One possible example could be:

hardware_interface::hardware_interface_ret_t
MyRobot::init()
{
  auto joint_names = {
    "my_robot_joint_1",
    "my_robot_joint_2",
  };
  size_t i = 0;
  for (auto & joint_name : joint_names) {
    hardware_interface::JointStateHandle state_handle(joint_name, &pos_[i], &vel_[i], &eff_[i]);
    joint_state_handles_[i] = state_handle;
    if (register_joint_state_handle(&joint_state_handles_[i]) != hardware_interface::HW_RET_OK) {
      throw std::runtime_error("unable to register " + joint_state_handles_[i].get_name());
    }

    hardware_interface::JointCommandHandle command_handle(joint_name, &cmd_[i]);
    joint_command_handles_[i] = command_handle;
    if (register_joint_command_handle(&joint_command_handles_[i]) !=
      hardware_interface::HW_RET_OK)
    {
      throw std::runtime_error("unable to register " + joint_command_handles_[i].get_name());
    }
    ++i;
  }
}

hardware_interface::hardware_interface_ret_t
MyRobot::read()
{
  // do robot specific stuff to update the pos_, vel_, eff_ arrays
}

hardware_interface::hardware_interface_ret_t
MyRobot::write()
{
  // do robot specific stuff to apply the command values from cmd_ to the robot
}

This robot hardware can then be loaded through the controller manager:

void
spin(std::shared_ptr<rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor> exe)
{
  exe->spin();
}

int main()
{
  // do all the init stuff
  
  // create my_robot instance
  auto my_robot = std::make_shared<MyRobot>();
  
  // initialize the robot
  if (my_robot->init() != hardware_interface::HW_RET_OK) {
    fprintf(stderr, "failed to initialized yumi hardware\n");
    return -1;
  }
  
  auto executor =
    std::make_shared<rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor>();
  
  // start the controller manager with the robot hardware
  controller_manager::ControllerManager cm(my_robot, executor);
  // load the joint state controller.
  // "ros_controllers" is the resource index from where to look for controllers
  // "ros_controllers::JointStateController" is the class we want to load
  // "my_robot_joint_state_controller" is the name for the node to spawn
  cm.load_controller(
    "ros_controllers",
    "ros_controllers::JointStateController",
    "my_robot_joint_state_controller");
  // load the trajectory controller
  cm.load_controller(
    "ros_controllers",
    "ros_controllers::JointTrajectoryController",
    "my_robot_joint_trajectory_controller");

  // there is no async spinner in ROS 2, so we have to put the spin() in its own thread
  auto future_handle = std::async(std::launch::async, spin, executor);

  // we can either configure each controller individually through its services
  // or we use the controller manager to configure every loaded controller
  if (cm.configure() != controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS) {
    RCUTILS_LOG_ERROR("at least one controller failed to configure")
    return -1;
  }
  // and activate all controller
  if (cm.activate() != controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS) {
    RCUTILS_LOG_ERROR("at least one controller failed to activate")
    return -1;
  }

  // main loop
  hardware_interface::hardware_interface_ret_t ret;
  while (active) {
    ret = my_robot->read();
    if (ret != hardware_interface::HW_RET_OK) {
      fprintf(stderr, "read failed!\n");
    }

    cm.update();

    ret = my_robot->write();
    if (ret != hardware_interface::HW_RET_OK) {
      fprintf(stderr, "write failed!\n");
    }

    r.sleep();
  }

  executor->cancel();
}

Contributing Guidelines

Thank you for your interest in contributing to ros2_control. Whether it’s a bug report, new feature, correction, or additional documentation, we greatly value feedback and contributions from our community.

Please read through this document before submitting any issues or pull requests to ensure we have all the necessary information to effectively respond to your bug report or contribution.

Reporting Bugs/Feature Requests

We welcome you to use the GitHub issue tracker to report bugs or suggest features.

When filing an issue, please check existing open, or recently closed, issues to make sure somebody else hasn’t already reported the issue. Please try to include as much information as you can. Details like these are incredibly useful:

• A reproducible test case or series of steps
• The version of our code being used
• Any modifications you’ve made relevant to the bug
• Anything unusual about your environment or deployment

Contributing via Pull Requests

Contributions via pull requests are much appreciated. Before sending us a pull request, please ensure that:

1. You are working against the latest source on the master branch.
2. You check existing open, and recently merged, pull requests to make sure someone else hasn’t addressed the problem already.
3. You open an issue to discuss any significant work - we would hate for your time to be wasted.

To send us a pull request, please:

1. Fork the repository.
2. Modify the source; please focus on the specific change you are contributing. If you also reformat all the code, it will be hard for us to focus on your change.
3. Ensure local tests pass.
4. Commit to your fork using clear commit messages.
5. Send a pull request, answering any default questions in the pull request interface.
6. Pay attention to any automated CI failures reported in the pull request, and stay involved in the conversation.

GitHub provides additional documentation on forking a repository and creating a pull request.

Finding contributions to work on

Looking at the existing issues is a great way to find something to contribute on. As this project, by default, uses the default GitHub issue labels (enhancement/bug/duplicate/help wanted/invalid/question/wontfix), looking at any ‘help wanted’ issues is a great place to start.

Licensing

Any contribution that you make to this repository will be under the Apache 2 License, as dictated by that license:

5. Submission of Contributions. Unless You explicitly state otherwise,
   any Contribution intentionally submitted for inclusion in the Work
   by You to the Licensor shall be under the terms and conditions of
   this License, without any additional terms or conditions.
   Notwithstanding the above, nothing herein shall supersede or modify
   the terms of any separate license agreement you may have executed
   with Licensor regarding such Contributions.