wanngwe/odin_ros_driver1
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

test ver

Browse Source
This commit is contained in:
jingyang-huang
2026-04-02 16:11:40 +08:00
parent 9ca6cf61f3
commit 84c6318121
18 changed files with 879 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/cloud_reprojection_ros_node.hpp
+12 -4
View File
@@ -23,6 +23,7 @@ limitations under the License.
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#else
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
@@ -32,6 +33,7 @@ limitations under the License.
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#endif
#include <pcl_conversions/pcl_conversions.h>
@@ -56,12 +58,14 @@ private:
std::string cloud_slam_topic_;
std::string odometry_topic_;
std::string wiwc_topic_;
std::string reprojected_image_topic_;
message_filters::Subscriber<PointCloud2> cloud_sub_;
message_filters::Subscriber<Odometry> odom_sub_;
message_filters::Subscriber<Odometry> wiwc_sub_;
typedef message_filters::sync_policies::ExactTime<PointCloud2, Odometry> MySyncPolicy;
typedef message_filters::sync_policies::ApproximateTime<PointCloud2, Odometry, Odometry> MySyncPolicy;
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
std::shared_ptr<Sync> sync_;
@@ -71,7 +75,8 @@ private:
void loadParameters();
void syncCallback(const PointCloud2::ConstSharedPtr& cloud_msg,
const Odometry::ConstSharedPtr& odom_msg);
const Odometry::ConstSharedPtr& odom_msg,
const Odometry::ConstSharedPtr& wiwc_msg);
};
#else
class CloudReprojectionRosNode
@@ -84,12 +89,14 @@ private:
std::string cloud_slam_topic_;
std::string odometry_topic_;
std::string wiwc_topic_;
std::string reprojected_image_topic_;
message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub_;
message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
message_filters::Subscriber<nav_msgs::Odometry> wiwc_sub_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, nav_msgs::Odometry> MySyncPolicy;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, nav_msgs::Odometry, nav_msgs::Odometry> MySyncPolicy;
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
std::shared_ptr<Sync> sync_;
@@ -99,6 +106,7 @@ private:
void loadParameters();
void syncCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
const nav_msgs::OdometryConstPtr& odom_msg);
const nav_msgs::OdometryConstPtr& odom_msg,
const nav_msgs::OdometryConstPtr& wiwc_msg);
};
#endif
include/cloud_reprojector.hpp
+7
View File
@@ -63,6 +63,13 @@ public:
const CameraParams& getCameraParams() const { return camera_params_; }
const ExtrinsicParams& getExtrinsicParams() const { return extrinsic_params_; }
// Update extrinsic parameters at runtime with real-time values from module
void updateExtrinsics(const Eigen::Matrix4d& Tcl, const Eigen::Matrix4d& Til) {
extrinsic_params_.Tcl = Tcl;
extrinsic_params_.Til = Til;
extrinsic_params_.Tic = calculateTic(Tcl, Til);
}
static Eigen::Matrix4d calculateTic(const Eigen::Matrix4d& Tcl, const Eigen::Matrix4d& Til);
private:
include/host_sdk_sample.h
+85 -1
View File
@@ -1019,12 +1019,25 @@ void publishRgb(capture_Image_List_t *stream) {
for (int idx = 0; idx < 16; ++idx) {
T_CL(idx / 4, idx % 4) = static_cast<double>(odom_data->pose_cov[idx]);
}
// Force last row to be [0, 0, 0, 1] for valid transformation matrix
T_CL(3, 0) = 0.0; T_CL(3, 1) = 0.0; T_CL(3, 2) = 0.0; T_CL(3, 3) = 1.0;
// Build TIL matrix (4x4) from twist_cov
Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity();
for (int idx = 0; idx < 16; ++idx) {
T_IL(idx / 4, idx % 4) = static_cast<double>(odom_data->twist_cov[idx]);
}
// Force last row to be [0, 0, 0, 1] for valid transformation matrix
T_IL(3, 0) = 0.0; T_IL(3, 1) = 0.0; T_IL(3, 2) = 0.0; T_IL(3, 3) = 1.0;
// Debug print to compare with cloud_reprojection values
// static int host_print_count = 0;
// if (host_print_count++) {
// std::cout << "=== host_sdk_sample T_CL from odom_data->pose_cov ===" << std::endl;
// std::cout << T_CL << std::endl;
// std::cout << "=== host_sdk_sample T_IL from odom_data->twist_cov ===" << std::endl;
// std::cout << T_IL << std::endl;
// }
// Extract rotation and translation from T_CL
Eigen::Matrix3d RCL = T_CL.block<3, 3>(0, 0);
@@ -1033,7 +1046,20 @@ void publishRgb(capture_Image_List_t *stream) {
// Extract rotation and translation from T_IL
Eigen::Matrix3d RIL = T_IL.block<3, 3>(0, 0);
Eigen::Vector3d TIL = T_IL.block<3, 1>(0, 3);
// Debug print extracted RCL and TCL
// static int rcl_print_count = 0;
// if (rcl_print_count++ ) {
// std::cout << "=== host_sdk_sample RCL (3x3 rotation from T_CL) ===" << std::endl;
// std::cout << RCL << std::endl;
// std::cout << "=== host_sdk_sample TCL (3x1 translation from T_CL) ===" << std::endl;
// std::cout << TCL.transpose() << std::endl;
// std::cout << "=== host_sdk_sample RIL (3x3 rotation from T_IL) ===" << std::endl;
// std::cout << RIL << std::endl;
// std::cout << "=== host_sdk_sample TIL (3x1 translation from T_IL) ===" << std::endl;
// std::cout << TIL.transpose() << std::endl;
// }
// Save RIL, TIL, RCL, TCL to YAML file
static int save_count = 0;
static int index_count = 0;
@@ -1096,6 +1122,59 @@ void publishRgb(capture_Image_List_t *stream) {
}
// Publish WIWC data (T_CL and T_IL extrinsics) as a separate topic
void publishWiwc(capture_Image_List_t* stream) {
uint32_t data_len = stream->imageList[0].length;
if (data_len != sizeof(ros_odom_convert_complete_t)) {
return;
}
ros_odom_convert_complete_t* odom_data = (ros_odom_convert_complete_t*)stream->imageList[0].pAddr;
#ifdef ROS2
auto msg = nav_msgs::msg::Odometry();
msg.header.stamp = make_aligned_stamp(odom_data->timestamp_ns, node_);
msg.header.frame_id = "odom";
#else
nav_msgs::Odometry msg;
msg.header.stamp = make_aligned_stamp(odom_data->timestamp_ns);
msg.header.frame_id = "odom";
#endif
// Store T_CL in pose.covariance (first 16 elements)
// Force last row to be [0, 0, 0, 1] for valid transformation matrix
for (int i = 0; i < 16; ++i) {
msg.pose.covariance[i] = odom_data->pose_cov[i];
}
msg.pose.covariance[12] = 0.0;
msg.pose.covariance[13] = 0.0;
msg.pose.covariance[14] = 0.0;
msg.pose.covariance[15] = 1.0;
// Fill remaining with zeros
for (int i = 16; i < 36; ++i) {
msg.pose.covariance[i] = 0.0;
}
// Store T_IL in twist.covariance (first 16 elements)
// Force last row to be [0, 0, 0, 1] for valid transformation matrix
for (int i = 0; i < 16; ++i) {
msg.twist.covariance[i] = odom_data->twist_cov[i];
}
msg.twist.covariance[12] = 0.0;
msg.twist.covariance[13] = 0.0;
msg.twist.covariance[14] = 0.0;
msg.twist.covariance[15] = 1.0;
// Fill remaining with zeros
for (int i = 16; i < 36; ++i) {
msg.twist.covariance[i] = 0.0;
}
#ifdef ROS2
wiwc_publisher_->publish(msg);
#else
wiwc_publisher_.publish(msg);
#endif
}
void publishOdometry(capture_Image_List_t* stream, OdometryType odom_type, bool show_path, bool show_camerapose) {
@@ -1161,6 +1240,7 @@ void publishRgb(capture_Image_List_t *stream) {
msg.twist.twist.angular.y = static_cast<double>(odom_data->angular_velocity[1]) / 1e6;
msg.twist.twist.angular.z = static_cast<double>(odom_data->angular_velocity[2]) / 1e6;
// Copy original covariance data
for (int i = 0; i < 36; ++i) {
msg.pose.covariance[i] = odom_data->pose_cov[i];
}
@@ -1606,6 +1686,7 @@ private:
compressed_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::CompressedImage>("odin1/image/compressed", qos_small);
undistort_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/undistorted", qos_sensor);
intensity_gray_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/intensity_gray", qos_sensor);
wiwc_publisher_ = node_->create_publisher<ros::Odometry>("odin1/wiwc", qos_small);
tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(node_);
#endif
}
@@ -1623,6 +1704,7 @@ private:
compressed_rgb_pub_ = nh.advertise<sensor_msgs::CompressedImage>("odin1/image/compressed", 100);
undistort_rgb_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/undistorted", 100);
intensity_gray_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/intensity_gray", 100);
wiwc_publisher_ = nh.advertise<ros::Odometry>("odin1/wiwc", 100);
tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>();
}
#endif
@@ -1643,6 +1725,7 @@ private:
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr undistort_rgb_pub_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr intensity_gray_pub_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_camera_pose_visual_;
rclcpp::Publisher<ros::Odometry>::SharedPtr wiwc_publisher_;
camera_pose_visualization cameraposevisual_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
#else
@@ -1661,6 +1744,7 @@ private:
ros::Publisher compressed_rgb_pub_; // New compressed image publisher
ros::Publisher undistort_rgb_pub_;
ros::Publisher intensity_gray_pub_;
ros::Publisher wiwc_publisher_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
#endif
};
include/image_overlay_node.hpp
+91
View File
@@ -0,0 +1,91 @@
/*
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
#pragma once
#ifdef ROS2
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <mutex>
#else
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#endif
#include <opencv2/opencv.hpp>
#include <string>
#include <memory>
#ifdef ROS2
class ImageOverlayNode : public rclcpp::Node
{
public:
ImageOverlayNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
private:
using Image = sensor_msgs::msg::Image;
std::string reprojected_topic_;
std::string camera_topic_;
std::string overlay_topic_;
double alpha_; // blend alpha for overlay
rclcpp::Subscription<Image>::SharedPtr reproj_sub_;
rclcpp::Subscription<Image>::SharedPtr camera_sub_;
rclcpp::Publisher<Image>::SharedPtr overlay_pub_;
// Cache latest images
cv::Mat latest_reproj_img_;
cv::Mat latest_camera_img_;
std_msgs::msg::Header latest_header_;
std::mutex mutex_;
void reprojCallback(const Image::ConstSharedPtr& msg);
void cameraCallback(const Image::ConstSharedPtr& msg);
void publishOverlay();
};
#else
#include <mutex>
class ImageOverlayNode
{
public:
ImageOverlayNode(ros::NodeHandle& nh, ros::NodeHandle& pnh);
private:
ros::NodeHandle nh_, pnh_;
std::string reprojected_topic_;
std::string camera_topic_;
std::string overlay_topic_;
double alpha_; // blend alpha for overlay
ros::Subscriber reproj_sub_;
ros::Subscriber camera_sub_;
ros::Publisher overlay_pub_;
// Cache latest images
cv::Mat latest_reproj_img_;
cv::Mat latest_camera_img_;
std_msgs::Header latest_header_;
std::mutex mutex_;
void reprojCallback(const sensor_msgs::ImageConstPtr& msg);
void cameraCallback(const sensor_msgs::ImageConstPtr& msg);
void publishOverlay();
};
#endif
include/lidar_api.h
+35
View File
@@ -244,6 +244,18 @@ int lidar_get_custom_parameter(device_handle device, const char* param_name, int
*/
int lidar_get_mapping_result(device_handle device, const char* dest_dir, const char* file_name);
/**
* @brief Set the image mask file for the device
*
* Read & send specified image mask file to device
*
* @param device Handle to the target device
* @param abs_path Absolute path to the image mask file (e.g., mask.png)
* @return int 0 on success, -1 on failure, -2 if file transfer in progress
*/
int lidar_set_image_mask(device_handle device, const char* abs_path);
/**
* @brief enable device log
*
@@ -266,6 +278,29 @@ int lidar_get_custom_parameter(device_handle device, const char* param_name, int
*/
int lidar_set_depth_parameter(device_handle device, const lidar_depth_para_t *params);
/**
* @brief Enable or disable IMU smooth sending feature
*
* When enabled, IMU data will be sent at precise intervals (default 400Hz)
* using a dedicated high-priority thread to reduce jitter and timing variance.
* When disabled, IMU data will be sent immediately upon reception.
*
* @param enable 1 to enable smooth sending, 0 to disable
* @return int 0 on success, -1 on failure
*/
int lidar_enable_imu_smooth_sending(int enable);
/**
* @brief Set IMU smooth sending frequency
*
* Set the target frequency for IMU smooth sending. Only effective when
* smooth sending is enabled via lidar_enable_imu_smooth_sending().
*
* @param frequency_hz Target frequency in Hz (1-1000 Hz, recommended 400 Hz)
* @return int 0 on success, -1 on failure
*/
int lidar_set_imu_smooth_frequency(uint32_t frequency_hz);
#ifdef __cplusplus
}
#endif
include/yaml_parser.h
+1 -1
View File
@@ -87,7 +87,7 @@ private:
std::map<std::string, std::string> register_keys_str_val_;
std::map<std::string, ParameterValue> custom_parameters_;
std::unordered_set<std::string> allowed_key_w_str_val = {"relocalization_map_abs_path", "mapping_result_dest_dir", "mapping_result_file_name"};
std::unordered_set<std::string> allowed_key_w_str_val = {"relocalization_map_abs_path", "mapping_result_dest_dir", "mapping_result_file_name", "image_mask_abs_path"};
};
}