ROS2 Controller parameter server

edit

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();
}