ROS2 Humble中自定义服务回调内嵌套调用GetMotionSequence服务的阻塞与响应延迟问题求助
我现在在ROS2 Humble环境下,想基于MoveGroupSequence教程实现一个自定义服务节点——这个节点的作用是接收自定义格式的请求,把它转换成GetMotionSequence服务要求的MotionSequenceRequest格式,再调用MoveIt的GetMotionSequence服务完成轨迹规划。但现在卡在了服务调用的等待环节,试了好几种方法都有问题,具体情况如下:
问题场景与尝试过程
第一种尝试:同步等待future结果(死循环卡timeout)
在自定义服务的回调函数里,我先完成了请求格式的转换,然后调用GetMotionSequence的异步客户端发送请求,接着用std::future::wait_for循环等待结果。但程序一直卡在timeout分支,永远到不了ready状态,GetMotionSequence的服务结果根本没被处理。关键代码片段:
void my_custom_callback(const std::shared_ptr<MyCustomService::Request> msg, std::shared_ptr<MyCustomService::Response> success){ // 将自定义请求转换为MotionSequenceRequest的逻辑 ... // 调用GetMotionSequence服务 auto motion_sequence_service_future = motion_sequence_service_client_->async_send_request( motion_sequence_service_request); // 等待服务结果(此处出现问题) std::future_status service_status; do { switch (service_status = motion_sequence_service_future.wait_for(std::chrono::seconds(1)); service_status) { case std::future_status::deferred: RCLCPP_ERROR(LOGGER, "Deferred"); break; case std::future_status::timeout: // 程序一直卡在这个分支 RCLCPP_INFO(LOGGER, "Waiting for trajectory plan..."); break; case std::future_status::ready: RCLCPP_INFO(LOGGER, "Service ready!"); break; } } while (service_status != std::future_status::ready); ... }第二种尝试:用spin_until_future_complete(线程冲突)
我查资料后尝试用rclcpp::spin_until_future_complete()来等待结果,但因为当前回调本身就在主执行器的spin()循环里运行,直接调用这个函数会触发线程重复spin的冲突问题,根本跑不起来。第三种尝试:带lambda回调的异步调用(响应返回太晚)
后来我改用带lambda回调的async_send_request重载版本,这样不会死锁,轨迹规划也能成功完成,但自定义服务的响应返回得太晚——lambda里给success赋值的时候,自定义服务的回调已经执行完毕,请求端拿不到正确的响应结果。代码片段:
motion_sequence_service_client_->async_send_request( motion_sequence_service_request, [success](rclcpp::Client<GetMotionSequence>::SharedFuture future){ auto response = future.get(); // 处理轨迹规划结果,给自定义服务的响应赋值 ... } );
我的理解
我大概猜到问题根源是:自定义服务的回调是在ROS执行器的线程里运行的,而GetMotionSequence的服务响应也需要通过同一个执行器来处理。如果我在回调里用阻塞式的等待,会占住执行器线程,导致响应消息根本没法被处理,所以永远等不到ready状态;但用异步lambda的话,自定义服务的响应又没法及时返回。
现在实在卡在这里好几天了,有没有大佬能指点一下,怎么在自定义服务的回调里安全地调用GetMotionSequence服务,既能拿到规划结果,又能及时返回自定义服务的响应?
内容来源于stack exchange

