New helper function getIdentityPose() (PickNikRobotics#122)

Dale-Koenig · Jun 5, 2020 · 0c6b946 · 0c6b946
1 parent 0c78ae2
commit 0c6b946
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 0 deletions.
diff --git a/include/rviz_visual_tools/rviz_visual_tools.h b/include/rviz_visual_tools/rviz_visual_tools.h
@@ -995,6 +995,7 @@ a   *        Warning: when using this in a loop be sure to call trigger() at end
    * \param Pose to fill in
    */
   static void generateEmptyPose(geometry_msgs::Pose& pose);
+  static geometry_msgs::Pose getIdentityPose();
 
   /**
    * \brief Test if two Eigen poses are close enough

diff --git a/src/rviz_visual_tools.cpp b/src/rviz_visual_tools.cpp
@@ -2685,6 +2685,22 @@ void RvizVisualTools::generateEmptyPose(geometry_msgs::Pose& pose)
   pose.orientation.w = 1;
 }
 
+geometry_msgs::Pose RvizVisualTools::getIdentityPose()
+{
+  geometry_msgs::Pose pose;
+  // Position
+  pose.position.x = 0;
+  pose.position.y = 0;
+  pose.position.z = 0;
+
+  // Orientation on place
+  pose.orientation.x = 0;
+  pose.orientation.y = 0;
+  pose.orientation.z = 0;
+  pose.orientation.w = 1;
+  return pose;
+}
+
 bool RvizVisualTools::posesEqual(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, double threshold)
 {
   static const std::size_t NUM_VARS = 16;  // size of eigen matrix