PickNikRobotics · eholum · Nov 9, 2023 · Nov 1, 2023 · Nov 1, 2023 · Nov 1, 2023
diff --git a/.gitignore b/.gitignore
@@ -1,3 +1,8 @@
 *.bin
 *.ppm
 *.bag
+
+# Ignore build artifacts
+build/*
+install/*
+log/*
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,60 +1,90 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.14)
 
 project(kinova_vision)
 
-# System Dependencies
-find_package(PkgConfig)
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
 
-pkg_check_modules(GSTREAMER REQUIRED gstreamer-1.0)
-pkg_check_modules(GST_APP REQUIRED gstreamer-app-1.0)
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra)
+endif()
 
-# Use Catkin
-find_package(catkin REQUIRED
-  COMPONENTS roscpp image_transport sensor_msgs nodelet
-  camera_calibration_parsers camera_info_manager tf
-  )
+# Ament System Dependencies
+find_package(ament_cmake REQUIRED)
+find_package(camera_calibration_parsers REQUIRED)
+find_package(camera_info_manager REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
 
-catkin_package(
-  INCLUDE_DIRS include
-  LIBRARIES KinovaVision
-  CATKIN_DEPENDS roscpp nodelet image_transport sensor_msgs
- camera_calibration_parsers camera_info_manager
-  DEPENDS GSTREAMER GST_APP
-  )
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  ament_cmake
+  camera_calibration_parsers
+  camera_info_manager
+  cv_bridge
+  image_transport
+  rclcpp
+  rclcpp_components
+  sensor_msgs
+  tf2
+  tf2_ros
+)
 
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
-  ${GLIB_INCLUDE_DIRS}
-  ${GST_APP_INCLUDE_DIRS})
-
-add_library(KinovaVision src/vision.cpp)
-target_link_libraries(KinovaVision
-  ${catkin_LIBRARIES}
-  ${GSTREAMER_LIBRARIES}
-  ${GST_APP_LIBRARIES})
-
-add_executable(KinovaVision_node src/vision_node.cpp)
-target_link_libraries(KinovaVision_node KinovaVision
-  ${catkin_LIBRARIES}
-  ${GSTREAMER_LIBRARIES}
-  ${GST_APP_LIBRARIES})
-set_target_properties(KinovaVision_node PROPERTIES OUTPUT_NAME kinova_vision)
+# Other Dependencies
+find_package(PkgConfig)
+pkg_check_modules(GSTREAMER REQUIRED
+  gstreamer-1.0
+  gstreamer-app-1.0
+)
+
+# Define targets
+include_directories(include)
+include_directories(${GSTREAMER_INCLUDE_DIRS})
+
+add_library(${PROJECT_NAME} SHARED src/vision.cpp)
+ament_target_dependencies(${PROJECT_NAME}
+  ${THIS_PACKAGE_INCLUDE_DEPENDS})
+target_link_libraries(${PROJECT_NAME}
+  "${GSTREAMER_LIBRARIES}")
+
+add_executable(${PROJECT_NAME}_node src/vision_node.cpp)
+ament_target_dependencies(${PROJECT_NAME}_node
+  ${THIS_PACKAGE_INCLUDE_DEPENDS})
+target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME})
 
 # Install directives
-install(TARGETS KinovaVision KinovaVision_node
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+install(TARGETS ${PROJECT_NAME} 
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib
 )
 
-install(DIRECTORY include/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-  PATTERN ".svn" EXCLUDE)
+install(TARGETS ${PROJECT_NAME}_node
+  ARCHIVE DESTINATION lib/${PROJECT_NAME}
+  LIBRARY DESTINATION lib/${PROJECT_NAME}
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
 
-install(DIRECTORY launch/calibration
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
-  FILES_MATCHING PATTERN "*.ini"
-  )
+# Include additional settings if needed
+set(CMAKE_CXX_FLAGS "-fpermissive")
 
-set(CMAKE_CXX_FLAGS "-fpermissive")
+## EXPORTS
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+ament_export_dependencies(
+)
+ament_package()
diff --git a/README.md b/README.md
@@ -1,11 +1,11 @@
 # Kinova Vision module package
 
 ## Overview
-This ROS package provides helper methods and launch scripts to access the Kinova Vision module depth and color streams.
+This ROS 2 package provides helper methods and launch scripts to access the Kinova Vision module depth and color streams.
 
 
-## Installation (using catkin)
-The following instructions are for ROS Kinetic Kame, running under **Ubuntu 16.04**
+## Installation (using colcon)
+The following instructions are for ROS 2, tested on Humble on Ubuntu 22.02.
 
 ### Building from Source
 
@@ -35,104 +35,40 @@ sudo apt-get install ros-kinetic-rgbd-launch
 
 To build from source, clone the latest version from this repository into your catkin workspace and compile the package.
 
-1. Create a catkin workspace
+1. Create a workspace
 ```bash
-mkdir -p ~/catkin_ws/src
-cd ~/catkin_ws/src/
+mkdir -p ~/colcon_ws/src
+cd ~/colcon_ws/src/
 ```
-2. Clone this git repo into `~/catkin_ws/src`
+2. Clone this git repo into `~/colcon_ws/src`
 ```bash
-git clone https://github.com/Kinovarobotics/ros_kortex_vision.git
+git clone -b ros2 https://github.com/PickNikRobotics/ros2_kortex_vision.git
 ```
 ```bash
-cd ~/catkin_ws/src/
-catkin_init_workspace 
-cd ..
-catkin_make clean
-catkin_make
-catkin_make install
-echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
-source ~/.bashrc
+cd ~/colcon_ws/
+colcon build
 ```
 
 ## Usage
 
-### Start roscore (if not already started)
-```bash
-roscore
-```
-
 ### Start kinova_vision node
 ```bash
-roslaunch kinova_vision kinova_vision.launch
+source ~/colcon_ws/setup.bash
+ros2 launch kinova_vision kinova_vision.launch.py 
 ```
-Refer to the [Launch files](#launch_files) section to see the available launch files.
-
-### Start rviz to view both cameras
+The launch file provides arguments for launching depth, color, or registered depth images, as well as overriding other parameters.
+For example, to only launch the color node,
 
 ```bash
-rosrun rviz rviz
+source ~/colcon_ws/setup.bash
+ros2 launch kinova_vision kinova_vision.launch.py launch_depth:=false
 ```
 
-Refer to the [Rviz configuration files](#rviz_config_files) section to see the configuration files for viewing streams in rviz.
-
-### Alternatively, use [image_view] to view a camera stream:
-
-```bash
-rosrun image_view image_view image:=/camera/color/image_raw
-rosrun image_view image_view image:=/camera/depth/image_raw
-```
-
-The `image` argument specifies the image topic to subscribe to.
-
-Refer to the [Nodes](#nodes) section to see the published topics using the message type [sensor_msgs/Image].
-
-<a name="launch_files"></a>
-## Launch files
-
-* **kinova_vision.launch:** Connect to both the color and the depth camera streams and publish their images as well as the depth point cloud data.
-
-     arguments:
-
-    - **`device`** device IP address (default: `192.168.1.10`)
-    - **`num_worker_threads`** number of worker threads for the nodelet manager (default: `4`)
-    - **`camera_link_frame_id`** camera link frame identifier (default: `camera_link`)
-    - **`color_frame_id`** color camera frame identifier (default: `camera_color_frame`)
-    - **`depth_frame_id`** depth camera frame identifier (default: `camera_depth_frame`)
-    - **`color_camera_info_url`** URL of color camera custom calibration file (see [camera_info_manager] documentation for calibration URL details)
-    - **`depth_camera_info_url`** URL of depth camera custom calibration file (see [camera_info_manager] documentation for calibration URL details)
-
-* **kinova_vision_rgbd.launch:** Connect to both the color and the depth camera streams and publish their images as well as the depth point cloud data with color information.
-
-     arguments:
-
-    - **`device`** device IP address (default: `192.168.1.10`)
-    - **`num_worker_threads`** number of worker threads for the nodelet manager (default: `4`)
-    - **`camera_link_frame_id`** camera link frame identifier (default: `camera_link`)
-    - **`color_frame_id`** color camera frame identifier (default: `camera_color_frame`)
-    - **`depth_frame_id`** depth camera frame identifier (default: `camera_depth_frame`)
-    - **`color_camera_info_url`** URL of color camera custom calibration file (see [camera_info_manager] documentation for calibration URL details)
-    - **`depth_camera_info_url`** URL of depth camera custom calibration file (see [camera_info_manager] documentation for calibration URL details)
-
-* **kinova_vision_color_only.launch:** Connect to the color camera stream only and publish its images.
-
-     arguments
-
-    - **`device`** device IP address (default: `192.168.1.10`)
-    - **`num_worker_threads`** number of worker threads for the nodelet manager (default: `4`)
-    - **`camera_link_frame_id`** camera link frame identifier (default: `camera_link`)
-    - **`color_frame_id`** color camera frame identifier (default: `camera_color_frame`)
-    - **`color_camera_info_url`** URL of color camera custom calibration file (see [camera_info_manager] documentation for calibration URL details)
-
-* **kinova_vision_depth_only.launch:** Connect to the depth camera stream only and publish its images as well as the depth point cloud data.
+Additional information is available below.
 
-     arguments
+### Start rviz to view both cameras
 
-    - **`device`** device IP address (default: `192.168.1.10`)
-    - **`num_worker_threads`** number of worker threads for the nodelet manager (default: `4`)
-    - **`camera_link_frame_id`** camera link frame identifier (default: `camera_link`)
-    - **`depth_frame_id`** depth camera frame identifier (default: `camera_depth_frame`)
-    - **`depth_camera_info_url`** URL of depth camera custom calibration file (see [camera_info_manager] documentation for calibration URL details)
+*TODO* Add `rviz2` save configurations and instructions for viewing camera streams.
 
 ### Specifying launch options
 It's possible to override the default argument values when launching the **kinova_vision** node.
@@ -141,8 +77,9 @@ Arguments are set using the following syntax: `<argument>:=<value>`.
 
 For instance, the default value of the `device` argument can be overridden to specify another IP address.
 ```bash
-roslaunch kinova_vision kinova_vision_rgbd.launch device:=10.20.0.100
+ros2 launch kinova_vision kinova_vision.launch.py device:=10.20.0.100
 ```
+
 #### Additional information on arguments `color_camera_info_url` and `depth_camera_info_url`
 
 These arguments specify the custom camera information file to use instead of the default camera information file.
@@ -155,7 +92,7 @@ The file is specified via a specific URL syntax, using either of these two forma
 
 For example:
 ```bash
-roslaunch kinova_vision kinova_vision_rgbd.launch color_camera_info_url:=file:///home/user/custom_color_calib_1280x720.ini depth_camera_info_url:=file:///home/user/custom_depth_calib_480x270.ini
+ros2 launch kinova_vision kinova_vision.launch.py color_camera_info_url:=file:///home/user/custom_color_calib_1280x720.ini depth_camera_info_url:=file:///home/user/custom_depth_calib_480x270.ini
 ```
 
 A custom camera information file is typically created from a default information file (refer to *launch/calibration/default_\*.ini*). Then, one simply needs to adjust the proper matrices.
@@ -175,25 +112,6 @@ FX 0.00000 PPX 0.00000
 
 The values for `FX`, `FY`, `PPX`, `PPY` can be obtained via the Vision module API. They represent the _focal length_ and the _principal point offset_ in both the _x_ and _y_ coordinates.
 
-<a name="rviz_config_files"></a>
-## Rviz configuration files
-
-* **color_only.rviz:** View the images coming from the color camera only. The package needs to be launched with *kinova_vision_color_only.launch*, *kinova_vision.launch* or *kinova_vision_rgbd.launch*.
-
-<p align="left"> <img alt="color_only.rviz.png" src="doc/color_only.rviz.png" title="Color only"/> </p>
-
-* **depth_only.rviz:** View the images and the depth cloud coming from the depth camera only. The package needs to be launched with *kinova_vision_depth_only.launch*, *kinova_vision.launch* or *kinova_vision_rgbd.launch*.
-
-<p align="left"> <img alt="depth_only.rviz.png" src="doc/depth_only.rviz.png" title="Depth only"/> </p>
-
-* **depth_and_color.rviz:** View the images coming from the color camera as well as the images and the depth cloud coming from the depth camera. The package needs to be launched with *kinova_vision.launch* or *kinova_vision_rgbd.launch*.
-
-<p align="left"> <img alt="depth_and_color.rviz.png" src="doc/depth_and_color.rviz.png" title="Depth and color"/> </p>
-
-* **depth_and_color_rgbd.rviz:** View the images coming from the color camera as well as the images, the RGBD point cloud and the depth cloud coming from the depth camera. The package needs to be launched with *kinova_vision_rgbd.launch*.
-
-<p align="left"> <img alt="depth_and_color_rgbd.rviz.png" src="doc/depth_and_color_rgbd.rviz.png" title="Depth and color with rgbd"/> </p>
-
 <a name="nodes"></a>
 ## Nodes
 
@@ -261,10 +179,14 @@ None
 
     Depth camera frame static transformation
 
-
 ### camera_nodelet_manager
 
-This node uses [rgbd_launch] package to create a nodelet graph, transforming raw data from the device driver into point clouds, rectified images, and other products suitable for processing and visualization.
+This node uses the [image_proc] package to create a nodelet graph, transforming raw data from the device driver into point clouds, rectified images, and other products suitable for processing and visualization.
+To include these,
+
+```bash
+ros2 launch kinova_vision kinova_vision.launch.py depth_registration:=true
+```
 
 #### Subscribed Topics
 
@@ -286,27 +208,15 @@ This node uses [rgbd_launch] package to create a nodelet graph, transforming raw
 
 #### Published Topics
 
-* **`/camera/color/image_rect_color`** ([sensor_msgs/Image])
-
-    Color rectified image (RGB8 encoding)
-
-* **`/camera/depth/image`** ([sensor_msgs/Image])
+* **`/camera/depth_registered/camera_info`** ([sensor_msgs/CameraInfo])
 
-    Depth image (meters - 32FC1 encoding)
-
-* **`/camera/depth/image_rect`** ([sensor_msgs/Image])
-
-    Depth rectified image (meters - 32FC1 encoding)
+    Color camera calibration and meta information
 
-* **`/camera/depth/image_rect_raw`** ([sensor_msgs/Image])
+* **`/camera/depth_registered/image_rect`** ([sensor_msgs/Image])
 
     Depth rectified image (millimeters - 16UC1 encoding)
 
-* **`/camera/depth/points`** ([sensor_msgs/PointCloud2])
-
-    Depth camera point cloud data
-
-* **`/camera/depth_registered/points`** ([sensor_msgs/PointCloud2])
+* **`/camera/depth/color/points`** ([sensor_msgs/PointCloud2])
 
     Depth camera point cloud data with color information (RGBD)
 
@@ -317,4 +227,4 @@ This node uses [rgbd_launch] package to create a nodelet graph, transforming raw
 [sensor_msgs/Image]: http://docs.ros.org/api/sensor_msgs/html/msg/Image.html
 [sensor_msgs/PointCloud2]: http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html
 [tf2_msgs/TFMessage]: http://docs.ros.org/api/tf2_msgs/html/msg/TFMessage.html
-[rgbd_launch]: http://wiki.ros.org/rgbd_launch
+[image_proc]: http://wiki.ros.org/image_proc
diff --git a/include/constants.h b/include/constants.h