Major package restructuring (#2)

* removes yaml from requirements and adds CMakeList to visualization

* removes double scipy requirement

* Adding some ros dependencies to package.xml

Add a hint in the readme to execute rosdep to get all the needed ros packages.

* moves msgs in separate pkg

* removes some more requirements

* changes camera topics and switches camera_info callback to be 'wait_for_message'

* removes message files from object_visualization pkg

* slight change to node.py

* prepares new launch files for test on rosbag and adds bench to CLASS_COLOR

* changes the way markers are published and adds tests for with and without distortion

* adds timing of detection (commented)

* restructures node and cleans up launch file

* adds dropper node to reduce back-log in detection and improves launch file structure

* publishes poseArray for the object positions.

* deletes unsused launch files, adds launch file for remote, i.e. jetson, adds rviz config

* Adds basic support for jetson

Still needs to be adjusted to SMB network.

* Re-arranging launch files to tackle multiple use-cases

by default, running object_detection.launch runs de-bayering locally,
while the actually object detection node is run on the jetson.

launch file arguments allow using the same launch file for different scenarios.

!running detection on recorded rosbag is not yet adjusted!

* Adjusting jetson_env.sh

- using hostnames works, no need to set ROS_IP
- no need to set ROS_MASTER_URI

* Update README.md

* remove submodule image_undistort

* use all lowercase for projector_config file

projector config addendum

* remove camera calibration

camera calibration is supplied via camera_info

* launch: removing argument camera_calib_path

* Use rospack to locate package path

* Adding default projector configs for remaining smbs

* Changing jetson setup/environment

! Now, the jetson_env.sh needs to be in the user home directory (~) !
Easy way to still follow changes: create a symlink as follows.

```
roscd object_detection
ln -s $PWD/launch/jetson_env.sh ~/jetson_env.sh
```

* squashed commit to fix large staged files. This commit includes including changes to load a local yolov5 repo and removes timing info and comments from code

* updates README

* load locally

* updates requirements

* change folder to point to shared space on jetson

* change folder to point to shared space on jetson

* removes object visualisation and updates readme

* changes back such that camera frame is taken from camera_info

Co-authored-by: mads <[email protected]>

object_detection/CMakeLists.txt

@@ -11,94 +11,17 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   ros_numpy
-  std_msgs
-  geometry_msgs
-  sensor_msgs
-  message_generation
+  object_detection_msgs
 )
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
 
-
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
 catkin_python_setup()
 
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-add_message_files(
-  FILES
-  ObjectDetection.msg
-  ObjectDetectionArray.msg
-)
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-generate_messages(
-  DEPENDENCIES
-  std_msgs
-  geometry_msgs
-  sensor_msgs
-)
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
 ###################################
 ## catkin specific configuration ##
 ###################################

object_detection/cfg/output_params.yaml

@@ -0,0 +1,6 @@
+project_object_points_to_image: true
+project_all_points_to_image: false
+object_detection_pos_topic: ~object_positions
+object_detection_output_image_topic: ~detections_in_image
+object_detection_point_clouds_topic: ~detection_point_clouds
+object_detection_info_topic: ~detection_info

object_detection/cfg/projector_config_smb261.yaml

@@ -0,0 +1,15 @@
+R_camera_lidar :
+  - [-1.0, 0.0, 0.0]
+  - [ 0.0, 0.0,-1.0]
+  - [ 0.0,-1.0, 0.0]
+
+R_correction   :
+  - [ 1.0000000,  0.0000000,  0.0000000]
+  - [ 0.0000000,  1.0000000,  0.0000000]
+  - [ 0.0000000,  0.0000000,  1.0000000]
+
+t_camera_lidar : [-0.045, -0.293, -0.241]
+
+t_correction : [0, 0, 0]
+
+forward_axis : 3  # x = 1, -x = -1, y = 2, -y = -2 , z = 3, -z = -3 

object_detection/cfg/projector_config_smb262.yaml

@@ -0,0 +1,15 @@
+R_camera_lidar :
+  - [-1.0, 0.0, 0.0]
+  - [ 0.0, 0.0,-1.0]
+  - [ 0.0,-1.0, 0.0]
+
+R_correction   :
+  - [ 1.0000000,  0.0000000,  0.0000000]
+  - [ 0.0000000,  1.0000000,  0.0000000]
+  - [ 0.0000000,  0.0000000,  1.0000000]
+
+t_camera_lidar : [-0.045, -0.293, -0.241]
+
+t_correction : [0, 0, 0]
+
+forward_axis : 3  # x = 1, -x = -1, y = 2, -y = -2 , z = 3, -z = -3 

object_detection/cfg/projector_config_smb263.yaml

@@ -0,0 +1,15 @@
+R_camera_lidar :
+  - [-1.0, 0.0, 0.0]
+  - [ 0.0, 0.0,-1.0]
+  - [ 0.0,-1.0, 0.0]
+
+R_correction   :
+  - [ 1.0000000,  0.0000000,  0.0000000]
+  - [ 0.0000000,  1.0000000,  0.0000000]
+  - [ 0.0000000,  0.0000000,  1.0000000]
+
+t_camera_lidar : [-0.045, -0.293, -0.241]
+
+t_correction : [0, 0, 0]
+
+forward_axis : 3  # x = 1, -x = -1, y = 2, -y = -2 , z = 3, -z = -3 

object_detection/launch/debayer.launch

@@ -0,0 +1,12 @@
+<launch>
+  <arg name="input_camera_name"
+       default="/versavis/cam0/slow"
+       doc="Namespace of the camera/image stream that should be debayerd" />
+
+  <node pkg="nodelet" type="nodelet" name="debayer"
+        args="standalone image_proc/debayer">
+    <remap from="image_raw" to="$(arg input_camera_name)/image_raw" />
+    <remap from="image_color" to="$(arg input_camera_name)/image_color" />
+    <remap from="image_mono" to="$(arg input_camera_name)/image_mono" />
+  </node>
+</launch>

object_detection/launch/jetson_env.sh

@@ -0,0 +1,6 @@
+#!/usr/bin/env bash
+
+export LD_PRELOAD=/usr/lib/aarch64-linux-gnu/libgomp.so.1:/usr/lib/aarch64-linux-gnu/libGLdispatch.so.0
+source ~/object_detection_ws/devel/setup.bash
+
+exec "$@"

object_detection/launch/object_detection.launch

@@ -1,55 +1,79 @@
 <launch>
+  <!-- launch file to run object detection:
+       by default, the object detection runs on the jetson xavier GPU
+       while the images are debayered on the local machine. -->
 
-  <arg name="camera_calib_path" default="$(find object_detection)/cfg/smb264_camera_model.yaml"/>
+  <arg  name="smb_name" 
+        default="$(optenv SMB_NAME smb261)"
+        doc="Name of the SMB in the format smb26x (relevant for calibrations)"/>
 
-  <arg name="optical_frame_name" default='blackfly_right_optical_link'/>
-  <arg name="input_camera_name" default="/versavis/cam0" />
-  <arg name="lidar_topic" default='/rslidar_points'/>
+  <arg  name="gpu" 
+        default="remote"
+        doc="Run on GPU? Options: 'local', 'remote' (default), 'off'"/>
 
-  <arg name="object_detection_classes" default="[0]"/>
+  <arg  name="GPU_user" 
+        default="$(env USER)"
+        doc="Username to use on the jetson xavier GPU"/>
+
+  <arg  name="input_camera_name" 
+        default="/versavis/cam0/slow" 
+        doc="Name of the camera, i.e. topic prefix. Relevant for topic subscriptions."/>
+
+  <arg  name="debayer_image" 
+        default="true" 
+        doc="Debayer the images (supplied in $input_camera_name/image_raw)."/>
+
+  <arg  name="lidar_topic" 
+        default='/rslidar_points'
+        doc="Topic containing the point cloud from the lidar."/>
+
+  <arg  name="object_detection_classes" 
+        default="[0,1,11,25,13,74]"
+        doc=  "List of the ids of classes for detection. Based on the COCO dataset, refer here: https://github.com/ethz-asl/darknet_catkin/blob/master/data/coco.names. bNOTICE, class ids start at 0."/>
+
+  <!-- evaluate where to run object detection node (based on argument gpu) -->
+  <arg name="object_detection_host" value="" unless="$(eval arg('gpu')=='remote')" />
+  <arg name="object_detection_host" value="jetson" if="$(eval arg('gpu')=='remote')" />
+
+  <!-- evaluate whether to run object detection on gpu (0) or cpu -->
+  <arg name="object_detection_device" value="0" unless="$(eval arg('gpu')=='off')" />
+  <arg name="object_detection_device" value="cpu" if="$(eval arg('gpu')=='off')" />
 
   <!--Debayer the image-->
-  <node pkg="image_proc" type="image_proc" name="debayer" ns="$(arg input_camera_name)/" />
-
-  <!--Undistored the debayered image-->
-  <!--https://github.com/ethz-asl/image_undistort-->
-  <node name="image_undistort_node" pkg="image_undistort" type="image_undistort_node" output="screen" clear_params="true">
-    <param name="input_camera_namespace" value="versavis"/>      
-    <param name="input_camera_info_from_ros_params" value = "true"/>    
-    <param name="scale" value="1.0"/>
-    <param name="output_camera_info_source" value="auto_generated"/>   <!--auto_generated,match_input--> 
-    <param name="publish_tf" value="false"/>
-    <param name="output_frame" value="$(arg optical_frame_name)"/>
-    <rosparam file="$(arg camera_calib_path)"/>
-    <remap from="input/image" to="$(arg input_camera_name)/image_color"/>
-    <remap from="output/image" to="$(arg input_camera_name)/undistorted"/>
-  </node>
+  <include file="$(find object_detection)/launch/debayer.launch" if="$(eval debayer_image)" >
+    <arg name="input_camera_name" value="$(arg input_camera_name)" />
+  </include>
 
-  <!--Object detection node-->
-  <node name="objectify" pkg="object_detection" type="node.py" output="screen">
+  <!-- object detection launched on jetson 
+       ! Assumes same username as on current host ! -->
+  <machine  name="jetson" 
+            address="jetson-xavier" 
+            env-loader="~/jetson_env.sh" 
+            user="$(arg GPU_user)"
+            default="false"/>
 
+    <node machine="$(arg object_detection_host)" name="object_detector" pkg="object_detection" type="node.py" output="screen">
     <!--Input related-->
-    <param name="camera_topic" value='$(arg input_camera_name)/undistorted'/>
+    <param name="camera_topic" value='$(arg input_camera_name)/image_color'/>
     <param name="camera_info_topic" value='$(arg input_camera_name)/camera_info'/>
     <param name="lidar_topic" value='$(arg lidar_topic)'/>
 
     <!--Output related-->
-    <param name="object_detection_topic" value='/objects'/>
-    <param name="verbose" value='False'/>
-
+    <rosparam file="$(find object_detection)/cfg/output_params.yaml" />
+
     <!--Camera Lidar synchronization related-->
     <param name="camera_lidar_sync_queue_size" value="10"/>
     <param name="camera_lidar_sync_slop" value="0.1"/>
 
-    <!--Config files-->
-    <param name="config_dir" value='$(find object_detection)/cfg/'/>
-
     <!--Point Projector related-->
-    <param name="project_config" value='projector_config_SMB264.yaml'/>
+    <param name="project_config" value='projector_config_$(arg smb_name).yaml'/>
 
     <!--Object detection related-->
-    <param name="model" value='yoLov5l6'/>
-    <param name="device" value='cpu'/>
+    <param name="model" value='yolov5l6'/>
+    <param name="model_path" value='/usr/share/yolo/models'/>
+    <!-- <param name="model_path" value=''/> -->
+    <param name="device" value='$(arg object_detection_device)'/>
+    <!-- <param name="device" value='cpu'/> -->
     <param name="confident" value='0.4'/>
     <param name="iou" value='0.1'/>
     <rosparam param="classes" subst_value="True">$(arg object_detection_classes)</rosparam> >
@@ -60,11 +84,4 @@
     <param name="ground_percentage" value='25'/>
     <param name="bb_contract_percentage" value='10.0'/>
   </node>
-
-  <include file="$(find object_visualization)/launch/object_visualization.launch">
-    <arg name="object_topic" value="/objects"/>
-    <arg name="visualize_all" value='False'/>
-    <arg name="only_BB" value='False'/>
-  </include>
-
-</launch>
+</launch>