[ROS2] How to define a thread (control loop) along with subscriber's callbacks with the new rclcpp::spin()?

asked 2023-02-01 10:39:00 -0500

iopoi97
23 ●6 ●7 ●9

updated 2023-02-02 02:10:24 -0500

Hello, everyone, I'm using ROS2 Galactic and I'm trying to define a control loop function that has to be periodic. In ROS1 I was using boost:thread and then ros.spin() to have all the callbacks to be executed, but now this procedure seems not to work because rclcpp::spin() requires parameters. My situation is like that: My Class:

class ROBOT_INVDYN : public rclcpp::Node{
    public:
        //some stuff WITH SUBSCRIBERS...
        void ctrl_loop();

    private:
        //other stuff...
};

the void ctrl_loop() implementation:

void ROBOT_INVDYN::ctrl_loop() {
       //INITIALIZATIONS...
        rclcpp::Rate r(250);
         while( rclcpp::ok() ) {        
               //PROPER CONTROL LOOP...
            r.sleep();
          }
 }

then my main

int main (int argc, char * argv[]){
   rclcpp::init(argc, argv);
   auto robot_node = std::make_shared<ROBOT_INVDYN>();
//instruction that should make the ctrl loop as a periodic thread but doesn't work: so it's just an attempt
       std::thread control_loop_t(robot_node->ctrl_loop); 
       rclcpp::spin(robot_node);
       rclcpp::shutdown();
    return 0;
    }

In ros 1 I would have used a class function run():

    void ROBOT_INVDYN::run() {
    boost::thread ctrl_loop_t ( &ROBOT_INVDYN::ctrl_loop, this);
    ros::spin();    
}

and then modify the main just to instanciate the class and call the run() function. What is the right way to make a similar thing in ros2 with the new rclcpp::spin()?

edit retag flag offensive close merge delete

add a comment

answered 2023-02-08 04:49:38 -0500

jrtg
145 ●3 ●14 ●15

Hello,

If you just want ctrl_loop() to be executed periodically, create a timer in your node with ctrl_loop() as its callback function, see the publisher node tutorial as an example.

If you want ctrl_loop() to be executed upon receiving new values (e.g. joint values or cartesian pose), create a publisher node that publishes the values periodically, and a listener node that processes the values in its subscription callback, see the listener node tutorial. The publisher node's frequency will be driving the periodicity of the processing node.

If you want to prioritize callbacks (e.g. higher priority control loop vs lower priority setpoint generator) you will want to look at callback groups. See also ROS 2 Executor: How to make it efficient, real-time and deterministic?

edit flag offensive delete link

Comments

What if the loop may not have a fixed period? I want the block inside the loop to restart again immediately after it finishes.

xibeisiber ( 2023-02-23 04:10:40 -0500 )edit

Can you describe your use case a bit more in detail? It is hard to give an answer as I don't really see the use case.

If you want to process data, maybe this could be implemented as a subscription callback, which will be called each time new data is published. So if you publish at a higher rate than the callback can process, the callback will continuously be called.

Or maybe you could implement a timer with a period that is smaller than the duration of your callback, so it is instantly retriggerd.

In both cases, you will want to use callback groups and a multithreaded executor, so your continuous task can run parallel to possible other subscriptions, timers, etc.

jrtg ( 2023-02-24 06:25:31 -0500 )edit

One case I encounter is to run two loops at the same time

let the manipulator moves from pose1->pose2->pose3---->pose1->pose2->pose3---->pose1.... Moving to each pose will call a ros2 server I write in another node.
let the AGV move forward / backward periodically.

Since each loop will take several minutes, and the period may differ due to the communication latency and the unstable navigation time, I tend to not set a timer with a period.

Currently I have to separate the manipulator movement and the AGV movement into two nodes. In each node, there is a

```

void run(){

while(rclcpp::ok()){

movement;

}

} ```

In the main function: ```

rclcpp::init(argc, argv);

std::shared_ptr<ManinulatorRun> node1 = std::make_shared<ManinulatorRun>("armloopnode");

node1->run();

rclcpp::shutdown();

```

and

```

rclcpp::init(argc, argv);

std::shared_ptr<AGVRun> node2 = std::make_shared<AGVRun>("agvloopnode");

node2->run();

rclcpp::shutdown();

```

xibeisiber ( 2023-02-25 03:57:33 -0500 )edit

The problem is that when I try to terminate the node using ctrl+C, it will output a lot message..

[rlrob_task_test_node-1] terminate called after throwing an instance of 'rclcpp::exceptions::RCLError' [rlrob_task_test_node-1] what(): failed to create guard condition: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/guard_condition.c:67 [rlrob_task_test_node-1] Stack trace (most recent call last) in thread 17276: [rlrob_task_test_node-1] #31 Source "/usr/include/c++/11/bits/std_thread.h", line 260, in operator() [0x55ffca1d3605] ...

BTW： how to display multi line codes...

xibeisiber ( 2023-02-25 05:35:54 -0500 )edit

add a comment

[ROS2] How to define a thread (control loop) along with subscriber's callbacks with the new rclcpp::spin()?

1 Answer

Comments

Question Tools

Stats

Related questions

[ROS2] How to define a thread (control loop) along with subscriber's callbacks with the new rclcpp::spin()? edit

1 Answer

Comments

Question Tools

Stats

Related questions

[ROS2] How to define a thread (control loop) along with subscriber's callbacks with the new rclcpp::spin()?