msg_helpers.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 *  Copyright (c) 2011, Willow Garage, Inc.
00004 *  All rights reserved.
00005 *
00006 *  Redistribution and use in source and binary forms, with or without
00007 *  modification, are permitted provided that the following conditions
00008 *  are met:
00009 *
00010 *   * Redistributions of source code must retain the above copyright
00011 *     notice, this list of conditions and the following disclaimer.
00012 *   * Redistributions in binary form must reproduce the above
00013 *     copyright notice, this list of conditions and the following
00014 *     disclaimer in the documentation and/or other materials provided
00015 *     with the distribution.
00016 *   * Neither the name of the Willow Garage nor the names of its
00017 *     contributors may be used to endorse or promote products derived
00018 *     from this software without specific prior written permission.
00019 *
00020 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 *  POSSIBILITY OF SUCH DAMAGE.
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     // A suite of simple msg "constructor" functions. It would be great if this feature was added to ROS msg generation...
00055     // Also includes a number of converter functions, for convenience...
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 tf::Transform &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 } // namepsace msg
00233 
00234 } // namespace object_manipulator
00235 
00236 #endif


object_manipulator
Author(s): Matei Ciocarlie and Kaijen Hsiao
autogenerated on Thu Jan 2 2014 11:39:04