33 #include <kdl/velocityprofile_spline.hpp> 60 ROS_WARN_STREAM(
"UniformSampleFilter, params has no attribute sample_duration.");
71 size_t size_in = trajectory_in.request.trajectory.points.size();
72 double duration_in = trajectory_in.request.trajectory.points.back().time_from_start.toSec();
73 double interpolated_time = 0.0;
76 trajectory_msgs::JointTrajectoryPoint p1, p2, interp_pt;
78 trajectory_out = trajectory_in;
81 trajectory_out.request.trajectory.points.clear();
83 while (interpolated_time < duration_in)
87 while (interpolated_time > trajectory_in.request.trajectory.points[index_in + 1].time_from_start.toSec())
90 "Interpolated time: " << interpolated_time <<
", next point time: " << (trajectory_in.request.trajectory.points[index_in + 1].time_from_start.toSec()));
93 if (index_in >= size_in)
96 "Programming error, index: " << index_in <<
", greater(or equal) to size: " << size_in <<
" input duration: " << duration_in <<
" interpolated time:)" << interpolated_time);
100 p1 = trajectory_in.request.trajectory.points[index_in];
101 p2 = trajectory_in.request.trajectory.points[index_in + 1];
107 trajectory_out.request.trajectory.points.push_back(interp_pt);
113 "Interpolated time exceeds original trajectory (quitting), original: " << duration_in <<
" final interpolated time: " << interpolated_time);
114 p2 = trajectory_in.request.trajectory.points.back();
119 trajectory_out.request.trajectory.points.push_back(p2);
122 "Uniform sampling, resample duraction: " <<
sample_duration_ <<
" input traj. size: " << trajectory_in.request.trajectory.points.size() <<
" output traj. size: " << trajectory_out.request.trajectory.points.size());
130 trajectory_msgs::JointTrajectoryPoint & p2,
double time_from_start,
131 trajectory_msgs::JointTrajectoryPoint & interp_pt)
134 double p1_time_from_start = p1.time_from_start.toSec();
135 double p2_time_from_start = p2.time_from_start.toSec();
139 if (time_from_start >= p1_time_from_start && time_from_start <= p2_time_from_start)
141 if (p1.positions.size() == p1.velocities.size() && p1.positions.size() == p1.accelerations.size())
143 if (p1.positions.size() == p2.positions.size() && p1.velocities.size() == p2.velocities.size()
144 && p1.accelerations.size() == p2.accelerations.size())
153 ROS_DEBUG_STREAM(
"---------------Begin interpolating joint point---------------");
155 for (
size_t i = 0; i < p1.positions.size(); ++i)
158 double time_from_p1 = time_from_start - p1.time_from_start.toSec();
159 double time_from_p1_to_p2 = p2_time_from_start - p1_time_from_start;
164 spline_calc.
SetProfileDuration(p1.positions[i], p1.velocities[i], p1.accelerations[i], p2.positions[i],
165 p2.velocities[i], p2.accelerations[i], time_from_p1_to_p2);
170 interp_pt.time_from_start = time_from_start_dur;
171 interp_pt.positions[i] = spline_calc.
Pos(time_from_p1);
172 interp_pt.velocities[i] = spline_calc.
Vel(time_from_p1);
173 interp_pt.accelerations[i] = spline_calc.
Acc(time_from_p1);
176 "p1.pos: " << p1.positions[i] <<
", vel: " << p1.velocities[i] <<
", acc: " << p1.accelerations[i] <<
", tfs: " << p1.time_from_start);
179 "p2.pos: " << p2.positions[i] <<
", vel: " << p2.velocities[i] <<
", acc: " << p2.accelerations[i] <<
", tfs: " << p2.time_from_start);
182 "interp_pt.pos: " << interp_pt.positions[i] <<
", vel: " << interp_pt.velocities[i] <<
", acc: " << interp_pt.accelerations[i] <<
", tfs: " << interp_pt.time_from_start);
184 ROS_DEBUG_STREAM(
"---------------End interpolating joint point---------------");
191 "Trajectory point 1, pos: " << p1.positions.size() <<
" vel: " << p1.velocities.size() <<
" acc: " << p1.accelerations.size());
193 "Trajectory point 2, pos: " << p2.positions.size() <<
" vel: " << p2.velocities.size() <<
" acc: " << p2.accelerations.size());
201 "Trajectory point not fully defined, pos: " << p1.positions.size() <<
" vel: " << p1.velocities.size() <<
" acc: " << p1.accelerations.size());
208 "Time: " << time_from_start <<
" not between interpolation point times[" << p1.time_from_start.toSec() <<
"," << p2.time_from_start.toSec() <<
"]");
virtual double Acc(double time) const
ros::NodeHandle nh_
Internal node handle (used for parameter lookup)
virtual double Vel(double time) const
std::string filter_name_
filter name
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
This version of the FilterBase<T> can be used to encapsulate the functionality from an arm navigation...
virtual void SetProfileDuration(double pos1, double pos2, double duration)
#define ROS_INFO_STREAM(args)
virtual double Pos(double time) const
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_STREAM(args)