ROS2 Controller parameter server

asked 2019-08-19 05:25:46 -0500

diameda_boot gravatar image

updated 2019-08-19 11:34:09 -0500

ROS version: Dashing

I was trying to perform the demo using the code snippet given in the README.md in ros2_control. However i kept running into issues with the parameter server with the following message

[INFO] [my_robot_joint_trajectory_controller]: parameter server not available

This causes the controller to fail to configure and as such could not be used. To my knowledge, there is no global parameter server in ROS2, so I am unsure if I need to create another parameter server or there is some issue with the parameter client.

Side note:

The failure occurs in line 115 of ros2_controllers/JointTrajectoryController at the parameters_client_->wait_for_service(1s) which always returns false even with longer time given. This causes the message above to be displayed through the code in line 138

The parameter client used is defined in ros2_control/controller_interface.hpp line 64, with the initialisation being found here in lines 50-55.

My code is as such:

#include <memory>
#include <iostream>
#include <rclcpp/executors/multi_threaded_executor.hpp>
#include <rclcpp/executors/single_threaded_executor.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/rate.hpp>
#include "controller_manager/controller_manager.hpp"
#include "my_robot.hpp"

using rclcpp::executors::MultiThreadedExecutor;
using myrobots_hardware_interface::MyRobot;
using rclcpp::Rate;

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

int main(int argc, char** argv)
{
// do all the init stuff
rclcpp::init(argc,argv);
Rate r(2.0);
bool active = true;

// 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 hardware\n");
    return -1;
}
// MultiThreadedExecutor executor1;
// executor1.add_node(master_node);
auto executor =
    std::make_shared<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();
}
edit retag flag offensive close merge delete

Comments

1

There is currently WIP for dashing. https://github.com/ros-controls/ros2_... The issue you mentioned is pretty much was I was referring to in the linked comment. The parameter server was developed at a time of ROS2 where there was no node-specific parameters yet. So for dashing, I actually don't really see a need for that global parameter server any longer.

Karsten gravatar imageKarsten ( 2019-08-19 11:41:54 -0500 )edit