Merge branch 'main' of github.com:dcaiti-robot/dcaiti_ws

src/dcaitirobot/config/slam.yml

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

src/dcaitirobot/dcaitirobot/scale_odom.py

@@ -0,0 +1,65 @@
+import rclpy
+from rclpy.node import Node
+
+from geometry_msgs.msg import Twist
+from std_msgs.msg import String
+from nav_msgs.msg import Odometry
+
+import numpy as np
+
+
+SCALE_FACTOR = 20.0
+
+
+
+class TwistPublisher(Node):
+    def __init__(
+        self,
+        scale_factor=20.0
+    ):
+        super().__init__("twist_publisher")
+
+        self.scale_factor = scale_factor
+
+        self.publisher_ = self.create_publisher(
+            Odometry, "/odom_scaled", 1
+        )
+
+        self.create_subscription(Odometry, "/odom", self.odom_callback, 1)
+
+    def odom_callback(self, odom_message:Odometry):
+        odom_message.twist.twist.linear.x *= self.scale_factor
+        odom_message.twist.twist.linear.y *= self.scale_factor
+        odom_message.twist.twist.linear.z *= self.scale_factor
+        odom_message.twist.twist.angular.x *= self.scale_factor
+        odom_message.twist.twist.angular.y *= self.scale_factor
+        odom_message.twist.twist.angular.z *= self.scale_factor
+        odom_message.pose.pose.position.x *= self.scale_factor
+        odom_message.pose.pose.position.y *= self.scale_factor
+        odom_message.pose.pose.position.z *= self.scale_factor
+        odom_message.pose.pose.orientation.x *= self.scale_factor
+        odom_message.pose.pose.orientation.y *= self.scale_factor
+        odom_message.pose.pose.orientation.z *= self.scale_factor
+
+        self.publisher_.publish(odom_message)
+
+
+
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    minimal_publisher = TwistPublisher(
+        SCALE_FACTOR
+    )
+
+    rclpy.spin(minimal_publisher)
+
+    minimal_publisher.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/dcaitirobot/dcaitirobot/twist.py

@@ -7,8 +7,8 @@ from sensor_msgs.msg import Joy
 
 import numpy as np
 
-SCALE_LINEAR_VELOCITY = 0.1
-SCALE_ANGULAR_VELOCITY = 0.1
+SCALE_LINEAR_VELOCITY = 0.2
+SCALE_ANGULAR_VELOCITY = 0.7
 PUBLISH_FREQUENCY_HZ = 30
 
 SMOOTH_WEIGHTS = np.array([1, 2, 4, 10, 20])

src/dcaitirobot/launch/frames.gv

@@ -1,19 +1,14 @@
 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"];
+"base_link" -> "velodyne"[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_footprint"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"chassis" -> "camera_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" -> "chassis"[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_link_optical"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"chassis" -> "caster_wheel_back"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"chassis" -> "caster_wheel_front"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"chassis" -> "laser_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
 edge [style=invis];
  subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";
-"Recorded at time: 1688900208.2025208"[ shape=plaintext ] ;
-}->"odom";
+"Recorded at time: 1688907915.1191485"[ shape=plaintext ] ;
+}->"base_link";
 }

src/dcaitirobot/launch/launch_slam.py

@@ -24,6 +24,27 @@ def generate_launch_description():
                     'slam_params_file': slam_params}.items()
     )
 
+    node_fix_lidar = Node(
+                package="dcaitirobot",
+                executable="fixlidar",
+                name="fixlidar",
+            )
+
+    node_scale_odom = Node(
+                package="dcaitirobot",
+                executable="scaleodom",
+                name="scaleodom",
+            )
+
+    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, node_scale_odom
     ])

src/dcaitirobot/setup.py

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