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 #ifndef UTILITIES_H_
00019 #define UTILITIES_H_
00020
00021 #include <ros/ros.h>
00022 #include <tf/tf.h>
00023 #include <tf/transform_listener.h>
00024 #include <geometry_msgs/PoseArray.h>
00025 #include <nav_msgs/Path.h>
00026 #include <object_manipulation_msgs/PlaceGoal.h>
00027 #include <object_manipulation_msgs/PickupGoal.h>
00028 #include <boost/tuple/tuple.hpp>
00029 #include <sensor_msgs/JointState.h>
00030 #include <trajectory_msgs/JointTrajectory.h>
00031 #include <mtconnect_task_parser/task.h>
00032
00033 namespace move_arm_utils
00034 {
00035
00036 bool parsePoint(XmlRpc::XmlRpcValue &val, geometry_msgs::Point &point);
00037
00038 bool parseOrientation(XmlRpc::XmlRpcValue &val, geometry_msgs::Quaternion &q);
00039
00040 bool parseOrientation(XmlRpc::XmlRpcValue &val, tf::Quaternion &q);
00041
00042 bool parseVect3(XmlRpc::XmlRpcValue &val,tf::Vector3 &v);
00043
00044 bool parseVect3(XmlRpc::XmlRpcValue &val, geometry_msgs::Vector3 &v);
00045
00046 bool parsePose(XmlRpc::XmlRpcValue &val, geometry_msgs::Pose &pose);
00047
00048 bool parseTransform(XmlRpc::XmlRpcValue &val, tf::Transform &t);
00049
00050 bool parseTaskXml(const std::string & xml,
00051 std::map<std::string, trajectory_msgs::JointTrajectoryPtr> & paths);
00052
00053 bool toJointTrajectory(boost::shared_ptr<mtconnect::Path> & path,
00054 trajectory_msgs::JointTrajectoryPtr & traj);
00055
00056 struct CartesianTrajectory
00057 {
00058 public:
00059 CartesianTrajectory():
00060 frame_id_("base_link"),
00061 cartesian_points_()
00062 {
00063
00064 }
00065
00066 ~CartesianTrajectory()
00067 {
00068
00069 }
00070
00071 std::string toString();
00072
00073 bool parseParameters(XmlRpc::XmlRpcValue ¶mVal);
00074
00075 void getMarker(nav_msgs::Path &p);
00076
00077 bool fetchParameters(std::string nameSpace= "/cartesian_trajectory");
00078
00079 std::string arm_group_;
00080 std::string frame_id_;
00081 std::string link_name_;
00082 std::vector<tf::Transform> cartesian_points_;
00083 };
00084
00085 struct PickupGoalInfo : object_manipulation_msgs::PickupGoal
00086 {
00087 public:
00088 PickupGoalInfo()
00089 {
00090
00091 }
00092
00093 ~PickupGoalInfo()
00094 {
00095
00096 }
00097
00098 bool fetchParameters(std::string nameSpace = "/pickup_goal");
00099
00100 bool parseParameters(XmlRpc::XmlRpcValue &val);
00101 };
00102
00103 struct PlaceGoalInfo : object_manipulation_msgs::PlaceGoal
00104 {
00105 public:
00106 PlaceGoalInfo()
00107 {
00108
00109 }
00110
00111 ~PlaceGoalInfo()
00112 {
00113
00114 }
00115
00116 bool fetchParameters(std::string nameSpace = "/place_goal");
00117
00118 bool parseParameters(XmlRpc::XmlRpcValue &val);
00119
00120 };
00121
00122 struct JointStateInfo: sensor_msgs::JointState
00123 {
00124 public:
00125 JointStateInfo():
00126 arm_group("")
00127 {
00128
00129 }
00130
00131 ~JointStateInfo()
00132 {
00133
00134 }
00135
00136 bool fetchParameters(std::string nameSpace = "/joint_state");
00137 bool parseParameters(XmlRpc::XmlRpcValue &val);
00138 void toJointConstraints(double tol_above,double tol_below, std::vector<arm_navigation_msgs::JointConstraint> &joint_constraints);
00139
00140 std::string arm_group;
00141 };
00142
00143 }
00144
00145 #endif