Standardized settings, add video borders and new video qos

Author: hilary-luo

turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py

@@ -30,11 +30,15 @@
 from irobot_create_msgs.msg import LightringLeds
 from turtlebot4_vision_tutorials.MovenetDepthaiEdge import MovenetDepthai
 
+
+# Settings
 SCORE_THRESH = 0.4
 SMART_CROP = True
-FRAME_HEIGHT = 432
+FRAME_HEIGHT = 432 # 1080 / 2.5
+PUBLISH_PERIOD = 0.0833  # seconds; 12 fps
+
 
-# keys:
+# Semaphore Alphabet Keys:
 #   (right, left) where each value is the angle with the horizontal
 #   quantized into 8 regions
 #   Human facing camera
@@ -172,8 +176,7 @@ def __init__(self):
             self.handle_stop_camera
         )
 
-        timer_period = 0.0833  # seconds
-        self.timer = self.create_timer(timer_period, self.pose_detect)
+        self.timer = self.create_timer(PUBLISH_PERIOD, self.pose_detect)
 
         self.pose = MovenetDepthai(input_src='rgb',
                                    model='thunder',

turtlebot4_vision_tutorials_cpp/launch/pose_display.launch.py

@@ -17,8 +17,6 @@
 # @author Hilary Luo (hluo@clearpathrobotics.com)
 
 from launch import LaunchDescription
-from launch.actions.declare_launch_argument import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 
 def generate_launch_description():

turtlebot4_vision_tutorials_cpp/src/pose_display.cpp

@@ -27,16 +27,18 @@
 #include "sensor_msgs/msg/battery_state.hpp"
 #include "sensor_msgs/msg/image.hpp"
 
+// Constants
 #define IMAGE_HEIGHT 432
 #define IMAGE_WIDTH 768
 #define TILE_X 2
 #define TILE_Y 3
-
-#define SCORE_THRESH 0.4
 #define LINE_NUM 16
 
 #define NODE_NAME "pose_display"
 #define WINDOW_NAME "Clearpath Turtlebot 4 Demo"
+
+// Settings
+#define SCORE_THRESH 0.4
 #define DISPLAY_PERIOD 83 //ms; 12 fps
 
 const int max_num_streams = TILE_X * TILE_Y;
@@ -57,6 +59,7 @@ void imageCallback(uint num, std::string ns, const sensor_msgs::msg::Image::Cons
     const int x = (num % TILE_X) * IMAGE_WIDTH;
     const int y = (num / TILE_X) * IMAGE_HEIGHT;
     frame.copyTo(full_frame(cv::Rect(x, y, IMAGE_WIDTH, IMAGE_HEIGHT)));
+
     cv::putText(full_frame, ns, cv::Point(x + 50, y + 50),
                 cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(0, 0, 255), 2);
     if (batt_perc[num])
@@ -88,6 +91,9 @@ void imageCallback(uint num, std::string ns, const sensor_msgs::msg::Image::Cons
         }
       }
     }
+
+    cv::rectangle(full_frame, cv::Rect(x, y, IMAGE_WIDTH, IMAGE_HEIGHT),
+                  cv::Scalar(150, 150, 150), 2, cv::LINE_AA);
   } 
   catch(const std::exception& e)
   {
@@ -154,12 +160,25 @@ int main(int argc, char ** argv)
   rclcpp::Subscription<geometry_msgs::msg::PoseArray>::SharedPtr body_sub[max_num_streams];
   rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr battery_sub[max_num_streams];
 
+  const rmw_qos_profile_t rmw_qos_profile_video =
+  {
+    RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+    1,
+    RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
+    RMW_QOS_POLICY_DURABILITY_VOLATILE,
+    RMW_QOS_DEADLINE_DEFAULT,
+    RMW_QOS_LIFESPAN_DEFAULT,
+    RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
+    RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
+    false
+  };
+
   for (uint i=0; i < namespaces.size(); i++)
   {
     frame_sub[i] = image_transport::create_subscription(
       node.get(), "/"+ namespaces[i]+"/oakd/rgb/preview/encoded",
       [i, namespaces] (const sensor_msgs::msg::Image::ConstSharedPtr & msg) {imageCallback(i, namespaces[i], msg);},
-      "ffmpeg", rmw_qos_profile_sensor_data);
+      "ffmpeg", rmw_qos_profile_video);
 
     body_sub[i] = node->create_subscription<geometry_msgs::msg::PoseArray>(
       "/"+ namespaces[i]+"/body_pose",