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 
41 #include <tf2_eigen/tf2_eigen.h>
46 #include <moveit_msgs/DisplayTrajectory.h>
49 
50 namespace
51 {
52 bool isStateValid(const planning_scene::PlanningScene* planning_scene,
54  const moveit::core::JointModelGroup* group, const double* ik_solution)
55 {
56  state->setJointGroupPositions(group, ik_solution);
57  state->update();
58  return (!planning_scene || !planning_scene->isStateColliding(*state, group->getName())) &&
59  (!constraint_set || constraint_set->decide(*state).satisfied);
60 }
61 } // namespace
62 
63 namespace move_group
64 {
66  : MoveGroupCapability("CartesianPathService"), display_computed_paths_(true)
67 {
68 }
69 
71 {
72  display_path_ = node_handle_.advertise<moveit_msgs::DisplayTrajectory>(
76 }
77 
78 bool MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath::Request& req,
79  moveit_msgs::GetCartesianPath::Response& res)
80 {
81  ROS_INFO_NAMED(getName(), "Received request to compute Cartesian path");
82  context_->planning_scene_monitor_->updateFrameTransforms();
83 
84  moveit::core::RobotState start_state =
85  planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)->getCurrentState();
86  moveit::core::robotStateMsgToRobotState(req.start_state, start_state);
87  if (const moveit::core::JointModelGroup* jmg = start_state.getJointModelGroup(req.group_name))
88  {
89  std::string link_name = req.link_name;
90  if (link_name.empty() && !jmg->getLinkModelNames().empty())
91  link_name = jmg->getLinkModelNames().back();
92 
93  bool ok = true;
94  EigenSTL::vector_Isometry3d waypoints(req.waypoints.size());
95  const std::string& default_frame = context_->planning_scene_monitor_->getRobotModel()->getModelFrame();
96  bool no_transform = req.header.frame_id.empty() ||
97  moveit::core::Transforms::sameFrame(req.header.frame_id, default_frame) ||
98  moveit::core::Transforms::sameFrame(req.header.frame_id, link_name);
99 
100  for (std::size_t i = 0; i < req.waypoints.size(); ++i)
101  {
102  if (no_transform)
103  tf2::fromMsg(req.waypoints[i], waypoints[i]);
104  else
105  {
106  geometry_msgs::PoseStamped p;
107  p.header = req.header;
108  p.pose = req.waypoints[i];
109  if (performTransform(p, default_frame))
110  tf2::fromMsg(p.pose, waypoints[i]);
111  else
112  {
113  ROS_ERROR_NAMED(getName(), "Error encountered transforming waypoints to frame '%s'", default_frame.c_str());
114  ok = false;
115  break;
116  }
117  }
118  }
119 
120  if (ok)
121  {
122  if (req.max_step < std::numeric_limits<double>::epsilon())
123  {
124  ROS_ERROR_NAMED(getName(), "Maximum step to take between consecutive configrations along Cartesian path"
125  "was not specified (this value needs to be > 0)");
126  res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
127  }
128  else
129  {
130  if (!waypoints.empty())
131  {
133  std::unique_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls;
134  std::unique_ptr<kinematic_constraints::KinematicConstraintSet> kset;
135  if (req.avoid_collisions || !moveit::core::isEmpty(req.path_constraints))
136  {
137  ls = std::make_unique<planning_scene_monitor::LockedPlanningSceneRO>(context_->planning_scene_monitor_);
138  kset = std::make_unique<kinematic_constraints::KinematicConstraintSet>((*ls)->getRobotModel());
139  kset->add(req.path_constraints, (*ls)->getTransforms());
140  constraint_fn = [scene = req.avoid_collisions ?
141  static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls).get() :
142  nullptr,
143  kset_ptr = kset.get()](moveit::core::RobotState* robot_state,
144  const moveit::core::JointModelGroup* joint_group,
145  const double* joint_group_variable_values) {
146  return isStateValid(scene, kset_ptr, robot_state, joint_group, joint_group_variable_values);
147  };
148  }
149  bool global_frame = !moveit::core::Transforms::sameFrame(link_name, req.header.frame_id);
151  "Attempting to follow %u waypoints for link '%s' using a step of %lf m "
152  "and jump threshold %lf (in %s reference frame)",
153  (unsigned int)waypoints.size(), link_name.c_str(), req.max_step, req.jump_threshold,
154  global_frame ? "global" : "link");
155  std::vector<moveit::core::RobotStatePtr> traj;
157  &start_state, jmg, traj, start_state.getLinkModel(link_name), waypoints, global_frame,
158  moveit::core::MaxEEFStep(req.max_step), moveit::core::JumpThreshold(req.jump_threshold), constraint_fn);
159  moveit::core::robotStateToRobotStateMsg(start_state, res.start_state);
160 
161  robot_trajectory::RobotTrajectory rt(context_->planning_scene_monitor_->getRobotModel(), req.group_name);
162  for (const moveit::core::RobotStatePtr& traj_state : traj)
163  rt.addSuffixWayPoint(traj_state, 0.0);
164 
165  // time trajectory
167  time_param.computeTimeStamps(rt, 1.0);
168 
169  // optionally compute timing to move the eef with constant speed
170  if (req.max_cartesian_speed > 0.0)
171  {
172  trajectory_processing::limitMaxCartesianLinkSpeed(rt, req.max_cartesian_speed,
173  req.cartesian_speed_limited_link);
174  }
175 
176  rt.getRobotTrajectoryMsg(res.solution);
177  ROS_INFO_NAMED(getName(), "Computed Cartesian path with %u points (followed %lf%% of requested trajectory)",
178  (unsigned int)traj.size(), res.fraction * 100.0);
180  {
181  moveit_msgs::DisplayTrajectory disp;
182  disp.model_id = context_->planning_scene_monitor_->getRobotModel()->getName();
183  disp.trajectory.resize(1, res.solution);
184  moveit::core::robotStateToRobotStateMsg(rt.getFirstWayPoint(), disp.trajectory_start);
185  display_path_.publish(disp);
186  }
187  }
188  res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
189  }
190  }
191  else
192  res.error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
193  }
194  else
195  res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
196 
197  return true;
198 }
199 
200 } // namespace move_group
201 
moveit::core::GroupStateValidityCallbackFn
boost::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
move_group::MoveGroupCartesianPathService::initialize
void initialize() override
Definition: cartesian_path_service_capability.cpp:70
move_group::MoveGroupCapability::performTransform
bool performTransform(geometry_msgs::PoseStamped &pose_msg, const std::string &target_frame) const
Definition: move_group_capability.cpp:166
robot_trajectory::RobotTrajectory::getWayPointCount
std::size_t getWayPointCount() const
EigenSTL::vector_Isometry3d
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
move_group::MoveGroupCapability
Definition: move_group_capability.h:89
move_group::MoveGroupCapability::context_
MoveGroupContextPtr context_
Definition: move_group_capability.h:132
moveit::core::JumpThreshold
move_group::MoveGroupCapability::getName
const std::string & getName() const
Definition: move_group_capability.h:104
collision_tools.h
planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC
static const std::string DISPLAY_PATH_TOPIC
tf2_eigen.h
tf2::fromMsg
void fromMsg(const A &, B &b)
move_group::MoveGroupCartesianPathService::display_computed_paths_
bool display_computed_paths_
Definition: cartesian_path_service_capability.h:120
limit_cartesian_speed.h
moveit::core::RobotState::getLinkModel
const LinkModel * getLinkModel(const std::string &link) const
planning_scene::PlanningScene
trajectory_processing::IterativeParabolicTimeParameterization
kinematic_constraints::KinematicConstraintSet::decide
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const
move_group::MoveGroupCartesianPathService
Definition: cartesian_path_service_capability.h:76
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
robot_trajectory::RobotTrajectory::addSuffixWayPoint
RobotTrajectory & addSuffixWayPoint(const moveit::core::RobotState &state, double dt)
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::RobotState
robot_state.h
kinematic_constraints::ConstraintEvaluationResult::satisfied
bool satisfied
move_group::MoveGroupCartesianPathService::display_path_
ros::Publisher display_path_
Definition: cartesian_path_service_capability.h:119
robot_trajectory::RobotTrajectory
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
ok
ROSCPP_DECL bool ok()
moveit::core::JointModelGroup::getName
const std::string & getName() const
trajectory_processing::limitMaxCartesianLinkSpeed
bool limitMaxCartesianLinkSpeed(robot_trajectory::RobotTrajectory &trajectory, const double speed, const moveit::core::LinkModel *link_model)
message_checks.h
moveit::core::isEmpty
bool isEmpty(const geometry_msgs::Pose &msg)
robot_trajectory::RobotTrajectory::getRobotTrajectoryMsg
void getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory &trajectory, const std::vector< std::string > &joint_filter=std::vector< std::string >()) const
moveit::core::RobotState::update
void update(bool force=false)
ROS_INFO_NAMED
#define ROS_INFO_NAMED(name,...)
move_group::MoveGroupCapability::root_node_handle_
ros::NodeHandle root_node_handle_
Definition: move_group_capability.h:129
move_group::MoveGroupCartesianPathService::MoveGroupCartesianPathService
MoveGroupCartesianPathService()
Definition: cartesian_path_service_capability.cpp:65
moveit::core::MaxEEFStep
kinematic_constraints::KinematicConstraintSet
move_group
Definition: capability_names.h:41
class_loader.hpp
planning_pipeline.h
robot_trajectory::RobotTrajectory::getFirstWayPoint
const moveit::core::RobotState & getFirstWayPoint() const
moveit::core::Transforms::sameFrame
static bool sameFrame(const std::string &frame1, const std::string &frame2)
moveit::core::CartesianInterpolator::computeCartesianPath
static double computeCartesianPath(RobotState *start_state, const JointModelGroup *group, std::vector< std::shared_ptr< RobotState >> &traj, const LinkModel *link, const Eigen::Isometry3d &target, bool global_reference_frame, const MaxEEFStep &max_step, const JumpThreshold &jump_threshold, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions(), const Eigen::Isometry3d &link_offset=Eigen::Isometry3d::Identity())
iterative_time_parameterization.h
trajectory_processing::IterativeParabolicTimeParameterization::computeTimeStamps
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const override
planning_scene_monitor::LockedPlanningSceneRO
moveit::core::JointModelGroup
cartesian_path_service_capability.h
move_group::MoveGroupCapability::node_handle_
ros::NodeHandle node_handle_
Definition: move_group_capability.h:130
cartesian_interpolator.h
conversions.h
CLASS_LOADER_REGISTER_CLASS
CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::AddIterativeSplineParameterization, planning_request_adapter::PlanningRequestAdapter)
move_group::CARTESIAN_PATH_SERVICE_NAME
static const std::string CARTESIAN_PATH_SERVICE_NAME
Definition: capability_names.h:89
moveit::core::RobotState::setJointGroupPositions
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
moveit::core::RobotState::getJointModelGroup
const JointModelGroup * getJointModelGroup(const std::string &group) const
planning_scene
move_group::MoveGroupCartesianPathService::computeService
bool computeService(moveit_msgs::GetCartesianPath::Request &req, moveit_msgs::GetCartesianPath::Response &res)
Definition: cartesian_path_service_capability.cpp:78
capability_names.h
moveit::core::robotStateToRobotStateMsg
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::RobotState &robot_state, bool copy_attached_bodies=true)
moveit::core::robotStateMsgToRobotState
bool robotStateMsgToRobotState(const moveit_msgs::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
move_group::MoveGroupCartesianPathService::cartesian_path_service_
ros::ServiceServer cartesian_path_service_
Definition: cartesian_path_service_capability.h:118


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Thu Nov 24 2022 03:33:13