Daisukes/adjust speed control

cabot/config/cabot-control.yaml

@@ -58,6 +58,9 @@ cabot:
       limit_topic: /cabot/people_speed
       odom_topic: /odom
       plan_topic: /plan
+      social_limit_topic: /cabot/social_speed
+      velocity_obstacle_limit_topic: /cabot/velocity_obstacle_speed
+      unconstrained_velocity_obstacle_limit_topic: /cabot/unconstrained_velocity_obstacle_speed
       # Social Distancing setting
       # X (front): may need to bigger in larger place
       # Y (side) : need to be small to avoid strange speed down when people are walking through very close

cabot/src/safety/people_speed_control_node.cpp

@@ -63,6 +63,9 @@ class PeopleSpeedControlNode : public rclcpp::Node
   std::string odom_topic_;
   std::string plan_topic_;
   std::string event_topic_;
+  std::string social_limit_topic_;
+  std::string velocity_obstacle_limit_topic_;
+  std::string unconstrained_velocity_obstacle_limit_topic_;
   std::string set_social_distance_topic_;
   std::string get_social_distance_topic_;
 
@@ -75,7 +78,11 @@ class PeopleSpeedControlNode : public rclcpp::Node
   double delay_;
   double social_distance_x_;
   double social_distance_y_;
+  double social_distance_min_radius_;
+  double social_distance_min_angle_;  // [rad]
+  double social_distance_max_angle_;  // [rad]
   double no_people_topic_max_speed_;
+  double velocity_obstacle_weight_;  // parameter to enable/disable velocity obstacle
   bool no_people_flag_;
 
   rclcpp::Subscription<people_msgs::msg::People>::SharedPtr people_sub_;
@@ -84,6 +91,9 @@ class PeopleSpeedControlNode : public rclcpp::Node
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr vis_pub_;
   rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr limit_pub_;
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr event_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr social_limit_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr velocity_obstacle_limit_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr unconstrained_velocity_obstacle_limit_pub_;
   rclcpp::Subscription<geometry_msgs::msg::Point>::SharedPtr set_social_distance_sub_;
   rclcpp::Publisher<geometry_msgs::msg::Point>::SharedPtr get_social_distance_pub_;
   std::mutex thread_sync_;
@@ -103,6 +113,9 @@ class PeopleSpeedControlNode : public rclcpp::Node
     odom_topic_("/odom"),
     plan_topic_("/plan"),
     event_topic_("/event"),
+    social_limit_topic_("/social_limit"),
+    velocity_obstacle_limit_topic_("/velocity_obstacle_limit"),
+    unconstrained_velocity_obstacle_limit_topic_("/unconstrained_velocity_obstacle_limit"),
     set_social_distance_topic_("/set_social_distance"),
     get_social_distance_topic_("/get_social_distance"),
     map_frame_("map"),
@@ -113,7 +126,11 @@ class PeopleSpeedControlNode : public rclcpp::Node
     delay_(0.5),
     social_distance_x_(2.0),
     social_distance_y_(1.0),
+    social_distance_min_radius_(0.45),
+    social_distance_min_angle_(-M_PI_2),
+    social_distance_max_angle_(M_PI_2),
     no_people_topic_max_speed_(0.5),
+    velocity_obstacle_weight_(1.0),
     no_people_flag_(false)
   {
     RCLCPP_INFO(get_logger(), "PeopleSpeedControlNodeClass Constructor");
@@ -147,12 +164,24 @@ class PeopleSpeedControlNode : public rclcpp::Node
     event_topic_ = declare_parameter("event_topic", event_topic_);
     event_pub_ = create_publisher<std_msgs::msg::String>(event_topic_, 100);
 
+    // publishers for intermediate speed limit values
+    social_limit_topic_ = declare_parameter("social_limit_topic", social_limit_topic_);
+    social_limit_pub_ = create_publisher<std_msgs::msg::Float32>(social_limit_topic_, rclcpp::SystemDefaultsQoS().transient_local());
+    velocity_obstacle_limit_topic_ = declare_parameter("velocity_obstacle_limit_topic", velocity_obstacle_limit_topic_);
+    velocity_obstacle_limit_pub_ = create_publisher<std_msgs::msg::Float32>(velocity_obstacle_limit_topic_, rclcpp::SystemDefaultsQoS().transient_local());
+    unconstrained_velocity_obstacle_limit_topic_ = declare_parameter("unconstrained_velocity_obstacle_limit_topic", unconstrained_velocity_obstacle_limit_topic_);
+    unconstrained_velocity_obstacle_limit_pub_ = create_publisher<std_msgs::msg::Float32>(unconstrained_velocity_obstacle_limit_topic_, rclcpp::SystemDefaultsQoS().transient_local());
+
     max_speed_ = declare_parameter("max_speed_", max_speed_);
     min_speed_ = declare_parameter("min_speed_", min_speed_);
     max_acc_ = declare_parameter("max_acc_", max_acc_);
     social_distance_x_ = declare_parameter("social_distance_x", social_distance_x_);
     social_distance_y_ = declare_parameter("social_distance_y", social_distance_y_);
+    social_distance_min_radius_ = declare_parameter("social_distance_min_radius", social_distance_min_radius_);
+    social_distance_min_angle_ = declare_parameter("social_distance_min_angle", social_distance_min_angle_);
+    social_distance_max_angle_ = declare_parameter("social_distance_max_angle", social_distance_max_angle_);
     no_people_topic_max_speed_ = declare_parameter("no_people_topic_max_speed", no_people_topic_max_speed_);
+    velocity_obstacle_weight_ = declare_parameter("velocity_obstacle_weight", velocity_obstacle_weight_);
 
     RCLCPP_INFO(
       get_logger(), "PeopleSpeedControl with max_speed=%.2f, social_distance=(%.2f, %.2f)",
@@ -253,6 +282,18 @@ class PeopleSpeedControlNode : public rclcpp::Node
       if (param.get_name() == "social_distance_y") {
         social_distance_y_ = param.as_double();
       }
+      if (param.get_name() == "social_distance_min_radius") {
+        social_distance_min_radius_ = param.as_double();
+      }
+      if (param.get_name() == "social_distance_min_angle") {
+        social_distance_min_angle_ = param.as_double();
+      }
+      if (param.get_name() == "social_distance_max_angle") {
+        social_distance_max_angle_ = param.as_double();
+      }
+      if (param.get_name() == "velocity_obstacle_weight") {
+        velocity_obstacle_weight_ = param.as_double();
+      }
     }
     results->successful = true;
     return *results;
@@ -307,16 +348,24 @@ class PeopleSpeedControlNode : public rclcpp::Node
 
       double x = p_local.x();
       double y = p_local.y();
+      // ignore people if it is in the minimum radius
+      if (std::hypot(x, y) <= social_distance_min_radius_) {
+        RCLCPP_INFO(get_logger(), "PeopleSpeedControl ignore %s, (%.2f %.2f)", person.name.c_str(), x, y);
+        continue;
+      }
+
       double vx = v_local.x();
 
       double RPy = atan2(y, x);
       double dist = hypot(x, y);
       double min_path_dist = 100;
 
-      if (abs(RPy) > M_PI_2) {
-        RCLCPP_INFO(get_logger(), "PeopleSpeedControl person is back %.2f", RPy);
+      if (RPy < social_distance_min_angle_ || social_distance_max_angle_ < RPy){
+        RCLCPP_INFO(get_logger(), "PeopleSpeedControl person %s (RPy=%.2f) is out of angle range [%.2f, %.2f]",
+                    person.name.c_str(), RPy, social_distance_min_angle_, social_distance_max_angle_);
         continue;
       }
+
       auto max_v = [](double D, double A, double d)
         {
           return (-2 * A * d + sqrt(4 * A * A * d * d + 8 * A * D)) / 2;
@@ -366,11 +415,17 @@ class PeopleSpeedControlNode : public rclcpp::Node
     // Velocity Obstacle
     std::vector<std::pair<double, double>> vo_intervals;
     for (size_t i = 0; i < input->people.size(); ++i) {
+      const auto & person = input->people[i];
       const auto & p_local = transformed_people[i].first;
       const auto & v_local = transformed_people[i].second;
 
       double x = p_local.x();
       double y = p_local.y();
+      // ignore people if it is in the minimum radius
+      if (std::hypot(x, y) <= social_distance_min_radius_) {
+        RCLCPP_INFO(get_logger(), "PeopleSpeedControl ignore %s, (%.2f %.2f)", person.name.c_str(), x, y);
+        continue;
+      }
       double vx = v_local.x();
       double vy = v_local.y();
 
@@ -414,13 +469,23 @@ class PeopleSpeedControlNode : public rclcpp::Node
       }
     }
 
-    double people_speed_limit = computeSafeSpeedLimit(social_speed_limit, vo_intervals);
+    // unnconstrained velocity obstacle speed
+    double unconstrained_vo_speed_limit = computeSafeSpeedLimit(max_speed_, vo_intervals);
+    // velocity obstacle speed constrained with social speed limit
+    double velocity_obstacle_speed_limit = computeSafeSpeedLimit(social_speed_limit, vo_intervals);
+    // weighted average
+    double people_speed_limit = (1.0 - velocity_obstacle_weight_) * social_speed_limit
+                                + velocity_obstacle_weight_ * velocity_obstacle_speed_limit;
 
     std_msgs::msg::Float32 msg;
     msg.data = people_speed_limit;
     // RCLCPP_INFO(get_logger(), "limit = %.2f", people_speed_limit);
     limit_pub_->publish(msg);
 
+    social_limit_pub_->publish(std_msgs::msg::Float32().set__data(social_speed_limit));
+    velocity_obstacle_limit_pub_->publish(std_msgs::msg::Float32().set__data(velocity_obstacle_speed_limit));
+    unconstrained_velocity_obstacle_limit_pub_->publish(std_msgs::msg::Float32().set__data(unconstrained_vo_speed_limit));
+
     if (logger_level <= RCUTILS_LOG_SEVERITY_DEBUG) {
       addSpeedLimitMarker(map_to_robot_tf2, people_speed_limit);
       CaBotSafety::commit(vis_pub_);

cabot_ui/cabot_ui/navigation.py

@@ -374,6 +374,7 @@ def __init__(self, node: Node, tf_node: Node, srv_node: Node, act_node: Node, so
 
         self._max_speed = node.declare_parameter("max_speed", 1.1).value
         self._max_acc = node.declare_parameter("max_acc", 0.3).value
+        self._speed_poi_params = node.declare_parameter("speed_poi_params", [0.5, 0.5, 0.5]).value
 
         self._global_map_name = node.declare_parameter("global_map_name", "map_global").value
         self.visualizer.global_map_name = self._global_map_name
@@ -893,7 +894,16 @@ def _check_nearby_facility(self, current_pose):
                 self._logger.info(F"_check_nearby_facility passed {entrance._id}")
                 self.delegate.passed_poi(poi=entrance)
 
+    """
+     robot              target     POI
+     o->                     |--D--o
+     The robot will decrease the speed towards the target and keep speed until passing the POI
+     THe robot velocity is calculated based on expected deceleration and delay
+    """
     def _check_speed_limit(self, current_pose):
+        target_distance = self._speed_poi_params[0]  # meters
+        expected_deceleration = self._speed_poi_params[1]  # meters/seconds^2
+        expected_delay = self._speed_poi_params[2]  # seconds
         def max_v(D, A, d):
             return (-2 * A * d + math.sqrt(4 * A * A * d * d + 8 * A * D)) / 2
 
@@ -903,7 +913,9 @@ def max_v(D, A, d):
             dist = poi.distance_to(current_pose, adjusted=True)  # distance adjusted by angle
             if dist < 5.0:
                 if poi.in_angle(current_pose):  # and poi.in_angle(c2p):
-                    limit = min(limit, max(poi.limit, max_v(max(0, dist-0.5), 0.5, 0.5)))
+                    limit = min(limit, max(poi.limit, max_v(max(0, dist-target_distance), expected_deceleration, expected_delay)))
+                    self._logger.debug(f"SpeedPOI {max_v(max(0, dist-target_distance), expected_deceleration, expected_delay)=}")
+                    self._logger.debug(f"SpeedPOI {dist=}, {target_distance=}, {expected_deceleration=}, {expected_delay=}, {limit=}, {poi.limit=}")
                 if limit < self._max_speed:
                     self._logger.debug(F"speed poi dist={dist:.2f}m, limit={limit:.2f}")
                     self.delegate.activity_log("cabot/navigation", "speed_poi", f"{limit}")

cabot_ui/launch/cabot_ui.launch.py

@@ -44,6 +44,7 @@ def generate_launch_description():
     plan_topic = LaunchConfiguration('plan_topic')
     show_topology = LaunchConfiguration('show_topology')
     announce_no_touch = LaunchConfiguration('announce_no_touch')
+    speed_poi_params = LaunchConfiguration('speed_poi_params')
 
     def hoge(text):
         return text
@@ -105,6 +106,11 @@ def hoge(text):
             default_value=EnvironmentVariable('CABOT_ANNOUNCE_NO_TOUCH', default_value='false'),
             description='True if you want to announce detection of no touch'
         ),
+        DeclareLaunchArgument(
+            'speed_poi_params',
+            default_value=EnvironmentVariable('CABOT_SPEED_POI_PARAMS', default_value='[0.5, 0.5, 0.5]'),
+            description='[target_distance, expected_deceleration, expected_delay]'
+        ),
         Node(
             package="cabot_ui",
             executable="cabot_ui_manager.py",
@@ -116,6 +122,7 @@ def hoge(text):
                 'global_map_name': global_map_name,
                 'plan_topic': plan_topic,
                 'menu_file': menu_file,
+                'speed_poi_params': speed_poi_params,
             }, NamespaceParameterFile('cabot_ui_manager', config_path)],
             ros_arguments=[
                 # '--log-level', 'cabot_ui_manager:=debug'

docker-compose-common.yaml

@@ -44,6 +44,7 @@ services:
       - CABOT_INITY
       - CABOT_INITZ
       - CABOT_INITA
+      - CABOT_SPEED_POI_PARAMS
 # ROS2/DDS
       - ROS_LOG_DIR
       - RMW_IMPLEMENTATION