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 _MSG_HELPERS_H_
00035 #define _MSG_HELPERS_H_
00036
00037 #include <ros/ros.h>
00038 #include <tf/tf.h>
00039
00040 #include <std_msgs/ColorRGBA.h>
00041
00042 #include <geometry_msgs/Vector3.h>
00043 #include <geometry_msgs/Point.h>
00044 #include <geometry_msgs/Quaternion.h>
00045 #include <geometry_msgs/Pose.h>
00046 #include <geometry_msgs/Transform.h>
00047 #include <geometry_msgs/PoseStamped.h>
00048
00049 namespace object_manipulator
00050 {
00051
00052 namespace msg
00053 {
00054
00055
00056
00057 inline std_msgs::ColorRGBA createColorMsg(const float red, const float green, const float blue, const float alpha = 1.0) {
00058 std_msgs::ColorRGBA color;
00059 color.r = std::min(red, 1.0f);
00060 color.g = std::min(green, 1.0f);
00061 color.b = std::min(blue, 1.0f);
00062 color.a = alpha;
00063 return color;
00064 }
00065
00066
00067 inline geometry_msgs::Vector3 createVector3Msg(const float x, const float y, const float z) {
00068 geometry_msgs::Vector3 msg;
00069 msg.x = x;
00070 msg.y = y;
00071 msg.z = z;
00072 return msg;
00073 }
00074
00075 inline geometry_msgs::Vector3 createVector3Msg(const tf::Vector3 &v) {
00076 geometry_msgs::Vector3 msg;
00077 msg.x = v.x();
00078 msg.y = v.y();
00079 msg.z = v.z();
00080 return msg;
00081 }
00082
00083 inline geometry_msgs::Vector3 createVector3Msg(const geometry_msgs::Point &v) {
00084 geometry_msgs::Vector3 msg;
00085 msg.x = v.x;
00086 msg.y = v.y;
00087 msg.z = v.z;
00088 return msg;
00089 }
00090
00091
00092 inline geometry_msgs::Point createPointMsg(const float x, const float y, const float z) {
00093 geometry_msgs::Point msg;
00094 msg.x = x;
00095 msg.y = y;
00096 msg.z = z;
00097 return msg;
00098 }
00099
00100 inline geometry_msgs::Point createPointMsg(const tf::Point &v) {
00101 geometry_msgs::Point msg;
00102 msg.x = v.x();
00103 msg.y = v.y();
00104 msg.z = v.z();
00105 return msg;
00106 }
00107
00108 inline geometry_msgs::Point createPointMsg(const geometry_msgs::Vector3 &v) {
00109 geometry_msgs::Point msg;
00110 msg.x = v.x;
00111 msg.y = v.y;
00112 msg.z = v.z;
00113 return msg;
00114 }
00115
00116
00117 inline geometry_msgs::Quaternion createQuaternionMsg(const float x, const float y, const float z, const float w) {
00118 geometry_msgs::Quaternion msg;
00119 msg.x = x;
00120 msg.y = y;
00121 msg.z = z;
00122 msg.w = w;
00123 return msg;
00124 }
00125
00126 inline geometry_msgs::Quaternion createQuaternionMsg(const tf::Quaternion &q) {
00127 geometry_msgs::Quaternion msg;
00128 msg.x = q.x();
00129 msg.y = q.y();
00130 msg.z = q.z();
00131 msg.w = q.w();
00132 return msg;
00133 }
00134
00135
00136 inline geometry_msgs::Pose createPoseMsg(const geometry_msgs::Point &v, const geometry_msgs::Quaternion &q)
00137 {
00138 geometry_msgs::Pose msg;
00139 msg.position = v;
00140 msg.orientation = q;
00141 return msg;
00142 }
00143
00144 inline geometry_msgs::Pose createPoseMsg(const geometry_msgs::Vector3 &v, const geometry_msgs::Quaternion &q)
00145 {
00146 geometry_msgs::Pose msg;
00147 msg.position = msg::createPointMsg(v.x, v.y, v.z);
00148 msg.orientation = q;
00149 return msg;
00150 }
00151
00152 inline geometry_msgs::Pose createPoseMsg(const tf::Pose &p)
00153 {
00154 geometry_msgs::Pose msg;
00155 tf::poseTFToMsg(p, msg);
00156 return msg;
00157 }
00158
00159
00160 inline geometry_msgs::PoseStamped createPoseStampedMsg(const geometry_msgs::Pose &p, const std::string &frame_id, const ros::Time &stamp)
00161 {
00162 geometry_msgs::PoseStamped msg;
00163 msg.pose = p;
00164 msg.header.frame_id = frame_id;
00165 msg.header.stamp = stamp;
00166 return msg;
00167 }
00168
00169 inline geometry_msgs::PoseStamped createPoseStampedMsg(const geometry_msgs::Point &v, const geometry_msgs::Quaternion &q, const std::string &frame_id, const ros::Time &stamp)
00170 {
00171 return createPoseStampedMsg( createPoseMsg(v, q), frame_id, stamp );
00172 }
00173
00174 inline geometry_msgs::PoseStamped createPoseStampedMsg(const geometry_msgs::TransformStamped ts)
00175 {
00176
00177 return createPoseStampedMsg( createPoseMsg(ts.transform.translation, ts.transform.rotation), ts.header.frame_id, ts.header.stamp );
00178 }
00179
00180 inline geometry_msgs::PoseStamped createPoseStampedMsg(const btTransform &pose, const std::string &frame_id, const ros::Time &stamp)
00181 {
00182
00183 return createPoseStampedMsg( createPoseMsg(pose), frame_id, stamp );
00184 }
00185
00186
00187 inline geometry_msgs::Transform createTransformMsg(const geometry_msgs::Point &v, const geometry_msgs::Quaternion &q)
00188 {
00189 geometry_msgs::Transform msg;
00190 msg.translation = msg::createVector3Msg(v.x, v.y, v.z);
00191 msg.rotation = q;
00192 return msg;
00193 }
00194
00195 inline geometry_msgs::Transform createTransformMsg(const geometry_msgs::Vector3 &v, const geometry_msgs::Quaternion &q)
00196 {
00197 geometry_msgs::Transform msg;
00198 msg.translation = v;
00199 msg.rotation = q;
00200 return msg;
00201 }
00202
00203 inline geometry_msgs::TransformStamped createTransformStampedMsg(const geometry_msgs::Transform &t, const std::string &frame_id, const ros::Time &stamp)
00204 {
00205 geometry_msgs::TransformStamped msg;
00206 msg.transform = t;
00207 msg.header.frame_id = frame_id;
00208 msg.header.stamp = stamp;
00209 return msg;
00210 }
00211
00212 inline geometry_msgs::TransformStamped createTransformStampedMsg(const geometry_msgs::Vector3 &v, const geometry_msgs::Quaternion &q, const std::string &frame_id, const ros::Time &stamp)
00213 {
00214 return createTransformStampedMsg( createTransformMsg(v, q), frame_id, stamp );
00215 }
00216
00218 inline geometry_msgs::Pose applyShift(const geometry_msgs::Pose &pose, const tf::Vector3 &shift)
00219 {
00220 geometry_msgs::Pose out;
00221 tf::Transform P;
00222 tf::poseMsgToTF(pose, P);
00223 tf::Transform T = tf::Transform::getIdentity();
00224 T.setOrigin(shift.x()*P.getBasis().getColumn(0) +
00225 shift.y()*P.getBasis().getColumn(1) +
00226 shift.z()*P.getBasis().getColumn(2));
00227 P = T*P;
00228 tf::poseTFToMsg(P, out);
00229 return out;
00230 }
00231
00232 }
00233
00234 }
00235
00236 #endif
