$search
00001 /* 00002 * UOS-ROS packages - Robot Operating System code by the University of Osnabrück 00003 * Copyright (C) 2010 University of Osnabrück 00004 * 00005 * This program is free software; you can redistribute it and/or 00006 * modify it under the terms of the GNU General Public License 00007 * as published by the Free Software Foundation; either version 2 00008 * of the License, or (at your option) any later version. 00009 * 00010 * This program is distributed in the hope that it will be useful, 00011 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00012 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00013 * GNU General Public License for more details. 00014 * 00015 * You should have received a copy of the GNU General Public License 00016 * along with this program; if not, write to the Free Software 00017 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 00018 * 00019 * katana_trajectory_filter.cpp 00020 * 00021 * Created on: 04.02.2011 00022 * Author: Martin Günther <mguenthe@uos.de> 00023 */ 00024 00025 #include <katana_trajectory_filter/katana_trajectory_filter.h> 00026 00027 namespace katana_trajectory_filter 00028 { 00029 00031 static const size_t MAX_NUM_POINTS = 16; 00032 00041 static const size_t DELETE_CHUNK_SIZE = 10; 00042 00043 template<typename T> 00044 KatanaTrajectoryFilter<T>::KatanaTrajectoryFilter() 00045 { 00046 } 00047 00048 template<typename T> 00049 KatanaTrajectoryFilter<T>::~KatanaTrajectoryFilter() 00050 { 00051 } 00052 00053 template<typename T> 00054 bool KatanaTrajectoryFilter<T>::smooth(const T& trajectory_in, T& trajectory_out) const 00055 { 00056 size_t num_points = trajectory_in.trajectory.points.size(); 00057 trajectory_out = trajectory_in; 00058 00059 if (!spline_smoother::checkTrajectoryConsistency(trajectory_out)) 00060 return false; 00061 00062 if (num_points <= MAX_NUM_POINTS) 00063 // nothing to do 00064 return true; 00065 00066 while (true) 00067 { 00068 const T trajectory_mid = trajectory_out; 00069 00070 size_t num_points_delete = num_points - MAX_NUM_POINTS; 00071 00072 if (num_points_delete > DELETE_CHUNK_SIZE) 00073 num_points_delete = DELETE_CHUNK_SIZE; 00074 else if (num_points_delete <= 0) 00075 break; 00076 00077 remove_smallest_segments(trajectory_mid, trajectory_out, num_points_delete); 00078 num_points = trajectory_out.trajectory.points.size(); 00079 } 00080 00081 // delete all velocities and accelerations so they will be re-computed by Katana 00082 for (size_t i = 0; i < trajectory_out.trajectory.points.size(); ++i) 00083 { 00084 trajectory_out.trajectory.points[i].velocities.resize(0); 00085 trajectory_out.trajectory.points[i].accelerations.resize(0); 00086 } 00087 00088 return true; 00089 } 00090 00091 template<typename T> 00092 void KatanaTrajectoryFilter<T>::remove_smallest_segments(const T& trajectory_in, T& trajectory_out, 00093 const size_t num_points_delete) const 00094 { 00095 size_t num_points = trajectory_in.trajectory.points.size(); 00096 std::vector<std::pair<size_t, double> > segment_durations(num_points - 1); 00097 00098 // calculate segment_durations 00099 for (size_t i = 0; i < num_points - 1; ++i) 00100 { 00101 double duration = (trajectory_in.trajectory.points[i + 1].time_from_start 00102 - trajectory_in.trajectory.points[i].time_from_start).toSec(); 00103 00104 segment_durations[i] = std::pair<size_t, double>(i, duration); 00105 } 00106 00107 for (size_t i = 0; i < segment_durations.size(); i++) 00108 ROS_DEBUG("segment_durations[%3zu] = <%3zu, %f>", i, segment_durations[i].first, segment_durations[i].second); 00109 00110 // sort segment_durations by their duration, in ascending order 00111 std::vector<std::pair<size_t, double> > sorted_segment_durations = segment_durations; 00112 std::sort(sorted_segment_durations.begin(), sorted_segment_durations.end(), boost::bind(&std::pair<size_t, double>::second, _1) 00113 < boost::bind(&std::pair<size_t, double>::second, _2)); 00114 00115 for (size_t i = 0; i < sorted_segment_durations.size(); i++) 00116 ROS_DEBUG("sorted_segment_durations[%3zu] = <%3zu, %f>", i, sorted_segment_durations[i].first, sorted_segment_durations[i].second); 00117 00118 // delete the smallest segments 00119 std::set<size_t> delete_set; 00120 for (size_t i = 0; i < num_points_delete; i++) 00121 { 00122 size_t point_to_delete = sorted_segment_durations[i].first; 00123 if (point_to_delete == 0) 00124 { 00125 // first segment too small --> merge right 00126 point_to_delete = 1; 00127 } 00128 else if (point_to_delete == num_points - 1) 00129 { 00130 // last segment too small --> merge left (default) 00131 } 00132 else 00133 { 00134 // some segment in the middle too small --> merge towards smaller segment 00135 if (segment_durations[point_to_delete - 1] > segment_durations[point_to_delete + 1]) 00136 point_to_delete++; 00137 00138 // note: this can lead to a situation where less than num_points_delete are actually deleted, 00139 // but we don't care 00140 } 00141 00142 delete_set.insert(point_to_delete); 00143 } 00144 00145 for (std::set<size_t>::iterator it = delete_set.begin(); it != delete_set.end(); it++) 00146 ROS_DEBUG("delete set entry: %zu", *it); 00147 00148 trajectory_out.trajectory.points.resize(0); 00149 for (size_t i = 0; i < num_points; i++) 00150 { 00151 if (delete_set.find(i) == delete_set.end()) 00152 { 00153 // segment i is not in the delete set --> copy 00154 trajectory_out.trajectory.points.push_back(trajectory_in.trajectory.points[i]); 00155 } 00156 } 00157 } 00158 00159 } // namespace katana_trajectory_filter 00160 00161 // PLUGINLIB_DECLARE_CLASS(package, class_name, class_type, filters::FilterBase<T>) 00162 PLUGINLIB_DECLARE_CLASS(katana_trajectory_filter, KatanaTrajectoryFilterFilterJointTrajectory, katana_trajectory_filter::KatanaTrajectoryFilter<arm_navigation_msgs::FilterJointTrajectory::Request>, filters::FilterBase<arm_navigation_msgs::FilterJointTrajectory::Request>) 00163 PLUGINLIB_DECLARE_CLASS(katana_trajectory_filter, KatanaTrajectoryFilterJointTrajectoryWithLimits, katana_trajectory_filter::KatanaTrajectoryFilter<arm_navigation_msgs::JointTrajectoryWithLimits>, filters::FilterBase<arm_navigation_msgs::JointTrajectoryWithLimits>) 00164 PLUGINLIB_DECLARE_CLASS(katana_trajectory_filter, KatanaTrajectoryFilterFilterJointTrajectoryWithConstraints, katana_trajectory_filter::KatanaTrajectoryFilter<arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request>, filters::FilterBase<arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request>)