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
00033
00034
00037 #ifndef CUBIC_SPLINE_SHORT_CUTTER_H_
00038 #define CUBIC_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/cubic_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 CubicSplineShortCutter: public spline_smoother::SplineSmoother<T>
00058 {
00059 public:
00063 CubicSplineShortCutter();
00064 virtual ~CubicSplineShortCutter();
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
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 CubicSplineShortCutter<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 CubicSplineShortCutter<T>::CubicSplineShortCutter()
00127 {
00128 if(!setupCollisionEnvironment())
00129 active_ = false;
00130 else
00131 active_ = true;
00132 }
00133
00134 template <typename T>
00135 CubicSplineShortCutter<T>::~CubicSplineShortCutter()
00136 {
00137 }
00138
00139 template <typename T>
00140 int CubicSplineShortCutter<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 CubicSplineShortCutter<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 CubicSplineShortCutter<T>::smooth(const T& trajectory_in,
00157 T& trajectory_out) const
00158 {
00159 double discretization = discretization_;
00160 srand(time(NULL));
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
00169 if(!collision_models_interface_->isPlanningSceneSet()) {
00170 ROS_INFO("Planning scene not set, can't do anything");
00171 return false;
00172 }
00173
00174 arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00175 std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> trajectory_error_codes;
00176 arm_navigation_msgs::RobotState robot_state;
00177 spline_smoother::CubicTrajectory trajectory_solver;
00178 spline_smoother::SplineTrajectory spline, shortcut_spline;
00179 arm_navigation_msgs::JointTrajectoryWithLimits shortcut, discretized_trajectory;
00180
00181 trajectory_out = trajectory_in;
00182
00183 collision_models_interface_->disableCollisionsForNonUpdatedLinks(trajectory_in.group_name);
00184
00185 planning_environment::setRobotStateAndComputeTransforms(trajectory_in.start_state,
00186 *collision_models_interface_->getPlanningSceneState());
00187
00188 if(!collision_models_interface_->isJointTrajectoryValid(*collision_models_interface_->getPlanningSceneState(),
00189 trajectory_out.trajectory,
00190 trajectory_in.goal_constraints,
00191 trajectory_in.path_constraints,
00192 error_code,
00193 trajectory_error_codes,
00194 false)) {
00195 ROS_INFO_STREAM("Original trajectory invalid with error code " << error_code.val);
00196 return false;
00197 }
00198
00199
00200 if (!spline_smoother::checkTrajectoryConsistency(trajectory_out))
00201 return false;
00202
00203 shortcut.limits = trajectory_in.limits;
00204 shortcut.trajectory.joint_names = trajectory_in.trajectory.joint_names;
00205
00206 discretized_trajectory.limits = trajectory_in.limits;
00207 discretized_trajectory.trajectory.joint_names = trajectory_in.trajectory.joint_names;
00208
00209 ros::Time start_time = ros::Time::now();
00210 ros::Duration timeout = trajectory_in.allowed_time;
00211
00212 bool success = trajectory_solver.parameterize(trajectory_out.trajectory,trajectory_in.limits,spline);
00213 getWaypoints(spline,trajectory_out.trajectory);
00214 printTrajectory(trajectory_out.trajectory);
00215
00216 for(unsigned int i = 0; i < trajectory_in.limits.size(); i++) {
00217 ROS_DEBUG_STREAM("Joint " << trajectory_in.limits[i].joint_name
00218 << " has " << (bool) trajectory_in.limits[i].has_position_limits
00219 << " low " << trajectory_in.limits[i].min_position
00220 << " high " << trajectory_in.limits[i].max_position);
00221 }
00222
00223 if(!collision_models_interface_->isJointTrajectoryValid(*collision_models_interface_->getPlanningSceneState(),
00224 trajectory_out.trajectory,
00225 trajectory_in.goal_constraints,
00226 trajectory_in.path_constraints,
00227 error_code,
00228 trajectory_error_codes,
00229 false)) {
00230 ROS_INFO_STREAM("Original sampled trajectory invalid with error code " << error_code.val);
00231 return false;
00232 } else {
00233 ROS_DEBUG_STREAM("Originally sampled trajectory ok");
00234 }
00235
00236 T test_traj = trajectory_out;
00237
00238 discretizeTrajectory(spline,discretization,test_traj.trajectory);
00239
00240 if(!collision_models_interface_->isJointTrajectoryValid(*collision_models_interface_->getPlanningSceneState(),
00241 test_traj.trajectory,
00242 trajectory_in.goal_constraints,
00243 trajectory_in.path_constraints,
00244 error_code,
00245 trajectory_error_codes,
00246 false)) {
00247 ROS_WARN_STREAM("Original discretized trajectory invalid with error code " << error_code.val);
00248 trajectory_out = test_traj;
00249 return true;
00250 } else {
00251 ROS_DEBUG_STREAM("Originally discretized trajectory ok");
00252 }
00253
00254
00255 std::vector<double> sample_times;
00256 sample_times.resize(2);
00257 bool first_try = true;
00258 while(ros::Time::now() - start_time < timeout)
00259 {
00260 double total_time = trajectory_out.trajectory.points.back().time_from_start.toSec();
00261 double segment_start_time = getRandomTimeStamp(0.0,total_time);
00262 double segment_end_time = getRandomTimeStamp(segment_start_time,total_time);
00263 if(segment_start_time == segment_end_time)
00264 continue;
00265 if(first_try)
00266 {
00267 segment_start_time = 0.0;
00268 segment_end_time = total_time;
00269 first_try = false;
00270 }
00271 sample_times[0] = segment_start_time;
00272 sample_times[1] = segment_end_time;
00273
00274 spline_smoother::sampleSplineTrajectory(spline,sample_times,shortcut.trajectory);
00275 ROS_DEBUG("Start time: %f, %f",segment_start_time,shortcut.trajectory.points[0].positions[0]);
00276 ROS_DEBUG("End time : %f, %f",segment_end_time,shortcut.trajectory.points[1].positions[0]);
00277 shortcut.trajectory.points[0].time_from_start = ros::Duration(0.0);
00278 shortcut.trajectory.points[1].time_from_start = ros::Duration(0.0);
00279
00280 if(!trajectory_solver.parameterize(shortcut.trajectory,trajectory_in.limits,shortcut_spline))
00281 return false;
00282 discretizeTrajectory(shortcut_spline,discretization,discretized_trajectory.trajectory);
00283 ROS_DEBUG("Succeeded in sampling trajectory with size: %d",(int)discretized_trajectory.trajectory.points.size());
00284
00285 arm_navigation_msgs::Constraints empty_goal_constraints;
00286
00287 if(collision_models_interface_->isJointTrajectoryValid(*collision_models_interface_->getPlanningSceneState(),
00288 discretized_trajectory.trajectory,
00289 empty_goal_constraints,
00290 trajectory_in.path_constraints,
00291 error_code,
00292 trajectory_error_codes,
00293 false))
00294 {
00295 ros::Duration shortcut_duration = discretized_trajectory.trajectory.points.back().time_from_start - discretized_trajectory.trajectory.points.front().time_from_start;
00296 if(segment_end_time-segment_start_time <= shortcut_duration.toSec())
00297 continue;
00298 if(!trimTrajectory(trajectory_out.trajectory,segment_start_time,segment_end_time))
00299 continue;
00300 ROS_DEBUG_STREAM("Trimmed trajectory has " << trajectory_out.trajectory.points.size() << " points");
00301
00302 ROS_DEBUG("Shortcut reduced duration from: %f to %f",
00303 segment_end_time-segment_start_time,
00304 shortcut_duration.toSec());
00305 shortcut.trajectory.points[0].time_from_start = ros::Duration(segment_start_time);
00306 shortcut.trajectory.points[1].time_from_start = ros::Duration(segment_start_time) + shortcut_duration;
00307 addToTrajectory(trajectory_out.trajectory,
00308 shortcut.trajectory.points[0],
00309 ros::Duration(0.0));
00310 addToTrajectory(trajectory_out.trajectory,
00311 shortcut.trajectory.points[1],
00312 shortcut_duration-ros::Duration(segment_end_time-segment_start_time));
00313 spline.segments.clear();
00314 if(!trajectory_solver.parameterize(trajectory_out.trajectory,trajectory_in.limits,spline))
00315 return false;
00316 if(!getWaypoints(spline,trajectory_out.trajectory))
00317 return false;
00318 printTrajectory(trajectory_out.trajectory);
00319 if(trajectory_out.trajectory.points.size() < 3)
00320 break;
00321 }
00322 else
00323 {
00324 ROS_DEBUG("Traj segment rejected with error code: %d",error_code.val);
00325 continue;
00326 }
00327 }
00328 ROS_INFO("Trajectory filter took %f seconds",(ros::Time::now() - start_time).toSec());
00329 for(unsigned int i=0; i < trajectory_out.trajectory.points.size(); i++)
00330 {
00331 trajectory_out.trajectory.points[i].accelerations.clear();
00332 }
00333 if(!collision_models_interface_->isJointTrajectoryValid(*collision_models_interface_->getPlanningSceneState(),
00334 trajectory_out.trajectory,
00335 trajectory_in.goal_constraints,
00336 trajectory_in.path_constraints,
00337 error_code,
00338 trajectory_error_codes,
00339 false)) {
00340 ROS_INFO_STREAM("Before refine trajectory invalid with error code " << error_code.val);
00341 } else {
00342 ROS_DEBUG_STREAM("Before refine trajectory ok");
00343 }
00344
00345 printTrajectory(trajectory_out.trajectory);
00346 refineTrajectory(trajectory_out);
00347
00348 if(!collision_models_interface_->isJointTrajectoryValid(*collision_models_interface_->getPlanningSceneState(),
00349 trajectory_out.trajectory,
00350 trajectory_in.goal_constraints,
00351 trajectory_in.path_constraints,
00352 error_code,
00353 trajectory_error_codes,
00354 false)) {
00355 ROS_INFO_STREAM("Before waypoints trajectory invalid with error code " << error_code.val);
00356 } else {
00357 ROS_DEBUG_STREAM("Before waypoints trajectory ok");
00358 }
00359
00360 if(!trajectory_solver.parameterize(trajectory_out.trajectory,trajectory_in.limits,spline))
00361 return false;
00362 if(!getWaypoints(spline,trajectory_out.trajectory))
00363 return false;
00364 if(!collision_models_interface_->isJointTrajectoryValid(*collision_models_interface_->getPlanningSceneState(),
00365 trajectory_out.trajectory,
00366 trajectory_in.goal_constraints,
00367 trajectory_in.path_constraints,
00368 error_code,
00369 trajectory_error_codes,
00370 false)) {
00371 ROS_INFO_STREAM("Before discretize trajectory invalid with error code " << error_code.val);
00372 } else {
00373 ROS_DEBUG_STREAM("Before discretize trajectory ok");
00374 }
00375 discretizeTrajectory(spline,discretization,trajectory_out.trajectory);
00376
00377 trajectory_out.limits = trajectory_in.limits;
00378
00379 printTrajectory(trajectory_out.trajectory);
00380
00381 ROS_DEBUG("Final trajectory has %d points and %f total time",(int)trajectory_out.trajectory.points.size(),
00382 trajectory_out.trajectory.points.back().time_from_start.toSec());
00383
00384 if(!collision_models_interface_->isJointTrajectoryValid(*collision_models_interface_->getPlanningSceneState(),
00385 trajectory_out.trajectory,
00386 trajectory_in.goal_constraints,
00387 trajectory_in.path_constraints,
00388 error_code,
00389 trajectory_error_codes,
00390 false)) {
00391 ROS_INFO_STREAM("Final trajectory invalid with error code " << error_code.val);
00392 ROS_INFO_STREAM("Trajectory error codes size is " << trajectory_error_codes.size());
00393
00394 } else {
00395 ROS_DEBUG_STREAM("Final trajectory ok");
00396 }
00397
00398
00399 return success;
00400 }
00401
00402 template <typename T>
00403 void CubicSplineShortCutter<T>::refineTrajectory(T &trajectory) const
00404 {
00405 if(trajectory.trajectory.points.size() < 3)
00406 return;
00407
00408 for(unsigned int i=1; i < trajectory.trajectory.points.size()-1; i++)
00409 {
00410 for(unsigned int j=0; j < trajectory.trajectory.points[i].positions.size(); j++)
00411 {
00412 double dq_first = trajectory.trajectory.points[i].positions[j] - trajectory.trajectory.points[i-1].positions[j];
00413 double dq_second = trajectory.trajectory.points[i+1].positions[j] - trajectory.trajectory.points[i].positions[j];
00414 double dq_dot = trajectory.trajectory.points[i].velocities[j];
00415 double dt_first = (trajectory.trajectory.points[i].time_from_start - trajectory.trajectory.points[i-1].time_from_start).toSec();
00416 double dt_second = (trajectory.trajectory.points[i+1].time_from_start - trajectory.trajectory.points[i].time_from_start).toSec();
00417 if( (dq_first > 0 && dq_second > 0) || (dq_first < 0 && dq_second < 0))
00418 {
00419 if(trajectory.trajectory.points[i].velocities[j] == 0.0)
00420 {
00421 trajectory.trajectory.points[i].velocities[j] = 0.5*(dq_first/dt_first + dq_second/dt_second);
00422 trajectory.trajectory.points[i].velocities[j] = std::max(std::min(trajectory.trajectory.points[i].velocities[j],
00423 trajectory.limits[j].max_velocity),
00424 -trajectory.limits[j].max_velocity);
00425 }
00426 }
00427 }
00428 }
00429 }
00430
00431 template <typename T>
00432 void CubicSplineShortCutter<T>::printTrajectory(const trajectory_msgs::JointTrajectory &trajectory) const
00433 {
00434 for(unsigned int i = 0; i < trajectory.points.size(); i++)
00435 {
00436 ROS_DEBUG("%f: %f %f %f %f %f %f %f %f %f %f %f %f %f %f",
00437 trajectory.points[i].time_from_start.toSec(),
00438 trajectory.points[i].positions[0],
00439 trajectory.points[i].positions[1],
00440 trajectory.points[i].positions[2],
00441 trajectory.points[i].positions[3],
00442 trajectory.points[i].positions[4],
00443 trajectory.points[i].positions[5],
00444 trajectory.points[i].positions[6],
00445 trajectory.points[i].velocities[0],
00446 trajectory.points[i].velocities[1],
00447 trajectory.points[i].velocities[2],
00448 trajectory.points[i].velocities[3],
00449 trajectory.points[i].velocities[4],
00450 trajectory.points[i].velocities[5],
00451 trajectory.points[i].velocities[6]);
00452 }
00453 ROS_DEBUG(" ");
00454 }
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475 template <typename T>
00476 void CubicSplineShortCutter<T>::discretizeTrajectory(const spline_smoother::SplineTrajectory &spline,
00477 const double &discretization,
00478 trajectory_msgs::JointTrajectory &joint_trajectory) const
00479 {
00480 if(spline.segments.empty())
00481 return;
00482 joint_trajectory.points.clear();
00483 ros::Duration segment_start_time(0.0);
00484 for(unsigned int i=0; i < spline.segments.size(); i++)
00485 {
00486 if(i == spline.segments.size()-1)
00487 discretizeAndAppendSegment(spline.segments[i],discretization,joint_trajectory,segment_start_time,true);
00488 else
00489 discretizeAndAppendSegment(spline.segments[i],discretization,joint_trajectory,segment_start_time,false);
00490 segment_start_time += spline.segments[i].duration;
00491 ROS_DEBUG("Discretizing and appending segment %d",i);
00492 }
00493 }
00494
00495 template <typename T>
00496 void CubicSplineShortCutter<T>::discretizeAndAppendSegment(const spline_smoother::SplineTrajectorySegment &spline_segment,
00497 const double &discretization,
00498 trajectory_msgs::JointTrajectory &joint_trajectory,
00499 const ros::Duration &segment_start_time,
00500 const bool &include_segment_end) const
00501 {
00502 ros::Duration time_from_start = segment_start_time;
00503 double total_time = spline_segment.duration.toSec();
00504 double sample_time = 0.0;
00505 trajectory_msgs::JointTrajectoryPoint start,end;
00506 spline_smoother::sampleSplineTrajectory(spline_segment,0.0,start);
00507 if(joint_trajectory.points.empty())
00508 {
00509 start.time_from_start = ros::Duration(0.0);
00510 joint_trajectory.points.push_back(start);
00511 sample_time += 0.01;
00512 }
00513 start = joint_trajectory.points.back();
00514 while(sample_time < total_time)
00515 {
00516 ROS_DEBUG("Sample time is %f",sample_time);
00517 spline_smoother::sampleSplineTrajectory(spline_segment,sample_time,end);
00518 double max_diff = maxLInfDistance(start,end);
00519 if(sample_time > 0 && max_diff < discretization)
00520 {
00521 ROS_DEBUG("Max diff is %f. Skipping",max_diff);
00522 sample_time += 0.01;
00523 continue;
00524 }
00525 end.time_from_start = time_from_start + ros::Duration(sample_time);
00526 joint_trajectory.points.push_back(end);
00527 ROS_DEBUG("Pushing back point with time: %f",end.time_from_start.toSec());
00528 sample_time += 0.01;
00529 start = end;
00530 }
00531 if(include_segment_end)
00532 {
00533 spline_smoother::sampleSplineTrajectory(spline_segment,total_time,end);
00534 end.time_from_start = time_from_start + ros::Duration(total_time);
00535 joint_trajectory.points.push_back(end);
00536 }
00537 }
00538
00539 template <typename T>
00540 double CubicSplineShortCutter<T>::maxLInfDistance(const trajectory_msgs::JointTrajectoryPoint &start,
00541 const trajectory_msgs::JointTrajectoryPoint &end) const
00542 {
00543 double max_diff = 0.0;
00544 for(unsigned int i=0; i< start.positions.size(); i++)
00545 {
00546 double diff = fabs(end.positions[i]-start.positions[i]);
00547 if(diff > max_diff)
00548 max_diff = diff;
00549 }
00550 return max_diff;
00551 }
00552
00553
00554
00555 template <typename T>
00556 bool CubicSplineShortCutter<T>::trimTrajectory(trajectory_msgs::JointTrajectory &trajectory_out,
00557 const double &segment_start_time,
00558 const double &segment_end_time) const
00559 {
00560 int index1;
00561 int index2;
00562 if(findTrajectoryPointsInInterval(trajectory_out,segment_start_time,segment_end_time,index1,index2))
00563 {
00564 ROS_DEBUG("Trimming trajectory between segments: %d and %d",index1,index2);
00565 std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator remove_start = trajectory_out.points.begin() + index1;
00566 std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator remove_end;
00567 if((unsigned int) index2 >= trajectory_out.points.size())
00568 remove_end = trajectory_out.points.end();
00569 else
00570 remove_end = trajectory_out.points.begin()+index2;
00571
00572
00573 if(remove_start != remove_end)
00574 trajectory_out.points.erase(remove_start,remove_end);
00575 else
00576 trajectory_out.points.erase(remove_start);
00577 }
00578 else
00579 return false;
00580 return true;
00581 }
00582
00583 template <typename T>
00584 bool CubicSplineShortCutter<T>::findTrajectoryPointsInInterval(const trajectory_msgs::JointTrajectory &trajectory,
00585 const double &segment_start_time,
00586 const double &segment_end_time,
00587 int &index_1,
00588 int &index_2) const
00589 {
00590 index_1 = -1;
00591 index_2 = -1;
00592 if(segment_start_time > segment_end_time)
00593 return false;
00594 for(unsigned int i=0; i < trajectory.points.size(); i++)
00595 if(trajectory.points[i].time_from_start.toSec() >= segment_start_time)
00596 {
00597 index_1 = i;
00598 break;
00599 }
00600 ROS_DEBUG("First trim index: %d",index_1);
00601 if(index_1>=0)
00602 for(unsigned int i=index_1; i < trajectory.points.size(); i++)
00603 {
00604 if(trajectory.points[i].time_from_start.toSec() > segment_end_time)
00605 {
00606 index_2 = i;
00607 break;
00608 }
00609 if(trajectory.points[i].time_from_start.toSec() == segment_end_time)
00610 {
00611 index_2 = i+1;
00612 break;
00613 }
00614 }
00615 ROS_DEBUG("Second trim index: %d",index_2);
00616 if(index_1 >= index_2 || index_1 < 0 || index_2 < 0)
00617 return false;
00618 return true;
00619 }
00620
00621 template <typename T>
00622 bool CubicSplineShortCutter<T>::getWaypoints(const spline_smoother::SplineTrajectory &spline,
00623 trajectory_msgs::JointTrajectory &joint_trajectory) const
00624 {
00625 std::vector<double> waypoint_times_vector;
00626 double waypoint_time = 0.0;
00627 waypoint_times_vector.push_back(waypoint_time);
00628 for(unsigned int i=0; i < spline.segments.size(); i++)
00629 {
00630 waypoint_time = waypoint_time + spline.segments[i].duration.toSec();
00631 waypoint_times_vector.push_back(waypoint_time);
00632 ROS_DEBUG("Spline segment time: %f",spline.segments[i].duration.toSec());
00633 }
00634 if(!spline_smoother::sampleSplineTrajectory(spline,waypoint_times_vector,joint_trajectory))
00635 return false;
00636 return true;
00637 }
00638
00639 template <typename T>
00640 bool CubicSplineShortCutter<T>::addToTrajectory(trajectory_msgs::JointTrajectory &trajectory_out,
00641 const trajectory_msgs::JointTrajectoryPoint &trajectory_point,
00642 const ros::Duration &delta_time) const
00643 {
00644
00645 ROS_DEBUG("Inserting point at time: %f",trajectory_point.time_from_start.toSec());
00646 ROS_DEBUG("Old trajectory has %u points",trajectory_out.points.size());
00647
00648 if(trajectory_out.points.empty())
00649 {
00650 trajectory_out.points.push_back(trajectory_point);
00651 return true;
00652 }
00653
00654 unsigned int counter = 0;
00655 unsigned int old_size = trajectory_out.points.size();
00656 for(std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator iter = trajectory_out.points.begin();
00657 iter != trajectory_out.points.end() ; iter++)
00658 {
00659 if(iter->time_from_start >= trajectory_point.time_from_start)
00660 {
00661 trajectory_out.points.insert(iter,trajectory_point);
00662 break;
00663 }
00664 counter++;
00665 }
00666
00667 if(delta_time == ros::Duration(0.0))
00668 return true;
00669
00670 if(counter == old_size)
00671 trajectory_out.points.push_back(trajectory_point);
00672 else
00673 if(counter+1 < trajectory_out.points.size())
00674 for(unsigned int i= counter+1; i < trajectory_out.points.size(); i++)
00675 {
00676 trajectory_out.points[i].time_from_start += delta_time;
00677 }
00678 return true;
00679 }
00680
00681 template <typename T>
00682 bool CubicSplineShortCutter<T>::setupCollisionEnvironment()
00683 {
00684 bool use_collision_map;
00685 ros::NodeHandle node_handle("~");
00686 node_handle.param<bool>("use_collision_map", use_collision_map, true);
00687
00688
00689 collision_models_interface_ = new planning_environment::CollisionModelsInterface("robot_description");
00690 if(!collision_models_interface_->loadedModels())
00691 return false;
00692
00693 return true;
00694 }
00695 }
00696
00697 #endif
