Using realtime_tools Action Server
I have troubles sending action feedback and result from ros_control plugin using realtime_tools' realtime_server_goal_handle.
I followed https://github.com/ros-controls/ros_c....
In the header I define the following types and rt_active_goal_
property:
typedef realtime_tools::RealtimeServerGoalHandle<MyAction> RealtimeGoalHandle;
typedef boost::shared_ptr<RealtimeGoalHandle> RealtimeGoalHandlePtr;
RealtimeGoalHandlePtr rt_active_goal_;
My goal callback function includes (with GoalHandle gh):
if (goal_ok) {
RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh));
gh.setAccepted("Goal accepted");
rt_active_goal_ = rt_goal;
} else {
ActionResult result;
result.fully_succeeded = false;
gh.setRejected(result);
}
goal_handle_timer = controller_nh_.createTimer(period, &RealtimeGoalHandle::runNonRealtime, rt_goal);
goal_handle_timer_.start();
In update callback I have:
if (rt_active_goal_) {
rt_active_goal_->preallocated_feedback_->... = ...
rt_active_goal_->setFeedback(rt_active_goal_->preallocated_feedback_);
}
if ( arrived_to_goal ) {
RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
if (rt_active_goal_) {
rt_active_goal_->preallocated_result_->fully_succeeded = true;
rt_active_goal_->setSucceeded(current_active_goal->preallocated_result_);
rt_active_goal_.reset();
}
}
However, I cannot get any messages when subscribed to result
and feedback
. If I send feedback/result though normal goal handle, e.g.:
rt_active_goal_->gh_.setSucceeded(*rt_active_goal_->preallocated_result_);
I can receive the message. I don't believe this is the intended way. Is there some other trick to use rt_active_goal_->setSucceeded() and rt_active_goal_->setFeedback() directly?