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