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, &MoveGroupStateValidationService::computeService, this);
00052 }
00053
00054 bool move_group::MoveGroupStateValidationService::computeService(moveit_msgs::GetStateValidity::Request &req, moveit_msgs::GetStateValidity::Response &res)
00055 {
00056 planning_scene_monitor::LockedPlanningSceneRO ls(context_->planning_scene_monitor_);
00057 robot_state::RobotState rs = ls->getCurrentState();
00058 robot_state::robotStateMsgToRobotState(req.robot_state, rs);
00059
00060 res.valid = true;
00061
00062
00063 collision_detection::CollisionRequest creq;
00064 creq.group_name = req.group_name;
00065 creq.cost = true;
00066 creq.contacts = true;
00067 creq.max_contacts = ls->getWorld()->size();
00068 creq.max_cost_sources = creq.max_contacts + ls->getRobotModel()->getLinkModelsWithCollisionGeometry().size();
00069 creq.max_contacts *= creq.max_contacts;
00070 collision_detection::CollisionResult cres;
00071
00072
00073 ls->checkCollision(creq, cres, rs);
00074
00075
00076 if (cres.collision)
00077 {
00078 ros::Time time_now = ros::Time::now();
00079 res.contacts.reserve(cres.contact_count);
00080 res.valid = false;
00081 for (collision_detection::CollisionResult::ContactMap::const_iterator it = cres.contacts.begin() ; it != cres.contacts.end() ; ++it)
00082 for (std::size_t k = 0 ; k < it->second.size() ; ++k)
00083 {
00084 res.contacts.resize(res.contacts.size() + 1);
00085 collision_detection::contactToMsg(it->second[k], res.contacts.back());
00086 res.contacts.back().header.frame_id = ls->getPlanningFrame();
00087 res.contacts.back().header.stamp = time_now;
00088 }
00089 }
00090
00091
00092 res.cost_sources.reserve(cres.cost_sources.size());
00093 for (std::set<collision_detection::CostSource>::const_iterator it = cres.cost_sources.begin() ; it != cres.cost_sources.end() ; ++it)
00094 {
00095 res.cost_sources.resize(res.cost_sources.size() + 1);
00096 collision_detection::costSourceToMsg(*it, res.cost_sources.back());
00097 }
00098
00099
00100 if (!kinematic_constraints::isEmpty(req.constraints))
00101 {
00102 kinematic_constraints::KinematicConstraintSet kset(ls->getRobotModel());
00103 kset.add(req.constraints, ls->getTransforms());
00104 std::vector<kinematic_constraints::ConstraintEvaluationResult> kres;
00105 kinematic_constraints::ConstraintEvaluationResult total_result = kset.decide(rs, kres);
00106 if (!total_result.satisfied)
00107 res.valid = false;
00108
00109
00110 res.constraint_result.resize(kres.size());
00111 for (std::size_t k = 0 ; k < kres.size() ; ++k)
00112 {
00113 res.constraint_result[k].result = kres[k].satisfied;
00114 res.constraint_result[k].distance = kres[k].distance;
00115 }
00116 }
00117
00118 return true;
00119 }
00120
00121
00122 #include <class_loader/class_loader.h>
00123 CLASS_LOADER_REGISTER_CLASS(move_group::MoveGroupStateValidationService, move_group::MoveGroupCapability)