Merge pull request #26 from mleegwt/sensors

Sensors

Author: mleegwt

ros_ws/src/boatcontrol/boatcontrol/simcontrolnode.py

@@ -0,0 +1,41 @@
+#python 3
+import time
+import math
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from std_msgs.msg import Float64
+
+class SimControlNode(Node):
+
+    def __init__(self):
+        super().__init__('sim_control_node')
+        self.cmd_sub = self.create_subscription(Twist, '/cmd_vel', self.props_callback, 10)
+        self.left_pub = self.create_publisher(Float64, '/left', 10)
+        self.right_pub = self.create_publisher(Float64, '/right', 10)
+
+    # Controls the propellors for the boat via duty cycle control.
+    def props_callback(self, cmd_vel):
+        # Converting twist message to differntial drive control, includes clipping
+        # This is an electronic speed controller
+        v = cmd_vel.linear.x     # (m/s)
+        w = cmd_vel.angular.z    # (rad/s)
+
+        B = 0.3                  # (m)
+        K = 1                    # To be determined!
+        left  = (v - w*B/2) / K
+        right = (v + w*B/2) / K
+
+        self.left_pub.publish(Float64(data = left))
+        self.right_pub.publish(Float64(data = right))
+
+def main():
+    rclpy.init()
+    node = SimControlNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
+

ros_ws/src/boatcontrol/launch/simcontrol.launch.py

@@ -0,0 +1,17 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import launch_testing.actions
+
+def generate_launch_description():
+    nodes = [
+        Node(
+            package='boatcontrol',
+            executable='simcontrol_node',
+            name='simcontrol_node',
+            output='screen',
+        )
+    ]
+
+    return LaunchDescription(nodes + [
+        launch_testing.actions.ReadyToTest()
+    ])

ros_ws/src/boatcontrol/setup.py

@@ -9,6 +9,7 @@
     data_files=[
         ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
         ('share/' + package_name + '/launch', ['launch/boatcontrol.launch.py']),
+        ('share/' + package_name + '/launch', ['launch/simcontrol.launch.py']),
         ('share/' + package_name, ['package.xml']),
     ],
     install_requires=['setuptools'],
@@ -20,7 +21,8 @@
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
-            'boatcontrol_node = boatcontrol.boatcontrolnode:main'
+            'boatcontrol_node = boatcontrol.boatcontrolnode:main',
+            'simcontrol_node = boatcontrol.simcontrolnode:main'
         ],
     },
 )

ros_ws/src/navigation/config/karaburan.xacro

@@ -53,13 +53,6 @@
       <origin xyz="0 0 0" rpy="0 0 0" />
       <material name="black" />
     </visual>
-
-    <sensor name="imu_sensor" type="imu">
-      <always_on>1</always_on>
-      <update_rate>1</update_rate>
-      <visualize>true</visualize>
-      <topic>imu</topic>
-    </sensor>
   </link>
 
   <link name="gps_link">
@@ -72,7 +65,7 @@
     </visual>
   </link>
 
-  <link name="lidar">
+  <link name="lidar_link">
     <visual>
       <geometry>
         <cylinder radius="0.05" length="0.05" />
@@ -87,34 +80,6 @@
       <origin xyz="0 0 0" rpy="0 0 0" />
       <material name="black" />
     </collision>
-
-    <sensor name="lidar" type="gpu_lidar">
-      <always_on>true</always_on>
-      <visualize>true</visualize>
-      <update_rate>5</update_rate>
-      <topic>scan</topic>
-      <gz_frame_id>lidar_link</gz_frame_id>
-      <lidar>
-        <scan>
-          <horizontal>
-            <samples>360</samples>
-            <resolution>1.000000</resolution>
-            <min_angle>0.000000</min_angle>
-            <max_angle>6.280000</max_angle>
-          </horizontal>
-        </scan>
-        <range>
-          <min>0.120000</min>
-          <max>3.5</max>
-          <resolution>0.015000</resolution>
-        </range>
-        <noise>
-          <type>gaussian</type>
-          <mean>0.0</mean>
-          <stddev>0.01</stddev>
-        </noise>
-      </lidar>
-    </sensor>
   </link>
 
   <joint name="base_imu_joint" type="fixed">
@@ -147,7 +112,7 @@
 
   <joint name="base_lidar_joint" type="fixed">
     <parent link="base_link" />
-    <child link="lidar" />
+    <child link="lidar_link" />
     <origin xyz="0 0 0.155" rpy="0 0 0" />
   </joint>
 

ros_ws/src/navigation/config/karaburan_boat.sdf

@@ -48,7 +48,7 @@
         <gz_frame_id>lidar_link</gz_frame_id>
         <topic>/scan</topic>
         <update_rate>10</update_rate>
-        <ray>
+        <lidar>
           <scan>
             <horizontal>
               <samples>720</samples>
@@ -68,7 +68,7 @@
             <max>50.0</max>
             <resolution>0.01</resolution>
           </range>
-        </ray>
+        </lidar>
         <always_on>1</always_on>
         <visualize>true</visualize>
       </sensor>

ros_ws/src/navigation/config/slam_params.yaml

@@ -0,0 +1,26 @@
+slam_toolbox:
+  ros__parameters:
+    use_sim_time: true
+
+    # Frames — laat SLAM de TF map->odom publiceren
+    odom_frame: "odom"
+    map_frame: "map"
+    base_frame: "base_link"
+    publish_tf: true
+    tf_buffer_duration: 10.0
+
+    # Sensor
+    scan_topic: "/scan"           # <-- zet jouw topic hier
+    use_imu_data: true            # IMU yaw helpt bij water/reflecties
+    imu_data_topic: "/imu/data"   # als je een ander pad hebt, aanpassen
+
+    # Matcher / update-trigger (beetje ruim voor eerste test)
+    minimum_travel_distance: 0.10
+    minimum_travel_heading: 0.10   # rad ~ 6°
+    transform_publish_period: 0.02 # 50 Hz TF is prima
+
+    # Kaart
+    resolution: 0.05               # m/cel
+    map_update_interval: 2.0       # s
+    enable_interactive_mode: false
+

ros_ws/src/navigation/launch/nav2_stack.launch.py

@@ -1,5 +1,6 @@
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import Command, PathJoinSubstitution, FindExecutable, LaunchConfiguration
 from launch_ros.substitutions import FindPackageShare
 from launch_ros.actions import Node, LifecycleNode
@@ -50,11 +51,18 @@ def generate_launch_description():
         'bt_navigator.yaml'
     )
     use_sim_time = LaunchConfiguration('use_sim_time')
+    slam_file = os.path.join(
+        get_package_share_directory('navigation'),
+        'config',
+        'slam_params_file.yaml'
+    )
+
+
     nodes = [
         Node(
             package='robot_state_publisher',
             executable='robot_state_publisher',
-            name='robot_state_publisher',
+            name='karaburan_robot_state_publisher',
             parameters=[{'robot_description': robot_description,
                          'publish_frequency': 1.0,
                          'use_sim_time': use_sim_time }],
@@ -174,7 +182,17 @@ def generate_launch_description():
     ]
 
     return LaunchDescription(
-            [ DeclareLaunchArgument('use_sim_time', default_value='true', description='Gebruik /clock (true) of systeemklok (false)') ] +
+            [ DeclareLaunchArgument('use_sim_time', default_value='true', description='Gebruik /clock (true) of systeemklok (false)'),
+                IncludeLaunchDescription(
+                    PythonLaunchDescriptionSource(PathJoinSubstitution([
+                        FindPackageShare("slam_toolbox"), "launch", "online_async_launch.py"
+                    ])),
+                    launch_arguments={
+                        'use_sim_time': use_sim_time,
+                        'slam_params_file': slam_file
+                    }.items()
+                )
+             ] +
             nodes + [
         launch_testing.actions.ReadyToTest()
     ])

ros_ws/src/navigation/launch/sim.launch.py

@@ -18,6 +18,13 @@
 import os
 
 def generate_launch_description():
+    boatcontrol_dir = get_package_share_directory('boatcontrol')
+    boatcontrol = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(boatcontrol_dir, 'launch', 'simcontrol.launch.py')
+        )
+    )
+
     # --- Launch arguments ---
     xacro_file = LaunchConfiguration("xacro_file")
     world_sdf = LaunchConfiguration("world_sdf")
@@ -53,11 +60,6 @@ def generate_launch_description():
 
     return LaunchDescription([
         # Paths / args
-        DeclareLaunchArgument(
-            "xacro_file",
-            default_value="navigation/config/karaburan.xacro",
-            description="Path to .urdf.xacro file"
-        ),
         DeclareLaunchArgument(
             "world_sdf",
             default_value="navigation/config/world.sdf",
@@ -76,9 +78,6 @@ def generate_launch_description():
         DeclareLaunchArgument("R", default_value="0.0"),
         DeclareLaunchArgument("P", default_value="0.0"),
         DeclareLaunchArgument("Y", default_value="0.0"),
-        DeclareLaunchArgument(
-            "xacro_args", default_value="",
-        ),
         # Launch args for the GZ - ROS2 bridge
         DeclareLaunchArgument('ns', default_value='', description='ROS namespace for the bridge'),
         DeclareLaunchArgument('imu_topic', default_value='/imu/data', description='GZ/ROS topic for IMU'),
@@ -99,21 +98,8 @@ def generate_launch_description():
             }.items()
         ),
 
-        Node(
-            package="robot_state_publisher",
-            executable="robot_state_publisher",
-            name="robot_state_publisher",
-            output="screen",
-            parameters=[{
-                "use_sim_time": True,
-                "robot_description": Command([
-                    FindExecutable(name="xacro"), " ",
-                    xacro_file, " ",
-                    xacro_args
-                ])
-            }]
-        ),
         nav_launch,
+        boatcontrol,
 
         TimerAction(
             period=2.0,
@@ -140,8 +126,8 @@ def generate_launch_description():
                         PythonExpression(['"', imu_topic, '" + \'@sensor_msgs/msg/Imu[gz.msgs.IMU\'']),
                         PythonExpression(['"', gps_topic, '" + \'@sensor_msgs/msg/NavSatFix[gz.msgs.NavSat\'']),
                         PythonExpression(['"', lidar_topic, '" + \'@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan\'']),
-                        PythonExpression(['"', left_topic, '" + \'@std_msgs/msg/Float[gz.msgs.Float\'']),
-                        PythonExpression(['"', right_topic, '" + \'@std_msgs/msg/Float[gz.msgs.Float\'']),
+                        PythonExpression(['"', left_topic, '" + \'@std_msgs/msg/Float64[gz.msgs.Double\'']),
+                        PythonExpression(['"', right_topic, '" + \'@std_msgs/msg/Float64[gz.msgs.Double\'']),
                         '/tf@tf2_msgs/msg/TFMessage@gz.msgs.Pose_V',
                         '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'
                     ],