convert_messages.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 #ifndef MOTION_PLANNING_CONVERT_MESSAGES_
35 #define MOTION_PLANNING_CONVERT_MESSAGES_
36 
37 #include <ros/ros.h>
38 #include <tf/tf.h>
39 
40 #include <arm_navigation_msgs/RobotTrajectory.h>
41 #include <arm_navigation_msgs/RobotState.h>
42 
43 #include <arm_navigation_msgs/JointConstraint.h>
44 #include <trajectory_msgs/JointTrajectory.h>
45 #include <sensor_msgs/JointState.h>
46 
47 #include <arm_navigation_msgs/OrientationConstraint.h>
48 #include <arm_navigation_msgs/SimplePoseConstraint.h>
49 #include <arm_navigation_msgs/PositionConstraint.h>
50 #include <arm_navigation_msgs/ArmNavigationErrorCodes.h>
51 #include <arm_navigation_msgs/GetMotionPlan.h>
52 
53 namespace arm_navigation_msgs
54 {
60 inline trajectory_msgs::JointTrajectoryPoint jointStateToJointTrajectoryPoint(const sensor_msgs::JointState &state)
61  {
62  trajectory_msgs::JointTrajectoryPoint point;
63  point.positions = state.position;
64  return point;
65  }
66 
72 inline arm_navigation_msgs::MultiDOFJointTrajectoryPoint multiDOFJointStateToMultiDOFJointTrajectoryPoint(const arm_navigation_msgs::MultiDOFJointState &state)
73  {
74  arm_navigation_msgs::MultiDOFJointTrajectoryPoint point;
75  point.poses = state.poses;
76  point.time_from_start = ros::Duration(0.0);
77  return point;
78  }
79 
85 inline void robotStateToRobotTrajectoryPoint(const arm_navigation_msgs::RobotState &state,
86  trajectory_msgs::JointTrajectoryPoint &point,
87  arm_navigation_msgs::MultiDOFJointTrajectoryPoint &multi_dof_point)
88  {
89  point = jointStateToJointTrajectoryPoint(state.joint_state);
90  multi_dof_point = multiDOFJointStateToMultiDOFJointTrajectoryPoint(state.multi_dof_joint_state);
91  return;
92  }
93 
99 inline sensor_msgs::JointState jointConstraintsToJointState(const std::vector<arm_navigation_msgs::JointConstraint> &constraints)
100  {
101  sensor_msgs::JointState state;
102  state.name.resize(constraints.size());
103  state.position.resize(constraints.size());
104  for(unsigned int i=0; i < constraints.size(); i++)
105  {
106  state.name[i] = constraints[i].joint_name;
107  state.position[i] = constraints[i].position;
108  }
109  return state;
110  }
111 
117 inline trajectory_msgs::JointTrajectory jointConstraintsToJointTrajectory(const std::vector<arm_navigation_msgs::JointConstraint> &constraints)
118  {
119  trajectory_msgs::JointTrajectory path;
120  if(constraints.empty())
121  return path;
122  sensor_msgs::JointState state = jointConstraintsToJointState(constraints);
123  trajectory_msgs::JointTrajectoryPoint point = jointStateToJointTrajectoryPoint(state);
124  // path.header = constraints[0].header;
125  path.points.push_back(point);
126  path.joint_names = state.name;
127  return path;
128  }
129 
136 inline geometry_msgs::PoseStamped poseConstraintsToPoseStamped(const arm_navigation_msgs::PositionConstraint &position_constraint, const arm_navigation_msgs::OrientationConstraint &orientation_constraint)
137  {
138  geometry_msgs::PoseStamped pose_stamped;
139  tf::Quaternion tmp_quat;
140  pose_stamped.header = position_constraint.header;
141  pose_stamped.pose.position = position_constraint.position;
142  // tmp_quat.setRPY(orientation_constraint.orientation.x,orientation_constraint.orientation.y,orientation_constraint.orientation.z);
143  // tf::quaternionTFToMsg(tmp_quat,pose_stamped.pose.orientation);
144  pose_stamped.pose.orientation = orientation_constraint.orientation;
145  return pose_stamped;
146  }
147 
154 inline sensor_msgs::JointState createJointState(std::vector<std::string> joint_names, std::vector<double> joint_values)
155  {
156  sensor_msgs::JointState state;
157  state.name = joint_names;
158  state.position = joint_values;
159  return state;
160  }
161 
168 inline void poseConstraintToPositionOrientationConstraints(const arm_navigation_msgs::SimplePoseConstraint &pose_constraint, arm_navigation_msgs::PositionConstraint &position_constraint, arm_navigation_msgs::OrientationConstraint &orientation_constraint)
169  {
170  position_constraint.header = pose_constraint.header;
171  position_constraint.link_name = pose_constraint.link_name;
172  position_constraint.position = pose_constraint.pose.position;
173  position_constraint.constraint_region_shape.type = arm_navigation_msgs::Shape::BOX;
174  position_constraint.constraint_region_shape.dimensions.push_back(2*pose_constraint.absolute_position_tolerance.x);
175  position_constraint.constraint_region_shape.dimensions.push_back(2*pose_constraint.absolute_position_tolerance.y);
176  position_constraint.constraint_region_shape.dimensions.push_back(2*pose_constraint.absolute_position_tolerance.z);
177 
178  position_constraint.constraint_region_orientation.x = 0.0;
179  position_constraint.constraint_region_orientation.y = 0.0;
180  position_constraint.constraint_region_orientation.z = 0.0;
181  position_constraint.constraint_region_orientation.w = 1.0;
182 
183  position_constraint.weight = 1.0;
184 
185  orientation_constraint.header = pose_constraint.header;
186  orientation_constraint.link_name = pose_constraint.link_name;
187  orientation_constraint.orientation = pose_constraint.pose.orientation;
188  orientation_constraint.type = pose_constraint.orientation_constraint_type;
189 
190  orientation_constraint.absolute_roll_tolerance = pose_constraint.absolute_roll_tolerance;
191  orientation_constraint.absolute_pitch_tolerance = pose_constraint.absolute_pitch_tolerance;
192  orientation_constraint.absolute_yaw_tolerance = pose_constraint.absolute_yaw_tolerance;
193  orientation_constraint.weight = 1.0;
194  }
195 
196 
205 inline void poseStampedToPositionOrientationConstraints(const geometry_msgs::PoseStamped &pose_stamped, const std::string &link_name,
206  arm_navigation_msgs::PositionConstraint &position_constraint,
207  arm_navigation_msgs::OrientationConstraint &orientation_constraint,
208  double region_box_dimension=.01,
209  double absolute_rpy_tolerance=.01)
210  {
211  position_constraint.header = pose_stamped.header;
212  position_constraint.link_name = link_name;
213  position_constraint.position = pose_stamped.pose.position;
214  position_constraint.weight = 1.0;
215  position_constraint.constraint_region_shape.type = arm_navigation_msgs::Shape::BOX;
216  position_constraint.constraint_region_shape.dimensions.push_back(region_box_dimension);
217  position_constraint.constraint_region_shape.dimensions.push_back(region_box_dimension);
218  position_constraint.constraint_region_shape.dimensions.push_back(region_box_dimension);
219 
220  orientation_constraint.header = pose_stamped.header;
221  orientation_constraint.link_name = link_name;
222  orientation_constraint.orientation = pose_stamped.pose.orientation;
223 
224  orientation_constraint.absolute_roll_tolerance = absolute_rpy_tolerance;
225  orientation_constraint.absolute_pitch_tolerance = absolute_rpy_tolerance;
226  orientation_constraint.absolute_yaw_tolerance = absolute_rpy_tolerance;
227  orientation_constraint.weight = 1.0;
228  }
229 
230 
235 inline void printJointState(const sensor_msgs::JointState &joint_state)
236  {
237  ROS_INFO("frame_id: %s stamp: %f",joint_state.header.frame_id.c_str(),joint_state.header.stamp.toSec());
238  if(joint_state.name.size() != joint_state.position.size())
239  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());
240  else
241  {
242  for(unsigned int i=0; i< joint_state.name.size(); i++)
243  {
244  ROS_INFO("Joint name: %s, position: %f",joint_state.name[i].c_str(),joint_state.position[i]);
245  }
246  }
247  }
248 
254  inline std::string armNavigationErrorCodeToString(const arm_navigation_msgs::ArmNavigationErrorCodes &error_code)
255  {
256  std::string result;
257  if(error_code.val == error_code.PLANNING_FAILED)
258  result = "Planning failed";
259  else if(error_code.val == error_code.SUCCESS)
260  result = "Success";
261  else if(error_code.val == error_code.TIMED_OUT)
262  result = "Timed out";
263  else if (error_code.val == error_code.START_STATE_IN_COLLISION)
264  result = "Start state in collision";
265  else if (error_code.val == error_code.START_STATE_VIOLATES_PATH_CONSTRAINTS)
266  result = "Start state violates path constraints";
267  else if (error_code.val == error_code.GOAL_IN_COLLISION)
268  result = "Goal in collision";
269  else if (error_code.val == error_code.GOAL_VIOLATES_PATH_CONSTRAINTS)
270  result = "Goal violates path constraints";
271  else if (error_code.val == error_code.INVALID_ROBOT_STATE)
272  result = "Initial robot state invalid";
273  else if (error_code.val == error_code.INCOMPLETE_ROBOT_STATE)
274  result = "Initial robot state incomplete";
275  else if (error_code.val == error_code.INVALID_PLANNER_ID)
276  result = "Invalid planner id";
277  else if (error_code.val == error_code.INVALID_NUM_PLANNING_ATTEMPTS)
278  result = "Invalid num planning attempts (must be > 0)";
279  else if (error_code.val == error_code.INVALID_ALLOWED_PLANNING_TIME)
280  result = "Invalid allowed planning time (must be > 0)";
281  else if (error_code.val == error_code.INVALID_GROUP_NAME)
282  result = "Invalid group name for planning";
283  else if (error_code.val == error_code.INVALID_GOAL_JOINT_CONSTRAINTS)
284  result = "Invalid goal joint constraints";
285  else if (error_code.val == error_code.INVALID_GOAL_POSITION_CONSTRAINTS)
286  result = "Invalid goal position constraints";
287  else if (error_code.val == error_code.INVALID_GOAL_ORIENTATION_CONSTRAINTS)
288  result = "Invalid goal orientation constraints";
289  else if (error_code.val == error_code.INVALID_PATH_JOINT_CONSTRAINTS)
290  result = "Invalid path joint constraints";
291  else if (error_code.val == error_code.INVALID_PATH_POSITION_CONSTRAINTS)
292  result = "Invalid path position constraints";
293  else if (error_code.val == error_code.INVALID_PATH_ORIENTATION_CONSTRAINTS)
294  result = "Invalid path orientation constraints";
295  else if (error_code.val == error_code.INVALID_TRAJECTORY)
296  result = "Invalid trajectory";
297  else if (error_code.val == error_code.INVALID_INDEX)
298  result = "Invalid index for trajectory check";
299  else if (error_code.val == error_code.JOINT_LIMITS_VIOLATED)
300  result = "Joint limits violated";
301  else if (error_code.val == error_code.PATH_CONSTRAINTS_VIOLATED)
302  result = "Path constraints violated";
303  else if (error_code.val == error_code.COLLISION_CONSTRAINTS_VIOLATED)
304  result = "Collision constraints violated";
305  else if (error_code.val == error_code.GOAL_CONSTRAINTS_VIOLATED)
306  result = "Goal constraints violated";
307  else if (error_code.val == error_code.JOINTS_NOT_MOVING)
308  result = "Joints not moving - robot may be stuck";
309  else if (error_code.val == error_code.TRAJECTORY_CONTROLLER_FAILED)
310  result = "Trajectory controller failed";
311  else if (error_code.val == error_code.FRAME_TRANSFORM_FAILURE)
312  result = "Frame transform failed";
313  else if (error_code.val == error_code.COLLISION_CHECKING_UNAVAILABLE)
314  result = "Collision checking unavailable";
315  else if (error_code.val == error_code.ROBOT_STATE_STALE)
316  result = "Robot state is not being updated";
317  else if (error_code.val == error_code.SENSOR_INFO_STALE)
318  result = "Sensor information is not being updated";
319  else if (error_code.val == error_code.NO_IK_SOLUTION)
320  result = "Inverse kinematics solution was not found";
321  else if (error_code.val == error_code.IK_LINK_IN_COLLISION)
322  result = "Inverse kinematics link was in collision";
323  else if (error_code.val == error_code.INVALID_LINK_NAME)
324  result = "Invalid link name";
325  else if (error_code.val == error_code.NO_FK_SOLUTION)
326  result = "No forward kinematics solution";
327  else if (error_code.val == error_code.KINEMATICS_STATE_IN_COLLISION)
328  result = "Current robot state is in collision";
329  else if (error_code.val == error_code.INVALID_TIMEOUT)
330  result = "Time given for planning invalid (must be > 0)";
331  else
332  result = "Unknown error code";
333  return result;
334  }
335 
342  inline bool constraintsToPoseStampedVector(const arm_navigation_msgs::Constraints &constraints,
343  std::vector<geometry_msgs::PoseStamped> &poses)
344  {
345  if(constraints.position_constraints.size() != constraints.orientation_constraints.size())
346  {
347  ROS_ERROR("Number of position constraints does not match number of orientation constraints");
348  return false;
349  }
350  for(unsigned int i =0; i < constraints.position_constraints.size(); i++)
351  {
352  geometry_msgs::PoseStamped pose = poseConstraintsToPoseStamped(constraints.position_constraints[i],
353  constraints.orientation_constraints[i]);
354  poses.push_back(pose);
355  }
356  return true;
357  }
358 
359 
366  inline arm_navigation_msgs::MultiDOFJointState poseConstraintsToMultiDOFJointState(const std::vector<arm_navigation_msgs::PositionConstraint> &position_constraints,
367  const std::vector<arm_navigation_msgs::OrientationConstraint> &orientation_constraints)
368  {
369  arm_navigation_msgs::MultiDOFJointState multi_dof_joint_state;
370  if(position_constraints.size() != orientation_constraints.size())
371  return multi_dof_joint_state;
372  for(unsigned int i=0; i < position_constraints.size(); i++)
373  {
374  if(position_constraints[i].header.frame_id != orientation_constraints[i].header.frame_id)
375  {
376  ROS_ERROR("Frame id for position constraint %d does not match frame id for corresponding orientation constraint",i);
377  return multi_dof_joint_state;
378  }
379  if(position_constraints[i].link_name != orientation_constraints[i].link_name)
380  {
381  ROS_ERROR("Link name for position constraint %d does not match link name for corresponding orientation constraint",i);
382  return multi_dof_joint_state;
383  }
384  }
385  return multi_dof_joint_state;
386  }
387 
388 
389 }
390 
391 #endif
arm_navigation_msgs::armNavigationErrorCodeToString
std::string armNavigationErrorCodeToString(const arm_navigation_msgs::ArmNavigationErrorCodes &error_code)
Convert an error code into a string value.
Definition: convert_messages.h:254
arm_navigation_msgs::robotStateToRobotTrajectoryPoint
void robotStateToRobotTrajectoryPoint(const arm_navigation_msgs::RobotState &state, trajectory_msgs::JointTrajectoryPoint &point, arm_navigation_msgs::MultiDOFJointTrajectoryPoint &multi_dof_point)
Convert a robot state to a robot trajectory message.
Definition: convert_messages.h:85
arm_navigation_msgs::createJointState
sensor_msgs::JointState createJointState(std::vector< std::string > joint_names, std::vector< double > joint_values)
Create a joint state from a std vector of names and values.
Definition: convert_messages.h:154
arm_navigation_msgs::poseConstraintsToMultiDOFJointState
arm_navigation_msgs::MultiDOFJointState poseConstraintsToMultiDOFJointState(const std::vector< arm_navigation_msgs::PositionConstraint > &position_constraints, const std::vector< arm_navigation_msgs::OrientationConstraint > &orientation_constraints)
Extract pose information from a position and orientation constraint into a multi dof joint state...
Definition: convert_messages.h:366
ROS_INFO
#define ROS_INFO(...)
arm_navigation_msgs::printJointState
void printJointState(const sensor_msgs::JointState &joint_state)
Print the joint state information.
Definition: convert_messages.h:235
arm_navigation_msgs::poseConstraintToPositionOrientationConstraints
void poseConstraintToPositionOrientationConstraints(const arm_navigation_msgs::SimplePoseConstraint &pose_constraint, arm_navigation_msgs::PositionConstraint &position_constraint, arm_navigation_msgs::OrientationConstraint &orientation_constraint)
Convert a simple pose constraint into a position and orientation constraint.
Definition: convert_messages.h:168
arm_navigation_msgs::jointConstraintsToJointState
sensor_msgs::JointState jointConstraintsToJointState(const std::vector< arm_navigation_msgs::JointConstraint > &constraints)
Extract joint position information from a set of joint constraints into a joint state message...
Definition: convert_messages.h:99
arm_navigation_msgs::jointStateToJointTrajectoryPoint
trajectory_msgs::JointTrajectoryPoint jointStateToJointTrajectoryPoint(const sensor_msgs::JointState &state)
Convert a joint state to a joint trajectory point message.
Definition: convert_messages.h:60
arm_navigation_msgs::poseStampedToPositionOrientationConstraints
void poseStampedToPositionOrientationConstraints(const geometry_msgs::PoseStamped &pose_stamped, const std::string &link_name, arm_navigation_msgs::PositionConstraint &position_constraint, arm_navigation_msgs::OrientationConstraint &orientation_constraint, double region_box_dimension=.01, double absolute_rpy_tolerance=.01)
Convert a stamped pose into a position and orientation constraint.
Definition: convert_messages.h:205
arm_navigation_msgs
Definition: convert_messages.h:53
arm_navigation_msgs::constraintsToPoseStampedVector
bool constraintsToPoseStampedVector(const arm_navigation_msgs::Constraints &constraints, std::vector< geometry_msgs::PoseStamped > &poses)
Extract pose information from a position and orientation constraint into a pose stamped message...
Definition: convert_messages.h:342
arm_navigation_msgs::jointConstraintsToJointTrajectory
trajectory_msgs::JointTrajectory jointConstraintsToJointTrajectory(const std::vector< arm_navigation_msgs::JointConstraint > &constraints)
Extract joint position information from a set of joint constraints into a joint state message...
Definition: convert_messages.h:117
arm_navigation_msgs::multiDOFJointStateToMultiDOFJointTrajectoryPoint
arm_navigation_msgs::MultiDOFJointTrajectoryPoint multiDOFJointStateToMultiDOFJointTrajectoryPoint(const arm_navigation_msgs::MultiDOFJointState &state)
Convert a multi-dof state to a multi-dof joint trajectory point message.
Definition: convert_messages.h:72
ROS_ERROR
#define ROS_ERROR(...)
arm_navigation_msgs::poseConstraintsToPoseStamped
geometry_msgs::PoseStamped poseConstraintsToPoseStamped(const arm_navigation_msgs::PositionConstraint &position_constraint, const arm_navigation_msgs::OrientationConstraint &orientation_constraint)
Extract pose information from a position and orientation constraint into a pose stamped message...
Definition: convert_messages.h:136

arm_navigation_msgs
Author(s): Gil Jones
autogenerated on Mon Jun 10 2019 14:26:17