ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Turtlesim /spawn service error : rcl node's context is invalid

asked 2021-11-10 12:06:20 -0600

The Ros-A-Tron gravatar image

updated 2021-11-10 15:22:55 -0600

I am using ros2 foxy, and I get this error when trying to call the /spawn service from the turtlesim_node :

"terminate called after throwing an instance of 'rclcpp::exceptions::RCLError' what(): could not create client: rcl node's context is invalid, at /tmp/binarydeb/ros-foxy-rcl-1.1.11/src/rcl/node.c:441"

Does anyone has an idea about this error ?

Here is my code :

#include "rclcpp/rclcpp.hpp"
#include "turtlesim/srv/spawn.hpp"

class MyturtleSpawner : public rclcpp::Node {
public:
    MyturtleSpawner() : Node("turtle_spawner"){
    threadV_.push_back(std::thread(std::bind(&MyturtleSpawner::callSpawnClient, this, 1.0, 1.0, 0.0)));
    }
    void callSpawnClient(float, float, float);

private:
    std::vector<std::thread> threadV_;
};
void MyturtleSpawner::callSpawnClient(float x, float y, float theta){
    auto spawnClient = this->create_client<turtlesim::srv::Spawn>("spawn");
    while (!spawnClient->wait_for_service(std::chrono::seconds(1))){
        RCLCPP_INFO(this->get_logger(), "Waiting for the Spawn server to respond ...");
    }

    auto spawnRequest = std::make_shared<turtlesim::srv::Spawn::Request>();
    spawnRequest->x = x;
    spawnRequest->y = y;
    spawnRequest->theta = theta;

    auto future = spawnClient->async_send_request(spawnRequest);

    try{
        auto spawnResponse = future.get();
        RCLCPP_INFO(this->get_logger(), "Turtle Spawned Successfully : %s", spawnResponse->name);
    }
    catch(const std::exception& e){
        RCLCPP_ERROR(this->get_logger(), "Spawn service call failed !");
    } 
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<MyturtleSpawner>();
    rclcpp::shutdown();
    return 0;
}
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2021-11-10 17:18:38 -0600

Geoff gravatar image

You need to give the node some processing time. Because you are creating a node here:

auto node = std::make_shared<MyturtleSpawner>();

And then immediately calling rclcpp::shutdown() on the next line, by the time the thread gets a chance to run and create the client, the ROS context has been cleaned up.

You need to create an executor and spin the node after creating it and before calling rclcpp::shutdown(). See this tutorial, and look at the main() function. It has a call to rclcpp::spin() after creating the node.

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2021-11-10 12:04:36 -0600

Seen: 2,473 times

Last updated: Nov 10 '21