15 #ifndef JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_
16 #define JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_
21 #include "joint_trajectory_controller/interpolation_methods.hpp"
22 #include "joint_trajectory_controller/visibility_control.h"
23 #include "rclcpp/time.hpp"
24 #include "trajectory_msgs/msg/joint_trajectory.hpp"
25 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
28 using TrajectoryPointIter = std::vector<trajectory_msgs::msg::JointTrajectoryPoint>::iterator;
29 using TrajectoryPointConstIter =
30 std::vector<trajectory_msgs::msg::JointTrajectoryPoint>::const_iterator;
35 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
38 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
39 explicit Trajectory(std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory);
41 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
43 const rclcpp::Time & current_time,
44 const trajectory_msgs::msg::JointTrajectoryPoint & current_point,
45 std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory);
55 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
57 const rclcpp::Time & current_time,
58 const trajectory_msgs::msg::JointTrajectoryPoint & current_point,
59 const std::vector<bool> & joints_angle_wraparound = std::vector<bool>());
61 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
62 void update(std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory);
92 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
94 const rclcpp::Time & sample_time,
95 const interpolation_methods::InterpolationMethod interpolation_method,
96 trajectory_msgs::msg::JointTrajectoryPoint & output_state,
97 TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr);
120 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
122 const rclcpp::Time & time_a,
const trajectory_msgs::msg::JointTrajectoryPoint & state_a,
123 const rclcpp::Time & time_b,
const trajectory_msgs::msg::JointTrajectoryPoint & state_b,
124 const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & output);
126 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
127 TrajectoryPointConstIter begin()
const;
129 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
130 TrajectoryPointConstIter end()
const;
132 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
133 rclcpp::Time time_from_start()
const;
135 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
136 bool has_trajectory_msg()
const;
138 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
139 bool has_nontrivial_msg()
const;
141 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
142 std::shared_ptr<trajectory_msgs::msg::JointTrajectory> get_trajectory_msg()
const
144 return trajectory_msg_;
147 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
148 bool is_sampled_already()
const {
return sampled_already_; }
151 void deduce_from_derivatives(
152 trajectory_msgs::msg::JointTrajectoryPoint & first_state,
153 trajectory_msgs::msg::JointTrajectoryPoint & second_state,
const size_t dim,
154 const double delta_t);
156 std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg_;
157 rclcpp::Time trajectory_start_time_;
159 rclcpp::Time time_before_traj_msg_;
160 trajectory_msgs::msg::JointTrajectoryPoint state_before_traj_msg_;
162 bool sampled_already_ =
false;
171 inline std::vector<size_t>
mapping(
const T & t1,
const T & t2)
174 if (t1.size() > t2.size())
176 return std::vector<size_t>();
179 std::vector<size_t> mapping_vector(t1.size());
180 for (
auto t1_it = t1.begin(); t1_it != t1.end(); ++t1_it)
182 auto t2_it = std::find(t2.begin(), t2.end(), *t1_it);
183 if (t2.end() == t2_it)
185 return std::vector<size_t>();
189 const size_t t1_dist = std::distance(t1.begin(), t1_it);
190 const size_t t2_dist = std::distance(t2.begin(), t2_it);
191 mapping_vector[t1_dist] = t2_dist;
194 return mapping_vector;
205 std::vector<double> & current_position,
const std::vector<double> next_position,
206 const std::vector<bool> & joints_angle_wraparound);
Definition: trajectory.hpp:33
JOINT_TRAJECTORY_CONTROLLER_PUBLIC void interpolate_between_points(const rclcpp::Time &time_a, const trajectory_msgs::msg::JointTrajectoryPoint &state_a, const rclcpp::Time &time_b, const trajectory_msgs::msg::JointTrajectoryPoint &state_b, const rclcpp::Time &sample_time, trajectory_msgs::msg::JointTrajectoryPoint &output)
Definition: trajectory.cpp:198
JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool sample(const rclcpp::Time &sample_time, const interpolation_methods::InterpolationMethod interpolation_method, trajectory_msgs::msg::JointTrajectoryPoint &output_state, TrajectoryPointConstIter &start_segment_itr, TrajectoryPointConstIter &end_segment_itr)
Definition: trajectory.cpp:89
JOINT_TRAJECTORY_CONTROLLER_PUBLIC void set_point_before_trajectory_msg(const rclcpp::Time ¤t_time, const trajectory_msgs::msg::JointTrajectoryPoint ¤t_point, const std::vector< bool > &joints_angle_wraparound=std::vector< bool >())
Definition: trajectory.cpp:45
Definition: interpolation_methods.hpp:24
std::vector< size_t > mapping(const T &t1, const T &t2)
Definition: trajectory.hpp:171
void wraparound_joint(std::vector< double > ¤t_position, const std::vector< double > next_position, const std::vector< bool > &joints_angle_wraparound)
Definition: trajectory.cpp:59