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 <aubo_trajectory_filters/uniform_sample_filter.h>
00033 #include <kdl/velocityprofile_spline.hpp>
00034 #include <ros/ros.h>
00035
00036 using namespace aubo_trajectory_filters;
00037
00038 const double DEFAULT_SAMPLE_DURATION = 0.050;
00039
00040 template<typename T>
00041 UniformSampleFilter<T>::UniformSampleFilter() :
00042 aubo_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 {
00086
00087 while (interpolated_time > trajectory_in.request.trajectory.points[index_in + 1].time_from_start.toSec())
00088 {
00089
00090
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
00113
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
00122
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
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
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
00162
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
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
00176
00177
00178
00179
00180 }
00181
00182 rtn = true;
00183 }
00184 else
00185 {
00186 ROS_ERROR_STREAM("Trajectory point size mismatch");
00187 ROS_ERROR_STREAM(
00188 "Trajectory point 1, pos: " << p1.positions.size() << " vel: " << p1.velocities.size() << " acc: " << p1.accelerations.size());
00189 ROS_ERROR_STREAM(
00190 "Trajectory point 2, pos: " << p2.positions.size() << " vel: " << p2.velocities.size() << " acc: " << p2.accelerations.size());
00191 rtn = false;
00192 }
00193
00194 }
00195 else
00196 {
00197 ROS_ERROR_STREAM(
00198 "Trajectory point not fully defined, pos: " << p1.positions.size() << " vel: " << p1.velocities.size() << " acc: " << p1.accelerations.size());
00199 rtn = false;
00200 }
00201 }
00202 else
00203 {
00204 ROS_ERROR_STREAM(
00205 "Time: " << time_from_start << " not between interpolation point times[" << p1.time_from_start.toSec() << "," << p2.time_from_start.toSec() << "]");
00206 rtn = false;
00207 }
00208
00209 return rtn;
00210 }
00211
00212
00213 CLASS_LOADER_REGISTER_CLASS( aubo_trajectory_filters::UniformSampleFilterAdapter,
00214 planning_request_adapter::PlanningRequestAdapter);
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224