moveit2
The MoveIt Motion Planning Framework for ROS 2.
add_time_optimal_parameterization.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
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
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan, Michael Ferguson */
36 
39 #include <class_loader/class_loader.hpp>
40 
42 {
43 using namespace trajectory_processing;
44 
45 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.add_time_optimal_parameterization");
46 
49 {
50 public:
52  {
53  }
54 
55  void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override
56  {
57  path_tolerance_ = getParam(node, LOGGER, parameter_namespace, "path_tolerance", 0.1);
58  resample_dt_ = getParam(node, LOGGER, parameter_namespace, "resample_dt", 0.1);
59  min_angle_change_ = getParam(node, LOGGER, parameter_namespace, "min_angle_change", 0.001);
60  }
61 
62  std::string getDescription() const override
63  {
64  return "Add Time Optimal Parameterization";
65  }
66 
67  bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
69  std::vector<std::size_t>& /*added_path_index*/) const override
70  {
71  bool result = planner(planning_scene, req, res);
72  if (result && res.trajectory_)
73  {
74  RCLCPP_DEBUG(LOGGER, " Running '%s'", getDescription().c_str());
75  TimeOptimalTrajectoryGeneration totg(path_tolerance_, resample_dt_, min_angle_change_);
76  if (!totg.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor,
77  req.max_acceleration_scaling_factor))
78  {
79  RCLCPP_WARN(LOGGER, " Time parametrization for the solution path failed.");
80  result = false;
81  }
82  }
83 
84  return result;
85  }
86 
87 protected:
89  double resample_dt_;
91 };
92 
93 } // namespace default_planner_request_adapters
94 
This adapter uses the time-optimal trajectory generation method.
std::string getDescription() const override
Get a short string that identifies the planning request adapter.
void initialize(const rclcpp::Node::SharedPtr &node, const std::string &parameter_namespace) override
Initialize parameters using the passed Node and parameter namespace. If no initialization is needed,...
bool adaptAndPlan(const PlannerFn &planner, const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res, std::vector< std::size_t > &) const override
Adapt the planning request if needed, call the planner function planner and update the planning respo...
std::function< bool(const planning_scene::PlanningSceneConstPtr &, const planning_interface::MotionPlanRequest &, planning_interface::MotionPlanResponse &)> PlannerFn
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const override
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
Generic interface to adapting motion planning requests.
This namespace includes the central class for representing planning contexts.
const rclcpp::Logger LOGGER
Definition: async_test.h:31
robot_trajectory::RobotTrajectoryPtr trajectory_
CLASS_LOADER_REGISTER_CLASS(trajopt_interface::TrajOptPlannerManager, planning_interface::PlannerManager)