diff --git a/requirements.txt b/requirements.txt
index 323d5b0..416634f 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -1 +1 @@
-pre-commit
+pre-commit
diff --git a/seaweed_interfaces/CMakeLists.txt b/seaweed_interfaces/CMakeLists.txt
index 844120d..624cded 100644
--- a/seaweed_interfaces/CMakeLists.txt
+++ b/seaweed_interfaces/CMakeLists.txt
@@ -8,12 +8,13 @@ endif()
find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()
-# rosidl_generate_interfaces(${PROJECT_NAME}
-# # "path_to_msg/action/srv"
-# DEPENDENCIES
-# # geometry_msgs
-# # sensor_msgs
-# # ...
-# )
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+ "action/MotionPlanner.action"
+ DEPENDENCIES
+ geometry_msgs
+ nav_msgs
+)
ament_auto_package()
diff --git a/seaweed_interfaces/action/MotionPlanner.action b/seaweed_interfaces/action/MotionPlanner.action
new file mode 100644
index 0000000..06d48d7
--- /dev/null
+++ b/seaweed_interfaces/action/MotionPlanner.action
@@ -0,0 +1,5 @@
+nav_msgs/Path path
+---
+geometry_msgs/Pose final_pose
+---
+float64 distance_to_goal
diff --git a/seaweed_interfaces/package.xml b/seaweed_interfaces/package.xml
index 8eae9a5..35a6f99 100644
--- a/seaweed_interfaces/package.xml
+++ b/seaweed_interfaces/package.xml
@@ -16,6 +16,7 @@
std_msgs
sensor_msgs
geometry_msgs
+ nav_msgs
rosidl_interface_packages
diff --git a/seaweed_navigation/CMakeLists.txt b/seaweed_navigation/CMakeLists.txt
index 1e086c9..f33f187 100644
--- a/seaweed_navigation/CMakeLists.txt
+++ b/seaweed_navigation/CMakeLists.txt
@@ -12,7 +12,9 @@ ament_python_install_package(${PROJECT_NAME})
# Python Nodes:
install(PROGRAMS
- # ${PROJECT_NAME}/my_node.py
+ ${PROJECT_NAME}/navigation_manager.py
+ ${PROJECT_NAME}/test_map_generator.py
+ ${PROJECT_NAME}/pd_motion_planner.py
DESTINATION lib/${PROJECT_NAME}
)
@@ -33,5 +35,5 @@ install(TARGETS
ament_auto_package(
USE_SCOPED_HEADER_INSTALL_DIR
- INSTALL_TO_SHARE launch config
+ INSTALL_TO_SHARE launch config rviz
)
diff --git a/seaweed_navigation/launch/.gitkeep b/seaweed_navigation/launch/.gitkeep
deleted file mode 100644
index e69de29..0000000
diff --git a/seaweed_navigation/launch/test_nav_manager.launch.py b/seaweed_navigation/launch/test_nav_manager.launch.py
new file mode 100644
index 0000000..ae33c4b
--- /dev/null
+++ b/seaweed_navigation/launch/test_nav_manager.launch.py
@@ -0,0 +1,45 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+ test_map_generator_node = Node(
+ package="seaweed_navigation",
+ executable="test_map_generator.py",
+ name="test_map_generator",
+ output="screen",
+ )
+
+ navigation_manager = Node(
+ package="seaweed_navigation",
+ executable="navigation_manager.py",
+ name="navigation_manager",
+ output="screen",
+ )
+
+ pd_motion_planner = Node(
+ package="seaweed_navigation",
+ executable="pd_motion_planner.py",
+ name="pd_motion_planner",
+ output="screen",
+ )
+
+ # nav_directory = get_package_share_directory("seaweed_navigation")
+ # rviz_config_file = os.path.join(nav_directory, "rviz", "map_dash.rviz")
+
+ # rviz = Node(
+ # package="rviz2",
+ # executable="rviz2",
+ # name="rviz2",
+ # output="screen",
+ # arguments=["-d", rviz_config_file],
+ # )
+
+ return LaunchDescription(
+ [
+ navigation_manager,
+ test_map_generator_node,
+ pd_motion_planner,
+ # rviz
+ ]
+ )
diff --git a/seaweed_navigation/launch/test_nav_manager_sim.launch.py b/seaweed_navigation/launch/test_nav_manager_sim.launch.py
new file mode 100644
index 0000000..3c6f017
--- /dev/null
+++ b/seaweed_navigation/launch/test_nav_manager_sim.launch.py
@@ -0,0 +1,187 @@
+import os
+
+from launch import LaunchDescription
+from ament_index_python.packages import get_package_share_directory
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def launch_setup(context, *args, **kwargs):
+ description_directory = get_package_share_directory("seaweed_description")
+ sim_directory = get_package_share_directory("seaweed_sim")
+ vrx_gz_directory = get_package_share_directory("vrx_gz")
+ localization_directory = get_package_share_directory("seaweed_localization")
+
+ # GAZEBO SIMULATION
+ local_world_directory = os.path.join(sim_directory, "worlds")
+ vrx_worlds_directory = os.path.join(vrx_gz_directory, "worlds")
+
+ resource_path = os.environ.get("GZ_SIM_RESOURCE_PATH", "")
+ os.environ["GZ_SIM_RESOURCE_PATH"] = f"{local_world_directory}:{resource_path}:{vrx_worlds_directory}"
+
+ model = context.perform_substitution(LaunchConfiguration("model"))
+
+ match model:
+ case "x_drive":
+ model_path = os.path.join(description_directory, "urdf", "x_drive_wamv", "wamv_target.urdf")
+ case "diff_thrust":
+ model_path = os.path.join(description_directory, "urdf", "diff_thrust_wamv", "wamv_target.urdf")
+ case _:
+ model_path = os.path.join(description_directory, "urdf", "diff_thrust_wamv", "wamv_target.urdf")
+
+ world = context.perform_substitution(LaunchConfiguration("world"))
+
+ vrx_sim_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource([vrx_gz_directory, "/launch/competition.launch.py"]),
+ launch_arguments={
+ "world": world,
+ "urdf": model_path,
+ "extra_gz_args": "-v 0", # verbose levels from 0 to 4
+ # "spawn_pose": "-532.0,162.0,0.0,0.0,0.0,1.0", # Original spawn point for sydney_regatta
+ "spawn_pose": "-520.0,180.0,0.0,0.0,0.0,1.57", # Original spawn point for sydney_regatta
+ }.items(),
+ )
+
+ # LOCALIZATION
+ sim_localization_params = os.path.join(localization_directory, "config", "sim_localization_params.yaml")
+
+ world = "sydney_regatta"
+ # manually determined init point; turn into a ros param to pick between more worlds as needed
+ locations = {"sydney_regatta": (-33.7226, 150.6741)}
+ lat, lon = locations[world]
+
+ navsat_transform_node = Node(
+ package="robot_localization",
+ executable="navsat_transform_node",
+ remappings=[
+ ("gps/fix", "/wamv/sensors/gps/gps/fix"),
+ ],
+ parameters=[
+ sim_localization_params,
+ # {"datum": [lat, lon, 0.0]}, # comment out to set origin at first gps reading
+ ],
+ )
+
+ ekf_node = Node(
+ package="robot_localization",
+ executable="ekf_node",
+ parameters=[sim_localization_params],
+ )
+
+ # Assume no sensor drift
+ static_transform_publisher_node = Node(
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_map_to_odom_publisher",
+ arguments=[
+ "--x",
+ "0",
+ "--y",
+ "0",
+ "--z",
+ "0",
+ "--roll",
+ "0",
+ "--pitch",
+ "0",
+ "--yaw",
+ "0",
+ "--frame-id",
+ "map",
+ "--child-frame-id",
+ "odom",
+ ],
+ )
+
+ # VISUALIZATION/TESTING
+ rviz_config_file = os.path.join(sim_directory, "rviz", "gazebo_full.rviz")
+
+ rviz = Node(
+ package="rviz2",
+ executable="rviz2",
+ parameters=[{"use_sim_time": True}],
+ name="rviz2",
+ output="screen",
+ arguments=["-d", rviz_config_file],
+ condition=IfCondition(LaunchConfiguration("rviz")),
+ )
+
+ return [
+ vrx_sim_launch,
+ ekf_node,
+ navsat_transform_node,
+ static_transform_publisher_node,
+ rviz,
+ ]
+
+
+def generate_launch_description():
+ test_map_generator_node = Node(
+ package="seaweed_navigation",
+ executable="test_map_generator.py",
+ name="test_map_generator",
+ output="screen",
+ )
+
+ navigation_manager = Node(
+ package="seaweed_navigation",
+ executable="navigation_manager.py",
+ name="navigation_manager",
+ output="screen",
+ )
+
+ joy_node = Node(
+ package="joy",
+ executable="joy_node",
+ name="joy_node",
+ output="screen",
+ parameters=[{"deadzone": 0.01}],
+ )
+
+ diff_thrust_controller = Node(
+ package="seaweed_sim",
+ executable="diff_thrust_controller.py",
+ name="diff_thrust_controller",
+ output="screen",
+ )
+
+ robot_model_arg = DeclareLaunchArgument(
+ name="model", default_value="diff_thrust", choices=["x_drive", "diff_thrust"]
+ )
+
+ world_arg = DeclareLaunchArgument(
+ name="world",
+ default_value="sydney_regatta",
+ choices=[
+ "sydney_regatta",
+ "sydney_regatta_empty",
+ "nbpark",
+ "follow_path",
+ ],
+ )
+
+ use_sim_time_arg = DeclareLaunchArgument(
+ name="use_sim_time",
+ default_value="true",
+ )
+
+ use_gui_arg = DeclareLaunchArgument(name="use_gui", default_value="true") # unused
+ rviz_arg = DeclareLaunchArgument("rviz", default_value="true", choices=["true", "false"])
+
+ return LaunchDescription(
+ [
+ robot_model_arg,
+ use_sim_time_arg,
+ use_gui_arg,
+ world_arg,
+ rviz_arg,
+ joy_node,
+ diff_thrust_controller,
+ navigation_manager,
+ test_map_generator_node,
+ OpaqueFunction(function=launch_setup),
+ ]
+ )
diff --git a/seaweed_navigation/launch/test_pd_motion_planner.launch.py b/seaweed_navigation/launch/test_pd_motion_planner.launch.py
new file mode 100644
index 0000000..1e33011
--- /dev/null
+++ b/seaweed_navigation/launch/test_pd_motion_planner.launch.py
@@ -0,0 +1,17 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+ pd_motion_planner = Node(
+ package="seaweed_navigation",
+ executable="pd_motion_planner.py",
+ name="pd_motion_planner",
+ output="screen",
+ )
+
+ return LaunchDescription(
+ [
+ pd_motion_planner,
+ ]
+ )
diff --git a/seaweed_navigation/package.xml b/seaweed_navigation/package.xml
index 547eb82..4408d23 100644
--- a/seaweed_navigation/package.xml
+++ b/seaweed_navigation/package.xml
@@ -13,6 +13,9 @@
rclpy
sensor_msgs
std_msgs
+ nav_msgs
+ tf_transformations
+ seaweed_interfaces
ament_cmake
diff --git a/seaweed_navigation/rviz/map_dash.rviz b/seaweed_navigation/rviz/map_dash.rviz
new file mode 100644
index 0000000..d6a5930
--- /dev/null
+++ b/seaweed_navigation/rviz/map_dash.rviz
@@ -0,0 +1,221 @@
+Panels:
+ - Class: rviz_common/Displays
+ Help Height: 138
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ Splitter Ratio: 0.5
+ Tree Height: 1434
+ - Class: rviz_common/Selection
+ Name: Selection
+ - Class: rviz_common/Tool Properties
+ Expanded:
+ - /2D Goal Pose1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz_common/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz_common/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz_default_plugins/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame:
+ Value: true
+ - Alpha: 0.699999988079071
+ Class: rviz_default_plugins/Map
+ Color Scheme: map
+ Draw Behind: false
+ Enabled: true
+ Name: Map
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /map
+ Update Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /map_updates
+ Use Timestamp: false
+ Value: true
+ - Alpha: 1
+ Buffer Length: 1
+ Class: rviz_default_plugins/Path
+ Color: 25; 255; 0
+ Enabled: true
+ Head Diameter: 0.30000001192092896
+ Head Length: 0.20000000298023224
+ Length: 0.30000001192092896
+ Line Style: Lines
+ Line Width: 0.029999999329447746
+ Name: Path
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Pose Color: 255; 85; 255
+ Pose Style: None
+ Radius: 0.029999999329447746
+ Shaft Diameter: 0.10000000149011612
+ Shaft Length: 0.10000000149011612
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /path
+ Value: true
+ - Angle Tolerance: 0.10000000149011612
+ Class: rviz_default_plugins/Odometry
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: true
+ Enabled: true
+ Keep: 100
+ Name: Odometry
+ Position Tolerance: 0.10000000149011612
+ Shape:
+ Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Color: 255; 25; 0
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Value: Arrow
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /odometry/filtered
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz_default_plugins/Interact
+ Hide Inactive Objects: true
+ - Class: rviz_default_plugins/MoveCamera
+ - Class: rviz_default_plugins/Select
+ - Class: rviz_default_plugins/FocusCamera
+ - Class: rviz_default_plugins/Measure
+ Line color: 128; 128; 0
+ - Class: rviz_default_plugins/SetInitialPose
+ Covariance x: 0.25
+ Covariance y: 0.25
+ Covariance yaw: 0.06853891909122467
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /initialpose
+ - Class: rviz_default_plugins/SetGoal
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /goal_pose
+ - Class: rviz_default_plugins/PublishPoint
+ Single click: true
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /clicked_point
+ Transformation:
+ Current:
+ Class: rviz_default_plugins/TF
+ Value: true
+ Views:
+ Current:
+ Class: rviz_default_plugins/Orbit
+ Distance: 9.22574234008789
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 1.1953976154327393
+ Target Frame:
+ Value: Orbit (rviz)
+ Yaw: 0.00539789441972971
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1865
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd0000000400000000000001f700000672fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000008200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000052000006720000011e00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f00000672fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000005200000672000000e000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000c000000005afc0100000002fb0000000800540069006d0065010000000000000c000000035b00fffffffb0000000800540069006d00650100000000000004500000000000000000000008980000067200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 3072
+ X: 0
+ Y: 31
diff --git a/seaweed_navigation/seaweed_navigation/a_star.py b/seaweed_navigation/seaweed_navigation/a_star.py
new file mode 100644
index 0000000..357d630
--- /dev/null
+++ b/seaweed_navigation/seaweed_navigation/a_star.py
@@ -0,0 +1,126 @@
+from base_grid_planner import WeightedNode, BasePlanner
+
+import heapq
+from geometry_msgs.msg import Point, PoseStamped
+from nav_msgs.msg import Path
+import rclpy
+from typing import List, Optional, Tuple, Set
+
+import math
+
+
+class AStarPlanner(BasePlanner):
+ def __init__(self, *args, **kwargs): # type: ignore
+ super().__init__(*args, **kwargs)
+
+ def heuristic(self, point1: Point, point2: Point) -> float:
+ return math.hypot(point1.x - point2.x, point1.y - point2.y)
+
+ def a_star(self, start: Point, goal: Point) -> Optional[WeightedNode]:
+ # directions = [(-1, 0), (1, 0), (0, -1), (0, 1)]
+ directions = [(-1, 0), (1, 0), (0, -1), (0, 1), (-1, -1), (-1, 1), (1, -1), (1, 1)]
+
+ pending_nodes: List[Tuple[float, WeightedNode]] = []
+ visited_nodes: Set[Tuple[float, float]] = set()
+
+ start_point: Point = self.world_to_grid(start)
+ goal_point: Point = self.world_to_grid(goal)
+
+ start_node = WeightedNode(point=start_point, cost=0)
+ heur = self.heuristic(start_point, goal_point)
+
+ heapq.heappush(pending_nodes, (start_node.cost + heur, start_node))
+
+ while pending_nodes and rclpy.ok():
+ _, current_node = heapq.heappop(pending_nodes)
+
+ if self.is_goal_reached(current_node.point, goal_point):
+ return current_node
+
+ current_point = (current_node.point.x, current_node.point.y)
+ if current_point in visited_nodes:
+ continue
+
+ visited_nodes.add(current_point)
+
+ for dx, dy in directions:
+ new_x = current_node.point.x + dx
+ new_y = current_node.point.y + dy
+ new_point = Point(x=new_x, y=new_y)
+ new_point_tuple = (new_x, new_y)
+
+ if new_point_tuple in visited_nodes:
+ continue
+
+ if not self.is_in_bounds(new_point):
+ continue
+
+ if not self.is_traversable(new_point):
+ continue
+
+ # NOTE: Not currently using each grid pts value in calc move cost
+ # Will need to change once implementing obstacle inflation regions
+ if abs(dx) + abs(dy) == 1: # Ortho move
+ move_cost = 1.0
+ else: # Diagonal move
+ move_cost = math.sqrt(2)
+
+ new_node_cost = current_node.cost + move_cost
+
+ new_node = WeightedNode(point=new_point, parent=current_node, cost=new_node_cost)
+ heur = self.heuristic(new_point, goal_point)
+
+ heapq.heappush(pending_nodes, (new_node.cost + heur, new_node))
+
+ return None
+
+ def reconstruct_path(self, node: WeightedNode) -> Path:
+ path = Path()
+ path.header.frame_id = self.map.header.frame_id
+
+ poses: List[PoseStamped] = []
+
+ while node and node.parent and rclpy.ok():
+ current_pose: PoseStamped = PoseStamped()
+ current_pose.pose.position.x = self.grid_to_world(node.point).x
+ current_pose.pose.position.y = self.grid_to_world(node.point).y
+ current_pose.header.frame_id = self.map.header.frame_id
+
+ poses.append(current_pose)
+ node = node.parent
+
+ poses.reverse()
+ path.poses = poses
+
+ return path
+
+ def plan(self, start: Point, goal: Point) -> Optional[Path]:
+ # start and goal inputs in world/map frame
+ self.grid_start = self.world_to_grid(start)
+ self.grid_goal = self.world_to_grid(goal)
+
+ if not self.is_in_bounds(self.grid_start):
+ self.logger.warn(f"Start pos {start} is out of bounds")
+ return None
+
+ if not self.is_in_bounds(self.grid_goal):
+ self.logger.warn(f"Goal pos {goal} is out of bounds")
+ return None
+
+ if not self.is_traversable(self.grid_start):
+ self.logger.warn(f"Start pos {start} is occupied")
+ return None
+
+ if not self.is_traversable(self.grid_goal):
+ self.logger.warn(f"Goal pos {goal} is occupied")
+ return None
+
+ goal_node = self.a_star(start, goal)
+
+ if not goal_node:
+ self.logger.warn("No path found")
+ return None
+
+ path: Path = self.reconstruct_path(goal_node)
+ self.logger.info(f"Path found w/ {len(path.poses)} poses")
+ return path
diff --git a/seaweed_navigation/seaweed_navigation/base_grid_planner.py b/seaweed_navigation/seaweed_navigation/base_grid_planner.py
new file mode 100644
index 0000000..11fe2d1
--- /dev/null
+++ b/seaweed_navigation/seaweed_navigation/base_grid_planner.py
@@ -0,0 +1,87 @@
+from abc import ABC, abstractmethod
+
+from geometry_msgs.msg import Point
+from nav_msgs.msg import OccupancyGrid, Path
+from rclpy.logging import get_logger
+
+import math
+from typing import Optional
+
+# Contains Utils for creating grid-based path planners
+
+
+class WeightedNode:
+ def __init__(self, point: Point, parent: Optional["WeightedNode"] = None, cost: float = 0):
+ self.cost = cost
+ self.point = point
+ self.parent = parent
+
+ def __lt__(self, other: "WeightedNode") -> bool:
+ return self.cost < other.cost
+
+ def __eq__(self, other: "WeightedNode"):
+ return self.point.x == other.point.x and self.point.y == other.point.y
+
+ def __hash__(self):
+ return hash((self.point.x, self.point.y))
+
+ def __add__(self, other: "WeightedNode"):
+ return WeightedNode(point=Point(x=self.point.x + other.point.x, y=self.point.y + other.point.y), cost=0)
+
+
+class BasePlanner(ABC):
+ def __init__(
+ self,
+ map: OccupancyGrid,
+ # start: Point,
+ # goal: Point,
+ # obstacle_tol: float,
+ goal_tolerance: float,
+ ):
+ self.map = map # OccupancyGrid()
+ self.debug_map: OccupancyGrid = OccupancyGrid()
+ # self.world_start = start
+ # self.world_goal = goal
+ # self.grid_start = self.world_to_grid(start)
+ # self.grid_goal = self.world_to_grid(goal)
+ # self.obstacle_tol = obstacle_tol
+ self.goal_tolerance = goal_tolerance
+ self.logger = get_logger("base_path_planner")
+
+ @abstractmethod
+ def plan(self) -> Optional[Path]:
+ pass
+
+ def world_to_grid(self, world_point: Point) -> Point:
+ origin = self.map.info.origin.position
+ resolution = self.map.info.resolution
+ grid_x = (world_point.x - origin.x) // resolution
+ grid_y = (world_point.y - origin.y) // resolution
+ return Point(x=grid_x, y=grid_y)
+
+ def grid_to_world(self, grid_point: Point) -> Point:
+ origin = self.map.info.origin.position
+ resolution = self.map.info.resolution
+ world_x = (grid_point.x * resolution) + origin.x
+ world_y = (grid_point.y * resolution) + origin.y
+ return Point(x=world_x, y=world_y)
+
+ def is_goal_reached(self, current_point: Point, goal_point: Point) -> bool:
+ goal_tolerance_grid_frame = self.goal_tolerance / self.map.info.resolution
+ return math.hypot(current_point.x - goal_point.x, current_point.y - goal_point.y) < goal_tolerance_grid_frame
+
+ def is_in_bounds(self, grid_point: Point) -> bool:
+ return 0 <= grid_point.x < self.map.info.width and 0 <= grid_point.y < self.map.info.height
+
+ def is_traversable(self, grid_point: Point) -> bool:
+ grid_val = self.get_grid_val(grid_point)
+ return 0 <= grid_val < 99
+
+ def get_grid_val(self, grid_point: Point) -> int:
+ # alr validate this earlier in a* so can remove perchance
+ if not self.is_in_bounds(grid_point):
+ return -1
+ return self.map.data[self.get_grid_index(grid_point)]
+
+ def get_grid_index(self, grid_point: Point) -> int:
+ return int(grid_point.x + grid_point.y * self.map.info.width)
diff --git a/seaweed_navigation/seaweed_navigation/navigation_manager.py b/seaweed_navigation/seaweed_navigation/navigation_manager.py
new file mode 100755
index 0000000..43ce6a1
--- /dev/null
+++ b/seaweed_navigation/seaweed_navigation/navigation_manager.py
@@ -0,0 +1,134 @@
+#!/usr/bin/env python3
+
+from typing import Optional
+import rclpy
+from rclpy.time import Time
+from rclpy.node import Node
+from rclpy.duration import Duration
+from rclpy.action import ActionClient
+
+# from rclpy.action import ActionServer
+from geometry_msgs.msg import PoseStamped, Pose, Point
+from a_star import AStarPlanner
+from nav_msgs.msg import OccupancyGrid, Path
+from tf2_ros import Buffer, TransformListener
+from rclpy.qos import QoSProfile, DurabilityPolicy
+from seaweed_interfaces.action import MotionPlanner
+
+
+class NavigationManager(Node):
+ def __init__(self):
+ super().__init__("navigation_manager")
+
+ # self.action_server = ActionServer(
+ # self,
+ # NavigateToPose,
+ # 'navigate_to_pose',
+ # execute_callback=self.execute_callback,
+ # cancel_callback=self.cancel_callback,
+ # goal_callback=self.goal_callback
+ # )
+
+ map_qos = QoSProfile(depth=10)
+ map_qos.durability = DurabilityPolicy.TRANSIENT_LOCAL
+
+ self.map_sub = self.create_subscription(OccupancyGrid, "/map", self.map_callback, map_qos)
+ self.pose_sub = self.create_subscription(PoseStamped, "/goal_pose", self.goal_callback, 10)
+
+ self.path_pub = self.create_publisher(Path, "/path", 10)
+
+ self.map = None
+ goal_tol = 0.1 # m
+ self.planner = AStarPlanner(map=self.map, goal_tolerance=goal_tol)
+
+ self.tf_buffer = Buffer()
+ self.tf_listener = TransformListener(self.tf_buffer, self)
+
+ # DEBUG: Visualize visited nodes
+ self.debug_map = OccupancyGrid()
+ self.debug_map_pub = self.create_publisher(OccupancyGrid, "/debug/map", 10)
+
+ self.action_client = ActionClient(self, MotionPlanner, "motion_planner")
+ self.send_goal_future = None
+ self.get_result_future = None
+
+ def map_callback(self, map_msg: OccupancyGrid):
+ self.map = map_msg
+ self.planner.map = map_msg
+
+ self.debug_map.header.frame_id = map_msg.header.frame_id
+ self.debug_map.info = map_msg.info
+
+ def goal_callback(self, goal_pose_stamped: PoseStamped):
+ if self.map is None:
+ self.get_logger().error("No map received")
+ return
+
+ base_link_frame = "wamv/base_link"
+ robot_pose = self.lookup_pose(target_frame=self.map.header.frame_id, source_frame=base_link_frame)
+
+ if robot_pose is None:
+ self.get_logger().error("Can't get robot pose in map frame")
+ return
+
+ robot_position = Point()
+ robot_position.x = robot_pose.position.x
+ robot_position.y = robot_pose.position.y
+
+ goal_point = Point()
+ goal_point.x = goal_pose_stamped.pose.position.x
+ goal_point.y = goal_pose_stamped.pose.position.y
+
+ path = self.planner.plan(robot_position, goal_point)
+ goal = MotionPlanner.Goal()
+
+ if path and path.poses:
+ goal.path = path
+ self.action_client.wait_for_server()
+ self.send_goal_future = self.action_client.send_goal_async(goal)
+ self.send_goal_future.add_done_callback(self.goal_response_callback)
+ else:
+ self.get_logger().warn("No path found")
+
+ def goal_response_callback(self, future): # type:ignore
+ goal_handle = future.result()
+ if not goal_handle.accepted:
+ self.get_logger().info("Goal rejected :(")
+ return
+
+ self.get_logger().info("Goal accepted :)")
+
+ self.get_result_future = goal_handle.get_result_async()
+ self.get_result_future.add_done_callback(self.get_result_callback)
+
+ def get_result_callback(self, future): # type:ignore
+ _ = future.result().result
+
+ def lookup_pose(self, target_frame: str, source_frame: str) -> Optional[Pose]:
+ try:
+ tf = self.tf_buffer.lookup_transform(target_frame, source_frame, Time(), timeout=Duration(seconds=1))
+
+ pose = Pose()
+ pose.position.x = tf.transform.translation.x
+ pose.position.y = tf.transform.translation.y
+ pose.position.z = tf.transform.translation.z
+ pose.orientation = tf.transform.rotation
+
+ return pose
+
+ except Exception as ex:
+ self.get_logger().error(f"Cannot transform from {source_frame} to {target_frame}: {ex}")
+ return None
+
+
+def main(args=None): # type: ignore
+ rclpy.init(args=args)
+ try:
+ navigation_manager = NavigationManager()
+ rclpy.spin(navigation_manager)
+ finally:
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/seaweed_navigation/seaweed_navigation/pd_motion_planner.py b/seaweed_navigation/seaweed_navigation/pd_motion_planner.py
new file mode 100755
index 0000000..4ed5d2a
--- /dev/null
+++ b/seaweed_navigation/seaweed_navigation/pd_motion_planner.py
@@ -0,0 +1,435 @@
+#!/usr/bin/env python3
+
+import threading
+import math
+import time
+from typing import List
+import rclpy
+from rclpy.time import Time
+from rclpy.node import Node
+from rclpy.duration import Duration
+from rclpy.action import ActionServer
+from rclpy.action import GoalResponse
+from rclpy.executors import MultiThreadedExecutor
+
+# from rclpy.action import ActionServer
+from geometry_msgs.msg import PoseStamped, Pose, TwistStamped
+from pid import PID
+from nav_msgs.msg import Path
+from std_msgs.msg import Float64
+from tf2_ros import Buffer, TransformListener
+from tf2_geometry_msgs import do_transform_pose
+# from rclpy.qos import QoSProfile, DurabilityPolicy
+
+from seaweed_interfaces.action import MotionPlanner
+
+
+class PDMotionPlanner(Node):
+ def __init__(self):
+ super().__init__("pd_motion_planner")
+
+ self.declare_parameter("heading_kp", 1.0)
+ self.declare_parameter("heading_kd", 0.3)
+
+ self.declare_parameter("cross_track_kp", 0.3)
+ self.declare_parameter("cross_track_kd", 0.1)
+
+ self.declare_parameter("ks", 0.5) # along-track gain
+ self.declare_parameter("kn", 0.1) # cross-track gain
+ self.declare_parameter("ktheta", 0.01) # heading gain
+
+ self.declare_parameter("goal_tolerance", 5.0)
+
+ self.declare_parameter("lookahead_dist", 1.5)
+ self.declare_parameter("max_linear_velocity", 1.0)
+ self.declare_parameter("max_angular_velocity", 1.0)
+
+ self.declare_parameter("open_loop_enabled", True)
+ self.declare_parameter("closed_loop_enabled", True)
+
+ self.heading_kp: float = self.get_parameter("heading_kp").value # type: ignore
+ self.heading_kd: float = self.get_parameter("heading_kd").value # type: ignore
+
+ self.goal_tolerance: float = self.get_parameter("goal_tolerance").value # type:ignore
+
+ self.cross_track_kp: float = self.get_parameter("cross_track_kp").value # type: ignore
+ self.cross_track_kd: float = self.get_parameter("cross_track_kd").value # type: ignore
+
+ self.ks: float = self.get_parameter("ks").value # type: ignore
+ self.kn: float = self.get_parameter("kn").value # type: ignore
+ self.ktheta: float = self.get_parameter("ktheta").value # type: ignore
+
+ self.lookahead_dist: float = self.get_parameter("lookahead_dist").value # type: ignore
+ self.max_linear_velocity: float = self.get_parameter("max_linear_velocity").value # type: ignore
+ self.max_angular_velocity: float = self.get_parameter("max_angular_velocity").value # type: ignore
+
+ self.open_loop_enabled: bool = self.get_parameter("open_loop_enabled").value # type: ignore
+ self.closed_loop_enabled: bool = self.get_parameter("closed_loop_enabled").value # type: ignore
+
+ self.along_track_pid = PID(0.1, 0.0, 0.0)
+ self.cross_track_pid = PID(self.cross_track_kp, 0.0, self.cross_track_kd)
+ self.heading_pid = PID(self.heading_kp, 0.0, self.heading_kd)
+
+ self.path_sub = self.create_subscription(Path, "/path", self.path_callback, 10)
+ self.cmd_vel_pub = self.create_publisher(TwistStamped, "/cmd_vel", 10)
+ self.control_loop = None
+
+ self.tf_buffer = Buffer()
+ self.tf_listener = TransformListener(self.tf_buffer, self)
+
+ self.goal_lock = threading.Lock()
+
+ self.path: Path = Path()
+ self.last_cycle_time = self.get_clock().now()
+ self.goal_reached = False
+ self._goal_handle = None
+ self._is_executing = False
+
+ self.action_server = ActionServer(
+ self,
+ MotionPlanner,
+ "motion_planner",
+ execute_callback=self.execute_callback,
+ goal_callback=self.goal_callback,
+ )
+ # DEBUG
+ self.debug_distance_from_goal = self.create_publisher(Float64, "/debug/distance_from_goal", 10)
+ self.debug_heading_error = self.create_publisher(Float64, "/debug/heading_error", 10)
+ self.debug_cross_track_error = self.create_publisher(Float64, "/debug/cross_track_error", 10)
+ self.debug_along_track_error = self.create_publisher(Float64, "/debug/along_track_error", 10)
+ self.debug_goal_heading = self.create_publisher(Float64, "/debug/goal_heading", 10)
+ self.debug_robot_heading = self.create_publisher(Float64, "/debug/robot_heading", 10)
+
+ def goal_callback(self, goal_request): # type: ignore
+ with self.goal_lock:
+ if self._is_executing:
+ self.get_logger().info("Goal rejected: Already busy!")
+ return GoalResponse.REJECT
+ else:
+ self._is_executing = True
+ return GoalResponse.ACCEPT
+
+ def execute_callback(self, goal_handle): # type: ignore
+ self.get_logger().info("Executing goal...")
+ self._goal_handle = goal_handle
+
+ feedback = MotionPlanner.Feedback()
+ feedback.distance_to_goal = 0.0
+
+ self.path = goal_handle.request.path
+ self.last_cycle_time = self.get_clock().now()
+ self.heading_pid.reset(reset_integral=True, reset_derivative=True)
+ self.cross_track_pid.reset(reset_integral=True, reset_derivative=True)
+ self.transform_plan()
+
+ self.goal_reached = False
+
+ robot_pose = self.get_robot_pose()
+ while not self.goal_reached or self._goal_handle is None:
+ self.update_controls()
+ robot_pose = self.get_robot_pose()
+ feedback.distance_to_goal = self.get_pose_error(robot_pose.pose, list(self.path.poses)[-1].pose) # type:ignore
+ goal_handle.publish_feedback(feedback)
+ time.sleep(0.1)
+
+ goal_handle.succeed()
+
+ with self.goal_lock:
+ self._goal_handle = None
+ self._is_executing = None
+
+ result = MotionPlanner.Result()
+ result.final_pose = self.get_robot_pose().pose
+ return result
+
+ def path_callback(self, msg: Path):
+ self.last_cycle_time = self.get_clock().now()
+ self.path = msg
+ self.heading_pid.reset(reset_integral=True, reset_derivative=True)
+ self.cross_track_pid.reset(reset_integral=True, reset_derivative=True)
+ self.transform_plan()
+
+ def update_controls(self):
+ if not self.path.poses:
+ self.publish_cmd_vel((0.0, 0.0, 0.0), (0.0, 0.0, 0.0))
+ return
+
+ dt = (self.get_clock().now() - self.last_cycle_time).nanoseconds * 1e-9
+
+ try:
+ robot_pose = self.get_robot_pose()
+ next_path_pose, second_next_path_pose = self.get_target_poses(robot_pose.pose, self.lookahead_dist)
+ except ValueError as ex:
+ self.get_logger().warn(f"Error performing robot/path transforms: {ex}")
+ self.publish_cmd_vel((0.0, 0.0, 0.0), (0.0, 0.0, 0.0))
+ self.last_cycle_time = self.get_clock().now()
+ return
+
+ next_path_pose, second_next_path_pose = self.get_target_poses(robot_pose.pose, self.lookahead_dist)
+
+ along_track_error = self.get_along_track_error(robot_pose.pose, next_path_pose)
+ cross_track_error = self.get_cross_track_error(robot_pose.pose, next_path_pose, second_next_path_pose)
+ heading_error = self.get_heading_error(robot_pose.pose, next_path_pose, second_next_path_pose)
+ test_goal_heading: float = -math.pi
+
+ # open-loop
+ desired_curvature = 0.0
+ desired_velocity = 0.0
+ if self.open_loop_enabled:
+ desired_velocity = self.get_desired_velocity(robot_pose.pose, next_path_pose, second_next_path_pose)
+ desired_curvature = self.get_desired_curvature(next_path_pose, second_next_path_pose, desired_velocity)
+
+ # feedback
+ velocity_correction = 0.0
+ curvature_correction = 0.0
+
+ if self.closed_loop_enabled:
+ velocity_correction = self.ks * along_track_error
+ curvature_correction = self.heading_pid.update(heading_error, dt) + self.cross_track_pid.update(
+ cross_track_error, dt
+ )
+
+ output_linear_velocity = desired_velocity + velocity_correction
+ output_curvature = desired_curvature + curvature_correction
+ output_angular_velocity = output_linear_velocity * output_curvature
+
+ final_velocity = max(-self.max_linear_velocity, min(self.max_linear_velocity, output_linear_velocity))
+ angular_velocity = max(-self.max_angular_velocity, min(self.max_angular_velocity, output_angular_velocity))
+
+ # DEBUG
+ self.debug_distance_from_goal.publish(
+ Float64(data=self.get_pose_error(robot_pose.pose, list(self.path.poses)[0].pose))
+ )
+ self.debug_heading_error.publish(Float64(data=heading_error))
+ self.debug_cross_track_error.publish(Float64(data=cross_track_error))
+ self.debug_goal_heading.publish(Float64(data=test_goal_heading))
+ self.debug_robot_heading.publish(Float64(data=self.get_robot_heading(robot_pose.pose)))
+
+ if self.check_reached_goal(robot_pose.pose, self.goal_tolerance):
+ self.get_logger().info("Goal reached!")
+ self.goal_reached = True
+ self.publish_cmd_vel((0.0, 0.0, 0.0), (0.0, 0.0, 0.0))
+ self._is_executing = False
+ elif self.goal_reached is True:
+ self.publish_cmd_vel((0.0, 0.0, 0.0), (0.0, 0.0, 0.0))
+ else:
+ self.goal_reached = False
+ self.publish_cmd_vel((final_velocity, 0.0, 0.0), (0.0, 0.0, angular_velocity))
+
+ self.last_cycle_time = self.get_clock().now()
+
+ def open_loop_controller(self):
+ pass
+
+ def get_desired_velocity(self, robot_pose: Pose, next_pose: Pose, second_next_pose: Pose | None) -> float:
+ # needs motion profiling or similar
+ return self.max_linear_velocity * 0.5
+
+ def get_desired_curvature(self, next_pose: Pose, second_next_pose: Pose | None, velocity: float) -> float:
+ if second_next_pose is None:
+ return 0.0
+ else:
+ dtheta = self.get_robot_heading(second_next_pose) - self.get_robot_heading(next_pose)
+ ds = math.hypot(
+ second_next_pose.position.x - next_pose.position.x, second_next_pose.position.y - next_pose.position.y
+ )
+ return dtheta / ds
+
+ def get_along_track_error(self, robot_pose: Pose, goal_pose: Pose) -> float:
+ target_dx = goal_pose.position.x - robot_pose.position.x
+ target_dy = goal_pose.position.y - robot_pose.position.y
+
+ robot_heading = self.get_robot_heading(robot_pose)
+ robot_heading_x = math.cos(robot_heading)
+ robot_heading_y = math.sin(robot_heading)
+
+ # Pos = robot behind goal; neg = robot ahead of goal
+ # neg should not occur as per how we select the next local goal
+ return target_dx * robot_heading_x + target_dy * robot_heading_y
+
+ def get_cross_track_error(
+ self, robot_pose: Pose, next_path_pose: Pose, second_next_path_pose: Pose | None
+ ) -> float:
+ # on last pose in path
+ if second_next_path_pose is None:
+ robot_dx = robot_pose.position.x - next_path_pose.position.x
+ robot_dy = robot_pose.position.y - next_path_pose.position.y
+ return math.sqrt(robot_dx**2 + robot_dy**2)
+
+ robot_dx = robot_pose.position.x - next_path_pose.position.x
+ robot_dy = robot_pose.position.y - next_path_pose.position.y
+
+ path_dx = second_next_path_pose.position.x - next_path_pose.position.x
+ path_dy = second_next_path_pose.position.y - next_path_pose.position.y
+ pose_vector_mag = math.sqrt(path_dx**2 + path_dy**2)
+
+ boat_x = robot_pose.position.x
+ boat_y = robot_pose.position.y
+
+ x2 = second_next_path_pose.position.x
+ y2 = second_next_path_pose.position.y
+ x1 = next_path_pose.position.x
+ y1 = next_path_pose.position.y
+
+ # Pos -> left of path; Neg -> right of path
+ return ((path_dy) * boat_x - (path_dx) * boat_y + x2 * y1 - y2 * x1) / pose_vector_mag
+
+ def get_heading_error(self, robot_pose: Pose, next_path_pose: Pose, second_next_path_pose: Pose | None) -> float:
+ # on last pose in path
+ if second_next_path_pose is None:
+ dx = next_path_pose.position.x - robot_pose.position.x
+ dy = next_path_pose.position.y - robot_pose.position.y
+
+ # if within threshold, ignore heading error
+ distance_to_target = math.sqrt(dx**2 + dy**2)
+ if distance_to_target < 0.1: # within 10cm (arb chosen for now)
+ return 0.0
+
+ else:
+ # Not on last pose
+ dx = second_next_path_pose.position.x - next_path_pose.position.x
+ dy = second_next_path_pose.position.y - next_path_pose.position.y
+
+ desired_heading = math.atan2(dy, dx)
+ robot_heading = self.get_robot_heading(robot_pose)
+
+ # normalize heading error to [-pi, pi]
+ heading_error = desired_heading - robot_heading
+ heading_error = math.atan2(math.sin(heading_error), math.cos(heading_error))
+
+ return heading_error
+
+ def get_robot_pose(self, target_frame: str = "map", source_frame: str = "wamv/base_link") -> PoseStamped:
+ try:
+ tf = self.tf_buffer.lookup_transform(target_frame, source_frame, Time(), timeout=Duration(seconds=1))
+
+ pose = PoseStamped()
+ pose.pose.position.x = tf.transform.translation.x
+ pose.pose.position.y = tf.transform.translation.y
+ pose.pose.position.z = tf.transform.translation.z
+ pose.pose.orientation = tf.transform.rotation
+
+ pose.header.frame_id = target_frame
+
+ return pose
+
+ except Exception as ex:
+ self.get_logger().error(f"Cannot transform from {source_frame} to {target_frame}: {ex}")
+ return PoseStamped()
+
+ def get_robot_heading(self, robot_pose: Pose) -> float:
+ q = [robot_pose.orientation.x, robot_pose.orientation.y, robot_pose.orientation.z, robot_pose.orientation.w]
+
+ _, _, yaw = self.quaternion_to_euler(q)
+ return yaw
+
+ def quaternion_to_euler(self, q: list[float]) -> tuple[float, float, float]:
+ """
+ Convert a quaternion to Euler angles (roll, pitch, yaw).
+
+ Args:
+ q: Quaternion as [x, y, z, w]
+
+ Returns:
+ Tuple of (roll, pitch, yaw) in radians
+ """
+ x, y, z, w = q
+
+ # Roll (rotation around x-axis)
+ sinr_cosp = 2 * (w * x + y * z)
+ cosr_cosp = 1 - 2 * (x * x + y * y)
+ roll = math.atan2(sinr_cosp, cosr_cosp)
+
+ # Pitch (rotation around y-axis)
+ sinp = 2 * (w * y - z * x)
+ if abs(sinp) >= 1:
+ pitch = math.copysign(math.pi / 2, sinp) # Gimbal lock: clamp to ±90°
+ else:
+ pitch = math.asin(sinp)
+
+ # Yaw (rotation around z-axis)
+ siny_cosp = 2 * (w * z + x * y)
+ cosy_cosp = 1 - 2 * (y * y + z * z)
+ yaw = math.atan2(siny_cosp, cosy_cosp)
+
+ return (roll, pitch, yaw)
+
+ def transform_plan(self, target_frame: str = "map") -> bool:
+ try:
+ tf = self.tf_buffer.lookup_transform(target_frame, self.path.header.frame_id, Time())
+ except Exception as ex:
+ self.get_logger().error(
+ f"Couldn't transform plan from frame {self.path.header.frame_id} to {target_frame}: {ex}"
+ )
+ return False
+
+ poses: List[PoseStamped] = self.path.poses
+ for poseStamped in poses:
+ poseStamped.pose = do_transform_pose(poseStamped.pose, tf)
+ poseStamped.header.frame_id = target_frame
+
+ self.path.header.frame_id = target_frame
+ return True
+
+ def get_target_poses(self, robot_pose: Pose, lookahead_dist: float) -> tuple[Pose, Pose | None]:
+ remaining_poses: List[PoseStamped] = list(self.path.poses)
+ if not remaining_poses:
+ raise ValueError("Empty Path (No poses)")
+
+ for i, pose in enumerate(self.path.poses):
+ dx = pose.pose.position.x - robot_pose.position.x
+ dy = pose.pose.position.y - robot_pose.position.y
+ distance = math.sqrt(dx * dx + dy * dy)
+
+ if distance >= lookahead_dist and self.get_along_track_error(robot_pose, pose.pose) >= 0:
+ self.path.poses = remaining_poses[i:]
+ second_next_pose = self.path.poses[1].pose if len(self.path.poses) >= 2 else None
+ return pose.pose, second_next_pose
+
+ # else, did not find any valid poses
+ self.get_logger().warn("No valid target pose found, returning pose of last waypoint")
+ return remaining_poses[-1].pose, None
+
+ def get_pose_error(self, pose1: Pose, pose2: Pose) -> float:
+ dx = pose1.position.x - pose2.position.x
+ dy = pose1.position.y - pose2.position.y
+ return math.sqrt(dx * dx + dy * dy)
+
+ def publish_cmd_vel(self, linear: tuple[float, float, float], angular: tuple[float, float, float]):
+ cmd_vel = TwistStamped()
+ cmd_vel.header.frame_id = "wamv/base_link"
+ cmd_vel.header.stamp = self.get_clock().now().to_msg()
+
+ cmd_vel.twist.linear.x = linear[0]
+ cmd_vel.twist.linear.y = linear[1]
+ cmd_vel.twist.linear.z = linear[2]
+
+ cmd_vel.twist.angular.x = angular[0]
+ cmd_vel.twist.angular.y = angular[1]
+ cmd_vel.twist.angular.z = angular[2]
+
+ self.cmd_vel_pub.publish(cmd_vel)
+
+ def check_reached_goal(self, robot_pose: Pose, goal_threshold: float) -> bool:
+ pose_list = list(self.path.poses)
+ last_pose = pose_list[-1]
+ dx = last_pose.pose.position.x - robot_pose.position.x
+ dy = last_pose.pose.position.y - robot_pose.position.y
+ distance = math.sqrt(dx * dx + dy * dy)
+ return distance <= goal_threshold
+
+
+def main(args=None): # type: ignore
+ rclpy.init(args=args)
+ pd_motion_planner = PDMotionPlanner()
+ executor = MultiThreadedExecutor()
+ executor.add_node(pd_motion_planner)
+ try:
+ executor.spin()
+ finally:
+ pd_motion_planner.destroy_node
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/seaweed_navigation/seaweed_navigation/pid.py b/seaweed_navigation/seaweed_navigation/pid.py
new file mode 100644
index 0000000..e4ae34f
--- /dev/null
+++ b/seaweed_navigation/seaweed_navigation/pid.py
@@ -0,0 +1,70 @@
+#!/usr/bin/env python3
+
+
+class PID:
+ def __init__(
+ self,
+ kp: float = 0.0,
+ ki: float = 0.0,
+ kd: float = 0.0,
+ ):
+ self.kp = kp
+ self.ki = ki
+ self.kd = kd
+ self.prev_error = None
+ self.integral_error = 0.0
+
+ self.output_min = None
+ self.output_max = None
+
+ self.integral_min = None
+ self.integral_max = None
+
+ def reset(self, reset_integral: bool, reset_derivative: bool):
+ if reset_integral:
+ self.integral_error = 0.0
+ if reset_derivative:
+ self.prev_error = None
+
+ def set_gains(self, kp: float, ki: float, kd: float):
+ self.kp = kp
+ self.ki = ki
+ self.kd = kd
+
+ def set_output_limits(self, output_min: float, output_max: float):
+ self.output_min = output_min
+ self.output_max = output_max
+
+ def set_integral_windup_limits(self, integral_min: float, integral_max: float):
+ self.integral_min = integral_min
+ self.integral_max = integral_max
+
+ def update(self, error: float, dt: float) -> float:
+ if dt <= 0.0:
+ return 0.0
+
+ p = self.kp * error
+
+ self.integral_error += error * dt
+ if self.integral_min is not None:
+ self.integral_error = max(self.integral_min, self.integral_error)
+ if self.integral_max is not None:
+ self.integral_error = min(self.integral_max, self.integral_error)
+
+ i = self.ki * self.integral_error
+
+ if self.prev_error is None:
+ d = 0.0
+ else:
+ error_derv = (error - self.prev_error) / dt
+ d = self.kd * error_derv
+
+ self.prev_error = error
+
+ output = p + i + d
+ if self.output_min is not None:
+ output = max(self.output_min, output)
+ if self.output_max is not None:
+ output = min(self.output_max, output)
+
+ return output
diff --git a/seaweed_navigation/seaweed_navigation/test_map_generator.py b/seaweed_navigation/seaweed_navigation/test_map_generator.py
new file mode 100755
index 0000000..1ea94e4
--- /dev/null
+++ b/seaweed_navigation/seaweed_navigation/test_map_generator.py
@@ -0,0 +1,125 @@
+#!/usr/bin/env python3
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import OccupancyGrid
+from tf2_ros import Buffer, TransformListener, TransformStamped
+from rclpy.qos import QoSProfile, DurabilityPolicy
+from tf2_ros import StaticTransformBroadcaster
+from enum import Enum
+from geometry_msgs.msg import PoseStamped
+
+
+class Direction(Enum):
+ HORIZONTAL = 0
+ VERTICAL = 1
+
+
+class TestMapGenerator(Node):
+ def __init__(self):
+ super().__init__("test_map")
+
+ map_qos = QoSProfile(depth=10)
+ map_qos.durability = DurabilityPolicy.TRANSIENT_LOCAL
+
+ self.map_pub = self.create_publisher(OccupancyGrid, "/map", map_qos)
+ self.map = OccupancyGrid()
+
+ self.goal_pub = self.create_publisher(PoseStamped, "/goal_pose", map_qos)
+ self.goalPose = PoseStamped()
+
+ self.tf_buffer = Buffer()
+ self.tf_listener = TransformListener(self.tf_buffer, self)
+ self.tf_broadcaster = StaticTransformBroadcaster(self)
+
+ # map params
+ self.map_width = 700
+ self.map_height = 700
+ self.map_resolution = 0.05
+ self.map_origin = [-20.0, -20.0, 0.0] # keep z = 0.0
+
+ self.init_map()
+
+ self.goalPose: PoseStamped = PoseStamped()
+ self.goalPose.pose.position.x = 10.0
+ self.goalPose.pose.position.y = 10.0
+ self.goalPose.header.frame_id = "map"
+ self.goalPose.header
+
+ # reference: Unknown = -1, Obstacles = 100, 0 < Free < 99
+ self.grid = np.full((self.map_height, self.map_width), 0, dtype=np.int8)
+
+ # ADD TEST OBSTACLES HERE
+ self.generate_rect(300, 300, 75, 30)
+ self.generate_rect(520, 400, 30, 30)
+ self.generate_rect(400, 520, 30, 30)
+
+ # self.publish_test_robot_tf()
+ self.generate_map_timer = self.create_timer(0.1, self.timer_callback)
+
+ def init_map(self):
+ self.map.header.frame_id = "map"
+ self.map.info.resolution = self.map_resolution
+ self.map.info.width = self.map_width
+ self.map.info.height = self.map_height
+ self.map.info.origin.position.x = self.map_origin[0]
+ self.map.info.origin.position.y = self.map_origin[1]
+ self.map.info.origin.position.z = self.map_origin[2]
+ self.map.info.origin.orientation.w = 1.0
+
+ def generate_circle(self, x: int, y: int, r: int):
+ pass
+
+ def generate_line(self, x: int, y: int, dist: int, direction: Direction):
+ if direction == Direction.HORIZONTAL:
+ for i in range(dist):
+ if 0 <= y < self.map_height and 0 <= x + i < self.map_width:
+ self.grid[y, x + i] = 100
+ elif direction == Direction.VERTICAL:
+ for i in range(dist):
+ if 0 <= y + i < self.map_height and 0 <= x < self.map_width:
+ self.grid[y + i, x] = 100
+
+ def generate_rect(self, x: int, y: int, l: int, w: int):
+ # l -> horizontal, w -> vertical
+ self.generate_line(x, y, l, Direction.HORIZONTAL)
+ self.generate_line(x, y + w, l, Direction.HORIZONTAL)
+ self.generate_line(x, y, w, Direction.VERTICAL)
+ self.generate_line(x + l, y, w, Direction.VERTICAL)
+
+ def publish_test_robot_tf(self):
+ t = TransformStamped()
+ t.header.stamp = self.get_clock().now().to_msg()
+ t.header.frame_id = "map"
+ t.child_frame_id = "wamv/base_link"
+
+ t.transform.translation.x = 0.5
+ t.transform.translation.y = 0.5
+ t.transform.translation.z = 0.0
+
+ t.transform.rotation.x = 0.0
+ t.transform.rotation.y = 0.0
+ t.transform.rotation.z = 0.0
+ t.transform.rotation.w = 1.0
+
+ self.tf_broadcaster.sendTransform(t)
+
+ def timer_callback(self):
+ self.map.header.stamp = self.get_clock().now().to_msg()
+ self.map.data = self.grid.flatten().astype(np.int8).tolist()
+ self.map_pub.publish(self.map)
+ self.goal_pub.publish(self.goalPose)
+
+
+def main(args=None): # type: ignore
+ rclpy.init(args=args)
+ try:
+ test_map = TestMapGenerator()
+ rclpy.spin(test_map)
+ finally:
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/seaweed_sim/launch/gazebo.launch.py b/seaweed_sim/launch/gazebo.launch.py
index 39ff2c7..91b7321 100644
--- a/seaweed_sim/launch/gazebo.launch.py
+++ b/seaweed_sim/launch/gazebo.launch.py
@@ -66,7 +66,7 @@ def generate_launch_description():
world_arg = DeclareLaunchArgument(
name="world",
- default_value="sydney_regatta",
+ default_value="follow_path",
choices=[
"sydney_regatta",
"sydney_regatta_empty",
diff --git a/seaweed_sim/rviz/gazebo_full.rviz b/seaweed_sim/rviz/gazebo_full.rviz
index d4a80fd..fcf5753 100644
--- a/seaweed_sim/rviz/gazebo_full.rviz
+++ b/seaweed_sim/rviz/gazebo_full.rviz
@@ -5,7 +5,7 @@ Panels:
Property Tree Widget:
Expanded: ~
Splitter Ratio: 0.5508981943130493
- Tree Height: 1018
+ Tree Height: 358
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
@@ -23,7 +23,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
- SyncSource: PointCloud2
+ SyncSource: ""
Visualization Manager:
Class: ""
Displays:
@@ -63,125 +63,6 @@ Visualization Manager:
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
- back_left_engine_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- back_left_propeller_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- back_right_engine_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- back_right_propeller_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- ball_shooter_base_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- ball_shooter_launcher_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- base_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- camera_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- camera_link_optical:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- camera_post_arm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- camera_post_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- cpu_cases_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- front_left_engine_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- front_left_propeller_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- front_right_engine_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- front_right_propeller_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- gps_wamv_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- imu_wamv_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- left_battery_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- lidar_wamv_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- lidar_wamv_post_arm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- lidar_wamv_post_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- receiver:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- right_battery_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
Mass Properties:
Inertia: false
Mass: false
@@ -216,7 +97,7 @@ Visualization Manager:
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
- Enabled: true
+ Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 0
@@ -237,7 +118,7 @@ Visualization Manager:
Value: /wamv/sensors/lidars/lidar_wamv_sensor/points
Use Fixed Frame: true
Use rainbow: true
- Value: true
+ Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
@@ -250,7 +131,7 @@ Visualization Manager:
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
- Enabled: true
+ Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 0
@@ -271,12 +152,12 @@ Visualization Manager:
Value: /wamv/sensors/lidars/lidar_wamv_sensor/scan
Use Fixed Frame: true
Use rainbow: true
- Value: true
+ Value: false
- Alpha: 0.699999988079071
Class: rviz_default_plugins/Map
Color Scheme: map
Draw Behind: false
- Enabled: false
+ Enabled: true
Name: Map
Topic:
Depth: 5
@@ -284,15 +165,15 @@ Visualization Manager:
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
- Value: ""
+ Value: /map
Update Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
- Value: ""
+ Value: /map_updates
Use Timestamp: false
- Value: false
+ Value: true
- Angle Tolerance: 0.10000000149011612
Class: rviz_default_plugins/Odometry
Covariance:
@@ -332,6 +213,34 @@ Visualization Manager:
Reliability Policy: Reliable
Value: /odometry/filtered
Value: true
+ - Alpha: 1
+ Buffer Length: 1
+ Class: rviz_default_plugins/Path
+ Color: 25; 255; 0
+ Enabled: true
+ Head Diameter: 0.30000001192092896
+ Head Length: 0.20000000298023224
+ Length: 0.30000001192092896
+ Line Style: Lines
+ Line Width: 0.029999999329447746
+ Name: Path
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Pose Color: 255; 85; 255
+ Pose Style: None
+ Radius: 0.029999999329447746
+ Shaft Diameter: 0.10000000149011612
+ Shaft Length: 0.10000000149011612
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /path
+ Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
@@ -378,7 +287,7 @@ Visualization Manager:
Views:
Current:
Class: rviz_default_plugins/Orbit
- Distance: 31.723081588745117
+ Distance: 46.48396301269531
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
@@ -393,20 +302,20 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
- Pitch: 0.8103987574577332
+ Pitch: 0.5297964811325073
Target Frame:
Value: Orbit (rviz)
- Yaw: 5.5753960609436035
+ Yaw: 0.8372100591659546
Saved: ~
Window Geometry:
Displays:
collapsed: false
- Height: 2432
+ Height: 1216
Hide Left Dock: false
Hide Right Dock: false
Image:
collapsed: false
- QMainWindow State: 000000ff00000000fd00000004000000000000030100000888fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b000000ab00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000069000004e80000017800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000055d000003940000004500fffffffb0000000a0049006d0061006700650100000299000000fe000000000000000000000001000001ac00000888fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000069000008880000012300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000f740000005afc0100000002fb0000000800540069006d0065010000000000000f740000046000fffffffb0000000800540069006d0065010000000000000450000000000000000000000aaf0000088800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd000000040000000000000301000003e9fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000008200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000520000023e0000011e00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000299000001a20000003400fffffffb0000000a0049006d0061006700650100000299000000fe000000000000000000000001000001ac000003e9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000052000003e9000000e000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007ba0000005afc0100000002fb0000000800540069006d00650100000000000007ba0000035b00fffffffb0000000800540069006d00650100000000000004500000000000000000000002fb000003e900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -415,6 +324,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: false
- Width: 3956
- X: 140
- Y: 54
+ Width: 1978
+ X: 855
+ Y: 401