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 #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
00156
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)