diff --git a/README.md b/README.md index e9960c2..85c488b 100644 --- a/README.md +++ b/README.md @@ -2,6 +2,8 @@ ROS driver suite for Odin sensor modules (Manifold Tech Ltd.) +Odin1 wiki: https://manifoldtehltd.github.io/wiki/Odin1/Cover.html + ## Odin_ROS_Driver Compatibility: @@ -16,18 +18,20 @@ This driver package provides core functionality for point cloud SLAM application ## 1. Version -Current Version: v0.6.1 +Current Version: v0.7.0 ## 2. Preparation ### 2.1 OS Requirement -● Ubuntu 18.04 for ROS Melodic; - ● Ubuntu 20.04 for ROS Noetic and ROS2 Foxy; ● Ubuntu 22.04 for ROS2 Humble; +● Ubuntu 18.04 is currently not supported; + +● Ubuntu 24.04 is not officially supported but may work with some modifications. + ### 2.2 Dependencies ● Opencv >= 4.5.0(recommand 4.5.5/4.8.0. Make sure only one version of opencv is installed) @@ -67,8 +71,6 @@ sudo apt-get install libopencv-dev ``` #### 2.3.4 ROS install -For ROS Melodic installation, please refer to: -[ROS Melodic installation instructions](https://wiki.ros.org/melodic/Installation) For ROS Noetic installation, please refer to: [ROS Noetic installation instructions](https://wiki.ros.org/noetic/Installation) @@ -437,6 +439,9 @@ Disable odin1/image with sendrgb = 0 in control_command.yaml and try again. If t Purge the unused version of opencv and maintain a single complete version, then rebuild the driver and try again. ## 6. Contact Information​​ + +You can contact our support through support@manifoldtech.cn + To help diagnose the issue, please provide the following details to our FAE engineer: 1. ​Current firmware version​​ diff --git a/config/control_command.yaml b/config/control_command.yaml index 073ea6e..233486a 100644 --- a/config/control_command.yaml +++ b/config/control_command.yaml @@ -1,23 +1,70 @@ register_keys: - strict_usb3.0_check: 0 # 0: off: 1: on; if off, allow connection even if usb connection is below usb 3.0 + # if off, allow connection even if usb connection is below usb 3.0 + # ATTENTION: usb 3.0 is always recommended, as advance functionality like SLAM mode requires usb 3.0 for reliable map file transfer + strict_usb3.0_check: 0 # 0: off: 1: on; + + # 0: use odin internal system time as data time stamp as before, typical and recommanded; + # 1: use host ros time (upon recieve) as data time stamp, only use if you specifically require this setup, not recommanded for most user + use_host_ros_time: 0 + streamctrl: 1 # 0: off; 1: on - sendrgb: 1 # 0: off; 1: on - sendimu: 1 # 0: off; 1: on - sendodom: 1 # 0: off; 1: on - senddtof: 1 # 0: off; 1: on - sendcloudslam: 1 # 0: off; 1: on - sendcloudrender: 1 # 0: off; 1: on + + # original rgb data in jpeg format from device sendrgbcompressed: 1 # 0: off; 1: on + + # RGB data, decoded from original jpeg data from device, bgr8 format + # Processed on host device + sendrgb: 1 # 0: off; 1: on + + # undistort rgb image processed from decoded rgb data. + # depends on sendrgb. related camera parameters can be found in ws/src/odin_ros_driver/config/calib.yaml + # Processed on host device + sendrgbundistort: 0 # 0: off; 1: on. + + # IMU data + sendimu: 1 # 0: off; 1: on + + # Odometry data + sendodom: 1 # 0: off; 1: on + + # TF from odom to base_link. Leave it on unless you specifically need it off. + # ATTENTION: critical for rviz to show cloud_raw. + send_odom_baselink_tf: 1 # 0: off; 1: on. + + # raw dtof data + senddtof: 1 # 0: off; 1: on + + # slam cloud data + sendcloudslam: 1 # 0: off; 1: on + + # processed with raw point cloud, rgb image, and calib.yaml from device + # Processed on host device + sendcloudrender: 1 # 0: off; 1: on + + # depth completion demo, high computing resource usage. + # for more information please refer to the readme file. + # Processed on host device senddepth: 0 # 0: off; 1: on - sendrgbundistort: 0 # 0: off; 1: on + + # record rgb, odometry, and slam cloud data as proprietary olx format for further processing in MindCloud(TM) software. + # save path: ws/src/odin_ros_driver/recorddata/{record_start_time}/ + # ATTENTION: please copy the full folder for post-processing. recorddata: 0 # 0: off; 1: on - devstatuslog: 1 # 0: off; 1: on + + # Save device runtime status info to ws/src/odin_ros_driver/log/Driver_{drvier_start_time}/Conn_{device_connection_time}/dev_status.csv + devstatuslog: 1 # 0: off; 1: on. + + save_log: 0 # 0: off; 1: on; + + # raw dtof sensor intensity data in gray format, mostly for debug purpose. pubintensitygray: 0 # 0: off; 1: on + showpath: 0 # 0: off; 1: on showcamerapose: 0 # 0: off; 1: on + custom_save_map: 0 custom_map_mode: 0 # 0: Odometry mode 1: SLAM mode 2: Relocalization mode custom_init_pos: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] relocalization_map_abs_path: "" # must be set for Relocalization mode or will fail mapping_result_dest_dir: "" # "": use default value; other: use custom value - mapping_result_file_name: "" # "": use default value; other: use custom value \ No newline at end of file + mapping_result_file_name: "" # "": use default value; other: use custom value diff --git a/include/host_sdk_sample.h b/include/host_sdk_sample.h index 2dc2bea..108335a 100644 --- a/include/host_sdk_sample.h +++ b/include/host_sdk_sample.h @@ -182,6 +182,10 @@ class RosNodeControlInterface { virtual ~RosNodeControlInterface() = default; virtual void setDtofSubframeODR(int odr) = 0; virtual int getDtofSubframeODR() const = 0; + virtual void setUseHostRosTime(bool use_host_ros_time) = 0; + virtual bool useHostRosTime() const = 0; + virtual void setSendOdomBaseLinkTF(bool send_odom_baselink_tf) = 0; + virtual bool sendOdomBaseLinkTF() const = 0; }; RosNodeControlInterface* getRosNodeControl(); @@ -233,7 +237,15 @@ public: ros::Imu imu_msg; #endif - imu_msg.header.stamp = ns_to_ros_time(stream->stamp); + if (getRosNodeControl()->useHostRosTime()) { + #ifdef ROS2 + imu_msg.header.stamp = node_->now(); + #else + imu_msg.header.stamp = ros::Time::now(); + #endif + } else { + imu_msg.header.stamp = ns_to_ros_time(stream->stamp); + } imu_msg.header.frame_id = "imu_link"; imu_msg.linear_acceleration.y = -1 * stream->accel_x; @@ -499,7 +511,7 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m } } - void publishIntensityCloud(capture_Image_List_t* stream, int idx) +void publishIntensityCloud(capture_Image_List_t* stream, int idx) { // Check index validity if (idx < 0 || idx >= 10) { @@ -534,7 +546,15 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m // Set message header msg->header.frame_id = "odin1_base_link"; - msg->header.stamp = ns_to_ros_time(cloud.timestamp); + if (getRosNodeControl()->useHostRosTime()) { + #ifdef ROS2 + msg->header.stamp = node_->now(); + #else + msg->header.stamp = ros::Time::now(); + #endif + } else { + msg->header.stamp = ns_to_ros_time(cloud.timestamp); + } msg->height = cloud.height; msg->width = cloud.width; @@ -653,173 +673,51 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m #endif } - void publishGrayUInt8(capture_Image_List_t *stream, int idx) { - ImageMsg msg; - msg.header.stamp = ns_to_ros_time(stream->imageList[idx].timestamp); - msg.header.frame_id = "map"; - - int width = stream->imageList[idx].width; - int height = stream->imageList[idx].height; - - msg.height = height; - msg.width = width; - msg.encoding = "mono8"; - msg.is_bigendian = false; - msg.step = width * sizeof(uint8_t); - - size_t image_size = msg.step * height; - - msg.data.resize(image_size); - - memcpy(msg.data.data(), stream->imageList[idx].pAddr, image_size); - +void publishGrayUInt8(capture_Image_List_t *stream, int idx) { + ImageMsg msg; + if (getRosNodeControl()->useHostRosTime()) { #ifdef ROS2 - intensity_gray_pub_->publish(msg); + msg.header.stamp = node_->now(); #else - intensity_gray_pub_.publish(msg); + msg.header.stamp = ros::Time::now(); #endif + } else { + msg.header.stamp = ns_to_ros_time(stream->imageList[idx].timestamp); } + msg.header.frame_id = "map"; - void publishRgb(capture_Image_List_t *stream) { + int width = stream->imageList[idx].width; + int height = stream->imageList[idx].height; + + msg.height = height; + msg.width = width; + msg.encoding = "mono8"; + msg.is_bigendian = false; + msg.step = width * sizeof(uint8_t); + + size_t image_size = msg.step * height; + + msg.data.resize(image_size); + + memcpy(msg.data.data(), stream->imageList[idx].pAddr, image_size); + + #ifdef ROS2 + intensity_gray_pub_->publish(msg); + #else + intensity_gray_pub_.publish(msg); + #endif +} + +void publishRgb(capture_Image_List_t *stream) { buffer_List_t &image = stream->imageList[0]; // old version yuv data if (image.length == image.width * image.height * 3 / 2) { - try { - const int height_nv12 = image.height * 3 / 2; - cv::Mat nv12_mat(height_nv12, image.width, CV_8UC1, image.pAddr); - cv::Mat bgr; - cv::cvtColor(nv12_mat, bgr, cv::COLOR_YUV2BGR_NV12); - - if (bgr.empty()) { - #ifndef ROS2 - ROS_ERROR("Failed to convert NV12 to BGR"); - #endif - return; - } - - //Create ROS image message - #ifdef ROS2 - auto header = std::make_shared(); - header->stamp = ns_to_ros_time(image.timestamp); // Offset compensation - - //RCLCPP_INFO(rclcpp::get_logger("device_cb"), "image rgb %ld",image.timestamp); - header->frame_id = "camera_rgb_frame"; - - auto cv_image = std::make_shared(*header, "bgr8", bgr); - auto msg = cv_image->toImageMsg(); - - // Add to unified queue - if (g_sendcloudrender) { - std::lock_guard lock(rgb_queue_mutex_); - if (rgb_image_queue_.size() >= 10) { - rgb_image_queue_.pop_front(); - } - rgb_image_queue_.push_back(msg); - } - - // Publish original image message - rgb_pub_->publish(*msg); - - // Create compressed image message - auto compressed_msg = std::make_shared(); - compressed_msg->header = *header; - compressed_msg->format = "jpeg"; - - // Set compression parameters - std::vector compression_params; - compression_params.push_back(cv::IMWRITE_JPEG_QUALITY); - compression_params.push_back(80); - - // Compress image - cv::imencode(".JPEG", bgr, compressed_msg->data, compression_params); - - // Enqueue binary logging for image - if (data_logger_) { - const uint32_t idx_now = image_index_.fetch_add(1, std::memory_order_relaxed); - const double ts_sec = static_cast(image.timestamp) / 1e9; - const uint32_t jpeg_size = static_cast(compressed_msg->data.size()); - std::vector blob; - blob.reserve(sizeof(uint32_t) + sizeof(double) + sizeof(uint32_t) + jpeg_size); - auto append_pod = [&](const auto& v) { - const uint8_t* p = reinterpret_cast(&v); - blob.insert(blob.end(), p, p + sizeof(v)); - }; - append_pod(idx_now); - append_pod(ts_sec); - append_pod(jpeg_size); - blob.insert(blob.end(), compressed_msg->data.begin(), compressed_msg->data.end()); - data_logger_->enqueueImageFrame(std::move(blob)); - } - - compressed_rgb_pub_->publish(*compressed_msg); - - #else - // ROS1 version - std_msgs::Header header; - header.stamp = ns_to_ros_time(image.timestamp); // Offset compensation - header.frame_id = "camera_rgb_frame"; - - auto cv_image = boost::make_shared(header, "bgr8", bgr); - auto msg = cv_image->toImageMsg(); - - // Add to unified queue - if (g_sendcloudrender) { - std::lock_guard lock(rgb_queue_mutex_); - if (rgb_image_queue_.size() >= 10) { - rgb_image_queue_.pop_front(); - } - rgb_image_queue_.push_back(msg); - } - - // Publish original image message - rgb_pub_.publish(msg); - - // Publish compressed image - always publish - // Create compressed image message - sensor_msgs::CompressedImagePtr compressed_msg(new sensor_msgs::CompressedImage()); - compressed_msg->header = header; - compressed_msg->format = "jpeg"; - - // Set compression parameters - std::vector compression_params; - compression_params.push_back(cv::IMWRITE_JPEG_QUALITY); - compression_params.push_back(80); // JPEG quality 80% - - // Compress image - cv::imencode(".jpg", bgr, compressed_msg->data, compression_params); - - // Enqueue binary logging for image - if (data_logger_) { - const uint32_t idx_now = image_index_.fetch_add(1, std::memory_order_relaxed); - // Convert ROS1 header.stamp to seconds - const double ts_sec = static_cast(header.stamp.sec) + static_cast(header.stamp.nsec) / 1e9; - const uint32_t jpeg_size = static_cast(compressed_msg->data.size()); - std::vector blob; - blob.reserve(sizeof(uint32_t) + sizeof(double) + sizeof(uint32_t) + jpeg_size); - auto append_pod = [&](const auto& v) { - const uint8_t* p = reinterpret_cast(&v); - blob.insert(blob.end(), p, p + sizeof(v)); - }; - append_pod(idx_now); - append_pod(ts_sec); - append_pod(jpeg_size); - blob.insert(blob.end(), compressed_msg->data.begin(), compressed_msg->data.end()); - data_logger_->enqueueImageFrame(std::move(blob)); - } - compressed_rgb_pub_.publish(compressed_msg); - - #endif - - } catch (const cv::Exception& e) { - #ifndef ROS2 - ROS_ERROR("OpenCV error in publishRgb: %s", e.what()); - #endif - } catch (const std::exception& e) { - #ifndef ROS2 - ROS_ERROR("Exception in publishRgb: %s", e.what()); - #endif - } + #ifdef ROS2 + RCLCPP_INFO(rclcpp::get_logger("publishRgb"), "old format rgb data, please upgrade device firmware"); + #else + ROS_INFO("old format rgb data, please upgrade device firmware"); + #endif } else {// new version jpeg data std::vector jpeg_data(static_cast(image.pAddr), @@ -829,7 +727,15 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m cv::Mat decoded_image = cv::imdecode(jpeg_data, cv::IMREAD_COLOR); cv_bridge::CvImage cv_image; - cv_image.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); + if (getRosNodeControl()->useHostRosTime()) { + #ifdef ROS2 + cv_image.header.stamp = node_->now(); + #else + cv_image.header.stamp = ros::Time::now(); + #endif + } else { + cv_image.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); + } cv_image.encoding = "bgr8"; cv_image.image = decoded_image; @@ -865,10 +771,19 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m if (m_undistort_map_init_success) { cv::remap(decoded_image, undistorted_image, m_undistort_map_x, m_undistort_map_y, cv::INTER_LINEAR); - cv_undistorted_image.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); + if (getRosNodeControl()->useHostRosTime()) { + #ifdef ROS2 + cv_undistorted_image.header.stamp = node_->now(); + #else + cv_undistorted_image.header.stamp = ros::Time::now(); + #endif + } else { + cv_undistorted_image.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); + } cv_undistorted_image.encoding = "bgr8"; cv_undistorted_image.image = undistorted_image; } + #ifdef ROS2 { rgb_pub_->publish(*cv_image.toImageMsg()); @@ -878,7 +793,11 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m // original jpeg sensor_msgs::msg::CompressedImage jpeg_msg; - jpeg_msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); + if (getRosNodeControl()->useHostRosTime()) { + jpeg_msg.header.stamp = node_->now(); + } else { + jpeg_msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); + } jpeg_msg.format = "jpeg"; jpeg_msg.data = jpeg_data; @@ -893,9 +812,11 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m // original jpeg sensor_msgs::CompressedImagePtr jpeg_msg(new sensor_msgs::CompressedImage()); - // compressed_msg->header = header; - // compressed_msg->format = "jpeg"; - jpeg_msg->header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); + if (getRosNodeControl()->useHostRosTime()) { + jpeg_msg->header.stamp = ros::Time::now(); + } else { + jpeg_msg->header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); + } jpeg_msg->format = "jpeg"; jpeg_msg->data = jpeg_data; @@ -912,7 +833,11 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m #ifdef ROS2 sensor_msgs::msg::PointCloud2 msg; msg.header.frame_id = "odom"; - msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); + if (getRosNodeControl()->useHostRosTime()) { + msg.header.stamp = node_->now(); + } else { + msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); + } //RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Point cloudrgba %ld",stream->imageList[0].timestamp); @@ -940,7 +865,11 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m #else sensor_msgs::PointCloud2 msg; msg.header.frame_id = "odom"; - msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); + if (getRosNodeControl()->useHostRosTime()) { + msg.header.stamp = ros::Time::now(); + } else { + msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); + } size_t pt_size = sizeof(int32_t) * 3 + sizeof(int32_t) * 4; uint32_t points = stream->imageList[idx].length / pt_size; @@ -1054,7 +983,15 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m if (data_len == sizeof(ros_odom_convert_complete_t)) { ros_odom_convert_complete_t* odom_data = (ros_odom_convert_complete_t*)stream->imageList[0].pAddr; - msg.header.stamp = ns_to_ros_time(odom_data->timestamp_ns); + if (getRosNodeControl()->useHostRosTime()) { + #ifdef ROS2 + msg.header.stamp = node_->now(); + #else + msg.header.stamp = ros::Time::now(); + #endif + } else { + msg.header.stamp = ns_to_ros_time(odom_data->timestamp_ns); + } msg.pose.pose.position.x = static_cast(odom_data->pos[0]) / 1e6; msg.pose.pose.position.y = static_cast(odom_data->pos[1]) / 1e6; @@ -1109,7 +1046,15 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m } else if (data_len == sizeof(ros2_odom_convert_t)) { ros2_odom_convert_t* odom_data = (ros2_odom_convert_t*)stream->imageList[0].pAddr; - msg.header.stamp = ns_to_ros_time(odom_data->timestamp_ns); + if (getRosNodeControl()->useHostRosTime()) { + #ifdef ROS2 + msg.header.stamp = node_->now(); + #else + msg.header.stamp = ros::Time::now(); + #endif + } else { + msg.header.stamp = ns_to_ros_time(odom_data->timestamp_ns); + } msg.pose.pose.position.x = static_cast(odom_data->pos[0]) / 1e6; msg.pose.pose.position.y = static_cast(odom_data->pos[1]) / 1e6; @@ -1125,18 +1070,24 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m switch(odom_type) { case OdometryType::STANDARD: { - geometry_msgs::msg::TransformStamped transformStamped; - transformStamped.header.stamp = msg.header.stamp; - transformStamped.header.frame_id = "odom"; - transformStamped.child_frame_id = "odin1_base_link"; - transformStamped.transform.translation.x = msg.pose.pose.position.x; - transformStamped.transform.translation.y = msg.pose.pose.position.y; - transformStamped.transform.translation.z = msg.pose.pose.position.z; - transformStamped.transform.rotation.x = msg.pose.pose.orientation.x; - transformStamped.transform.rotation.y = msg.pose.pose.orientation.y; - transformStamped.transform.rotation.z = msg.pose.pose.orientation.z; - transformStamped.transform.rotation.w = msg.pose.pose.orientation.w; - tf_broadcaster->sendTransform(transformStamped); + if (getRosNodeControl()->sendOdomBaseLinkTF()) { + geometry_msgs::msg::TransformStamped transformStamped; + if (getRosNodeControl()->useHostRosTime()) { + transformStamped.header.stamp = node_->now(); + } else { + transformStamped.header.stamp = msg.header.stamp; + } + transformStamped.header.frame_id = "odom"; + transformStamped.child_frame_id = "odin1_base_link"; + transformStamped.transform.translation.x = msg.pose.pose.position.x; + transformStamped.transform.translation.y = msg.pose.pose.position.y; + transformStamped.transform.translation.z = msg.pose.pose.position.z; + transformStamped.transform.rotation.x = msg.pose.pose.orientation.x; + transformStamped.transform.rotation.y = msg.pose.pose.orientation.y; + transformStamped.transform.rotation.z = msg.pose.pose.orientation.z; + transformStamped.transform.rotation.w = msg.pose.pose.orientation.w; + tf_broadcaster->sendTransform(transformStamped); + } odom_publisher_->publish(msg); // Publish odom trajectory as visualization markers (green lines connecting adjacent points) @@ -1201,7 +1152,11 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m case OdometryType::TRANSFORM: { geometry_msgs::msg::TransformStamped transformStamped; - transformStamped.header.stamp = msg.header.stamp; + if (getRosNodeControl()->useHostRosTime()) { + transformStamped.header.stamp = node_->now(); + } else { + transformStamped.header.stamp = msg.header.stamp; + } transformStamped.header.frame_id = "odom"; transformStamped.child_frame_id = "map"; transformStamped.transform.translation.x = msg.pose.pose.position.x; @@ -1219,18 +1174,24 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m switch(odom_type) { case OdometryType::STANDARD: { - geometry_msgs::TransformStamped transformStamped; - transformStamped.header.stamp = msg.header.stamp; - transformStamped.header.frame_id = "odom"; - transformStamped.child_frame_id = "odin1_base_link"; - transformStamped.transform.translation.x = msg.pose.pose.position.x; - transformStamped.transform.translation.y = msg.pose.pose.position.y; - transformStamped.transform.translation.z = msg.pose.pose.position.z; - transformStamped.transform.rotation.x = msg.pose.pose.orientation.x; - transformStamped.transform.rotation.y = msg.pose.pose.orientation.y; - transformStamped.transform.rotation.z = msg.pose.pose.orientation.z; - transformStamped.transform.rotation.w = msg.pose.pose.orientation.w; - tf_broadcaster->sendTransform(transformStamped); + if (getRosNodeControl()->sendOdomBaseLinkTF()) { + geometry_msgs::TransformStamped transformStamped; + if (getRosNodeControl()->useHostRosTime()) { + transformStamped.header.stamp = ros::Time::now(); + } else { + transformStamped.header.stamp = msg.header.stamp; + } + transformStamped.header.frame_id = "odom"; + transformStamped.child_frame_id = "odin1_base_link"; + transformStamped.transform.translation.x = msg.pose.pose.position.x; + transformStamped.transform.translation.y = msg.pose.pose.position.y; + transformStamped.transform.translation.z = msg.pose.pose.position.z; + transformStamped.transform.rotation.x = msg.pose.pose.orientation.x; + transformStamped.transform.rotation.y = msg.pose.pose.orientation.y; + transformStamped.transform.rotation.z = msg.pose.pose.orientation.z; + transformStamped.transform.rotation.w = msg.pose.pose.orientation.w; + tf_broadcaster->sendTransform(transformStamped); + } odom_publisher_.publish(msg); if (show_path) { @@ -1296,7 +1257,11 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m case OdometryType::TRANSFORM: { geometry_msgs::TransformStamped transformStamped; - transformStamped.header.stamp = msg.header.stamp; + if (getRosNodeControl()->useHostRosTime()) { + transformStamped.header.stamp = ros::Time::now(); + } else { + transformStamped.header.stamp = msg.header.stamp; + } transformStamped.header.frame_id = "odom"; transformStamped.child_frame_id = "map"; transformStamped.transform.translation.x = msg.pose.pose.position.x; @@ -1533,18 +1498,18 @@ private: } #ifdef ROS1 void initialize_publishers(ros::NodeHandle& nh) { - imu_pub_ = nh.advertise("odin1/imu", 10); - rgb_pub_ = nh.advertise("odin1/image", 10); - cloud_pub_ = nh.advertise("odin1/cloud_raw", 10); - xyzrgbacloud_pub_ = nh.advertise("odin1/cloud_slam", 10); - odom_publisher_ = nh.advertise("odin1/odometry", 10); - odom_highfreq_publisher_ = nh.advertise("odin1/odometry_highfreq", 10); - path_publisher_ = nh.advertise("odin1/path", 10); - pub_camera_pose_visual_ = nh.advertise("odin1/camera_pose_visual", 10); - rgbcloud_pub_ = nh.advertise("odin1/cloud_render", 10); - compressed_rgb_pub_ = nh.advertise("odin1/image/compressed", 10); - undistort_rgb_pub_ = nh.advertise("odin1/image/undistorted", 10); - intensity_gray_pub_ = nh.advertise("odin1/image/intensity_gray", 10); + imu_pub_ = nh.advertise("odin1/imu", 4000); + rgb_pub_ = nh.advertise("odin1/image", 100); + cloud_pub_ = nh.advertise("odin1/cloud_raw", 100); + xyzrgbacloud_pub_ = nh.advertise("odin1/cloud_slam", 100); + odom_publisher_ = nh.advertise("odin1/odometry", 100); + odom_highfreq_publisher_ = nh.advertise("odin1/odometry_highfreq", 4000); + path_publisher_ = nh.advertise("odin1/path", 100); + pub_camera_pose_visual_ = nh.advertise("odin1/camera_pose_visual", 100); + rgbcloud_pub_ = nh.advertise("odin1/cloud_render", 100); + compressed_rgb_pub_ = nh.advertise("odin1/image/compressed", 100); + undistort_rgb_pub_ = nh.advertise("odin1/image/undistorted", 100); + intensity_gray_pub_ = nh.advertise("odin1/image/intensity_gray", 100); tf_broadcaster = std::make_unique(); } #endif diff --git a/include/lidar_api.h b/include/lidar_api.h index cad84d4..1324179 100644 --- a/include/lidar_api.h +++ b/include/lidar_api.h @@ -245,6 +245,16 @@ 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 enable device log + * + * + * @param device Handle to the target device + * @param dest_dir Destination directory to save the logs + * @return int 0 on success, -1 on failure + */ + int lidar_enable_encrypted_device_log(device_handle device, const char* dest_dir); + #ifdef __cplusplus } #endif diff --git a/include/lidar_api_type.h b/include/lidar_api_type.h index 40bbf91..5e7d56c 100644 --- a/include/lidar_api_type.h +++ b/include/lidar_api_type.h @@ -56,12 +56,14 @@ typedef enum { LIDAR_DT_DEV_STATUS, LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ, LIDAR_DT_SLAM_ODOMETRY_TF, + LIDAR_DT_SLAM_WIWC } lidar_data_type_e; typedef struct { int8_t serial[LIDAR_SERIAL_MAX]; int8_t model[LIDAR_MODEL_MAX]; bool online; + uint32_t initial_state; } lidar_device_info_t; typedef struct { @@ -187,6 +189,7 @@ typedef struct{ } lidar_soc_thermal_t; typedef struct { + double uptime_seconds; lidar_soc_thermal_t soc_thermal; int cpu_use_rate[8]; /* cpu usage rate */ @@ -202,6 +205,14 @@ typedef struct } lidar_device_status_t; +typedef enum { + LIDAR_DEVICE_NONE = 0, + LIDAR_DEVICE_NOT_INITIALIZED, + LIDAR_DEVICE_INITIALIZED, + LIDAR_DEVICE_STREAMING, + LIDAR_DEVICE_STREAM_STOPPED, +} lidar_device_initial_state_e; + #ifdef __cplusplus } #endif diff --git a/lib/liblydHostApi_amd.a b/lib/liblydHostApi_amd.a index 449725f..9ca742e 100644 Binary files a/lib/liblydHostApi_amd.a and b/lib/liblydHostApi_amd.a differ diff --git a/lib/liblydHostApi_arm.a b/lib/liblydHostApi_arm.a index 7928ebb..d0e7ece 100644 Binary files a/lib/liblydHostApi_arm.a and b/lib/liblydHostApi_arm.a differ diff --git a/src/host_sdk_sample.cpp b/src/host_sdk_sample.cpp index 4c0b916..3832366 100644 --- a/src/host_sdk_sample.cpp +++ b/src/host_sdk_sample.cpp @@ -35,6 +35,7 @@ limitations under the License. #include #include #include +#include // #include #include #include @@ -45,7 +46,9 @@ limitations under the License. #include #include #endif -#define ros_driver_version "0.6.1" +#define ros_driver_version "0.7.0" +#define recommended_firmware_version "0.8.0" + // Global variable declarations static device_handle odinDevice = nullptr; static std::atomic deviceConnected(false); @@ -88,6 +91,7 @@ int g_sendrgb = 1; int g_sendimu = 1; int g_senddtof = 1; int g_sendodom = 1; +int g_send_odom_baselink_tf = 0; int g_sendcloudslam = 0; int g_sendcloudrender = 0; int g_sendrgb_compressed = 0; @@ -98,6 +102,8 @@ int g_pub_intensity_gray = 0; int g_show_path = 0; int g_show_camerapose = 0; int g_strict_usb3_0_check = 0; +int g_use_host_ros_time = 0; +int g_save_log = 0; std::filesystem::path log_root_dir_; int g_custom_map_mode = 0; @@ -169,9 +175,27 @@ class RosNodeControlImpl : public RosNodeControlInterface { int getDtofSubframeODR() const override { return dtof_subframe_interval_time; } + + void setUseHostRosTime(bool use_host_ros_time) override { + pub_use_host_ros_time = use_host_ros_time; + } + + bool useHostRosTime() const override { + return pub_use_host_ros_time; + } + void setSendOdomBaseLinkTF(bool send_odom_baselink_tf) override { + pub_odom_baselink_tf = send_odom_baselink_tf; + } + + bool sendOdomBaseLinkTF() const override { + return pub_odom_baselink_tf; + } + private: int dtof_subframe_interval_time = 0; + bool pub_use_host_ros_time = false; + bool pub_odom_baselink_tf = false; }; static RosNodeControlImpl g_rosNodeControlImpl; @@ -790,9 +814,8 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data) if (dev_status_csv_file) { // append the data row int rc = 0; - rc = std::fprintf(dev_status_csv_file, "%d,%d,%d,%d,%d,%d,", // %.0f - // get_uptime_seconds(), - 0, + rc = std::fprintf(dev_status_csv_file, "%.2f,%d,%d,%d,%d,%d,", + dev_info_data->uptime_seconds, dev_info_data->soc_thermal.package_temp, dev_info_data->soc_thermal.cpu_temp, dev_info_data->soc_thermal.center_temp, @@ -937,7 +960,14 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data) break; case LIDAR_DT_SLAM_ODOMETRY_TF: { - g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, OdometryType::TRANSFORM, false, false); + if (g_custom_map_mode == 2) { + g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, OdometryType::TRANSFORM, false, false); + } + } + break; + case LIDAR_DT_SLAM_WIWC: + { + //... } break; default: @@ -988,18 +1018,6 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach #endif return; } - - if (lidar_open_device(odinDevice)) { - #ifdef ROS2 - RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Open device failed"); - #else - ROS_ERROR("Open device failed"); - #endif - lidar_destory_device(odinDevice); - odinDevice = nullptr; - return; - } - const std::string package_name = "odin_ros_driver"; std::string config_dir = ""; #ifdef ROS2 @@ -1024,7 +1042,41 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach #else ROS_INFO("Calibration files will be saved to: %s", config_dir.c_str()); #endif - + + std::filesystem::path per_con_log_root_dir; + { + auto connection_time = std::chrono::system_clock::now(); + std::time_t t = std::chrono::system_clock::to_time_t(connection_time); + std::tm tm{}; + #ifdef _WIN32 + localtime_s(&tm, &t); + #else + localtime_r(&t, &tm); + #endif + char buf[32]; + std::strftime(buf, sizeof(buf), "%Y%m%d_%H%M%S", &tm); + std::string folder_name = std::string("Conn_") + std::string(buf); + std::filesystem::path base_log_dir = log_root_dir_.empty() + ? std::filesystem::path(config_dir) + : log_root_dir_; + per_con_log_root_dir = base_log_dir / folder_name; + + std::error_code per_con_dir_err; + std::filesystem::create_directories(per_con_log_root_dir, per_con_dir_err); + if (per_con_dir_err) { + #ifdef ROS2 + RCLCPP_WARN(rclcpp::get_logger("device_cb"), + "Failed to create per-connection log directory %s: %s", + per_con_log_root_dir.c_str(), + per_con_dir_err.message().c_str()); + #else + ROS_WARN("Failed to create per-connection log directory %s: %s", + per_con_log_root_dir.c_str(), + per_con_dir_err.message().c_str()); + #endif + } + } + auto now = std::chrono::steady_clock::now(); auto elapsed = std::chrono::duration_cast(now - software_connect_start); if (elapsed.count() >= 60) { @@ -1055,30 +1107,122 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach exit(1); } else { - printf("ros_driver_version:%s\n", ros_driver_version); + printf("ros_driver_version:%s, recommended_firmware_version:%s\n", ros_driver_version, recommended_firmware_version); printf("get version success.\n"); } - - if (lidar_get_calib_file(odinDevice, config_dir.c_str())) { + + if (g_save_log) { + if (lidar_enable_encrypted_device_log(const_cast(device), per_con_log_root_dir.c_str())) { + #ifdef ROS2 + RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Enable log failed"); + #else + ROS_ERROR("Enable log failed"); + #endif + lidar_close_device(odinDevice); + lidar_destory_device(odinDevice); + odinDevice = nullptr; + return; + } + #ifdef ROS2 - RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Failed to get calibration file"); + RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Encrypted device log enabled at: %s", per_con_log_root_dir.c_str()); #else - ROS_ERROR("Failed to get calibration file"); + ROS_INFO("Encrypted device log enabled at: %s", per_con_log_root_dir.c_str()); + #endif + } else { + #ifdef ROS2 + RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Encrypted device log disabled via configuration"); + #else + ROS_INFO("Encrypted device log disabled via configuration"); #endif - lidar_close_device(odinDevice); - lidar_destory_device(odinDevice); - odinDevice = nullptr; - return; } - - #ifdef ROS2 - RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Successfully retrieved calibration files"); - #else - ROS_INFO("Successfully retrieved calibration files"); - #endif + + bool need_open_device = true; + bool need_configure_device = true; + switch (device->initial_state) { + case LIDAR_DEVICE_NOT_INITIALIZED: + #ifdef ROS2 + RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Device state: not initialized, performing full setup"); + #else + ROS_INFO("Device state: not initialized, performing full setup"); + #endif + break; + case LIDAR_DEVICE_INITIALIZED: + need_open_device = false; + #ifdef ROS2 + RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Device state: initialized, skip opening device"); + #else + ROS_INFO("Device state: initialized, skip opening device"); + #endif + break; + case LIDAR_DEVICE_STREAMING: + need_open_device = false; + need_configure_device = false; + #ifdef ROS2 + RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Device state: streaming, skip opening and configuring"); + #else + ROS_INFO("Device state: streaming, skip opening and configuring"); + #endif + break; + case LIDAR_DEVICE_STREAM_STOPPED: + need_open_device = false; + need_configure_device = false; + #ifdef ROS2 + RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Device state: stream stopped, resume streaming"); + #else + ROS_INFO("Device state: stream stopped, resume streaming"); + #endif + break; + default: + #ifdef ROS2 + RCLCPP_WARN(rclcpp::get_logger("device_cb"), "Unknown device initial state: %d", device->initial_state); + #else + ROS_WARN("Unknown device initial state: %d", device->initial_state); + #endif + break; + } + + if (need_open_device) { + if (lidar_open_device(odinDevice)) { + #ifdef ROS2 + RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Open device failed"); + #else + ROS_ERROR("Open device failed"); + #endif + lidar_destory_device(odinDevice); + odinDevice = nullptr; + return; + } + } std::string calib_config = config_dir + "/calib.yaml"; calib_file_ = calib_config; + if (need_configure_device) { + if (lidar_get_calib_file(odinDevice, config_dir.c_str())) { + #ifdef ROS2 + RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Failed to get calibration file"); + #else + ROS_ERROR("Failed to get calibration file"); + #endif + lidar_close_device(odinDevice); + lidar_destory_device(odinDevice); + odinDevice = nullptr; + return; + } + + #ifdef ROS2 + RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Successfully retrieved calibration files"); + #else + ROS_INFO("Successfully retrieved calibration files"); + #endif + } else { + #ifdef ROS2 + RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Skipping calibration retrieval for current device state"); + #else + ROS_INFO("Skipping calibration retrieval for current device state"); + #endif + } + if (std::filesystem::exists(calib_config)) { g_renderer = std::make_shared(); if (g_renderer->init(calib_config)) { @@ -1102,24 +1246,32 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach #endif } - if (lidar_set_mode(odinDevice, type)) { - #ifdef ROS2 - RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Set mode failed"); - #else - ROS_ERROR("Set mode failed"); - #endif - lidar_close_device(odinDevice); - lidar_destory_device(odinDevice); - odinDevice = nullptr; - return; - } + if (need_configure_device) { + if (lidar_set_mode(odinDevice, type)) { + #ifdef ROS2 + RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Set mode failed"); + #else + ROS_ERROR("Set mode failed"); + #endif + lidar_close_device(odinDevice); + lidar_destory_device(odinDevice); + odinDevice = nullptr; + return; + } - // Apply custom parameters after setting mode - if (g_parser && !g_parser->applyCustomParameters(odinDevice)) { + // Apply custom parameters after setting mode + if (g_parser && !g_parser->applyCustomParameters(odinDevice)) { + #ifdef ROS2 + RCLCPP_WARN(rclcpp::get_logger("device_cb"), "Some custom parameters failed to apply"); + #else + ROS_WARN("Some custom parameters failed to apply"); + #endif + } + } else { #ifdef ROS2 - RCLCPP_WARN(rclcpp::get_logger("device_cb"), "Some custom parameters failed to apply"); + RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Skipping device mode configuration for current state"); #else - ROS_WARN("Some custom parameters failed to apply"); + ROS_INFO("Skipping device mode configuration for current state"); #endif } @@ -1166,20 +1318,7 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach return; } - auto con_time = std::chrono::system_clock::now(); - std::time_t t = std::chrono::system_clock::to_time_t(con_time); - std::tm tm{}; - #ifdef _WIN32 - localtime_s(&tm, &t); - #else - localtime_r(&t, &tm); - #endif - char buf[32]; - std::strftime(buf, sizeof(buf), "%Y%m%d_%H%M%S", &tm); - std::string folder_name = std::string("Conn_") + std::string(buf); - std::filesystem::path per_con_log_root_dir_ = log_root_dir_ / folder_name; - std::filesystem::create_directories(per_con_log_root_dir_); - std::string dev_status_csv_file_path_ = per_con_log_root_dir_ / "dev_status.csv"; + std::string dev_status_csv_file_path_ = per_con_log_root_dir / "dev_status.csv"; if (dev_status_csv_file) { std::fflush(dev_status_csv_file); @@ -1359,6 +1498,7 @@ int main(int argc, char *argv[]) g_sendimu = get_key_value("sendimu", 1); g_senddtof = get_key_value("senddtof", 1); g_sendodom = get_key_value("sendodom", 1); + g_send_odom_baselink_tf = get_key_value("send_odom_baselink_tf", 0); g_sendcloudslam = get_key_value("sendcloudslam", 0); g_sendcloudrender = get_key_value("sendcloudrender", 1); g_sendrgb_compressed = get_key_value("sendrgbcompressed", 1); @@ -1371,6 +1511,16 @@ int main(int argc, char *argv[]) g_show_camerapose = get_key_value("showcamerapose", 0); g_log_level = get_key_value("log_devel", LOG_LEVEL_INFO); g_strict_usb3_0_check = get_key_value("strict_usb3.0_check", 1); + g_use_host_ros_time = get_key_value("use_host_ros_time", 0); + g_save_log = get_key_value("save_log", 0); + + if (g_use_host_ros_time) { + g_rosNodeControlImpl.setUseHostRosTime(true); + } + + if (g_send_odom_baselink_tf) { + g_rosNodeControlImpl.setSendOdomBaseLinkTF(true); + } auto get_key_str_value = [&](const std::string& key, const std::string& default_value) -> std::string { auto it = keys_w_str_val.find(key);