Count people by pose and posture

igonzf · Jul 10, 2024 · 50d665c · 50d665c
1 parent 1ac4ad8
commit 50d665c
Show file tree

Hide file tree

Showing 4 changed files with 49 additions and 21 deletions.
diff --git a/bt_nodes/perception/include/perception/count_people.hpp b/bt_nodes/perception/include/perception/count_people.hpp
@@ -57,6 +57,7 @@ class CountPeople : public BT::ActionNodeBase
         BT::InputPort<float>("confidence"),
         BT::InputPort<std::string>("color"),
         BT::InputPort<std::string>("pose"),
+        BT::InputPort<std::string>("gesture"),
         BT::InputPort<int>("input_num_person"),
 
         BT::OutputPort<std::vector<std::string>>("frames"),
@@ -74,11 +75,13 @@ class CountPeople : public BT::ActionNodeBase
   int max_entities_;
   std::vector<std::string> frames_;
   std::string color_;
+  std::string gesture_;
 
   std::map<std::string, cv::Scalar> colors_;
   std::map<std::string, std::vector<int>> gestures_;
+  std::map<int, std::string> pose_names_;
 
-  std::string none_value_ = "none";
+  std::string none_value_ = "unknown";
 };
 
 }  // namespace perception

diff --git a/bt_nodes/perception/src/perception/count_people.cpp b/bt_nodes/perception/src/perception/count_people.cpp
@@ -32,22 +32,31 @@ CountPeople::CountPeople(const std::string & xml_tag_name, const BT::NodeConfigu
 : BT::ActionNodeBase(xml_tag_name, conf),
   max_entities_(10),
   colors_({
-    {"lower_blue", cv::Scalar(90,50,50)}, {"upper_blue", cv::Scalar(125,255,255)},
+     {"lower_blue", cv::Scalar(90,50,50)}, {"upper_blue", cv::Scalar(125,255,255)},
     {"lower_yellow", cv::Scalar(25,150,200)}, {"upper_yellow", cv::Scalar(30,255,255)},
     {"lower_black", cv::Scalar(0,0,0)}, {"upper_black", cv::Scalar(255,255,50)},
     {"lower_white", cv::Scalar(0,0,200)}, {"upper_white", cv::Scalar(255,30,255)},
     {"lower_red", cv::Scalar(170,150,180)}, {"upper_red", cv::Scalar(205,255,255)},
     {"lower_orange", cv::Scalar(10,120,120)}, {"upper_orange", cv::Scalar(20,255,255)},
-    {"lower_gray", cv::Scalar(0,0,100)}, {"upper_gray", cv::Scalar(180,30,200)}
+    {"lower_gray", cv::Scalar(0,0,100)}, {"upper_gray", cv::Scalar(180,30,200)}}),
+  gestures_({
+      {"pointing_right", {0, 1}},
+      {"pointing_left", {3, 4}},
+
+      {"waving", {5, 6, 7}},
+      {"rising_left", {6}},
+      {"rising_left", {6}},
+    }),
+  pose_names_({
+    {0, "lying"},
+    {1, "sitting"},
+    {2, "standing"},
+    {-1, "unknown"},
   })
 {
   config().blackboard->get("node", node_);
   config().blackboard->get("tf_buffer", tf_buffer_);
   config().blackboard->get("tf_listener", tf_listener_);
-
-  gestures_["waving"] = {5, 6, 7};
-  gestures_["raising"] = {6};
-  gestures_["pointing"] = {0, 1, 3, 4};
 }
 
 BT::NodeStatus CountPeople::tick()
@@ -59,6 +68,7 @@ BT::NodeStatus CountPeople::tick()
 
   getInput("color", color_);
   getInput("pose", pose_);
+  getInput("gesture", gesture_);
   getInput("max_entities", max_entities_);
   getInput("confidence", threshold_);
   getInput("cam_frame", cam_frame_);
@@ -116,12 +126,23 @@ BT::NodeStatus CountPeople::tick()
         }
       }
 
-      // Pose filtering
-      if (pose_ != "none" && !removed) {
-        if (std::find(gestures_[pose_].begin(), gestures_[pose_].end(), detection.pointing_direction) != gestures_[pose_].end()) {
-          RCLCPP_INFO(node_->get_logger(), "[CountPeople] Detection %s is %s", detection.unique_id.c_str(), pose_.c_str());
+      // gesture filtering
+      if (gesture_ != "unknown" && !removed) {
+        if (std::find(gestures_[gesture_].begin(), gestures_[gesture_].end(), detection.pointing_direction) != gestures_[gesture_].end()) {
+          RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Detection %s is %s", detection.unique_id.c_str(), gesture_.c_str());
+        } else {
+          RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Detection %s is not %s", detection.unique_id.c_str(), gesture_.c_str());
+          it = detections.erase(it);
+          removed = true;
+        }
+      }
+
+      // pose filtering
+      if (pose_ != "unknown" && !removed) {
+        if (pose_names_[detection.body_pose] == pose_) {
+          RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Detection %s is %s", detection.unique_id.c_str(), pose_.c_str());
         } else {
-          RCLCPP_INFO(node_->get_logger(), "[CountPeople] Detection %s is not %s", detection.unique_id.c_str(), pose_.c_str());
+          RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Detection %s is not %s", detection.unique_id.c_str(), pose_.c_str());
           it = detections.erase(it);
           removed = true;
         }

diff --git a/bt_nodes/perception/src/perception/filter_prev_detections.cpp b/bt_nodes/perception/src/perception/filter_prev_detections.cpp
@@ -43,8 +43,7 @@ inline double distance(const geometry_msgs::msg::TransformStamped & t1, const ge
 {
   return std::sqrt(
     std::pow(t1.transform.translation.x - t2.transform.translation.x, 2) +
-    std::pow(t1.transform.translation.y - t2.transform.translation.y, 2) +
-    std::pow(t1.transform.translation.z - t2.transform.translation.z, 2));
+    std::pow(t1.transform.translation.y - t2.transform.translation.y, 2));
 }
 
 BT::NodeStatus FilterPrevDetections::tick()
@@ -54,9 +53,6 @@ BT::NodeStatus FilterPrevDetections::tick()
     getInput("margin", margin_);
     getInput("frame_id", frame_id_);
 
-    RCLCPP_INFO(node_->get_logger(), "[FilterPrevDetections] %lu detections", prev_detections_->items.size());
-    RCLCPP_INFO(node_->get_logger(), "[FilterPrevDetections] %lu new detections", new_detections_.size());
-
     std::list<geometry_msgs::msg::TransformStamped> new_transforms;
 
     for (auto & detection : new_detections_) {
@@ -71,8 +67,6 @@ BT::NodeStatus FilterPrevDetections::tick()
         }
     }
 
-    RCLCPP_INFO(node_->get_logger(), "[FilterPrevDetections] %lu new transforms", new_transforms.size());
-
     for (auto & new_transform : new_transforms) {
         bool close = false;
 
@@ -83,12 +77,18 @@ BT::NodeStatus FilterPrevDetections::tick()
                     prev_transform.child_frame_id.c_str());
                 close = true;
                 break;
+            // } else if (new_transform.transform.translation.x < 0.50 && new_transform.transform.translation.y < 0.50
+            //          && new_transform.transform.translation.x > 8.0 && new_transform.transform.translation.y > 7.23
+            // ) {
+            //     RCLCPP_INFO(
+            //         node_->get_logger(), "Detection %s is out of the apartment, removing it", new_transform.child_frame_id.c_str());
+            //     close = true;
+            //     break;
             }
         }
 
         if (!close) {
             RCLCPP_INFO(node_->get_logger(), "Detection %s is not close to any previous detection", new_transform.child_frame_id.c_str());
-            RCLCPP_INFO(node_->get_logger(), "%f %f %f", new_transform.transform.translation.x, new_transform.transform.translation.y, new_transform.transform.translation.z);
 
             auto size = prev_detections_->items.size();
             std::string detection_type = new_transform.child_frame_id.substr(0, new_transform.child_frame_id.find("_"));

diff --git a/robocup_bringup/config/gpsr/gpsr.yaml b/robocup_bringup/config/gpsr/gpsr.yaml
@@ -39,14 +39,16 @@ behaviors_main:
         "bathroom",
         "entrance",
         "living_room",
-        "bedroom"
+        "bedroom",
+        "instruction_point"
       ]
     waypoints:
       kitchen: [3.79, 6.77, 0.12]
       bathroom: [4.89, 1.64, 1.0]
       entrance: [1.25, 6.30, 0.67]
       living_room: [1.55, 0.03, 0.72]
       bedroom: [7.50, 4.89, 0.65]
+      instruction_point: [1.527, 1.360, 0.06, 0.096]
     plugins:
       - dialogConfirmation_bt_node
       - listen_bt_node
@@ -58,3 +60,5 @@ behaviors_main:
       - setup_gpsr_bt_node
       - set_start_position_bt_node
       - move_to_bt_node
+      - start_music_bt_node
+      - stop_music_bt_node