Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "state_validation_service_capability.h"
00038 #include <moveit/robot_state/conversions.h>
00039 #include <moveit/kinematic_constraints/utils.h>
00040 #include <moveit/collision_detection/collision_tools.h>
00041 #include <eigen_conversions/eigen_msg.h>
00042 #include <moveit/move_group/capability_names.h>
00043
00044 move_group::MoveGroupStateValidationService::MoveGroupStateValidationService()
00045 : MoveGroupCapability("StateValidationService")
00046 {
00047 }
00048
00049 void move_group::MoveGroupStateValidationService::initialize()
00050 {
00051 validity_service_ = root_node_handle_.advertiseService(STATE_VALIDITY_SERVICE_NAME,
00052 &MoveGroupStateValidationService::computeService, this);
00053 }
00054
00055 bool move_group::MoveGroupStateValidationService::computeService(moveit_msgs::GetStateValidity::Request& req,
00056 moveit_msgs::GetStateValidity::Response& res)
00057 {
00058 planning_scene_monitor::LockedPlanningSceneRO ls(context_->planning_scene_monitor_);
00059 robot_state::RobotState rs = ls->getCurrentState();
00060 robot_state::robotStateMsgToRobotState(req.robot_state, rs);
00061
00062 res.valid = true;
00063
00064
00065 collision_detection::CollisionRequest creq;
00066 creq.group_name = req.group_name;
00067 creq.cost = true;
00068 creq.contacts = true;
00069 creq.max_contacts = ls->getWorld()->size();
00070 creq.max_cost_sources = creq.max_contacts + ls->getRobotModel()->getLinkModelsWithCollisionGeometry().size();
00071 creq.max_contacts *= creq.max_contacts;
00072 collision_detection::CollisionResult cres;
00073
00074
00075 ls->checkCollision(creq, cres, rs);
00076
00077
00078 if (cres.collision)
00079 {
00080 ros::Time time_now = ros::Time::now();
00081 res.contacts.reserve(cres.contact_count);
00082 res.valid = false;
00083 for (collision_detection::CollisionResult::ContactMap::const_iterator it = cres.contacts.begin();
00084 it != cres.contacts.end(); ++it)
00085 for (std::size_t k = 0; k < it->second.size(); ++k)
00086 {
00087 res.contacts.resize(res.contacts.size() + 1);
00088 collision_detection::contactToMsg(it->second[k], res.contacts.back());
00089 res.contacts.back().header.frame_id = ls->getPlanningFrame();
00090 res.contacts.back().header.stamp = time_now;
00091 }
00092 }
00093
00094
00095 res.cost_sources.reserve(cres.cost_sources.size());
00096 for (std::set<collision_detection::CostSource>::const_iterator it = cres.cost_sources.begin();
00097 it != cres.cost_sources.end(); ++it)
00098 {
00099 res.cost_sources.resize(res.cost_sources.size() + 1);
00100 collision_detection::costSourceToMsg(*it, res.cost_sources.back());
00101 }
00102
00103
00104 if (!kinematic_constraints::isEmpty(req.constraints))
00105 {
00106 kinematic_constraints::KinematicConstraintSet kset(ls->getRobotModel());
00107 kset.add(req.constraints, ls->getTransforms());
00108 std::vector<kinematic_constraints::ConstraintEvaluationResult> kres;
00109 kinematic_constraints::ConstraintEvaluationResult total_result = kset.decide(rs, kres);
00110 if (!total_result.satisfied)
00111 res.valid = false;
00112
00113
00114 res.constraint_result.resize(kres.size());
00115 for (std::size_t k = 0; k < kres.size(); ++k)
00116 {
00117 res.constraint_result[k].result = kres[k].satisfied;
00118 res.constraint_result[k].distance = kres[k].distance;
00119 }
00120 }
00121
00122 return true;
00123 }
00124
00125 #include <class_loader/class_loader.h>
00126 CLASS_LOADER_REGISTER_CLASS(move_group::MoveGroupStateValidationService, move_group::MoveGroupCapability)