From ee8edb14c34c6bdb360c9060210eb302f538c1dc Mon Sep 17 00:00:00 2001 From: Rooholla-KhorramBakht Date: Wed, 14 Feb 2024 09:03:10 +0800 Subject: [PATCH] laser scan publisher bug fixed --- .../src/manager/source_driver_ros2.hpp | 9 ++++---- .../sportmode_nav2/launch/mapping.launch.py | 21 ------------------- .../sportmode_nav2/params/mapping_async.yaml | 2 +- 3 files changed, 6 insertions(+), 26 deletions(-) diff --git a/deploy/dock_ws/src/lidar_node/src/manager/source_driver_ros2.hpp b/deploy/dock_ws/src/lidar_node/src/manager/source_driver_ros2.hpp index 2c53ba6..a936a4a 100644 --- a/deploy/dock_ws/src/lidar_node/src/manager/source_driver_ros2.hpp +++ b/deploy/dock_ws/src/lidar_node/src/manager/source_driver_ros2.hpp @@ -295,13 +295,13 @@ inline void SourceDriver::publishLaserScan( scan_msg->angle_min = -3.1415; scan_msg->angle_max = 3.1415; - scan_msg->angle_increment = 2*3.1415/1800.; + scan_msg->angle_increment = 0.0087/2.; scan_msg->time_increment = 0.0; - scan_msg->scan_time = 1./10.; + scan_msg->scan_time = 0.3333; scan_msg->range_min = 0.2; scan_msg->range_max = 100; - float max_height_ = 0.5; - float min_height_ = -0.5; + float max_height_ = 1.0; + float min_height_ = 0.; // determine amount of rays to create @@ -310,6 +310,7 @@ inline void SourceDriver::publishLaserScan( // determine if laserscan rays with no obstacle data will evaluate to infinity or max_range scan_msg->ranges.assign(ranges_size, std::numeric_limits::infinity()); + // scan_msg->intensities.assign(ranges_size, 255); // Iterate through pointcloud for (sensor_msgs::PointCloud2ConstIterator iter_x(*cloud_msg, "x"), iter_y(*cloud_msg, "y"), iter_z(*cloud_msg, "z"); diff --git a/deploy/nav2_ws/src/sportmode_nav2/launch/mapping.launch.py b/deploy/nav2_ws/src/sportmode_nav2/launch/mapping.launch.py index 5d9d345..526fc21 100644 --- a/deploy/nav2_ws/src/sportmode_nav2/launch/mapping.launch.py +++ b/deploy/nav2_ws/src/sportmode_nav2/launch/mapping.launch.py @@ -9,26 +9,6 @@ from launch_ros.actions import Node def generate_launch_description(): - laser_scan = Node( - package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node', - remappings=[('cloud_in', '/go2/lidar_points'), - ('scan', '/go2/lidar_scans')], - parameters=[{ - 'target_frame': 'go2/hesai_lidar', - 'transform_tolerance': 0.01, - 'min_height': 0.0, - 'max_height': 1.0, - 'angle_min': -1.5708, # -M_PI/2 - 'angle_max': 1.5708, # M_PI/2 - 'angle_increment': 0.0087, # M_PI/360.0 - 'scan_time': 0.3333, - 'range_min': 0.45, - 'range_max': 100.0, - 'use_inf': True, - 'inf_epsilon': 1.0 - }], - name='pointcloud_to_laserscan' - ) robot_localization = IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join( @@ -44,5 +24,4 @@ def generate_launch_description(): return LaunchDescription([ robot_localization, async_mapping, - laser_scan ]) \ No newline at end of file diff --git a/deploy/nav2_ws/src/sportmode_nav2/params/mapping_async.yaml b/deploy/nav2_ws/src/sportmode_nav2/params/mapping_async.yaml index ceccbda..b4dfd4c 100644 --- a/deploy/nav2_ws/src/sportmode_nav2/params/mapping_async.yaml +++ b/deploy/nav2_ws/src/sportmode_nav2/params/mapping_async.yaml @@ -13,7 +13,7 @@ slam_toolbox: odom_frame: odom map_frame: map base_frame: base_link # ToDo: add a base_footprint to the URDF - scan_topic: /go2/lidar_scans + scan_topic: /go2/scan mode: mapping #localization # if you'd like to immediately start continuing a map at a given pose