Skip to content

Commit d00f501

Browse files
igonzfigonzf
committed
add detection by identity
1 parent 1302f95 commit d00f501

9 files changed

Lines changed: 270 additions & 6 deletions

File tree

‎bt_nodes/perception/CMakeLists.txt‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -115,6 +115,12 @@ list(APPEND plugin_libs switch_yolo_model_bt_node)
115115
add_library(extract_pc_from_class_bt_node SHARED src/perception/extract_pc_from_class.cpp)
116116
list(APPEND plugin_libs extract_pc_from_class_bt_node)
117117

118+
add_library(store_identity_vector_bt_node SHARED src/perception/store_identity_vector.cpp)
119+
list(APPEND plugin_libs store_identity_vector_bt_node)
120+
121+
add_library(enable_detect_by_identity_bt_node SHARED src/perception/enable_detect_by_identity.cpp)
122+
list(APPEND plugin_libs enable_detect_by_identity_bt_node)
123+
118124
foreach(bt_plugin ${plugin_libs})
119125
ament_target_dependencies(${bt_plugin} ${dependencies})
120126
target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)

‎bt_nodes/perception/include/perception/IsDetected.hpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,7 @@ class IsDetected : public BT::ConditionNode
6161
BT::InputPort<std::string>("gesture", "unknown", "gesture"),
6262
BT::InputPort<std::string>("pose", "unknown", "pose"),
6363
BT::InputPort<bool>("pub_bb_img"),
64+
BT::InputPort<std::string>("target_identity"),
6465

6566
BT::OutputPort<std::vector<std::string>>("frames"),
6667
BT::OutputPort<std::string>("best_detection")});
@@ -79,6 +80,7 @@ class IsDetected : public BT::ConditionNode
7980
std::string color_;
8081
std::string gesture_;
8182
std::string pose_;
83+
std::string target_identity_;
8284

8385
double hue_threshold_{20.0};
8486
double saturation_threshold_{50.0};

‎bt_nodes/perception/include/perception/enable_detect_by_identity.hpp‎

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
// Copyright 2024 Intelligent Robotics Lab - Gentlebots
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions andGO2OBJECT
13+
// limitations under the License.
14+
15+
#ifndef PERCEPTION__ENABLE_DETECT_BY_IDENTITY_HPP_
16+
#define PERCEPTION__ENABLE_DETECT_BY_IDENTITY_HPP_
17+
18+
#include <string>
19+
20+
#include "behaviortree_cpp_v3/behavior_tree.h"
21+
#include "behaviortree_cpp_v3/bt_factory.h"
22+
#include "perception/bt_service_node.hpp"
23+
#include "rclcpp/rclcpp.hpp"
24+
#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
25+
#include "std_srvs/srv/set_bool.hpp"
26+
27+
28+
namespace perception
29+
{
30+
31+
class EnableDetectByIdentity
32+
: public perception::BtServiceNode<std_srvs::srv::SetBool,
33+
rclcpp_cascade_lifecycle::CascadeLifecycleNode>
34+
{
35+
public:
36+
explicit EnableDetectByIdentity(
37+
const std::string & xml_tag_name, const std::string & action_name,
38+
const BT::NodeConfiguration & conf);
39+
40+
void on_tick() override;
41+
void on_result() override;
42+
43+
static BT::PortsList providedPorts()
44+
{
45+
return BT::PortsList(
46+
{
47+
BT::InputPort<bool>("enable", false, "Enable/disable detection")
48+
});
49+
}
50+
51+
private:
52+
bool enable_;
53+
};
54+
55+
} // namespace perception
56+
57+
#endif // PERCEPTION__ENABLE_DETECT_BY_IDENTITY_HPP_

‎bt_nodes/perception/include/perception/store_identity_vector.hpp‎

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
// Copyright 2024 Intelligent Robotics Lab - Gentlebots
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions andGO2OBJECT
13+
// limitations under the License.
14+
15+
#ifndef PERCEPTION__STORE_IDENTITY_VECTOR_HPP_
16+
#define PERCEPTION__STORE_IDENTITY_VECTOR_HPP_
17+
18+
#include <string>
19+
20+
#include "behaviortree_cpp_v3/behavior_tree.h"
21+
#include "behaviortree_cpp_v3/bt_factory.h"
22+
#include "perception/bt_service_node.hpp"
23+
#include "rclcpp/rclcpp.hpp"
24+
#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
25+
#include "yolo_msgs/srv/person_identity.hpp"
26+
27+
28+
namespace perception
29+
{
30+
31+
class StoreIdentityVector
32+
: public perception::BtServiceNode<yolo_msgs::srv::PersonIdentity,
33+
rclcpp_cascade_lifecycle::CascadeLifecycleNode>
34+
{
35+
public:
36+
explicit StoreIdentityVector(
37+
const std::string & xml_tag_name, const std::string & action_name,
38+
const BT::NodeConfiguration & conf);
39+
40+
void on_tick() override;
41+
void on_result() override;
42+
43+
static BT::PortsList providedPorts()
44+
{
45+
return BT::PortsList(
46+
{
47+
BT::InputPort<std::string>("identity", "unknown", "identity to store")
48+
});
49+
}
50+
51+
private:
52+
std::string identity_;
53+
};
54+
55+
} // namespace perception
56+
57+
#endif // PERCEPTION__STORE_IDENTITY_VECTOR_HPP_

‎bt_nodes/perception/package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@
2828
<depend>perception_system</depend>
2929
<depend>perception_system_interfaces</depend>
3030
<depend>sensor_msgs</depend>
31+
<depend>std_srvs</depend>
3132

3233
<test_depend>ament_lint_auto</test_depend>
3334
<test_depend>ament_lint_common</test_depend>

‎bt_nodes/perception/src/perception/IsDetected.cpp‎

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -90,6 +90,7 @@ BT::NodeStatus IsDetected::tick()
9090
getInput("color", color_);
9191
getInput("gesture", gesture_);
9292
getInput("pose", pose_);
93+
getInput("target_identity", target_identity_);
9394

9495
if (status() == BT::NodeStatus::IDLE) {
9596
RCLCPP_DEBUG(node_->get_logger(), "IsDetected idle");
@@ -102,7 +103,16 @@ BT::NodeStatus IsDetected::tick()
102103
pl::getInstance(node_)->update(35);
103104
rclcpp::spin_some(node_->get_node_base_interface());
104105

105-
auto detections = pl::getInstance(node_)->get_by_type(interest_);
106+
std::vector<perception_system_interfaces::msg::Detection> detections;
107+
108+
if (!target_identity_.empty() && target_identity_ != "unknown") {
109+
std::string perception_id = "person_" + target_identity_;
110+
detections = pl::getInstance(node_)->get_by_id(perception_id);
111+
} else {
112+
detections = pl::getInstance(node_)->get_by_type(interest_);
113+
}
114+
115+
//auto detections = pl::getInstance(node_)->get_by_type(interest_);
106116

107117
if (detections.empty()) {
108118
RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 2000, "[IsDetected] No detections");

‎bt_nodes/perception/src/perception/enable_detect_by_identity.cpp‎

Lines changed: 65 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,65 @@
1+
// Copyright 2024 Intelligent Robotics Lab - Gentlebots
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "perception/enable_detect_by_identity.hpp"
16+
17+
#include <string>
18+
19+
namespace perception
20+
{
21+
22+
using namespace std::chrono_literals;
23+
using namespace std::placeholders;
24+
25+
EnableDetectByIdentity::EnableDetectByIdentity(
26+
const std::string & xml_tag_name, const std::string & action_name,
27+
const BT::NodeConfiguration & conf)
28+
: perception::BtServiceNode<std_srvs::srv::SetBool,
29+
rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
30+
xml_tag_name, action_name, conf)
31+
{
32+
}
33+
34+
void EnableDetectByIdentity::on_tick()
35+
{
36+
RCLCPP_DEBUG(node_->get_logger(), "EnableDetectByIdentity ticked");
37+
38+
getInput("enable", enable_);
39+
request_->data = enable_;
40+
}
41+
42+
void EnableDetectByIdentity::on_result()
43+
{
44+
if (result_.success) {
45+
std::cout << "Success EnableDetectByIdentity" << std::endl;
46+
setStatus(BT::NodeStatus::SUCCESS);
47+
48+
} else {
49+
std::cout << "Failure EnableDetectByIdentity" << std::endl;
50+
setStatus(BT::NodeStatus::FAILURE);
51+
}
52+
}
53+
54+
} // namespace perception
55+
56+
#include "behaviortree_cpp_v3/bt_factory.h"
57+
BT_REGISTER_NODES(factory)
58+
{
59+
BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
60+
return std::make_unique<perception::EnableDetectByIdentity>(
61+
name, "/detect_person_by_identity", config);
62+
};
63+
64+
factory.registerBuilder<perception::EnableDetectByIdentity>("EnableDetectByIdentity", builder);
65+
}

‎bt_nodes/perception/src/perception/store_identity_vector.cpp‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
// Copyright 2024 Intelligent Robotics Lab - Gentlebots
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "perception/store_identity_vector.hpp"
16+
17+
#include <string>
18+
19+
namespace perception
20+
{
21+
22+
using namespace std::chrono_literals;
23+
using namespace std::placeholders;
24+
25+
StoreIdentityVector::StoreIdentityVector(
26+
const std::string & xml_tag_name, const std::string & action_name,
27+
const BT::NodeConfiguration & conf)
28+
: perception::BtServiceNode<yolo_msgs::srv::PersonIdentity,
29+
rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
30+
xml_tag_name, action_name, conf)
31+
{
32+
}
33+
34+
void StoreIdentityVector::on_tick()
35+
{
36+
RCLCPP_DEBUG(node_->get_logger(), "StoreIdentityVector ticked");
37+
38+
getInput("identity", identity_);
39+
40+
request_->identity = identity_;
41+
}
42+
43+
void StoreIdentityVector::on_result()
44+
{
45+
if (result_.success) {
46+
std::cout << "Success StoreIdentityVector" << std::endl;
47+
setStatus(BT::NodeStatus::SUCCESS);
48+
49+
} else {
50+
std::cout << "Failure StoreIdentityVector" << std::endl;
51+
setStatus(BT::NodeStatus::FAILURE);
52+
}
53+
}
54+
55+
} // namespace perception
56+
57+
#include "behaviortree_cpp_v3/bt_factory.h"
58+
BT_REGISTER_NODES(factory)
59+
{
60+
BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
61+
return std::make_unique<perception::StoreIdentityVector>(
62+
name, "/store_identity_vector", config);
63+
};
64+
65+
factory.registerBuilder<perception::StoreIdentityVector>("StoreIdentityVector", builder);
66+
}

‎robocup_bringup/launch/dialog.launch.py‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -126,11 +126,11 @@ def generate_launch_description():
126126
ld.add_action(declare_model_repo_cmd)
127127
ld.add_action(declare_model_filename_cmd)
128128
ld.add_action(declare_min_silence_ms_cmd)
129-
ld.add_action(whisper_cmd)
130-
ld.add_action(llava_cmd)
131-
ld.add_action(audio_common_tts_node)
132-
ld.add_action(audio_common_player_node)
133-
# ld.add_action(kb_cmd)
129+
# ld.add_action(whisper_cmd)
130+
# ld.add_action(llava_cmd)
131+
# ld.add_action(audio_common_tts_node)
132+
# ld.add_action(audio_common_player_node)
133+
ld.add_action(kb_cmd)
134134

135135
#ld.add_action(music_player_node)
136136

0 commit comments

Comments
 (0)