Ros1 fixes for 3D Detection and 2D/3D tracking

projects/opendr_ws/README.md

@@ -50,12 +50,12 @@ Currently, apart from tools, opendr_ws contains the following ROS nodes (categor
 7. [Semantic Segmentation](src/perception/README.md#semantic-segmentation-ros-node)
 8. [Video Human Activity Recognition](src/perception/README.md#human-action-recognition-ros-node)
 9. [Landmark-based Facial Expression Recognition](src/perception/README.md#landmark-based-facial-expression-recognition-ros-node)
-10. [Deep Sort Object Tracking 2D](src/perception/README.md#deep-sort-object-tracking-2d-ros-node)
-11. [Skeleton-based Human Action Recognition](src/perception/README.md#skeleton-based-human-action-recognition-ros-node)
+10. [FairMOT Object Tracking 2D](src/perception/README.md#fairmot-object-tracking-2d-ros-node)
+11. [Deep Sort Object Tracking 2D](src/perception/README.md#deep-sort-object-tracking-2d-ros-node)
+12. [Skeleton-based Human Action Recognition](src/perception/README.md#skeleton-based-human-action-recognition-ros-node)
 ## Point cloud input
 1. [Voxel Object Detection 3D](src/perception/README.md#voxel-object-detection-3d-ros-node)
 2. [AB3DMOT Object Tracking 3D](src/perception/README.md#ab3dmot-object-tracking-3d-ros-node)
-3. [FairMOT Object Tracking 2D](src/perception/README.md#fairmot-object-tracking-2d-ros-node)
 ## RGB + Infrared input
 1. [End-to-End Multi-Modal Object Detection (GEM)](src/perception/README.md#gem-ros-node)
 ## RGBD input nodes

projects/opendr_ws/src/perception/README.md

@@ -38,11 +38,11 @@ rosrun perception pose_estimation.py
 ```
 
 3. You can examine the annotated image stream using `rqt_image_view` (select the topic `/opendr/image_pose_annotated`) or
-   `rostopic echo /opendr/poses`. 
+   `rostopic echo /opendr/poses`.
 
 Note that to use the pose messages properly, you need to create an appropriate subscriber that will convert the ROS pose messages back to OpenDR poses which you can access as described in the [documentation](https://github.com/opendr-eu/opendr/blob/master/docs/reference/engine-target.md#posekeypoints-confidence):
 ```python
-        ... 
+        ...
         rospy.Subscriber("opendr/poses", Detection2DArray, self.callback)
         ...
         def callback(self, data):
@@ -212,7 +212,7 @@ A ROS node for performing heart anomaly (atrial fibrillation) detection from ecg
 ```shell
 rosrun perception heart_anomaly_detection.py ECG_TOPIC MODEL
 ```
-with `ECG_TOPIC` specifying the ROS topic to which the node will subscribe, and `MODEL` set to either *gru* or *anbof*. The predictied classes are published to the topic `/opendr/heartanomaly`.
+with `ECG_TOPIC` specifying the ROS topic to which the node will subscribe, and `MODEL` set to either *gru* or *anbof*. The predicted classes are published to the topic `/opendr/heartanomaly`.
 
 ## Human Action Recognition ROS Node
 
@@ -221,7 +221,7 @@ Assuming the drivers have been installed and OpenDR catkin workspace has been so
 ```shell
 rosrun perception video_activity_recognition.py
 ```
-The predictied class id and confidence is published under the topic name `/opendr/human_activity_recognition`, and the human-readable class name under `/opendr/human_activity_recognition_description`.
+The predicted class id and confidence is published under the topic name `/opendr/human_activity_recognition`, and the human-readable class name under `/opendr/human_activity_recognition_description`.
 
 ## Landmark-based Facial Expression Recognition ROS Node
 
@@ -230,16 +230,16 @@ Assuming the drivers have been installed and OpenDR catkin workspace has been so
 ```shell
 rosrun perception landmark_based_facial_expression_recognition.py
 ```
-The predictied class id and confidence is published under the topic name `/opendr/landmark_based_expression_recognition`, and the human-readable class name under `/opendr/landmark_based_expression_recognition_description`.
+The predicted class id and confidence is published under the topic name `/opendr/landmark_based_expression_recognition`, and the human-readable class name under `/opendr/landmark_based_expression_recognition_description`.
 
 ## Skeleton-based Human Action Recognition ROS Node
 
-A ROS node for performing Skeleton-based Human Action Recognition using either ST-GCN or PST-GCN models pretrained on NTU-RGBD-60 dataset. The human body poses of the image are first extracted by the light-weight Openpose method which is implemented in the toolkit, and they are passed to the skeleton-based action recognition method to be categorized.
+A ROS node for performing Skeleton-based Human Action Recognition using either ST-GCN or PST-GCN models pretrained on NTU-RGBD-60 dataset. The human body poses of the image are first extracted by the light-weight Open-pose method which is implemented in the toolkit, and they are passed to the skeleton-based action recognition method to be categorized.
 Assuming the drivers have been installed and OpenDR catkin workspace has been sourced, the node can be started as:
 ```shell
 rosrun perception skeleton_based_action_recognition.py
 ```
-The predictied class id and confidence is published under the topic name `/opendr/skeleton_based_action_recognition`, and the human-readable class name under `/opendr/skeleton_based_action_recognition_description`.
+The predicted class id and confidence is published under the topic name `/opendr/skeleton_based_action_recognition`, and the human-readable class name under `/opendr/skeleton_based_action_recognition_description`.
 Besides, the annotated image is published in `/opendr/image_pose_annotated` as well as the corresponding poses in `/opendr/poses`.
 
 ## Speech Command Recognition ROS Node
@@ -270,7 +270,7 @@ To get a point cloud from a dataset on the disk, you can start a `point_cloud_da
 ```shell
 rosrun perception point_cloud_dataset.py
 ```
-This will pulbish the dataset point clouds to a `/opendr/dataset_point_cloud` topic by default, which means that the `input_point_cloud_topic` should be set to `/opendr/dataset_point_cloud`.
+This will publish the dataset point clouds to a `/opendr/dataset_point_cloud` topic by default, which means that the `input_point_cloud_topic` should be set to `/opendr/dataset_point_cloud`.
 
 ## AB3DMOT Object Tracking 3D ROS Node
 
@@ -286,7 +286,7 @@ To get a point cloud from a dataset on the disk, you can start a `point_cloud_da
 ```shell
 rosrun perception point_cloud_dataset.py
 ```
-This will pulbish the dataset point clouds to a `/opendr/dataset_point_cloud` topic by default, which means that the `input_point_cloud_topic` should be set to `/opendr/dataset_point_cloud`.
+This will publish the dataset point clouds to a `/opendr/dataset_point_cloud` topic by default, which means that the `input_point_cloud_topic` should be set to `/opendr/dataset_point_cloud`.
 
 
 ## FairMOT Object Tracking 2D ROS Node
@@ -305,7 +305,7 @@ If you want to use a dataset from the disk, you can start a `image_dataset.py` n
 ```shell
 rosrun perception image_dataset.py
 ```
-This will pulbish the dataset images to an `/opendr/dataset_image` topic by default, which means that the `input_image_topic` should be set to `/opendr/dataset_image`.
+This will publish the dataset images to an `/opendr/dataset_image` topic by default, which means that the `input_image_topic` should be set to `/opendr/dataset_image`.
 
 ## Deep Sort Object Tracking 2D ROS Node
 
@@ -323,5 +323,5 @@ If you want to use a dataset from the disk, you can start an `image_dataset.py`
 ```shell
 rosrun perception image_dataset.py
 ```
-This will pulbish the dataset images to an `/opendr/dataset_image` topic by default, which means that the `input_image_topic` should be set to `/opendr/dataset_image`.
+This will publish the dataset images to an `/opendr/dataset_image` topic by default, which means that the `input_image_topic` should be set to `/opendr/dataset_image`.
 

projects/opendr_ws/src/perception/scripts/image_dataset.py

@@ -13,6 +13,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import argparse
 import os
 import rospy
 import time
@@ -26,21 +27,21 @@ class ImageDatasetNode:
     def __init__(
         self,
         dataset: DatasetIterator,
-        output_image_topic="/opendr/dataset_image",
+        output_rgb_image_topic="/opendr/dataset_image",
+        data_fps=30,
     ):
         """
         Creates a ROS Node for publishing dataset images
         """
 
-        # Initialize the face detector
         self.dataset = dataset
         # Initialize OpenDR ROSBridge object
         self.bridge = ROSBridge()
+        self.delay = 1.0 / data_fps
 
-        if output_image_topic is not None:
-            self.output_image_publisher = rospy.Publisher(
-                output_image_topic, ROS_Image, queue_size=10
-            )
+        self.output_image_publisher = rospy.Publisher(
+            output_rgb_image_topic, ROS_Image, queue_size=10
+        )
 
     def start(self):
         rospy.loginfo("Timing images")
@@ -53,32 +54,60 @@ def start(self):
 
             rospy.loginfo("Publishing image [" + str(i) + "]")
             message = self.bridge.to_ros_image(
-                image, encoding="rgb8"
+                image, encoding="bgr8"
             )
             self.output_image_publisher.publish(message)
 
-            time.sleep(0.1)
+            time.sleep(self.delay)
 
             i += 1
 
 
-if __name__ == "__main__":
-
-    rospy.init_node('opendr_image_dataset')
-
-    dataset_path = MotDataset.download_nano_mot20(
-        "MOT", True
-    ).path
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("-d", "--dataset_path", help="Path to a dataset",
+                        type=str, default="MOT")
+    parser.add_argument(
+        "-ks", "--mot20_subsets_path", help="Path to mot20 subsets",
+        type=str, default=os.path.join(
+            "..", "..", "src", "opendr", "perception", "object_tracking_2d",
+            "datasets", "splits", "nano_mot20.train"
+        )
+    )
+    parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name to publish the data",
+                        type=str, default="/opendr/dataset_image")
+    parser.add_argument("-f", "--fps", help="Data FPS",
+                        type=float, default=30)
+    args = parser.parse_args()
+
+    dataset_path = args.dataset_path
+    mot20_subsets_path = args.mot20_subsets_path
+    output_rgb_image_topic = args.output_rgb_image_topic
+    data_fps = args.fps
+
+    if not os.path.exists(dataset_path):
+        dataset_path = MotDataset.download_nano_mot20(
+            "MOT", True
+        ).path
 
     dataset = RawMotDatasetIterator(
         dataset_path,
         {
-            "mot20": os.path.join(
-                "..", "..", "src", "opendr", "perception", "object_tracking_2d",
-                "datasets", "splits", "nano_mot20.train"
-            )
+            "mot20": mot20_subsets_path
         },
         scan_labels=False
     )
-    dataset_node = ImageDatasetNode(dataset)
+
+    rospy.init_node("image_dataset", anonymous=True)
+
+    dataset_node = ImageDatasetNode(
+        dataset,
+        output_rgb_image_topic=output_rgb_image_topic,
+        data_fps=data_fps,
+    )
+
     dataset_node.start()
+
+
+if __name__ == '__main__':
+    main()

projects/opendr_ws/src/perception/scripts/object_detection_3d_voxel.py

@@ -13,6 +13,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import argparse
 import torch
 import os
 import rospy
@@ -39,7 +40,7 @@ def __init__(
         """
         Creates a ROS Node for 3D object detection
         :param input_point_cloud_topic: Topic from which we are reading the input point cloud
-        :type input_image_topic: str
+        :type input_point_cloud_topic: str
         :param output_detection3d_topic: Topic to which we are publishing the annotations
         :type output_detection3d_topic:  str
         :param device: device on which we are running inference ('cpu' or 'cuda')
@@ -58,15 +59,13 @@ def __init__(
 
         self.learner.load(os.path.join(temp_dir, model_name), verbose=True)
 
-        # Initialize OpenDR ROSBridge object
+        self.input_point_cloud_topic = input_point_cloud_topic
         self.bridge = ROSBridge()
 
         self.detection_publisher = rospy.Publisher(
-            output_detection3d_topic, Detection3DArray, queue_size=10
+            output_detection3d_topic, Detection3DArray, queue_size=1
         )
 
-        rospy.Subscriber(input_point_cloud_topic, ROS_PointCloud, self.callback)
-
     def callback(self, data):
         """
         Callback that process the input data and publishes to the corresponding topics
@@ -80,39 +79,67 @@ def callback(self, data):
 
         # Convert detected boxes to ROS type and publish
         ros_boxes = self.bridge.to_ros_boxes_3d(detection_boxes, classes=["Car", "Van", "Truck", "Pedestrian", "Cyclist"])
-        if self.detection_publisher is not None:
-            self.detection_publisher.publish(ros_boxes)
-            rospy.loginfo("Published detection boxes")
+        self.detection_publisher.publish(ros_boxes)
+        rospy.loginfo("Published detection boxes")
+
+    def listen(self):
+        """
+        Start the node and begin processing input data.
+        """
+        rospy.init_node('voxel_detection_3d', anonymous=True)
+        rospy.Subscriber(self.input_point_cloud_topic, ROS_PointCloud, self.callback, queue_size=1, buff_size=10000000)
 
-if __name__ == "__main__":
-    # Automatically run on GPU/CPU
-    device = "cuda:0" if torch.cuda.is_available() else "cpu"
+        rospy.loginfo("Object Detection 3D Voxel Node started.")
+        rospy.spin()
 
-    # initialize ROS node
-    rospy.init_node("opendr_voxel_detection_3d", anonymous=True)
-    rospy.loginfo("Voxel Detection 3D node started")
 
-    model_name = rospy.get_param("~model_name", "tanet_car_xyres_16")
-    model_config_path = rospy.get_param(
-        "~model_config_path", os.path.join(
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("-n", "--model_name", help="Name of the trained model",
+                        type=str, default="tanet_car_xyres_16", choices=["tanet_car_xyres_16"])
+    parser.add_argument(
+        "-c", "--model_config_path", help="Path to a model .proto config",
+        type=str, default=os.path.join(
             "..", "..", "src", "opendr", "perception", "object_detection_3d",
             "voxel_object_detection_3d", "second_detector", "configs", "tanet",
-            "car", "test_short.proto"
+            "car", "xyres_16.proto"
         )
     )
-    temp_dir = rospy.get_param("~temp_dir", "temp")
-    input_point_cloud_topic = rospy.get_param(
-        "~input_point_cloud_topic", "/opendr/dataset_point_cloud"
-    )
-    rospy.loginfo("Using model_name: {}".format(model_name))
+    parser.add_argument("-t", "--temp_dir", help="Path to a temporary directory with models",
+                        type=str, default="temp")
+    parser.add_argument("-i", "--input_point_cloud_topic",
+                        help="Point Cloud topic provided by either a point_cloud_dataset_node or any other 3D Point Cloud Node",
+                        type=str, default="/opendr/dataset_point_cloud")
+    parser.add_argument("-o", "--output_detection3d_topic",
+                        help="Output detections topic",
+                        type=str, default="/opendr/detection3d")
+    parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"",
+                        type=str, default="cuda", choices=["cuda", "cpu"])
+    args = parser.parse_args()
+    try:
+        if args.device == "cuda" and torch.cuda.is_available():
+            device = "cuda"
+        elif args.device == "cuda":
+            print("GPU not found. Using CPU instead.")
+            device = "cpu"
+        else:
+            print("Using CPU.")
+            device = "cpu"
+    except:
+        print("Using CPU.")
+        device = "cpu"
 
-    # created node object
     voxel_node = ObjectDetection3DVoxelNode(
         device=device,
-        model_name=model_name,
-        model_config_path=model_config_path,
-        input_point_cloud_topic=input_point_cloud_topic,
-        temp_dir=temp_dir,
+        model_name=args.model_name,
+        model_config_path=args.model_config_path,
+        input_point_cloud_topic=args.input_point_cloud_topic,
+        temp_dir=args.temp_dir,
+        output_detection3d_topic=args.output_detection3d_topic,
     )
-    # begin ROS communications
-    rospy.spin()
+
+    voxel_node.listen()
+
+
+if __name__ == '__main__':
+    main()