Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include <industrial_trajectory_filters/uniform_sample_filter.h>
00033 #include <kdl/velocityprofile_spline.hpp>
00034 #include <ros/ros.h>
00035
00036 using namespace industrial_trajectory_filters;
00037
00038 const double DEFAULT_SAMPLE_DURATION = 0.050;
00039
00040 template<typename T>
00041 UniformSampleFilter<T>::UniformSampleFilter() :
00042 industrial_trajectory_filters::FilterBase<T>()
00043 {
00044 ROS_INFO_STREAM("Constructing N point filter");
00045 sample_duration_ = DEFAULT_SAMPLE_DURATION;
00046 this->filter_name_ = "UniformSampleFilter";
00047 this->filter_type_ = "UniformSampleFilter";
00048 }
00049
00050 template<typename T>
00051 UniformSampleFilter<T>::~UniformSampleFilter()
00052 {
00053 }
00054
00055 template<typename T>
00056 bool UniformSampleFilter<T>::configure()
00057 {
00058 if (!this->nh_.getParam("sample_duration", sample_duration_))
00059 {
00060 ROS_WARN_STREAM( "UniformSampleFilter, params has no attribute sample_duration.");
00061 }
00062 ROS_INFO_STREAM("Using a sample_duration value of " << sample_duration_);
00063
00064 return true;
00065 }
00066
00067 template<typename T>
00068 bool UniformSampleFilter<T>::update(const T& trajectory_in, T& trajectory_out)
00069 {
00070 bool success = false;
00071 size_t size_in = trajectory_in.request.trajectory.points.size();
00072 double duration_in = trajectory_in.request.trajectory.points.back().time_from_start.toSec();
00073 double interpolated_time = 0.0;
00074 size_t index_in = 0;
00075
00076 trajectory_msgs::JointTrajectoryPoint p1, p2, interp_pt;
00077
00078 trajectory_out = trajectory_in;
00079
00080
00081 trajectory_out.request.trajectory.points.clear();
00082
00083 while (interpolated_time < duration_in)
00084 {
00085 ROS_INFO_STREAM("Interpolated time: " << interpolated_time);
00086
00087 while (interpolated_time > trajectory_in.request.trajectory.points[index_in + 1].time_from_start.toSec())
00088 {
00089 ROS_INFO_STREAM(
00090 "Interpolated time: " << interpolated_time << ", next point time: " << (trajectory_in.request.trajectory.points[index_in + 1].time_from_start.toSec()));
00091 ROS_INFO_STREAM("Incrementing index");
00092 index_in++;
00093 if (index_in >= size_in)
00094 {
00095 ROS_ERROR_STREAM(
00096 "Programming error, index: " << index_in << ", greater(or equal) to size: " << size_in << " input duration: " << duration_in << " interpolated time:)" << interpolated_time);
00097 return false;
00098 }
00099 }
00100 p1 = trajectory_in.request.trajectory.points[index_in];
00101 p2 = trajectory_in.request.trajectory.points[index_in + 1];
00102 if (!interpolatePt(p1, p2, interpolated_time, interp_pt))
00103 {
00104 ROS_ERROR_STREAM("Failed to interpolate point");
00105 return false;
00106 }
00107 trajectory_out.request.trajectory.points.push_back(interp_pt);
00108 interpolated_time += sample_duration_;
00109
00110 }
00111
00112 ROS_INFO_STREAM(
00113 "Interpolated time exceeds original trajectory (quitting), original: " << duration_in << " final interpolated time: " << interpolated_time);
00114 p2 = trajectory_in.request.trajectory.points.back();
00115 p2.time_from_start = ros::Duration(interpolated_time);
00116
00117
00118
00119 trajectory_out.request.trajectory.points.push_back(p2);
00120
00121 ROS_INFO_STREAM(
00122 "Uniform sampling, resample duraction: " << sample_duration_ << " input traj. size: " << trajectory_in.request.trajectory.points.size() << " output traj. size: " << trajectory_out.request.trajectory.points.size());
00123
00124 success = true;
00125 return success;
00126 }
00127
00128 template<typename T>
00129 bool UniformSampleFilter<T>::interpolatePt(trajectory_msgs::JointTrajectoryPoint & p1,
00130 trajectory_msgs::JointTrajectoryPoint & p2, double time_from_start,
00131 trajectory_msgs::JointTrajectoryPoint & interp_pt)
00132 {
00133 bool rtn = false;
00134 double p1_time_from_start = p1.time_from_start.toSec();
00135 double p2_time_from_start = p2.time_from_start.toSec();
00136
00137 ROS_INFO_STREAM("time from start: " << time_from_start);
00138
00139 if (time_from_start >= p1_time_from_start && time_from_start <= p2_time_from_start)
00140 {
00141 if (p1.positions.size() == p1.velocities.size() && p1.positions.size() == p1.accelerations.size())
00142 {
00143 if (p1.positions.size() == p2.positions.size() && p1.velocities.size() == p2.velocities.size()
00144 && p1.accelerations.size() == p2.accelerations.size())
00145 {
00146
00147 interp_pt = p1;
00148
00149
00150
00151
00152 KDL::VelocityProfile_Spline spline_calc;
00153 ROS_INFO_STREAM( "---------------Begin interpolating joint point---------------");
00154
00155 for (size_t i = 0; i < p1.positions.size(); ++i)
00156 {
00157
00158 double time_from_p1 = time_from_start - p1.time_from_start.toSec();
00159 double time_from_p1_to_p2 = p2_time_from_start - p1_time_from_start;
00160
00161 ROS_INFO_STREAM("time from p1: " << time_from_p1);
00162 ROS_INFO_STREAM( "time_from_p1_to_p2: " << time_from_p1_to_p2);
00163
00164 spline_calc.SetProfileDuration(p1.positions[i], p1.velocities[i], p1.accelerations[i], p2.positions[i],
00165 p2.velocities[i], p2.accelerations[i], time_from_p1_to_p2);
00166
00167 ros::Duration time_from_start_dur(time_from_start);
00168 ROS_INFO_STREAM( "time from start_dur: " << time_from_start_dur);
00169
00170 interp_pt.time_from_start = time_from_start_dur;
00171 interp_pt.positions[i] = spline_calc.Pos(time_from_p1);
00172 interp_pt.velocities[i] = spline_calc.Vel(time_from_p1);
00173 interp_pt.accelerations[i] = spline_calc.Acc(time_from_p1);
00174
00175 ROS_INFO_STREAM(
00176 "p1.pos: " << p1.positions[i] << ", vel: " << p1.velocities[i] << ", acc: " << p1.accelerations[i] << ", tfs: " << p1.time_from_start);
00177
00178 ROS_INFO_STREAM(
00179 "p2.pos: " << p2.positions[i] << ", vel: " << p2.velocities[i] << ", acc: " << p2.accelerations[i] << ", tfs: " << p2.time_from_start);
00180
00181 ROS_INFO_STREAM(
00182 "interp_pt.pos: " << interp_pt.positions[i] << ", vel: " << interp_pt.velocities[i] << ", acc: " << interp_pt.accelerations[i] << ", tfs: " << interp_pt.time_from_start);
00183 }
00184 ROS_INFO_STREAM( "---------------End interpolating joint point---------------");
00185 rtn = true;
00186 }
00187 else
00188 {
00189 ROS_ERROR_STREAM("Trajectory point size mismatch");
00190 ROS_ERROR_STREAM(
00191 "Trajectory point 1, pos: " << p1.positions.size() << " vel: " << p1.velocities.size() << " acc: " << p1.accelerations.size());
00192 ROS_ERROR_STREAM(
00193 "Trajectory point 2, pos: " << p2.positions.size() << " vel: " << p2.velocities.size() << " acc: " << p2.accelerations.size());
00194 rtn = false;
00195 }
00196
00197 }
00198 else
00199 {
00200 ROS_ERROR_STREAM(
00201 "Trajectory point not fully defined, pos: " << p1.positions.size() << " vel: " << p1.velocities.size() << " acc: " << p1.accelerations.size());
00202 rtn = false;
00203 }
00204 }
00205 else
00206 {
00207 ROS_ERROR_STREAM(
00208 "Time: " << time_from_start << " not between interpolation point times[" << p1.time_from_start.toSec() << "," << p2.time_from_start.toSec() << "]");
00209 rtn = false;
00210 }
00211
00212 return rtn;
00213 }
00214
00215
00216 CLASS_LOADER_REGISTER_CLASS( industrial_trajectory_filters::UniformSampleFilterAdapter,
00217 planning_request_adapter::PlanningRequestAdapter);
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227