Path Planning and Vehicle Control

This repository contains the modules responsible for path planning and vehicle control. These components receive perception and mapping information and generate motion commands that drive the vehicle along the intended trajectory.

The planning subsystem handles:

Path generation based on the mapped environment Trajectory interpolation and smoothing Waypoint sequencing and motion constraints

The control subsystem is responsible for:

Tracking the reference trajectory Computing steering, throttle, and braking commands Ensuring stable and consistent motion execution

System Data Flow (Conceptual)

Perception → Mapping → Path Planning → Vehicle Control → Actuation

Both subsystems interface through ROS 2 topics using consistent timestamping and coordinate frames, enabling integration with upstream modules such as mapping and perception, and downstream modules for actuation.

---

ROS Interfaces

Topics

Module	Direction	Topic	Message Type	Notes
Path Planning	Sub	/odom	sensor_msgs/Odom	Odometry input
Path Planning	Sub	/mission/go_signal	std_msgs/Bool	Trigger for planning
Path Planning	Sub	/track	nav_msgs/Track	Track input
Path Planning	Pub	/path	fsds_msgs/Path	Reference path
Path Planning	Pub	/path_concatenated	nav_msgs/Path	Reference path (Control input)
Path Planning	Pub	/track_pointcloud	nav_msgs/PointCloud2	Track for debugging

Topics and messages used in Path Planning package.

---

Module	Direction	Topic	Message Type	Notes
Vehicle Control	Sub	/path_concatenated	fsds_msgs/Path	Trajectoty input
Vehicle Control	Sub	/odom	sensor_msgs/Odom	Odometry input
Vehicle Control	Pub	/control	fsds_msgs/ControlCommand	Control output
Vehicle Control	Pub	/speed	std_msgs/Float32	Reference speed command
Vehicle Control	Pub	/erro_ant	std_msgs/Float32	Previous control error
Vehicle Control	Pub	/eh	std_msgs/Float32	Heading error
Vehicle Control	Pub	/ey	std_msgs/Float32	Lateral position error
Vehicle Control	Pub	/reference_path	fsds_msgs/Path	Reference path for debugging

Topics and messages used in Vehicle Control package.

---

Coordinate Frames

Common frames in use:

• map – global SLAM / mapping frame
• /fsds/map – fsds frame
• base_link – vehicle base frame (control reference)

Frame transforms are managed through TF2.

---

Dependencies

Core dependencies (minimum):

• ROS 2 Humble (or newer)
• rclcpp / rclpy
• nav_msgs, geometry_msgs, sensor_msgs, lifecycle_msgs
• tf2 + tf2_ros
• colcon (build system)

---

Commands for compiling packages

For compiling both, use:

    colcon build 

For compiling individualy, use:

    colcon build --packages-select path_planning

    colcon build --packages-select control

Running & Launching

Path Planning launchs:

    ros2 run path_planning path_node.py

    ros2 launch path_planning path_planning.launch.py

Control launchs:

    ros2 run control control_node.py

    ros2 launch control control.launch.py