diff --git a/cabot/config/cabot-control.yaml b/cabot/config/cabot-control.yaml index 0eab813d..b065ee00 100644 --- a/cabot/config/cabot-control.yaml +++ b/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 diff --git a/cabot/src/safety/people_speed_control_node.cpp b/cabot/src/safety/people_speed_control_node.cpp index e9338c99..232dcb9b 100644 --- a/cabot/src/safety/people_speed_control_node.cpp +++ b/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::SharedPtr people_sub_; @@ -84,6 +91,9 @@ class PeopleSpeedControlNode : public rclcpp::Node rclcpp::Publisher::SharedPtr vis_pub_; rclcpp::Publisher::SharedPtr limit_pub_; rclcpp::Publisher::SharedPtr event_pub_; + rclcpp::Publisher::SharedPtr social_limit_pub_; + rclcpp::Publisher::SharedPtr velocity_obstacle_limit_pub_; + rclcpp::Publisher::SharedPtr unconstrained_velocity_obstacle_limit_pub_; rclcpp::Subscription::SharedPtr set_social_distance_sub_; rclcpp::Publisher::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(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(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(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(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> 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_); diff --git a/cabot_ui/cabot_ui/navigation.py b/cabot_ui/cabot_ui/navigation.py index 97d5554d..91752a9a 100644 --- a/cabot_ui/cabot_ui/navigation.py +++ b/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}") diff --git a/cabot_ui/launch/cabot_ui.launch.py b/cabot_ui/launch/cabot_ui.launch.py index e088eb2c..c6bc2721 100755 --- a/cabot_ui/launch/cabot_ui.launch.py +++ b/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' diff --git a/docker-compose-common.yaml b/docker-compose-common.yaml index 38c4d5e2..efab218c 100644 --- a/docker-compose-common.yaml +++ b/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