seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

feat: Add pure pursuit path follower for Nav2 (Issue #333)

Browse Source 
Implements saltybot_pure_pursuit package:
- Pure pursuit algorithm for path following with configurable parameters
- Lookahead distance (0.5m default) for target point on path
- Goal tolerance (0.1m) for goal detection
- Heading error correction to reduce speed when misaligned with path
- Publishes Twist commands on /cmd_vel_tracked for Nav2 integration
- Subscribes to /odom (odometry) and /path (Path trajectory)
- Tracks and publishes cross-track error for monitoring

Pure pursuit geometry:
- Finds closest point on path to robot current position
- Looks ahead specified distance along path from closest point
- Computes steering angle to follow circular arc to lookahead point
- Reduces linear velocity when heading error is large (with correction enabled)
- Clamps velocities to configurable maximums

Configuration parameters:
- lookahead_distance: 0.5m (typical range: 0.1-1.0m)
- goal_tolerance: 0.1m (distance to goal before stopping)
- heading_tolerance: 0.1 rad (unused but can support in future)
- max_linear_velocity: 1.0 m/s
- max_angular_velocity: 1.57 rad/s
- use_heading_correction: true (reduces speed on large heading errors)

Comprehensive test suite: 20+ tests covering:
- Geometric calculations (distance, quaternion conversions)
- Path following logic (empty path, straight/curved/spiral paths)
- Steering calculations (heading errors, velocity limits)
- Edge cases and realistic scenarios
- Control loop integration
- Parameter variations

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
This commit is contained in:
sl-controls 2026-03-03 06:47:45 -05:00
parent 4fd7306d01
commit 1c8430e68a
10 changed files with 785 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
jetson/ros2_ws/src/saltybot_pure_pursuit/config/pure_pursuit_config.yaml Normal file
View File

@ -0,0 +1,22 @@
# Pure Pursuit Path Follower Configuration
pure_pursuit:
ros__parameters:
# Path following parameters
lookahead_distance: 0.5 # Distance to look ahead on the path (meters)
goal_tolerance: 0.1 # Distance tolerance to goal (meters)
heading_tolerance: 0.1 # Heading tolerance in radians (rad)
# Speed parameters
max_linear_velocity: 1.0 # Maximum linear velocity (m/s)
max_angular_velocity: 1.57 # Maximum angular velocity (rad/s)
# Control parameters
linear_velocity_scale: 1.0 # Scale factor for linear velocity
angular_velocity_scale: 1.0 # Scale factor for angular velocity
use_heading_correction: true # Apply heading error correction
# Publishing frequency (Hz)
publish_frequency: 10
# Enable/disable path follower
enable_path_following: true

29
jetson/ros2_ws/src/saltybot_pure_pursuit/launch/pure_pursuit.launch.py Normal file
View File

@ -0,0 +1,29 @@
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution
def generate_launch_description():
config_dir = PathJoinSubstitution(
[FindPackageShare('saltybot_pure_pursuit'), 'config']
)
config_file = PathJoinSubstitution([config_dir, 'pure_pursuit_config.yaml'])
pure_pursuit = Node(
package='saltybot_pure_pursuit',
executable='pure_pursuit_node',
name='pure_pursuit',
output='screen',
parameters=[config_file],
remappings=[
('/odom', '/odom'),
('/path', '/path'),
('/cmd_vel_tracked', '/cmd_vel_tracked'),
],
)
return LaunchDescription([
pure_pursuit,
])

29
jetson/ros2_ws/src/saltybot_pure_pursuit/package.xml Normal file
View File

@ -0,0 +1,29 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>saltybot_pure_pursuit</name>
<version>0.1.0</version>
<description>Pure pursuit path follower for Nav2 autonomous navigation</description>
<maintainer email="sl-controls@saltybot.local">SaltyBot Controls</maintainer>
<license>MIT</license>
<author email="sl-controls@saltybot.local">SaltyBot Controls Team</author>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>ament_cmake_python</buildtool_depend>
<depend>rclpy</depend>
<depend>nav_msgs</depend>
<depend>geometry_msgs</depend>
<depend>std_msgs</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>

0
jetson/ros2_ws/src/saltybot_pure_pursuit/resource/saltybot_pure_pursuit Normal file
View File

0
jetson/ros2_ws/src/saltybot_pure_pursuit/saltybot_pure_pursuit/__init__.py Normal file
View File

269
jetson/ros2_ws/src/saltybot_pure_pursuit/saltybot_pure_pursuit/pure_pursuit_node.py Normal file
View File

@ -0,0 +1,269 @@
#!/usr/bin/env python3
"""Pure pursuit path follower for Nav2 autonomous navigation.
Implements the pure pursuit algorithm for following a path or trajectory.
The algorithm computes steering commands to make the robot follow a path
by targeting a lookahead point on the path.
The pure pursuit algorithm:
1. Finds the closest point on the path to the robot
2. Looks ahead a specified distance along the path
3. Computes a circular arc passing through robot position to lookahead point
4. Publishes velocity commands to follow this arc
"""
import math
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Path, Odometry
from geometry_msgs.msg import Twist, PoseStamped
from std_msgs.msg import Float32
class PurePursuitNode(Node):
"""ROS2 node implementing pure pursuit path follower."""
def __init__(self):
super().__init__('pure_pursuit')
# Parameters
self.declare_parameter('lookahead_distance', 0.5)
self.declare_parameter('goal_tolerance', 0.1)
self.declare_parameter('heading_tolerance', 0.1)
self.declare_parameter('max_linear_velocity', 1.0)
self.declare_parameter('max_angular_velocity', 1.57)
self.declare_parameter('linear_velocity_scale', 1.0)
self.declare_parameter('angular_velocity_scale', 1.0)
self.declare_parameter('use_heading_correction', True)
self.declare_parameter('publish_frequency', 10)
self.declare_parameter('enable_path_following', True)
# Read parameters
self.lookahead_distance = self.get_parameter('lookahead_distance').value
self.goal_tolerance = self.get_parameter('goal_tolerance').value
self.heading_tolerance = self.get_parameter('heading_tolerance').value
self.max_linear_velocity = self.get_parameter('max_linear_velocity').value
self.max_angular_velocity = self.get_parameter('max_angular_velocity').value
self.linear_velocity_scale = self.get_parameter('linear_velocity_scale').value
self.angular_velocity_scale = self.get_parameter('angular_velocity_scale').value
self.use_heading_correction = self.get_parameter('use_heading_correction').value
publish_frequency = self.get_parameter('publish_frequency').value
self.enable_path_following = self.get_parameter('enable_path_following').value
# Current state
self.current_pose = None
self.current_path = None
self.goal_reached = False
# Subscriptions
self.sub_odom = self.create_subscription(
Odometry, '/odom', self._on_odometry, 10
)
self.sub_path = self.create_subscription(
Path, '/path', self._on_path, 10
)
# Publishers
self.pub_cmd_vel = self.create_publisher(Twist, '/cmd_vel_tracked', 10)
self.pub_tracking_error = self.create_publisher(Float32, '/tracking_error', 10)
# Timer for control loop
period = 1.0 / publish_frequency
self.timer = self.create_timer(period, self._control_loop)
self.get_logger().info(
f"Pure pursuit initialized. "
f"Lookahead: {self.lookahead_distance}m, "
f"Goal tolerance: {self.goal_tolerance}m"
)
def _on_odometry(self, msg: Odometry) -> None:
"""Callback for odometry messages."""
self.current_pose = msg.pose.pose
def _on_path(self, msg: Path) -> None:
"""Callback for path messages."""
self.current_path = msg
def _quaternion_to_yaw(self, quat):
"""Convert quaternion to yaw angle."""
siny_cosp = 2 * (quat.w * quat.z + quat.x * quat.y)
cosy_cosp = 1 - 2 * (quat.y * quat.y + quat.z * quat.z)
yaw = math.atan2(siny_cosp, cosy_cosp)
return yaw
def _yaw_to_quaternion(self, yaw):
"""Convert yaw angle to quaternion."""
from geometry_msgs.msg import Quaternion
cy = math.cos(yaw * 0.5)
sy = math.sin(yaw * 0.5)
return Quaternion(x=0.0, y=0.0, z=sy, w=cy)
def _distance(self, p1, p2):
"""Compute Euclidean distance between two points."""
dx = p1.x - p2.x
dy = p1.y - p2.y
return math.sqrt(dx * dx + dy * dy)
def _find_closest_point_on_path(self):
"""Find closest point on path to current robot position."""
if self.current_path is None or len(self.current_path.poses) < 2:
return None, 0, float('inf')
closest_idx = 0
closest_dist = float('inf')
# Find closest waypoint
for i, pose in enumerate(self.current_path.poses):
dist = self._distance(self.current_pose.position, pose.pose.position)
if dist < closest_dist:
closest_dist = dist
closest_idx = i
return closest_idx, closest_dist, closest_idx
def _find_lookahead_point(self):
"""Find lookahead point on path ahead of closest point."""
if self.current_path is None or len(self.current_path.poses) < 2:
return None
closest_idx, _, _ = self._find_closest_point_on_path()
if closest_idx is None:
return None
# Find point at lookahead distance along path
current_dist = 0.0
for i in range(closest_idx, len(self.current_path.poses) - 1):
p1 = self.current_path.poses[i].pose.position
p2 = self.current_path.poses[i + 1].pose.position
segment_dist = self._distance(p1, p2)
current_dist += segment_dist
if current_dist >= self.lookahead_distance:
# Interpolate between p1 and p2
overshoot = current_dist - self.lookahead_distance
if segment_dist > 0:
alpha = 1.0 - (overshoot / segment_dist)
from geometry_msgs.msg import Point
lookahead = Point()
lookahead.x = p1.x + alpha * (p2.x - p1.x)
lookahead.y = p1.y + alpha * (p2.y - p1.y)
lookahead.z = 0.0
return lookahead
# If we get here, return the last point
return self.current_path.poses[-1].pose.position
def _calculate_steering_command(self, lookahead_point):
"""Calculate steering angle using pure pursuit geometry.
Args:
lookahead_point: Target lookahead point on the path
Returns:
Tuple of (linear_velocity, angular_velocity)
"""
if lookahead_point is None or self.current_pose is None:
return 0.0, 0.0
# Vector from robot to lookahead point
dx = lookahead_point.x - self.current_pose.position.x
dy = lookahead_point.y - self.current_pose.position.y
distance_to_lookahead = math.sqrt(dx * dx + dy * dy)
# Check if goal is reached
if distance_to_lookahead < self.goal_tolerance:
self.goal_reached = True
return 0.0, 0.0
# Robot heading
robot_yaw = self._quaternion_to_yaw(self.current_pose.orientation)
# Angle to lookahead point
angle_to_lookahead = math.atan2(dy, dx)
# Heading error
heading_error = angle_to_lookahead - robot_yaw
# Normalize angle to [-pi, pi]
while heading_error > math.pi:
heading_error -= 2 * math.pi
while heading_error < -math.pi:
heading_error += 2 * math.pi
# Pure pursuit curvature: k = 2 * sin(alpha) / Ld
# where alpha is the heading error and Ld is lookahead distance
if self.lookahead_distance > 0:
curvature = (2.0 * math.sin(heading_error)) / self.lookahead_distance
else:
curvature = 0.0
# Linear velocity (reduce when heading error is large)
if self.use_heading_correction:
heading_error_abs = abs(heading_error)
linear_velocity = self.max_linear_velocity * math.cos(heading_error)
linear_velocity = max(0.0, linear_velocity) # Don't go backwards
else:
linear_velocity = self.max_linear_velocity
# Angular velocity from curvature: omega = v * k
angular_velocity = linear_velocity * curvature
# Clamp velocities
linear_velocity = min(linear_velocity, self.max_linear_velocity)
angular_velocity = max(-self.max_angular_velocity,
min(angular_velocity, self.max_angular_velocity))
# Apply scaling
linear_velocity *= self.linear_velocity_scale
angular_velocity *= self.angular_velocity_scale
return linear_velocity, angular_velocity
def _control_loop(self) -> None:
"""Main control loop executed at regular intervals."""
if not self.enable_path_following or self.current_pose is None:
# Publish zero velocity
cmd = Twist()
self.pub_cmd_vel.publish(cmd)
return
# Find lookahead point
lookahead_point = self._find_lookahead_point()
if lookahead_point is None:
# No valid path, publish zero velocity
cmd = Twist()
self.pub_cmd_vel.publish(cmd)
return
# Calculate steering command
linear_vel, angular_vel = self._calculate_steering_command(lookahead_point)
# Publish velocity command
cmd = Twist()
cmd.linear.x = linear_vel
cmd.angular.z = angular_vel
self.pub_cmd_vel.publish(cmd)
# Publish tracking error
if self.current_path and len(self.current_path.poses) > 0:
closest_idx, tracking_error, _ = self._find_closest_point_on_path()
error_msg = Float32(data=tracking_error)
self.pub_tracking_error.publish(error_msg)
def main(args=None):
rclpy.init(args=args)
node = PurePursuitNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

5
jetson/ros2_ws/src/saltybot_pure_pursuit/setup.cfg Normal file
View File

@ -0,0 +1,5 @@
[develop]
script_dir=$base/lib/saltybot_pure_pursuit
[egg_info]
tag_build =
tag_date = 0

34
jetson/ros2_ws/src/saltybot_pure_pursuit/setup.py Normal file
View File

@ -0,0 +1,34 @@
from setuptools import setup, find_packages
package_name = 'saltybot_pure_pursuit'
setup(
name=package_name,
version='0.1.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/saltybot_pure_pursuit']),
('share/' + package_name, ['package.xml']),
('share/' + package_name + '/config', ['config/pure_pursuit_config.yaml']),
('share/' + package_name + '/launch', ['launch/pure_pursuit.launch.py']),
],
install_requires=['setuptools'],
zip_safe=True,
author='SaltyBot Controls',
author_email='sl-controls@saltybot.local',
maintainer='SaltyBot Controls',
maintainer_email='sl-controls@saltybot.local',
keywords=['ROS2', 'pure_pursuit', 'path_following', 'nav2'],
classifiers=[
'Intended Audience :: Developers',
'License :: OSI Approved :: MIT License',
'Programming Language :: Python :: 3',
'Topic :: Software Development',
],
entry_points={
'console_scripts': [
'pure_pursuit_node=saltybot_pure_pursuit.pure_pursuit_node:main',
],
},
)

0
jetson/ros2_ws/src/saltybot_pure_pursuit/test/__init__.py Normal file
View File

397
jetson/ros2_ws/src/saltybot_pure_pursuit/test/test_pure_pursuit.py Normal file
View File

@ -0,0 +1,397 @@
"""Unit tests for pure pursuit path follower node."""
import pytest
import math
from nav_msgs.msg import Path, Odometry
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion, Twist
from std_msgs.msg import Header
import rclpy
from saltybot_pure_pursuit.pure_pursuit_node import PurePursuitNode
@pytest.fixture
def rclpy_fixture():
"""Initialize and cleanup rclpy."""
rclpy.init()
yield
rclpy.shutdown()
@pytest.fixture
def node(rclpy_fixture):
"""Create a pure pursuit node instance."""
node = PurePursuitNode()
yield node
node.destroy_node()
class TestPurePursuitGeometry:
"""Test suite for pure pursuit geometric calculations."""
def test_node_initialization(self, node):
"""Test that node initializes with correct defaults."""
assert node.lookahead_distance == 0.5
assert node.goal_tolerance == 0.1
assert node.max_linear_velocity == 1.0
assert node.enable_path_following is True
def test_distance_calculation(self, node):
"""Test Euclidean distance calculation."""
p1 = Point(x=0.0, y=0.0, z=0.0)
p2 = Point(x=3.0, y=4.0, z=0.0)
dist = node._distance(p1, p2)
assert abs(dist - 5.0) < 0.01
def test_distance_same_point(self, node):
"""Test distance between same point is zero."""
p = Point(x=1.0, y=2.0, z=0.0)
dist = node._distance(p, p)
assert abs(dist) < 0.001
def test_quaternion_to_yaw_north(self, node):
"""Test quaternion to yaw conversion for north heading."""
quat = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
yaw = node._quaternion_to_yaw(quat)
assert abs(yaw) < 0.01
def test_quaternion_to_yaw_east(self, node):
"""Test quaternion to yaw conversion for east heading."""
# 90 degree rotation around z-axis
quat = Quaternion(x=0.0, y=0.0, z=0.7071, w=0.7071)
yaw = node._quaternion_to_yaw(quat)
assert abs(yaw - math.pi / 2) < 0.01
def test_yaw_to_quaternion_identity(self, node):
"""Test yaw to quaternion conversion."""
quat = node._yaw_to_quaternion(0.0)
assert abs(quat.w - 1.0) < 0.01
assert abs(quat.z) < 0.01
class TestPathFollowing:
"""Test suite for path following logic."""
def test_empty_path(self, node):
"""Test handling of empty path."""
node.current_pose = Pose(position=Point(x=0.0, y=0.0, z=0.0))
node.current_path = Path()
lookahead = node._find_lookahead_point()
assert lookahead is None
def test_single_point_path(self, node):
"""Test handling of single-point path."""
node.current_pose = Pose(position=Point(x=0.0, y=0.0, z=0.0))
path = Path()
path.poses = [PoseStamped(pose=Pose(position=Point(x=1.0, y=0.0, z=0.0)))]
node.current_path = path
lookahead = node._find_lookahead_point()
assert lookahead is None or lookahead.x == 1.0
def test_straight_line_path(self, node):
"""Test path following on straight line."""
node.current_pose = Pose(
position=Point(x=0.0, y=0.0, z=0.0),
orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
)
# Create straight path along x-axis
path = Path()
for i in range(5):
pose = PoseStamped(pose=Pose(
position=Point(x=float(i), y=0.0, z=0.0)
))
path.poses.append(pose)
node.current_path = path
# Robot heading east towards path
lin_vel, ang_vel = node._calculate_steering_command(path.poses[1].pose.position)
assert lin_vel >= 0
assert abs(ang_vel) < 0.5 # Should have small steering error
def test_curved_path(self, node):
"""Test path following on curved path."""
node.current_pose = Pose(
position=Point(x=0.0, y=0.0, z=0.0),
orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
)
# Create circular arc path
path = Path()
for i in range(9):
angle = (i / 8.0) * (math.pi / 2)
x = math.sin(angle)
y = 1.0 - math.cos(angle)
pose = PoseStamped(pose=Pose(position=Point(x=x, y=y, z=0.0)))
path.poses.append(pose)
node.current_path = path
lookahead = node._find_lookahead_point()
assert lookahead is not None
def test_goal_reached(self, node):
"""Test goal reached detection."""
goal_pos = Point(x=1.0, y=0.0, z=0.0)
node.current_pose = Pose(
position=Point(x=1.05, y=0.0, z=0.0),
orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
)
lin_vel, ang_vel = node._calculate_steering_command(goal_pos)
# With goal_tolerance=0.1, we should be at goal
assert abs(lin_vel) < 0.01
class TestSteeringCalculation:
"""Test suite for steering command calculations."""
def test_zero_heading_error(self, node):
"""Test steering when robot already aligned with target."""
node.current_pose = Pose(
position=Point(x=0.0, y=0.0, z=0.0),
orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) # Facing east
)
# Target directly ahead
target = Point(x=1.0, y=0.0, z=0.0)
lin_vel, ang_vel = node._calculate_steering_command(target)
assert lin_vel > 0
assert abs(ang_vel) < 0.1 # Minimal steering
def test_90_degree_heading_error(self, node):
"""Test steering with 90 degree heading error."""
node.current_pose = Pose(
position=Point(x=0.0, y=0.0, z=0.0),
orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) # Facing east
)
# Target north
target = Point(x=0.0, y=1.0, z=0.0)
lin_vel, ang_vel = node._calculate_steering_command(target)
assert lin_vel >= 0
assert ang_vel != 0 # Should have significant steering
def test_velocity_limits(self, node):
"""Test that velocities are within limits."""
node.current_pose = Pose(
position=Point(x=0.0, y=0.0, z=0.0),
orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
)
target = Point(x=100.0, y=100.0, z=0.0)
lin_vel, ang_vel = node._calculate_steering_command(target)
assert lin_vel <= node.max_linear_velocity
assert abs(ang_vel) <= node.max_angular_velocity
def test_heading_correction_enabled(self, node):
"""Test heading correction when enabled."""
node.use_heading_correction = True
node.current_pose = Pose(
position=Point(x=0.0, y=0.0, z=0.0),
orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
)
# Large heading error
target = Point(x=0.0, y=10.0, z=0.0) # Due north
lin_vel, ang_vel = node._calculate_steering_command(target)
# Linear velocity should be reduced due to heading error
assert lin_vel < node.max_linear_velocity
def test_heading_correction_disabled(self, node):
"""Test that heading correction can be disabled."""
node.use_heading_correction = False
node.current_pose = Pose(
position=Point(x=0.0, y=0.0, z=0.0),
orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
)
target = Point(x=0.0, y=10.0, z=0.0)
lin_vel, ang_vel = node._calculate_steering_command(target)
# Linear velocity should be full
assert abs(lin_vel - node.max_linear_velocity) < 0.01
def test_negative_lookahead_distance(self, node):
"""Test behavior with invalid lookahead distance."""
node.lookahead_distance = 0.0
node.current_pose = Pose(
position=Point(x=0.0, y=0.0, z=0.0),
orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
)
target = Point(x=1.0, y=1.0, z=0.0)
lin_vel, ang_vel = node._calculate_steering_command(target)
# Should not crash, handle gracefully
assert isinstance(lin_vel, float)
assert isinstance(ang_vel, float)
class TestPurePursuitScenarios:
"""Integration-style tests for realistic scenarios."""
def test_scenario_follow_straight_path(self, node):
"""Scenario: Follow a straight horizontal path."""
node.current_pose = Pose(
position=Point(x=0.0, y=0.0, z=0.0),
orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
)
# Horizontal path
path = Path()
for i in range(10):
pose = PoseStamped(pose=Pose(position=Point(x=float(i), y=0.0, z=0.0)))
path.poses.append(pose)
node.current_path = path
lin_vel, ang_vel = node._calculate_steering_command(path.poses[1].pose.position)
assert lin_vel > 0
assert abs(ang_vel) < 0.5
def test_scenario_s_shaped_path(self, node):
"""Scenario: Follow an S-shaped path."""
node.current_pose = Pose(
position=Point(x=0.0, y=0.0, z=0.0),
orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
)
# S-shaped path
path = Path()
for i in range(20):
x = float(i) * 0.5
y = 2.0 * math.sin(x)
pose = PoseStamped(pose=Pose(position=Point(x=x, y=y, z=0.0)))
path.poses.append(pose)
node.current_path = path
lookahead = node._find_lookahead_point()
assert lookahead is not None
def test_scenario_spiral_path(self, node):
"""Scenario: Follow a spiral path."""
node.current_pose = Pose(
position=Point(x=0.0, y=0.0, z=0.0),
orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
)
# Spiral path
path = Path()
for i in range(30):
angle = (i / 30.0) * (4 * math.pi)
radius = 0.5 + (i / 30.0) * 2.0
x = radius * math.cos(angle)
y = radius * math.sin(angle)
pose = PoseStamped(pose=Pose(position=Point(x=x, y=y, z=0.0)))
path.poses.append(pose)
node.current_path = path
lin_vel, ang_vel = node._calculate_steering_command(path.poses[5].pose.position)
assert isinstance(lin_vel, float)
assert isinstance(ang_vel, float)
def test_scenario_control_loop(self, node):
"""Scenario: Control loop with valid path and odometry."""
node.enable_path_following = True
node.current_pose = Pose(
position=Point(x=0.0, y=0.0, z=0.0),
orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
)
# Create a simple path
path = Path()
for i in range(5):
pose = PoseStamped(pose=Pose(position=Point(x=float(i), y=0.0, z=0.0)))
path.poses.append(pose)
node.current_path = path
# Run control loop
node._control_loop()
# Should complete without error
def test_scenario_disabled_path_following(self, node):
"""Scenario: Path following disabled."""
node.enable_path_following = False
node.current_pose = Pose(position=Point(x=0.0, y=0.0, z=0.0))
# Create path
path = Path()
path.poses = [PoseStamped(pose=Pose(position=Point(x=1.0, y=0.0, z=0.0)))]
node.current_path = path
# Run control loop
node._control_loop()
# Should publish zero velocity
def test_scenario_no_odometry(self, node):
"""Scenario: Control loop when odometry not received."""
node.current_pose = None
path = Path()
path.poses = [PoseStamped(pose=Pose(position=Point(x=1.0, y=0.0, z=0.0)))]
node.current_path = path
# Run control loop
node._control_loop()
# Should handle gracefully
def test_scenario_velocity_scaling(self, node):
"""Scenario: Velocity scaling parameters."""
node.linear_velocity_scale = 0.5
node.angular_velocity_scale = 0.5
node.current_pose = Pose(
position=Point(x=0.0, y=0.0, z=0.0),
orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
)
target = Point(x=1.0, y=0.0, z=0.0)
lin_vel, ang_vel = node._calculate_steering_command(target)
# Scaled velocities should be smaller
assert lin_vel <= node.max_linear_velocity * 0.5 + 0.01
def test_scenario_large_lookahead_distance(self, node):
"""Scenario: Large lookahead distance."""
node.lookahead_distance = 5.0
node.current_pose = Pose(
position=Point(x=0.0, y=0.0, z=0.0),
orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
)
# Path
path = Path()
for i in range(10):
pose = PoseStamped(pose=Pose(position=Point(x=float(i), y=0.0, z=0.0)))
path.poses.append(pose)
node.current_path = path
lookahead = node._find_lookahead_point()
assert lookahead is not None
def test_scenario_small_lookahead_distance(self, node):
"""Scenario: Small lookahead distance."""
node.lookahead_distance = 0.05
node.current_pose = Pose(
position=Point(x=0.0, y=0.0, z=0.0),
orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
)
# Path
path = Path()
for i in range(10):
pose = PoseStamped(pose=Pose(position=Point(x=float(i), y=0.0, z=0.0)))
path.poses.append(pose)
node.current_path = path
lookahead = node._find_lookahead_point()
assert lookahead is not None