void plan_execution::PlanExecution::planAndExecute(ExecutableMotionPlan &plan, const moveit_msgs::PlanningScene &scene_diff, const Options &opt)
{
if (planning_scene::PlanningScene::isEmpty(scene_diff))
planAndExecute(plan, opt);
else
{
plan.planning_scene_monitor_ = planning_scene_monitor_;
{
planning_scene_monitor::LockedPlanningSceneRO lscene(planning_scene_monitor_); // lock the scene so that it does not modify the world representation while diff() is called
plan.planning_scene_ = lscene->diff(scene_diff);
}
planAndExecuteHelper(plan, opt);
}
}
void move_group::MoveGroupPickPlaceAction::fillGrasps(moveit_msgs::PickupGoal& goal)
{
planning_scene_monitor::LockedPlanningSceneRO lscene(planning_scene_monitor_);
if (lscene->hasObjectType(goal.target_name))
{
// const object_recognition_msgs::ObjectType &ot = lscene->getObjectType(goal->target_name);
// need to call the grasp planner here
}
if (goal.possible_grasps.empty())
{
// add a number of default grasp points
// \todo add more!
manipulation_msgs::Grasp g;
g.grasp_pose.header.frame_id = goal.target_name;
g.grasp_pose.pose.position.x = 0.0;
g.grasp_pose.pose.position.y = 0.0;
g.grasp_pose.pose.position.z = 0.0;
g.grasp_pose.pose.orientation.x = 0.0;
g.grasp_pose.pose.orientation.y = 0.0;
g.grasp_pose.pose.orientation.z = 0.0;
g.grasp_pose.pose.orientation.w = 1.0;
g.approach.direction.vector.x = 1.0;
g.approach.min_distance = 0.1;
g.approach.desired_distance = 0.3;
g.retreat.direction.vector.z = 1.0;
g.retreat.direction.header.frame_id = lscene->getPlanningFrame();
g.retreat.min_distance = 0.1;
g.retreat.desired_distance = 0.3;
if (lscene->getRobotModel()->hasEndEffector(goal.end_effector))
{
g.pre_grasp_posture.name = lscene->getRobotModel()->getEndEffector(goal.end_effector)->getJointModelNames();
g.pre_grasp_posture.position.resize(g.pre_grasp_posture.name.size(), std::numeric_limits<double>::max());
g.grasp_posture.name = g.pre_grasp_posture.name;
g.grasp_posture.position.resize(g.grasp_posture.name.size(), -std::numeric_limits<double>::max());
}
goal.possible_grasps.push_back(g);
}
}
void move_group::MoveGroupPickPlaceAction::fillGrasps(moveit_msgs::PickupGoal& goal)
{
planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);
ROS_DEBUG_NAMED("manipulation", "Using default grasp poses");
goal.minimize_object_distance = true;
// add a number of default grasp points
// \todo add more!
moveit_msgs::Grasp g;
g.grasp_pose.header.frame_id = goal.target_name;
g.grasp_pose.pose.position.x = -0.2;
g.grasp_pose.pose.position.y = 0.0;
g.grasp_pose.pose.position.z = 0.0;
g.grasp_pose.pose.orientation.x = 0.0;
g.grasp_pose.pose.orientation.y = 0.0;
g.grasp_pose.pose.orientation.z = 0.0;
g.grasp_pose.pose.orientation.w = 1.0;
g.pre_grasp_approach.direction.header.frame_id = lscene->getPlanningFrame();
g.pre_grasp_approach.direction.vector.x = 1.0;
g.pre_grasp_approach.min_distance = 0.1;
g.pre_grasp_approach.desired_distance = 0.2;
g.post_grasp_retreat.direction.header.frame_id = lscene->getPlanningFrame();
g.post_grasp_retreat.direction.vector.z = 1.0;
g.post_grasp_retreat.min_distance = 0.1;
g.post_grasp_retreat.desired_distance = 0.2;
if (lscene->getRobotModel()->hasEndEffector(goal.end_effector))
{
g.pre_grasp_posture.joint_names = lscene->getRobotModel()->getEndEffector(goal.end_effector)->getJointModelNames();
g.pre_grasp_posture.points.resize(1);
g.pre_grasp_posture.points[0].positions.resize(g.pre_grasp_posture.joint_names.size(),
std::numeric_limits<double>::max());
g.grasp_posture.joint_names = g.pre_grasp_posture.joint_names;
g.grasp_posture.points.resize(1);
g.grasp_posture.points[0].positions.resize(g.grasp_posture.joint_names.size(), -std::numeric_limits<double>::max());
}
goal.possible_grasps.push_back(g);
}
bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionPlan &plan, const std::pair<int, int> &path_segment)
{
if (path_segment.first >= 0 && path_segment.second >= 0 && plan.plan_components_[path_segment.first].trajectory_monitoring_)
{
planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_); // lock the scene so that it does not modify the world representation while isStateValid() is called
const robot_trajectory::RobotTrajectory &t = *plan.plan_components_[path_segment.first].trajectory_;
const collision_detection::AllowedCollisionMatrix *acm = plan.plan_components_[path_segment.first].allowed_collision_matrix_.get();
std::size_t wpc = t.getWayPointCount();
collision_detection::CollisionRequest req;
req.group_name = t.getGroupName();
for (std::size_t i = std::max(path_segment.second - 1, 0) ; i < wpc ; ++i)
{
collision_detection::CollisionResult res;
if (acm)
plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i), *acm);
else
plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i));
if (res.collision || !plan.planning_scene_->isStateFeasible(t.getWayPoint(i), false))
{
// Dave's debacle
ROS_INFO("Trajectory component '%s' is invalid", plan.plan_components_[path_segment.first].description_.c_str());
// call the same functions again, in verbose mode, to show what issues have been detected
plan.planning_scene_->isStateFeasible(t.getWayPoint(i), true);
req.verbose = true;
res.clear();
if (acm)
plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i), *acm);
else
plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i));
return false;
}
}
}
return true;
}
bool plan_execution::PlanWithSensing::computePlan(ExecutableMotionPlan& plan,
const ExecutableMotionPlanComputationFn& motion_planner,
unsigned int max_look_attempts, double max_safe_path_cost)
{
if (max_safe_path_cost <= std::numeric_limits<double>::epsilon())
max_safe_path_cost = default_max_safe_path_cost_;
if (max_look_attempts == 0)
max_look_attempts = default_max_look_attempts_;
double previous_cost = 0.0;
unsigned int look_attempts = 0;
// this flag is set to true when all conditions for looking around are met, and the command is sent.
// the intention is for the planning looop not to terminate when having just looked around
bool just_looked_around = false;
// this flag indicates whether the last lookAt() operation failed. If this operation fails once, we assume that
// maybe some information was gained anyway (the sensor moved part of the way) and we try to plan one more time.
// If we have two sensor pointing failures in a row, we fail
bool look_around_failed = false;
// there can be a maximum number of looking attempts as well that lead to replanning, if the cost
// of the path is above a maximum threshold.
do
{
bool solved = motion_planner(plan);
if (!solved || plan.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
return solved;
// determine the sources of cost for this path
std::set<collision_detection::CostSource> cost_sources;
{
planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_); // it is ok if
// planning_scene_monitor_ is
// null; there just will be
// no locking done
for (std::size_t i = 0; i < plan.plan_components_.size(); ++i)
{
std::set<collision_detection::CostSource> cost_sources_i;
plan.planning_scene_->getCostSources(*plan.plan_components_[i].trajectory_, max_cost_sources_,
plan.plan_components_[i].trajectory_->getGroupName(), cost_sources_i,
discard_overlapping_cost_sources_);
cost_sources.insert(cost_sources_i.begin(), cost_sources_i.end());
if (cost_sources.size() > max_cost_sources_)
{
std::set<collision_detection::CostSource> other;
other.swap(cost_sources);
std::size_t j = 0;
for (std::set<collision_detection::CostSource>::iterator it = other.begin(); j < max_cost_sources_; ++it, ++j)
cost_sources.insert(*it);
}
}
}
// display the costs if needed
if (display_cost_sources_)
{
visualization_msgs::MarkerArray arr;
collision_detection::getCostMarkers(arr, plan.planning_scene_->getPlanningFrame(), cost_sources);
cost_sources_publisher_.publish(arr);
}
double cost = collision_detection::getTotalCost(cost_sources);
ROS_DEBUG("The total cost of the trajectory is %lf.", cost);
if (previous_cost > 0.0)
ROS_DEBUG("The change in the trajectory cost is %lf after the perception step.", cost - previous_cost);
if (cost > max_safe_path_cost && look_attempts < max_look_attempts)
{
++look_attempts;
ROS_INFO("The cost of the trajectory is %lf, which is above the maximum safe cost of %lf. Attempt %u (of at most "
"%u) at looking around.",
cost, max_safe_path_cost, look_attempts, max_look_attempts);
bool looked_at_result = lookAt(cost_sources, plan.planning_scene_->getPlanningFrame());
if (looked_at_result)
ROS_INFO("Sensor was succesfully actuated. Attempting to recompute a motion plan.");
else
{
if (look_around_failed)
ROS_WARN("Looking around seems to keep failing. Giving up.");
else
ROS_WARN("Looking around seems to have failed. Attempting to recompute a motion plan anyway.");
}
if (looked_at_result || !look_around_failed)
{
previous_cost = cost;
just_looked_around = true;
}
look_around_failed = !looked_at_result;
// if we are unable to look, let this loop continue into the next if statement
if (just_looked_around)
continue;
}
if (cost > max_safe_path_cost)
{
plan.error_code_.val = moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA;
return true;
}
else
return true;
} while (true);
return false;
}
