40#include <geometry_msgs/msg/pose_stamped.hpp>
49#include <rclcpp/rclcpp.hpp>
69 void declareOrGetParam(
const rclcpp::Node::SharedPtr& node,
const std::string& param_name, T& output_value,
73 if (!node->get_parameter_or(param_name, output_value, default_value))
75 RCLCPP_WARN(node->get_logger(),
76 "Parameter \'%s\' not found in config use default value instead, check parameter type and "
77 "namespace in YAML file",
78 (param_name).c_str());
82 void load(
const rclcpp::Node::SharedPtr& node,
const std::string& param_namespace =
"")
85 std::string ns =
"plan_request_params.";
86 if (!param_namespace.empty())
88 ns = param_namespace +
".plan_request_params.";
92 declareOrGetParam<std::string>(node, ns +
"planner_id",
planner_id, std::string(
""));
93 declareOrGetParam<std::string>(node, ns +
"planning_pipeline",
planning_pipeline, std::string(
""));
94 declareOrGetParam<double>(node, ns +
"planning_time",
planning_time, 1.0);
109 const std::vector<std::string>& planning_pipeline_names)
113 for (
const auto& planning_pipeline_name : planning_pipeline_names)
116 parameters.
load(node, planning_pipeline_name);
131 PlanningComponent(
const std::string& group_name,
const rclcpp::Node::SharedPtr& node);
161 void setWorkspace(
double minx,
double miny,
double minz,
double maxx,
double maxy,
double maxz);
178 bool setGoal(
const std::vector<moveit_msgs::msg::Constraints>& goal_constraints);
182 bool setGoal(
const geometry_msgs::msg::PoseStamped& goal_pose,
const std::string& link_name);
184 bool setGoal(
const std::string& named_target);
212 [[deprecated(
"Use MoveItCpp::execute()")]]
bool execute(
bool )
223 std::vector<::planning_interface::MotionPlanRequest>
228 rclcpp::Node::SharedPtr node_;
229 MoveItCppPtr moveit_cpp_;
230 const std::string group_name_;
236 moveit::core::RobotStatePtr considered_start_state_;
237 std::vector<moveit_msgs::msg::Constraints> current_goal_constraints_;
238 moveit_msgs::msg::Constraints current_path_constraints_;
239 moveit_msgs::msg::TrajectoryConstraints current_trajectory_constraints_;
240 moveit_msgs::msg::WorkspaceParameters workspace_parameters_;
241 bool workspace_parameters_set_ =
false;
242 rclcpp::Logger logger_;
#define MOVEIT_CLASS_FORWARD(C)
Representation of a robot's state. This includes position, velocity, acceleration and effort.
PlanningComponent & operator=(const PlanningComponent &)=delete
void setStartStateToCurrentState()
Set the start state for planning to be the current state of the robot.
bool setStartState(const moveit::core::RobotState &start_state)
Set the robot state that should be considered as start state for planning.
std::map< std::string, double > getNamedTargetStateValues(const std::string &name)
Get the joint values for targets specified by name.
PlanningComponent(PlanningComponent &&other)=delete
planning_interface::MotionPlanResponse plan()
Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() usi...
void unsetWorkspace()
Remove the workspace bounding box from planning.
bool execute(bool)
Execute the latest computed solution trajectory computed by plan(). By default this function terminat...
bool setGoal(const std::vector< moveit_msgs::msg::Constraints > &goal_constraints)
Set the goal constraints used for planning.
~PlanningComponent()=default
Destructor.
const std::string & getPlanningGroupName() const
Get the name of the planning group.
PlanningComponent(const PlanningComponent &)=delete
This class owns unique resources (e.g. action clients, threads) and its not very meaningful to copy....
::planning_interface::MotionPlanRequest getMotionPlanRequest(const PlanRequestParameters &plan_request_parameters)
Utility function to get a MotionPlanRequest from PlanRequestParameters and the internal state of the ...
PlanningComponent & operator=(PlanningComponent &&other)=delete
bool setTrajectoryConstraints(const moveit_msgs::msg::TrajectoryConstraints &trajectory_constraints)
Set the trajectory constraints generated from a moveit msg Constraints.
void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
Specify the workspace bounding box. The box is specified in the planning frame (i....
bool setPathConstraints(const moveit_msgs::msg::Constraints &path_constraints)
Set the path constraints generated from a moveit msg Constraints.
std::vector<::planning_interface::MotionPlanRequest > getMotionPlanRequestVector(const MultiPipelinePlanRequestParameters &multi_pipeline_plan_request_parameters)
Utility function to get a Vector of MotionPlanRequest from a vector of PlanRequestParameters and the ...
const std::vector< std::string > getNamedTargetStates()
Get the names of the named robot states available as targets.
moveit::core::RobotStatePtr getStartState()
Get the considered start state if defined, otherwise get the current state.
std::function< bool(const PlanResponsesContainer &plan_responses_container, const std::vector<::planning_interface::MotionPlanRequest > &plan_requests)> StoppingCriterionFunction
A stopping criterion callback function for the parallel planning API of planning component.
::planning_interface::MotionPlanResponse getShortestSolution(const std::vector<::planning_interface::MotionPlanResponse > &solutions)
Function that returns the shortest solution out of a vector of solutions based on robot_trajectory::p...
std::function<::planning_interface::MotionPlanResponse(const std::vector<::planning_interface::MotionPlanResponse > &solutions)> SolutionSelectionFunction
A solution callback function type for the parallel planning API of planning component.
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
This namespace includes the central class for representing planning contexts.
Planner parameters provided with the MotionPlanRequest.
std::vector< PlanRequestParameters > plan_request_parameter_vector
MultiPipelinePlanRequestParameters()
MultiPipelinePlanRequestParameters(const rclcpp::Node::SharedPtr &node, const std::vector< std::string > &planning_pipeline_names)
Planner parameters provided with the MotionPlanRequest.
void load(const rclcpp::Node::SharedPtr &node, const std::string ¶m_namespace="")
double max_velocity_scaling_factor
double max_acceleration_scaling_factor
void declareOrGetParam(const rclcpp::Node::SharedPtr &node, const std::string ¶m_name, T &output_value, T default_value)
std::string planning_pipeline
Response to a planning query.