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 
00046 move_group::MoveGroupCartesianPathService::MoveGroupCartesianPathService() :
00047   MoveGroupCapability("CartesianPathService"),
00048   display_computed_paths_(true)
00049 {
00050 }
00051 
00052 void move_group::MoveGroupCartesianPathService::initialize()
00053 {
00054   display_path_ = node_handle_.advertise<moveit_msgs::DisplayTrajectory>(planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC, 10, true);
00055   cartesian_path_service_ = root_node_handle_.advertiseService(CARTESIAN_PATH_SERVICE_NAME, &MoveGroupCartesianPathService::computeService, this);
00056 }
00057 
00058 namespace
00059 {
00060 bool isStateValid(const planning_scene::PlanningScene *planning_scene,
00061                   const kinematic_constraints::KinematicConstraintSet *constraint_set,
00062                   robot_state::RobotState *state,
00063                   const robot_state::JointModelGroup *group, const double *ik_solution)
00064 {
00065   state->setJointGroupPositions(group, ik_solution);
00066   state->update();
00067   return (!planning_scene || !planning_scene->isStateColliding(*state, group->getName())) &&
00068     (!constraint_set || constraint_set->decide(*state).satisfied);
00069 }
00070 }
00071 
00072 bool move_group::MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath::Request &req, moveit_msgs::GetCartesianPath::Response &res)
00073 {
00074   ROS_INFO("Received request to compute Cartesian path");
00075   context_->planning_scene_monitor_->updateFrameTransforms();
00076 
00077   robot_state::RobotState start_state = planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)->getCurrentState();
00078   robot_state::robotStateMsgToRobotState(req.start_state, start_state);
00079   if (const robot_model::JointModelGroup *jmg = start_state.getJointModelGroup(req.group_name))
00080   {
00081     std::string link_name = req.link_name;
00082     if (link_name.empty() && !jmg->getLinkModelNames().empty())
00083       link_name = jmg->getLinkModelNames().back();
00084 
00085     bool ok = true;
00086     EigenSTL::vector_Affine3d waypoints(req.waypoints.size());
00087     const std::string &default_frame = context_->planning_scene_monitor_->getRobotModel()->getModelFrame();
00088     bool no_transform = req.header.frame_id.empty() || robot_state::Transforms::sameFrame(req.header.frame_id, default_frame) ||
00089       robot_state::Transforms::sameFrame(req.header.frame_id, link_name);
00090     
00091     for (std::size_t i = 0 ; i < req.waypoints.size() ; ++i)
00092     {
00093       if (no_transform)
00094         tf::poseMsgToEigen(req.waypoints[i], waypoints[i]);
00095       else
00096       {
00097         geometry_msgs::PoseStamped p;
00098         p.header = req.header;
00099         p.pose = req.waypoints[i];
00100         if (performTransform(p, default_frame))
00101           tf::poseMsgToEigen(p.pose, waypoints[i]);
00102         else
00103         {
00104           ROS_ERROR("Error encountered transforming waypoints to frame '%s'", default_frame.c_str());
00105           ok = false;
00106           break;
00107         }
00108       }
00109     }
00110     
00111     if (ok)
00112     {
00113       if (req.max_step < std::numeric_limits<double>::epsilon())
00114       {
00115         ROS_ERROR("Maximum step to take between consecutive configrations along Cartesian path was not specified (this value needs to be > 0)");
00116         res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
00117       }
00118       else
00119       {
00120         if (waypoints.size() > 0)
00121         {
00122           robot_state::GroupStateValidityCallbackFn constraint_fn;
00123           boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls;
00124           boost::scoped_ptr<kinematic_constraints::KinematicConstraintSet> kset;
00125           if (req.avoid_collisions || !kinematic_constraints::isEmpty(req.path_constraints))
00126           {
00127             ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_));
00128             kset.reset(new kinematic_constraints::KinematicConstraintSet((*ls)->getRobotModel()));
00129             kset->add(req.path_constraints, (*ls)->getTransforms());
00130             constraint_fn = boost::bind(&isStateValid, req.avoid_collisions ? static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls).get() : NULL, kset->empty() ? NULL : kset.get(), _1, _2, _3);
00131           }
00132           bool global_frame = !robot_state::Transforms::sameFrame(link_name, req.header.frame_id);
00133           ROS_INFO("Attempting to follow %u waypoints for link '%s' using a step of %lf m and jump threshold %lf (in %s reference frame)",
00134                    (unsigned int)waypoints.size(), link_name.c_str(), req.max_step, req.jump_threshold, global_frame ? "global" : "link");
00135           std::vector<robot_state::RobotStatePtr> traj;
00136           res.fraction = start_state.computeCartesianPath(jmg, traj, start_state.getLinkModel(link_name), waypoints, global_frame, req.max_step, req.jump_threshold, constraint_fn);
00137           robot_state::robotStateToRobotStateMsg(start_state, res.start_state);
00138           
00139           robot_trajectory::RobotTrajectory rt(context_->planning_scene_monitor_->getRobotModel(), req.group_name);
00140           for (std::size_t i = 0 ; i < traj.size() ; ++i)
00141             rt.addSuffixWayPoint(traj[i], 0.2); // \todo make 0.2 a param; better: compute time stemps based on eef distance and param m/s speed for eef;
00142           rt.getRobotTrajectoryMsg(res.solution);
00143           ROS_INFO("Computed Cartesian path with %u points (followed %lf%% of requested trajectory)", (unsigned int)traj.size(), res.fraction * 100.0);
00144           if (display_computed_paths_ && rt.getWayPointCount() > 0)
00145           {
00146             moveit_msgs::DisplayTrajectory disp;
00147             disp.model_id = context_->planning_scene_monitor_->getRobotModel()->getName();
00148             disp.trajectory.resize(1, res.solution);
00149             robot_state::robotStateToRobotStateMsg(rt.getFirstWayPoint(), disp.trajectory_start);
00150             display_path_.publish(disp);
00151           }
00152         }
00153         res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00154       }
00155     }
00156     else
00157       res.error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
00158   }
00159   else
00160     res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00161   
00162   return true;
00163 }
00164 
00165 #include <class_loader/class_loader.h>
00166 CLASS_LOADER_REGISTER_CLASS(move_group::MoveGroupCartesianPathService, move_group::MoveGroupCapability)


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Aug 26 2015 12:43:56