0
$\begingroup$

i'm trying to transform ego-planner-swarm founded in:

https://github.com/ZJU-FAST-Lab/ego-planner-swarm

to ros2...

the problem was in one of the subscription's callbacks there was a function in which there was a "while loop" with only delay in it... and this loop's condition was dependent on other timers callbacks... and if the other timers callbacks wouldn't be called, the code would stuck in while loop. in ros1 to slove this kind of problem we can use

ros::spinOnce();

in the loop when the node is already spinning and that would prevent the code to stuck in the "while loop" but in ros 2 if the the node is already spinning and you use

rclcpp::spin_some(node);

in the loop, you would get

terminate called after throwing an instance of 'std::runtime_error'

what(): Node has already been added to an executor.

that's because in ros2 when spin commands which get nodes as input argument are called, they automatically add node to an executor...

the second thing i tried was spinning the node using SingleThreadedExecutor and passed the Executor Instance to the while loop. then i used spin commands with using the Executor Instance in the "while loop"... things like:

Executor.spin();
Executor.spin_some();
Executor.sp_once();

this way i was not adding the node to an Executor... but i was trying to spin a node which is already spinning! and i got :

terminate called after throwing an instance of 'std::runtime_error'

what(): spin_once() called while already spinning

then how i can fix this problem?

$\endgroup$
2
  • $\begingroup$ As @rubimat already answered: you add a node to an executor to spin it. 'Spinning a node' means that the executor waits for something to happen (receiving a message, service call, timer period lapsed, etc.) and then calls the corresponding callback. You cannot call spin_once() from a callback. If you want a better answer, you need to adapt your question to clarify what functionality you want to achieve, and then we can write an answer on how to best achieve that. But in its current form, the only answer is: "It is not possible.". $\endgroup$
    – JRTG
    Dec 13, 2023 at 11:26
  • $\begingroup$ You have edited your question and placed the solution inside it. This is not how a Q&A site, such as Stack Exchange works. Please edit and remove the solution from your question and post the solution as an answer in the answer section below. Also, please remove the SOLVED from the title, Stack Exchange already has a mechanism for that, which is marking an answer as the accepted one. Finally, please take the tour to better understand how SE works. Thanks. $\endgroup$ Dec 21, 2023 at 23:42

3 Answers 3

0
$\begingroup$

I guess that the functionality you are looking for, is the following:

A node that:

  • Runs a blocking while loop in one of its callbacks,
  • While at the same time also keeps responding to other events (such as receiving messages, changing paramters, etc).

You can do this by:

  • Running the node in a multithreaded executor,
  • Defining callback groups:
    • Either assign all callbacks to a Reentrant Callback group (if it's ok to run them concurrently), instead of the default (= MutuallyExclusive) callback group, or
    • Use (at least) two callback groups: one for the blocking callback and one (or multiple) for the other callbacks.

Callback groups work as follows:

  • Callbacks belonging to different callback groups can be called concurrently (as long as there are available threads in the multithreaded executor),
  • Callback groups belonging to the same MutuallyExclusive callback group are not called concurrently,
  • Callback groups belonging to the same Reentrant callback group can be called concurrently (as long as there are available threads in the multithreaded executor).

Note that if you run a node with concurrent callbacks, you need to take thread safety into account. E.g. you cannot just write and read the same variables concurrently from different threads.

If this solves your issue, please upvote and accept. If it does not: refer to my comment: you will need to clarify you question as to what functionality exactly you try to achieve, rather than asking "how do I call spin_some() in a node that is already being spun".

$\endgroup$
2
  • $\begingroup$ thank you, this exactly what i wanted(callback groups)... the only thing that is different, is that the blocking while loop is not part of a callback $\endgroup$ Dec 13, 2023 at 14:35
  • $\begingroup$ but i have to say the "loop while" is in a function which is called by one of the subscription's callback so i would say functionality you described is exactly what i needed. $\endgroup$ Dec 16, 2023 at 6:21
1
$\begingroup$

what solved it for me was Callback Groups in ros2 which user @JRTG introduced...(https://docs.ros.org/en/iron/How-To-Guides/Using-callback-groups.html)

now i will put my code and explain how Callback Groups Solved it.

using namespace ego_planner;

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("ego_planner_node");

  EGOReplanFSM rebo_replan;
  
  rclcpp::executors::MultiThreadedExecutor executor;
  rebo_replan.init(node);
  executor.add_node(node);
  executor.spin();
  rclcpp::shutdown();
  return 0;
}

here i make MultiThreadedExecutor Instance and spin the Node with it. i'm using MultiThreadedExecutor because in order to use Callback Groups Correctly one should use MultiThreadedExecutor based on the guide found in : https://docs.ros.org/en/iron/How-To-Guides/Using-callback-groups.html

namespace ego_planner
{

  void EGOReplanFSM::init(rclcpp::Node::SharedPtr node)
  {
    .
    .
    .
    auto target_callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

    rclcpp::SubscriptionOptions options;
    options.callback_group = target_callback_group;
    waypoint_sub_ = node->create_subscription<geometry_msgs::msg::PoseStamped>(
    "/move_base_simple/goal", 1,
    [this, node](const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
    waypointCallback(msg, node);}, options);
    .
    .
    .
  }//EGOReplanFSM::init

  void EGOReplanFSM::waypointCallback(const geometry_msgs::msg::PoseStamped:EGOReplanFSM::init:SharedPtr &msg,rclcpp::Node::SharedPtr node)
  {
    .
    .
    .
    planNextWaypoint(end_wp,node);
  }//EGOReplanFSM::waypointCallback

void EGOReplanFSM::planNextWaypoint(const Eigen::Vector3d next_wp,rclcpp::Node::SharedPtr node)
  {
      .
      .
      .
      while (rclcpp::ok() &&exec_state_ != EXEC_TRAJ)
          {
               rclcpp::sleep_for(std::chrono::milliseconds(1));
          }
      .
      .
      .
  }//EGOReplanFSM::planNextWaypoint

}//ego_planner

in EGOReplanFSM::init function i have a subscription with the name of "/move_base_simple/goal"...this subscription's callback name is 'waypointCallback' and in it there is another function with the name of 'planNextWaypoint' and in this function lies the problematic "while loop":

while (rclcpp::ok() &&exec_state_ != EXEC_TRAJ)
        {
          rclcpp::sleep_for(std::chrono::milliseconds(1));
        }

by creating a callback group by the name of 'target_callback_group' and assigning "/move_base_simple/goal" Subscriber to it, i was able to solve this "while loop" deadlock... in this code i have two callback groups... one is 'target_callback_group' and its for "/move_base_simple/goal" subscriber and second is the node's default callback group for all the other callbacks...(when you don't specify a callback group, ros2 uses node's default callback group) and both of these callback groups are MutuallyExclusive.

i tried to explain my problem and what solved it to the best of my abilities... i hope this help others as it helped me(sorry if my writing is not good, English is not my first language)

$\endgroup$
0
$\begingroup$

The call rclcpp::spin creates an executor under the hood. The node can not be executed by two executors at the same time.

Equivalent to an rclcpp::spin call would be to add your node to and executor and spin the executor.

rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
executor.spin();

You could then use the same executor instance in your while loop. Note that a spin call is blocking. You might want to redesign your implementation a little by just using the while loop instead.

For more information about executors, please take a look at the official documentation.

$\endgroup$
4
  • $\begingroup$ thank you for your answer... i used executor.spin(); and then passed the executor instance to the while loop... but this time i get the "while already spinning"... i test with spin() and spin_once() and spin_some() $\endgroup$ Dec 11, 2023 at 5:28
  • $\begingroup$ Could you update your question with your newest code, i.e. a liitle example so recreate the issue? $\endgroup$
    – rubimat
    Dec 12, 2023 at 7:09
  • $\begingroup$ of course, i just updated my code... this time i used SingleThreadedExecutor to spin the code... but i get "node already spinning" when using executor.spin_once(); in the while loop $\endgroup$ Dec 12, 2023 at 14:45
  • $\begingroup$ So you cannot have both, spin() and spin_once(). Either you spin it with spin() or your have your while loop woth spin_once() $\endgroup$
    – rubimat
    Dec 13, 2023 at 19:11

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.