[ROS 2 Humble] Removing Callback Groups from within Node
Hi ROS 2 Community,
i am facing a problem with the Callback Groups.
Within my Node i create Objects within a std::vector
. This Objects themself create Publishers with their own CallbackGroup
class CBGObject
{
public:
rclcpp::CallbackGroup::SharedPtr cbg_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_pose_;
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_init_;
CBGObject(rclcpp_lifecycle::LifecycleNode * node){
cbg_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::PublisherOptions pub_options;
pub_options.callback_group = cbg_;
pub_pose_ = node_->create_publisher<nav_msgs::msg::Odometry>("/pose", 1, pub_options);
rclcpp::SubscriptionOptions sub_options;
sub_options.callback_group = cbg_;
sub_init_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("/initialpose", 1, std::bind(&CBGObject::callbackPose, this, std::placeholders::_1), sub_options);
}
If i am now erasing the object from my std::vector
i get a Segmentation Fault with
(gdb) bt
#0 0x00007f6999b1a9cf in rmw_fastrtps_shared_cpp::__rmw_wait(char const*, rmw_subscriptions_s*, rmw_guard_conditions_s*, rmw_services_s*, rmw_clients_s*, rmw_events_s*, rmw_wait_set_s*, rmw_time_s const*) () from /opt/ros/humble/lib/librmw_fastrtps_shared_cpp.so
#1 0x00007f6999b71707 in rmw_wait () from /opt/ros/humble/lib/librmw_fastrtps_cpp.so
#2 0x00007f699b139718 in rcl_wait () from /opt/ros/humble/lib/librcl.so
#3 0x00007f699b6f0dbc in rclcpp::Executor::wait_for_work(std::chrono::duration<long, std::ratio<1l, 1000000000l> >) () from /opt/ros/humble/lib/librclcpp.so
#4 0x00007f699b6f3ab3 in rclcpp::Executor::get_next_executable(rclcpp::AnyExecutable&, std::chrono::duration<long, std::ratio<1l, 1000000000l> >) () from /opt/ros/humble/lib/librclcpp.so
#5 0x00007f699b6fa8e2 in rclcpp::executors::MultiThreadedExecutor::run(unsigned long) () from /opt/ros/humbl/lib/librclcpp.so
#6 0x00007f699b2dc2b3 in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#7 0x00007f699ae94b43 in start_thread (arg=<optimized out>) at ./nptl/pthread_create.c:442
#8 0x00007f699af26a00 in clone3 () at ../sysdeps/unix/sysv/linux/x86_64/clone3.S:81
Changing the code to not use a external cbg but rather use the nullptr
standard cbg does not crash the program.
Therefore i think the segmentation fault occurs as the Executor wants to access the Pointer to the cbg but it does not exist anymore is it was erased with the object.
As i am using a MultiThreaded Executor and the Code can run in parallel i would like to use the cbg group functionality, is there any option to remove the cbg from the executor / or out of the node again? I think form the executor point of view this could be possible but if i want to compose the node i don't even have the executor exposed to me, and with the standard node process (https://github.com/ros2/examples/blob...) the executor is within the main function and spins, i think it would be a big effort to expose every cbg which is created to the main function and check if any of them should be removed.