safety_limiter: enable omni-directional safety brake

safety_limiter/src/safety_limiter.cpp

@@ -426,9 +426,9 @@ class SafetyLimiterNode
     Eigen::Affine3f move_inv;
     Eigen::Affine3f motion =
         Eigen::AngleAxisf(-twist_.angular.z * dt_, Eigen::Vector3f::UnitZ()) *
-        Eigen::Translation3f(Eigen::Vector3f(-twist_.linear.x * dt_, 0.0, 0.0));
+        Eigen::Translation3f(Eigen::Vector3f(-twist_.linear.x * dt_, -twist_.linear.y * dt_, 0.0));
     Eigen::Affine3f motion_inv =
-        Eigen::Translation3f(Eigen::Vector3f(twist_.linear.x * dt_, 0.0, 0.0)) *
+        Eigen::Translation3f(Eigen::Vector3f(twist_.linear.x * dt_, twist_.linear.y * dt_, 0.0)) *
         Eigen::AngleAxisf(twist_.angular.z * dt_, Eigen::Vector3f::UnitZ());
     move.setIdentity();
     move_inv.setIdentity();
@@ -442,13 +442,14 @@ class SafetyLimiterNode
     float d_escape_remain = 0;
     float yaw_escape_remain = 0;
     has_collision_at_now_ = false;
+    const double linear_vel = std::hypot(twist_.linear.x, twist_.linear.y);
 
     for (float t = 0; t < tmax_; t += dt_)
     {
       if (t != 0)
       {
-        d_col += twist_.linear.x * dt_;
-        d_escape_remain -= std::abs(twist_.linear.x) * dt_;
+        d_col += linear_vel * dt_;
+        d_escape_remain -= linear_vel * dt_;
         yaw_col += twist_.angular.z * dt_;
         yaw_escape_remain -= std::abs(twist_.angular.z) * dt_;
         move = move * motion;
@@ -457,7 +458,6 @@ class SafetyLimiterNode
 
       pcl::PointXYZ center;
       center = pcl::transformPoint(center, move_inv);
-
       std::vector<int> indices;
       std::vector<float> dist;
       const int num = kdtree.radiusSearch(center, footprint_radius_, indices, dist);
@@ -541,7 +541,7 @@ class SafetyLimiterNode
             std::sqrt(std::pow(acc_dtsq[1], 2) + 2 * acc_dtsq[1] * std::abs(yaw_col)));
 
     float d_r =
-        std::sqrt(std::abs(2 * acc_[0] * d_col)) / std::abs(twist_.linear.x);
+        std::sqrt(std::abs(2 * acc_[0] * d_col)) / linear_vel;
     float yaw_r =
         std::sqrt(std::abs(2 * acc_[1] * yaw_col)) / std::abs(twist_.angular.z);
     if (!std::isfinite(d_r))
@@ -559,14 +559,16 @@ class SafetyLimiterNode
     if (r_lim_ < 1.0 - EPSILON)
     {
       out.linear.x *= r_lim_;
+      out.linear.y *= r_lim_;
       out.angular.z *= r_lim_;
       if (std::abs(in.linear.x - out.linear.x) > EPSILON ||
+          std::abs(in.linear.y - out.linear.y) > EPSILON ||
           std::abs(in.angular.z - out.angular.z) > EPSILON)
       {
         ROS_WARN_THROTTLE(
-            1.0, "safety_limiter: (%0.2f, %0.2f)->(%0.2f, %0.2f)",
-            in.linear.x, in.angular.z,
-            out.linear.x, out.angular.z);
+            1.0, "safety_limiter: (%0.2f, %0.2f, %0.2f)->(%0.2f, %0.2f, %0.2f)",
+            in.linear.x, in.linear.y, in.angular.z,
+            out.linear.x, out.linear.y, out.angular.z);
       }
     }
     return out;
@@ -576,9 +578,12 @@ class SafetyLimiterNode
   limitMaxVelocities(const geometry_msgs::Twist& in)
   {
     auto out = in;
-    out.linear.x = (out.linear.x > 0) ?
-                       std::min(out.linear.x, max_values_[0]) :
-                       std::max(out.linear.x, -max_values_[0]);
+    const double vel_ratio = max_values_[0] / std::hypot(out.linear.x, out.linear.y);
+    if (vel_ratio < 1.0)
+    {
+      out.linear.x *= vel_ratio;
+      out.linear.y *= vel_ratio;
+    }
 
     out.angular.z = (out.angular.z > 0) ?
                         std::min(out.angular.z, max_values_[1]) :

safety_limiter/test/include/test_safety_limiter_base.h

@@ -166,10 +166,12 @@ class SafetyLimiterTest : public ::testing::Test
   }
   inline void publishTwist(
       const float lin,
-      const float ang)
+      const float ang,
+      const float lin_y = 0.0)
   {
     geometry_msgs::Twist cmd_vel_out;
     cmd_vel_out.linear.x = lin;
+    cmd_vel_out.linear.y = lin_y;
     cmd_vel_out.angular.z = ang;
     pub_cmd_vel_.publish(cmd_vel_out);
   }

safety_limiter/test/src/test_safety_limiter.cpp

@@ -609,6 +609,86 @@ TEST_F(SafetyLimiterTest, SafetyLimitMaxVelocitiesValues)
   }
 }
 
+TEST_F(SafetyLimiterTest, SafetyLimitOmniDirectional)
+{
+  ros::Rate wait(20.0);
+  const double vel = 1.5;
+  const double threshold_angle = std::atan2(1.0, 0.1);
+  for (double angle = -M_PI; angle < M_PI; angle += M_PI / 8)
+  {
+    for (int i = 0; i < 10 && ros::ok(); ++i)
+    {
+      publishSinglePointPointcloud2(2.0, 0, 0, "base_link", ros::Time::now());
+      publishWatchdogReset();
+      publishTwist(0, 0);
+      wait.sleep();
+      ros::spinOnce();
+    }
+
+    double obstacle_x = 0.5 * std::cos(angle);
+    double obstacle_y = 0.5 * std::sin(angle);
+    if ((M_PI - threshold_angle) < std::abs(angle))
+    {
+      // Moving backward
+      obstacle_x += -2.0;
+    }
+    else if (threshold_angle < std::abs(angle))
+    {
+      if (angle > 0)
+      {
+        // Moving left
+        obstacle_x += -1.0;
+        obstacle_y += 0.1;
+      }
+      else
+      {
+        // Moving right
+        obstacle_x += -1.0;
+        obstacle_y += -0.1;
+      }
+    }
+    // Otherwise moving forward.
+
+    // 1.0 m/ss, obstacle at 0.5 m: limited to 1.0 m/s
+    bool received = false;
+    bool en = false;
+
+    for (int i = 0; i < 10 && ros::ok(); ++i)
+    {
+      if (i > 5)
+        en = true;
+      publishSinglePointPointcloud2(obstacle_x, obstacle_y, 0, "base_link", ros::Time::now());
+      publishWatchdogReset();
+      publishTwist(vel * std::cos(angle), 0, vel * std::sin(angle));
+      broadcastTF("odom", "base_link", 0.0, 0.0);
+
+      wait.sleep();
+      ros::spinOnce();
+      if (en && cmd_vel_)
+      {
+        received = true;
+        const double current_speed = std::hypot(cmd_vel_->linear.x, cmd_vel_->linear.y);
+        ASSERT_NEAR(current_speed, 1.0, 1e-1)
+            << " Angle: " << angle << " i: " << i
+            << " vel: (" << cmd_vel_->linear.x << "," << cmd_vel_->linear.y << ")";
+        ASSERT_FLOAT_EQ(std::atan2(cmd_vel_->linear.y, cmd_vel_->linear.x), angle)
+            << " Angle: " << angle << " i: " << i
+            << " vel: (" << cmd_vel_->linear.x << "," << cmd_vel_->linear.y << ")";
+      }
+    }
+    ASSERT_TRUE(hasDiag());
+    EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level)
+        << "message: " << diag_->status[0].message;
+
+    ASSERT_TRUE(hasStatus());
+    EXPECT_TRUE(status_->is_cloud_available);
+    EXPECT_FALSE(status_->has_watchdog_timed_out);
+    EXPECT_EQ(status_->stuck_started_since, ros::Time(0));
+
+    ASSERT_TRUE(received);
+  }
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);

safety_limiter/test/test/safety_limiter_compat_rostest.test

@@ -3,7 +3,7 @@
   <env name="GCOV_PREFIX" value="/tmp/gcov/safety_limiter_safert_limiter_compat" />
   <param name="neonavigation_compatible" value="0" />
 
-  <test test-name="test_safety_limiter" pkg="safety_limiter" type="test_safety_limiter" time-limit="100.0">
+  <test test-name="test_safety_limiter" pkg="safety_limiter" type="test_safety_limiter" time-limit="120.0">
     <remap from="cmd_vel_in" to="/safety_limiter/cmd_vel_in" />
     <remap from="cloud" to="/safety_limiter/cloud" />
     <remap from="watchdog_reset" to="/safety_limiter/watchdog_reset" />

safety_limiter/test/test/safety_limiter_rostest.test

@@ -3,7 +3,7 @@
   <env name="GCOV_PREFIX" value="/tmp/gcov/safety_limiter_safert_limiter" />
   <param name="neonavigation_compatible" value="1" />
 
-  <test test-name="test_safety_limiter" pkg="safety_limiter" type="test_safety_limiter" time-limit="100.0" />
+  <test test-name="test_safety_limiter" pkg="safety_limiter" type="test_safety_limiter" time-limit="120.0" />
 
   <node pkg="safety_limiter" type="safety_limiter" name="safety_limiter" output="screen">
     <rosparam param="footprint">[[0.0, -0.1], [0.0, 0.1], [-2.0, 0.1], [-2.0, -0.1]]</rosparam>