Social navigation patch

cabot/src/safety/lidar_speed_control_node.cpp

@@ -58,6 +58,8 @@ class LiDARSpeedControlNode : public rclcpp::Node
   double limit_factor_;
   double min_distance_;
   double front_angle_in_degree_;
+  double front_min_range_;
+  double blind_spot_min_range_;
 
   rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr vis_pub_;
@@ -82,7 +84,9 @@ class LiDARSpeedControlNode : public rclcpp::Node
     max_acc_(0.6),
     limit_factor_(3.0),
     min_distance_(0.5),
-    front_angle_in_degree_(60)
+    front_angle_in_degree_(60),
+    front_min_range_(0.25),
+    blind_spot_min_range_(0.25)
   {
     RCLCPP_INFO(get_logger(), "LiDARSpeedControlNodeClass Constructor");
     tfBuffer = new tf2_ros::Buffer(get_clock());
@@ -149,6 +153,8 @@ class LiDARSpeedControlNode : public rclcpp::Node
     limit_factor_ = declare_parameter("limit_factor", limit_factor_);
     min_distance_ = declare_parameter("min_distance", min_distance_);
     front_angle_in_degree_ = declare_parameter("front_angle_in_degree", front_angle_in_degree_);
+    front_min_range_ = declare_parameter("front_min_range", front_min_range_);
+    blind_spot_min_range_ = declare_parameter("blind_spot_min_range", blind_spot_min_range_);
 
     RCLCPP_INFO(
       get_logger(), "LiDARSpeedControl with check_blind_space=%s, check_front_obstacle=%s, max_speed=%.2f",
@@ -230,9 +236,12 @@ class LiDARSpeedControlNode : public rclcpp::Node
         curr.transform(map_to_robot_tf2 * robot_to_lidar_tf2);
 
         CaBotSafety::Line line(robotp, curr);
-        double dot = robot.dot(line) / robot.length() / line.length();
-        RCLCPP_DEBUG(get_logger(), "%.2f,%.2f,%.2f,%.2f,%.2f", line.s.x, line.s.y, line.e.x, line.e.y, dot);
+        if (line.length() < front_min_range_) {
+          continue;
+        }
 
+        double dot = robot.dot(line) / robot.length() / line.length();
+        RCLCPP_DEBUG(get_logger(), "%.2f,%.2f,%.2f,%.2f,%.2f,%.2f", line.s.x, line.s.y, line.e.x, line.e.y, dot, line.length());
         if (dot < cos(front_angle_in_degree_ / 180.0 * M_PI_2)) {
           continue;
         }
@@ -243,13 +252,19 @@ class LiDARSpeedControlNode : public rclcpp::Node
       }
       // calculate the speed
       speed_limit = std::min(max_speed_, std::max(min_speed_, (min_range - min_distance_) / limit_factor_));
+      if (speed_limit < max_speed_) {
+        RCLCPP_INFO(get_logger(), "limit by front obstacle (%.2f), min_range=%.2f", speed_limit, min_range);
+      }
     }
 
     if (check_blind_space_) {  // Check blind space
       Point prev;
       for (uint64_t i = 0; i < input->ranges.size(); i++) {
         double angle = input->angle_min + input->angle_increment * i;
         double range = input->ranges[i];
+        if (range < blind_spot_min_range_) {
+          continue;
+        }
         if (range == inf) {
           range = input->range_max;
         }
@@ -332,6 +347,9 @@ class LiDARSpeedControlNode : public rclcpp::Node
         // update speed limit
         if (limit < speed_limit) {
           speed_limit = limit;
+          if (speed_limit < max_speed_) {
+            RCLCPP_INFO(get_logger(), "limit by blind spot (%.2f), critical_distance=%.2f", speed_limit, critical_distance);
+          }
         }
       }
 

cabot/src/safety/people_speed_control_node.cpp

@@ -261,7 +261,7 @@ class PeopleSpeedControlNode : public rclcpp::Node
     geometry_msgs::msg::TransformStamped transform_msg;
     try {
       transform_msg = tfBuffer->lookupTransform(
-        "base_footprint", "map",
+        "base_footprint", input->header.frame_id,
         rclcpp::Time(0), rclcpp::Duration(std::chrono::duration<double>(1.0)));
     } catch (tf2::TransformException & ex) {
       RCLCPP_WARN(get_logger(), "%s", ex.what());
@@ -286,14 +286,54 @@ class PeopleSpeedControlNode : public rclcpp::Node
       double y = p_local.y();
       double vx = v_local.x();
       double vy = v_local.y();
-      double dist = sqrt(x * x + y * y);
+      double vr = last_odom_.twist.twist.linear.x;
+
+      // velocity obstacle
+      double RPy = atan2(y, x);
+      double dist = hypot(x, y);
+      if (vx > 0 || vy > 0) {
+        double pr = 1.0;
+        double s = atan2(pr, dist) + M_PI_2;
+        double Px1 = x + cos(RPy + s) * pr;
+        double Py1 = y + sin(RPy + s) * pr;
+        double Px2 = x + cos(RPy - s) * pr;
+        double Py2 = y + sin(RPy - s) * pr;
+        double v1 = vy / -(Py1 / Px1);
+        double t1 = -Py1 / vy;
+        double v2 = vy / -(Py2 / Px2);
+        double t2 = -Py2 / vy;
+        bool swapped = false;
+        if ((0 < t1 && t1 < 5) || (0 < t2 && t2 < 5)) {
+          v1 -= vx;
+          v2 -= vx;
+          if (t1 < t2) {
+            double temp = v1;
+            v1 = v2;
+            v2 = temp;
+            temp = t1;
+            t1 = t2;
+            t2 = temp;
+            swapped = true;
+          }
+          speed_limit = std::max(0.0, v1);
+          if (v1 < v2 && v2 < 1.0) {
+            speed_limit = max_speed_;
+          }
+          RCLCPP_INFO(
+            get_logger(),
+            "PeopleSpeedControl collision cone (%.2f, %.2f) (%.2f, %.2f) (%.2f, %.2f) (%.2f, %.2f) (%.2f, %.2f) (%.2f, %.2f)"
+            " - %.2f - %.2f, (%d), %.2f, %.2f",
+            x, y, vx, vy, Px1, Py1, Px2, Py2, t1, t2, v1, v2, vr, speed_limit, swapped, RPy, s);
+        }
+      }
 
-      double pt = atan2(y, x);
-      double sdx = abs(social_distance_x_ * cos(pt));
-      double sdy = abs(social_distance_y_ * sin(pt));
+      // social distance
+      double sdx = abs(social_distance_x_ * cos(RPy));
+      double sdy = abs(social_distance_y_ * sin(RPy));
       double min_path_dist = 100;
 
-      if (abs(pt) > M_PI_2) {
+      if (abs(RPy) > M_PI_2) {
+        RCLCPP_INFO(get_logger(), "PeopleSpeedControl person is back %.2f", RPy);
         continue;
       }
       auto max_v = [](double D, double A, double d)
@@ -322,9 +362,9 @@ class PeopleSpeedControlNode : public rclcpp::Node
       }
 
       RCLCPP_INFO(
-        get_logger(), "PeopleSpeedControl people_limit %s dist from path=%.2f x=%.2f y=%.2f vx=%.2f"
-        " vy=%.2f pt=%.2f sdx=%.2f sdy=%.2f dist=%.2f limit=%.2f",
-        it->name.c_str(), min_path_dist, x, y, vx, vy, pt, sdx, sdy, dist, speed_limit);
+        get_logger(), "PeopleSpeedControl people_limit %s, %.2f %.2f (%.2f %.2f) - %.2f (%.2f)",
+        it->name.c_str(), min_path_dist, dist, social_distance_x_, social_distance_y_, speed_limit,
+        max_v(std::max(0.0, dist - social_distance_y_), max_acc_, delay_));
 
       if (speed_limit < max_speed_) {
         std_msgs::msg::String msg;

cabot_navigation2/CMakeLists.txt

@@ -12,7 +12,7 @@ if(NOT CMAKE_CXX_STANDARD)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
+  add_compile_options(-Wall -Wextra -Wpedantic -Wfatal-errors)
 endif()
 
 # find dependencies
@@ -21,6 +21,7 @@ find_package(geometry_msgs REQUIRED)
 find_package(nav2_lifecycle_manager REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
 find_package(nav2_core REQUIRED)
+find_package(nav2_rotation_shim_controller REQUIRED)
 find_package(angles REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
@@ -69,8 +70,6 @@ set(dependencies
   people_msgs
   queue_msgs
   sensor_msgs
-  dwb_core
-  dwb_critics
   diagnostic_updater
   yaml_cpp_vendor
 #  OpenMP
@@ -86,7 +85,8 @@ add_library(cabot_critics SHARED
 target_compile_definitions(cabot_critics PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
 ament_target_dependencies(cabot_critics
-  ${dependencies}
+  dwb_core
+  dwb_critics
 )
 
 ### goal checker
@@ -98,7 +98,8 @@ add_library(cabot_goals SHARED
 target_compile_definitions(cabot_goals PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
 ament_target_dependencies(cabot_goals
-  ${dependencies}
+  nav2_core
+  pluginlib
 )
 
 ### costmap layers
@@ -111,7 +112,10 @@ add_library(cabot_layers SHARED
 target_compile_definitions(cabot_layers PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
 ament_target_dependencies(cabot_layers
-  ${dependencies}
+  nav2_costmap_2d
+  nav2_core
+  people_msgs
+  pluginlib
 )
 
 ### planner plugin
@@ -126,7 +130,13 @@ add_library(cabot_planners SHARED
 target_compile_definitions(cabot_planners PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
 ament_target_dependencies(cabot_planners
-  ${dependencies}
+  angles
+  nav2_core
+  people_msgs
+  pluginlib
+  queue_msgs
+  rclcpp
+  tf2
 )
 
 target_link_libraries(cabot_planners
@@ -140,22 +150,35 @@ add_executable(cabot_scan
 )
 
 ament_target_dependencies(cabot_scan
-  ${dependencies}
+  diagnostic_updater
+  rclcpp
+  sensor_msgs
 )
 
 ### cabot lifecycle manager
 add_executable(cabot_lifecycle_manager
   src/cabot_lifecycle_manager.cpp
 )
 ament_target_dependencies(cabot_lifecycle_manager
-  ${dependencies}
+  nav2_lifecycle_manager
+)
+
+### controller plugin
+add_library(cabot_controllers SHARED
+  plugins/cabot_rotation_shim_controller.cpp
+)
+ament_target_dependencies(cabot_controllers
+  nav2_core
+  nav2_rotation_shim_controller
+  pluginlib
 )
 
 install(TARGETS
   cabot_critics
   cabot_goals
   cabot_layers
   cabot_planners
+  cabot_controllers
   cabot_scan
   cabot_lifecycle_manager
   ARCHIVE DESTINATION lib
@@ -168,6 +191,7 @@ install(FILES
   goal_plugins.xml
   layers_plugins.xml
   planners_plugins.xml
+  controllers_plugins.xml
   DESTINATION share/${PROJECT_NAME}
 )
 
@@ -187,13 +211,16 @@ ament_export_libraries(
   cabot_critics
   cabot_goals
   cabot_layers
-  cabot_planners)
+  cabot_planners
+  cabot_controllers
+)
 ament_export_dependencies(${dependencies})
 
 pluginlib_export_plugin_description_file(dwb_core critics_plugins.xml)
 pluginlib_export_plugin_description_file(nav2_core goal_plugins.xml)
 pluginlib_export_plugin_description_file(nav2_costmap_2d layers_plugins.xml)
 pluginlib_export_plugin_description_file(nav2_core planners_plugins.xml)
+pluginlib_export_plugin_description_file(nav2_core controllers_plugins.xml)
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)

cabot_navigation2/controllers_plugins.xml

@@ -0,0 +1,30 @@
+<!--
+ Copyright (c) 2024  Carnegie Mellon University
+
+ Permission is hereby granted, free of charge, to any person obtaining a copy
+ of this software and associated documentation files (the "Software"), to deal
+ in the Software without restriction, including without limitation the rights
+ to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ copies of the Software, and to permit persons to whom the Software is
+ furnished to do so, subject to the following conditions:
+
+ The above copyright notice and this permission notice shall be included in all
+ copies or substantial portions of the Software.
+
+ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ SOFTWARE.
+-->
+
+<class_libraries>
+  <library path="cabot_controllers">
+    <class type="cabot_navigation2::CaBotRotationShimController"
+	   base_class_type="nav2_core::Controller">
+      <description>Custmozied Rotation Shim Controller</description>
+    </class>
+  </library>
+</class_libraries>

cabot_navigation2/include/cabot_navigation2/cabot_rotation_shim_controller.hpp

@@ -0,0 +1,41 @@
+// Copyright (c) 2024  Carnegie Mellon University
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#ifndef CABOT_NAVIGATION2__CABOT_ROTATION_SHIM_CONTROLLER_HPP_
+#define CABOT_NAVIGATION2__CABOT_ROTATION_SHIM_CONTROLLER_HPP_
+#include <nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp>
+
+namespace cabot_navigation2
+{
+class CaBotRotationShimController : public nav2_rotation_shim_controller::RotationShimController
+{
+public:
+  CaBotRotationShimController();
+  geometry_msgs::msg::TwistStamped computeVelocityCommands(
+    const geometry_msgs::msg::PoseStamped & pose,
+    const geometry_msgs::msg::Twist & velocity,
+    nav2_core::GoalChecker * goal_checker) override;
+
+protected:
+  geometry_msgs::msg::PoseStamped getNearestPathPt(const geometry_msgs::msg::PoseStamped & pose);
+  geometry_msgs::msg::Pose transformPoseToBaseFrame(const geometry_msgs::msg::PoseStamped & pt);
+};
+}  // namespace cabot_navigation2
+#endif  // CABOT_NAVIGATION2__CABOT_ROTATION_SHIM_CONTROLLER_HPP_

cabot_navigation2/params/nav2_params.yaml

@@ -184,7 +184,13 @@ controller_server:
       stateful: True
     # DWB parameters
     FollowPath:
-      plugin: "dwb_core::DWBLocalPlanner"
+      plugin: "cabot_navigation2::CaBotRotationShimController"
+      primary_controller: "dwb_core::DWBLocalPlanner"
+      angular_dist_threshold: 0.3
+      forward_sampling_distance: 1.0
+      rotate_to_heading_angular_vel: 0.5
+      max_angular_accel: 1.6
+      simulate_ahead_time: 0.5
       #trajectory_generator_name: "dwb_plugins::LimitedAccelGenerator" # default: dwb_plugins::StandardTrajectoryGenerator
       trajectory_generator_name: "dwb_plugins::StandardTrajectoryGenerator"
       debug_trajectory_details: True