cyy/yiliao2026
1
0
Fork 0
forked from zbw/yiliao2026
Code Pull Requests Activity

Please enter the commit message for your changes. Lines starting

Browse Source 
with '#' will be ignored, and an empty message aborts the commit.

On branch master
Your branch is ahead of 'origin/master' by 2 commits.
  (use "git push" to publish your local commits)

Changes to be committed:
	modified:   .gitignore
	modified:   README.md
	new file:   bashes/auto-wifi-connect.service
	new file:   bashes/auto-wifi-connect.sh
	deleted:    keyboard_control.py
	new file:   my_model/image.png
	new file:   path_follower_demo.py
	new file:   scripts/PIDtracking.py
	new file:   scripts/__pycache__/publish_sine_path.cpython-310.pyc
	new file:   scripts/publish_sine_path.py
	modified:   src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml
	modified:   src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak
	new file:   src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak2
	modified:   src/origincar_base/config/ekf.yaml
	new file:   src/origincar_base/config/ekf.yaml.bak
	modified:   src/origincar_base/launch/base_serial.launch.py
	new file:   src/origincar_base/launch/base_serial.launch.py.bak
	modified:   src/origincar_base/launch/origincar_bringup.launch.py
	new file:   src/past_control/CMakeLists.txt
	new file:   src/past_control/config/past_control.yaml
	new file:   src/past_control/include/past_control/tools.h
	new file:   src/past_control/launch/past_control.launch.py
	new file:   src/past_control/msg/Obstacle.msg
	new file:   src/past_control/msg/ObstacleArray.msg
	new file:   src/past_control/package.xml
	new file:   src/past_control/src/lane_follower_node.cpp
	new file:   src/past_control/src/obstacle_detector_node.cpp
	new file:   src/past_control/src/racing_orchestrator.cpp
	new file:   src/planner/CMakeLists.txt
	new file:   src/planner/config/planner.yaml
	new file:   src/planner/launch/planner.launch.py
	new file:   src/planner/package.xml
	new file:   src/planner/src/planner_version.cpp
	modified:   src/qr_detection/src/qr_dete_depth.cpp
	new file:   src/racing_control/CMakeLists.txt
	new file:   src/racing_control/include/racing_control/racing_control.hpp
	new file:   src/racing_control/package.xml
	new file:   src/racing_control/src/racing_control.cpp
	modified:   src/vlm_detect/setup.py
	new file:   src/vlm_detect/vlm_detect/__pycache__/__init__.cpython-310.pyc
	new file:   src/vlm_detect/vlm_detect/__pycache__/tts_node.cpython-310.pyc
	new file:   src/vlm_detect/vlm_detect/test_publisher.py
	new file:   src/vlm_detect/vlm_detect/tts_node.py
	modified:   src/vlm_detect/vlm_detect/vlm_node.py
	new file:   tools/measure_turning_radius.py
	new file:   tools/set_volume.py
	new file:   tools/udp_to_cmdvel.py
	new file:   tools/windows_keyboard_control.py
	new file:   vlm_server.py
	new file:   "\350\260\203\350\257\225\350\256\260\345\275\225.Assets/1.png"
	renamed:    "\350\260\203\350\257\225\350\256\260\345\275\225.log" -> "\350\260\203\350\257\225\350\256\260\345\275\225.md"
This commit is contained in:
Orange
2026-06-22 17:16:45 +08:00
parent a9bfeef59b
commit 779b32362a
51 changed files with 3974 additions and 398 deletions
Download Patch File Download Diff File Expand all files Collapse all files

85
src/past_control/CMakeLists.txt Normal file
View File

@@ -0,0 +1,85 @@
cmake_minimum_required(VERSION 3.5)
project(past_control)
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(std_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(nav2_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(origincar_msg REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(builtin_interfaces REQUIRED)
# generate custom messages
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Obstacle.msg"
"msg/ObstacleArray.msg"
DEPENDENCIES std_msgs
ADD_LINTER_TESTS
)
include_directories(include)
# lane_follower_node
add_executable(lane_follower_node
src/lane_follower_node.cpp
)
ament_target_dependencies(lane_follower_node
rclcpp std_msgs geometry_msgs
)
# obstacle_detector_node
add_executable(obstacle_detector_node
src/obstacle_detector_node.cpp
)
ament_target_dependencies(obstacle_detector_node
rclcpp std_msgs geometry_msgs nav_msgs tf2 tf2_ros visualization_msgs
)
rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} rosidl_typesupport_cpp)
target_link_libraries(obstacle_detector_node ${cpp_typesupport_target})
# racing_orchestrator — subscribes to /plan (Nav2 planner_server) + PID following
add_executable(racing_orchestrator
src/racing_orchestrator.cpp
)
ament_target_dependencies(racing_orchestrator
rclcpp rclcpp_action std_msgs nav_msgs nav2_msgs geometry_msgs tf2 tf2_ros origincar_msg visualization_msgs
)
install(TARGETS
lane_follower_node
obstacle_detector_node
racing_orchestrator
DESTINATION lib/${PROJECT_NAME}
)
install(
DIRECTORY launch config
DESTINATION share/${PROJECT_NAME}
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

51
src/past_control/config/past_control.yaml Normal file
View File

@@ -0,0 +1,51 @@
# past_control — Racing control stack configuration
# No a_star_planner_node: global planning handled by Nav2 planner_server via /plan
# ============================================================
# lane_follower_node — Visual lane centering
# ============================================================
lane_follower_node:
ros__parameters:
follow_linear_speed: 0.3
follow_angular_ratio: 1.0
image_width: 640.0
# ============================================================
# obstacle_detector_node — DNN-based obstacle perception
# ============================================================
obstacle_detector_node:
ros__parameters:
confidence_threshold: 0.5
processing_latency: 0.1
camera_hfov: 1.0472 # 60 degrees
camera_height: 0.3
camera_pitch: 0.0
max_detection_range: 3.0
publish_rate: 10.0
# ============================================================
# racing_orchestrator — Task FSM + PID path following (Nav2 /plan) + arbitration
# ============================================================
racing_orchestrator:
ros__parameters:
# PID path following (path source: Nav2 planner_server /plan)
follow_linear_speed: 0.4
guide_step: 5
arrive_square: 0.25 # distance^2 threshold (0.5m)
angular_kp: 10.0
angular_ki: 0.0
angular_kd: 0.1
angular_integral_max: 1.0
angular_output_max: 2.0
angular_max_err: 3.14
# Obstacle avoidance fallback
avoid_linear_speed: 0.2
avoid_angular_z: 0.8
# QR scan mode
qr_scan_speed: 0.15
qr_scan_duration: 3.0
# Control loop rate
control_rate: 20.0

87
src/past_control/include/past_control/tools.h Normal file
View File

@@ -0,0 +1,87 @@
#ifndef PAST_CONTROL__TOOLS_H
#define PAST_CONTROL__TOOLS_H
#include <algorithm>
#include <chrono>
namespace past_control
{
inline double limit(double val, double min_val, double max_val)
{
return std::min(std::max(val, min_val), max_val);
}
class PID
{
public:
PID()
: kp_(0.0), ki_(0.0), kd_(0.0),
integral_max_(0.0), output_max_(0.0),
integral_(0.0), prev_error_(0.0), first_run_(true),
max_err_(0.0)
{
}
void init(double kp, double ki, double kd,
double integral_max, double output_max, double max_err = 0.0)
{
kp_ = kp;
ki_ = ki;
kd_ = kd;
integral_max_ = integral_max;
output_max_ = output_max;
max_err_ = max_err;
reset();
}
void reset()
{
integral_ = 0.0;
prev_error_ = 0.0;
first_run_ = true;
}
double update(double error, double dt = 0.02)
{
// Clamp error if max_err_ > 0
if (max_err_ > 0.0) {
error = limit(error, -max_err_, max_err_);
}
// Proportional
double p_out = kp_ * error;
// Integral (with clamping)
integral_ += error * dt;
integral_ = limit(integral_, -integral_max_, integral_max_);
double i_out = ki_ * integral_;
// Derivative (skip on first call)
double d_out = 0.0;
if (!first_run_) {
double derivative = (error - prev_error_) / dt;
d_out = kd_ * derivative;
}
first_run_ = false;
prev_error_ = error;
// Output clamping
double output = p_out + i_out + d_out;
output = limit(output, -output_max_, output_max_);
return output;
}
private:
double kp_, ki_, kd_;
double integral_max_, output_max_;
double integral_;
double prev_error_;
bool first_run_;
double max_err_; // initialized to 0 in constructor
};
} // namespace past_control
#endif // PAST_CONTROL__TOOLS_H

52
src/past_control/launch/past_control.launch.py Normal file
View File

@@ -0,0 +1,52 @@
"""
Launch file for past_control — racing control stack.
Launches 3 nodes (a_star_planner removed — Nav2 planner_server handles global planning):
1. lane_follower_node — visual lane centering
2. obstacle_detector_node — DNN obstacle perception
3. racing_orchestrator — task FSM + PID following (/plan from Nav2) + cmd_vel arbitration
Usage:
ros2 launch past_control past_control.launch.py
"""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
pkg_dir = get_package_share_directory("past_control")
config_path = os.path.join(pkg_dir, "config", "past_control.yaml")
lane_follower = Node(
package="past_control",
executable="lane_follower_node",
name="lane_follower_node",
output="screen",
parameters=[config_path],
)
obstacle_detector = Node(
package="past_control",
executable="obstacle_detector_node",
name="obstacle_detector_node",
output="screen",
parameters=[config_path],
)
racing_orchestrator = Node(
package="past_control",
executable="racing_orchestrator",
name="racing_orchestrator",
output="screen",
parameters=[config_path],
)
return LaunchDescription([
lane_follower,
obstacle_detector,
racing_orchestrator,
])

5
src/past_control/msg/Obstacle.msg Normal file
View File

@@ -0,0 +1,5 @@
std_msgs/Header header
float64 x
float64 y
float64 radius
string type

1
src/past_control/msg/ObstacleArray.msg Normal file
View File

@@ -0,0 +1 @@
Obstacle[] obstacles

36
src/past_control/package.xml Normal file
View File

@@ -0,0 +1,36 @@
<?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>past_control</name>
<version>0.0.0</version>
<description>Racing control stack: lane follower, obstacle detector, and racing orchestrator (Nav2 /plan subscriber + PID following)</description>
<maintainer email="2314753575@qq.com">sunrise</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>std_msgs</depend>
<depend>nav_msgs</depend>
<depend>nav2_msgs</depend>
<depend>geometry_msgs</depend>
<depend>sensor_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>visualization_msgs</depend>
<depend>origincar_msg</depend>
<depend>builtin_interfaces</depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

81
src/past_control/src/lane_follower_node.cpp Normal file
View File

@@ -0,0 +1,81 @@
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
class LaneFollowerNode : public rclcpp::Node
{
public:
LaneFollowerNode()
: Node("lane_follower_node")
{
this->declare_parameter("follow_linear_speed", 0.3);
this->declare_parameter("follow_angular_ratio", 1.0);
this->declare_parameter("image_width", 640.0);
follow_linear_speed_ = this->get_parameter("follow_linear_speed").as_double();
follow_angular_ratio_ = this->get_parameter("follow_angular_ratio").as_double();
image_width_ = this->get_parameter("image_width").as_double();
// Subscriber: racing track center detection
// Expects Float32MultiArray with [center_x] in image coordinates,
// or a custom format. We assume center_x relative to image center.
track_sub_ = this->create_subscription<std_msgs::msg::Float32MultiArray>(
"racing_track_center_detection", 10,
std::bind(&LaneFollowerNode::trackCallback, this, std::placeholders::_1));
// Publisher: lane cmd_vel (NOT /cmd_vel directly)
cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
"/lane_cmd_vel", 10);
RCLCPP_INFO(this->get_logger(),
"LaneFollowerNode started. linear=%.2f, angular_ratio=%.2f",
follow_linear_speed_, follow_angular_ratio_);
}
private:
void trackCallback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
{
if (msg->data.empty()) {
// No track detected — stop
geometry_msgs::msg::Twist cmd;
cmd.linear.x = 0.0;
cmd.angular.z = 0.0;
cmd_vel_pub_->publish(cmd);
return;
}
// center_x from detection: pixel offset from image center
// range: [-image_width/2, image_width/2]
double center_offset = msg->data[0]; // in pixels
// Normalize to [-1, 1]
double normalized_error = center_offset / (image_width_ / 2.0);
normalized_error = std::max(-1.0, std::min(1.0, normalized_error));
// Angular velocity: steer toward center line
// Positive error = line is to the right → positive angular to turn right
double angular_z = follow_angular_ratio_ * normalized_error;
geometry_msgs::msg::Twist cmd;
cmd.linear.x = follow_linear_speed_;
cmd.angular.z = angular_z;
cmd_vel_pub_->publish(cmd);
}
rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr track_sub_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
double follow_linear_speed_;
double follow_angular_ratio_;
double image_width_;
};
int main(int argc, char* argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<LaneFollowerNode>());
rclcpp::shutdown();
return 0;
}

222
src/past_control/src/obstacle_detector_node.cpp Normal file
View File

@@ -0,0 +1,222 @@
#include <memory>
#include <mutex>
#include <cmath>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "visualization_msgs/msg/marker_array.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "std_msgs/msg/header.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "past_control/msg/obstacle_array.hpp"
#include "past_control/msg/obstacle.hpp"
// DNN detection format (per the existing hobot_dnn convention)
// Typically a custom array of detections: [class_id, x1, y1, x2, y2, confidence, ...]
// We'll use Float32MultiArray for input
class ObstacleDetectorNode : public rclcpp::Node
{
public:
ObstacleDetectorNode()
: Node("obstacle_detector_node"), has_odom_(false)
{
this->declare_parameter("confidence_threshold", 0.5);
this->declare_parameter("processing_latency", 0.1); // seconds
this->declare_parameter("camera_hfov", 1.0472); // 60 degrees
this->declare_parameter("camera_height", 0.3); // meters above ground
this->declare_parameter("camera_pitch", 0.0); // radians
this->declare_parameter("max_detection_range", 3.0); // meters
this->declare_parameter("publish_rate", 10.0);
confidence_threshold_ = this->get_parameter("confidence_threshold").as_double();
processing_latency_ = this->get_parameter("processing_latency").as_double();
camera_hfov_ = this->get_parameter("camera_hfov").as_double();
camera_height_ = this->get_parameter("camera_height").as_double();
camera_pitch_ = this->get_parameter("camera_pitch").as_double();
max_detection_range_ = this->get_parameter("max_detection_range").as_double();
// Subscribers
dnn_sub_ = this->create_subscription<std_msgs::msg::Float32MultiArray>(
"hobot_dnn_detection", 10,
std::bind(&ObstacleDetectorNode::dnnCallback, this, std::placeholders::_1));
odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
"/odom", 10,
std::bind(&ObstacleDetectorNode::odomCallback, this, std::placeholders::_1));
// Publishers
obstacles_pub_ = this->create_publisher<past_control::msg::ObstacleArray>(
"/obstacles", 10);
marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
"/obstacle_markers", 10);
double rate = this->get_parameter("publish_rate").as_double();
int period_ms = static_cast<int>(1000.0 / rate);
timer_ = this->create_wall_timer(
std::chrono::milliseconds(period_ms),
std::bind(&ObstacleDetectorNode::publishLoop, this));
RCLCPP_INFO(this->get_logger(), "ObstacleDetectorNode started");
}
private:
void dnnCallback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
// Parse DNN detections
// Expected format: interleaved [class_id, x_center, y_center, width, height, conf]
// Normalized coordinates (0-1) within image frame
const int fields_per_detection = 6;
int num_detections = msg->data.size() / fields_per_detection;
latest_detections_.clear();
for (int i = 0; i < num_detections; ++i) {
int base = i * fields_per_detection;
Detection det;
det.class_id = static_cast<int>(msg->data[base + 0]);
det.x_center = msg->data[base + 1]; // normalized [0,1]
det.y_center = msg->data[base + 2]; // normalized [0,1]
det.width = msg->data[base + 3]; // normalized
det.height = msg->data[base + 4]; // normalized
det.confidence = msg->data[base + 5];
if (det.confidence >= confidence_threshold_) {
latest_detections_.push_back(det);
}
}
has_detections_ = true;
}
void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
cur_x_ = msg->pose.pose.position.x;
cur_y_ = msg->pose.pose.position.y;
// Extract yaw from quaternion
double qx = msg->pose.pose.orientation.x;
double qy = msg->pose.pose.orientation.y;
double qz = msg->pose.pose.orientation.z;
double qw = msg->pose.pose.orientation.w;
cur_yaw_ = std::atan2(2.0*(qw*qz + qx*qy), 1.0 - 2.0*(qy*qy + qz*qz));
has_odom_ = true;
}
void publishLoop()
{
std::lock_guard<std::mutex> lock(mutex_);
past_control::msg::ObstacleArray obs_array;
visualization_msgs::msg::MarkerArray marker_array;
if (!has_odom_ || !has_detections_) {
// Publish empty
obstacles_pub_->publish(obs_array);
marker_pub_->publish(marker_array);
return;
}
int marker_id = 0;
for (const auto& det : latest_detections_) {
// Convert pixel coords to world coords using simple pinhole model
// x_center normalized [0,1] → angle offset from camera optical axis
double pixel_offset = det.x_center - 0.5; // [-0.5, 0.5]
double angle_offset = pixel_offset * camera_hfov_; // radians
double obstacle_angle = cur_yaw_ + angle_offset;
// Estimate distance from bounding box height
// Larger bbox = closer object (simple inverse relationship)
double est_distance = (1.0 - det.height) * max_detection_range_ + 0.5;
// World coordinates of obstacle
double obs_x = cur_x_ + est_distance * std::cos(obstacle_angle);
double obs_y = cur_y_ + est_distance * std::sin(obstacle_angle);
double obs_radius = 0.15; // default obstacle radius
// Add to obstacle array
past_control::msg::Obstacle obs;
obs.header.stamp = this->now();
obs.header.frame_id = "odom";
obs.x = obs_x;
obs.y = obs_y;
obs.radius = obs_radius;
obs.type = "circle";
obs_array.obstacles.push_back(obs);
// Add marker
visualization_msgs::msg::Marker marker;
marker.header.stamp = this->now();
marker.header.frame_id = "odom";
marker.ns = "obstacles";
marker.id = marker_id++;
marker.type = visualization_msgs::msg::Marker::CYLINDER;
marker.action = visualization_msgs::msg::Marker::ADD;
marker.pose.position.x = obs_x;
marker.pose.position.y = obs_y;
marker.pose.position.z = 0.0;
marker.pose.orientation.w = 1.0;
marker.scale.x = obs_radius * 2;
marker.scale.y = obs_radius * 2;
marker.scale.z = 0.3;
marker.color.r = 1.0f;
marker.color.g = 0.0f;
marker.color.b = 0.0f;
marker.color.a = 0.8f;
marker.lifetime = rclcpp::Duration::from_seconds(0.5);
marker_array.markers.push_back(marker);
}
obstacles_pub_->publish(obs_array);
marker_pub_->publish(marker_array);
}
struct Detection
{
int class_id;
double x_center, y_center;
double width, height;
double confidence;
};
// Subscribers
rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr dnn_sub_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
// Publishers
rclcpp::Publisher<past_control::msg::ObstacleArray>::SharedPtr obstacles_pub_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
// Timer
rclcpp::TimerBase::SharedPtr timer_;
// State
std::vector<Detection> latest_detections_;
double cur_x_, cur_y_, cur_yaw_;
bool has_odom_, has_detections_;
std::mutex mutex_;
// Parameters
double confidence_threshold_;
double processing_latency_;
double camera_hfov_;
double camera_height_;
double camera_pitch_;
double max_detection_range_;
};
int main(int argc, char* argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<ObstacleDetectorNode>());
rclcpp::shutdown();
return 0;
}

448
src/past_control/src/racing_orchestrator.cpp Normal file
View File

@@ -0,0 +1,448 @@
#include <chrono>
#include <memory>
#include <mutex>
#include <cmath>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_msgs/action/compute_path_to_pose.hpp"
#include "origincar_msg/msg/sign.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "past_control/tools.h"
using namespace std::chrono_literals;
// Task states
enum class TaskState
{
IDLE = 0,
GOING = 1,
RETURN = 2,
QR_SCAN = 3,
RESET = 4
};
class RacingOrchestrator : public rclcpp::Node
{
public:
using ComputePathToPose = nav2_msgs::action::ComputePathToPose;
using GoalHandleComputePathToPose = rclcpp_action::ClientGoalHandle<ComputePathToPose>;
RacingOrchestrator()
: Node("racing_orchestrator"),
state_(TaskState::IDLE),
has_odom_(false), has_goal_(false),
has_path_(false), got_lane_cmd_(false),
is_back_(false), activate_avoid_(false),
waypoint_index_(0), guide_step_(5),
follow_linear_speed_(0.4),
sub_target_(true),
cmd_vel_linear_x_(0.0), cmd_vel_angular_z_(0.0),
cur_x_(0.0), cur_y_(0.0), cur_yaw_(0.0)
{
// Parameters
// PID path following
this->declare_parameter("follow_linear_speed", 0.4);
this->declare_parameter("guide_step", 5);
this->declare_parameter("arrive_square", 0.25);
this->declare_parameter("angular_kp", 1.5);
this->declare_parameter("angular_ki", 0.0);
this->declare_parameter("angular_kd", 0.1);
this->declare_parameter("angular_integral_max", 1.0);
this->declare_parameter("angular_output_max", 2.0);
this->declare_parameter("angular_max_err", 3.14);
// Obstacle avoidance
this->declare_parameter("avoid_linear_speed", 0.2);
this->declare_parameter("avoid_angular_z", 0.8);
// QR scan params
this->declare_parameter("qr_scan_speed", 0.15);
this->declare_parameter("qr_scan_duration", 3.0);
// Control rate
this->declare_parameter("control_rate", 20.0);
// Load params
follow_linear_speed_ = this->get_parameter("follow_linear_speed").as_double();
guide_step_ = this->get_parameter("guide_step").as_int();
arrive_square_ = this->get_parameter("arrive_square").as_double();
avoid_linear_speed_ = this->get_parameter("avoid_linear_speed").as_double();
avoid_angular_z_ = this->get_parameter("avoid_angular_z").as_double();
qr_scan_speed_ = this->get_parameter("qr_scan_speed").as_double();
qr_scan_duration_ = this->get_parameter("qr_scan_duration").as_double();
// Init PID
double kp = this->get_parameter("angular_kp").as_double();
double ki = this->get_parameter("angular_ki").as_double();
double kd = this->get_parameter("angular_kd").as_double();
double i_max = this->get_parameter("angular_integral_max").as_double();
double o_max = this->get_parameter("angular_output_max").as_double();
double max_err = this->get_parameter("angular_max_err").as_double();
angular_pid_.init(kp, ki, kd, i_max, o_max, max_err);
// Subscribers
sign_sub_ = this->create_subscription<origincar_msg::msg::Sign>(
"sign4return", 10,
std::bind(&RacingOrchestrator::signCallback, this, std::placeholders::_1));
sign_foxglove_sub_ = this->create_subscription<origincar_msg::msg::Sign>(
"sign_foxglove", 10,
std::bind(&RacingOrchestrator::signFoxgloveCallback, this, std::placeholders::_1));
path_sub_ = this->create_subscription<nav_msgs::msg::Path>(
"/plan", 10,
std::bind(&RacingOrchestrator::pathCallback, this, std::placeholders::_1));
goal_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"/goal_pose", 10,
std::bind(&RacingOrchestrator::goalCallback, this, std::placeholders::_1));
odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
"/odom", 10,
std::bind(&RacingOrchestrator::odomCallback, this, std::placeholders::_1));
lane_cmd_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
"/lane_cmd_vel", 10,
std::bind(&RacingOrchestrator::laneCmdCallback, this, std::placeholders::_1));
// Publishers
cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
// ComputePathToPose action client (triggers Nav2 planner_server)
planner_client_ = rclcpp_action::create_client<ComputePathToPose>(
this, "compute_path_to_pose");
// Control loop
double rate = this->get_parameter("control_rate").as_double();
int period_ms = static_cast<int>(1000.0 / rate);
timer_ = this->create_wall_timer(
std::chrono::milliseconds(period_ms),
std::bind(&RacingOrchestrator::controlLoop, this));
RCLCPP_INFO(this->get_logger(), "RacingOrchestrator started. State: IDLE");
}
private:
// ─────────────────────────────────────────────
// A. Task State Machine — sign callbacks
// ─────────────────────────────────────────────
void signCallback(const origincar_msg::msg::Sign::SharedPtr msg)
{
processSign(msg->sign_data);
}
void signFoxgloveCallback(const origincar_msg::msg::Sign::SharedPtr msg)
{
processSign(msg->sign_data);
}
void processSign(int sign_data)
{
RCLCPP_INFO(this->get_logger(), "Received sign: %d", sign_data);
if (sign_data == -1) {
state_ = TaskState::RESET;
has_path_ = false;
global_path_ = nav_msgs::msg::Path();
waypoint_index_ = 0;
angular_pid_.reset();
RCLCPP_INFO(this->get_logger(), "State -> RESET");
}
else if (sign_data == 3 || sign_data == 4) {
state_ = TaskState::QR_SCAN;
qr_scan_start_ = this->now();
RCLCPP_INFO(this->get_logger(), "State -> QR_SCAN");
}
else if (sign_data == 5) {
state_ = TaskState::GOING;
is_back_ = false;
sub_target_ = true;
has_path_ = false;
waypoint_index_ = 0;
angular_pid_.reset();
// Trigger Nav2 replanning if we have a goal
if (has_goal_) {
requestPlan();
}
RCLCPP_INFO(this->get_logger(), "State -> GOING (forward)");
}
else if (sign_data == -2) {
state_ = TaskState::RETURN;
is_back_ = true;
has_path_ = false;
waypoint_index_ = 0;
angular_pid_.reset();
if (has_goal_) {
requestPlan();
}
RCLCPP_INFO(this->get_logger(), "State -> RETURN");
}
}
// ─────────────────────────────────────────────
// B. Callbacks for path, goal, odom, lane cmd_vel
// ─────────────────────────────────────────────
void pathCallback(const nav_msgs::msg::Path::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
if (msg->poses.empty()) {
has_path_ = false;
return;
}
global_path_ = *msg;
has_path_ = true;
waypoint_index_ = 0;
angular_pid_.reset();
// Auto-transition IDLE -> GOING when receiving an external path
// (e.g. publish_sine_path.py publishes /plan without /goal_pose)
if (state_ == TaskState::IDLE || state_ == TaskState::RESET) {
state_ = TaskState::GOING;
is_back_ = false;
sub_target_ = true;
RCLCPP_INFO(this->get_logger(),
"Auto State -> GOING (external path, %zu waypoints)",
msg->poses.size());
}
}
void goalCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
{
{
std::lock_guard<std::mutex> lock(mutex_);
goal_pose_ = *msg;
has_goal_ = true;
RCLCPP_INFO(this->get_logger(), "New goal: (%.2f, %.2f)",
msg->pose.position.x, msg->pose.position.y);
// Auto-transition IDLE → GOING if not already active
if (state_ == TaskState::IDLE || state_ == TaskState::RESET) {
state_ = TaskState::GOING;
is_back_ = false;
sub_target_ = true;
has_path_ = false;
waypoint_index_ = 0;
angular_pid_.reset();
RCLCPP_INFO(this->get_logger(), "Auto State -> GOING (goal received)");
}
}
// Request plan outside mutex to avoid blocking other callbacks
requestPlan();
}
void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
cur_x_ = msg->pose.pose.position.x;
cur_y_ = msg->pose.pose.position.y;
double qx = msg->pose.pose.orientation.x;
double qy = msg->pose.pose.orientation.y;
double qz = msg->pose.pose.orientation.z;
double qw = msg->pose.pose.orientation.w;
cur_yaw_ = std::atan2(2.0 * (qw * qz + qx * qy),
1.0 - 2.0 * (qy * qy + qz * qz));
has_odom_ = true;
}
void laneCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
lane_cmd_vel_ = *msg;
got_lane_cmd_ = true;
}
// ─────────────────────────────────────────────
// C. ComputePathToPose action client
// ─────────────────────────────────────────────
void requestPlan()
{
if (!planner_client_->wait_for_action_server(std::chrono::seconds(2))) {
RCLCPP_WARN(this->get_logger(),
"Planner action server (compute_path_to_pose) not available");
return;
}
auto goal_msg = ComputePathToPose::Goal();
goal_msg.goal = goal_pose_;
goal_msg.planner_id = "GridBased";
auto send_goal_options = rclcpp_action::Client<ComputePathToPose>::SendGoalOptions();
send_goal_options.result_callback =
[this](const GoalHandleComputePathToPose::WrappedResult& result) {
if (result.code == rclcpp_action::ResultCode::SUCCEEDED) {
RCLCPP_INFO(this->get_logger(), "Plan received: %zu waypoints",
result.result->path.poses.size());
} else {
RCLCPP_WARN(this->get_logger(), "Plan request failed");
}
};
RCLCPP_INFO(this->get_logger(), "Requesting plan from Nav2 planner_server...");
planner_client_->async_send_goal(goal_msg, send_goal_options);
}
// ─────────────────────────────────────────────
// D. Main Control Loop — cmd_vel arbitration
// ─────────────────────────────────────────────
void controlLoop()
{
std::lock_guard<std::mutex> lock(mutex_);
// Priority 1: Task state control
switch (state_)
{
case TaskState::IDLE:
publishStop();
return;
case TaskState::RESET:
publishStop();
state_ = TaskState::IDLE;
return;
case TaskState::QR_SCAN:
{
auto elapsed = this->now() - qr_scan_start_;
if (elapsed.seconds() < 1.0) {
geometry_msgs::msg::Twist cmd;
cmd.linear.x = qr_scan_speed_;
cmd.angular.z = 0.0;
cmd_vel_pub_->publish(cmd);
}
else if (elapsed.seconds() < qr_scan_duration_) {
publishStop();
}
else {
state_ = TaskState::GOING;
RCLCPP_INFO(this->get_logger(), "QR scan complete, resuming");
}
return;
}
case TaskState::GOING:
case TaskState::RETURN:
break;
}
// Priority 2: Obstacle avoidance
if (activate_avoid_) {
geometry_msgs::msg::Twist cmd;
cmd.linear.x = avoid_linear_speed_;
cmd.angular.z = avoid_angular_z_;
cmd_vel_pub_->publish(cmd);
return;
}
// Priority 3: Lane following (sub_target_ == false)
if (!sub_target_ && got_lane_cmd_) {
cmd_vel_pub_->publish(lane_cmd_vel_);
return;
}
// Priority 4: PID path following (path from Nav2 planner_server via /plan)
if (sub_target_ && has_path_ && has_odom_) {
if (waypoint_index_ >= static_cast<int>(global_path_.poses.size())) {
RCLCPP_INFO(this->get_logger(), "Path complete - stopping");
publishStop();
has_path_ = false;
return;
}
int target_idx = std::min(waypoint_index_ + guide_step_,
static_cast<int>(global_path_.poses.size()) - 1);
const auto& target = global_path_.poses[target_idx];
double dx = target.pose.position.x - cur_x_;
double dy = target.pose.position.y - cur_y_;
double distance_sq = dx * dx + dy * dy;
if (distance_sq < arrive_square_) {
waypoint_index_++;
angular_pid_.reset();
}
double target_yaw = std::atan2(dy, dx);
double heading_error = target_yaw - cur_yaw_;
while (heading_error > M_PI) heading_error -= 2.0 * M_PI;
while (heading_error < -M_PI) heading_error += 2.0 * M_PI;
double dt = 1.0 / this->get_parameter("control_rate").as_double();
double angular_z = angular_pid_.update(heading_error, dt);
geometry_msgs::msg::Twist cmd;
cmd.linear.x = follow_linear_speed_;
cmd.angular.z = angular_z;
cmd_vel_pub_->publish(cmd);
return;
}
// Priority 5: No path - stop and wait
publishStop();
}
void publishStop()
{
geometry_msgs::msg::Twist cmd;
cmd.linear.x = 0.0;
cmd.angular.z = 0.0;
cmd_vel_pub_->publish(cmd);
}
// ─────────────────────────────────────────────
// Members (order must match initializer list)
// ─────────────────────────────────────────────
TaskState state_;
bool has_odom_, has_goal_;
bool has_path_, got_lane_cmd_;
bool is_back_, activate_avoid_;
int waypoint_index_;
int guide_step_;
double follow_linear_speed_;
bool sub_target_;
double cmd_vel_linear_x_, cmd_vel_angular_z_;
double cur_x_, cur_y_, cur_yaw_;
double arrive_square_;
past_control::PID angular_pid_;
double avoid_linear_speed_;
double avoid_angular_z_;
double qr_scan_speed_;
double qr_scan_duration_;
rclcpp::Time qr_scan_start_;
nav_msgs::msg::Path global_path_;
geometry_msgs::msg::Twist lane_cmd_vel_;
geometry_msgs::msg::PoseStamped goal_pose_;
rclcpp::Subscription<origincar_msg::msg::Sign>::SharedPtr sign_sub_;
rclcpp::Subscription<origincar_msg::msg::Sign>::SharedPtr sign_foxglove_sub_;
rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr lane_cmd_sub_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
rclcpp_action::Client<ComputePathToPose>::SharedPtr planner_client_;
rclcpp::TimerBase::SharedPtr timer_;
std::mutex mutex_;
};
int main(int argc, char* argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<RacingOrchestrator>());
rclcpp::shutdown();
return 0;
}