diff --git a/bt_nodes/perception/include/perception/count_people.hpp b/bt_nodes/perception/include/perception/count_people.hpp index 8622f33..05ea087 100644 --- 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("confidence"), BT::InputPort("color"), BT::InputPort("pose"), + BT::InputPort("gesture"), BT::InputPort("input_num_person"), BT::OutputPort>("frames"), @@ -74,11 +75,13 @@ class CountPeople : public BT::ActionNodeBase int max_entities_; std::vector frames_; std::string color_; + std::string gesture_; std::map colors_; std::map> gestures_; + std::map 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 index 2c9ff16..a37693f 100644 --- 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 index ed6bccb..3d15f32 100644 --- 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 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 index 1dfed0c..68a27ed 100644 --- a/robocup_bringup/config/gpsr/gpsr.yaml +++ b/robocup_bringup/config/gpsr/gpsr.yaml @@ -39,7 +39,8 @@ behaviors_main: "bathroom", "entrance", "living_room", - "bedroom" + "bedroom", + "instruction_point" ] waypoints: kitchen: [3.79, 6.77, 0.12] @@ -47,6 +48,7 @@ behaviors_main: 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