cartesian_path_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 
44 #include <moveit_msgs/DisplayTrajectory.h>
46 
48  : MoveGroupCapability("CartesianPathService"), display_computed_paths_(true)
49 {
50 }
51 
53 {
54  display_path_ = node_handle_.advertise<moveit_msgs::DisplayTrajectory>(
58 }
59 
60 namespace
61 {
62 bool isStateValid(const planning_scene::PlanningScene* planning_scene,
63  const kinematic_constraints::KinematicConstraintSet* constraint_set, robot_state::RobotState* state,
64  const robot_state::JointModelGroup* group, const double* ik_solution)
65 {
66  state->setJointGroupPositions(group, ik_solution);
67  state->update();
68  return (!planning_scene || !planning_scene->isStateColliding(*state, group->getName())) &&
69  (!constraint_set || constraint_set->decide(*state).satisfied);
70 }
71 }
72 
73 bool move_group::MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath::Request& req,
74  moveit_msgs::GetCartesianPath::Response& res)
75 {
76  ROS_INFO("Received request to compute Cartesian path");
77  context_->planning_scene_monitor_->updateFrameTransforms();
78 
79  robot_state::RobotState start_state =
80  planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)->getCurrentState();
81  robot_state::robotStateMsgToRobotState(req.start_state, start_state);
82  if (const robot_model::JointModelGroup* jmg = start_state.getJointModelGroup(req.group_name))
83  {
84  std::string link_name = req.link_name;
85  if (link_name.empty() && !jmg->getLinkModelNames().empty())
86  link_name = jmg->getLinkModelNames().back();
87 
88  bool ok = true;
89  EigenSTL::vector_Affine3d waypoints(req.waypoints.size());
90  const std::string& default_frame = context_->planning_scene_monitor_->getRobotModel()->getModelFrame();
91  bool no_transform = req.header.frame_id.empty() ||
92  robot_state::Transforms::sameFrame(req.header.frame_id, default_frame) ||
93  robot_state::Transforms::sameFrame(req.header.frame_id, link_name);
94 
95  for (std::size_t i = 0; i < req.waypoints.size(); ++i)
96  {
97  if (no_transform)
98  tf::poseMsgToEigen(req.waypoints[i], waypoints[i]);
99  else
100  {
101  geometry_msgs::PoseStamped p;
102  p.header = req.header;
103  p.pose = req.waypoints[i];
104  if (performTransform(p, default_frame))
105  tf::poseMsgToEigen(p.pose, waypoints[i]);
106  else
107  {
108  ROS_ERROR("Error encountered transforming waypoints to frame '%s'", default_frame.c_str());
109  ok = false;
110  break;
111  }
112  }
113  }
114 
115  if (ok)
116  {
117  if (req.max_step < std::numeric_limits<double>::epsilon())
118  {
119  ROS_ERROR("Maximum step to take between consecutive configrations along Cartesian path was not specified (this "
120  "value needs to be > 0)");
121  res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
122  }
123  else
124  {
125  if (waypoints.size() > 0)
126  {
127  robot_state::GroupStateValidityCallbackFn constraint_fn;
128  std::unique_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls;
129  std::unique_ptr<kinematic_constraints::KinematicConstraintSet> kset;
130  if (req.avoid_collisions || !kinematic_constraints::isEmpty(req.path_constraints))
131  {
132  ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_));
133  kset.reset(new kinematic_constraints::KinematicConstraintSet((*ls)->getRobotModel()));
134  kset->add(req.path_constraints, (*ls)->getTransforms());
135  constraint_fn = boost::bind(
136  &isStateValid,
137  req.avoid_collisions ? static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls).get() : NULL,
138  kset->empty() ? NULL : kset.get(), _1, _2, _3);
139  }
140  bool global_frame = !robot_state::Transforms::sameFrame(link_name, req.header.frame_id);
141  ROS_INFO("Attempting to follow %u waypoints for link '%s' using a step of %lf m and jump threshold %lf (in "
142  "%s reference frame)",
143  (unsigned int)waypoints.size(), link_name.c_str(), req.max_step, req.jump_threshold,
144  global_frame ? "global" : "link");
145  std::vector<robot_state::RobotStatePtr> traj;
146  res.fraction =
147  start_state.computeCartesianPath(jmg, traj, start_state.getLinkModel(link_name), waypoints, global_frame,
148  req.max_step, req.jump_threshold, constraint_fn);
149  robot_state::robotStateToRobotStateMsg(start_state, res.start_state);
150 
151  robot_trajectory::RobotTrajectory rt(context_->planning_scene_monitor_->getRobotModel(), req.group_name);
152  for (std::size_t i = 0; i < traj.size(); ++i)
153  rt.addSuffixWayPoint(traj[i], 0.0);
154 
155  // time trajectory
156  // \todo optionally compute timing to move the eef with constant speed
158  time_param.computeTimeStamps(rt, 1.0);
159 
160  rt.getRobotTrajectoryMsg(res.solution);
161  ROS_INFO("Computed Cartesian path with %u points (followed %lf%% of requested trajectory)",
162  (unsigned int)traj.size(), res.fraction * 100.0);
163  if (display_computed_paths_ && rt.getWayPointCount() > 0)
164  {
165  moveit_msgs::DisplayTrajectory disp;
166  disp.model_id = context_->planning_scene_monitor_->getRobotModel()->getName();
167  disp.trajectory.resize(1, res.solution);
168  robot_state::robotStateToRobotStateMsg(rt.getFirstWayPoint(), disp.trajectory_start);
169  display_path_.publish(disp);
170  }
171  }
172  res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
173  }
174  }
175  else
176  res.error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
177  }
178  else
179  res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
180 
181  return true;
182 }
183 
bool isEmpty(const moveit_msgs::Constraints &constr)
void publish(const boost::shared_ptr< M > &message) const
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
bool computeService(moveit_msgs::GetCartesianPath::Request &req, moveit_msgs::GetCartesianPath::Response &res)
static const std::string CARTESIAN_PATH_SERVICE_NAME
#define ROS_INFO(...)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
static const std::string DISPLAY_PATH_TOPIC
bool performTransform(geometry_msgs::PoseStamped &pose_msg, const std::string &target_frame) const
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const
void addSuffixWayPoint(const robot_state::RobotState &state, double dt)
#define ROS_ERROR(...)
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
bool isStateColliding(const std::string &group="", bool verbose=false)
CLASS_LOADER_REGISTER_CLASS(Dog, Base)


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:18:14