47 static const rclcpp::Logger
LOGGER = rclcpp::get_logger(
"moveit.ros_planning_interface.planning_component");
52 joint_model_group_ = moveit_cpp_->getRobotModel()->getJointModelGroup(group_name);
53 if (!joint_model_group_)
55 std::string error =
"Could not find joint model group '" + group_name +
"'.";
56 RCLCPP_FATAL_STREAM(LOGGER, error);
57 throw std::runtime_error(error);
59 planning_pipeline_names_ = moveit_cpp_->getPlanningPipelineNames(group_name);
60 plan_request_parameters_.
load(node_);
62 LOGGER,
"Plan request parameters loaded with --"
64 <<
" planner_id: " << plan_request_parameters_.
planner_id <<
","
65 <<
" planning_time: " << plan_request_parameters_.
planning_time <<
","
74 joint_model_group_ = moveit_cpp_->getRobotModel()->getJointModelGroup(group_name);
75 if (!joint_model_group_)
77 std::string error =
"Could not find joint model group '" + group_name +
"'.";
78 RCLCPP_FATAL_STREAM(LOGGER, error);
79 throw std::runtime_error(error);
81 planning_pipeline_names_ = moveit_cpp_->getPlanningPipelineNames(group_name);
86 if (joint_model_group_)
92 RCLCPP_WARN(LOGGER,
"Unable to find joint group with name '%s'.", group_name_.c_str());
95 std::vector<std::string> empty;
112 last_plan_solution_ = std::make_shared<PlanSolution>();
113 if (!joint_model_group_)
115 RCLCPP_ERROR(LOGGER,
"Failed to retrieve joint model group for name '%s'.", group_name_.c_str());
116 last_plan_solution_->error_code = moveit::core::MoveItErrorCode::INVALID_GROUP_NAME;
117 return *last_plan_solution_;
122 moveit_cpp_->getPlanningSceneMonitorNonConst();
129 req.group_name = group_name_;
135 if (workspace_parameters_set_)
136 req.workspace_parameters = workspace_parameters_;
139 moveit::core::RobotStatePtr start_state = considered_start_state_;
141 start_state = moveit_cpp_->getCurrentState();
142 start_state->update();
147 if (current_goal_constraints_.empty())
149 RCLCPP_ERROR(LOGGER,
"No goal constraints set for planning request");
150 last_plan_solution_->error_code = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS;
151 return *last_plan_solution_;
153 req.goal_constraints = current_goal_constraints_;
156 req.path_constraints = current_path_constraints_;
160 if (planning_pipeline_names_.find(parameters.
planning_pipeline) == planning_pipeline_names_.end())
162 RCLCPP_ERROR(LOGGER,
"No planning pipeline available for name '%s'", parameters.
planning_pipeline.c_str());
163 last_plan_solution_->error_code = moveit::core::MoveItErrorCode::FAILURE;
164 return *last_plan_solution_;
166 const planning_pipeline::PlanningPipelinePtr pipeline =
169 last_plan_solution_->error_code = res.
error_code_.val;
172 RCLCPP_ERROR(LOGGER,
"Could not compute plan successfully");
173 return *last_plan_solution_;
175 last_plan_solution_->start_state = req.start_state;
189 return *last_plan_solution_;
194 return plan(plan_request_parameters_);
199 considered_start_state_ = std::make_shared<moveit::core::RobotState>(start_state);
205 if (considered_start_state_)
206 return considered_start_state_;
209 moveit::core::RobotStatePtr s;
210 moveit_cpp_->getCurrentState(s, 1.0);
218 if (std::find(named_targets.begin(), named_targets.end(), start_state_name) == named_targets.end())
220 RCLCPP_ERROR(LOGGER,
"No predefined joint state found for target name '%s'", start_state_name.c_str());
230 considered_start_state_.reset();
236 std::map<std::string, double> positions;
243 workspace_parameters_.header.frame_id = moveit_cpp_->getRobotModel()->getModelFrame();
244 workspace_parameters_.header.stamp = node_->now();
245 workspace_parameters_.min_corner.x = minx;
246 workspace_parameters_.min_corner.y = miny;
247 workspace_parameters_.min_corner.z = minz;
248 workspace_parameters_.max_corner.x = maxx;
249 workspace_parameters_.max_corner.y = maxy;
250 workspace_parameters_.max_corner.z = maxz;
251 workspace_parameters_set_ =
true;
256 workspace_parameters_set_ =
false;
261 current_goal_constraints_ = goal_constraints;
280 if (std::find(named_targets.begin(), named_targets.end(), goal_state_name) == named_targets.end())
282 RCLCPP_ERROR(LOGGER,
"No predefined joint state found for target name '%s'", goal_state_name.c_str());
292 if (!last_plan_solution_)
294 RCLCPP_ERROR(LOGGER,
"There is no successful plan to execute");
306 return moveit_cpp_->execute(group_name_, last_plan_solution_->trajectory, blocking);
311 return last_plan_solution_;
bool getVariableDefaultPositions(const std::string &name, std::map< std::string, double > &values) const
Get the values that correspond to a named state as read from the URDF. Return false on failure.
const std::vector< std::string > & getDefaultStateNames() const
Get the names of the known default states (as specified in the SRDF)
Representation of a robot's state. This includes position, velocity, acceleration and effort.
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
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.
void unsetWorkspace()
Remove the workspace bounding box from planning.
bool setGoal(const std::vector< moveit_msgs::msg::Constraints > &goal_constraints)
Set the goal constraints used for planning.
PlanSolution plan()
Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() usi...
const std::string & getPlanningGroupName() const
Get the name of the planning group.
const PlanSolutionPtr getLastPlanSolution()
Return the last plan solution.
PlanningComponent(const std::string &group_name, const rclcpp::Node::SharedPtr &node)
Constructor.
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.
const std::vector< std::string > getNamedTargetStates()
Get the names of the named robot states available as targets.
bool execute(bool blocking=true)
Execute the latest computed solution trajectory computed by plan(). By default this function terminat...
moveit::core::RobotStatePtr getStartState()
Get the considered start state if defined, otherwise get the current state.
moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::RobotState &state, const moveit::core::JointModelGroup *jmg, double tolerance_below, double tolerance_above)
Generates a constraint message intended to be used as a goal constraint for a joint group....
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::msg::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt robot state to a robot state message.
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
This namespace includes the central class for representing planning contexts.
const rclcpp::Logger LOGGER
Planner parameters provided with the MotionPlanRequest.
double max_velocity_scaling_factor
void load(const rclcpp::Node::SharedPtr &node)
double max_acceleration_scaling_factor
std::string planning_pipeline
The representation of a plan solution.
robot_trajectory::RobotTrajectoryPtr trajectory_
moveit_msgs::msg::MoveItErrorCodes error_code_