Merge pull request #15 from mleegwt/master

Navigation configuration improvements.

Author: mleegwt

ros_ws/src/boatcontrol/boatcontrol/boatcontrolnode.py

@@ -56,7 +56,7 @@ def send_command(self, command):
             while "OK" not in response: 
               response = ser.readline().decode().strip()  # Read response from the actuator
               time.sleep(0.2)
-            self.get_logger().info(f"Sent: {command}, Received: {response}")
+            self.get_logger().debug(f"Sent: {command}, Received: {response}")
             return response
 
     # Function to send enable motor command

ros_ws/src/navigation/config/behavior_server.yaml

@@ -0,0 +1,12 @@
+behavior_server:
+  ros__parameters:
+    use_sim_time: false
+    behavior_plugins: ["spin", "backup", "wait"]
+
+    spin:
+      plugin: "nav2_behaviors::Spin"
+    backup:
+      plugin: "nav2_behaviors::BackUp"
+    wait:
+      plugin: "nav2_behaviors::Wait"
+

ros_ws/src/navigation/config/controller_server.yaml

@@ -0,0 +1,44 @@
+controller_server:
+  ros__parameters:
+    use_sim_time: true
+    controller_plugins: ["FollowPath"]
+
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+
+      debug_trajectory_details: true
+      min_vel_x: 0.0
+      max_vel_x: 0.26
+      min_vel_y: 0.0
+      max_vel_y: 0.0
+      max_vel_theta: 1.0
+      min_speed_xy: 0.0
+      max_speed_xy: 0.26
+      min_speed_theta: 0.0
+
+      acc_lim_x: 2.5
+      acc_lim_y: 0.0
+      acc_lim_theta: 3.2
+
+      decel_lim_x: -1.0
+      decel_lim_y: 0.0
+      decel_lim_theta: -1.0
+
+      vx_samples: 20
+      vtheta_samples: 40
+
+      sim_time: 1.7
+      linear_granularity: 0.05
+      angular_granularity: 0.025
+      transform_tolerance: 0.2
+
+      critics: [
+        "RotateToGoal",
+        #        "ObstacleFootprint",
+        "GoalAlign",
+        "PathAlign",
+        "PathDist",
+        "GoalDist",
+        "PreferForward"
+      ]
+

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

@@ -1,33 +1,56 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
+from launch_ros.actions import LifecycleNode
 import launch_testing.actions
+from ament_index_python.packages import get_package_share_directory
+import os
+
 
 def generate_launch_description():
+    controller_yaml = os.path.join(
+        get_package_share_directory('navigation'),
+        'config',
+        'controller_server.yaml'
+    )
+    behavior_yaml = os.path.join(
+        get_package_share_directory('navigation'),
+        'config',
+        'behavior_server.yaml'
+    )
+    map_yaml = os.path.join(
+        get_package_share_directory('navigation'),
+        'maps',
+        'empty.yml'
+    )
     nodes = [
-        Node(
+        LifecycleNode(
             package='nav2_bt_navigator',
             executable='bt_navigator',
             name='bt_navigator',
+            namespace='',
             parameters=[{'use_sim_time': False}],
         ),
-        Node(
+        LifecycleNode(
             package='nav2_planner',
             executable='planner_server',
             name='planner_server',
+            namespace='',
             parameters=[{'use_sim_time': False}],
         ),
-        Node(
+        LifecycleNode(
             package='nav2_controller',
             executable='controller_server',
             name='controller_server',
-            parameters=[{'use_sim_time': False}],
+            namespace='',
+            parameters=[controller_yaml],
         ),
-        Node(
+        LifecycleNode(
             package='nav2_map_server',
             executable='map_server',
             name='map_server',
+            namespace='',
             parameters=[{
-                'yaml_filename': '../maps/empty.yml',
+                'yaml_filename': map_yaml,
                 'use_sim_time': False
             }],
         ),
@@ -43,7 +66,44 @@ def generate_launch_description():
             name='navigation_node',
             output='screen',
             parameters=[{'use_sim_time': False}]
-        )
+        ),
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            name='static_map_to_odom',
+            arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']
+        ),
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            name='static_tf_map_to_base_link',
+            arguments=['0', '0', '0', '0', '0', '0', 'map', 'base_link'],
+            output='screen'
+        ),
+        LifecycleNode(
+            package='nav2_behaviors',
+            executable='behavior_server',
+            name='behavior_server',
+            namespace='',
+            output='screen',
+            parameters=[behavior_yaml],
+        ),
+        Node(
+            package='nav2_lifecycle_manager',
+            executable='lifecycle_manager',
+            name='lifecycle_manager_navigation',
+            output='screen',
+            parameters=[{
+                'autostart': True,
+                'node_names': [
+                    'map_server',
+                    'controller_server',
+                    'planner_server',
+                    'behavior_server',
+                    'bt_navigator',
+                ]
+            }]
+        ),
     ]
 
     return LaunchDescription(nodes + [

ros_ws/src/navigation/maps/empty.yml

@@ -1,6 +1,6 @@
-image: map.pgm
+image: empty.pgm
 resolution: 0.5  # Elke pixel is 0.5 meter → 50x50 m kaart
 origin: [0.0, 0.0, 0.0]  # Beginpunt in wereldcoördinaten
 occupied_thresh: 0.65
 free_thresh: 0.196
-negate: 0
+negate: 0

ros_ws/src/navigation/navigation/boatnavigator.py

@@ -11,10 +11,14 @@ class BoatNavigator(Node):
     def __init__(self):
         super().__init__('boat_navigator')
 
+        self.get_logger().info("Waiting for Basic navigator to be activated")
+
         # Initialize the Nav2 navigator
         self.navigator = BasicNavigator()
         self.navigator.waitUntilNav2Active()
 
+        self.get_logger().info("Basic navigator has been activated")
+
         # Publishers for heading and speed
         self.heading_pub = self.create_publisher(Float64, '/desired_heading', 10)
         self.speed_pub = self.create_publisher(Float64, '/desired_speed', 10)

ros_ws/src/navigation/setup.py

@@ -10,6 +10,8 @@
         ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
         ('share/' + package_name + '/launch', ['launch/nav2_stack.launch.py']),
         ('share/' + package_name + '/launch', ['launch/boat.launch.py']),
+        ('share/' + package_name + '/config', ['config/controller_server.yaml']),
+        ('share/' + package_name + '/config', ['config/behavior_server.yaml']),
         ('share/' + package_name + '/maps', ['maps/empty.yml', 'maps/empty.pgm']),
         ('share/' + package_name, ['package.xml']),
     ],

ros_ws/src/sensorfusion/sensorfusion/sensorfusion_node.py

@@ -1,7 +1,8 @@
 import math
 
 import rclpy
-from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from geometry_msgs.msg import PoseWithCovariance
 from geometry_msgs.msg import Point
 from geometry_msgs.msg import Pose
 from geometry_msgs.msg import Quaternion
@@ -30,7 +31,7 @@ def __init__(self):
         super().__init__('sensor_fusion_node')
         self.gps_sub = self.create_subscription(NavSatFix, '/fix', self.gps_callback, 10)
         self.compass_sub = self.create_subscription(Float64, '/compass', self.compass_callback, 10)
-        self.pose_pub = self.create_publisher(PoseStamped, '/amcl_pose', 100)
+        self.pose_pub = self.create_publisher(PoseWithCovarianceStamped, '/amcl_pose', 100)
         self.current_lat = 0.0
         self.current_lon = 0.0
         self.current_heading = 0  # Heading from your custom compass
@@ -55,7 +56,8 @@ def publish_pose(self):
         orientation = heading_to_quaternion(self.current_heading)
         pose = Pose(position = position, orientation = orientation)
 
-        pose_msg = PoseStamped(header = hdr, pose = pose)
+        pose_ext = PoseWithCovariance(pose = pose)
+        pose_msg = PoseWithCovarianceStamped(header = hdr, pose = pose_ext)
         self.get_logger().debug(f"Pose with cov: {pose_msg}")
         self.pose_pub.publish(pose_msg)
 

ros_ws/src/sensorfusion/test/test_pub_error.py

@@ -3,7 +3,8 @@
 from rclpy.node import Node
 from sensor_msgs.msg import NavSatFix
 from std_msgs.msg import Float64
-from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from geometry_msgs.msg import PoseWithCovariance
 from rclpy.wait_for_message import wait_for_message
 import launch
 import launch_ros.actions
@@ -47,7 +48,7 @@ def setUp(self):
     # Create publishers for GPS & Compass
     self.gps_pub = self.node.create_publisher(NavSatFix, "/fix", 10)
     self.compass_pub = self.node.create_publisher(Float64, "/compass", 10)
-    self.pose_sub = self.node.create_subscription(PoseStamped, "/amcl_pose", lambda msg: self.msgs.append(msg), 10)
+    self.pose_sub = self.node.create_subscription(PoseWithCovarianceStamped, "/amcl_pose", lambda msg: self.msgs.append(msg), 10)
 
   def tearDown(self):
     self.node.destroy_subscription(self.pose_sub)
@@ -86,9 +87,10 @@ def test_sensorfusion_publishes_fused_data(self, launch_service, proc_info, sens
     fused_pose = self.msgs[-1]
     assert fused_pose is not None, f"SensorFusion did not publish a fused Pose! {fused_pose}"
     assert fused_pose.pose is not None, f"SensorFusion did not include a Pose! {fused_pose}"
-    assert fused_pose.pose.position is not None, f"SensorFusion did not include a Position! {fused_pose}"
-    assert fused_pose.pose.position.x != 0, "Expected nonzero position"
-    assert fused_pose.pose.position.y != 0, "Expected nonzero position"
-    assert fused_pose.pose.orientation.z != 0, "Expected valid heading"
+    assert fused_pose.pose.pose is not None, f"SensorFusion did not include a Pose! {fused_pose}"
+    assert fused_pose.pose.pose.position is not None, f"SensorFusion did not include a Position! {fused_pose}"
+    assert fused_pose.pose.pose.position.x != 0, "Expected nonzero position"
+    assert fused_pose.pose.pose.position.y != 0, "Expected nonzero position"
+    assert fused_pose.pose.pose.orientation.z != 0, "Expected valid heading"