44 #include <moveit_msgs/DisplayTrajectory.h> 64 const robot_state::JointModelGroup* group,
const double* ik_solution)
66 state->setJointGroupPositions(group, ik_solution);
68 return (!planning_scene || !planning_scene->
isStateColliding(*state, group->getName())) &&
74 moveit_msgs::GetCartesianPath::Response& res)
76 ROS_INFO(
"Received request to compute Cartesian path");
77 context_->planning_scene_monitor_->updateFrameTransforms();
79 robot_state::RobotState start_state =
81 robot_state::robotStateMsgToRobotState(req.start_state, start_state);
82 if (
const robot_model::JointModelGroup* jmg = start_state.getJointModelGroup(req.group_name))
84 std::string link_name = req.link_name;
85 if (link_name.empty() && !jmg->getLinkModelNames().empty())
86 link_name = jmg->getLinkModelNames().back();
90 const std::string& default_frame =
context_->planning_scene_monitor_->getRobotModel()->getModelFrame();
91 bool no_transform = req.header.frame_id.empty() ||
92 robot_state::Transforms::sameFrame(req.header.frame_id, default_frame) ||
93 robot_state::Transforms::sameFrame(req.header.frame_id, link_name);
95 for (std::size_t i = 0; i < req.waypoints.size(); ++i)
101 geometry_msgs::PoseStamped p;
102 p.header = req.header;
103 p.pose = req.waypoints[i];
108 ROS_ERROR(
"Error encountered transforming waypoints to frame '%s'", default_frame.c_str());
117 if (req.max_step < std::numeric_limits<double>::epsilon())
119 ROS_ERROR(
"Maximum step to take between consecutive configrations along Cartesian path was not specified (this " 120 "value needs to be > 0)");
121 res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
125 if (waypoints.size() > 0)
127 robot_state::GroupStateValidityCallbackFn constraint_fn;
128 std::unique_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls;
129 std::unique_ptr<kinematic_constraints::KinematicConstraintSet> kset;
134 kset->add(req.path_constraints, (*ls)->getTransforms());
135 constraint_fn = boost::bind(
137 req.avoid_collisions ? static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls).get() : NULL,
138 kset->empty() ? NULL : kset.get(), _1, _2, _3);
140 bool global_frame = !robot_state::Transforms::sameFrame(link_name, req.header.frame_id);
141 ROS_INFO(
"Attempting to follow %u waypoints for link '%s' using a step of %lf m and jump threshold %lf (in " 142 "%s reference frame)",
143 (
unsigned int)waypoints.size(), link_name.c_str(), req.max_step, req.jump_threshold,
144 global_frame ?
"global" :
"link");
145 std::vector<robot_state::RobotStatePtr> traj;
147 start_state.computeCartesianPath(jmg, traj, start_state.getLinkModel(link_name), waypoints, global_frame,
148 req.max_step, req.jump_threshold, constraint_fn);
149 robot_state::robotStateToRobotStateMsg(start_state, res.start_state);
152 for (std::size_t i = 0; i < traj.size(); ++i)
160 rt.getRobotTrajectoryMsg(res.solution);
161 ROS_INFO(
"Computed Cartesian path with %u points (followed %lf%% of requested trajectory)",
162 (
unsigned int)traj.size(), res.fraction * 100.0);
165 moveit_msgs::DisplayTrajectory disp;
166 disp.model_id =
context_->planning_scene_monitor_->getRobotModel()->getName();
167 disp.trajectory.resize(1, res.solution);
168 robot_state::robotStateToRobotStateMsg(rt.getFirstWayPoint(), disp.trajectory_start);
172 res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
176 res.error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
179 res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
bool display_computed_paths_
bool isEmpty(const moveit_msgs::Constraints &constr)
void publish(const boost::shared_ptr< M > &message) const
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
ros::NodeHandle node_handle_
bool computeService(moveit_msgs::GetCartesianPath::Request &req, moveit_msgs::GetCartesianPath::Response &res)
MoveGroupCartesianPathService()
static const std::string CARTESIAN_PATH_SERVICE_NAME
MoveGroupContextPtr context_
ros::Publisher display_path_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::ServiceServer cartesian_path_service_
static const std::string DISPLAY_PATH_TOPIC
bool performTransform(geometry_msgs::PoseStamped &pose_msg, const std::string &target_frame) const
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const
ros::NodeHandle root_node_handle_
virtual void initialize()
void addSuffixWayPoint(const robot_state::RobotState &state, double dt)
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
bool isStateColliding(const std::string &group="", bool verbose=false)
CLASS_LOADER_REGISTER_CLASS(Dog, Base)