constraint_aware_spline_smoother: constraint_aware_spline_smoother: linear_spline

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00037 #ifndef LINEAR_SPLINE_SHORT_CUTTER_H_
00038 #define LINEAR_SPLINE_SHORT_CUTTER_H_
00039 
00040 #include <ros/ros.h>
00041 #include <tf/tf.h>
00042 #include <spline_smoother/spline_smoother.h>
00043 #include <spline_smoother/linear_trajectory.h>
00044 #include <planning_environment/models/collision_models_interface.h>
00045 #include <planning_environment/models/model_utils.h>
00046 #include <arm_navigation_msgs/RobotState.h>
00047 #include <arm_navigation_msgs/ArmNavigationErrorCodes.h>
00048 #include <trajectory_msgs/JointTrajectoryPoint.h>
00049 
00050 namespace constraint_aware_spline_smoother
00051 {
00052 
00056 template <typename T>
00057 class LinearSplineShortCutter: public spline_smoother::SplineSmoother<T>
00058 {
00059 public:
00063   LinearSplineShortCutter();
00064   virtual ~LinearSplineShortCutter();
00065   virtual bool smooth(const T& trajectory_in, 
00066                       T& trajectory_out) const;
00069   virtual bool configure();
00070 private:
00071   bool active_;
00072   double discretization_;
00073   bool setupCollisionEnvironment();
00074   planning_environment::CollisionModelsInterface *collision_models_interface_;
00075   ros::NodeHandle node_handle_;
00076   int getRandomInt(int min,int max) const;
00077   double getRandomTimeStamp(double min,double max) const;
00078   void discretizeTrajectory(const spline_smoother::SplineTrajectory &spline, 
00079                             const double &discretization,
00080                             trajectory_msgs::JointTrajectory &joint_trajectory) const;
00081   bool trimTrajectory(trajectory_msgs::JointTrajectory &trajectory_out, 
00082                       const double &segment_start_time, 
00083                       const double &segment_end_time) const;
00084   bool findTrajectoryPointsInInterval(const trajectory_msgs::JointTrajectory &trajectory,
00085                                       const double &segment_start_time, 
00086                                       const double &segment_end_time,
00087                                       int &index_1,
00088                                       int &index_2) const;
00089   bool getWaypoints(const spline_smoother::SplineTrajectory &spline, 
00090                     trajectory_msgs::JointTrajectory &joint_trajectory) const;
00091   bool addToTrajectory(trajectory_msgs::JointTrajectory &trajectory_out, 
00092                        const trajectory_msgs::JointTrajectoryPoint &trajectory_point,
00093                        const ros::Duration& delta_time) const;
00094 
00095   void printTrajectory(const trajectory_msgs::JointTrajectory &joint_trajectory) const;
00096 
00097   void discretizeAndAppendSegment(const spline_smoother::SplineTrajectorySegment &spline_segment,
00098                                   const double &discretization,
00099                                   trajectory_msgs::JointTrajectory &joint_trajectory,
00100                                   const ros::Duration &segment_start_time,
00101                                   const bool &include_segment_end) const;
00102 
00103   double maxLInfDistance(const trajectory_msgs::JointTrajectoryPoint &start, 
00104                          const trajectory_msgs::JointTrajectoryPoint &end) const;
00105 
00106   void refineTrajectory(T &trajectory) const;
00107 
00108 };
00109 
00110 template <typename T>
00111 bool LinearSplineShortCutter<T>::configure()
00112 {
00113   if (!spline_smoother::SplineSmoother<T>::getParam("discretization", discretization_))
00114   {
00115     ROS_ERROR("Spline smoother, \"%s\", params has no attribute discretization.", spline_smoother::SplineSmoother<T>::getName().c_str());
00116     return false;
00117   }
00118   else
00119   {
00120     ROS_DEBUG("Using a discretization value of %f",discretization_);
00121     return true;
00122   }
00123 };
00124 
00125 template <typename T>
00126 LinearSplineShortCutter<T>::LinearSplineShortCutter()
00127 {
00128   if(!setupCollisionEnvironment())
00129     active_ = false;
00130   else
00131     active_ = true;
00132 }
00133 
00134 template <typename T>
00135 LinearSplineShortCutter<T>::~LinearSplineShortCutter()
00136 {
00137 }
00138 
00139 template <typename T>
00140 int LinearSplineShortCutter<T>::getRandomInt(int min_index,int max_index)const
00141 {
00142   int rand_num = rand()%100+1;
00143   double result = min_index + (double)((max_index-min_index)*rand_num)/101.0;
00144   return (int) result;
00145 }
00146 
00147 template <typename T>
00148 double LinearSplineShortCutter<T>::getRandomTimeStamp(double min,double max)const
00149 {
00150   int rand_num = rand()%100+1;
00151   double result = min + (double)((max-min)*rand_num)/101.0;
00152   return result;
00153 }
00154 
00155 template <typename T>
00156 bool LinearSplineShortCutter<T>::smooth(const T& trajectory_in, 
00157                                        T& trajectory_out) const
00158 {
00159   double discretization = discretization_;
00160   srand(time(NULL)); // initialize random seed: 
00161   if(!active_)
00162   {
00163     ROS_ERROR("Smoother is not active");
00164     return false;
00165   }
00166 
00167   ROS_INFO("Got trajectory with %d points",(int)trajectory_in.trajectory.points.size());
00168   if(!collision_models_interface_->isPlanningSceneSet()) {
00169     ROS_INFO("Planning scene not set, can't do anything");
00170     return false;
00171   }
00172 
00173   arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00174   std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> trajectory_error_codes;
00175   arm_navigation_msgs::RobotState robot_state;
00176   spline_smoother::LinearTrajectory trajectory_solver;
00177   spline_smoother::SplineTrajectory spline, shortcut_spline;
00178   arm_navigation_msgs::JointTrajectoryWithLimits shortcut, discretized_trajectory;
00179 
00180   trajectory_out = trajectory_in;
00181 
00182   collision_models_interface_->disableCollisionsForNonUpdatedLinks(trajectory_in.group_name);
00183 
00184   planning_environment::setRobotStateAndComputeTransforms(trajectory_in.start_state,
00185                                                           *collision_models_interface_->getPlanningSceneState());
00186 
00187   if (!spline_smoother::checkTrajectoryConsistency(trajectory_out))
00188     return false;
00189 
00190   shortcut.limits = trajectory_in.limits;
00191   shortcut.trajectory.joint_names = trajectory_in.trajectory.joint_names;
00192 
00193   discretized_trajectory.limits = trajectory_in.limits;
00194   discretized_trajectory.trajectory.joint_names = trajectory_in.trajectory.joint_names;
00195 
00196   ros::Time start_time = ros::Time::now();
00197   ros::Duration timeout = trajectory_in.allowed_time;
00198 
00199   bool success = trajectory_solver.parameterize(trajectory_out.trajectory,trajectory_in.limits,spline);      
00200   getWaypoints(spline,trajectory_out.trajectory);
00201   printTrajectory(trajectory_out.trajectory);
00202   std::vector<double> sample_times;
00203   sample_times.resize(2);
00204   bool first_try = true;
00205   while(ros::Time::now() - start_time < timeout)
00206   {
00207     double total_time = trajectory_out.trajectory.points.back().time_from_start.toSec();
00208     double segment_start_time = getRandomTimeStamp(0.0,total_time);
00209     double segment_end_time = getRandomTimeStamp(segment_start_time,total_time);
00210     if(segment_start_time == segment_end_time)
00211       continue;
00212     if(first_try)
00213     {
00214       segment_start_time = 0.0;
00215       segment_end_time = total_time;
00216       first_try = false;
00217     }
00218     sample_times[0] = segment_start_time;
00219     sample_times[1] = segment_end_time;
00220 
00221     spline_smoother::sampleSplineTrajectory(spline,sample_times,shortcut.trajectory);
00222     ROS_DEBUG("Start time: %f, %f",segment_start_time,shortcut.trajectory.points[0].positions[0]);
00223     ROS_DEBUG("End time  : %f, %f",segment_end_time,shortcut.trajectory.points[1].positions[0]);
00224     shortcut.trajectory.points[0].time_from_start = ros::Duration(0.0);
00225     shortcut.trajectory.points[1].time_from_start = ros::Duration(0.0);
00226     
00227     if(!trajectory_solver.parameterize(shortcut.trajectory,trajectory_in.limits,shortcut_spline))
00228       return false;
00229     discretizeTrajectory(shortcut_spline,discretization,discretized_trajectory.trajectory);
00230     ROS_DEBUG("Succeeded in sampling trajectory with size: %d",(int)discretized_trajectory.trajectory.points.size());
00231 
00232     arm_navigation_msgs::Constraints empty_goal_constraints;
00233 
00234     if(collision_models_interface_->isJointTrajectoryValid(*collision_models_interface_->getPlanningSceneState(),
00235                                                            discretized_trajectory.trajectory,
00236                                                            empty_goal_constraints,
00237                                                            trajectory_in.path_constraints,
00238                                                            error_code,
00239                                                            trajectory_error_codes,
00240                                                            false))
00241     {
00242      ros::Duration shortcut_duration = discretized_trajectory.trajectory.points.back().time_from_start - discretized_trajectory.trajectory.points.front().time_from_start;
00243       if(segment_end_time-segment_start_time <= shortcut_duration.toSec())
00244         continue;
00245       if(!trimTrajectory(trajectory_out.trajectory,segment_start_time,segment_end_time))
00246         continue;
00247       ROS_DEBUG("Trimmed trajectory has %u points",trajectory_out.trajectory.points.size());
00248 
00249       ROS_DEBUG("Shortcut reduced duration from: %f to %f",
00250                 segment_end_time-segment_start_time,
00251                 shortcut_duration.toSec());
00252       shortcut.trajectory.points[0].time_from_start = ros::Duration(segment_start_time);
00253       shortcut.trajectory.points[1].time_from_start = ros::Duration(segment_start_time) + shortcut_duration;
00254       addToTrajectory(trajectory_out.trajectory,
00255                       shortcut.trajectory.points[0],
00256                       ros::Duration(0.0));
00257       addToTrajectory(trajectory_out.trajectory,
00258                       shortcut.trajectory.points[1],
00259                       shortcut_duration-ros::Duration(segment_end_time-segment_start_time));
00260       spline.segments.clear();
00261       if(!trajectory_solver.parameterize(trajectory_out.trajectory,trajectory_in.limits,spline))
00262         return false;
00263       if(!getWaypoints(spline,trajectory_out.trajectory))
00264         return false;
00265       printTrajectory(trajectory_out.trajectory);
00266       if(trajectory_out.trajectory.points.size() < 3)
00267         break;
00268     }
00269     else 
00270     {
00271       ROS_DEBUG("Traj segment rejected with error code: %d",error_code.val);
00272       continue;
00273     }
00274   }
00275   ROS_INFO("Trajectory filter took %f seconds",(ros::Time::now() - start_time).toSec());
00276   for(unsigned int i=0; i < trajectory_out.trajectory.points.size(); i++)
00277   {
00278     trajectory_out.trajectory.points[i].accelerations.clear();
00279     trajectory_out.trajectory.points[i].velocities.clear();
00280   }
00281   printTrajectory(trajectory_out.trajectory);
00282   /*
00283   refineTrajectory(trajectory_out);
00284   if(!trajectory_solver.parameterize(trajectory_out.trajectory,trajectory_in.limits,spline))
00285     return false;
00286   if(!getWaypoints(spline,trajectory_out.trajectory))
00287     return false;
00288   discretizeTrajectory(spline,discretization,trajectory_out.trajectory);
00289   */
00290   trajectory_out.limits = trajectory_in.limits;
00291   printTrajectory(trajectory_out.trajectory);
00292         
00293   ROS_DEBUG("Final trajectory has %d points and %f total time",(int)trajectory_out.trajectory.points.size(),
00294             trajectory_out.trajectory.points.back().time_from_start.toSec());
00295   //  planning_monitor_->getEnvironmentModel()->unlock();
00296   //  planning_monitor_->getKinematicModel()->unlock();
00297   return success;
00298 }
00299 
00300 template <typename T>
00301 void LinearSplineShortCutter<T>::refineTrajectory(T &trajectory) const
00302 {
00303   if(trajectory.trajectory.points.size() < 3)
00304     return;
00305 
00306   for(unsigned int i=1; i < trajectory.trajectory.points.size()-1; i++)
00307   {
00308     for(unsigned int j=0; j < trajectory.trajectory.points[i].positions.size(); j++)
00309     {
00310       double dq_first = trajectory.trajectory.points[i].positions[j] - trajectory.trajectory.points[i-1].positions[j];
00311       double dq_second = trajectory.trajectory.points[i+1].positions[j] - trajectory.trajectory.points[i].positions[j];
00312       double dq_dot = trajectory.trajectory.points[i].velocities[j];
00313       double dt_first = (trajectory.trajectory.points[i].time_from_start - trajectory.trajectory.points[i-1].time_from_start).toSec();
00314       double dt_second = (trajectory.trajectory.points[i+1].time_from_start - trajectory.trajectory.points[i].time_from_start).toSec();
00315       if( (dq_first > 0 && dq_second > 0) || (dq_first < 0 && dq_second < 0)) 
00316       {       
00317         if(trajectory.trajectory.points[i].velocities[j] == 0.0)
00318         {
00319           trajectory.trajectory.points[i].velocities[j] = 0.5*(dq_first/dt_first + dq_second/dt_second);
00320           trajectory.trajectory.points[i].velocities[j] = std::max(std::min(trajectory.trajectory.points[i].velocities[j],
00321                                                                             trajectory.limits[j].max_velocity),
00322                                                                    -trajectory.limits[j].max_velocity);
00323         }
00324       }
00325     }
00326   }
00327 }
00328 
00329 template <typename T>
00330 void LinearSplineShortCutter<T>::printTrajectory(const trajectory_msgs::JointTrajectory &trajectory) const
00331 {
00332   for(unsigned int i = 0; i < trajectory.points.size(); i++)
00333   {
00334     ROS_DEBUG("%f: %f %f %f %f %f %f %f %f %f %f %f %f %f %f",
00335              trajectory.points[i].time_from_start.toSec(),
00336              trajectory.points[i].positions[0],
00337              trajectory.points[i].positions[1],
00338              trajectory.points[i].positions[2],
00339              trajectory.points[i].positions[3],
00340              trajectory.points[i].positions[4],
00341              trajectory.points[i].positions[5],
00342              trajectory.points[i].positions[6],
00343              trajectory.points[i].velocities[0],
00344              trajectory.points[i].velocities[1],
00345              trajectory.points[i].velocities[2],
00346              trajectory.points[i].velocities[3],
00347              trajectory.points[i].velocities[4],
00348              trajectory.points[i].velocities[5],
00349              trajectory.points[i].velocities[6]);
00350   }
00351   ROS_DEBUG(" ");
00352 }
00353 
00354 /*template <typename T>
00355 void LinearSplineShortCutter<T>::discretizeTrajectory(const spline_smoother::SplineTrajectory &spline, 
00356                                                      const double &discretization,
00357                                                      trajectory_msgs::JointTrajectory &joint_trajectory) const
00358 {
00359   double total_time;
00360   spline_smoother::getTotalTime(spline,total_time);
00361   std::vector<double> times;
00362   double time_index = 0.0;
00363   while(time_index < total_time)
00364   {
00365     times.push_back(time_index);
00366     time_index += discretization;
00367   }
00368   times.push_back(total_time);  
00369   spline_smoother::sampleSplineTrajectory(spline,times,joint_trajectory);
00370 }
00371 */
00372 
00373 template <typename T>
00374 void LinearSplineShortCutter<T>::discretizeTrajectory(const spline_smoother::SplineTrajectory &spline, 
00375                                                      const double &discretization,
00376                                                      trajectory_msgs::JointTrajectory &joint_trajectory) const
00377 {
00378   if(spline.segments.empty())
00379     return;
00380   joint_trajectory.points.clear();
00381   ros::Duration segment_start_time(0.0);
00382   for(unsigned int i=0; i < spline.segments.size(); i++)
00383   {
00384     if(i == spline.segments.size()-1)
00385       discretizeAndAppendSegment(spline.segments[i],discretization,joint_trajectory,segment_start_time,true);
00386     else
00387       discretizeAndAppendSegment(spline.segments[i],discretization,joint_trajectory,segment_start_time,false);
00388     segment_start_time += spline.segments[i].duration;
00389     ROS_DEBUG("Discretizing and appending segment %d",i);
00390   }
00391 }
00392 
00393 template <typename T>
00394 void LinearSplineShortCutter<T>::discretizeAndAppendSegment(const spline_smoother::SplineTrajectorySegment &spline_segment,
00395                                                            const double &discretization,
00396                                                            trajectory_msgs::JointTrajectory &joint_trajectory,
00397                                                            const ros::Duration &segment_start_time,
00398                                                            const bool &include_segment_end) const
00399 {
00400   ros::Duration time_from_start = segment_start_time;
00401   double total_time = spline_segment.duration.toSec();
00402   double sample_time = 0.0;
00403   trajectory_msgs::JointTrajectoryPoint start,end;
00404   spline_smoother::sampleSplineTrajectory(spline_segment,0.0,start);
00405   if(joint_trajectory.points.empty())
00406   {
00407     start.time_from_start = ros::Duration(0.0);
00408     joint_trajectory.points.push_back(start);
00409     sample_time += 0.01;
00410   }
00411   start = joint_trajectory.points.back();
00412   while(sample_time < total_time)
00413   {
00414      ROS_DEBUG("Sample time is %f",sample_time);
00415      spline_smoother::sampleSplineTrajectory(spline_segment,sample_time,end);
00416      double max_diff = maxLInfDistance(start,end);
00417      if(sample_time > 0 && max_diff < discretization)
00418      {
00419        ROS_DEBUG("Max diff is %f. Skipping",max_diff);
00420        sample_time += 0.01;
00421        continue;
00422      }
00423      end.time_from_start = time_from_start + ros::Duration(sample_time);
00424      joint_trajectory.points.push_back(end);
00425      ROS_DEBUG("Pushing back point with time: %f",end.time_from_start.toSec());
00426      sample_time += 0.01;
00427      start = end;
00428   }
00429   if(include_segment_end)
00430   {
00431     spline_smoother::sampleSplineTrajectory(spline_segment,total_time,end);
00432     end.time_from_start = time_from_start + ros::Duration(total_time);
00433     joint_trajectory.points.push_back(end);
00434   }    
00435 }
00436 
00437 template <typename T>
00438 double LinearSplineShortCutter<T>::maxLInfDistance(const trajectory_msgs::JointTrajectoryPoint &start, 
00439                                                   const trajectory_msgs::JointTrajectoryPoint &end) const
00440 {
00441   double max_diff = 0.0;
00442   for(unsigned int i=0; i< start.positions.size(); i++)
00443   {
00444     double diff = fabs(end.positions[i]-start.positions[i]);
00445     if(diff > max_diff)
00446       max_diff = diff;
00447   }
00448   return max_diff;
00449 }
00450       
00451 
00452 
00453 template <typename T>
00454 bool LinearSplineShortCutter<T>::trimTrajectory(trajectory_msgs::JointTrajectory &trajectory_out, 
00455                                                const double &segment_start_time, 
00456                                                const double &segment_end_time) const
00457 {
00458   int index1;
00459   int index2;
00460   if(findTrajectoryPointsInInterval(trajectory_out,segment_start_time,segment_end_time,index1,index2))
00461   {
00462     ROS_DEBUG("Trimming trajectory between segments: %d and %d",index1,index2);
00463     std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator remove_start = trajectory_out.points.begin() + index1;
00464     std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator remove_end;
00465     if((unsigned int) index2 >= trajectory_out.points.size())
00466       remove_end = trajectory_out.points.end();
00467     else
00468       remove_end = trajectory_out.points.begin()+index2;
00469       
00470 
00471     if(remove_start != remove_end)
00472       trajectory_out.points.erase(remove_start,remove_end);
00473     else
00474       trajectory_out.points.erase(remove_start);
00475   }
00476   else
00477     return false;
00478   return true;
00479 }
00480 
00481 template <typename T>
00482 bool LinearSplineShortCutter<T>::findTrajectoryPointsInInterval(const trajectory_msgs::JointTrajectory &trajectory,
00483                                                                const double &segment_start_time, 
00484                                                                const double &segment_end_time,
00485                                                                int &index_1,
00486                                                                int &index_2) const
00487 {
00488   index_1 = -1;
00489   index_2 = -1;
00490   if(segment_start_time > segment_end_time)
00491     return false;
00492   for(unsigned int i=0; i < trajectory.points.size(); i++)
00493     if(trajectory.points[i].time_from_start.toSec() >= segment_start_time)
00494     {
00495       index_1 = i;
00496       break;
00497     }
00498   ROS_DEBUG("First trim index: %d",index_1);
00499   if(index_1>=0)
00500     for(unsigned int i=index_1; i < trajectory.points.size(); i++)
00501     {
00502       if(trajectory.points[i].time_from_start.toSec() > segment_end_time)
00503       {
00504         index_2 = i;
00505         break;
00506       }
00507       if(trajectory.points[i].time_from_start.toSec() == segment_end_time)
00508       {
00509         index_2 = i+1;
00510         break;
00511       }
00512     }
00513   ROS_DEBUG("Second trim index: %d",index_2);
00514   if(index_1 >= index_2 || index_1 < 0 || index_2 < 0)
00515     return false;
00516   return true;
00517 }
00518 
00519 template <typename T>
00520 bool LinearSplineShortCutter<T>::getWaypoints(const spline_smoother::SplineTrajectory &spline, 
00521                                              trajectory_msgs::JointTrajectory &joint_trajectory) const
00522 {
00523   std::vector<double> waypoint_times_vector;
00524   double waypoint_time = 0.0;
00525   waypoint_times_vector.push_back(waypoint_time);
00526   for(unsigned int i=0; i < spline.segments.size(); i++)
00527   {
00528     waypoint_time = waypoint_time + spline.segments[i].duration.toSec();
00529     waypoint_times_vector.push_back(waypoint_time);
00530     ROS_DEBUG("Spline segment time: %f",spline.segments[i].duration.toSec());
00531   }
00532   if(!spline_smoother::sampleSplineTrajectory(spline,waypoint_times_vector,joint_trajectory))
00533     return false;
00534   return true;
00535 }
00536 
00537 template <typename T>
00538 bool LinearSplineShortCutter<T>::addToTrajectory(trajectory_msgs::JointTrajectory &trajectory_out, 
00539                                                 const trajectory_msgs::JointTrajectoryPoint &trajectory_point,
00540                                                 const ros::Duration &delta_time) const
00541 {
00542 
00543   ROS_DEBUG("Inserting point at time: %f",trajectory_point.time_from_start.toSec());
00544   ROS_DEBUG("Old trajectory has %u points",trajectory_out.points.size());
00545 
00546   if(trajectory_out.points.empty())
00547   {
00548     trajectory_out.points.push_back(trajectory_point);
00549     return true;
00550   }
00551 
00552   unsigned int counter = 0;
00553   unsigned int old_size = trajectory_out.points.size();
00554   for(std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator iter = trajectory_out.points.begin(); 
00555       iter != trajectory_out.points.end() ; iter++)
00556   {   
00557     if(iter->time_from_start >= trajectory_point.time_from_start)
00558     {
00559       trajectory_out.points.insert(iter,trajectory_point);
00560       break;
00561     }
00562     counter++;
00563   }
00564 
00565   if(delta_time == ros::Duration(0.0))
00566     return true;
00567 
00568   if(counter == old_size)
00569     trajectory_out.points.push_back(trajectory_point);
00570   else
00571     if(counter+1 < trajectory_out.points.size())
00572       for(unsigned int i= counter+1; i < trajectory_out.points.size(); i++)
00573       {
00574         trajectory_out.points[i].time_from_start += delta_time;
00575       } 
00576   return true;
00577 }
00578 
00579 template <typename T>
00580 bool LinearSplineShortCutter<T>::setupCollisionEnvironment()
00581 {
00582   bool use_collision_map;
00583   node_handle_.param<bool>("use_collision_map", use_collision_map, true);
00584 
00585   // monitor robot
00586   collision_models_interface_ = new planning_environment::CollisionModelsInterface("robot_description");
00587 
00588   return true;
00589 }
00590 }
00591 
00592 #endif /* CUBIC_SPLINE_SMOOTHER_H_ */
linear_spline_shortcutter.h
