slam toolbox working

src/dcaitirobot/config/slam.yml

@@ -1,3 +1,5 @@
 slam_toolbox:
   ros__parameters:
     base_frame: base_link
+    map_update_interval: 1.0
+    scan_topic: /scan_no_nan

src/dcaitirobot/dcaitirobot/handle_nan_scan.py

@@ -0,0 +1,42 @@
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import LaserScan
+import geometry_msgs.msg as gm
+
+
+class TF2PublisherNode(Node):
+
+    def __init__(self):
+        super().__init__('nanhandler')
+        
+        self.create_subscription(LaserScan, "/scan", self.callback, 1)
+        self.pub = self.create_publisher(LaserScan, "/scan_no_nan", 1)
+    def callback(self, msg):
+        print(msg.ranges[0])
+        
+        msg.ranges[0] = float("inf")
+        msg.intensities[0] = 0.0
+
+        msg.ranges.append(float("inf"))
+        msg.intensities.append(0.0)
+        # ranges = np.array(msg.ranges)
+        # mask_inf_dist = np.isinf(ranges)
+        # print(np.array(msg.intensities)[mask_inf_dist])
+        
+        self.pub.publish(msg)
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    tf2_publisher = TF2PublisherNode()
+    rclpy.spin(tf2_publisher)
+
+    tf2_publisher.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/dcaitirobot/dcaitirobot/tf2pleasework.py

@@ -0,0 +1,57 @@
+import rclpy
+from rclpy.node import Node
+import tf2_ros
+import geometry_msgs.msg as gm
+
+
+
+
+class TF2PublisherNode(Node):
+
+    def __init__(self):
+        super().__init__('tf2_publisher_node')
+        self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)
+        
+        self.timer = self.create_timer(0.1, self.publish_identity_transform)
+
+    def send_parent_child_identity_tf(self, parent, child):
+        transform_stamped = gm.TransformStamped()
+        transform_stamped.header.stamp = self.get_clock().now().to_msg()
+        transform_stamped.header.frame_id = parent
+        transform_stamped.child_frame_id = child
+
+        # Identity transform (no translation, no rotation)
+        transform_stamped.transform.translation.x = 0.0
+        transform_stamped.transform.translation.y = 0.0
+        transform_stamped.transform.translation.z = 0.0
+        transform_stamped.transform.rotation.x = 0.0
+        transform_stamped.transform.rotation.y = 0.0
+        transform_stamped.transform.rotation.z = 0.0
+        transform_stamped.transform.rotation.w = 1.0
+
+        self.tf_broadcaster.sendTransform(transform_stamped)
+
+
+    def publish_identity_transform(self):
+        # self.send_parent_child_identity_tf("map", "odom")
+        # self.send_parent_child_identity_tf("odom", "base_link")
+       # self.send_parent_child_identity_tf("base_link", "base_scan")
+       # self.send_parent_child_identity_tf("base_scan", "velodyne")
+        self.send_parent_child_identity_tf("base_link", "velodyne")
+        self.send_parent_child_identity_tf("base_link", "base_footprint")
+    
+
+        self.get_logger().info('Published identity transform')
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    tf2_publisher = TF2PublisherNode()
+    rclpy.spin(tf2_publisher)
+
+    tf2_publisher.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/dcaitirobot/launch/frames.gv

@@ -0,0 +1,19 @@
+digraph G {
+"odom" -> "base_footprint"[label=" Broadcaster: default_authority\nAverage rate: 29.645\nBuffer length: 4.284\nMost recent transform: 109.56\nOldest transform: 105.276\n"];
+"base_footprint" -> "base_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"camera_link" -> "camera_depth_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"base_link" -> "camera_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"camera_depth_frame" -> "camera_depth_optical_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"camera_link" -> "camera_rgb_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"camera_rgb_frame" -> "camera_rgb_optical_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"base_link" -> "caster_back_left_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"base_link" -> "caster_back_right_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"base_link" -> "imu_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"base_link" -> "base_scan"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"base_link" -> "wheel_left_link"[label=" Broadcaster: default_authority\nAverage rate: 17.882\nBuffer length: 4.25\nMost recent transform: 109.56\nOldest transform: 105.31\n"];
+"base_link" -> "wheel_right_link"[label=" Broadcaster: default_authority\nAverage rate: 17.882\nBuffer length: 4.25\nMost recent transform: 109.56\nOldest transform: 105.31\n"];
+edge [style=invis];
+ subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";
+"Recorded at time: 1688900208.2025208"[ shape=plaintext ] ;
+}->"odom";
+}

src/dcaitirobot/launch/launch_slam.py

@@ -9,9 +9,8 @@ from ament_index_python import get_package_share_directory
 
 
 def generate_launch_description():
-
     slam_params = os.path.join(
-        get_package_share_directory('dcaitirobot'), # <-- Replace with your package name
+        str(Path(__file__).absolute().parent.parent), # <-- Replace with your package name
         'config',
         'slam.yml'
         )
@@ -27,6 +26,21 @@ def generate_launch_description():
                     'slam_params_file': slam_params}.items()
     )
 
+    node_fix_lidar = Node(
+                package="dcaitirobot",
+                executable="fixlidar",
+                name="fixlidar",
+            )
+
+    tfs1 = Node(package = "tf2_ros", 
+                       executable = "static_transform_publisher",
+                       arguments = ["0", "0", "0" ,"0", "0", "0", "base_link", "velodyne"])
+
+    tfs2 = Node(package = "tf2_ros", 
+                       executable = "static_transform_publisher",
+                       arguments = ["0", "0", "0" ,"0", "0", "0", "base_link", "base_footprint"])
+
+
     return LaunchDescription([
-        slam_toolbox
+        slam_toolbox, tfs1, tfs2, node_fix_lidar
     ])

src/dcaitirobot/launch/start_turtle_slam_sim.py

@@ -28,17 +28,17 @@ def generate_launch_description():
                     )
                 )
             ),
-            IncludeLaunchDescription(
-                PythonLaunchDescriptionSource(
-                    str(
-                        (
-                            Path(get_package_share_directory("slam_toolbox")).absolute()
-                            / "launch"
-                            / "online_async_launch.py"
-                        )
-                    )
-                ),
-                launch_arguments=[("use_sim_time", "True")],
-            ),
+            # IncludeLaunchDescription(
+            #     PythonLaunchDescriptionSource(
+            #         str(
+            #             (
+            #                 Path(get_package_share_directory("slam_toolbox")).absolute()
+            #                 / "launch"
+            #                 / "online_async_launch.py"
+            #             )
+            #         )
+            #     ),
+            #     launch_arguments=[("use_sim_time", "True")],
+            # ),
         ]
     )

src/dcaitirobot/setup.py

@@ -28,6 +28,7 @@ setup(
             'twistcalc = dcaitirobot.twist:main',
             'serial_comms = dcaitirobot.serial_comms:main',
             'joy = dcaitirobot.joy:main',
+            "fixlidar = dcaitirobot.handle_nan_scan:main"
         ],
     },
 )