uniform_sample_filter.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Southwest Research Institute
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * * Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * * Neither the name of the Southwest Research Institute, nor the names
16  * of its contributors may be used to endorse or promote products derived
17  * from this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 
33 #include <kdl/velocityprofile_spline.hpp>
34 #include <ros/ros.h>
35 
36 using namespace industrial_trajectory_filters;
37 
38 const double DEFAULT_SAMPLE_DURATION = 0.050; //seconds
39 
40 template<typename T>
43  {
44  ROS_INFO_STREAM("Constructing N point filter");
46  this->filter_name_ = "UniformSampleFilter";
47  this->filter_type_ = "UniformSampleFilter";
48  }
49 
50 template<typename T>
52  {
53  }
54 
55 template<typename T>
57  {
58  if (!this->nh_.getParam("sample_duration", sample_duration_))
59  {
60  ROS_WARN_STREAM( "UniformSampleFilter, params has no attribute sample_duration.");
61  }
62  ROS_INFO_STREAM("Using a sample_duration value of " << sample_duration_);
63 
64  return true;
65  }
66 
67 template<typename T>
68  bool UniformSampleFilter<T>::update(const T& trajectory_in, T& trajectory_out)
69  {
70  bool success = false;
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;
74  size_t index_in = 0;
75 
76  trajectory_msgs::JointTrajectoryPoint p1, p2, interp_pt;
77 
78  trajectory_out = trajectory_in;
79 
80  // Clear out the trajectory points
81  trajectory_out.request.trajectory.points.clear();
82 
83  while (interpolated_time < duration_in)
84  {
85  ROS_DEBUG_STREAM("Interpolated time: " << interpolated_time);
86  // Increment index until the interpolated time is past the start time.
87  while (interpolated_time > trajectory_in.request.trajectory.points[index_in + 1].time_from_start.toSec())
88  {
90  "Interpolated time: " << interpolated_time << ", next point time: " << (trajectory_in.request.trajectory.points[index_in + 1].time_from_start.toSec()));
91  ROS_DEBUG_STREAM("Incrementing index");
92  index_in++;
93  if (index_in >= size_in)
94  {
96  "Programming error, index: " << index_in << ", greater(or equal) to size: " << size_in << " input duration: " << duration_in << " interpolated time:)" << interpolated_time);
97  return false;
98  }
99  }
100  p1 = trajectory_in.request.trajectory.points[index_in];
101  p2 = trajectory_in.request.trajectory.points[index_in + 1];
102  if (!interpolatePt(p1, p2, interpolated_time, interp_pt))
103  {
104  ROS_ERROR_STREAM("Failed to interpolate point");
105  return false;
106  }
107  trajectory_out.request.trajectory.points.push_back(interp_pt);
108  interpolated_time += sample_duration_;
109 
110  }
111 
113  "Interpolated time exceeds original trajectory (quitting), original: " << duration_in << " final interpolated time: " << interpolated_time);
114  p2 = trajectory_in.request.trajectory.points.back();
115  p2.time_from_start = ros::Duration(interpolated_time);
116  // TODO: Really should check that appending the last point doesn't result in
117  // really slow motion at the end. This could happen if the sample duration is a
118  // large percentage of the trajectory duration (not likely).
119  trajectory_out.request.trajectory.points.push_back(p2);
120 
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());
123 
124  success = true;
125  return success;
126  }
127 
128 template<typename T>
129  bool UniformSampleFilter<T>::interpolatePt(trajectory_msgs::JointTrajectoryPoint & p1,
130  trajectory_msgs::JointTrajectoryPoint & p2, double time_from_start,
131  trajectory_msgs::JointTrajectoryPoint & interp_pt)
132  {
133  bool rtn = false;
134  double p1_time_from_start = p1.time_from_start.toSec();
135  double p2_time_from_start = p2.time_from_start.toSec();
136 
137  ROS_DEBUG_STREAM("time from start: " << time_from_start);
138 
139  if (time_from_start >= p1_time_from_start && time_from_start <= p2_time_from_start)
140  {
141  if (p1.positions.size() == p1.velocities.size() && p1.positions.size() == p1.accelerations.size())
142  {
143  if (p1.positions.size() == p2.positions.size() && p1.velocities.size() == p2.velocities.size()
144  && p1.accelerations.size() == p2.accelerations.size())
145  {
146  // Copy p1 to ensure the interp_pt has the correct size vectors
147  interp_pt = p1;
148  // TODO: Creating a new spline calculator in this function means that
149  // it may be created multiple times for the same points (assuming the
150  // resample duration is less that the actual duration, which it might
151  // be sometimes)
152  KDL::VelocityProfile_Spline spline_calc;
153  ROS_DEBUG_STREAM( "---------------Begin interpolating joint point---------------");
154 
155  for (size_t i = 0; i < p1.positions.size(); ++i)
156  {
157  // Calculated relative times for spline calculation
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;
160 
161  ROS_DEBUG_STREAM("time from p1: " << time_from_p1);
162  ROS_DEBUG_STREAM( "time_from_p1_to_p2: " << time_from_p1_to_p2);
163 
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);
166 
167  ros::Duration time_from_start_dur(time_from_start);
168  ROS_DEBUG_STREAM( "time from start_dur: " << time_from_start_dur);
169 
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);
174 
176  "p1.pos: " << p1.positions[i] << ", vel: " << p1.velocities[i] << ", acc: " << p1.accelerations[i] << ", tfs: " << p1.time_from_start);
177 
179  "p2.pos: " << p2.positions[i] << ", vel: " << p2.velocities[i] << ", acc: " << p2.accelerations[i] << ", tfs: " << p2.time_from_start);
180 
182  "interp_pt.pos: " << interp_pt.positions[i] << ", vel: " << interp_pt.velocities[i] << ", acc: " << interp_pt.accelerations[i] << ", tfs: " << interp_pt.time_from_start);
183  }
184  ROS_DEBUG_STREAM( "---------------End interpolating joint point---------------");
185  rtn = true;
186  }
187  else
188  {
189  ROS_ERROR_STREAM("Trajectory point size mismatch");
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());
194  rtn = false;
195  }
196 
197  }
198  else
199  {
201  "Trajectory point not fully defined, pos: " << p1.positions.size() << " vel: " << p1.velocities.size() << " acc: " << p1.accelerations.size());
202  rtn = false;
203  }
204  }
205  else
206  {
208  "Time: " << time_from_start << " not between interpolation point times[" << p1.time_from_start.toSec() << "," << p2.time_from_start.toSec() << "]");
209  rtn = false;
210  }
211 
212  return rtn;
213  }
214 
215 // registering planner adapter
218 
219 /*
220  * Old plugin declaration for arm navigation trajectory filters
221  PLUGINLIB_DECLARE_CLASS(industrial_trajectory_filters,
222  IndustrialNPointFilterJointTrajectoryWithConstraints,
223  industrial_trajectory_filters::NPointFilter<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>,
224  filters::FilterBase<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>);
225 
226  */
227 
virtual double Acc(double time) const
This is a simple filter which performs a uniforming sampling of a trajectory using linear interpolati...
bool interpolatePt(trajectory_msgs::JointTrajectoryPoint &p1, trajectory_msgs::JointTrajectoryPoint &p2, double time_from_start, trajectory_msgs::JointTrajectoryPoint &interp_pt)
Perform interpolation between p1 and p2. Time from start must be in between p1 and p2 times...
ros::NodeHandle nh_
Internal node handle (used for parameter lookup)
Definition: filter_base.h:202
virtual double Vel(double time) const
std::string filter_name_
filter name
Definition: filter_base.h:187
const double DEFAULT_SAMPLE_DURATION
bool update(const T &trajectory_in, T &trajectory_out)
virtual bool configure()
FilterBase method for the sub class to configure the filter This function must be implemented in the ...
#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...
Definition: filter_base.h:123
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)
CLASS_LOADER_REGISTER_CLASS(industrial_trajectory_filters::UniformSampleFilterAdapter, planning_request_adapter::PlanningRequestAdapter)
double sample_duration_
uniform sample duration (sec)


open_manipulator_with_tb3_waffle_pi_moveit
Author(s): Darby Lim
autogenerated on Mon Jun 10 2019 14:13:54