Fix Eigen::Affine3d for Melodic (using Eigen::Isometry3d)

MIGRATION.md

@@ -4,5 +4,5 @@ API changes in Rviz Visual Tools releases
 
 ## ROS Melodic
 
+- Affine3d no longer used, replaced by more computationally efficient Isometry3d. Simple find-replace should suffice
 - RvizVisualTools::publishMarkers called ros::spinOnce() in previous versions. This was usually a bug as it might lead to running ROS callback functions in unrelated threads (e.g. your UI thread). In the unlikely case that your program relied on publishMarkers calling ros::spinOnce() you will now have to add either a ros::spinOnce after your call to publishMarkers or simply add a ros::AsyncSpinner. See http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning for more details.
-

README.md

@@ -93,7 +93,7 @@ Start rviz and create a new marker using the 'Add' button at the bottom right. C
 In the following snippet we create a pose at xyz (0.1, 0.1, 0.1) and rotate the pose down 45 degrees along the Y axis. Then we publish the pose as a arrow for visualziation in Rviz. Make sure your Rviz fixed frame is the same as the one chosen in the code.
 
     // Create pose
-    Eigen::Affine3d pose;
+    Eigen::Isometry3d pose;
     pose = Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitY()); // rotate along X axis by 45 degrees
     pose.translation() = Eigen::Vector3d( 0.1, 0.1, 0.1 ); // translate x,y,z
 
@@ -257,7 +257,7 @@ This class quickly gives you basic 6dof pose interactive marker funcitonality. A
 This tool lets you easily debug Eigen transforms in Rviz. Demo use:
 
     rviz_visual_tools::TFVisualTools tf_visualizer;
-    Eigen::Affine3d world_to_shelf_transform = Eigen::Affine3d::Identity(); // or whatever value
+    Eigen::Isometry3d world_to_shelf_transform = Eigen::Isometry3d::Identity(); // or whatever value
     tf_visualizer.publishTransform(world_to_shelf_transform, "world", "shelf");
 
 *Note: this is a work in progress*

include/rviz_visual_tools/imarker_simple.h

@@ -78,7 +78,7 @@ class IMarkerSimple
 
   geometry_msgs::Pose& getPose();
 
-  void setPose(const Eigen::Affine3d& pose);
+  void setPose(const Eigen::Isometry3d& pose);
 
   void setPose(const geometry_msgs::Pose& pose);
 

include/rviz_visual_tools/tf_visual_tools.h

@@ -74,7 +74,7 @@ class TFVisualTools
    * \brief Visualize transforms in Rviz, etc
    * \return true on success
    */
-  bool publishTransform(const Eigen::Affine3d& transform, const std::string& from_frame, const std::string& to_frame);
+  bool publishTransform(const Eigen::Isometry3d& transform, const std::string& from_frame, const std::string& to_frame);
 
   /**
    * \brief At a certain frequency update the tf transforms that we are tracking

src/imarker_simple.cpp

@@ -63,7 +63,7 @@ geometry_msgs::Pose& IMarkerSimple::getPose()
   return latest_pose_;
 }
 
-void IMarkerSimple::setPose(const Eigen::Affine3d& pose)
+void IMarkerSimple::setPose(const Eigen::Isometry3d& pose)
 {
   geometry_msgs::Pose pose_msg;
   rviz_visual_tools::RvizVisualTools::convertPoseSafe(pose, pose_msg);

src/rviz_visual_tools_demo.cpp

@@ -79,18 +79,18 @@ class RvizVisualToolsDemo
     visual_tools_->enableBatchPublishing();
   }
 
-  void publishLabelHelper(const Eigen::Affine3d& pose, const std::string& label)
+  void publishLabelHelper(const Eigen::Isometry3d& pose, const std::string& label)
   {
-    Eigen::Affine3d pose_copy = pose;
+    Eigen::Isometry3d pose_copy = pose;
     pose_copy.translation().x() -= 0.2;
     visual_tools_->publishText(pose_copy, label, rvt::WHITE, rvt::XXLARGE, false);
   }
 
   void testRows(double& x_location)
   {
     // Create pose
-    Eigen::Affine3d pose1 = Eigen::Affine3d::Identity();
-    Eigen::Affine3d pose2 = Eigen::Affine3d::Identity();
+    Eigen::Isometry3d pose1 = Eigen::Isometry3d::Identity();
+    Eigen::Isometry3d pose2 = Eigen::Isometry3d::Identity();
 
     pose1.translation().x() = x_location;
 
@@ -137,7 +137,7 @@ class RvizVisualToolsDemo
 
     // --------------------------------------------------------------------
     ROS_INFO_STREAM_NAMED(name_, "Displaying Arrows");
-    pose1 = Eigen::Affine3d::Identity();
+    pose1 = Eigen::Isometry3d::Identity();
     y += space_between_rows;
     pose1.translation().y() = y;
     step = 0.025;
@@ -158,8 +158,8 @@ class RvizVisualToolsDemo
     ROS_INFO_STREAM_NAMED(name_, "Displaying Rectangular Cuboid");
     double cuboid_max_size = 0.075;
     double cuboid_min_size = 0.01;
-    pose1 = Eigen::Affine3d::Identity();
-    pose2 = Eigen::Affine3d::Identity();
+    pose1 = Eigen::Isometry3d::Identity();
+    pose2 = Eigen::Isometry3d::Identity();
     y += space_between_rows;
     pose1.translation().y() = y;
     pose2.translation().y() = y;
@@ -185,8 +185,8 @@ class RvizVisualToolsDemo
     ROS_INFO_STREAM_NAMED(name_, "Displaying Lines");
     double line_max_size = 0.075;
     double line_min_size = 0.01;
-    pose1 = Eigen::Affine3d::Identity();
-    pose2 = Eigen::Affine3d::Identity();
+    pose1 = Eigen::Isometry3d::Identity();
+    pose2 = Eigen::Isometry3d::Identity();
     y += space_between_rows;
     pose1.translation().y() = y;
     pose2.translation().y() = y;
@@ -210,7 +210,7 @@ class RvizVisualToolsDemo
 
     // --------------------------------------------------------------------
     ROS_INFO_STREAM_NAMED(name_, "Displaying Cylinder");
-    pose1 = Eigen::Affine3d::Identity();
+    pose1 = Eigen::Isometry3d::Identity();
     y += space_between_rows;
     pose1.translation().y() = y;
     step = 0.025;
@@ -229,7 +229,7 @@ class RvizVisualToolsDemo
 
     // --------------------------------------------------------------------
     ROS_INFO_STREAM_NAMED(name_, "Displaying Axis Cone");
-    pose1 = Eigen::Affine3d::Identity();
+    pose1 = Eigen::Isometry3d::Identity();
     y += space_between_rows;
     pose1.translation().y() = y;
     step = 0.025;
@@ -252,7 +252,7 @@ class RvizVisualToolsDemo
 
     // --------------------------------------------------------------------
     ROS_INFO_STREAM_NAMED(name_, "Displaying Wireframe Cuboid");
-    pose1 = Eigen::Affine3d::Identity();
+    pose1 = Eigen::Isometry3d::Identity();
     y += space_between_rows;
     pose1.translation().y() = y;
     step = 0.1;
@@ -275,7 +275,7 @@ class RvizVisualToolsDemo
 
     // --------------------------------------------------------------------
     ROS_INFO_STREAM_NAMED(name_, "Displaying Sized Wireframe Cuboid");
-    pose1 = Eigen::Affine3d::Identity();
+    pose1 = Eigen::Isometry3d::Identity();
     y += space_between_rows;
     pose1.translation().y() = y;
     step = 0.1;
@@ -295,7 +295,7 @@ class RvizVisualToolsDemo
 
     // --------------------------------------------------------------------
     ROS_INFO_STREAM_NAMED(name_, "Displaying Planes");
-    pose1 = Eigen::Affine3d::Identity();
+    pose1 = Eigen::Isometry3d::Identity();
     y += space_between_rows;
     pose1.translation().y() = y;
     step = 0.2;
@@ -317,7 +317,7 @@ class RvizVisualToolsDemo
 
     // --------------------------------------------------------------------
     ROS_INFO_STREAM_NAMED(name_, "Displaying Graph");
-    pose1 = Eigen::Affine3d::Identity();
+    pose1 = Eigen::Isometry3d::Identity();
     y += space_between_rows;
     pose1.translation().y() = y;
     step = 0.1;
@@ -373,8 +373,8 @@ class RvizVisualToolsDemo
 
     // --------------------------------------------------------------------
     ROS_INFO_STREAM_NAMED(name_, "Displaying Multi-Color Path");
-    pose1 = Eigen::Affine3d::Identity();
-    pose2 = Eigen::Affine3d::Identity();
+    pose1 = Eigen::Isometry3d::Identity();
+    pose2 = Eigen::Isometry3d::Identity();
     y += space_between_rows;
     pose1.translation().y() = y;
     step = 0.1;
@@ -414,8 +414,8 @@ class RvizVisualToolsDemo
   void testSize(double& x_location, scales scale)
   {
     // Create pose
-    Eigen::Affine3d pose1 = Eigen::Affine3d::Identity();
-    Eigen::Affine3d pose2 = Eigen::Affine3d::Identity();
+    Eigen::Isometry3d pose1 = Eigen::Isometry3d::Identity();
+    Eigen::Isometry3d pose2 = Eigen::Isometry3d::Identity();
 
     // Reusable vector of 2 colors
     std::vector<colors> colors;
@@ -545,8 +545,8 @@ class RvizVisualToolsDemo
     ROS_INFO_STREAM_NAMED(name_, "Testing sizes of marker scale");
 
     // Create pose
-    Eigen::Affine3d pose1 = Eigen::Affine3d::Identity();
-    Eigen::Affine3d pose2;
+    Eigen::Isometry3d pose1 = Eigen::Isometry3d::Identity();
+    Eigen::Isometry3d pose2;
 
     // Show test label
     pose1.translation().x() = x_location - 0.1;

src/tf_visual_tools.cpp

@@ -54,7 +54,7 @@ TFVisualTools::TFVisualTools(double loop_hz)
   ROS_INFO_STREAM_NAMED("tf_visual_tools", "TFVisualTools Ready.");
 }
 
-bool TFVisualTools::publishTransform(const Eigen::Affine3d& transform, const std::string& from_frame,
+bool TFVisualTools::publishTransform(const Eigen::Isometry3d& transform, const std::string& from_frame,
                                      const std::string& to_frame)
 {
   ROS_DEBUG_STREAM_NAMED("tf_visual_tools", "Publishing transform from " << from_frame << " to " << to_frame);

tests/rvt_test.cpp

@@ -64,7 +64,7 @@ class RVTTest
     return true;
   }
 
-  bool testAffine3d(const std::string& id, const Eigen::Affine3d& expect, const Eigen::Affine3d& actual)
+  bool testIsometry3d(const std::string& id, const Eigen::Isometry3d& expect, const Eigen::Isometry3d& actual)
   {
     static const double EPSILON = 0.000001;
     EXPECT_GT(EPSILON, fabs(expect.translation().x() - actual.translation().x()))
@@ -112,7 +112,7 @@ TEST(RVTTest, initialize)
 TEST(RVTTest, test_rpy_conversions)
 {
   // Identity conversions with RPY
-  Eigen::Affine3d expected_affine = Eigen::Affine3d::Identity();
+  Eigen::Isometry3d expected_affine = Eigen::Isometry3d::Identity();
   std::vector<double> xyzrpy;
   base.visual_tools_->convertToXYZRPY(expected_affine, xyzrpy);
   std::vector<double> expected_vector;
@@ -125,8 +125,8 @@ TEST(RVTTest, test_rpy_conversions)
   EXPECT_TRUE(base.testVector("Identity: ", expected_vector, xyzrpy));
 
   // Identity conversion back to Eigen
-  Eigen::Affine3d expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy, rviz_visual_tools::XYZ);
-  EXPECT_TRUE(base.testAffine3d("Identity convert back", expected_affine, expected_affine2));
+  Eigen::Isometry3d expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy, rviz_visual_tools::XYZ);
+  EXPECT_TRUE(base.testIsometry3d("Identity convert back", expected_affine, expected_affine2));
 
   // -------------------------------------------------------------------
   // Translation conversions to RPY
@@ -141,17 +141,17 @@ TEST(RVTTest, test_rpy_conversions)
 
   // Translation convertion back to Eigen
   expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy, rviz_visual_tools::XYZ);
-  EXPECT_TRUE(base.testAffine3d("123 convert back", expected_affine, expected_affine2));
+  EXPECT_TRUE(base.testIsometry3d("123 convert back", expected_affine, expected_affine2));
 
   // Translation convertion back to Eigen via long function
   expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy[0], xyzrpy[1], xyzrpy[2], xyzrpy[3], xyzrpy[4],
                                                            xyzrpy[5], rviz_visual_tools::XYZ);
-  EXPECT_TRUE(base.testAffine3d("123 convert back long", expected_affine, expected_affine2));
+  EXPECT_TRUE(base.testIsometry3d("123 convert back long", expected_affine, expected_affine2));
 
   // Translation convertion back to Eigen via NEW long function
   expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy[0], xyzrpy[1], xyzrpy[2], xyzrpy[3], xyzrpy[4],
                                                            xyzrpy[5], rviz_visual_tools::XYZ);
-  EXPECT_TRUE(base.testAffine3d("123 convert back new long", expected_affine, expected_affine2));
+  EXPECT_TRUE(base.testIsometry3d("123 convert back new long", expected_affine, expected_affine2));
 
   // -------------------------------------------------------------------
   // Rotation conversions to RPY
@@ -162,17 +162,17 @@ TEST(RVTTest, test_rpy_conversions)
 
   // Rotation convertion back to Eigen
   expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy, rviz_visual_tools::XYZ);
-  EXPECT_TRUE(base.testAffine3d("123 convert back", expected_affine, expected_affine2));
+  EXPECT_TRUE(base.testIsometry3d("123 convert back", expected_affine, expected_affine2));
 
   // Rotation convertion back to Eigen via long function
   expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy[0], xyzrpy[1], xyzrpy[2], xyzrpy[3], xyzrpy[4],
                                                            xyzrpy[5], rviz_visual_tools::XYZ);
-  EXPECT_TRUE(base.testAffine3d("123 convert back long", expected_affine, expected_affine2));
+  EXPECT_TRUE(base.testIsometry3d("123 convert back long", expected_affine, expected_affine2));
 
   // Rotation convertion back to Eigen via NEW long function
   expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy[0], xyzrpy[1], xyzrpy[2], xyzrpy[3], xyzrpy[4],
                                                            xyzrpy[5], rviz_visual_tools::XYZ);
-  EXPECT_TRUE(base.testAffine3d("123 convert back new long", expected_affine, expected_affine2));
+  EXPECT_TRUE(base.testIsometry3d("123 convert back new long", expected_affine, expected_affine2));
 }
 
 /* Main  ------------------------------------------------------------------------------------- */