43 #include <class_loader/class_loader.hpp>
44 #include <moveit_msgs/msg/move_it_error_codes.hpp>
61 return std::string(
"AddRuckigTrajectorySmoothing");
64 void adapt(
const planning_scene::PlanningSceneConstPtr& ,
68 RCLCPP_DEBUG(logger_,
" Running '%s'", getDescription().c_str());
73 "Cannot apply response adapter '%s' because MotionPlanResponse does not contain a trajectory to smooth.",
74 getDescription().c_str());
75 res.
error_code = moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN;
79 if (smoother_.applySmoothing(*res.
trajectory, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor))
81 res.
error_code = moveit::core::MoveItErrorCode::SUCCESS;
85 RCLCPP_ERROR(logger_,
"Response adapter '%s' failed to smooth trajectory.", getDescription().c_str());
86 res.
error_code = moveit::core::MoveItErrorCode::FAILURE;
92 rclcpp::Logger logger_;
Use ruckig (https://github.com/pantor/ruckig) to adapt the trajectory to be jerk-constrained and time...
std::string getDescription() const override
Get a description of this adapter.
AddRuckigTrajectorySmoothing()
void adapt(const planning_scene::PlanningSceneConstPtr &, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res) const override
Adapt the planning response.
Concept in MoveIt which can be used to modify the planning solution (post-processing) in a planning p...
rclcpp::Logger getLogger()
Main namespace for MoveIt.
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
CLASS_LOADER_REGISTER_CLASS(default_planning_request_adapters::ResolveConstraintFrames, planning_interface::PlanningRequestAdapter)
Response to a planning query.
moveit::core::MoveItErrorCode error_code
robot_trajectory::RobotTrajectoryPtr trajectory