cartesian_path_service_capability.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include "cartesian_path_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 #include <moveit/planning_pipeline/planning_pipeline.h>
00044 #include <moveit_msgs/DisplayTrajectory.h>
00045 #include <moveit/trajectory_processing/iterative_time_parameterization.h>
00046 
00047 move_group::MoveGroupCartesianPathService::MoveGroupCartesianPathService()
00048   : MoveGroupCapability("CartesianPathService"), display_computed_paths_(true)
00049 {
00050 }
00051 
00052 void move_group::MoveGroupCartesianPathService::initialize()
00053 {
00054   display_path_ = node_handle_.advertise<moveit_msgs::DisplayTrajectory>(
00055       planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC, 10, true);
00056   cartesian_path_service_ = root_node_handle_.advertiseService(CARTESIAN_PATH_SERVICE_NAME,
00057                                                                &MoveGroupCartesianPathService::computeService, this);
00058 }
00059 
00060 namespace
00061 {
00062 bool isStateValid(const planning_scene::PlanningScene* planning_scene,
00063                   const kinematic_constraints::KinematicConstraintSet* constraint_set, robot_state::RobotState* state,
00064                   const robot_state::JointModelGroup* group, const double* ik_solution)
00065 {
00066   state->setJointGroupPositions(group, ik_solution);
00067   state->update();
00068   return (!planning_scene || !planning_scene->isStateColliding(*state, group->getName())) &&
00069          (!constraint_set || constraint_set->decide(*state).satisfied);
00070 }
00071 }
00072 
00073 bool move_group::MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath::Request& req,
00074                                                                moveit_msgs::GetCartesianPath::Response& res)
00075 {
00076   ROS_INFO("Received request to compute Cartesian path");
00077   context_->planning_scene_monitor_->updateFrameTransforms();
00078 
00079   robot_state::RobotState start_state =
00080       planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)->getCurrentState();
00081   robot_state::robotStateMsgToRobotState(req.start_state, start_state);
00082   if (const robot_model::JointModelGroup* jmg = start_state.getJointModelGroup(req.group_name))
00083   {
00084     std::string link_name = req.link_name;
00085     if (link_name.empty() && !jmg->getLinkModelNames().empty())
00086       link_name = jmg->getLinkModelNames().back();
00087 
00088     bool ok = true;
00089     EigenSTL::vector_Affine3d waypoints(req.waypoints.size());
00090     const std::string& default_frame = context_->planning_scene_monitor_->getRobotModel()->getModelFrame();
00091     bool no_transform = req.header.frame_id.empty() ||
00092                         robot_state::Transforms::sameFrame(req.header.frame_id, default_frame) ||
00093                         robot_state::Transforms::sameFrame(req.header.frame_id, link_name);
00094 
00095     for (std::size_t i = 0; i < req.waypoints.size(); ++i)
00096     {
00097       if (no_transform)
00098         tf::poseMsgToEigen(req.waypoints[i], waypoints[i]);
00099       else
00100       {
00101         geometry_msgs::PoseStamped p;
00102         p.header = req.header;
00103         p.pose = req.waypoints[i];
00104         if (performTransform(p, default_frame))
00105           tf::poseMsgToEigen(p.pose, waypoints[i]);
00106         else
00107         {
00108           ROS_ERROR("Error encountered transforming waypoints to frame '%s'", default_frame.c_str());
00109           ok = false;
00110           break;
00111         }
00112       }
00113     }
00114 
00115     if (ok)
00116     {
00117       if (req.max_step < std::numeric_limits<double>::epsilon())
00118       {
00119         ROS_ERROR("Maximum step to take between consecutive configrations along Cartesian path was not specified (this "
00120                   "value needs to be > 0)");
00121         res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
00122       }
00123       else
00124       {
00125         if (waypoints.size() > 0)
00126         {
00127           robot_state::GroupStateValidityCallbackFn constraint_fn;
00128           boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls;
00129           boost::scoped_ptr<kinematic_constraints::KinematicConstraintSet> kset;
00130           if (req.avoid_collisions || !kinematic_constraints::isEmpty(req.path_constraints))
00131           {
00132             ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_));
00133             kset.reset(new kinematic_constraints::KinematicConstraintSet((*ls)->getRobotModel()));
00134             kset->add(req.path_constraints, (*ls)->getTransforms());
00135             constraint_fn = boost::bind(
00136                 &isStateValid,
00137                 req.avoid_collisions ? static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls).get() : NULL,
00138                 kset->empty() ? NULL : kset.get(), _1, _2, _3);
00139           }
00140           bool global_frame = !robot_state::Transforms::sameFrame(link_name, req.header.frame_id);
00141           ROS_INFO("Attempting to follow %u waypoints for link '%s' using a step of %lf m and jump threshold %lf (in "
00142                    "%s reference frame)",
00143                    (unsigned int)waypoints.size(), link_name.c_str(), req.max_step, req.jump_threshold,
00144                    global_frame ? "global" : "link");
00145           std::vector<robot_state::RobotStatePtr> traj;
00146           res.fraction =
00147               start_state.computeCartesianPath(jmg, traj, start_state.getLinkModel(link_name), waypoints, global_frame,
00148                                                req.max_step, req.jump_threshold, constraint_fn);
00149           robot_state::robotStateToRobotStateMsg(start_state, res.start_state);
00150 
00151           robot_trajectory::RobotTrajectory rt(context_->planning_scene_monitor_->getRobotModel(), req.group_name);
00152           for (std::size_t i = 0; i < traj.size(); ++i)
00153             rt.addSuffixWayPoint(traj[i], 0.0);
00154 
00155           // time trajectory
00156           // \todo optionally compute timing to move the eef with constant speed
00157           trajectory_processing::IterativeParabolicTimeParameterization time_param;
00158           time_param.computeTimeStamps(rt, 1.0);
00159 
00160           rt.getRobotTrajectoryMsg(res.solution);
00161           ROS_INFO("Computed Cartesian path with %u points (followed %lf%% of requested trajectory)",
00162                    (unsigned int)traj.size(), res.fraction * 100.0);
00163           if (display_computed_paths_ && rt.getWayPointCount() > 0)
00164           {
00165             moveit_msgs::DisplayTrajectory disp;
00166             disp.model_id = context_->planning_scene_monitor_->getRobotModel()->getName();
00167             disp.trajectory.resize(1, res.solution);
00168             robot_state::robotStateToRobotStateMsg(rt.getFirstWayPoint(), disp.trajectory_start);
00169             display_path_.publish(disp);
00170           }
00171         }
00172         res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00173       }
00174     }
00175     else
00176       res.error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
00177   }
00178   else
00179     res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00180 
00181   return true;
00182 }
00183 
00184 #include <class_loader/class_loader.h>
00185 CLASS_LOADER_REGISTER_CLASS(move_group::MoveGroupCartesianPathService, move_group::MoveGroupCapability)


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Jul 24 2017 02:21:44