Merge pull request #22 from mrvsht/master

add poseToDirectionVector and poseToPositionVector

mesh_map/include/mesh_map/util.h

@@ -40,11 +40,13 @@
 
 #include <geometry_msgs/Point.h>
 #include <geometry_msgs/Pose.h>
+#include <geometry_msgs/PoseStamped.h>
 #include <lvr2/geometry/Handles.hpp>
 #include <lvr2/attrmaps/AttrMaps.hpp>
 #include <lvr2/geometry/BaseVector.hpp>
 #include <lvr2/geometry/Normal.hpp>
 #include <std_msgs/ColorRGBA.h>
+#include <tf2/LinearMath/Vector3.h>
 
 namespace mesh_map
 {
@@ -206,6 +208,24 @@ std_msgs::ColorRGBA getRainbowColor(const float value);
  */
 void getRainbowColor(float value, float& r, float& g, float& b);
 
+/**
+ * Converts the orientation of a geometry_msgs/PoseStamped message to a direction vector
+ * @param pose      the pose to convert
+ * @return          direction normal vector
+ */
+mesh_map::Normal poseToDirectionVector(
+      const geometry_msgs::PoseStamped& pose,
+      const tf2::Vector3& axis=tf2::Vector3(1,0,0));
+
+
+/**
+ * Converts the position of a geometry_msgs/PoseStamped message to a position vector
+ * @param pose      the pose to convert
+ * @return          position vector
+ */
+  mesh_map::Vector poseToPositionVector(
+      const geometry_msgs::PoseStamped& pose);
+
 } /* namespace mesh_map */
 
 #endif  // MESH_MAP__UTIL_H

mesh_map/src/util.cpp

@@ -232,4 +232,18 @@ void getRainbowColor(float value, float& r, float& g, float& b)
     r = 1, g = n, b = 0;
 }
 
+mesh_map::Normal poseToDirectionVector(const geometry_msgs::PoseStamped& pose, const tf2::Vector3& axis)
+{
+  tf2::Stamped<tf2::Transform> transform;
+  fromMsg(pose, transform);
+  tf2::Vector3 v = transform.getBasis() * axis;
+  return mesh_map::Normal(v.x(), v.y(), v.z());
+}
+
+
+mesh_map::Vector poseToPositionVector(const geometry_msgs::PoseStamped& pose)
+{
+  return mesh_map::Vector(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z);
+}
+
 } /* namespace mesh_map */