plan_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 
40 
42 {
43 }
44 
46 {
49 }
50 
51 bool move_group::MoveGroupPlanService::computePlanService(moveit_msgs::GetMotionPlan::Request& req,
52  moveit_msgs::GetMotionPlan::Response& res)
53 {
54  ROS_INFO("Received new planning service request...");
55  // before we start planning, ensure that we have the latest robot state received...
56  if (req.motion_plan_request.start_state.is_diff == true)
57  context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now());
58  context_->planning_scene_monitor_->updateFrameTransforms();
59 
60  planning_scene_monitor::LockedPlanningSceneRO ps(context_->planning_scene_monitor_);
61  try
62  {
64  context_->planning_pipeline_->generatePlan(ps, req.motion_plan_request, mp_res);
65  mp_res.getMessage(res.motion_plan_response);
66  }
67  catch (std::exception& ex)
68  {
69  ROS_ERROR("Planning pipeline threw an exception: %s", ex.what());
70  res.motion_plan_response.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
71  }
72 
73  return true;
74 }
75 
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void getMessage(moveit_msgs::MotionPlanResponse &msg) const
bool computePlanService(moveit_msgs::GetMotionPlan::Request &req, moveit_msgs::GetMotionPlan::Response &res)
#define ROS_INFO(...)
static const std::string PLANNER_SERVICE_NAME
static Time now()
#define ROS_ERROR(...)
CLASS_LOADER_REGISTER_CLASS(Dog, Base)


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