Add TimeStamp xavier and local

Author: rorosi

CHANGELOG.rst

@@ -2,11 +2,16 @@
 Changelog for package clpe_ros
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.1.1 (2022-08-24)
+------------------
+* remove EEPROM
+
 0.1.0 (2022-04-20)
 ------------------
-* Provides ROS 1 Noetic driver for CANLAB CLPE-G-NVP2650D vision system
+* Provides ROS 2 driver for CANLAB CLPE-G-NVP2650D vision system
+* Support ROS 2 Foxy and ROS 2 Galactic
 * By default the driver will publish three topics for each camera X.
-  * /clpe_ros/cam_X/image_raw: The raw image published as sensor_msgs::msg::Image. The default encoding is yuv422. For other supported encodings, see Configuration.
-  * /clpe_ros/cam_X/camera_info: The intrinsics of the camera published as a sensor_msgs::msg::CameraInfo message.
-  * /clpe_ros/cam_X/clpe_camera_info: The intrinsics of the camera published as a clpe_ros_msgs::ClpeCameraInfo message.
-* Contributors: Teo Koon Peng, Yadunund Vijay
+  * /clpe/cam_X/image_raw: The raw image published as sensor_msgs::msg::Image. The default encoding is yuv422. For other supported encodings, see Configuration.
+  * /clpe/cam_X/camera_info: The intrinsics of the camera published as a sensor_msgs::msg::CameraInfo message.
+  * /clpe/cam_X/clpe_camera_info: The intrinsics of the camera published as a clpe_ros_msgs::msg::ClpeCameraInfo message.
+* Contributors: Teo Koon Peng, Yadunund Vijay

CMakeLists.txt

@@ -5,51 +5,88 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-find_package(catkin REQUIRED)
+find_package(ament_cmake REQUIRED)
 find_package(clpe REQUIRED)
-find_package(roscpp REQUIRED)
+find_package(rclcpp REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(clpe_ros_msgs REQUIRED)
 
-catkin_package(DEPENDS roscpp image_transport clpe_ros_msgs)
-
 add_library(${PROJECT_NAME}_lib INTERFACE)
 target_include_directories(${PROJECT_NAME}_lib INTERFACE
-  ${roscpp_INCLUDE_DIRS}
   ${geometry_msgs_INCLUDE_DIRS}
   ${sensor_msgs_INCLUDE_DIRS}
   ${image_transport_INCLUDE_DIRS}
   ${cv_bridge_INCLUDE_DIRS}
   ${clpe_ros_msgs_INCLUDE_DIRS}
 )
 target_link_libraries(${PROJECT_NAME}_lib INTERFACE
-  clpe
-  ${roscpp_LIBRARIES}
+  clpe::clpe
+  rclcpp::rclcpp
   ${geometry_msgs_LIBRARIES}
   ${sensor_msgs_LIBRARIES}
   ${image_transport_LIBRARIES}
   ${cv_bridge_LIBRARIES}
   ${clpe_ros_msgs_LIBRARIES}
 )
 target_compile_features(${PROJECT_NAME}_lib INTERFACE cxx_std_14)
-
 add_executable(${PROJECT_NAME}
   src/main.cpp)
 target_link_libraries(${PROJECT_NAME} PRIVATE ${PROJECT_NAME}_lib)
 install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION lib/${PROJECT_NAME})
 
-install(DIRECTORY launch
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-        PATTERN "*~" EXCLUDE)
-install(DIRECTORY config
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-        PATTERN "*~" EXCLUDE)
+###############################################################################
+# ROS2 component
+###############################################################################
+
+include(GenerateExportHeader)
+find_package(rclcpp_components REQUIRED)
+
+# Create shared library
+add_library(${PROJECT_NAME}_component SHARED
+  src/component.cpp)
+target_compile_definitions(${PROJECT_NAME}_component
+  PRIVATE "COMPOSITION_BUILDING_DLL")
+target_include_directories(${PROJECT_NAME}_component PRIVATE
+  ${CMAKE_CURRENT_BINARY_DIR}
+  ${rclcpp_components_INCLUDE_DIRS})
+target_link_libraries(${PROJECT_NAME}_component
+  ${PROJECT_NAME}_lib
+  ${rclcpp_components_LIBRARIES})
+set_target_properties(${PROJECT_NAME}_component PROPERTIES
+  CXX_VISIBILITY_PRESET hidden
+  VISIBILITY_INLINES_HIDDEN ON)
+
+# Generate export headers
+GENERATE_EXPORT_HEADER(${PROJECT_NAME}_component)
+
+# register component
+rclcpp_components_register_nodes(${PROJECT_NAME}_component "clpe::ClpeComponentNode")
+set(node_plugins "${node_plugins}clpe::ClpeComponentNode;$<TARGET_FILE:${PROJECT_NAME}_component>\n")
+file(GENERATE
+  OUTPUT
+    "${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$<CONFIG>/share/ament_index/resource_index/node_plugin/${PROJECT_NAME}"
+  CONTENT "${node_plugins}")
+
+# install target
+install(TARGETS ${PROJECT_NAME}_component)
+
+###############################################################################
 
 option(CLPE_ROS_BUILD_BENCHMARKS "build benchmarks")
 if (${CLPE_ROS_BUILD_BENCHMARKS})
   add_subdirectory(benchmarks)
 endif()
+
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME})
+
+install(
+  FILES fastdds.xml
+  DESTINATION share/${PROJECT_NAME})
+
+ament_package()

README.md

@@ -4,86 +4,121 @@ This repository contains ROS 1 and ROS 2 drivers for the CANLAB CLPE-G-NVP2650D
 CLPE-G-NVP2650D is a multi camera grabber board for N2.0 GMSL camera.
 This drivers exposes camera images and information from CLPE-G-NVP2650D as ROS messages using the image_transport framework.
 
-**This branch contains the `ROS 1` driver. For the `ROS 2` driver, please switch to the `main` branch.**
+**This branch contains the `ROS 2` driver. For the `ROS 1` driver, please switch to the `noetic` branch.**
 
 # System Requirements
 
 Requirements:
   * CANLAB CLPE-G-NVP2650D with supplied PC running [Ubuntu 20.04](https://releases.ubuntu.com/20.04/)
   * [CLPE_G_NVP2650D_SDK](https://github.com/canlab-co/CLPE_G_NVP2650D_SDK)
-  * [ROS 1 Noetic](http://wiki.ros.org/noetic)
+  * [ROS 2 Foxy](https://docs.ros.org/en/foxy/index.html) or [ROS 2 Galactic](https://docs.ros.org/en/galactic/index.html)
 
-> Note: It is strongly recommended to install `Ubuntu 20.04` on the PC shipped with CLPE-G-NVP2650D. This way ROS 1 binaries can be [installed as debian packages](http://wiki.ros.org/noetic/Installation/Ubuntu). If the PC is running `Ubuntu 18.04`, ROS 1 will need to be [built from source](http://wiki.ros.org/noetic/Installation/Source) which is not officially supported on `Ubuntu 18.04`.
+> Note: It is strongly recommended to install `Ubuntu 20.04` on the PC shipped with CLPE-G-NVP2650D. This way ROS 2 binaries can be [installed as debian packages](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html). If the PC is running `Ubuntu 18.04`, ROS 2 will need to be [built from source](https://docs.ros.org/en/foxy/Installation/Ubuntu-Development-Setup.html) which is not officially supported on `Ubuntu 18.04`.
 Alternatively, containers or VM may be used but the CLPE-G-NVP2650D drivers has to be properly passed through.
 
+
 # Setup
 
 ## System dependencies
 ```bash
 sudo apt update && sudo apt install git cmake wget gcc python3-colcon* python3-rosdep python3-vcstool -y
 ```
 
-## ROS 1 Installation
-Depending on the version of Ubuntu installed, follow the instructions in the links above to install ROS 1 Noetic binaries or from source.
-To source ROS 1
+## ROS 2 Installation
+Depending on the version of Ubuntu installed, follow the instructions in the links above to install ROS 2 binaries or from source.
+To source ROS 2
 ```bash
-source /opt/ros/noetic/setup.bash # if binaries are installed
-source ~/ros1_noetic/install/setup.bash # if installed from source following link above
+source /opt/ros/foxy/setup.bash # if binaries are installed
+source ~/ros2_foxy/install/setup.bash # if installed from source following link above
 ```
+> Note: Replace `foxy` with `galactic` if using ROS 2 Galactic
 
-## Install CLPE SDK and ROS 1 Driver
+## Install CLPE SDK and ROS 2 Driver
 Create workspace
 ```bash
 mkdir -p ~/ws_clpe/src
 cd ~/ws_clpe/
-wget https://raw.githubusercontent.com/canlab-co/clpe_ros/noetic-AP0202/clpe.repos
+wget https://raw.githubusercontent.com/canlab-co/clpe_ros/main/clpe.repos
 vcs import src < clpe.repos
 ```
 
 Install dependencies
 
 ```bash
 cd ~/ws_clpe
+# source your ROS 2 workspace before this command
 sudo rosdep init # if you have not done this before
 rosdep update
-rosdep install --from-paths src --ignore-src --rosdistro noetic -y
+rosdep install --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y
 ```
+
 Build the driver
 
 ```bash
 cd ~/ws_clpe
-# source your ROS 1 workspace if you have not already
-catkin_make install --cmake-args -DCMAKE_BUILD_TYPE=Release
+# source your ROS 2 workspace if you have not already
+colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
 # To also build the benchmarking scripts
-catkin_make install --cmake-args -DCMAKE_BUILD_TYPE=Release -DCLPE_ROS_BUILD_BENCHMARKS=On
+colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DCLPE_ROS_BUILD_BENCHMARKS=On
 ```
 
 # Run the driver
 
+## DDS Configuration
+For better image transport performance over DDS, we recommend using [FastDDS](https://github.com/eProsima/Fast-DDS) with Shared Memory Transport enabled.
+First copy the the `fastdds.xml` config file to a suitable directory, eg. `$HOME/fastdds.xml`
+```bash
+cd ~/ws_clpe/src
+cp canlab/fastdds.xml ~/
+```
+
+Next add these two lines to your `~/.bashrc`
+```bash
+export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
+export FASTRTPS_DEFAULT_PROFILES_FILE=$HOME/fastdds.xml
+```
+
+Make sure to `source ~/.bashrc` first on all terminals before launching any ROS 2 nodes including the driver.
+
+## Launch
+
 ```bash
 cd ~/ws_clpe
 source install/setup.bash
-roslaunch clpe_ros clpe_ros.launch password:=<sudo-password> encoding:=yuv422
+ros2 launch clpe_ros clpe_ros.launch.py password:=<sudo-password> encoding:=yuv422
 ```
 
-> Note: If `sudo-password` is a number, say `42`, you should pass it as `password:=\'42\'`
+> Note: If `sudo_password` is a number, say `42`, you should pass it as `password:=\'42\'`
+
+By default the driver will publish three topics per camera X.
+* /clpe/cam_X/image_raw: The raw image published as `sensor_msgs::msg::Image`. The default encoding is `yuv422`. For other supported encodings, see Configuration below.
+* /clpe/cam_X/camera_info: The intrinsics of the camera published as a `sensor_msgs::msg::CameraInfo` message.
+* /clpe/cam_X/clpe_camera_info: The intrinsics of the camera published as a clpe_ros_msgs::msg::ClpeCameraInfo message.
 
-By default the driver will publish two topics per camera (X).
-* /clpe_ros/cam_X/image_raw: The raw image published as `sensor_msgs::Image`. The default encoding is `yuv422`. For other supported encodings, see Configuration below.
 
-## Visualizing in Rviz
+## ROS2 Component
 
-By default, images are published in yuv encoding. Rviz does not support yuv so it needs to be converted to a different format.
+The driver is also supported as a ROS2 component. To run it, start the component container
 
 ```bash
-roslaunch clpe_ros clpe_ros.launch password:=<sudo-password> encoding:=bgr8
+ros2 run rclcpp_components component_container
 ```
 
-Note that this conversion incurs significant overhead so should be avoided if performance is important.
+In another terminal, load the component
+
+```bash
+ros2 component load /ComponentManager clpe_ros clpe::ClpeComponentNode -p password:=<password>
+```
+
+Check that the component is loaded with
+
+```bash
+ros2 component list
+```
 
 # Configuration
 
-The driver supports the following ROS parameters to configure its behavior. The `password` and `encoding` parameters can be overwritten at run time. Other parameters can be modified within the [config file](config/clpe_config.yaml)
+The driver supports the following ROS parameters to configure its behavior. The `password` and `encoding` parameters can be overwritten at run time. Other parameters can be modified within the [launch file](launch/clpe_ros.launch.py).
 
 | Key | Description | Default |
 |-|-|-|
@@ -92,9 +127,7 @@ The driver supports the following ROS parameters to configure its behavior. The
 | cam_{n}_enable | Enable camera | true |
 | cam_{n}_pose | Pose relative to the base, 6 values corresponding to [x, y, z, roll, pitch, yaw] | [0,0,0,0,0,0] |
 | cam_{n}_frame_id | Defines the frame_id all static transformations refers to | base_link |
-| cam_{n}_image_latch | Controls latching for camera image messages | false |
-| cam_{n}_image_queue_size | Controls queue size for camera image messages | 10 |
-| cam_{n}_info_latch | Controls latching for camera info messages | false |
-| cam_{n}_info_queue_size | Controls queue size for camera info messages | 10 |
+| cam_{n}_image_qos | Sets the QoS by which the topic is published. Available values are the following strings: SYSTEM_DEFAULT, PARAMETER_EVENTS, SERVICES_DEFAULT, PARAMETERS, DEFAULT, SENSOR_DATA, HID_DEFAULT (= DEFAULT with depth of 100), EXTRINSICS_DEFAULT (= DEFAULT with depth of 1 and transient local durabilty). | SENSOR_DATA |
+| cam_{n}_info_qos | Sets the QoS by which the topic is published. Available values are the following strings: SYSTEM_DEFAULT, PARAMETER_EVENTS, SERVICES_DEFAULT, PARAMETERS, DEFAULT, SENSOR_DATA, HID_DEFAULT (= DEFAULT with depth of 100), EXTRINSICS_DEFAULT (= DEFAULT with depth of 1 and transient local durabilty). | SYSTEM_DEFAULT |
 
 Unless specified otherwise, all parameters are read-only, that is, they must be initialized at startup and cannot be changed at runtime.

benchmarks/CMakeLists.txt

@@ -1,6 +1,6 @@
 add_executable(latency latency.cpp)
-target_include_directories(latency PRIVATE ${roscpp_INCLUDE_DIRS} ${sensor_msgs_INCLUDE_DIRS})
-target_link_libraries(latency PRIVATE ${roscpp_LIBRARIES} ${sensor_msgs_LIBRARIES})
+target_include_directories(latency PRIVATE ${sensor_msgs_INCLUDE_DIRS})
+target_link_libraries(latency PRIVATE rclcpp::rclcpp ${sensor_msgs_LIBRARIES})
 # always build in release mode
 separate_arguments(BUILD_FLAGS NATIVE_COMMAND ${CMAKE_CXX_FLAGS_RELEASE})
 target_compile_options(latency PRIVATE ${BUILD_FLAGS})

benchmarks/latency.cpp

@@ -13,49 +13,47 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  *
- */
-
-#include <ros/ros.h>
+*/
 
+#include <builtin_interfaces/msg/time.hpp>
 #include <iomanip>
-#include <sensor_msgs/Image.h>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
 
 //==============================================================================
-int main(int argc, char* argv[])
+int main(int argc, char * argv[])
 {
-  ros::init(argc, argv, "clpe");
+  rclcpp::init(argc, argv);
 
   const std::size_t cam_count = 4;
 
-  auto node = ros::NodeHandle("canlab_benchmark_latency");
+  auto node = rclcpp::Node::make_shared("canlab_benchmark_latency");
   std::array<std::size_t, cam_count> cam_sub_count;
   // Latency in millis
   std::array<double, cam_count> avg_latencies;
-  for (auto i = 0; i < cam_count; ++i)
-  {
+  for (auto i = 0; i < cam_count; ++i) {
     cam_sub_count[i] = 1;
     avg_latencies[i] = 0.0;
   }
 
-  std::array<ros::Subscriber, cam_count> subs;
-  using sensor_msgs::Image;
-  for (int i = 0; i < cam_count; ++i)
-  {
-    subs[i] = node.subscribe<Image>("/clpe_node/cam_" + std::to_string(i) + "/image_raw", 10,
-                                    [i, &node, &cam_sub_count, &avg_latencies](const Image::ConstPtr& image) {
-                                      const auto local_time = ros::Time::now().toNSec();
-                                      const auto cam_time = image->header.stamp.toNSec();
-                                      const auto diff_ns = local_time - cam_time;
-                                      const double latency = diff_ns / 1000000.0;
-                                      avg_latencies[i] =
-                                          (avg_latencies[i] * cam_sub_count[i] + latency) / (cam_sub_count[i] + 1);
-                                      cam_sub_count[i]++;
-                                      std::cout << "cam_" << i << " latency = " << std::fixed << std::setprecision(3)
-                                                << avg_latencies[i] << "ms" << std::endl;
-                                    });
+  std::array<rclcpp::SubscriptionBase::SharedPtr, cam_count> subs;
+  using sensor_msgs::msg::Image;
+  for (int i = 0; i < cam_count; ++i) {
+    subs[i] = node->create_subscription<Image>(
+      "cam_" + std::to_string(i) + "/image_raw", rclcpp::SystemDefaultsQoS(),
+      [i, &node, &cam_sub_count, &avg_latencies](const Image::SharedPtr image) {
+        const auto local_time = node->get_clock()->now().nanoseconds();
+        const auto cam_time = rclcpp::Time(image->header.stamp).nanoseconds();
+        const auto diff_ns = local_time - cam_time;
+        const double latency = diff_ns / 1000000.0;
+        avg_latencies[i] = (avg_latencies[i] * cam_sub_count[i] + latency) / (cam_sub_count[i] + 1);
+        cam_sub_count[i]++;
+        std::cout << "cam_" << i << " latency = " << std::fixed << std::setprecision(3) <<
+          avg_latencies[i] << "ms" << std::endl;
+      });
   }
 
-  ros::spin();
+  rclcpp::spin(node);
 
   return 0;
 }

clpe.repos

@@ -1,13 +1,13 @@
 repositories:
-  canlab-co/CLPE_G_AP0202_SDK:
+  canlab-co/CLPE_G_NVP2650D_SDK:
     type: git
-    url: https://github.com/canlab-co/CLPE_G_AP0202_SDK.git
-    version: noetic
+    url: https://github.com/canlab-co/CLPE_G_NVP2650D_SDK.git
+    version: main
   canlab-co/clpe_ros_msgs:
     type: git
     url: https://github.com/canlab-co/clpe_ros_msgs.git
-    version: noetic
+    version: main
   canlab-co/clpe_ros:
     type: git
     url: https://github.com/canlab-co/clpe_ros.git
-    version: noetic-AP0202
+    version: main