arm_navigation_msgs: arm_navigation_msgs: convert

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  * 
00004  *  Copyright (c) 2008, 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 the 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 #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     //    path.header = constraints[0].header;
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     btQuaternion tmp_quat;
00140     pose_stamped.header = position_constraint.header;
00141     pose_stamped.pose.position = position_constraint.position;
00142     //    tmp_quat.setRPY(orientation_constraint.orientation.x,orientation_constraint.orientation.y,orientation_constraint.orientation.z);
00143     //    tf::quaternionTFToMsg(tmp_quat,pose_stamped.pose.orientation);
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
convert_messages.h
