15 #ifndef JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_
16 #define JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_
23 #include "control_msgs/action/follow_joint_trajectory.hpp"
24 #include "control_msgs/msg/joint_trajectory_controller_state.hpp"
25 #include "control_msgs/srv/query_trajectory_state.hpp"
26 #include "control_toolbox/pid.hpp"
27 #include "controller_interface/controller_interface.hpp"
28 #include "hardware_interface/types/hardware_interface_type_values.hpp"
29 #include "joint_trajectory_controller/interpolation_methods.hpp"
30 #include "joint_trajectory_controller/tolerances.hpp"
31 #include "joint_trajectory_controller/trajectory.hpp"
32 #include "joint_trajectory_controller/visibility_control.h"
33 #include "rclcpp/duration.hpp"
34 #include "rclcpp/subscription.hpp"
35 #include "rclcpp/time.hpp"
36 #include "rclcpp/timer.hpp"
37 #include "rclcpp_action/server.hpp"
38 #include "rclcpp_lifecycle/state.hpp"
39 #include "realtime_tools/realtime_buffer.hpp"
40 #include "realtime_tools/realtime_publisher.hpp"
41 #include "realtime_tools/realtime_server_goal_handle.hpp"
42 #include "trajectory_msgs/msg/joint_trajectory.hpp"
43 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
46 #include "joint_trajectory_controller_parameters.hpp"
48 using namespace std::chrono_literals;
56 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
62 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
68 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
71 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
72 controller_interface::return_type update(
73 const rclcpp::Time & time,
const rclcpp::Duration & period)
override;
75 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
76 controller_interface::CallbackReturn on_init()
override;
78 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
79 controller_interface::CallbackReturn on_configure(
80 const rclcpp_lifecycle::State & previous_state)
override;
82 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
83 controller_interface::CallbackReturn on_activate(
84 const rclcpp_lifecycle::State & previous_state)
override;
86 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
87 controller_interface::CallbackReturn on_deactivate(
88 const rclcpp_lifecycle::State & previous_state)
override;
90 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
91 controller_interface::CallbackReturn on_cleanup(
92 const rclcpp_lifecycle::State & previous_state)
override;
94 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
95 controller_interface::CallbackReturn on_error(
96 const rclcpp_lifecycle::State & previous_state)
override;
98 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
99 controller_interface::CallbackReturn on_shutdown(
100 const rclcpp_lifecycle::State & previous_state)
override;
105 const std::vector<std::string> allowed_interface_types_ = {
113 trajectory_msgs::msg::JointTrajectoryPoint state_current_;
114 trajectory_msgs::msg::JointTrajectoryPoint command_current_;
115 trajectory_msgs::msg::JointTrajectoryPoint command_next_;
116 trajectory_msgs::msg::JointTrajectoryPoint state_desired_;
117 trajectory_msgs::msg::JointTrajectoryPoint state_error_;
123 std::vector<std::string> command_joint_names_;
126 std::shared_ptr<ParamListener> param_listener_;
128 rclcpp::Duration update_period_{0, 0};
130 rclcpp::Time traj_time_;
132 trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_;
134 interpolation_methods::InterpolationMethod interpolation_method_{
135 interpolation_methods::DEFAULT_INTERPOLATION};
140 template <
typename T>
141 using InterfaceReferences = std::vector<std::vector<std::reference_wrapper<T>>>;
143 InterfaceReferences<hardware_interface::LoanedCommandInterface> joint_command_interface_;
144 InterfaceReferences<hardware_interface::LoanedStateInterface> joint_state_interface_;
146 bool has_position_state_interface_ =
false;
147 bool has_velocity_state_interface_ =
false;
148 bool has_acceleration_state_interface_ =
false;
149 bool has_position_command_interface_ =
false;
150 bool has_velocity_command_interface_ =
false;
151 bool has_acceleration_command_interface_ =
false;
152 bool has_effort_command_interface_ =
false;
155 bool use_closed_loop_pid_adapter_ =
false;
156 using PidPtr = std::shared_ptr<control_toolbox::Pid>;
157 std::vector<PidPtr> pids_;
159 std::vector<double> ff_velocity_scale_;
162 std::vector<bool> joints_angle_wraparound_;
164 std::vector<double> tmp_command_;
171 bool subscriber_is_active_ =
false;
172 rclcpp::Subscription<trajectory_msgs::msg::JointTrajectory>::SharedPtr joint_command_subscriber_ =
175 rclcpp::Service<control_msgs::srv::QueryTrajectoryState>::SharedPtr query_state_srv_;
177 std::shared_ptr<Trajectory> traj_external_point_ptr_ =
nullptr;
179 traj_msg_external_point_ptr_;
181 std::shared_ptr<trajectory_msgs::msg::JointTrajectory> hold_position_msg_ptr_ =
nullptr;
183 using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState;
185 using StatePublisherPtr = std::unique_ptr<StatePublisher>;
186 rclcpp::Publisher<ControllerStateMsg>::SharedPtr publisher_;
187 StatePublisherPtr state_publisher_;
189 using FollowJTrajAction = control_msgs::action::FollowJointTrajectory;
191 using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
194 rclcpp_action::Server<FollowJTrajAction>::SharedPtr action_server_;
197 rclcpp::TimerBase::SharedPtr goal_handle_timer_;
198 rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms);
201 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
202 void topic_callback(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> msg);
205 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
206 rclcpp_action::GoalResponse goal_received_callback(
207 const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const FollowJTrajAction::Goal> goal);
208 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
209 rclcpp_action::CancelResponse goal_cancelled_callback(
210 const std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle);
211 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
212 void goal_accepted_callback(
213 std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle);
215 using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint;
225 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
226 void compute_error_for_joint(
227 JointTrajectoryPoint & error,
const size_t index,
const JointTrajectoryPoint & current,
228 const JointTrajectoryPoint & desired)
const;
231 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
232 void fill_partial_goal(
233 std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg)
const;
235 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
236 void sort_to_local_joint_order(
237 std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg)
const;
238 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
239 bool validate_trajectory_msg(
const trajectory_msgs::msg::JointTrajectory & trajectory)
const;
240 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
241 void add_new_trajectory_msg(
242 const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg);
243 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
244 bool validate_trajectory_point_field(
245 size_t joint_names_size,
const std::vector<double> & vector_field,
246 const std::string & string_for_vector_field,
size_t i,
bool allow_empty)
const;
253 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
254 void preempt_active_goal();
258 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
259 std::shared_ptr<trajectory_msgs::msg::JointTrajectory> set_hold_position();
265 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
266 std::shared_ptr<trajectory_msgs::msg::JointTrajectory> set_success_trajectory_point();
268 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
271 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
272 bool has_active_trajectory()
const;
274 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
276 const rclcpp::Time & time,
const JointTrajectoryPoint & desired_state,
277 const JointTrajectoryPoint & current_state,
const JointTrajectoryPoint & state_error);
279 void read_state_from_state_interfaces(JointTrajectoryPoint & state);
287 bool read_state_from_command_interfaces(JointTrajectoryPoint & state);
288 bool read_commands_from_command_interfaces(JointTrajectoryPoint & commands);
290 void query_state_service(
291 const std::shared_ptr<control_msgs::srv::QueryTrajectoryState::Request> request,
292 std::shared_ptr<control_msgs::srv::QueryTrajectoryState::Response> response);
297 bool contains_interface_type(
298 const std::vector<std::string> & interface_type_list,
const std::string & interface_type);
300 void init_hold_position_msg();
301 void resize_joint_trajectory_point(
302 trajectory_msgs::msg::JointTrajectoryPoint & point,
size_t size);
303 void resize_joint_trajectory_point_command(
304 trajectory_msgs::msg::JointTrajectoryPoint & point,
size_t size);
314 template <
typename T>
315 void assign_interface_from_point(
316 const T & joint_interface,
const std::vector<double> & trajectory_point_interface)
318 for (
size_t index = 0; index < dof_; ++index)
320 joint_interface[index].get().set_value(trajectory_point_interface[index]);
Definition: controller_interface.hpp:28
Definition: joint_trajectory_controller.hpp:54
realtime_tools::RealtimeBuffer< bool > rt_has_pending_goal_
Is there a pending action goal?
Definition: joint_trajectory_controller.hpp:196
RealtimeGoalHandleBuffer rt_active_goal_
Currently active action goal, if any.
Definition: joint_trajectory_controller.hpp:195
constexpr char HW_IF_EFFORT[]
Constant defining effort interface name.
Definition: hardware_interface_type_values.hpp:27
constexpr char HW_IF_ACCELERATION[]
Constant defining acceleration interface name.
Definition: hardware_interface_type_values.hpp:25
constexpr char HW_IF_VELOCITY[]
Constant defining velocity interface name.
Definition: hardware_interface_type_values.hpp:23
constexpr char HW_IF_POSITION[]
Constant defining position interface name.
Definition: hardware_interface_type_values.hpp:21
Definition: interpolation_methods.hpp:24
Configuring what command/state interfaces to claim.
Definition: controller_interface_base.hpp:58
Trajectory segment tolerances.
Definition: tolerances.hpp:59