SE-2601

.github/workflows/ci.yml

@@ -181,7 +181,7 @@ jobs:
           run: |
             sudo apt-get update
             sudo apt-get install -y build-essential libopencv-dev locales \
-              software-properties-common curl
+              software-properties-common curl libcurl4-openssl-dev 
             sudo locale-gen en_US en_US.UTF-8
             sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
             export LANG=en_US.UTF-8

CMakeLists.txt

@@ -34,10 +34,18 @@ find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(vision_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
 find_package(camera_info_manager REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(camera_calibration_parsers REQUIRED)
 find_package(OpenCV 4 REQUIRED)
+find_package(CURL REQUIRED)
+
+# Special handling for legacy ROS messages (i.e. Foxy)
+add_compile_definitions(ROS_VERSION_MAJOR=${std_msgs_VERSION_MAJOR})
+add_compile_definitions(ROS_VERSION_MINOR=${std_msgs_VERSION_MINOR})
+
 
 add_library(bottlenose_camera_driver SHARED
         src/bottlenose_camera_driver.cpp
@@ -56,10 +64,13 @@ ament_target_dependencies(bottlenose_camera_driver
     "rclcpp_components"
     "sensor_msgs"
     "std_msgs"
+    "vision_msgs"
+    "visualization_msgs"
     "camera_info_manager"
     "image_transport"
     "camera_calibration_parsers"
     "OpenCV"
+    "CURL"
 )
 
 rclcpp_components_register_nodes(bottlenose_camera_driver "bottlenose_camera_driver::CameraDriver")

include/bottlenose_camera_driver.hpp

@@ -29,8 +29,12 @@
 #include "std_msgs/msg/string.hpp"
 #include "sensor_msgs/msg/image.hpp"
 #include "sensor_msgs/msg/camera_info.hpp"
-
 #include "sensor_msgs/image_encodings.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
+#include "visualization_msgs/msg/image_marker.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "vision_msgs/msg/detection2_d_array.hpp"
 
 #include <camera_info_manager/camera_info_manager.hpp>
 #include <image_transport/image_transport.hpp>
@@ -44,6 +48,8 @@
 #include <PvDeviceGEV.h>
 #include <PvStreamGEV.h>
 
+#include "bottlenose_chunk_parser.hpp"
+
 namespace bottlenose_camera_driver {
   class CameraDriver : public rclcpp::Node {
   public:
@@ -59,6 +65,7 @@ namespace bottlenose_camera_driver {
      */
     std::shared_ptr<sensor_msgs::msg::Image> convertFrameToMessage(IPvImage *image, uint64_t timestamp);
 
+    bool is_stereo();                     ///< Check if device is Bottlenose stereo
     bool set_interval();                  ///< Set camera frame rate.
     bool set_format();                    ///< Set camera format.
     bool set_ccm_profile();               ///< Apply color profile
@@ -70,16 +77,26 @@ namespace bottlenose_camera_driver {
     void disconnect();                    ///< Disconnect from camera.
     bool queue_buffers();                 ///< Queue buffers for GEV stack.
     void abort_buffers();                 ///< Abort buffers for GEV stack.
+    void publish_features(const std::vector<keypoints_t> &features, const uint64_t &timestamp); ///< Publish keypoints
+    void publish_bboxes(const bboxes_t &bboxes, const uint64_t &timestamp); ///< Publish bounding boxes
+    void publish_pointcloud(const pointcloud_t &pointcloud, const uint64_t &timestamp); ///< Publish point cloud
     void management_thread();             ///< Management thread for interacting with GEV stack.
     void status_callback();               ///< ROS2 status callback and orchestration polled from a timer.
     static bool is_ebus_loaded();         ///< Check if the eBusSDK Driver is loaded.
-    bool enable_chunk(std::string chunk); ///< Enable chunk data
+    bool set_chunk(std::string chunk, bool enable);    ///< Enable, Disable chunk data
     bool enable_ntp(bool enable);         ///< Enable NTP
+    bool configure_feature_points();      ///< Configure feature points
+    bool configure_ai_model();            ///< Configure AI model
+    bool configure_point_cloud();         ///< Configure triangulated point cloud
 
     bool load_calibration(uint32_t sid, std::string cname); ///< load calibration data
     bool set_calibration();                 ///< set calibration on to camera
     uint32_t get_num_sensors();             ///< returns the number of sensors: 1=mono and 2=stereo    
     bool set_register(std::string, std::variant<int64_t, double, bool>); ///< set a register value on the camera
+    bool set_enum_register(std::string, std::string); ///< set a register value on the camera
+
+    bool ftp_upload(const std::string &ftp_url, const std::string &file_path);
+
     bool m_calibrated;
 
     std::atomic<bool> done;               ///< Flag for management thread to terminate.
@@ -106,6 +123,16 @@ namespace bottlenose_camera_driver {
     /// Camera publisher.
     image_transport::CameraPublisher m_image_color;
     image_transport::CameraPublisher m_image_color_1;
+
+    // Keypoints publisher.
+    rclcpp::Publisher<visualization_msgs::msg::ImageMarker>::SharedPtr m_keypoints;
+    rclcpp::Publisher<visualization_msgs::msg::ImageMarker>::SharedPtr m_keypoints_1;
+
+    // Detections publisher (only sensor 0 "left")
+    rclcpp::Publisher<vision_msgs::msg::Detection2DArray>::SharedPtr m_detections;
+
+    // Triangulated point-cloud publisher.
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr m_pointcloud;
 };
 } // namespace bottlenose_camera_driver
 #endif //__BOTTLENOSE_CAMERA_DRIVER_HPP__

include/bottlenose_chunk_parser.hpp

@@ -22,6 +22,10 @@
 
 #include <PvBuffer.h>
 #include <vector>
+#include <stdint.h>
+
+#define MAX_KEYPOINTS 0xFFFF
+#define MAX_BBOXES (100)
 
 /**
  * @brief ChunkIDs for possible buffers appended to the GEV buffer.
@@ -32,24 +36,122 @@ typedef enum {
   CHUNK_ID_DNNBBOXES = 0x4003,   ///< Bounding boxes for detected targets
   CHUNK_ID_EMBEDDINGS = 0x4004,  ///< Embeddings
   CHUNK_ID_INFO = 0x4005,        ///< Meta information
+  CHUNK_ID_MATCHES = 0x4006,     ///< Matches
+  CHUNK_ID_POINTCLOUD = 0x4007   ///< Point cloud
 } chunk_type_t;
 
 /**
- * @brief Meta information chunk data to decode timestamps.
+ * @brief Chunk data representation of a keypoint.
+ */
+typedef struct point_u16 {
+  uint16_t x;  ///< x coordinate  of the point
+  uint16_t y;  ///< y coordinate of the point
+} point_u16_t;
+
+/**
+ * @brief Chunk data a 3-dimensional coordinate.
+ */
+typedef struct vector3f {
+  float x;  ///< x coordinate  of the point
+  float y;  ///< y coordinate  of the point
+  float z;  ///< z coordinate  of the point
+} vector3f_t;
+
+/**
+ * @brief Chunk data representation of keypoint AKAZE descriptors.
+ */
+typedef struct __attribute__((packed, aligned(4))) {
+  uint8_t data[64];         ///< up to 486 bits of descriptor data LSB first
+} descriptor_t;
+
+/**
+ * @brief Chunk data representation of keypoints.
  */
 typedef struct __attribute__((packed, aligned(4))) {
-  uint64_t real_time;
   uint32_t count;
+  uint32_t fid;
+  point_u16_t points[MAX_KEYPOINTS];
+} keypoints_t;
+
+/**
+ * @brief Chunk data representation of keypoint descriptors.
+ */
+typedef struct __attribute__((packed, aligned(4))) {
+  uint32_t count;
+  uint32_t length;
+  uint32_t fid;
+  descriptor_t descriptors[MAX_KEYPOINTS];
+} descriptors_t;
+
+/**
+ * @brief Chunk data representation of a bounding box.
+ */
+typedef struct bbox {
+  int32_t cid;    ///< box class id
+  float score;    ///< detection score
+  int32_t left;   ///< left-most point of the box
+  int32_t top;    ///< top-most point of the box
+  int32_t right;  ///< right-most point of the box
+  int32_t bottom; ///< bottom-most point of the box
+  char label[24]; ///< box label;
+} bbox_t;
+
+/**
+ * @brief Chunk data representation of bounding boxes.
+ */
+typedef struct __attribute__((packed, aligned(4))) {
+  uint32_t fid;
+  uint32_t count;
+  bbox_t box[MAX_BBOXES];
+} bboxes_t;
+
+typedef struct __attribute__((packed, aligned(4))) {
+  uint32_t count;
+  vector3f_t points[MAX_KEYPOINTS];
+} pointcloud_t;
+
+/**
+ * @brief Meta information chunk data to decode timestamps.
+ */
+typedef struct __attribute__((packed, aligned(4))) {
+  uint64_t real_time; ///< Realtime in milliseconds
+  uint32_t count;     ///< Frame count
+  float gain;         ///< Gain value
+  float exposure;     ///< Exposure value
 } info_t;
 
 /**
  * Decode meta information from buffer, if present.
  * @param buffer Buffer received on GEV interface
  * @param info Meta information
- * @return
+ * @return true if present, false if not present or corrupted.
  */
 bool chunkDecodeMetaInformation(PvBuffer *buffer, info_t *info);
 
+/**
+ * Decode keypoints from buffer, if present.
+ * @param buffer Buffer received on GEV interface
+ * @param keypoints Vector of keypoints.
+ * @return true if present, false if not present or corrupted.
+ */
+bool chunkDecodeKeypoints(PvBuffer *buffer, std::vector<keypoints_t> &keypoints);
+
+/**
+ * Decode bounding boxes from buffer, if present.
+ * @param buffer Buffer received on GEV interface
+ * @param bboxes Decoded bounding boxes.
+ * @return true if present, false if not present or corrupted.
+ */
+bool chunkDecodeBoundingBoxes(PvBuffer *buffer, bboxes_t &bboxes);
+
+/**
+ * Decode point cloud from buffer, if present.
+ * @param buffer Buffer received on GEV interface
+ * @param pointcloud Decoded point cloud
+ * @return true if present, false if not present or corrupted.
+ */
+bool chunkDecodePointCloud(PvBuffer *buffer, pointcloud_t &pointcloud);
+
 std::string ms_to_date_string(uint64_t ms);
 
 

include/bottlenose_parameters.hpp

@@ -33,6 +33,9 @@ const parameter_t bottlenose_parameters[] = {
   {"mac_address", rclcpp::ParameterValue("00:00:00:00:00:00")},
   {"keep_partial", rclcpp::ParameterValue(false)},
   {"stereo", rclcpp::ParameterValue(false)},
+  {"feature_points", rclcpp::ParameterValue("none")},
+  {"ai_model", rclcpp::ParameterValue("")},
+  {"sparse_point_cloud", rclcpp::ParameterValue(false)},
   /* Device parameters */
   {"fps", rclcpp::ParameterValue(10.0)},
   {"exposure", rclcpp::ParameterValue(20.0)},
@@ -42,6 +45,10 @@ const parameter_t bottlenose_parameters[] = {
   {"dgainGB", rclcpp::ParameterValue(1024)},
   {"dgainGR", rclcpp::ParameterValue(1024)},
   {"dgainRed", rclcpp::ParameterValue(1024)},
+  {"OffsetX", rclcpp::ParameterValue(108)},
+  {"OffsetY", rclcpp::ParameterValue(440)},
+  {"OffsetX1", rclcpp::ParameterValue(108)},
+  {"OffsetY1", rclcpp::ParameterValue(440)},
   /* ISP parameters */
   {"wbBlue", rclcpp::ParameterValue(1.0)},
   {"wbGreen", rclcpp::ParameterValue(1.0)},
@@ -63,21 +70,48 @@ const parameter_t bottlenose_parameters[] = {
   {"autoExposureEnable", rclcpp::ParameterValue(false)},
   {"autoExposureLuminanceTarget", rclcpp::ParameterValue(0x4000)},
 
-  /* GEV Parameters */
-  {"AnswerTimeout", rclcpp::ParameterValue(100)},
-  {"CommandRetryCount", rclcpp::ParameterValue(50)},
-  {"MaximumPendingResends", rclcpp::ParameterValue(0)},
-  {"MaximumResendRequestRetryByPacket", rclcpp::ParameterValue(0)},
-  {"MaximumResendGroupSize", rclcpp::ParameterValue(0)},
-  {"ResendRequestTimeout", rclcpp::ParameterValue(100)},
-  {"RequestTimeout", rclcpp::ParameterValue(10000)},
-  {"ResetOnIdle", rclcpp::ParameterValue(2000)},
+  /* GEV Parameters (Pleora defaults) */
+  {"AnswerTimeout", rclcpp::ParameterValue(1000)},
+  {"CommandRetryCount", rclcpp::ParameterValue(3)},
+  {"MaximumPendingResends", rclcpp::ParameterValue(512)},
+  {"MaximumResendRequestRetryByPacket", rclcpp::ParameterValue(3)},
+  {"MaximumResendGroupSize", rclcpp::ParameterValue(15)},
+  {"ResendRequestTimeout", rclcpp::ParameterValue(5000)},
+  {"RequestTimeout", rclcpp::ParameterValue(1000)},
+  {"ResetOnIdle", rclcpp::ParameterValue(200)},
 
   /* Calibration file parameters */
   {"camera_calibration_file", rclcpp::ParameterValue("")},
   {"left_camera_calibration_file", rclcpp::ParameterValue("")},
   {"right_camera_calibration_file", rclcpp::ParameterValue("")},
 
+  /* Keypoint parameters */
+  {"features_max",  rclcpp::ParameterValue(1000)},
+  {"features_threshold",  rclcpp::ParameterValue(100)},
+  {"features_nms",  rclcpp::ParameterValue(false)},
+  {"gftt_detector", rclcpp::ParameterValue("harris")},
+  {"features_quality", rclcpp::ParameterValue(500)},
+  {"features_min_distance", rclcpp::ParameterValue(15)},
+  {"features_harrisk", rclcpp::ParameterValue(0.0)},
+
+  /* Pointcloud parameters */
+  {"AKAZELength", rclcpp::ParameterValue(120)},
+  {"AKAZEWindow", rclcpp::ParameterValue(20)},
+  {"HAMATXOffset", rclcpp::ParameterValue(0)},
+  {"HAMATYOffset", rclcpp::ParameterValue(0)},
+  {"HAMATRect1X", rclcpp::ParameterValue(64)},
+  {"HAMATRect1Y", rclcpp::ParameterValue(64)},
+  {"HAMATRect2X", rclcpp::ParameterValue(128)},
+  {"HAMATRect2Y", rclcpp::ParameterValue(128)},
+  {"HAMATMinThreshold", rclcpp::ParameterValue(500)},
+  {"HAMATRatioThreshold",rclcpp::ParameterValue(1023)},
+
+  /* DNN parameters */
+//  {"DNNTopK", rclcpp::ParameterValue(1)}, // Not needed for Bounding boxes
+  {"DNNMaxDetections", rclcpp::ParameterValue(100)},
+  {"DNNNonMaxSuppression", rclcpp::ParameterValue(0.45)},
+  {"DNNConfidence", rclcpp::ParameterValue(0.2)},
+
   /* NTP support */
   {"ntpEnable", rclcpp::ParameterValue(false)}
 };

launch/foxglove_bridge_launch.xml

@@ -0,0 +1,7 @@
+<launch>
+  <!-- Including in another launch file -->
+  <include file="$(find-pkg-share foxglove_bridge)/launch/foxglove_bridge_launch.xml"/>
+    <arg name="port" value="8765"/>
+    <!-- ... other arguments ... -->
+  </include>
+</launch>

package.xml

@@ -30,6 +30,8 @@
   <depend>rclcpp_components</depend>
   <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
+  <depend>vision_msgs</depend>
+  <depend>visualization_msgs</depend>
   <depend>image_transport</depend>
   <depend>camera_calibration_parsers</depend>
   <depend>camera_info_manager</depend>