39 #include <class_loader/class_loader.hpp>
45 static const rclcpp::Logger
LOGGER = rclcpp::get_logger(
"moveit_ros.add_time_optimal_parameterization");
55 void initialize(
const rclcpp::Node::SharedPtr& node,
const std::string& parameter_namespace)
override
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);
64 return "Add Time Optimal Parameterization";
69 std::vector<std::size_t>& )
const override
74 RCLCPP_DEBUG(LOGGER,
" Running '%s'", getDescription().c_str());
77 req.max_acceleration_scaling_factor))
79 RCLCPP_WARN(LOGGER,
" Time parametrization for the solution path failed.");
This adapter uses the time-optimal trajectory generation method.
std::string getDescription() const override
Get a short string that identifies the planning request adapter.
AddTimeOptimalParameterization()
void initialize(const rclcpp::Node::SharedPtr &node, const std::string ¶meter_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
robot_trajectory::RobotTrajectoryPtr trajectory_
CLASS_LOADER_REGISTER_CLASS(trajopt_interface::TrajOptPlannerManager, planning_interface::PlannerManager)