state_validation_service_capability.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
43 
45  : MoveGroupCapability("StateValidationService")
46 {
47 }
48 
50 {
53 }
54 
55 bool move_group::MoveGroupStateValidationService::computeService(moveit_msgs::GetStateValidity::Request& req,
56  moveit_msgs::GetStateValidity::Response& res)
57 {
58  planning_scene_monitor::LockedPlanningSceneRO ls(context_->planning_scene_monitor_);
59  robot_state::RobotState rs = ls->getCurrentState();
60  robot_state::robotStateMsgToRobotState(req.robot_state, rs);
61 
62  res.valid = true;
63 
64  // configure collision request
66  creq.group_name = req.group_name;
67  creq.cost = true;
68  creq.contacts = true;
69  creq.max_contacts = ls->getWorld()->size();
70  creq.max_cost_sources = creq.max_contacts + ls->getRobotModel()->getLinkModelsWithCollisionGeometry().size();
71  creq.max_contacts *= creq.max_contacts;
73 
74  // check collision
75  ls->checkCollision(creq, cres, rs);
76 
77  // copy contacts if any
78  if (cres.collision)
79  {
80  ros::Time time_now = ros::Time::now();
81  res.contacts.reserve(cres.contact_count);
82  res.valid = false;
83  for (collision_detection::CollisionResult::ContactMap::const_iterator it = cres.contacts.begin();
84  it != cres.contacts.end(); ++it)
85  for (std::size_t k = 0; k < it->second.size(); ++k)
86  {
87  res.contacts.resize(res.contacts.size() + 1);
88  collision_detection::contactToMsg(it->second[k], res.contacts.back());
89  res.contacts.back().header.frame_id = ls->getPlanningFrame();
90  res.contacts.back().header.stamp = time_now;
91  }
92  }
93 
94  // copy cost sources
95  res.cost_sources.reserve(cres.cost_sources.size());
96  for (std::set<collision_detection::CostSource>::const_iterator it = cres.cost_sources.begin();
97  it != cres.cost_sources.end(); ++it)
98  {
99  res.cost_sources.resize(res.cost_sources.size() + 1);
100  collision_detection::costSourceToMsg(*it, res.cost_sources.back());
101  }
102 
103  // evaluate constraints
104  if (!kinematic_constraints::isEmpty(req.constraints))
105  {
106  kinematic_constraints::KinematicConstraintSet kset(ls->getRobotModel());
107  kset.add(req.constraints, ls->getTransforms());
108  std::vector<kinematic_constraints::ConstraintEvaluationResult> kres;
109  kinematic_constraints::ConstraintEvaluationResult total_result = kset.decide(rs, kres);
110  if (!total_result.satisfied)
111  res.valid = false;
112 
113  // copy constraint results
114  res.constraint_result.resize(kres.size());
115  for (std::size_t k = 0; k < kres.size(); ++k)
116  {
117  res.constraint_result[k].result = kres[k].satisfied;
118  res.constraint_result[k].distance = kres[k].distance;
119  }
120  }
121 
122  return true;
123 }
124 
static const std::string STATE_VALIDITY_SERVICE_NAME
bool isEmpty(const moveit_msgs::Constraints &constr)
void costSourceToMsg(const CostSource &cost_source, moveit_msgs::CostSource &msg)
void contactToMsg(const Contact &contact, moveit_msgs::ContactInformation &msg)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
std::set< CostSource > cost_sources
bool add(const moveit_msgs::Constraints &c, const robot_state::Transforms &tf)
bool computeService(moveit_msgs::GetStateValidity::Request &req, moveit_msgs::GetStateValidity::Response &res)
static Time now()
CLASS_LOADER_REGISTER_CLASS(Dog, Base)


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jul 10 2019 04:03:52