added 180 filter

Author: N-rwal

go2_robot_sdk/config/mapper_params_online_async.yaml

@@ -13,22 +13,22 @@ slam_toolbox:
     odom_frame: odom
     map_frame: map
     base_frame: base_link
-    scan_topic: /scan
+    scan_topic: /scan_filtered
     use_map_saver: true
     mode: mapping
 
     # if you'd like to immediately start continuing a map at a given pose
     # or at the dock, but they are mutually exclusive, if pose is given
     # will use pose
-    map_start_pose: [0.0, 0.0, 0.0]
-    #map_start_at_dock: true
+    #map_start_pose: [0.0, 0.0, 0.0]
+    map_start_at_dock: true
 
     debug_logging: false
     throttle_scans: 1
     transform_publish_period: 0.02 # if 0 never publishes odometry
     map_update_interval: 1.0
-    resolution: 0.05
-    max_laser_range: 5.0
+    resolution: 0.07
+    max_laser_range: 7.0
     minimum_time_interval: 0.5
     transform_timeout: 0.2
     tf_buffer_duration: 30.0

go2_robot_sdk/config/nav2_params_navigation.yaml

@@ -19,7 +19,7 @@ amcl:
     laser_model_type: "likelihood_field"
     max_beams: 90
     max_particles: 800
-    min_particles: 200
+    min_particles: 300
     odom_frame_id: "odom"
     pf_err: 0.05
     pf_z: 0.99
@@ -38,7 +38,7 @@ amcl:
     z_max: 0.05
     z_rand: 0.03
     z_short: 0.05
-    scan_topic: "scan"
+    scan_topic: "scan_filtered"
 #    set_initial_pose: false
 
 bt_navigator:
@@ -100,7 +100,7 @@ controller_server:
     # DWB parameters
     FollowPath:
       plugin: "dwb_core::DWBLocalPlanner"
-      debug_trajectory_details: True
+      #debug_trajectory_details: True
       min_vel_x: 0.0
       min_vel_y: 0.0
       max_vel_x: 2.0 #changed
@@ -168,7 +168,7 @@ local_costmap:
         mark_threshold: 1
         observation_sources: scan
         scan:
-          topic: /scan
+          topic: /scan_filtered
           max_obstacle_height: 2.0
           clearing: True
           marking: True
@@ -201,7 +201,7 @@ global_costmap:
         enabled: True
         observation_sources: scan
         scan:
-          topic: /scan
+          topic: /scan_filtered
           max_obstacle_height: 2.0
           clearing: True
           marking: True
@@ -333,7 +333,7 @@ collision_monitor:
     observation_sources: ["scan"]
     scan:
       type: "scan"
-      topic: "scan"
+      topic: "scan_filtered"
       min_height: 0.15
       max_height: 2.0
       enabled: True

go2_robot_sdk/go2_robot_sdk/scan_angle_filter.py

@@ -0,0 +1,66 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import LaserScan
+import math
+
+
+class ScanAngleFilter(Node):
+
+    def __init__(self):
+        super().__init__('scan_angle_filter')
+
+        # keep rear 180° -> outside [-pi/2, +pi/2]
+        self.front_min = -math.pi / 2
+        self.front_max = math.pi / 2
+
+        self.sub = self.create_subscription(
+            LaserScan,
+            '/scan',
+            self.scan_callback,
+            10
+        )
+
+        self.pub = self.create_publisher(
+            LaserScan,
+            '/scan_filtered',
+            10
+        )
+
+    def scan_callback(self, msg):
+
+        filtered = LaserScan()
+
+        # copy metadata
+        filtered.header = msg.header
+        filtered.angle_min = msg.angle_min
+        filtered.angle_max = msg.angle_max
+        filtered.angle_increment = msg.angle_increment
+        filtered.time_increment = msg.time_increment
+        filtered.scan_time = msg.scan_time
+        filtered.range_min = msg.range_min
+        filtered.range_max = msg.range_max
+
+        filtered.ranges = list(msg.ranges)
+        filtered.intensities = list(msg.intensities)
+
+        for i in range(len(filtered.ranges)):
+
+            angle = msg.angle_min + i * msg.angle_increment
+
+            # mask the FRONT 180°
+            if self.front_min <= angle <= self.front_max:
+                filtered.ranges[i] = float('inf')
+
+        self.pub.publish(filtered)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ScanAngleFilter()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

go2_robot_sdk/go2_robot_sdk/sequential_nav_test.py

@@ -57,18 +57,14 @@ def send_goal(self, pose):
 
     def run_sequence(self):
         i = 0
-        for i in range (0,2):
-            self.send_goal(self.make_pose(-1.61775, 17.2127, 1.70432)) #main entrance
+        for i in range (0,3):
+            self.send_goal(self.make_pose(-46.2362, 73.379, -1.29229)) #main entrance
             time.sleep(1.0)
-            self.send_goal(self.make_pose(9.57915, 19.4981, -1.21812)) #front main entrance
+            self.send_goal(self.make_pose(-51.2816, 68.0833, -3.08686)) #front main entrance
             time.sleep(1.0)
-            self.send_goal(self.make_pose(-1.61775, 17.2127, 2.36622)) #main entrance
+            self.send_goal(self.make_pose(-55.4247, 72.4208, 1.56608)) #main entrance
             time.sleep(1.0)
-            self.send_goal(self.make_pose(2.27953, -1.53849, -1.41637)) #second hall
-            time.sleep(1.0)
-            self.send_goal(self.make_pose(12.5844, 1.70121, -1.21812)) #front second entrance
-            time.sleep(1.0)
-            self.send_goal(self.make_pose(2.27953, -1.53849, 2.36622)) #second hall
+            self.send_goal(self.make_pose(-50.5007, 77.558, -0.357111)) #second hall
             time.sleep(1.0)
 
 

go2_robot_sdk/go2_robot_sdk/wip_main.py

@@ -0,0 +1,91 @@
+#!/usr/bin/env python3
+
+import serial
+import socket
+import threading
+import time
+from enum import IntFlag
+
+
+class Outputs(IntFlag):
+    MAIN_LED = 0x08
+    STATUS_LED = 0x02
+    ALARM = 0x04
+
+alarm_stop_event = threading.Event()
+
+UDP_IP = "0.0.0.0"
+UDP_PORT = 5005
+
+def send_command(ser, cmd_byte):
+    ser.write(bytes([cmd_byte]))
+
+def flash_led_alarm(ser, duration=5):
+    end_time = time.time() + duration
+    while time.time() < end_time:
+        if alarm_stop_event.is_set():  # Stop immediately if event is set
+            break
+        send_command(ser, Outputs.MAIN_LED | Outputs.ALARM)
+        time.sleep(0.5)
+        if alarm_stop_event.is_set():
+            break
+        send_command(ser, 0x00)
+        time.sleep(0.5)
+
+    send_command(ser, 0x00)
+    alarm_stop_event.clear()
+
+    send_command(ser, 0x00)
+
+def handle_command(ser, command):
+    cmd_num = int(command)
+
+    if cmd_num == 0:
+        alarm_stop_event.set()  # Stop any running alarm
+        send_command(ser, 0x00)
+
+    elif cmd_num == 1:
+        threading.Thread(
+            target=flash_led_alarm,
+            args=(ser, 5),
+            daemon=True
+        ).start()
+
+    elif cmd_num == 2:
+        send_command(ser, Outputs.MAIN_LED)
+
+def main():
+
+    ser = serial.Serial('/dev/ttyUSB1', 115200, timeout=1)
+
+    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    sock.bind((UDP_IP, UDP_PORT))
+
+    print(f"Listening for UDP commands on port {UDP_PORT}")
+
+    try:
+
+        while True:
+
+            data, addr = sock.recvfrom(1024)
+            message = data.decode().strip()
+            send_command(ser, 0x02)
+
+            print(f"Received UDP from {addr}: {message}")
+
+            if message.startswith("cmd:"):
+                cmd_value = message.split("cmd:", 1)[1]
+                handle_command(ser, cmd_value)
+            else:
+                print("Ignored non-command message")
+
+    except KeyboardInterrupt:
+        pass
+
+    finally:
+        ser.close()
+        sock.close()
+
+if __name__ == "__main__":
+    main()
+

go2_robot_sdk/launch/2_mapping.launch.py

@@ -1,6 +1,3 @@
-# Minimal mapping launch file - optimized for SLAM with external 2D LiDAR
-# Usage: ros2 launch go2_robot_sdk mapping_minimal.launch.py
-
 import os
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
@@ -29,8 +26,6 @@ def generate_launch_description():
         'sllidar_launch': os.path.join(sllidar_package_dir, 'launch', 'sllidar_s3_launch.py'),
     }
 
-    print(f"Minimal mapping stack")
-
     # Launch arguments
     use_sim_time = LaunchConfiguration('use_sim_time', default='false')
     with_slam = LaunchConfiguration('slam', default='true')
@@ -74,7 +69,7 @@ def generate_launch_description():
             package='tf2_ros',
             executable='static_transform_publisher',
             name='static_transform_publisher',
-            arguments=['0.2', '0', '0.1', '0', '0', '0', 'base_link', 'laser'],
+            arguments=['0.2', '0', '0.1', '3.1415926535', '0', '0', 'base_link', 'laser'],
         ),
     ]
 
@@ -96,9 +91,18 @@ def generate_launch_description():
         }.items(),
     )
 
+    #LiDAR filter
+    filterer_node = Node(
+        package='go2_robot_sdk',
+        executable='scan_angle_filter',
+        name='scan_angle_filter',
+        output='screen'
+    )
+
+
     # Combine all components
     return LaunchDescription(
         launch_args +
         core_nodes +
-        [lidar_launch, slam_launch]  # Combine them in a list
+        [lidar_launch, slam_launch, filterer_node]
     )

go2_robot_sdk/launch/navigation_bringup_launch.py

@@ -73,7 +73,7 @@ def generate_launch_description():
             package='tf2_ros',
             executable='static_transform_publisher',
             name='base_to_laser_tf',
-            arguments=['0.2', '0', '0.1', '0', '0', '0', 'base_link', 'laser'],
+            arguments=['0.2', '0', '0.1', '3.1415926535', '0', '0', 'base_link', 'laser'],
         ),
 
         # Initial map to odom transform
@@ -125,10 +125,24 @@ def generate_launch_description():
             )
         ]
     )
+    #LiDAR filter
+    filterer_node = Node(
+        package='go2_robot_sdk',
+        executable='scan_angle_filter',
+        name='scan_angle_filter',
+        output='screen'
+    )#
+    #alarm script
+    alarm_node = Node(
+        package='go2_robot_sdk',
+        executable='wip_main',
+        name='wip_main',
+        output='screen'
+    )
 
     # Combine all components
     return LaunchDescription(
         launch_args +
         core_nodes +
-        [lidar_launch, nav_launch, initial_pose_timer, controller_node]
+        [lidar_launch, nav_launch, initial_pose_timer, controller_node, filterer_node, alarm_node]
     )

go2_robot_sdk/map/office.pgm

@@ -1,7 +1,7 @@
 image: office.pgm
 mode: trinary
-resolution: 0.050
-origin: [-6.633, -6.401, 0]
+resolution: 0.070
+origin: [-62.084, 63.224, 0]
 negate: 0
 occupied_thresh: 0.65
 free_thresh: 0.196

go2_robot_sdk/map/office.yaml

@@ -41,6 +41,8 @@
             'initial_pose_pub = go2_robot_sdk.initial_pose_pub:main',
             'sequential_nav_test = go2_robot_sdk.sequential_nav_test:main',
             'nav_controller = go2_robot_sdk.nav_controller:main',
+            'scan_angle_filter = go2_robot_sdk.scan_angle_filter:main',
+            'wip_main = go2_robot_sdk.wip_main:main',
         ],
     },
 )