Go to the documentation of this file.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 #ifndef MOTION_PLANNING_CONVERT_MESSAGES_
00035 #define MOTION_PLANNING_CONVERT_MESSAGES_
00036
00037 #include <ros/ros.h>
00038 #include <tf/tf.h>
00039
00040 #include <arm_navigation_msgs/RobotTrajectory.h>
00041 #include <arm_navigation_msgs/RobotState.h>
00042
00043 #include <arm_navigation_msgs/JointConstraint.h>
00044 #include <trajectory_msgs/JointTrajectory.h>
00045 #include <sensor_msgs/JointState.h>
00046
00047 #include <arm_navigation_msgs/OrientationConstraint.h>
00048 #include <arm_navigation_msgs/SimplePoseConstraint.h>
00049 #include <arm_navigation_msgs/PositionConstraint.h>
00050 #include <arm_navigation_msgs/ArmNavigationErrorCodes.h>
00051 #include <arm_navigation_msgs/GetMotionPlan.h>
00052
00053 namespace arm_navigation_msgs
00054 {
00060 inline trajectory_msgs::JointTrajectoryPoint jointStateToJointTrajectoryPoint(const sensor_msgs::JointState &state)
00061 {
00062 trajectory_msgs::JointTrajectoryPoint point;
00063 point.positions = state.position;
00064 return point;
00065 }
00066
00072 inline arm_navigation_msgs::MultiDOFJointTrajectoryPoint multiDOFJointStateToMultiDOFJointTrajectoryPoint(const arm_navigation_msgs::MultiDOFJointState &state)
00073 {
00074 arm_navigation_msgs::MultiDOFJointTrajectoryPoint point;
00075 point.poses = state.poses;
00076 point.time_from_start = ros::Duration(0.0);
00077 return point;
00078 }
00079
00085 inline void robotStateToRobotTrajectoryPoint(const arm_navigation_msgs::RobotState &state,
00086 trajectory_msgs::JointTrajectoryPoint &point,
00087 arm_navigation_msgs::MultiDOFJointTrajectoryPoint &multi_dof_point)
00088 {
00089 point = jointStateToJointTrajectoryPoint(state.joint_state);
00090 multi_dof_point = multiDOFJointStateToMultiDOFJointTrajectoryPoint(state.multi_dof_joint_state);
00091 return;
00092 }
00093
00099 inline sensor_msgs::JointState jointConstraintsToJointState(const std::vector<arm_navigation_msgs::JointConstraint> &constraints)
00100 {
00101 sensor_msgs::JointState state;
00102 state.name.resize(constraints.size());
00103 state.position.resize(constraints.size());
00104 for(unsigned int i=0; i < constraints.size(); i++)
00105 {
00106 state.name[i] = constraints[i].joint_name;
00107 state.position[i] = constraints[i].position;
00108 }
00109 return state;
00110 }
00111
00117 inline trajectory_msgs::JointTrajectory jointConstraintsToJointTrajectory(const std::vector<arm_navigation_msgs::JointConstraint> &constraints)
00118 {
00119 trajectory_msgs::JointTrajectory path;
00120 if(constraints.empty())
00121 return path;
00122 sensor_msgs::JointState state = jointConstraintsToJointState(constraints);
00123 trajectory_msgs::JointTrajectoryPoint point = jointStateToJointTrajectoryPoint(state);
00124
00125 path.points.push_back(point);
00126 path.joint_names = state.name;
00127 return path;
00128 }
00129
00136 inline geometry_msgs::PoseStamped poseConstraintsToPoseStamped(const arm_navigation_msgs::PositionConstraint &position_constraint, const arm_navigation_msgs::OrientationConstraint &orientation_constraint)
00137 {
00138 geometry_msgs::PoseStamped pose_stamped;
00139 tf::Quaternion tmp_quat;
00140 pose_stamped.header = position_constraint.header;
00141 pose_stamped.pose.position = position_constraint.position;
00142
00143
00144 pose_stamped.pose.orientation = orientation_constraint.orientation;
00145 return pose_stamped;
00146 }
00147
00154 inline sensor_msgs::JointState createJointState(std::vector<std::string> joint_names, std::vector<double> joint_values)
00155 {
00156 sensor_msgs::JointState state;
00157 state.name = joint_names;
00158 state.position = joint_values;
00159 return state;
00160 }
00161
00168 inline void poseConstraintToPositionOrientationConstraints(const arm_navigation_msgs::SimplePoseConstraint &pose_constraint, arm_navigation_msgs::PositionConstraint &position_constraint, arm_navigation_msgs::OrientationConstraint &orientation_constraint)
00169 {
00170 position_constraint.header = pose_constraint.header;
00171 position_constraint.link_name = pose_constraint.link_name;
00172 position_constraint.position = pose_constraint.pose.position;
00173 position_constraint.constraint_region_shape.type = arm_navigation_msgs::Shape::BOX;
00174 position_constraint.constraint_region_shape.dimensions.push_back(2*pose_constraint.absolute_position_tolerance.x);
00175 position_constraint.constraint_region_shape.dimensions.push_back(2*pose_constraint.absolute_position_tolerance.y);
00176 position_constraint.constraint_region_shape.dimensions.push_back(2*pose_constraint.absolute_position_tolerance.z);
00177
00178 position_constraint.constraint_region_orientation.x = 0.0;
00179 position_constraint.constraint_region_orientation.y = 0.0;
00180 position_constraint.constraint_region_orientation.z = 0.0;
00181 position_constraint.constraint_region_orientation.w = 1.0;
00182
00183 position_constraint.weight = 1.0;
00184
00185 orientation_constraint.header = pose_constraint.header;
00186 orientation_constraint.link_name = pose_constraint.link_name;
00187 orientation_constraint.orientation = pose_constraint.pose.orientation;
00188 orientation_constraint.type = pose_constraint.orientation_constraint_type;
00189
00190 orientation_constraint.absolute_roll_tolerance = pose_constraint.absolute_roll_tolerance;
00191 orientation_constraint.absolute_pitch_tolerance = pose_constraint.absolute_pitch_tolerance;
00192 orientation_constraint.absolute_yaw_tolerance = pose_constraint.absolute_yaw_tolerance;
00193 orientation_constraint.weight = 1.0;
00194 }
00195
00196
00205 inline void poseStampedToPositionOrientationConstraints(const geometry_msgs::PoseStamped &pose_stamped, const std::string &link_name,
00206 arm_navigation_msgs::PositionConstraint &position_constraint,
00207 arm_navigation_msgs::OrientationConstraint &orientation_constraint,
00208 double region_box_dimension=.01,
00209 double absolute_rpy_tolerance=.01)
00210 {
00211 position_constraint.header = pose_stamped.header;
00212 position_constraint.link_name = link_name;
00213 position_constraint.position = pose_stamped.pose.position;
00214 position_constraint.weight = 1.0;
00215 position_constraint.constraint_region_shape.type = arm_navigation_msgs::Shape::BOX;
00216 position_constraint.constraint_region_shape.dimensions.push_back(region_box_dimension);
00217 position_constraint.constraint_region_shape.dimensions.push_back(region_box_dimension);
00218 position_constraint.constraint_region_shape.dimensions.push_back(region_box_dimension);
00219
00220 orientation_constraint.header = pose_stamped.header;
00221 orientation_constraint.link_name = link_name;
00222 orientation_constraint.orientation = pose_stamped.pose.orientation;
00223
00224 orientation_constraint.absolute_roll_tolerance = absolute_rpy_tolerance;
00225 orientation_constraint.absolute_pitch_tolerance = absolute_rpy_tolerance;
00226 orientation_constraint.absolute_yaw_tolerance = absolute_rpy_tolerance;
00227 orientation_constraint.weight = 1.0;
00228 }
00229
00230
00235 inline void printJointState(const sensor_msgs::JointState &joint_state)
00236 {
00237 ROS_INFO("frame_id: %s stamp: %f",joint_state.header.frame_id.c_str(),joint_state.header.stamp.toSec());
00238 if(joint_state.name.size() != joint_state.position.size())
00239 ROS_ERROR("Size of joint_names field: %d does not match size of positions field: %d",(int) joint_state.name.size(),(int) joint_state.position.size());
00240 else
00241 {
00242 for(unsigned int i=0; i< joint_state.name.size(); i++)
00243 {
00244 ROS_INFO("Joint name: %s, position: %f",joint_state.name[i].c_str(),joint_state.position[i]);
00245 }
00246 }
00247 }
00248
00254 inline std::string armNavigationErrorCodeToString(const arm_navigation_msgs::ArmNavigationErrorCodes &error_code)
00255 {
00256 std::string result;
00257 if(error_code.val == error_code.PLANNING_FAILED)
00258 result = "Planning failed";
00259 else if(error_code.val == error_code.SUCCESS)
00260 result = "Success";
00261 else if(error_code.val == error_code.TIMED_OUT)
00262 result = "Timed out";
00263 else if (error_code.val == error_code.START_STATE_IN_COLLISION)
00264 result = "Start state in collision";
00265 else if (error_code.val == error_code.START_STATE_VIOLATES_PATH_CONSTRAINTS)
00266 result = "Start state violates path constraints";
00267 else if (error_code.val == error_code.GOAL_IN_COLLISION)
00268 result = "Goal in collision";
00269 else if (error_code.val == error_code.GOAL_VIOLATES_PATH_CONSTRAINTS)
00270 result = "Goal violates path constraints";
00271 else if (error_code.val == error_code.INVALID_ROBOT_STATE)
00272 result = "Initial robot state invalid";
00273 else if (error_code.val == error_code.INCOMPLETE_ROBOT_STATE)
00274 result = "Initial robot state incomplete";
00275 else if (error_code.val == error_code.INVALID_PLANNER_ID)
00276 result = "Invalid planner id";
00277 else if (error_code.val == error_code.INVALID_NUM_PLANNING_ATTEMPTS)
00278 result = "Invalid num planning attempts (must be > 0)";
00279 else if (error_code.val == error_code.INVALID_ALLOWED_PLANNING_TIME)
00280 result = "Invalid allowed planning time (must be > 0)";
00281 else if (error_code.val == error_code.INVALID_GROUP_NAME)
00282 result = "Invalid group name for planning";
00283 else if (error_code.val == error_code.INVALID_GOAL_JOINT_CONSTRAINTS)
00284 result = "Invalid goal joint constraints";
00285 else if (error_code.val == error_code.INVALID_GOAL_POSITION_CONSTRAINTS)
00286 result = "Invalid goal position constraints";
00287 else if (error_code.val == error_code.INVALID_GOAL_ORIENTATION_CONSTRAINTS)
00288 result = "Invalid goal orientation constraints";
00289 else if (error_code.val == error_code.INVALID_PATH_JOINT_CONSTRAINTS)
00290 result = "Invalid path joint constraints";
00291 else if (error_code.val == error_code.INVALID_PATH_POSITION_CONSTRAINTS)
00292 result = "Invalid path position constraints";
00293 else if (error_code.val == error_code.INVALID_PATH_ORIENTATION_CONSTRAINTS)
00294 result = "Invalid path orientation constraints";
00295 else if (error_code.val == error_code.INVALID_TRAJECTORY)
00296 result = "Invalid trajectory";
00297 else if (error_code.val == error_code.INVALID_INDEX)
00298 result = "Invalid index for trajectory check";
00299 else if (error_code.val == error_code.JOINT_LIMITS_VIOLATED)
00300 result = "Joint limits violated";
00301 else if (error_code.val == error_code.PATH_CONSTRAINTS_VIOLATED)
00302 result = "Path constraints violated";
00303 else if (error_code.val == error_code.COLLISION_CONSTRAINTS_VIOLATED)
00304 result = "Collision constraints violated";
00305 else if (error_code.val == error_code.GOAL_CONSTRAINTS_VIOLATED)
00306 result = "Goal constraints violated";
00307 else if (error_code.val == error_code.JOINTS_NOT_MOVING)
00308 result = "Joints not moving - robot may be stuck";
00309 else if (error_code.val == error_code.TRAJECTORY_CONTROLLER_FAILED)
00310 result = "Trajectory controller failed";
00311 else if (error_code.val == error_code.FRAME_TRANSFORM_FAILURE)
00312 result = "Frame transform failed";
00313 else if (error_code.val == error_code.COLLISION_CHECKING_UNAVAILABLE)
00314 result = "Collision checking unavailable";
00315 else if (error_code.val == error_code.ROBOT_STATE_STALE)
00316 result = "Robot state is not being updated";
00317 else if (error_code.val == error_code.SENSOR_INFO_STALE)
00318 result = "Sensor information is not being updated";
00319 else if (error_code.val == error_code.NO_IK_SOLUTION)
00320 result = "Inverse kinematics solution was not found";
00321 else if (error_code.val == error_code.IK_LINK_IN_COLLISION)
00322 result = "Inverse kinematics link was in collision";
00323 else if (error_code.val == error_code.INVALID_LINK_NAME)
00324 result = "Invalid link name";
00325 else if (error_code.val == error_code.NO_FK_SOLUTION)
00326 result = "No forward kinematics solution";
00327 else if (error_code.val == error_code.KINEMATICS_STATE_IN_COLLISION)
00328 result = "Current robot state is in collision";
00329 else if (error_code.val == error_code.INVALID_TIMEOUT)
00330 result = "Time given for planning invalid (must be > 0)";
00331 else
00332 result = "Unknown error code";
00333 return result;
00334 }
00335
00342 inline bool constraintsToPoseStampedVector(const arm_navigation_msgs::Constraints &constraints,
00343 std::vector<geometry_msgs::PoseStamped> &poses)
00344 {
00345 if(constraints.position_constraints.size() != constraints.orientation_constraints.size())
00346 {
00347 ROS_ERROR("Number of position constraints does not match number of orientation constraints");
00348 return false;
00349 }
00350 for(unsigned int i =0; i < constraints.position_constraints.size(); i++)
00351 {
00352 geometry_msgs::PoseStamped pose = poseConstraintsToPoseStamped(constraints.position_constraints[i],
00353 constraints.orientation_constraints[i]);
00354 poses.push_back(pose);
00355 }
00356 return true;
00357 }
00358
00359
00366 inline arm_navigation_msgs::MultiDOFJointState poseConstraintsToMultiDOFJointState(const std::vector<arm_navigation_msgs::PositionConstraint> &position_constraints,
00367 const std::vector<arm_navigation_msgs::OrientationConstraint> &orientation_constraints)
00368 {
00369 arm_navigation_msgs::MultiDOFJointState multi_dof_joint_state;
00370 if(position_constraints.size() != orientation_constraints.size())
00371 return multi_dof_joint_state;
00372 for(unsigned int i=0; i < position_constraints.size(); i++)
00373 {
00374 if(position_constraints[i].header.frame_id != orientation_constraints[i].header.frame_id)
00375 {
00376 ROS_ERROR("Frame id for position constraint %d does not match frame id for corresponding orientation constraint",i);
00377 return multi_dof_joint_state;
00378 }
00379 if(position_constraints[i].link_name != orientation_constraints[i].link_name)
00380 {
00381 ROS_ERROR("Link name for position constraint %d does not match link name for corresponding orientation constraint",i);
00382 return multi_dof_joint_state;
00383 }
00384 }
00385 return multi_dof_joint_state;
00386 }
00387
00388
00389 }
00390
00391 #endif