00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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);
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)