Skip to content

Commit c535c9b

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

File tree

4 files changed

+289
-1
lines changed

4 files changed

+289
-1
lines changed

‎CMakeLists.txt

Lines changed: 2 additions & 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()
@@ -13,6 +13,7 @@ add_library(
1313
experimental_behaviors
1414
SHARED
1515
src/get_pose_stamped_from_topic.cpp
16+
src/execute_tracked_trajectory.cpp
1617
src/register_behaviors.cpp)
1718
target_include_directories(
1819
experimental_behaviors

‎include/experimental_behaviors/execute_tracked_trajectory.hpp

Lines changed: 56 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,56 @@
1+
#pragma once
2+
3+
#include <atomic>
4+
#include <chrono>
5+
#include <future>
6+
#include <memory>
7+
#include <string>
8+
#include <vector>
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_optional_ports.hpp>
19+
#include <moveit_studio_behavior_interface/get_required_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(const std::string& name, const BT::NodeConfiguration& config,
31+
const std::shared_ptr<BehaviorContext>& shared_resources);
32+
~ExecuteAndTrackJointTrajectory() override;
33+
34+
static BT::PortsList providedPorts();
35+
36+
std::shared_future<tl::expected<bool, std::string>>& getFuture() override;
37+
38+
protected:
39+
tl::expected<bool, std::string> doWork() override;
40+
tl::expected<void, std::string> doHalt() override;
41+
42+
private:
43+
void handleTrigger(const std::shared_ptr<std_srvs::srv::Trigger::Response> res);
44+
void processFeedback(const std::shared_ptr<const FollowJointTrajectory::Feedback> feedback);
45+
trajectory_msgs::msg::JointTrajectory computeRemainderTrajectory();
46+
47+
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr trigger_service_;
48+
rclcpp_action::Client<FollowJointTrajectory>::SharedPtr action_client_;
49+
rclcpp_action::ClientGoalHandle<FollowJointTrajectory>::SharedPtr goal_handle_;
50+
trajectory_msgs::msg::JointTrajectory full_trajectory_;
51+
double last_feedback_time_{ 0.0 };
52+
std::atomic<bool> triggered_{ false };
53+
std::shared_future<tl::expected<bool, std::string>> future_;
54+
};
55+
56+
} // namespace moveit_studio::behaviors

‎src/execute_tracked_trajectory.cpp

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

‎src/register_behaviors.cpp

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

5+
#include "experimental_behaviors/execute_tracked_trajectory.hpp"
56
#include "experimental_behaviors/get_pose_stamped_from_topic.hpp"
67

78
#include <pluginlib/class_list_macros.hpp>
@@ -17,6 +18,8 @@ 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);
2023
}
2124
};
2225
} // namespace experimental_behaviors

0 commit comments

Comments
 (0)