Merge pull request #16 from mleegwt/master

Preparing for MPU9250 and improving Nav2 configuration. Never been so close to waypoint navigation.

Author: mleegwt

.github/workflows/action.yml

@@ -12,6 +12,6 @@ jobs:
           required-ros-distributions: jazzy
       - uses: ros-tooling/action-ros-ci@v0.3
         with:
-          package-name: boat_interfaces boatcontrol navigation tempreader sensorfusion
+          package-name: boat_interfaces boatcontrol navigation tempreader
           target-ros2-distro: jazzy
 

.gitignore

@@ -1,4 +1,3 @@
-.*
 ros_ws/log/*
 ros_ws/src/log/*
 ros_ws/build/*

README.md

@@ -6,6 +6,7 @@ Load ROS 2:
 Navigate to ROS workspace:
 `cd ros_ws`
 
+# Temerature sensor
 Build and run temperature sensor node (my SensorID):
 `colcon build && source ./install/setup.bash && ros2 run tempreader tempreaderNode --ros-args -p sensorId:=28.C23646D48524 &`
 
@@ -19,3 +20,26 @@ Read fixes from GPS:
 
 Read temperature:
 `ros2 topic echo /temperature`
+
+# Boat total setup launch file
+
+This currently works on My Machine TM (use the generic instructions at the top first):
+`colcon build && source install/setup.bash && ros2 launch navigation boat.launch.py`
+
+## Installation of ROS2 packages
+
+Currently the following packages have been installed (these will introduce many dependencies, list generated with `apt-mark showmanual > ~/manual-packages.txt`:
+ros-dev-tools
+ros-jazzy-gps-tools
+ros-jazzy-gpsd-client
+ros-jazzy-mapviz-interfaces
+ros-jazzy-nav2-bringup
+ros-jazzy-nav2-waypoint-follower
+ros-jazzy-navigation2
+ros-jazzy-robot-localization
+ros-jazzy-ros-base
+ros-jazzy-ros2-control
+ros-jazzy-ros2-controllers
+ros-jazzy-tf-transformations
+ros2-apt-source
+

ros_ws/src/boatcontrol/boatcontrol/boatcontrolnode.py

@@ -19,7 +19,7 @@ def __init__(self):
         self.heading_sub = self.create_subscription(Float64, '/desired_heading', self.heading_callback, 10)
         self.speed_sub = self.create_subscription(Float64, '/desired_speed', self.speed_callback, 10)
         self.current_heading = None
-        timer_period = 1  # Seconds
+        timer_period = 0.05  # Seconds
         self.timer = self.create_timer(timer_period, self.timer_callback)
 
     def start(self):
@@ -29,8 +29,7 @@ def start(self):
         response = ser.readline().decode().strip()  # Read response from the actuator
         self.get_logger().info(f"Received: {response}")
         self.send_enable_command()
-        time.sleep(1)
-        self.send_calib_command()
+        time.sleep(0.1)
         self.send_speed_command(30)
 
     def timer_callback(self):
@@ -51,11 +50,11 @@ def speed_callback(self, speed):
     def send_command(self, command):
         if ser.is_open:
             ser.write(command.encode())
-            time.sleep(1)
+            time.sleep(0.01)
             response=""
             while "OK" not in response: 
               response = ser.readline().decode().strip()  # Read response from the actuator
-              time.sleep(0.2)
+              time.sleep(0.01)
             self.get_logger().debug(f"Sent: {command}, Received: {response}")
             return response
 

ros_ws/src/navigation/config/controller_server.yaml

@@ -1,44 +1,29 @@
-controller_server:
+/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"
-      ]
+    use_sim_time: false
+    controller_plugins: ["FollowWaypoints"]
+
+    local_costmap:
+      global_frame: local_enu
+      robot_base_frame: base_link
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      rolling_window: true
+      width: 1000.0
+      height: 1000.0
+      resolution: 5.0
+      transform_tolerance: 1.0
+      inflation_radius: 0.5
+      cost_scaling_factor: 10.0
+
+      plugins:
+        - name: obstacle_layer
+          type: "nav2_costmap_2d::ObstacleLayer"
+        - name: inflation_layer
+          type: "nav2_costmap_2d::InflationLayer"
+
+    FollowWaypoints:
+      plugin: "nav2_controller::FollowWaypoints"
+      allow_in_place_goal: true
+      controller_frequency: 10.0
 

ros_ws/src/navigation/config/ekf.yaml

@@ -0,0 +1,32 @@
+/ekf_filter_node:
+  ros__parameters:
+    frequency: 5.0
+    sensor_timeout: 5.0
+    initial_state_timeout: 10.0
+    two_d_mode: true
+    publish_tf: true
+    map_frame: map
+    odom_frame: odom
+    base_link_frame: base_link
+    world_frame: map
+    imu0: /imu/data
+    imu0_config: [false, false, false,
+                  true, true, true,
+                  false, false, false,
+                  false, false, false,
+                  true, true, true]
+    imu0_differential: false
+    imu0_remove_gravitational_acceleration: true
+    imu0_covariance: [
+      0.02, 0.0,    0.0,
+      0.0,    0.02, 0.0,
+      0.0,    0.0,    0.02
+    ]
+    imu0_queue_size: 10
+
+    gps0: /fix/valid
+    gps0_config: [true,  true,  false,
+                false, false, false,
+                false, false, false]
+    gps0_queue_size: 10
+

ros_ws/src/navigation/config/navsat.yaml

@@ -0,0 +1,23 @@
+navsat_transform_node:
+  ros__parameters:
+    frequency: 2.5
+    delay: 1.0
+    magnetic_declination_radians: 0.0
+    yaw_offset: 0.0
+    zero_altitude: true
+    broadcast_cartesian_transform: true
+    publish_filtered_gps: true
+
+    use_odometry_yaw: false        # We hebben een IMU!
+    use_local_cartesian: true      # Handig voor testgebieden
+    wait_for_datum: false          # Gebruik eerste fix als referentie
+    world_frame: map
+    map_frame: map
+    child_frame_id: local_enu
+
+    gps_topic: /fix/valid
+    imu_topic: /imu/data
+    odometry_topic: /odometry/gps
+
+    imu_queue_size: 50
+    gps_queue_size: 10

ros_ws/src/navigation/config/planner_server.yaml

@@ -0,0 +1,15 @@
+/planner_server:
+  ros__parameters:
+    use_sim_time: false
+    planner_frequency: 0.5
+    expected_planner_frequency: 0.5
+
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_smac_planner::SmacPlanner2D"
+
+    expected_planner_frequency: 20.0
+
+    global_costmap:
+      static_layer:
+        enabled: false

ros_ws/src/navigation/launch/boat.launch.py

@@ -20,13 +20,14 @@ def generate_launch_description():
         )
     )
 
-    sensorfusion_dir = get_package_share_directory('sensorfusion')
-    sensorfusion = IncludeLaunchDescription(
+    imu_dir = get_package_share_directory('mpu9250')
+    imu = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            os.path.join(sensorfusion_dir, 'launch', 'sensorfusion.launch.py')
+            os.path.join(imu_dir, 'launch', 'mpu9250.launch.py')
         )
     )
 
+
     nav_dir = get_package_share_directory('navigation')
     nav_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -37,6 +38,6 @@ def generate_launch_description():
     return LaunchDescription([
         gpsd_launch,
         boatcontrol,
-        sensorfusion,
+        imu,
         nav_launch
     ])