Merge branch 'humble-devel' of https://github.com/CoreSenseEU/CoreSense4Home into humble-devel

Author: igonzf

bt_nodes/configuration/src/configuration/init_receptionist.cpp

@@ -67,7 +67,7 @@ BT::NodeStatus InitReceptionist::tick()
       std::to_string(wp_names_.size()).c_str());
 
     for (auto wp : wp_names_) {
-      // node_->declare_parameter("waypoints." + wp, std::vector<double>());
+      node_->declare_parameter("waypoints." + wp, std::vector<double>());
       std::vector<double> wp_pos;
       node_->get_parameter("waypoints." + wp, wp_pos);
       RCLCPP_INFO(

bt_nodes/perception/src/perception/IsDetected.cpp

@@ -208,14 +208,13 @@ BT::NodeStatus IsDetected::tick()
       }
 
       if (!removed) {
-        frames_.push_back(detection.class_name + "_" + std::to_string(entity_counter));
-
-        if (
-          pl::getInstance(node_)->publicTF(
-            detection, std::to_string(entity_counter)) == -1)
-        {
-          return BT::NodeStatus::FAILURE;
-        }
+        frames_.push_back(detection.unique_id);
+        // if (
+        //   pl::getInstance(node_)->publicTF(
+        //     detection) == -1)
+        // {
+        //   return BT::NodeStatus::FAILURE;
+        // }
         entity_counter++;
         ++it;
       }
@@ -234,7 +233,7 @@ BT::NodeStatus IsDetected::tick()
   }
 
 
-  setOutput("best_detection", detections[0].class_name);
+  setOutput("best_detection", detections[0].unique_id);
 
   if (pub_bb_img_) {
     cv::Point center2d(

robocup_bringup/CMakeLists.txt

@@ -43,7 +43,7 @@ ament_target_dependencies(behaviors_main ${dependencies})
 add_executable(real_time_main src/real_time_main.cpp)
 ament_target_dependencies(real_time_main ${dependencies})
 
-install(DIRECTORY bt_xml launch config maps
+install(DIRECTORY bt_xml launch config maps models
   DESTINATION share/${PROJECT_NAME}
 )
 

robocup_bringup/config/real_time_params.yaml

@@ -7,8 +7,8 @@ attention_server:
     period: 0.05
     max_yaw: 1.3
     max_pitch: 0.3
-    max_vel_yaw: 1.75 
-    max_vel_pitch: 1.75
+    max_vel_yaw: 0.5 
+    max_vel_pitch: 0.5
     rotation_threshold: 0.436332 # 25 degrees
 
 perception_people_detection:

robocup_bringup/config/receptionist/tiago_nav_params.yaml

@@ -37,7 +37,7 @@ amcl:
     z_max: 0.05
     z_rand: 0.5
     z_short: 0.05
-    scan_topic: /scan_raw
+    scan_topic: /scan_filtered
     set_initial_pose: true
     initial_pose:
       x: 2.0
@@ -235,7 +235,7 @@ local_costmap:
         mark_threshold: 0
         observation_sources: scan
         scan:
-          topic: /scan_raw
+          topic: /scan_filtered
           max_obstacle_height: 2.0
           clearing: True
           marking: True
@@ -256,7 +256,7 @@ local_costmap:
         enabled: True
         observation_sources: scan
         scan:
-          topic: /scan_raw
+          topic: /scan_filtered
           max_obstacle_height: 2.0
           clearing: True
           marking: True
@@ -284,7 +284,7 @@ global_costmap:
         enabled: True
         observation_sources: scan
         scan:
-          topic: /scan_raw
+          topic: /scan_filtered
           max_obstacle_height: 2.0
           clearing: True
           marking: True
@@ -444,7 +444,7 @@ slam_toolbox:
     resolution: 0.05
     scan_buffer_maximum_scan_distance: 10.0
     scan_buffer_size: 10
-    scan_topic: /scan_raw
+    scan_topic: /scan_filtered
     solver_plugin: solver_plugins::CeresSolver
     stack_size_to_use: 40000000
     tf_buffer_duration: 30.0

robocup_bringup/launch/receptionist_dependencies.launch.py

@@ -18,6 +18,7 @@
 from launch import LaunchDescription
 from launch.actions import IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
 # from launch.actions import LogInfo, RegisterEventHandler
 # from launch.event_handlers import OnExecutionComplete
 # import lifecycle_msgs
@@ -32,6 +33,7 @@ def generate_launch_description():
     yolo3d_dir = get_package_share_directory('yolo_bringup')
     navigation_dir = get_package_share_directory('navigation_system')
     knowledge_core_dir = get_package_share_directory('knowledge_core')
+    person_tracker_dir = get_package_share_directory('cs4home_person_tracker')
 
     # manipulation launchers
     move_group = IncludeLaunchDescription(
@@ -59,8 +61,8 @@ def generate_launch_description():
         ),
         launch_arguments={
             # 'namespace': 'perception_system',
-            'use_3d': 'True',
-            'model': 'yolov8n.pt',
+            # 'use_3d': 'True',
+            'model': 'yolo11n.pt',
             'input_image_topic': '/head_front_camera/rgb/image_raw',
             'input_depth_topic': '/head_front_camera/depth/image_raw',
             'input_depth_info_topic': '/head_front_camera/rgb/camera_info',
@@ -83,7 +85,7 @@ def generate_launch_description():
         launch_arguments={
             'rviz': 'True',
             # 'map': package_dir + '/maps/robocup_arena_1.yaml', # ARENA C
-            'map': package_dir + '/maps/apartamento_leon_gimp_con_mesa_tv.yaml', # ARENA B
+            'map': package_dir + '/maps/ir_lab.yaml', # ARENA B
             'params_file': package_dir +
                     '/config/receptionist/tiago_nav_params.yaml',
             'slam_params_file': package_dir +
@@ -98,11 +100,31 @@ def generate_launch_description():
         )
     )
 
+    laser_people_detector = Node(
+        package='upo_laser_people_detector',
+        executable='lasermodelnode',
+        output='screen',
+        parameters=[
+            {'model_file': os.path.join(package_dir, 'models', 'LFE-PPN.onnx')},
+            {'laser_topic': '/scan'}
+        ]
+    )
+
+
+    person_tracker = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(person_tracker_dir, 'launch', 'person_tracker.launch.py')
+        )
+    )
+
     ld = LaunchDescription()
     ld.add_action(navigation)
     ld.add_action(dialog)
     ld.add_action(yolo3d)
     ld.add_action(real_time)
+    ld.add_action(knowledge_core)
+    ld.add_action(laser_people_detector)
+    ld.add_action(person_tracker)
     # ld.add_action(move_group)
     ld.add_action(manipulation_server)
 

robocup_bringup/models/LFE-PPN.onnx

@@ -18,6 +18,8 @@
   <exec_depend>hri</exec_depend>
   <exec_depend>motion</exec_depend>
   <exec_depend>perception</exec_depend>
+  <exec_depend>upo_laser_people_detector</exec_depend>
+  <exec_depend>cs4home_person_tracker</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

robocup_bringup/package.xml

@@ -71,7 +71,15 @@ repositories:
     type: git
     url: https://github.com/igonzf/GPSR_planning
     version: main
-
-    
-
-    
+  ThirdParty/upo_laser_people_detector:
+    type: git
+    url: https://github.com/robotics-upo/upo_laser_people_detector.git
+    version: ros2
+  ThirdParty/cs4home_person_tracker:
+    type: git
+    url: https://github.com/CoreSenseEU/cs4home_person_tracker_module.git
+    version: main
+  ThirdParty/state_observers:
+    type: git
+    url: https://github.com/JRL-CARI-CNR-UNIBS/state_observers.git
+    version: master