heethesh · Sep 6, 2022 · Sep 26, 2022
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -13,6 +13,9 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
+  tf2_sensor_msgs
+  geometry_msgs
+  message_generation
 )
 
 ## System dependencies are found with CMake's conventions
@@ -49,11 +52,14 @@ find_package(catkin REQUIRED COMPONENTS
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
+add_message_files(
+  FILES
+  BoundingBox.msg
+  BoundingBoxes.msg
+  ObjectCount.msg
+  Obstacle.msg
+  Obstacles.msg
+)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -70,10 +76,11 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  geometry_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -109,6 +116,7 @@ catkin_package(
 #  LIBRARIES lidar_camera_calibration
 #  CATKIN_DEPENDS image_proc image_view roscpp rospy std_msgs
 #  DEPENDS system_lib
+  CATKIN_DEPENDS message_runtime
 )
 
 ###########
@@ -199,3 +207,7 @@ include_directories(
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
+
+catkin_install_python(PROGRAMS scripts/calibrate_camera_lidar.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
diff --git a/calibration_data/lidar_camera_calibration/extrinsics.npz b/calibration_data/lidar_camera_calibration/extrinsics.npz
diff --git a/calibration_data/lidar_camera_calibration/img_corners.npy b/calibration_data/lidar_camera_calibration/img_corners.npy
diff --git a/calibration_data/lidar_camera_calibration/pcl_corners.npy b/calibration_data/lidar_camera_calibration/pcl_corners.npy
diff --git a/launch/bagfile_filter.launch b/launch/bagfile_filter.launch
@@ -0,0 +1,22 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+    <arg name="bagfile" default="kitti_2011_09_26_drive_0119_synced.bag" />
+    <arg name="topicname" default="/kitti/camera_gray_left/camera_info /kitti/camera_gray_left/image_raw /kitti/velo/pointcloud" />
+
+    <!-- Play rosbag record -->
+    <node 
+        pkg="rosbag"
+        type="play"
+        name="original_bag_play"
+        output="screen"
+        args="--clock $(find lidar_camera_calibration)/../../$(arg bagfile)" />
+
+    <node
+	pkg="rosbag"
+	type="record"
+	name="rosbag_filtered"
+	output="screen"
+	args="-o $(find lidar_camera_calibration)/bagfiles/kitti.bag $(arg topicname)" />	
+
+</launch>
diff --git a/launch/camera_calibration.launch b/launch/camera_calibration.launch
@@ -1,14 +1,14 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
 
-    <arg name="camera" default="/sensors/camera" />
-    <arg name="size" default="7x5" />
-    <arg name="square" default="0.05" />
+    <arg name="camera" default="/kitti/camera_gray_left" />
+    <arg name="size1" default="11x7" />
+    <arg name="square" default="9.95e-2" />
+    <arg name="size2" default="7x5" />
     <arg name="k-coefficients" default="2" />
 
     <!-- Play rosbag record -->
     <include file="$(find lidar_camera_calibration)/launch/play_rosbag.launch">
-        <arg name="bagfile" value="2016-11-22-14-32-13_test.bag" />
     </include>
 
     <!-- Run camera calibration -->
@@ -17,13 +17,15 @@
         type="cameracalibrator.py"
         name="cameracalibrator"
         output="screen"
-        args="--size=$(arg size) 
+        args="--size=$(arg size1) 
+              --square=$(arg square)
+              --size=$(arg size2) 
               --square=$(arg square)
               --k-coefficients=$(arg k-coefficients) 
               --no-service-check" >
 
         <!-- Remap input topics -->
-        <remap from="image" to="$(arg camera)/image_color" />
+        <remap from="image" to="$(arg camera)/image_raw" />
         <remap from="camera" to="$(arg camera)/camera_info" />
     </node>
 

diff --git a/launch/display_camera_calibration.launch b/launch/display_camera_calibration.launch
@@ -1,12 +1,11 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
 
-    <arg name="camera" default="/sensors/camera" />
+    <arg name="camera" default="/camera/color" />
+    <!-- for kitti : /kitti/camera_gray_left -->
 
     <!-- Play rosbag record -->
-    <include file="$(find lidar_camera_calibration)/launch/play_rosbag.launch">
-        <arg name="bagfile" value="2016-11-22-14-32-13_test_updated.bag" />
-    </include>
+    <include file="$(find lidar_camera_calibration)/launch/play_rosbag.launch"/>
 
     <!-- Nodelet manager for this pipeline -->
     <node
@@ -29,23 +28,23 @@
         args="load image_proc/rectify lidar_camera_manager --no-bond" >
 
         <!-- Remap input topics -->
-        <remap from="image_mono" to="$(arg camera)/image_color" />
+        <remap from="image_mono" to="$(arg camera)/image_raw" />
         <remap from="camera_info" to="$(arg camera)/camera_info" />
 
         <!-- Remap output topics -->
-        <remap from="image_rect" to="$(arg camera)/image_rect_color" />
+        <remap from="image_rect" to="$(arg camera)/image_rect" />
     </node>
 
     <!-- Run image view to display the unrectified image -->
     <node 
         name="unrectified" 
-        pkg="image_view"
+        pkg="image_view"	
         type="image_view"
         respawn="false"
         output="screen">
 
         <!-- Remap input topics -->
-        <remap from="image" to="$(arg camera)/image_color" />
+        <remap from="image" to="$(arg camera)/image_raw" />
     </node>
 
     <!-- Run image view to display the rectified image -->
@@ -57,7 +56,7 @@
         output="screen">
 
         <!-- Remap input topics -->
-        <remap from="image" to="$(arg camera)/image_rect_color" />
+        <remap from="image" to="$(arg camera)/image_rect" />
     </node>
 
 </launch>
diff --git a/launch/display_camera_lidar_calibration.launch b/launch/display_camera_lidar_calibration.launch
@@ -1,11 +1,10 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
 
-    <arg name="camera" default="/sensors/camera" />
+    <arg name="camera" default="/camera/color" />
 
     <!-- Play rosbag record -->
     <include file="$(find lidar_camera_calibration)/launch/play_rosbag.launch">
-        <arg name="bagfile" value="2016-11-22-14-32-13_test_updated.bag" />
     </include>
 
     <!-- Nodelet manager for this pipeline -->
@@ -29,26 +28,26 @@
         args="load image_proc/rectify lidar_camera_manager --no-bond" >
 
         <!-- Remap input topics -->
-        <remap from="image_mono" to="$(arg camera)/image_color" />
+        <remap from="image_mono" to="$(arg camera)/image_raw" />
         <remap from="camera_info" to="$(arg camera)/camera_info" />
 
         <!-- Remap output topics -->
-        <remap from="image_rect" to="$(arg camera)/image_rect_color" />
+        <remap from="image_rect" to="$(arg camera)/image_rect" />
     </node>
 
     <!-- Wire static transform from the world to velodyne frame -->
+    <!-- Fill args below with estimated transform X Y Z Y P R -->
     <node
         pkg="tf2_ros"
         type="static_transform_publisher"
         name="world_velodyne_tf"
         output="screen"
-        <!-- Fill this with estimated transform X Y Z Y P R -->
-        args="0 0 0 0 0 0 world velodyne" />
+        args="0.00728906 -0.05762633 -0.13652829 1.5693861555125765 -1.5111674638103325 0.0014911849306030126 world velodyne" />
 
     <!-- Setup params for Camera-LiDAR calibration script -->
     <param name="camera_info_topic" type="str" value="$(arg camera)/camera_info" />
-    <param name="image_color_topic" type="str" value="$(arg camera)/image_rect_color" />
-    <param name="velodyne_points_topic" type="str" value="/sensors/velodyne_points" />
+    <param name="image_color_topic" type="str" value="$(arg camera)/image_rect" />
+    <param name="velodyne_points_topic" type="str" value="/os_cloud_node/points" />
     <param name="camera_lidar_topic" type="str" value="$(arg camera)/camera_lidar" />
     <param name="project_mode" type="bool" value="true" />
 
@@ -67,8 +66,8 @@
         respawn="false"
         output="screen">
 
-        <!-- Remap input topics -->
+        <!-- Remap input topics -->  
         <remap from="image" to="$(arg camera)/camera_lidar" />
-    </node>
+    </node> 
 
 </launch>
diff --git a/launch/display_obstacles.launch b/launch/display_obstacles.launch
@@ -0,0 +1,85 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+    <arg name="camera" default="/camera/color" />
+
+    <!-- Play rosbag record -->
+    <include file="$(find lidar_camera_calibration)/launch/play_rosbag.launch">
+    </include>
+
+    <!-- Nodelet manager for this pipeline -->
+    <node
+        pkg="nodelet"
+        type="nodelet"
+        args="manager"
+        name="lidar_camera_manager"
+        output="screen" />
+
+    <node
+        pkg="image_proc"
+        type="image_proc" 
+        name="image_proc_node1" />
+
+    <!-- Run image_proc/rectify nodelet -->
+    <node
+        pkg="nodelet"
+        type="nodelet"
+        name="rectify_color"
+        args="load image_proc/rectify lidar_camera_manager --no-bond" >
+
+        <!-- Remap input topics -->
+        <remap from="image_mono" to="$(arg camera)/image_raw" />
+        <remap from="camera_info" to="$(arg camera)/camera_info" />
+
+        <!-- Remap output topics -->
+        <remap from="image_rect" to="$(arg camera)/image_rect" />
+    </node>
+
+    <!-- Wire static transform from the world to velodyne frame -->
+    <!-- Fill args below with estimated transform X Y Z Y P R -->
+    <node
+        pkg="tf2_ros"
+        type="static_transform_publisher"
+        name="world_velodyne_tf"
+        output="screen"
+        args="0.00728906 -0.05762633 -0.13652829 1.5693861555125765 -1.5111674638103325 0.0014911849306030126 world velodyne" />
+
+    <!-- Setup params for Camera-LiDAR calibration script -->
+    <param name="camera_info_topic" type="str" value="$(arg camera)/camera_info" />
+    <param name="image_color_topic" type="str" value="$(arg camera)/image_rect" />
+    <param name="velodyne_points_topic" type="str" value="/os_cloud_node/points" />
+    <param name="bounding_boxes_topic" type="str" value="/darknet_ros/bounding_boxes" />
+    <param name="found_object_topic" type="str" value="/darknet_ros/found_object" />
+    <!-- for velodyne pointcloud : /kitti/velo/pointcloud  and ours : /os_cloud_node/points -->
+
+    <param name="camera_lidar_topic" type="str" value="$(arg camera)/camera_lidar" />
+    <param name="obstacle_info_topic" type="str" value="/obstacle_info" />
+    <param name="project_mode" type="bool" value="false" />
+
+    <!-- Run Camera-LiDAR projection script -->
+    <node   
+        pkg="lidar_camera_calibration"
+        type="calibrate_camera_lidar.py"
+        name="calibrate_camera_lidar"
+        output="screen" />
+
+    <!-- Run display-obstacles script -->
+    <node   
+        pkg="lidar_camera_calibration"
+        type="display_obstacles.py"
+        name="display_obstacles"
+        output="screen" />
+
+    <!-- Run image view to display the projected points image -->
+    <node 
+        name="camera_lidar_projection" 
+        pkg="image_view"
+        type="image_view"
+        respawn="false"
+        output="screen">
+
+        <!-- Remap input topics -->  
+        <remap from="image" to="$(arg camera)/camera_lidar" />
+    </node> 
+
+</launch>
diff --git a/launch/play_rosbag.launch b/launch/play_rosbag.launch
@@ -1,14 +1,16 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
 
-    <arg name="bagfile" default="2016-11-22-14-32-13_test_updated.bag" />
+    <!-- <arg name="bagfile" default="2021-11-05-12-41-17_modified.bag" /> -->
+    <arg name="bagfile" default="visit_test_modified.bag" />
+    <!-- <arg name="bagfile" default="data_gather_2021-11-05-12-42-49_modified.bag" /> -->
 
     <!-- Play rosbag record -->
-    <node 
+    <node
         pkg="rosbag"
         type="play"
         name="player"
         output="screen"
-        args="--clock --loop $(find lidar_camera_calibration)/bagfiles/$(arg bagfile)" />
+        args="-q --loop $(find lidar_camera_calibration)/bagfiles/$(arg bagfile)" />
 
 </launch>
diff --git a/msg/BoundingBox.msg b/msg/BoundingBox.msg
@@ -0,0 +1,7 @@
+float64 probability
+int64 xmin
+int64 ymin
+int64 xmax
+int64 ymax
+int16 id
+string Class
diff --git a/msg/BoundingBoxes.msg b/msg/BoundingBoxes.msg
@@ -0,0 +1,3 @@
+Header header
+Header image_header
+BoundingBox[] bounding_boxes
diff --git a/msg/ObjectCount.msg b/msg/ObjectCount.msg
@@ -0,0 +1,2 @@
+Header header
+int8 count
diff --git a/msg/Obstacle.msg b/msg/Obstacle.msg
@@ -0,0 +1,8 @@
+float64 xCenter
+float64 yCenter
+float64 zCenter
+float64 width
+float64 length
+float64 height
+string Class
+geometry_msgs/Point[] points
diff --git a/msg/Obstacles.msg b/msg/Obstacles.msg
@@ -0,0 +1,2 @@
+Header header
+Obstacle[] obstacles
diff --git a/package.xml b/package.xml
@@ -27,6 +27,7 @@
   <build_depend>rosbag</build_depend>
   <build_depend>tf2_ros</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
   <build_export_depend>image_proc</build_export_depend>
   <build_export_depend>image_view</build_export_depend>
   <build_export_depend>roscpp</build_export_depend>
@@ -41,5 +42,6 @@
   <exec_depend>rosbag</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
   <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
 
 </package>