orbbec · ArendJan · Jan 3, 2024 · Jan 3, 2024 · Jan 3, 2024 · Jul 4, 2024
diff --git a/README.MD b/README.MD
@@ -16,7 +16,7 @@ official [ROS 2 installation guide](https://docs.ros.org/en/galactic/Installatio
 ```bash
  Assuming you have sourced the ros environment, same below
 sudo apt install libgflags-dev  ros-$ROS_DISTRO-image-geometry ros-$ROS_DISTRO-camera-info-manager\
-ros-$ROS_DISTRO-image-transport ros-$ROS_DISTRO-image-publisher libgoogle-glog-dev libusb-1.0-0-dev libeigen3-dev
+ ros-$ROS_DISTRO-image-transport ros-$ROS_DISTRO-image-publisher libgoogle-glog-dev libusb-1.0-0-dev libeigen3-dev nlohmann-json3-dev
 
 ```
 

diff --git a/astra_camera/CMakeLists.txt b/astra_camera/CMakeLists.txt
@@ -12,6 +12,26 @@ if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif ()
 
+# Some includes changed between Humble and Jammy, so this next code checks for the ROS version to define a macro variable
+# in the Cpp/Hpp files, the correct header files can be included based on this macro
+set(ROS_FOUND FALSE)
+if(DEFINED ENV{ROS_DISTRO})
+  set(FOUND_ROS_DISTRO $ENV{ROS_DISTRO})
+  set(ROS_FOUND TRUE)
+endif()
+if(ROS_FOUND)
+  # message("Using ROS distro: " $ENV{ROS_DISTRO}) 
+  if($ENV{ROS_DISTRO} STREQUAL "humble")
+    add_compile_definitions(ROS2_HUMBLE)
+  elseif($ENV{ROS_DISTRO} STREQUAL "jammy")
+    add_compile_definitions(ROS2_JAMMY)
+  endif()
+else()
+  message("ROS distro is unknown. Compilation might fail. Fallback to humble")
+  add_compile_definitions(ROS2_HUMBLE)
+endif()
+
+
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake REQUIRED)

diff --git a/astra_camera/include/astra_camera/constants.h b/astra_camera/include/astra_camera/constants.h
@@ -14,6 +14,7 @@
 
 #include <string>
 #include <cstdlib>
+#include <cstdint>
 
 #define OB_ROS_MAJOR_VERSION 1
 #define OB_ROS_MINOR_VERSION 1

diff --git a/astra_camera/include/astra_camera/ob_camera_node.h b/astra_camera/include/astra_camera/ob_camera_node.h
@@ -16,7 +16,11 @@
 
 
 #include <opencv2/opencv.hpp>
+#if defined(ROS2_HUMBLE)
 #include <cv_bridge/cv_bridge.h>
+#elif defined(ROS2_JAMMY)
+#include <cv_bridge/cv_bridge.hpp>
+#endif
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 #include <tf2_ros/static_transform_broadcaster.h>
@@ -219,8 +223,7 @@ class OBCameraNode {
   std::map<stream_index_pair, openni::VideoMode> stream_video_mode_;
   std::map<stream_index_pair, std::vector<openni::VideoMode>> supported_video_modes_;
   std::map<stream_index_pair, int> unit_step_size_;
-  std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr>
-      image_publishers_;
+  std::map<stream_index_pair, image_transport::Publisher> image_publishers_;
   std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>
       camera_info_publishers_;
 

diff --git a/astra_camera/include/astra_camera/point_cloud_proc/point_cloud_xyz.h b/astra_camera/include/astra_camera/point_cloud_proc/point_cloud_xyz.h
@@ -34,7 +34,11 @@
 #include <rclcpp/rclcpp.hpp>
 #include <image_transport/image_transport.hpp>
 #include <sensor_msgs/image_encodings.hpp>
-#include <image_geometry/pinhole_camera_model.h>
+#if defined(ROS2_HUMBLE)
+  #include <image_geometry/pinhole_camera_model.h>
+#elif defined(ROS2_JAMMY)
+  #include <image_geometry/pinhole_camera_model.hpp>
+#endif
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 #include <memory>
 #include "depth_traits.h"

diff --git a/astra_camera/include/astra_camera/point_cloud_proc/point_cloud_xyzrgb.h b/astra_camera/include/astra_camera/point_cloud_proc/point_cloud_xyzrgb.h
@@ -41,9 +41,14 @@
 #include <sensor_msgs/image_encodings.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
-#include <image_geometry/pinhole_camera_model.h>
+#if defined(ROS2_HUMBLE)
+  #include <cv_bridge/cv_bridge.h>
+  #include <image_geometry/pinhole_camera_model.h>
+#elif defined(ROS2_JAMMY)
+  #include <cv_bridge/cv_bridge.hpp>
+  #include <image_geometry/pinhole_camera_model.hpp>
+#endif
 
-#include <cv_bridge/cv_bridge.h>
 #include <opencv4/opencv2/imgproc/imgproc.hpp>
 #include <memory>
 #include <limits>

diff --git a/astra_camera/include/astra_camera/ros_param_backend.h b/astra_camera/include/astra_camera/ros_param_backend.h
@@ -14,12 +14,18 @@
 #include <rclcpp/rclcpp.hpp>
 
 namespace astra_camera {
+  #if defined(ROS2_HUMBLE) // Jammy changed the name, just moving 'Set' forwards
+  using OnSetCallback = rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType;
+#elif defined(ROS2_JAMMY)
+  using OnSetCallback = rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType;
+#endif
+
 class ParametersBackend {
  public:
   explicit ParametersBackend(rclcpp::Node* node);
   ~ParametersBackend();
   void addOnSetParametersCallback(
-      rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType callback);
+       OnSetCallback callback);
 
  private:
   rclcpp::Node* node_;

diff --git a/astra_camera/include/astra_camera/uvc_camera_driver.h b/astra_camera/include/astra_camera/uvc_camera_driver.h
@@ -151,7 +151,7 @@ class UVCCameraDriver {
   rclcpp::Service<SetBool>::SharedPtr set_uvc_mirror_srv_;
   rclcpp::Service<SetBool>::SharedPtr toggle_uvc_camera_srv_;
 
-  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_publisher_;
+  image_transport::Publisher image_publisher_;
   rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_publisher_;
   sensor_msgs::msg::CameraInfo camera_info_;
   std::unique_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_ = nullptr;

diff --git a/...a_camera/launch/astro_pro_plus.launch.xml → ...a_camera/launch/astra_pro_plus.launch.xml b/...a_camera/launch/astro_pro_plus.launch.xml → ...a_camera/launch/astra_pro_plus.launch.xml
diff --git a/astra_camera/scripts/install.sh b/astra_camera/scripts/install.sh
diff --git a/astra_camera/src/ob_camera_node.cpp b/astra_camera/src/ob_camera_node.cpp
@@ -12,7 +12,11 @@
 
 #include "astra_camera/ob_camera_node.h"
 #include "astra_camera/utils.h"
-#include <cv_bridge/cv_bridge.h>
+#if defined(ROS2_HUMBLE)
+  #include <cv_bridge/cv_bridge.h>
+#elif defined(ROS2_JAMMY)
+  #include <cv_bridge/cv_bridge.hpp>
+#endif
 namespace astra_camera {
 
 void OBCameraNode::init() {
@@ -383,9 +387,7 @@ void OBCameraNode::setupPublishers() {
       std::string topic = name + "/image_raw";
       auto image_qos = image_qos_[stream_index];
       auto image_qos_profile = getRMWQosProfileFromString(image_qos);
-      image_publishers_[stream_index] = node_->create_publisher<sensor_msgs::msg::Image>(
-          topic,
-          rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_qos_profile), image_qos_profile));
+      image_publishers_[stream_index] = image_transport::create_publisher(node_, topic, image_qos_profile);
       topic = name + "/camera_info";
       auto camera_info_qos = camera_info_qos_[stream_index];
       auto camera_info_qos_profile = getRMWQosProfileFromString(camera_info_qos);
@@ -531,7 +533,7 @@ void OBCameraNode::onNewFrameCallback(const openni::VideoFrameRef& frame,
   image_msg->height = stream_index == DEPTH ? height * depth_scale_ : height;
   image_msg->step = image_msg->width * unit_step_size_[stream_index];
   image_msg->is_bigendian = false;
-  image_publisher->publish(*image_msg);
+  image_publisher.publish(image_msg);
   sensor_msgs::msg::CameraInfo camera_info;
   if (stream_index == DEPTH) {
     camera_info = getDepthCameraInfo();

diff --git a/astra_camera/src/point_cloud_proc/point_cloud_xyz.cpp b/astra_camera/src/point_cloud_proc/point_cloud_xyz.cpp
@@ -34,7 +34,11 @@
 
 #include <image_transport/image_transport.hpp>
 #include <sensor_msgs/image_encodings.hpp>
+#if defined(ROS2_HUMBLE)
 #include <image_geometry/pinhole_camera_model.h>
+#elif defined(ROS2_JAMMY)
+#include <image_geometry/pinhole_camera_model.hpp>
+#endif
 #include "astra_camera/point_cloud_proc/point_cloud_xyz.h"
 
 namespace astra_camera {

diff --git a/astra_camera/src/point_cloud_proc/point_cloud_xyzrgb.cpp b/astra_camera/src/point_cloud_proc/point_cloud_xyzrgb.cpp
@@ -39,10 +39,14 @@
 #include <sensor_msgs/image_encodings.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
-#include <image_geometry/pinhole_camera_model.h>
-#include <cv_bridge/cv_bridge.h>
+#if defined(ROS2_HUMBLE)
+  #include <cv_bridge/cv_bridge.h>
+  #include <image_geometry/pinhole_camera_model.h>
+#elif defined(ROS2_JAMMY)
+  #include <cv_bridge/cv_bridge.hpp>
+  #include <image_geometry/pinhole_camera_model.hpp>
+#endif
 #include <opencv2/imgproc/imgproc.hpp>
-
 #include <memory>
 #include <string>
 #include <vector>

diff --git a/astra_camera/src/ros_param_backend.cpp b/astra_camera/src/ros_param_backend.cpp
@@ -24,7 +24,7 @@ ParametersBackend::~ParametersBackend() {
 }
 
 void ParametersBackend::addOnSetParametersCallback(
-    rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType callback) {
+    OnSetCallback callback) {
   ros_callback_ = node_->add_on_set_parameters_callback(std::move(callback));
 }
 

diff --git a/astra_camera/src/uvc_camera_driver.cpp b/astra_camera/src/uvc_camera_driver.cpp
@@ -12,7 +12,11 @@
 
 #include <utility>
 #include <opencv2/opencv.hpp>
+#if defined(ROS2_HUMBLE)
 #include <cv_bridge/cv_bridge.h>
+#elif defined(ROS2_JAMMY)
+#include <cv_bridge/cv_bridge.hpp>
+#endif
 #include "astra_camera/uvc_camera_driver.h"
 #include "astra_camera/utils.h"
 
@@ -87,9 +91,7 @@ UVCCameraDriver::UVCCameraDriver(rclcpp::Node* node, std::shared_ptr<Parameters>
     camera_info_manager_ = std::make_unique<camera_info_manager::CameraInfoManager>(
         node_, "rgb_camera", color_info_url_);
   }
-  image_publisher_ = node_->create_publisher<sensor_msgs::msg::Image>(
-      "color/image_raw",
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(color_qos_profile_), color_qos_profile_));
+  image_publisher_ = image_transport::create_publisher(node_, "color/image_raw", color_qos_profile_);
   camera_info_publisher_ = node_->create_publisher<sensor_msgs::msg::CameraInfo>(
       "color/camera_info", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(color_info_qos_profile_),
                                        color_info_qos_profile_));
@@ -395,7 +397,7 @@ void UVCCameraDriver::frameCallback(uvc_frame_t* frame) {
     cv_image_ptr->image = cv_img;
     image = *(cv_image_ptr->toImageMsg());
   }
-  image_publisher_->publish(image);
+  image_publisher_.publish(image);
   auto camera_info = getCameraInfo();
   camera_info.header.stamp = image.header.stamp;
   camera_info.header.frame_id = image.header.frame_id;