3 #ifndef ROBOWFLEX_YAML_
4 #define ROBOWFLEX_YAML_
7 #include <ros/duration.h>
9 #include <std_msgs/Header.h>
10 #include <std_msgs/ColorRGBA.h>
12 #include <geometry_msgs/Vector3.h>
13 #include <geometry_msgs/Point.h>
14 #include <geometry_msgs/Quaternion.h>
15 #include <geometry_msgs/Pose.h>
16 #include <geometry_msgs/Transform.h>
17 #include <geometry_msgs/Twist.h>
18 #include <geometry_msgs/Wrench.h>
19 #include <geometry_msgs/PoseStamped.h>
20 #include <geometry_msgs/TransformStamped.h>
22 #include <shape_msgs/SolidPrimitive.h>
23 #include <shape_msgs/MeshTriangle.h>
24 #include <shape_msgs/Mesh.h>
25 #include <shape_msgs/Plane.h>
27 #include <sensor_msgs/JointState.h>
28 #include <sensor_msgs/MultiDOFJointState.h>
30 #include <octomap_msgs/Octomap.h>
31 #include <octomap_msgs/OctomapWithPose.h>
33 #include <trajectory_msgs/JointTrajectory.h>
34 #include <trajectory_msgs/JointTrajectoryPoint.h>
35 #include <trajectory_msgs/MultiDOFJointTrajectory.h>
36 #include <trajectory_msgs/MultiDOFJointTrajectoryPoint.h>
38 #include <object_recognition_msgs/ObjectType.h>
40 #include <moveit_msgs/LinkPadding.h>
41 #include <moveit_msgs/LinkScale.h>
42 #include <moveit_msgs/ObjectColor.h>
43 #include <moveit_msgs/RobotState.h>
44 #include <moveit_msgs/WorkspaceParameters.h>
45 #include <moveit_msgs/BoundingVolume.h>
46 #include <moveit_msgs/JointConstraint.h>
47 #include <moveit_msgs/PositionConstraint.h>
48 #include <moveit_msgs/OrientationConstraint.h>
49 #include <moveit_msgs/VisibilityConstraint.h>
50 #include <moveit_msgs/TrajectoryConstraints.h>
51 #include <moveit_msgs/RobotTrajectory.h>
52 #include <moveit_msgs/Constraints.h>
53 #include <moveit_msgs/CollisionObject.h>
54 #include <moveit_msgs/AttachedCollisionObject.h>
55 #include <moveit_msgs/AllowedCollisionEntry.h>
56 #include <moveit_msgs/AllowedCollisionMatrix.h>
57 #include <moveit_msgs/PlanningScene.h>
58 #include <moveit_msgs/PlanningSceneWorld.h>
59 #include <moveit_msgs/MotionPlanRequest.h>
61 #include <moveit_msgs/MoveGroupGoal.h>
62 #include <moveit_msgs/MoveGroupResult.h>
64 #include <yaml-cpp/yaml.h>
71 #if ROBOWFLEX_AT_LEAST_KINETIC
74 struct convert<signed char>
76 static Node encode(
const signed char &rhs);
77 static bool decode(
const Node &node,
signed char &rhs);
82 struct convert<moveit_msgs::PlanningScene>
84 static Node encode(
const moveit_msgs::PlanningScene &rhs);
85 static bool decode(
const Node &node, moveit_msgs::PlanningScene &rhs);
89 struct convert<moveit_msgs::RobotState>
91 static Node encode(
const moveit_msgs::RobotState &rhs);
92 static bool decode(
const Node &node, moveit_msgs::RobotState &rhs);
96 struct convert<geometry_msgs::TransformStamped>
98 static Node encode(
const geometry_msgs::TransformStamped &rhs);
99 static bool decode(
const Node &node, geometry_msgs::TransformStamped &rhs);
103 struct convert<std_msgs::Header>
105 static Node encode(
const std_msgs::Header &rhs);
106 static bool decode(
const Node &node, std_msgs::Header &rhs);
110 struct convert<geometry_msgs::Pose>
112 static Node encode(
const geometry_msgs::Pose &rhs);
113 static bool decode(
const Node &node, geometry_msgs::Pose &rhs);
117 struct convert<geometry_msgs::PoseStamped>
119 static Node encode(
const geometry_msgs::PoseStamped &rhs);
120 static bool decode(
const Node &node, geometry_msgs::PoseStamped &rhs);
124 struct convert<geometry_msgs::Transform>
126 static Node encode(
const geometry_msgs::Transform &rhs);
127 static bool decode(
const Node &node, geometry_msgs::Transform &rhs);
131 struct convert<geometry_msgs::Vector3>
133 static Node encode(
const geometry_msgs::Vector3 &rhs);
134 static bool decode(
const Node &node, geometry_msgs::Vector3 &rhs);
138 struct convert<geometry_msgs::Point>
140 static Node encode(
const geometry_msgs::Point &rhs);
141 static bool decode(
const Node &node, geometry_msgs::Point &rhs);
145 struct convert<geometry_msgs::Quaternion>
147 static Node encode(
const geometry_msgs::Quaternion &rhs);
148 static bool decode(
const Node &node, geometry_msgs::Quaternion &rhs);
152 struct convert<geometry_msgs::Twist>
154 static Node encode(
const geometry_msgs::Twist &rhs);
155 static bool decode(
const Node &node, geometry_msgs::Twist &rhs);
159 struct convert<geometry_msgs::Wrench>
161 static Node encode(
const geometry_msgs::Wrench &rhs);
162 static bool decode(
const Node &node, geometry_msgs::Wrench &rhs);
166 struct convert<sensor_msgs::JointState>
168 static Node encode(
const sensor_msgs::JointState &rhs);
169 static bool decode(
const Node &node, sensor_msgs::JointState &rhs);
173 struct convert<sensor_msgs::MultiDOFJointState>
175 static Node encode(
const sensor_msgs::MultiDOFJointState &rhs);
176 static bool decode(
const Node &node, sensor_msgs::MultiDOFJointState &rhs);
180 struct convert<moveit_msgs::AttachedCollisionObject>
182 static Node encode(
const moveit_msgs::AttachedCollisionObject &rhs);
183 static bool decode(
const Node &node, moveit_msgs::AttachedCollisionObject &rhs);
187 struct convert<trajectory_msgs::JointTrajectory>
189 static Node encode(
const trajectory_msgs::JointTrajectory &rhs);
190 static bool decode(
const Node &node, trajectory_msgs::JointTrajectory &rhs);
194 struct convert<trajectory_msgs::JointTrajectoryPoint>
196 static Node encode(
const trajectory_msgs::JointTrajectoryPoint &rhs);
197 static bool decode(
const Node &node, trajectory_msgs::JointTrajectoryPoint &rhs);
201 struct convert<moveit_msgs::CollisionObject>
203 static Node encode(
const moveit_msgs::CollisionObject &rhs);
204 static bool decode(
const Node &node, moveit_msgs::CollisionObject &rhs);
208 struct convert<object_recognition_msgs::ObjectType>
210 static Node encode(
const object_recognition_msgs::ObjectType &rhs);
211 static bool decode(
const Node &node, object_recognition_msgs::ObjectType &rhs);
215 struct convert<moveit_msgs::LinkPadding>
217 static Node encode(
const moveit_msgs::LinkPadding &rhs);
218 static bool decode(
const Node &node, moveit_msgs::LinkPadding &rhs);
222 struct convert<moveit_msgs::LinkScale>
224 static Node encode(
const moveit_msgs::LinkScale &rhs);
225 static bool decode(
const Node &node, moveit_msgs::LinkScale &rhs);
229 struct convert<moveit_msgs::AllowedCollisionMatrix>
231 static Node encode(
const moveit_msgs::AllowedCollisionMatrix &rhs);
232 static bool decode(
const Node &node, moveit_msgs::AllowedCollisionMatrix &rhs);
236 struct convert<moveit_msgs::AllowedCollisionEntry>
238 static Node encode(
const moveit_msgs::AllowedCollisionEntry &rhs);
239 static bool decode(
const Node &node, moveit_msgs::AllowedCollisionEntry &rhs);
243 struct convert<moveit_msgs::PlanningSceneWorld>
245 static Node encode(
const moveit_msgs::PlanningSceneWorld &rhs);
246 static bool decode(
const Node &node, moveit_msgs::PlanningSceneWorld &rhs);
250 struct convert<moveit_msgs::ObjectColor>
252 static Node encode(
const moveit_msgs::ObjectColor &rhs);
253 static bool decode(
const Node &node, moveit_msgs::ObjectColor &rhs);
257 struct convert<std_msgs::ColorRGBA>
259 static Node encode(
const std_msgs::ColorRGBA &rhs);
260 static bool decode(
const Node &node, std_msgs::ColorRGBA &rhs);
264 struct convert<octomap_msgs::Octomap>
266 static Node encode(
const octomap_msgs::Octomap &rhs);
267 static bool decode(
const Node &node, octomap_msgs::Octomap &rhs);
271 struct convert<octomap_msgs::OctomapWithPose>
273 static Node encode(
const octomap_msgs::OctomapWithPose &rhs);
274 static bool decode(
const Node &node, octomap_msgs::OctomapWithPose &rhs);
278 struct convert<ros::Time>
280 static Node encode(
const ros::Time &rhs);
281 static bool decode(
const Node &node, ros::Time &rhs);
285 struct convert<ros::Duration>
287 static Node encode(
const ros::Duration &rhs);
288 static bool decode(
const Node &node, ros::Duration &rhs);
292 struct convert<shape_msgs::SolidPrimitive>
294 static Node encode(
const shape_msgs::SolidPrimitive &rhs);
295 static bool decode(
const Node &node, shape_msgs::SolidPrimitive &rhs);
299 struct convert<shape_msgs::Mesh>
301 static Node encode(
const shape_msgs::Mesh &rhs);
302 static bool decode(
const Node &node, shape_msgs::Mesh &rhs);
306 struct convert<shape_msgs::MeshTriangle>
308 static Node encode(
const shape_msgs::MeshTriangle &rhs);
309 static bool decode(
const Node &node, shape_msgs::MeshTriangle &rhs);
313 struct convert<shape_msgs::Plane>
315 static Node encode(
const shape_msgs::Plane &rhs);
316 static bool decode(
const Node &node, shape_msgs::Plane &rhs);
320 struct convert<moveit_msgs::WorkspaceParameters>
322 static Node encode(
const moveit_msgs::WorkspaceParameters &rhs);
323 static bool decode(
const Node &node, moveit_msgs::WorkspaceParameters &rhs);
327 struct convert<moveit_msgs::Constraints>
329 static Node encode(
const moveit_msgs::Constraints &rhs);
330 static bool decode(
const Node &node, moveit_msgs::Constraints &rhs);
334 struct convert<moveit_msgs::JointConstraint>
336 static Node encode(
const moveit_msgs::JointConstraint &rhs);
337 static bool decode(
const Node &node, moveit_msgs::JointConstraint &rhs);
341 struct convert<moveit_msgs::PositionConstraint>
343 static Node encode(
const moveit_msgs::PositionConstraint &rhs);
344 static bool decode(
const Node &node, moveit_msgs::PositionConstraint &rhs);
348 struct convert<moveit_msgs::OrientationConstraint>
350 static Node encode(
const moveit_msgs::OrientationConstraint &rhs);
351 static bool decode(
const Node &node, moveit_msgs::OrientationConstraint &rhs);
355 struct convert<moveit_msgs::VisibilityConstraint>
357 static Node encode(
const moveit_msgs::VisibilityConstraint &rhs);
358 static bool decode(
const Node &node, moveit_msgs::VisibilityConstraint &rhs);
362 struct convert<moveit_msgs::TrajectoryConstraints>
364 static Node encode(
const moveit_msgs::TrajectoryConstraints &rhs);
365 static bool decode(
const Node &node, moveit_msgs::TrajectoryConstraints &rhs);
369 struct convert<moveit_msgs::BoundingVolume>
371 static Node encode(
const moveit_msgs::BoundingVolume &rhs);
372 static bool decode(
const Node &node, moveit_msgs::BoundingVolume &rhs);
378 static Node encode(
const moveit_msgs::MotionPlanRequest &rhs);
379 static bool decode(
const Node &node, moveit_msgs::MotionPlanRequest &rhs);
383 struct convert<trajectory_msgs::MultiDOFJointTrajectory>
385 static Node encode(
const trajectory_msgs::MultiDOFJointTrajectory &rhs);
386 static bool decode(
const Node &node, trajectory_msgs::MultiDOFJointTrajectory &rhs);
390 struct convert<trajectory_msgs::MultiDOFJointTrajectoryPoint>
392 static Node encode(
const trajectory_msgs::MultiDOFJointTrajectoryPoint &rhs);
393 static bool decode(
const Node &node, trajectory_msgs::MultiDOFJointTrajectoryPoint &rhs);
397 struct convert<moveit_msgs::RobotTrajectory>
399 static Node encode(
const moveit_msgs::RobotTrajectory &rhs);
400 static bool decode(
const Node &node, moveit_msgs::RobotTrajectory &rhs);
moveit_msgs::MotionPlanRequest MotionPlanRequest