Skip to content

Commit 372f2b8

Browse files
MikeWrockMikeWrock
committed
Add trajectory pausable behaviour
1 parent c737470 commit 372f2b8

File tree

4 files changed

+305
-1
lines changed

4 files changed

+305
-1
lines changed

‎CMakeLists.txt

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@ project(experimental_behaviors CXX)
44
find_package(moveit_studio_common REQUIRED)
55
moveit_studio_package()
66

7-
set(THIS_PACKAGE_INCLUDE_DEPENDS geometry_msgs moveit_studio_behavior_interface pluginlib moveit_studio_common behaviortree_cpp tl_expected)
7+
set(THIS_PACKAGE_INCLUDE_DEPENDS geometry_msgs moveit_studio_behavior_interface pluginlib moveit_studio_common behaviortree_cpp tl_expected control_msgs)
88
foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
99
find_package(${package} REQUIRED)
1010
endforeach()

‎include/experimental_behaviors/execute_tracked_trajectory.hpp

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
#pragma once
2+
3+
#include <chrono>
4+
#include <string>
5+
#include <vector>
6+
#include <future>
7+
#include <memory>
8+
#include <atomic>
9+
10+
#include <rclcpp_action/rclcpp_action.hpp>
11+
#include <std_srvs/srv/trigger.hpp>
12+
13+
#include <control_msgs/action/follow_joint_trajectory.hpp>
14+
#include <trajectory_msgs/msg/joint_trajectory.hpp>
15+
#include <trajectory_msgs/msg/joint_trajectory_point.hpp>
16+
17+
#include <moveit_studio_behavior_interface/async_behavior_base.hpp>
18+
#include <moveit_studio_behavior_interface/get_required_ports.hpp>
19+
#include <moveit_studio_behavior_interface/get_optional_ports.hpp>
20+
#include <tl_expected/expected.hpp>
21+
22+
using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
23+
24+
namespace moveit_studio::behaviors
25+
{
26+
27+
class ExecuteAndTrackJointTrajectory : public AsyncBehaviorBase
28+
{
29+
public:
30+
ExecuteAndTrackJointTrajectory(
31+
const std::string & name,
32+
const BT::NodeConfiguration & config,
33+
const std::shared_ptr<BehaviorContext> & shared_resources);
34+
~ExecuteAndTrackJointTrajectory() override;
35+
36+
static BT::PortsList providedPorts();
37+
38+
std::shared_future<tl::expected<bool, std::string>>& getFuture() override;
39+
40+
protected:
41+
tl::expected<bool, std::string> doWork() override;
42+
tl::expected<void, std::string> doHalt() override;
43+
44+
private:
45+
void handleTrigger(const std::shared_ptr<std_srvs::srv::Trigger::Response> res);
46+
void processFeedback(const std::shared_ptr<const FollowJointTrajectory::Feedback> feedback);
47+
trajectory_msgs::msg::JointTrajectory computeRemainderTrajectory();
48+
49+
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr trigger_service_;
50+
rclcpp_action::Client<FollowJointTrajectory>::SharedPtr action_client_;
51+
rclcpp_action::ClientGoalHandle<FollowJointTrajectory>::SharedPtr goal_handle_;
52+
trajectory_msgs::msg::JointTrajectory full_trajectory_;
53+
double last_feedback_time_{0.0};
54+
std::atomic<bool> triggered_{false};
55+
std::shared_future<tl::expected<bool, std::string>> future_;
56+
};
57+
58+
} // namespace moveit_studio::behaviors

‎src/execute_tracked_trajectory.cpp

Lines changed: 242 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,242 @@
1+
#include "experimental_behaviors/execute_tracked_trajectory.hpp"
2+
namespace moveit_studio::behaviors
3+
{
4+
5+
using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
6+
7+
ExecuteAndTrackJointTrajectory::ExecuteAndTrackJointTrajectory(
8+
const std::string & name,
9+
const BT::NodeConfiguration & config,
10+
const std::shared_ptr<BehaviorContext> & shared_resources)
11+
: AsyncBehaviorBase(name, config, shared_resources)
12+
{}
13+
14+
ExecuteAndTrackJointTrajectory::~ExecuteAndTrackJointTrajectory() = default;
15+
16+
BT::PortsList ExecuteAndTrackJointTrajectory::providedPorts()
17+
{
18+
return {
19+
BT::InputPort<std::string>("action_name", "/joint_trajectory_controller/follow_joint_trajectory",
20+
"Name of the FollowJointTrajectory action server"),
21+
BT::InputPort<std::string>("service_name", "/pause_trajectory",
22+
"Name of the Trigger service for pausing trajectories"),
23+
BT::InputPort<trajectory_msgs::msg::JointTrajectory>("joint_trajectory_msg", "",
24+
"JointTrajectory message to execute"),
25+
BT::InputPort<double>("goal_position_tolerance", 0.0,
26+
"Tolerance for position error (radians)"),
27+
BT::InputPort<double>("goal_time_tolerance", 0.0,
28+
"Tolerance for time error (seconds)"),
29+
BT::InputPort<double>("goal_duration_tolerance", -1.0,
30+
"Time (in seconds) to wait before canceling the goal. Negative means wait indefinitely"),
31+
BT::InputPort<double>("goal_accept_tolerance", 3.0,
32+
"Time (in seconds) to wait for the goal to be accepted"),
33+
BT::OutputPort<trajectory_msgs::msg::JointTrajectory>("trajectory_remainder", "{joint_trajectory_remainder}",
34+
"Remaining trajectory after cancellation"),
35+
BT::OutputPort<double>("time_from_start", "{time_from_start}",
36+
"Latest feedback time_from_start in seconds")
37+
};
38+
}
39+
40+
std::shared_future<tl::expected<bool, std::string>>&
41+
ExecuteAndTrackJointTrajectory::getFuture()
42+
{
43+
return future_;
44+
}
45+
46+
tl::expected<bool, std::string> ExecuteAndTrackJointTrajectory::doWork()
47+
{
48+
notifyCanHalt();
49+
50+
auto ports = getRequiredInputs(
51+
getInput<std::string>("action_name"),
52+
getInput<std::string>("service_name"),
53+
getInput<trajectory_msgs::msg::JointTrajectory>("joint_trajectory_msg"),
54+
getInput<double>("goal_position_tolerance"),
55+
getInput<double>("goal_time_tolerance"),
56+
getInput<double>("goal_accept_tolerance"),
57+
getInput<double>("goal_duration_tolerance")
58+
);
59+
if (!ports) {
60+
return tl::make_unexpected(std::string("Failed to get required input: ") + ports.error());
61+
}
62+
auto [action_name, service_name, joint_trajectory,
63+
position_tolerance, time_tolerance,
64+
acceptance_tolerance, duration_tolerance] = ports.value();
65+
full_trajectory_ = joint_trajectory;
66+
67+
// Advertise cancellation trigger service
68+
trigger_service_ = shared_resources_->node->create_service<std_srvs::srv::Trigger>(
69+
service_name,
70+
[this](
71+
const std::shared_ptr<rmw_request_id_t>,
72+
const std::shared_ptr<std_srvs::srv::Trigger::Request>,
73+
std::shared_ptr<std_srvs::srv::Trigger::Response> res)
74+
{
75+
handleTrigger(res);
76+
});
77+
78+
action_client_ = rclcpp_action::create_client<FollowJointTrajectory>(
79+
shared_resources_->node, action_name);
80+
if (!action_client_->wait_for_action_server(
81+
std::chrono::duration<double>(acceptance_tolerance)))
82+
{
83+
trigger_service_.reset();
84+
return tl::make_unexpected("FollowJointTrajectory action server not available.");
85+
}
86+
87+
if (full_trajectory_.points.empty()) {
88+
return tl::make_unexpected("Empty JointTrajectory Message received, nothing to execute.");
89+
}
90+
if (position_tolerance < 0.0 || time_tolerance < 0.0) {
91+
return tl::make_unexpected("Position and time tolerances must be positive or zero.");
92+
}
93+
94+
// Build goal
95+
FollowJointTrajectory::Goal goal;
96+
goal.trajectory = full_trajectory_;
97+
for (const auto &jn : goal.trajectory.joint_names) {
98+
control_msgs::msg::JointTolerance tol;
99+
tol.name = jn;
100+
tol.position = position_tolerance;
101+
goal.goal_tolerance.push_back(tol);
102+
}
103+
goal.goal_time_tolerance = rclcpp::Duration::from_seconds(time_tolerance);
104+
105+
// Send goal with feedback callback
106+
typename rclcpp_action::Client<FollowJointTrajectory>::SendGoalOptions options;
107+
options.feedback_callback = [this](
108+
auto, std::shared_ptr<const FollowJointTrajectory::Feedback> fb)
109+
{
110+
processFeedback(fb);
111+
};
112+
auto future_goal_handle = action_client_->async_send_goal(goal, options);
113+
if (future_goal_handle.wait_for(
114+
std::chrono::duration<double>(acceptance_tolerance))
115+
== std::future_status::timeout)
116+
{
117+
trigger_service_.reset();
118+
return tl::make_unexpected("Timed out waiting for goal to be accepted.");
119+
}
120+
goal_handle_ = future_goal_handle.get();
121+
if (!goal_handle_) {
122+
trigger_service_.reset();
123+
return tl::make_unexpected("Goal request was rejected by the server.");
124+
}
125+
126+
// Wait for result (with optional timeout)
127+
auto future_result = action_client_->async_get_result(goal_handle_);
128+
if (duration_tolerance >= 0.0) {
129+
if (future_result.wait_for(
130+
std::chrono::duration<double>(duration_tolerance))
131+
== std::future_status::timeout)
132+
{
133+
action_client_->async_cancel_goal(goal_handle_);
134+
trigger_service_.reset();
135+
return tl::make_unexpected("Timed out waiting for result.");
136+
}
137+
}
138+
auto wrapped_result = future_result.get();
139+
140+
// Handle completion or cancellation
141+
switch (wrapped_result.code) {
142+
case rclcpp_action::ResultCode::SUCCEEDED:
143+
trigger_service_.reset();
144+
return true;
145+
146+
case rclcpp_action::ResultCode::CANCELED:
147+
trigger_service_.reset();
148+
if (triggered_) {
149+
auto rem = computeRemainderTrajectory();
150+
setOutput<trajectory_msgs::msg::JointTrajectory>(
151+
"trajectory_remainder", rem);
152+
return tl::make_unexpected("Trajectory halted");
153+
} else {
154+
return tl::make_unexpected("Trajectory canceled unexpectedly");
155+
}
156+
157+
case rclcpp_action::ResultCode::ABORTED:
158+
trigger_service_.reset();
159+
return tl::make_unexpected(
160+
"Action aborted: " + wrapped_result.result->error_string);
161+
162+
default:
163+
trigger_service_.reset();
164+
return tl::make_unexpected("Unknown action result code");
165+
}
166+
}
167+
168+
tl::expected<void, std::string> ExecuteAndTrackJointTrajectory::doHalt()
169+
{
170+
triggered_ = true;
171+
if (goal_handle_) {
172+
try {
173+
action_client_->async_cancel_goal(goal_handle_);
174+
} catch (const rclcpp_action::exceptions::UnknownGoalHandleError & e) {
175+
RCLCPP_WARN(rclcpp::get_logger("ExecuteAndTrackJointTrajectory"),
176+
"doHalt cancel failed: %s", e.what());
177+
}
178+
goal_handle_.reset();
179+
}
180+
trigger_service_.reset();
181+
return {};
182+
}
183+
184+
void ExecuteAndTrackJointTrajectory::handleTrigger(
185+
const std::shared_ptr<std_srvs::srv::Trigger::Response> res)
186+
{
187+
triggered_ = true;
188+
if (goal_handle_) {
189+
try {
190+
action_client_->async_cancel_goal(goal_handle_);
191+
} catch (const rclcpp_action::exceptions::UnknownGoalHandleError & e) {
192+
RCLCPP_WARN(rclcpp::get_logger("ExecuteAndTrackJointTrajectory"),
193+
"Failed to cancel goal: %s", e.what());
194+
}
195+
goal_handle_.reset();
196+
}
197+
res->success = true;
198+
res->message = "Trajectory cancellation triggered";
199+
}
200+
201+
void ExecuteAndTrackJointTrajectory::processFeedback(
202+
const std::shared_ptr<const FollowJointTrajectory::Feedback> feedback)
203+
{
204+
last_feedback_time_ =
205+
feedback->desired.time_from_start.sec +
206+
feedback->desired.time_from_start.nanosec * 1e-9;
207+
setOutput<double>("time_from_start", last_feedback_time_);
208+
}
209+
210+
trajectory_msgs::msg::JointTrajectory
211+
ExecuteAndTrackJointTrajectory::computeRemainderTrajectory()
212+
{
213+
trajectory_msgs::msg::JointTrajectory new_traj;
214+
new_traj.joint_names = full_trajectory_.joint_names;
215+
216+
size_t start_index = 0;
217+
for (; start_index < full_trajectory_.points.size(); ++start_index) {
218+
double t =
219+
full_trajectory_.points[start_index].time_from_start.sec +
220+
full_trajectory_.points[start_index].time_from_start.nanosec * 1e-9;
221+
if (t >= last_feedback_time_) break;
222+
}
223+
if (start_index >= full_trajectory_.points.size()) return new_traj;
224+
225+
double offset =
226+
full_trajectory_.points[start_index].time_from_start.sec +
227+
full_trajectory_.points[start_index].time_from_start.nanosec * 1e-9;
228+
for (size_t i = start_index; i < full_trajectory_.points.size(); ++i) {
229+
auto pt = full_trajectory_.points[i];
230+
double orig =
231+
pt.time_from_start.sec +
232+
pt.time_from_start.nanosec * 1e-9;
233+
double adjusted = orig - offset;
234+
pt.time_from_start.sec = static_cast<uint32_t>(adjusted);
235+
pt.time_from_start.nanosec =
236+
static_cast<uint32_t>((adjusted - pt.time_from_start.sec) * 1e9);
237+
new_traj.points.push_back(pt);
238+
}
239+
return new_traj;
240+
}
241+
242+
} // namespace moveit_studio::behaviors

‎src/register_behaviors.cpp

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
#include <moveit_studio_behavior_interface/shared_resources_node_loader.hpp>
44

55
#include "experimental_behaviors/get_pose_stamped_from_topic.hpp"
6+
#include "experimental_behaviors/execute_tracked_trajectory.hpp"
67

78
#include <pluginlib/class_list_macros.hpp>
89

@@ -17,6 +18,9 @@ class ExperimentalBehaviorsLoader : public moveit_studio::behaviors::SharedResou
1718
{
1819
moveit_studio::behaviors::registerBehavior<GetPoseStampedFromTopic>(factory, "GetPoseStampedFromTopic",
1920
shared_resources);
21+
moveit_studio::behaviors::registerBehavior<moveit_studio::behaviors::ExecuteAndTrackJointTrajectory>(
22+
factory, "ExecuteAndTrackJointTrajectory", shared_resources);
23+
2024
}
2125
};
2226
} // namespace experimental_behaviors

0 commit comments

Comments
 (0)