From 0c40a1d87b05e02b318c8babadce579919502339 Mon Sep 17 00:00:00 2001 From: Abraham Monrroy Cano Date: Tue, 8 Aug 2023 13:35:04 +0900 Subject: [PATCH 1/2] refactor: rename transport_drivers to boost_transport_driver (#46) * rename transport_drivers to boost_transport_driver Signed-off-by: amc-nu * cspell. fix Signed-off-by: amc-nu --------- Signed-off-by: amc-nu --- build_depends.repos | 2 +- .../nebula_hw_interface_base.hpp | 4 ++-- .../nebula_hw_interfaces_hesai/hesai_hw_interface.hpp | 6 +++--- .../velodyne_hw_interface.hpp | 4 ++-- nebula_hw_interfaces/package.xml | 4 ++-- .../nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp | 2 +- .../nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp | 7 ++++++- nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp | 8 -------- 8 files changed, 17 insertions(+), 20 deletions(-) diff --git a/build_depends.repos b/build_depends.repos index c00499828..e28379f1a 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -3,4 +3,4 @@ repositories: transport_drivers: type: git url: https://github.com/MapIV/transport_drivers - version: tcp + version: boost diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp index c46194f5e..026e55d63 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp @@ -3,7 +3,7 @@ #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" -#include "udp_driver/udp_driver.hpp" +#include "boost_udp_driver/udp_driver.hpp" #include #include @@ -20,7 +20,7 @@ class NebulaHwInterfaceBase /** * Callback function to receive the Cloud Packet data from the UDP Driver * @param buffer Buffer containing the data received from the UDP socket - * @return Status::OK if no error occured. + * @return Status::OK if no error occurred. */ virtual void ReceiveCloudPacketCallback(const std::vector & buffer) = 0; // virtual Status RegisterScanCallback( diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index bc9f56a67..5ebeb91bb 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -14,9 +14,9 @@ #include "nebula_common/hesai/hesai_status.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp" -#include "tcp_driver/http_client_driver.hpp" -#include "tcp_driver/tcp_driver.hpp" -#include "udp_driver/udp_driver.hpp" +#include "boost_tcp_driver/http_client_driver.hpp" +#include "boost_tcp_driver/tcp_driver.hpp" +#include "boost_udp_driver/udp_driver.hpp" #include diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp index efc90bdbd..a2bb8fc70 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp @@ -14,8 +14,8 @@ #include "nebula_common/velodyne/velodyne_common.hpp" #include "nebula_common/velodyne/velodyne_status.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" -#include "tcp_driver/http_client_driver.hpp" -#include "udp_driver/udp_driver.hpp" +#include "boost_tcp_driver/http_client_driver.hpp" +#include "boost_udp_driver/udp_driver.hpp" #include diff --git a/nebula_hw_interfaces/package.xml b/nebula_hw_interfaces/package.xml index 816f53302..aa169d320 100644 --- a/nebula_hw_interfaces/package.xml +++ b/nebula_hw_interfaces/package.xml @@ -16,8 +16,8 @@ pandar_msgs rclcpp sensor_msgs - tcp_driver - udp_driver + boost_tcp_driver + boost_udp_driver velodyne_msgs ament_cmake_gtest diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp index fe1e43c79..dd65699bd 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp @@ -5,7 +5,7 @@ #include "nebula_common/nebula_common.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" #include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" -#include "tcp_driver/tcp_driver.hpp" +#include "boost_tcp_driver/tcp_driver.hpp" #include #include diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp index b0d7f2a45..b6484e73a 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp @@ -5,7 +5,7 @@ #include "nebula_common/nebula_common.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" #include "nebula_ros/common/nebula_hw_monitor_ros_wrapper_base.hpp" -#include "tcp_driver/tcp_driver.hpp" +#include "boost_tcp_driver/tcp_driver.hpp" #include #include @@ -15,6 +15,11 @@ #include #include +#include +#include +#include + +#include namespace nebula { diff --git a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp index e2fc38d60..0c77058f8 100644 --- a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp @@ -1,13 +1,5 @@ #include "nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp" -#include "tcp_driver/tcp_driver.hpp" - -#include -#include -#include - -#include - namespace nebula { namespace ros From cb258963caf7c4262a3336b5fe83d40d25169efe Mon Sep 17 00:00:00 2001 From: Abraham Monrroy Cano Date: Wed, 9 Aug 2023 07:55:52 +0900 Subject: [PATCH 2/2] fix: Update velodyne point stamp calculation (#47) * Update nebula_common.cpp * Update nebula_common.cpp * Update velodyne_decoder_ros_wrapper.cpp * match AWF hecta degrees in the azimuth --- nebula_common/src/nebula_common.cpp | 4 ++-- nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/nebula_common/src/nebula_common.cpp b/nebula_common/src/nebula_common.cpp index ce2594b93..7af599032 100644 --- a/nebula_common/src/nebula_common.cpp +++ b/nebula_common/src/nebula_common.cpp @@ -58,9 +58,9 @@ pcl::PointCloud::Ptr convertPointXYZIRCAEDTToPointXYZIRADT( point.z = p.z; point.intensity = p.intensity; point.ring = p.channel; - point.azimuth = rad2deg(p.azimuth); + point.azimuth = rad2deg(p.azimuth) * 100.0; point.distance = p.distance; - point.time_stamp = static_cast(stamp) + p.time_stamp*10e-9; + point.time_stamp = stamp + static_cast(p.time_stamp)*1e-9; output_pointcloud->points.emplace_back(point); } diff --git a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp index c0632c6d8..56a54be34 100644 --- a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp @@ -49,6 +49,7 @@ void VelodyneDriverRosWrapper::ReceiveScanMsgCallback( std::tuple pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(scan_msg); nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); + double cloud_stamp = std::get<1>(pointcloud_ts); if (pointcloud == nullptr) { RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); return; @@ -77,7 +78,7 @@ void VelodyneDriverRosWrapper::ReceiveScanMsgCallback( aw_points_ex_pub_->get_subscription_count() > 0 || aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { const auto autoware_ex_cloud = - nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT(pointcloud, 0.0);//velodyne drivers originally doesn't use absolute time + nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT(pointcloud, cloud_stamp); auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); ros_pc_msg_ptr->header.stamp =