Update config files and cleanup publisher

src/perception/lidar_object_detection/lidar_object_detection/config/eve_config.yaml

@@ -1,4 +1,4 @@
-camera_object_detection_node:
+lidar_object_detection_node:
   ros__parameters:
     lidar_topic: /velodyne_points
     model_config_path: /home/bolty/OpenPCDet/tools/cfgs/kitti_models/pv_rcnn.yaml

src/perception/lidar_object_detection/lidar_object_detection/config/nuscenes_config.yaml

@@ -1,4 +1,4 @@
-camera_object_detection_node:
+lidar_object_detection_node:
   ros__parameters:
     lidar_topic: /LIDAR_TOP
     model_config_path: /home/bolty/OpenPCDet/tools/cfgs/nuscenes_models/cbgs_voxel0075_voxelnext.yaml

src/perception/lidar_object_detection/lidar_object_detection/lidar_object_detection/lidar_object_detection_node.py

@@ -5,16 +5,14 @@
 from sensor_msgs.msg import PointCloud2, PointField
 import numpy as np
 import torch
-import struct
 import sys
 sys.path.append('/home/bolty/OpenPCDet')
 from pcdet.config import cfg, cfg_from_yaml_file
 from pcdet.datasets import DatasetTemplate
 from pcdet.models import build_network, load_data_to_gpu
 from pcdet.utils import common_utils
 from visualization_msgs.msg import Marker, MarkerArray
-
-OPEN3D_FLAG = True
+from vision_msgs.msg import ObjectHypothesisWithPose, Detection3D, Detection3DArray 
 
 class LidarObjectDetection(Node):
     def __init__(self):
@@ -26,14 +24,14 @@ def __init__(self):
         self.model_config_path = self.get_parameter("model_config_path").value
         self.lidar_data = self.get_parameter("lidar_topic").value
 
-        self.bbox_publisher = self.create_publisher(MarkerArray, '/BOUNDING_BOXES', 2)
+        self.bbox_publisher = self.create_publisher(MarkerArray, '/BOUNDING_BOXES', 10)
+        self.detections_publisher = self.create_publisher(Detection3DArray, '/detections_3d', 10)
 
         self.subscription = self.create_subscription(
             PointCloud2,
             self.lidar_data,
             self.point_cloud_callback,
             10)
-        self.subscription  
 
         args, cfg = self.parse_config()
         self.logger = common_utils.create_logger()
@@ -60,7 +58,7 @@ def point_cloud_callback(self, msg):
         with torch.no_grad():
             pred_dicts, _ = self.model.forward(data_dict)
 
-        self.publish_bounding_boxes(pred_dicts, msg.header.frame_id)
+        self.publish_bounding_boxes(msg, pred_dicts)
 
     def pointcloud2_to_xyz_array(self, cloud_msg):
         num_points = cloud_msg.width * cloud_msg.height
@@ -73,12 +71,12 @@ def pointcloud2_to_xyz_array(self, cloud_msg):
 
         return cloud_array
 
-    def publish_bounding_boxes(self, pred_dicts, frame_id):
+    def publish_bounding_boxes(self, pointcloud_msg, pred_dicts):
         marker_array = MarkerArray()
         for idx, box in enumerate(pred_dicts[0]['pred_boxes']):
             marker = Marker()
-            marker.header.frame_id = frame_id
-            marker.header.stamp = self.get_clock().now().to_msg()
+            marker.header.frame_id = pointcloud_msg.header.frame_id
+            marker.header.stamp = pointcloud_msg.header.stamp
             marker.id = idx
             marker.type = Marker.CUBE
             marker.action = Marker.ADD
@@ -92,10 +90,28 @@ def publish_bounding_boxes(self, pred_dicts, frame_id):
             marker.color.a = 0.8
             marker.color.r = 1.0
             marker.color.g = 0.0
-            marker.color.b = 0.0
+            marker.color.b = float(pred_dicts[0]['pred_labels'][idx]) / 3
             marker_array.markers.append(marker)
 
+        detections = Detection3DArray()
+        detections.header = pointcloud_msg.header
+        for idx, box in enumerate(pred_dicts[0]['pred_boxes']):
+            detection = Detection3D()
+            detection.header = pointcloud_msg.header
+            detection.bbox.center.position.x = float(box[0])
+            detection.bbox.center.position.y = float(box[1])
+            detection.bbox.center.position.z = float(box[2])
+            detection.bbox.size.x = float(box[3])
+            detection.bbox.size.y = float(box[4])
+            detection.bbox.size.z = float(box[5])
+            detected_object = ObjectHypothesisWithPose()
+            detected_object.hypothesis.class_id = str(pred_dicts[0]['pred_labels'][idx])
+            detected_object.hypothesis.score = float(pred_dicts[0]['pred_scores'][idx])
+            detection.results.append(detected_object)
+            detections.detections.append(detection)
+
         self.bbox_publisher.publish(marker_array)
+        self.detections_publisher.publish(detections)
 
     def parse_config(self):
         parser = argparse.ArgumentParser(description='arg parser')
@@ -107,11 +123,6 @@ def parse_config(self):
         cfg_from_yaml_file(args.cfg_file, cfg)
         return args, cfg
 
-    def make_header(self):
-        header = Header()
-        header.stamp = self.get_clock().now().to_msg()
-        header.frame_id = 'base_link'
-        return header
 
     def create_cloud_xyz32(self, header, points):
         """
@@ -172,4 +183,4 @@ def main(args=None):
     rclpy.shutdown()
 
 if __name__ == '__main__':
-    main()
+    main()

src/perception/lidar_object_detection/lidar_object_detection/package.xml

@@ -9,12 +9,11 @@
 
   <depend>rclpy</depend>
   <depend>std_msgs</depend>
-  <exec_depend>rospy</exec_depend>
-  <exec_depend>sensor_msgs</exec_depend>
-  <exec_depend>nav_msgs</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>tf</exec_depend>
-  <exec_depend>visualization_msgs</exec_depend>
+  <depend>rospy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>vision_msgs</depend>
+  <depend>tf</depend>
+  <depend>visualization_msgs</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>