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
00033
00034
00035
00036
00037 #include <moveit/planning_request_adapter/planning_request_adapter.h>
00038 #include <moveit/trajectory_processing/iterative_time_parameterization.h>
00039 #include <class_loader/class_loader.h>
00040 #include <ros/console.h>
00041
00042 namespace default_planner_request_adapters
00043 {
00044 class AddTimeParameterization : public planning_request_adapter::PlanningRequestAdapter
00045 {
00046 public:
00047 AddTimeParameterization() : planning_request_adapter::PlanningRequestAdapter()
00048 {
00049 }
00050
00051 virtual std::string getDescription() const
00052 {
00053 return "Add Time Parameterization";
00054 }
00055
00056 virtual bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
00057 const planning_interface::MotionPlanRequest& req,
00058 planning_interface::MotionPlanResponse& res,
00059 std::vector<std::size_t>& added_path_index) const
00060 {
00061 bool result = planner(planning_scene, req, res);
00062 if (result && res.trajectory_)
00063 {
00064 ROS_DEBUG("Running '%s'", getDescription().c_str());
00065 if (!time_param_.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor,
00066 req.max_acceleration_scaling_factor))
00067 ROS_WARN("Time parametrization for the solution path failed.");
00068 }
00069
00070 return result;
00071 }
00072
00073 private:
00074 trajectory_processing::IterativeParabolicTimeParameterization time_param_;
00075 };
00076 }
00077
00078 CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::AddTimeParameterization,
00079 planning_request_adapter::PlanningRequestAdapter);