Merge branch 'main' of github.com:Rooholla-KhorramBakht/Go2Py into main

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<double>::infinity());
+  // scan_msg->intensities.assign(ranges_size, 255);
   // Iterate through pointcloud
   for (sensor_msgs::PointCloud2ConstIterator<float> iter_x(*cloud_msg, "x"),
     iter_y(*cloud_msg, "y"), iter_z(*cloud_msg, "z");

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
    ])

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