44 const planning_scene::PlanningSceneConstPtr& scene,
45 const collision_detection::AllowedCollisionMatrixConstPtr& collision_matrix,
46 const constraint_samplers::ConstraintSamplerManagerPtr& constraints_sampler_manager)
48 , planning_scene_(scene)
49 , collision_matrix_(collision_matrix)
50 , constraints_sampler_manager_(constraints_sampler_manager)
71 planning_scene->checkCollision(req, res, *state, *collision_matrix);
78 for (std::size_t i = 0; i < manipulation_plan->
approach_posture_.points.size(); ++i)
84 planning_scene->checkCollision(req, res, *state, *collision_matrix);
95 tf2::fromMsg(plan->goal_pose_.pose, plan->transformed_goal_pose_);
96 plan->transformed_goal_pose_ =
97 planning_scene_->getFrameTransform(token_state, plan->goal_pose_.header.frame_id) * plan->transformed_goal_pose_;
102 req.
group_name = plan->shared_data_->end_effector_group_->getName();
103 planning_scene_->checkCollision(req, res, token_state, *collision_matrix_);
111 if (isEndEffectorFree(plan, *token_state))
119 plan->goal_pose_.pose =
tf2::toMsg(plan->transformed_goal_pose_);
120 plan->goal_pose_.header.frame_id = planning_scene_->getPlanningFrame();
124 plan->goal_constraints_ =
127 const std::string& planning_group = plan->shared_data_->planning_group_->getName();
131 plan->goal_sampler_ =
132 constraints_sampler_manager_->selectSampler(planning_scene_, planning_group, plan->goal_constraints_);
133 if (plan->goal_sampler_)
135 plan->goal_sampler_->setGroupStateValidityCallback(
136 [scene = planning_scene_.get(), acm = collision_matrix_.get(),
verbose = verbose_,
138 const double* joint_group_variable_values) {
139 return isStateCollisionFree(scene, acm, verbose, p, robot_state, joint_group, joint_group_variable_values);
141 plan->goal_sampler_->setVerbose(verbose_);
142 if (plan->goal_sampler_->sample(*token_state, plan->shared_data_->max_goal_sampling_attempts_))
144 plan->possible_goal_states_.push_back(token_state);
148 ROS_INFO_NAMED(
"manipulation",
"Sampler failed to produce a state");
153 plan->error_code_.val = moveit_msgs::MoveItErrorCodes::GOAL_IN_COLLISION;