new launch files

hni_cpp/launch/chat_launch.py

@@ -13,37 +13,37 @@ def generate_launch_description():
             package='sound_play',
             executable='soundplay_node.py',
             name='soundplay_node',
-            output='screen'
+            output='screen',
         ),
         Node(
             package='hni_py',
             executable='gstt_service',
             name='gstt_service_node',
-            output='screen'
+            output='screen',
         ),
         Node(
             package='hni_py',
             executable='chat_service',
             name='chat_service_node',
-            output='screen'
+            output='screen',
         ),
         Node(
             package='hni_py',
             executable='gtts_service',
             name='gtts_service_node',
-            output='screen'
+            output='screen',
         ),
         Node(
             package='hni_cpp',
             executable='joints_play_action_server',
             name='joints_play_action_server',
-            output='screen'
+            output='screen',
         ),
         Node(
             package='nao_led_server',
             executable='led_action_server',
             name='led_action_server',
-            output='screen'
+            output='screen',
         ),
         #Node(
         #    package='hni_cpp',
@@ -55,6 +55,6 @@ def generate_launch_description():
             package='hni_cpp',
             executable='chat_action_client',
             name='chat_action_client',
-            output='screen'
+            output='screen',
         ),
     ])

hni_cpp/launch/experiment_pc_launch.py

@@ -4,8 +4,16 @@
 def generate_launch_description():
     return LaunchDescription([
         Node(
-            package='hni_py', executable='face_track_server', name='face_track_server_node',
+            package='hni_py', executable='yolo_video_track_server', name='yolo_face_track_server_node',
             output='screen',
+            remappings=[
+                ('video_obj_tracker/_action/feedback', 'video_face_tracker/_action/feedback'),
+                ('video_obj_tracker/_action/status', 'video_face_tracker/_action/status'),
+                ('video_obj_tracker/_action/cancel_goal', 'video_face_tracker/_action/cancel_goal'),
+                ('video_obj_tracker/_action/get_result', 'video_face_tracker/_action/get_result'),
+                ('video_obj_tracker/_action/send_goal', 'video_face_tracker/_action/send_goal'),
+            ],
+
         ),
         Node(
             package='hni_cpp', executable='chat_action_server', name='chat_action_server',

hni_cpp/launch/head_track_launch.py

@@ -10,12 +10,44 @@ def generate_launch_description():
         ),
         Node(
             package='hni_cpp',
-            executable='head_track_action_server',
-            name='head_track_action_server_node'
+            executable='head_track_action_client',
+            name='head_track_action_client_node'
         ),
         Node(
             package='hni_cpp',
-            executable='head_track_action_client',
-            name='head_track_action_client_node'
-        )
+            executable='head_track_action_server2',
+            name='head_track_action_server_node',
+            remappings=[
+                ('video_obj_tracker/_action/feedback', 'video_face_tracker/_action/feedback'),
+                ('video_obj_tracker/_action/status', 'video_face_tracker/_action/status'),
+                ('video_obj_tracker/_action/cancel_goal', 'video_face_tracker/_action/cancel_goal'),
+                ('video_obj_tracker/_action/get_result', 'video_face_tracker/_action/get_result'),
+                ('video_obj_tracker/_action/send_goal', 'video_face_tracker/_action/send_goal'),
+            ],
+        ),
+        Node(
+            package='nao_pos_server',
+            executable='nao_pos_action_client',
+            name='nao_pos_action_client_head',
+            remappings=[
+                ('action_req', 'action_req_head'),
+                ('nao_pos_action/_action/feedback', 'nao_pos_action_head/_action/feedback'),
+                ('nao_pos_action/_action/status', 'nao_pos_action_head/_action/status'),
+                ('nao_pos_action/_action/cancel_goal', 'nao_pos_action_head/_action/cancel_goal'),
+                ('nao_pos_action/_action/get_result', 'nao_pos_action_head/_action/get_result'),
+                ('nao_pos_action/_action/send_goal', 'nao_pos_action_head/_action/send_goal'),
+            ],
+        ),
+        Node(
+            package='nao_pos_server',
+            executable='nao_pos_action_server',
+            name='nao_pos_action_server_head',
+            remappings=[
+                ('nao_pos_action/_action/feedback', 'nao_pos_action_head/_action/feedback'),
+                ('nao_pos_action/_action/status', 'nao_pos_action_head/_action/status'),
+                ('nao_pos_action/_action/cancel_goal', 'nao_pos_action_head/_action/cancel_goal'),
+                ('nao_pos_action/_action/get_result', 'nao_pos_action_head/_action/get_result'),
+                ('nao_pos_action/_action/send_goal', 'nao_pos_action_head/_action/send_goal'),
+            ],
+        ),
     ])

hni_cpp/src/head_track_action_server2.cpp

@@ -50,12 +50,12 @@ HeadTrackActionServer2::HeadTrackActionServer2(const rclcpp::NodeOptions & optio
   using namespace std::placeholders;
 
   this->client_ptr_ =
-    rclcpp_action::create_client<hni_interfaces::action::VideoTracker>(this, "face_tracker");
+    rclcpp_action::create_client<hni_interfaces::action::VideoTracker>(this, "video_obj_tracker");
 
   this->timer_ = this->create_wall_timer(
     std::chrono::milliseconds(500), std::bind(&HeadTrackActionServer2::sendGoal, this));
 
-  this->publisher_ = this->create_publisher<std_msgs::msg::String>("action_req2", 10);
+  this->publisher_ = this->create_publisher<std_msgs::msg::String>("action_req_head", 10);
 
   this->action_server_ = rclcpp_action::create_server<HeadTrack>(
     this, "head_track", std::bind(&HeadTrackActionServer2::handleGoal, this, _1, _2),

hni_py/hni_py/face_track_client.py → hni_py/hni_py/video_track_client.py

@@ -25,11 +25,11 @@
 from rclpy.node import Node
 
 
-class FaceTrackClient(Node):
+class VideoTrackingClient(Node):
 
     def __init__(self):
-        super().__init__('face_tracker_client_node')
-        self._action_client = ActionClient(self, VideoTracker, 'face_tracker')
+        super().__init__('video_tracking_client_node')
+        self._action_client = ActionClient(self, VideoTracker, 'obj_tracker')
 
     def goal_response_callback(self, future):
         goal_handle = future.result()
@@ -75,7 +75,7 @@ def send_goal(self):
 def main(args=None):
     rclpy.init(args=args)
 
-    action_client = FaceTrackClient()
+    action_client = VideoTrackingClient()
 
     action_client.send_goal()
 

hni_py/hni_py/face_track_server.py → hni_py/hni_py/yolo_video_track_server.py

@@ -29,6 +29,7 @@
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
 from rclpy.timer import Rate,Timer
+from rcl_interfaces.msg import ParameterDescriptor
 
 from collections import defaultdict
 
@@ -42,12 +43,12 @@
 class VideoTrackerOb(Node):
 
     def __init__(self):
-        super().__init__('video_tracker_server_node')
+        super().__init__('yolo_video_tracker_server_node')
 
         self._action_server = ActionServer(
             self,
             VideoTracker,
-            'face_tracker',
+            'video_obj_tracker',
             execute_callback = self.execute_callback,
             callback_group = ReentrantCallbackGroup(),
             goal_callback = self.goal_callback,
@@ -67,7 +68,14 @@ def __init__(self):
         self.br = CvBridge()
 
         # Load the YOLOv8 model
-        self.model = YOLO('src/hni/hni_py/hni_py/models/yolov8n-face.pt')
+
+        parameter_descriptor = ParameterDescriptor(description='This parameter sets the Yolo tracking model you want to use')
+
+        self.declare_parameter('yolo_track_model', 'src/hni/hni_py/hni_py/models/yolov8n-face.pt', parameter_descriptor)
+
+        model_param = self.get_parameter('yolo_track_model').get_parameter_value().string_value
+
+        self.model = YOLO(model_param)
 
         # Store the track history
         self.track_history = defaultdict(lambda: [])
@@ -140,14 +148,16 @@ async def execute_callback(self, goal_handle):
                     results = self.model.track(self.frame, persist=True, tracker="bytetrack.yaml")
 
                     # Get the boxes and track IDs
-                    boxes = results[0].boxes.xywh.cuda()
-
+                    #boxes = results[0].boxes.xywh.cuda()
+                    boxes = results[0].boxes.xywh.cpu()
+
                     #boxes = results[0].boxes.xywh.cuda()
                     #track_ids = results[0].boxes.id.int().cuda().tolist()
 
 
                     try:
-                        track_ids = results[0].boxes.id.int().cuda().tolist()
+                        #track_ids = results[0].boxes.id.int().cuda().tolist()
+                        track_ids = results[0].boxes.id.int().cpu().tolist()
 
                         # Plot the tracks
                         for box, track_id in zip(boxes, track_ids):
@@ -174,7 +184,7 @@ async def execute_callback(self, goal_handle):
 
 
                     except AttributeError:
-                        self.get_logger().warning('no face detected')
+                        self.get_logger().warning('no obj detected')
                         center.x = -1.0
                         center.y = -1.0
                         feedback_msg.center = center

hni_py/setup.py

@@ -22,70 +22,14 @@
         'console_scripts': [
             'chat_client = hni_py.chat_client:main',
             'chat_service = hni_py.chat_service:main',
-            'face_track_client = hni_py.face_track_client:main',
-            'face_track_server = hni_py.face_track_server:main',
             'g2stt_service = hni_py.g2stt_service:main',
             'gstt_client = hni_py.gstt_client:main',
             'gstt_service = hni_py.gstt_service:main',
             'gtts_client = hni_py.gtts_client:main',
             'gtts_service = hni_py.gtts_service:main',
+            'video_track_client = hni_py.video_track_client:main',
+            'yolo_video_track_server = hni_py.yolo_video_track_server:main',
         ],
     },
 
 )
-
-"""
-
-import os
-from glob import glob
-from setuptools import setup
-
-package_name = 'my_package'
-
-setup(
-    name=package_name,
-    version='1.0.0',
-    # Packages to export
-    packages=[package_name],
-    # Files we want to install, specifically launch files
-    data_files=[
-        # Install marker file in the package index
-        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
-        # Include our package.xml file
-        (os.path.join('share', package_name), ['package.xml']),
-        # Include all launch files.
-        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
-    ],
-    # This is important as well
-    install_requires=['setuptools'],
-    zip_safe=True,
-    author='ROS 2 Developer',
-    author_email='ros2@ros.com',
-    maintainer='ROS 2 Developer',
-    maintainer_email='ros2@ros.com',
-    keywords=['foo', 'bar'],
-    classifiers=[
-        'Intended Audience :: Developers',
-        'License :: TODO',
-        'Programming Language :: Python',
-        'Topic :: Software Development',
-    ],
-    description='My awesome package.',
-    license='TODO',
-    # Like the CMakeLists add_executable macro, you can add your python
-    # scripts here.
-    entry_points={
-        'console_scripts': [
-            'chat_client = hni_py.chat_client:main',
-            'chat_service = hni_py.chat_service:main',
-            'face_track_client = hni_py.face_track_client:main',
-            'face_track_server = hni_py.face_track_server:main',
-            'g2stt_service = hni_py.g2stt_service:main',
-            'gstt_client = hni_py.gstt_client:main',
-            'gstt_service = hni_py.gstt_service:main',
-            'gtts_client = hni_py.gtts_client:main',
-            'gtts_service = hni_py.gtts_service:main',
-        ],
-    },
-)
-"""