added confidence to handData msg

anion0278 · Dec 6, 2021 · 5a10c60 · 5a10c60
1 parent e3f6bff
commit 5a10c60
Show file tree

Hide file tree

Showing 14 changed files with 68 additions and 49 deletions.
diff --git a/.gitignore b/.gitignore
@@ -12,3 +12,6 @@ jetson_camera_node/src/mediapipe_node/model/__pycache__/__init__.cpython-36.pyc
 jetson_camera_node/src/mediapipe_node/model/keypoint_classifier/__pycache__/keypoint_classifier.cpython-36.pyc
 jetson_camera_node/src/mediapipe_node/model/point_history_classifier/__pycache__/point_history_classifier.cpython-36.pyc
 jetson_camera_node/src/mediapipe_node/__pycache__/hand_data.cpython-36.pyc
+jetson_camera_node/src/mediapipe_node/__pycache__/hand_data.cpython-36.pyc
+jetson_camera_node/src/mediapipe_node/model/__pycache__/__init__.cpython-36.pyc
+jetson_camera_node/src/mediapipe_node/model/keypoint_classifier/__pycache__/keypoint_classifier.cpython-36.pyc
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
@@ -1,18 +1,23 @@
 {
-  "configurations": [
-    {
-      "browse": {
-        "databaseFilename": "",
-        "limitSymbolsToIncludedHeaders": true
-      },
-      "includePath": [
-        "/opt/ros/melodic/include/**",
-        "/home/k354jn1/catkin_ws/devel/include/**",
-        "/home/k354jn1/catkin_ws/src/vision_opencv/cv_bridge/include/**",
-        "/home/k354jn1/catkin_ws/src/vision_opencv/image_geometry/include/**",
-        "/usr/include/**"
-      ],
-      "name": "ROS"
-    }
-  ]
+    "configurations": [
+        {
+            "browse": {
+                "databaseFilename": "",
+                "limitSymbolsToIncludedHeaders": true
+            },
+            "includePath": [
+                "/opt/ros/melodic/include/**",
+                "/home/k354jn1/catkin_ws/devel/include/**",
+                "/home/k354jn1/catkin_ws/src/vision_opencv/cv_bridge/include/**",
+                "/home/k354jn1/catkin_ws/src/vision_opencv/image_geometry/include/**",
+                "/usr/include/**"
+            ],
+            "name": "ROS",
+            "intelliSenseMode": "linux-gcc-x64",
+            "compilerPath": "/usr/bin/clang",
+            "cStandard": "c11",
+            "cppStandard": "c++14"
+        }
+    ],
+    "version": 4
 }
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -5,5 +5,11 @@
         "/opt/ros/melodic/lib/python2.7/dist-packages",
         "/usr/local/lib",
         "/usr/local/lib/python3.6"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/k354jn1/catkin_ws/devel/lib/python2.7/dist-packages",
+        "/opt/ros/melodic/lib/python2.7/dist-packages",
+        "/usr/local/lib",
+        "/usr/local/lib/python3.6"
     ]
 }
diff --git a/jetson_camera_node/.vscode/launch.json b/jetson_camera_node/.vscode/launch.json
@@ -0,0 +1,6 @@
+{
+    // Use IntelliSense to learn about possible attributes.
+    // Hover to view descriptions of existing attributes.
+    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+    "version": "0.2.0"
+}
diff --git a/jetson_camera_node/.vscode/settings.json b/jetson_camera_node/.vscode/settings.json
@@ -4,5 +4,11 @@
         "/opt/ros/melodic/lib/python2.7/dist-packages",
         "/usr/local/lib",
         "/usr/local/lib/python3.6"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/k354jn1/catkin_ws/devel/lib/python2.7/dist-packages",
+        "/opt/ros/melodic/lib/python2.7/dist-packages",
+        "/usr/local/lib",
+        "/usr/local/lib/python3.6"
     ]
 }
diff --git a/jetson_camera_node/launch/start_test.launch b/jetson_camera_node/launch/start_test.launch
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <launch>
   <arg name="robot_ip" default="192.168.1.111"/>
-    <node name="hand_tracker_1" pkg="jetson_camera_node" type="hand_tracker_emulator.py" respawn="false" output="screen"/>
-    <node name="hand_tracker_2" pkg="jetson_camera_node" type="hand_tracker_emulator.py" respawn="false" output="screen"/>
+    <node name="hands_tracker_1" pkg="jetson_camera_node" type="hand_tracker_emulator.py" respawn="false" output="screen"/>
+    <node name="hands_tracker_2" pkg="jetson_camera_node" type="hand_tracker_emulator.py" respawn="false" output="screen"/>
 </launch>
diff --git a/jetson_camera_node/msg/HandData.msg b/jetson_camera_node/msg/HandData.msg
@@ -1,3 +1,4 @@
 geometry_msgs/Point[] landmarks
 int32 gestureType
 int32 handSide
+float64 confidence
diff --git a/jetson_camera_node/src/MainNode.cpp b/jetson_camera_node/src/MainNode.cpp
@@ -272,7 +272,7 @@ void PublishCameraData(ros::Publisher rosCameraDataPublisher, Matrix cameraToRob
 		cv::cvtColor(cvRgbImg, cvRgbImg, cv::COLOR_BGR2RGB);
 		cv::cvtColor(cvDepthImg, cvDepthImg, cv::COLOR_GRAY2RGB);
 		hconcat(cvRgbImg, cvDepthImg, cvRgbImg);
-		imshow("RGB and aligned Depth", cvRgbImg);
+		imshow("[PO] RGB and aligned Depth", cvRgbImg);
 	}
 }
 
@@ -532,8 +532,7 @@ int main(int argc, char** argv) // TODO Petr, please, divide this god-method int
 		}
 
 		auto depth_cv = mask.ToOpenCV();
-		imshow("Depth mask", depth_cv);
-
+		imshow("[PO] Depth mask", depth_cv);
 
 		if(manual_calibration)
 		{

diff --git a/jetson_camera_node/src/master_node/main_node_processor.py b/jetson_camera_node/src/master_node/main_node_processor.py
@@ -9,18 +9,19 @@
 import message_filters
 from udp import MainPcCommunication
 from collections import Counter
+import rosnode
 
 class DataAggregateProcessor():
     def __init__(self, node_names):
         rospy.init_node('hand_aggregation_processor')
         self.__init_cam_subs(node_names)
-        self.pc_communication = MainPcCommunication()
+        self.pc_communication = MainPcCommunication("192.168.0.149") #"192.168.1.20" PO
 
     def __init_cam_subs(self, node_names):
         if len(node_names) == 0: ValueError("Check node names!")
         subs = []
-        for jetson_node in node_names:
-            subs.append(message_filters.Subscriber(jetson_node+"/hands_data", MultiHandData, queue_size=1))
+        for node_name in node_names:
+            subs.append(message_filters.Subscriber(node_name + "/hands_data", MultiHandData, queue_size=1))
         self.sync = message_filters.ApproximateTimeSynchronizer(subs, queue_size=1, slop=0.1)
         self.sync.registerCallback(self.on_sync_data)
 
@@ -36,9 +37,9 @@ def on_sync_data(self, *sensors_msgs):
         # TVORIT POINT CLOUD
         if len(hands) > 0:
             index_point = ros_numpy.numpify(hands[0].landmarks[8])
-            filtered_hands_data = [[*index_point, float(hands[0].gestureType)]]
+            filtered_hands_data = [*index_point, float(hands[0].gestureType)]
             print(filtered_hands_data)
-            self.pc_communication.send_hand_data(filtered_hands_data)
+            self.pc_communication.send_hand_data(filtered_hands_data, filtered_hands_data)
             return
         print("recieved empty data msg")
 
@@ -60,6 +61,7 @@ def merge_hand_data(self, merge_data_list): # unused for now
         return handSide, gestureType, landmarksList
 
 if __name__ == "__main__":
-    proc = DataAggregateProcessor(["/hand_tracker"])
+    hand_tracker_nodes = list(filter(lambda t: t.startswith("/hands_tracker"), rosnode.get_node_names()))
+    proc = DataAggregateProcessor(hand_tracker_nodes)
     proc.run()
 
diff --git a/jetson_camera_node/src/master_node/udp.py b/jetson_camera_node/src/master_node/udp.py
@@ -15,11 +15,9 @@ def close(self):
         self.socket.close()
 
 class MainPcCommunication():
-    def __init__(self):
-        #self.udp_com = UDPClient("192.168.0.2", 4023)
-        #self.udp_com = UDPClient("169.254.59.148", 4023)
-        self.udp_com = UDPClient("192.168.1.20", 4023) # UCR - PO PC
+    def __init__(self, ip):
+        self.udp_com = UDPClient(ip, 4023) 
 
-    def send_hand_data(self, hand_data):
-        udp_msg = hand_data[0] 
+    def send_hand_data(self, left_hand_data, right_hand_data):
+        udp_msg = [*left_hand_data, *right_hand_data]
         self.udp_com.send_floats(udp_msg)
diff --git a/jetson_camera_node/src/mediapipe_node/config.py b/jetson_camera_node/src/mediapipe_node/config.py
@@ -5,4 +5,5 @@ class HandSide(Enum):
     LEFT = 2
 
 hand_recognition_servise = "hand_recognition"
-hands_data_topic = "/hands_data"
+hands_data_topic = "/hands_data"
+hands_tracker_node_name = "/hands_tracker"
diff --git a/jetson_camera_node/src/mediapipe_node/hand_tracker.py b/jetson_camera_node/src/mediapipe_node/hand_tracker.py
@@ -1,32 +1,29 @@
 #!/usr/bin/env python3
 
 import cv2
-import mediapipe as mp
-import time
 import rospy
 import config
 import ros_numpy
 import numpy as np
 import pyrealsense2 as rs
-import jetson_media as jm
-from sensor_msgs.msg import Image
+import recognizer as rec
 from geometry_msgs.msg import Point
-from jetson_camera_node.srv import ImageRec, ImageRecRequest, ImageRecResponse
 from jetson_camera_node.msg import CameraData, HandData, MultiHandData
 from pympler.asizeof import asizeof
 
 ros_cam_data_msg_size = 421248
 
 class HandRecognizer():
     def __init__(self):
-        rospy.init_node("hand_tracker")
-        self.recognizer = jm.MPRecognizer(debug = True)
+        rospy.init_node(config.hands_tracker_node_name)
+        self.recognizer = rec.MPRecognizer(max_num_hands = 2, debug = True)
         self.subscriber = rospy.Subscriber("camera_data", CameraData, self.__process_topic_data, queue_size = 1, 
                                             buff_size= ros_cam_data_msg_size * 2) # fixes latency problem: https://answers.ros.org/question/220502/image-subscriber-lag-despite-queue-1/
         self.hands_pub = rospy.Publisher(rospy.get_name() + config.hands_data_topic, MultiHandData, queue_size = 1)
 
     def run(self):
         rospy.spin()
+
     def __process_topic_data(self, cameraData):
         #print("Size of ROS message (min buff size): " + str(asizeof(cameraData))) # currently 421248
         cv_color_img = ros_numpy.numpify(cameraData.color)
@@ -49,6 +46,7 @@ def __publish_hands(self, recognized_hands_list):
             hand_msg = HandData() 
             hand_msg.handSide = hand.side.value
             hand_msg.gestureType = hand.gest
+            hand_msg.confidence = hand.confidence
             landmarks = hand.pos3D
             for i in range(len(landmarks)):
                 xyz_point = landmarks[i]

diff --git a/jetson_camera_node/src/mediapipe_node/hand_tracker_emulator.py b/jetson_camera_node/src/mediapipe_node/hand_tracker_emulator.py
@@ -5,20 +5,15 @@
 import time
 import rospy
 import config
-import ros_numpy
 from std_msgs.msg import Header
-import numpy as np
-import pyrealsense2 as rs
 from cv_bridge import CvBridge
-from sensor_msgs.msg import Image
 from geometry_msgs.msg import Point
-from jetson_camera_node.srv import ImageRec, ImageRecRequest, ImageRecResponse
 from jetson_camera_node.msg import CameraData, HandData, MultiHandData
 
 
 def get_test_data():
     multi_hand_data = MultiHandData()
-    for hand in range(3):
+    for hand in range(2):
         landmarks = []
         for i in range(21):
             landmarks.append(Point(x=1,y=2,z=3))

diff --git a/...a_node/src/mediapipe_node/jetson_media.py → ...era_node/src/mediapipe_node/recognizer.py b/...a_node/src/mediapipe_node/jetson_media.py → ...era_node/src/mediapipe_node/recognizer.py
@@ -3,7 +3,6 @@
 import mediapipe as mp
 import pyrealsense2 as rs
 import hand_data as hd
-import csv
 from model import KeyPointClassifier #tensorflow https://docs.nvidia.com/deeplearning/frameworks/install-tf-jetson-platform/index.html
 
 class MPRecognizer:
@@ -81,7 +80,7 @@ def recognize_hand(self,color,depth,intrinsics,scale,extrinsics):
                 cv2.putText(color,self.keypoint_classifier_labels[hand.gest],index_point,cv2.FONT_HERSHEY_SIMPLEX,0.6,(255,255,255),1,cv2.LINE_AA)
                 #cv2.putText(color,hand.side.name,index_point,cv2.FONT_HERSHEY_SIMPLEX,0.6,(255,255,255),1,cv2.LINE_AA)
             stack = np.concatenate((cv2.cvtColor(color, cv2.COLOR_RGB2BGR), depth_tf), axis=1)
-            cv2.imshow("Processed RGB + depth", stack)
+            cv2.imshow("[AS] Processed RGB + depth", stack)
             cv2.waitKey(2)
             #print("depth: %s" % depth_value)
         return hands