safety_limiter: use fixed frame for accumulating input cloud (#421)

* Use fixed frame for accumulating input cloud
* Update tests
* Minimize tf delay influence
* Rename variable

safety_limiter/CMakeLists.txt

@@ -8,6 +8,7 @@ set(CATKIN_DEPENDS
     diagnostic_updater
     geometry_msgs
     pcl_conversions
+    pcl_ros
     sensor_msgs
     std_msgs
     tf2_ros
@@ -57,6 +58,7 @@ add_dependencies(safety_limiter ${catkin_EXPORTED_TARGETS})
 if(CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
   find_package(nav_msgs REQUIRED)
+  find_package(tf2_geometry_msgs REQUIRED)
   add_subdirectory(test)
 
   find_package(roslint REQUIRED)

safety_limiter/package.xml

@@ -12,12 +12,14 @@
 
   <depend>roscpp</depend>
   <test_depend>nav_msgs</test_depend>
+  <test_depend>tf2_geometry_msgs</test_depend>
   <test_depend>roslint</test_depend>
   <test_depend>rostest</test_depend>
 
   <depend>diagnostic_updater</depend>
   <depend>geometry_msgs</depend>
   <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>tf2_ros</depend>

safety_limiter/src/safety_limiter.cpp

@@ -57,6 +57,7 @@
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/transforms.h>
 
 #include <neonavigation_common/compatibility.h>
 
@@ -108,7 +109,7 @@ class SafetyLimiterNode
 
   geometry_msgs::Twist twist_;
   ros::Time last_cloud_stamp_;
-  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_accum_;
   bool cloud_clear_;
   double hz_;
   double timeout_;
@@ -125,7 +126,8 @@ class SafetyLimiterNode
   double z_range_[2];
   float footprint_radius_;
   double downsample_grid_;
-  std::string frame_id_;
+  std::string fixed_frame_id_;
+  std::string base_frame_id_;
 
   ros::Time last_disable_cmd_;
   ros::Duration hold_;
@@ -148,7 +150,7 @@ class SafetyLimiterNode
     : nh_()
     , pnh_("~")
     , tfl_(tfbuf_)
-    , cloud_(new pcl::PointCloud<pcl::PointXYZ>)
+    , cloud_accum_(new pcl::PointCloud<pcl::PointXYZ>)
     , cloud_clear_(false)
     , last_disable_cmd_(0)
     , watchdog_stop_(false)
@@ -207,7 +209,8 @@ class SafetyLimiterNode
     pnh_.param("yaw_margin", yaw_margin_, 0.2);
     pnh_.param("yaw_escape", yaw_escape_, 0.05);
     pnh_.param("downsample_grid", downsample_grid_, 0.05);
-    pnh_.param("frame_id", frame_id_, std::string("base_link"));
+    pnh_.param("base_frame", base_frame_id_, std::string("base_link"));
+    pnh_.param("fixed_frame", fixed_frame_id_, std::string("odom"));
     double hold_d;
     pnh_.param("hold", hold_d, 0.0);
     hold_ = ros::Duration(std::max(hold_d, 1.0 / hz_));
@@ -307,7 +310,7 @@ class SafetyLimiterNode
       geometry_msgs::Twist cmd_vel;
       pub_twist_.publish(cmd_vel);
 
-      cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
+      cloud_accum_.reset(new pcl::PointCloud<pcl::PointXYZ>);
       has_cloud_ = false;
       r_lim_ = 0;
 
@@ -330,9 +333,49 @@ class SafetyLimiterNode
   }
   double predict(const geometry_msgs::Twist& in)
   {
+    if (cloud_accum_->size() == 0)
+    {
+      if (allow_empty_cloud_)
+      {
+        return 1.0;
+      }
+      ROS_WARN_THROTTLE(1.0, "safety_limiter: Empty pointcloud passed.");
+      return 0.0;
+    }
+
+    const bool can_transform = tfbuf_.canTransform(
+        base_frame_id_, cloud_accum_->header.frame_id,
+        pcl_conversions::fromPCL(cloud_accum_->header.stamp));
+    const ros::Time stamp =
+        can_transform ? pcl_conversions::fromPCL(cloud_accum_->header.stamp) : ros::Time(0);
+
+    geometry_msgs::TransformStamped fixed_to_base;
+    try
+    {
+      fixed_to_base = tfbuf_.lookupTransform(
+          base_frame_id_, cloud_accum_->header.frame_id, stamp);
+    }
+    catch (tf2::TransformException& e)
+    {
+      ROS_WARN_THROTTLE(1.0, "safety_limiter: Transform failed: %s", e.what());
+      return 0.0;
+    }
+
+    const Eigen::Affine3f fixed_to_base_eigen =
+        Eigen::Translation3f(
+            fixed_to_base.transform.translation.x,
+            fixed_to_base.transform.translation.y,
+            fixed_to_base.transform.translation.z) *
+        Eigen::Quaternionf(
+            fixed_to_base.transform.rotation.w,
+            fixed_to_base.transform.rotation.x,
+            fixed_to_base.transform.rotation.y,
+            fixed_to_base.transform.rotation.z);
+    pcl::transformPointCloud(*cloud_accum_, *cloud_accum_, fixed_to_base_eigen);
+
     pcl::PointCloud<pcl::PointXYZ>::Ptr pc(new pcl::PointCloud<pcl::PointXYZ>);
     pcl::VoxelGrid<pcl::PointXYZ> ds;
-    ds.setInputCloud(cloud_);
+    ds.setInputCloud(cloud_accum_);
     ds.setLeafSize(downsample_grid_, downsample_grid_, downsample_grid_);
     ds.filter(*pc);
 
@@ -370,7 +413,7 @@ class SafetyLimiterNode
     move.setIdentity();
     move_inv.setIdentity();
     sensor_msgs::PointCloud col_points;
-    col_points.header.frame_id = frame_id_;
+    col_points.header.frame_id = base_frame_id_;
     col_points.header.stamp = ros::Time::now();
 
     float d_col = 0;
@@ -641,28 +684,35 @@ class SafetyLimiterNode
   }
   void cbCloud(const sensor_msgs::PointCloud2::ConstPtr& msg)
   {
-    pcl::PointCloud<pcl::PointXYZ>::Ptr pc(new pcl::PointCloud<pcl::PointXYZ>());
+    const bool can_transform = tfbuf_.canTransform(
+        fixed_frame_id_, msg->header.frame_id, msg->header.stamp);
+    const ros::Time stamp =
+        can_transform ? msg->header.stamp : ros::Time(0);
 
+    sensor_msgs::PointCloud2 cloud_msg_fixed;
     try
     {
-      sensor_msgs::PointCloud2 pc2_tmp;
-      geometry_msgs::TransformStamped trans = tfbuf_.lookupTransform(
-          frame_id_, msg->header.frame_id, msg->header.stamp, ros::Duration(0.1));
-      tf2::doTransform(*msg, pc2_tmp, trans);
-      pcl::fromROSMsg(pc2_tmp, *pc);
+      const geometry_msgs::TransformStamped cloud_to_fixed =
+          tfbuf_.lookupTransform(fixed_frame_id_, msg->header.frame_id, stamp);
+      tf2::doTransform(*msg, cloud_msg_fixed, cloud_to_fixed);
     }
     catch (tf2::TransformException& e)
     {
       ROS_WARN_THROTTLE(1.0, "safety_limiter: Transform failed: %s", e.what());
       return;
     }
 
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_fixed(new pcl::PointCloud<pcl::PointXYZ>());
+    cloud_fixed->header.frame_id = fixed_frame_id_;
+    pcl::fromROSMsg(cloud_msg_fixed, *cloud_fixed);
+
     if (cloud_clear_)
     {
       cloud_clear_ = false;
-      cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
+      cloud_accum_.reset(new pcl::PointCloud<pcl::PointXYZ>);
     }
-    *cloud_ += *pc;
+    *cloud_accum_ += *cloud_fixed;
+    cloud_accum_->header.frame_id = fixed_frame_id_;
     last_cloud_stamp_ = msg->header.stamp;
     has_cloud_ = true;
   }

safety_limiter/test/include/test_safety_limiter_base.h

@@ -40,6 +40,8 @@
 #include <sensor_msgs/point_cloud2_iterator.h>
 #include <std_msgs/Empty.h>
 #include <safety_limiter_msgs/SafetyLimiterStatus.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_broadcaster.h>
 
 #include <gtest/gtest.h>
 
@@ -87,6 +89,8 @@ class SafetyLimiterTest : public ::testing::Test
   ros::Subscriber sub_status_;
   ros::Subscriber sub_cmd_vel_;
 
+  tf2_ros::TransformBroadcaster tfb_;
+
   inline void cbDiag(const diagnostic_msgs::DiagnosticArray::ConstPtr& msg)
   {
     diag_ = msg;
@@ -123,6 +127,7 @@ class SafetyLimiterTest : public ::testing::Test
     {
       publishEmptyPointPointcloud2("base_link", ros::Time::now());
       publishWatchdogReset();
+      broadcastTF("odom", "base_link", 0.0, 0.0);
 
       wait.sleep();
       ros::spinOnce();
@@ -168,6 +173,20 @@ class SafetyLimiterTest : public ::testing::Test
     cmd_vel_out.angular.z = ang;
     pub_cmd_vel_.publish(cmd_vel_out);
   }
+  inline void broadcastTF(
+      const std::string parent_frame_id,
+      const std::string child_frame_id,
+      const float lin,
+      const float ang)
+  {
+    geometry_msgs::TransformStamped trans;
+    trans.header.stamp = ros::Time::now();
+    trans.transform = tf2::toMsg(
+        tf2::Transform(tf2::Quaternion(tf2::Vector3(0, 0, 1), ang), tf2::Vector3(lin, 0, 0)));
+    trans.header.frame_id = parent_frame_id;
+    trans.child_frame_id = child_frame_id;
+    tfb_.sendTransform(trans);
+  }
   inline bool hasDiag() const
   {
     if (!diag_)

safety_limiter/test/src/test_safety_limiter.cpp

@@ -74,6 +74,7 @@ TEST_F(SafetyLimiterTest, Timeouts)
         const float vel = 0.1;
         const float ang_vel = 0.2;
         publishTwist(vel, ang_vel);
+        broadcastTF("odom", "base_link", 0.0, 0.0);
 
         wait.sleep();
         ros::spinOnce();
@@ -162,6 +163,7 @@ TEST_F(SafetyLimiterTest, CloudBuffering)
 
     publishWatchdogReset();
     publishTwist(2.0, 0);
+    broadcastTF("odom", "base_link", 0.0, 0.0);
 
     wait.sleep();
     ros::spinOnce();
@@ -201,6 +203,7 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinear)
       publishSinglePointPointcloud2(0.5, 0, 0, "base_link", ros::Time::now());
       publishWatchdogReset();
       publishTwist(vel, ((i % 5) - 2.0) * 0.01);
+      broadcastTF("odom", "base_link", 0.0, 0.0);
 
       wait.sleep();
       ros::spinOnce();
@@ -251,6 +254,7 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinearBackward)
       publishSinglePointPointcloud2(-2.5, 0, 0, "base_link", ros::Time::now());
       publishWatchdogReset();
       publishTwist(vel, ((i % 5) - 2.0) * 0.01);
+      broadcastTF("odom", "base_link", 0.0, 0.0);
 
       wait.sleep();
       ros::spinOnce();
@@ -302,6 +306,7 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinearEscape)
       publishSinglePointPointcloud2(-0.05, 0, 0, "base_link", ros::Time::now());
       publishWatchdogReset();
       publishTwist(vel, 0.0);
+      broadcastTF("odom", "base_link", 0.0, 0.0);
 
       wait.sleep();
       ros::spinOnce();
@@ -369,6 +374,7 @@ TEST_F(SafetyLimiterTest, SafetyLimitAngular)
       publishSinglePointPointcloud2(-1, -1.1, 0, "base_link", ros::Time::now());
       publishWatchdogReset();
       publishTwist((i % 3) * 0.01, vel);
+      broadcastTF("odom", "base_link", 0.0, 0.0);
 
       wait.sleep();
       ros::spinOnce();
@@ -420,6 +426,7 @@ TEST_F(SafetyLimiterTest, SafetyLimitAngularEscape)
       publishSinglePointPointcloud2(-1, -0.09, 0, "base_link", ros::Time::now());
       publishWatchdogReset();
       publishTwist(0.0, vel);
+      broadcastTF("odom", "base_link", 0.0, 0.0);
 
       wait.sleep();
       ros::spinOnce();
@@ -478,6 +485,7 @@ TEST_F(SafetyLimiterTest, NoCollision)
         publishSinglePointPointcloud2(1000, 1000, 0, "base_link", ros::Time::now());
         publishWatchdogReset();
         publishTwist(vel, ang_vel);
+        broadcastTF("odom", "base_link", 0.0, 0.0);
 
         wait.sleep();
         ros::spinOnce();
@@ -526,6 +534,7 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinearSimpleSimulation)
 
       publishWatchdogReset();
       publishTwist(vel, 0.0);
+      broadcastTF("odom", "base_link", x, 0.0);
 
       wait.sleep();
       ros::spinOnce();

safety_limiter/test/src/test_safety_limiter2.cpp

@@ -71,6 +71,7 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinearSimpleSimulationWithMargin)
         publishSinglePointPointcloud2(-3.0 - x, 0, 0, "base_link", ros::Time::now());
       publishWatchdogReset();
       publishTwist(vel, 0.0);
+      broadcastTF("odom", "base_link", x, 0.0);
 
       wait.sleep();
       ros::spinOnce();