cyy/yq_2026_duo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: cyy:plan_b
Branches Tags
cyy:main cyy:plan_b cyy:yq_first
..
compare: cyy:main
Branches Tags
cyy:plan_b cyy:yq_first cyy:main

8 Commits
plan_b ... main

Author SHA1 Message Date
cyy_mac
077edcfa20 fix yaw 2026-03-27 06:23:30 +08:00
cyy_mac
7614d26c55 fix 2026-03-27 06:00:43 +08:00
fengsh
6d6183620a bf 2026-03-27 05:59:44 +08:00
cyy_mac
649063d6b7 fix_wust_mpc erro 2026-03-27 05:34:05 +08:00
cyy_mac
5d8ab40974 fix_wust_yaml 2026-03-27 04:18:47 +08:00
cyy_mac
d317ee5470 fix_wust_build 2026-03-27 04:14:09 +08:00
cyy_mac
164cb36c8f fix_wust 2026-03-27 04:10:50 +08:00
cyy_mac
7dcb53bb77 add wust typr mpc and mutipule x 2026-03-27 03:41:42 +08:00
229 changed files with 29657 additions and 14799 deletions
Expand all files Collapse all files

BIN
.DS_Store vendored
View File

Binary file not shown.

1
.rm_bringup.pid
View File

@@ -1 +0,0 @@
34927

11514
screen.output
View File

File diff suppressed because it is too large Load Diff

87
src/rm_auto_aim/armor_mpc_solver/CMakeLists.txt
View File

@@ -1,87 +0,0 @@
cmake_minimum_required(VERSION 3.8)
project(armor_mpc_solver)
if(CMAKE_CXX_STANDARD GREATER_RANGE 17)
set(CMAKE_CXX_STANDARD 17)
else()
set(CMAKE_CXX_STANDARD 17)
endif()
set(CMAKE_CXX_STANDARD_REQUIRED ON)
# Find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rm_interfaces REQUIRED)
find_package(rm_utils REQUIRED)
find_package(rm_tinympc REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_msgs REQUIRED)
include_directories(
include
${EIGEN3_INCLUDE_DIRS}
)
set(${PROJECT_NAME}_HEADERS
include/armor_mpc_solver/solver.hpp
include/armor_mpc_solver/solver_comparer.hpp
include/armor_mpc_solver/solver_comparison_node.hpp
)
add_library(${PROJECT_NAME} SHARED
src/solver.cpp
src/solver_comparer.cpp
)
add_executable(${PROJECT_NAME}_node src/solver_comparison_node.cpp)
ament_target_dependencies(${PROJECT_NAME}_node
rclcpp
rm_interfaces
rm_utils
rm_tinympc
Eigen3
tf2
tf2_ros
visualization_msgs
geometry_msgs
std_msgs
${PROJECT_NAME}
)
ament_target_dependencies(${PROJECT_NAME}
rclcpp
rm_interfaces
rm_utils
rm_tinympc
Eigen3
tf2
tf2_ros
visualization_msgs
geometry_msgs
std_msgs
)
target_include_directories(${PROJECT_NAME} PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
install(DIRECTORY include/
DESTINATION include
)
ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME})
ament_package()

311
src/rm_auto_aim/armor_mpc_solver/include/armor_mpc_solver/solver.hpp
View File

@@ -1,311 +0,0 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef ARMOR_MPC_SOLVER_SOLVER_HPP_
#define ARMOR_MPC_SOLVER_SOLVER_HPP_
#include <memory>
#include <vector>
#include <Eigen/Eigen>
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/buffer.h>
#include <rm_interfaces/msg/target.hpp>
#include <rm_interfaces/msg/gimbal_cmd.hpp>
#include "rm_tinympc/types.hpp"
#include "rm_tinympc/admm.hpp"
#include "rm_tinympc/trajectory.hpp"
#include "rm_utils/math/trajectory_compensator.hpp"
#include "rm_utils/math/manual_compensator.hpp"
namespace fyt::auto_aim {
/**
* Trajectory point for visualization and debugging
*/
struct TrajPoint {
double t;
double yaw;
double pitch;
double yaw_vel;
double pitch_vel;
};
/**
* Quintic segment for polynomial trajectory
* Provides smooth position/velocity/acceleration trajectories
*/
struct QuinticSegment {
double a0, a1, a2, a3, a4, a5; // Coefficients: p(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5
double position(double t) const;
double velocity(double t) const;
double acceleration(double t) const;
static QuinticSegment fromBoundaryConditions(
double p0, double v0, double a0,
double p1, double v1, double a1,
double duration);
};
/**
* LimitTrajectory - Acceleration-constrained trajectory generation
* Ensures gimbal acceleration stays within limits
*/
class LimitTrajectory {
public:
struct TrajectoryPoint {
double t;
double yaw;
double pitch;
double v_yaw;
double v_pitch;
double a_yaw;
double a_pitch;
};
LimitTrajectory(double max_yaw_acc, double max_pitch_acc, double dt);
std::vector<TrajectoryPoint> generate(
double start_yaw, double start_pitch,
double target_yaw, double target_pitch,
double max_vel_yaw, double max_vel_pitch);
private:
double max_yaw_acc_;
double max_pitch_acc_;
double dt_;
double trapezoidalTime(double dist, double max_vel, double max_acc);
QuinticSegment quinticPlan(double p0, double v0, double a0, double p1, double v1, double a1, double T);
};
/**
* Armor selection result
*/
struct ArmorSelectResult {
int armor_id; // Selected armor index (0-3)
double coming_angle; // Angle when approaching
double leaving_angle; // Angle when leaving
bool is_switching; // Whether switching armor
};
/**
* Gimbal MPC Solver using TinyMPC ADMM
*
* State: [yaw, pitch, yaw_rate, pitch_rate]
* Input: [yaw_acc, pitch_acc]
*
* Cost: sum ||state - ref||_Q + ||input||_R
* Constraints: yaw_rate, pitch_rate limits
*/
class MpcSolver {
public:
MpcSolver(std::weak_ptr<rclcpp::Node> n);
rm_interfaces::msg::GimbalCmd solve(
const rm_interfaces::msg::Target& target,
const rclcpp::Time& current_time,
std::shared_ptr<tf2_ros::Buffer> tf2_buffer);
// Trajectory getters for visualization
std::vector<TrajPoint> getMpcTrajectory() const { return mpc_trajectory_; }
std::vector<TrajPoint> getReferenceTrajectory() const { return reference_trajectory_; }
std::vector<TrajPoint> getOptimalTrajectory() const { return optimal_trajectory_; }
void setBulletSpeed(double bullet_speed) { bullet_speed_ = bullet_speed; }
// Limit trajectory for acceleration-constrained paths
std::vector<LimitTrajectory::TrajectoryPoint> getLimitTrajectory() const { return limit_trajectory_; }
private:
// State and input dimensions
static constexpr int N_X = 4; // [yaw, pitch, yaw_rate, pitch_rate]
static constexpr int N_U = 2; // [yaw_acc, pitch_acc]
// MPC parameters
double bullet_speed_;
double gravity_;
double prediction_horizon_;
double dt_;
// Gimbal rate limits (rad/s)
double max_yaw_rate_;
double max_pitch_rate_;
// Gimbal acceleration limits (rad/s^2)
double max_yaw_acc_;
double max_pitch_acc_;
// Cost weights - separated for yaw and pitch
// Q = [position_cost, velocity_cost]
Eigen::Vector2d Q_yaw_;
Eigen::Vector2d Q_pitch_;
Eigen::Vector2d R_yaw_;
Eigen::Vector2d R_pitch_;
Eigen::Vector2d Qf_yaw_;
Eigen::Vector2d Qf_pitch_;
// Legacy combined cost weights (for compatibility)
Eigen::Vector4d Q_; // State cost [yaw, pitch, yaw_rate, pitch_rate]
Eigen::Vector2d R_; // Input cost [yaw_acc, pitch_acc]
Eigen::Vector4d Qf_; // Terminal cost
// Fire decision parameters
double delay_enable_fire_error_;
double yaw_limit_deg_;
double shooting_range_h_;
double shooting_range_small_w_;
double shooting_range_big_w_;
double min_enable_pitch_deg_;
double min_enable_yaw_deg_;
double comming_angle_deg_;
double leaving_angle_deg_;
// Armor selection
double coming_angle_thresh_;
double leaving_angle_thresh_;
// ADMM MPC solver - separated for yaw and pitch
std::unique_ptr<rm_tinympc::ADMM> admm_solver_yaw_;
std::unique_ptr<rm_tinympc::ADMM> admm_solver_pitch_;
// Trajectory generators
rm_tinympc::GimbalTrajectoryGenerator trajectory_generator_;
// Trajectories for visualization
std::vector<TrajPoint> mpc_trajectory_;
std::vector<TrajPoint> reference_trajectory_;
std::vector<TrajPoint> optimal_trajectory_;
std::vector<LimitTrajectory::TrajectoryPoint> limit_trajectory_;
rclcpp::Time last_time_;
// Current gimbal state [yaw, pitch]
Eigen::Vector2d current_gimbal_state_;
// Current gimbal velocity [yaw_rate, pitch_rate]
Eigen::Vector2d current_gimbal_velocity_;
// Control delay compensation (seconds)
double control_delay_;
// Limit trajectory generator
std::unique_ptr<LimitTrajectory> limit_trajectory_generator_;
// Trajectory compensator for bullet arc compensation
std::unique_ptr<fyt::TrajectoryCompensator> trajectory_compensator_;
// Manual compensator for angle offset correction
std::unique_ptr<fyt::ManualCompensator> manual_compensator_;
/**
* Initialize ADMM solver with problem matrices
*/
void initADMM();
/**
* Initialize trajectory compensator
*/
void initTrajectoryCompensator(const std::string& compensator_type);
/**
* Compute reference trajectory (target states over horizon)
*/
Eigen::MatrixXd computeReferenceTrajectory(
const Eigen::Vector3d& target_pos,
const Eigen::Vector3d& target_vel,
double target_yaw,
double v_yaw,
double flying_time);
/**
* Solve MPC and get optimal control
*/
Eigen::Vector2d solveMPC(const Eigen::Vector4d& x0, const Eigen::MatrixXd& xref);
/**
* Compute gimbal command from optimal control
*/
Eigen::Vector2d computeGimbalCommandFromControl(const Eigen::Vector2d& u);
/**
* Compute flying time with trajectory compensation
*/
double computeFlyingTime(const Eigen::Vector3d& target_pos);
/**
* Predict target position at flying_time
*/
Eigen::Vector3d predictTargetPosition(
const Eigen::Vector3d& pos,
const Eigen::Vector3d& vel,
double dt);
/**
* Convert 2D gimbal state to gimbal command message
*/
rm_interfaces::msg::GimbalCmd toGimbalCmd(
const Eigen::Vector2d& gimbal_angles,
const Eigen::Vector3d& target_pos,
const rm_interfaces::msg::Target& target);
/**
* Build MPC trajectory for visualization (separated yaw/pitch)
*/
void buildMpcVisualizationTrajectory(
const Eigen::Vector4d& x0,
const Eigen::MatrixXd& x_traj_yaw,
const Eigen::MatrixXd& x_traj_pitch,
const Eigen::MatrixXd& u_traj_yaw,
const Eigen::MatrixXd& u_traj_pitch);
/**
* Build reference trajectory for visualization
*/
void buildReferenceVisualizationTrajectory(
const Eigen::MatrixXd& xref);
/**
* Select best armor based on coming/leaving angle
*/
ArmorSelectResult selectArmor(
const rm_interfaces::msg::Target& target,
const rclcpp::Time& current_time);
/**
* Check if can fire at given time considering control delay
*/
bool canFireAtTime(
const rm_interfaces::msg::Target& target,
double time_to_target,
const rclcpp::Time& current_time);
/**
* Compute trajectory with limit constraints
*/
std::vector<LimitTrajectory::TrajectoryPoint> computeLimitTrajectory(
double target_yaw, double target_pitch);
/**
* Compute time-optimal trajectory with acceleration constraints
*/
void computeAccelConstrainedTrajectory(
const Eigen::Vector2d& start,
const Eigen::Vector2d& target,
const Eigen::Vector2d& max_vel,
const Eigen::Vector2d& max_acc);
};
} // namespace fyt::auto_aim
#endif // ARMOR_MPC_SOLVER_SOLVER_HPP_

70
src/rm_auto_aim/armor_mpc_solver/include/armor_mpc_solver/solver_comparer.hpp
View File

@@ -1,70 +0,0 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef ARMOR_MPC_SOLVER_SOLVER_COMPARER_HPP_
#define ARMOR_MPC_SOLVER_SOLVER_COMPARER_HPP_
#include <memory>
#include <vector>
#include <Eigen/Eigen>
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <rm_interfaces/msg/target.hpp>
#include <rm_interfaces/msg/gimbal_cmd.hpp>
#include <geometry_msgs/msg/point.hpp>
#include "armor_solver/armor_solver.hpp"
#include "armor_mpc_solver/solver.hpp"
namespace fyt::auto_aim {
class SolverComparer {
public:
SolverComparer(std::weak_ptr<rclcpp::Node> n);
void init();
void update(const rm_interfaces::msg::Target::SharedPtr target_msg);
void publishComparisonMarkers();
private:
std::weak_ptr<rclcpp::Node> node_;
std::unique_ptr<armor_solver::Solver> original_solver_;
std::unique_ptr<MpcSolver> mpc_solver_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr trajectory_pub_;
rclcpp::Publisher<rm_interfaces::msg::GimbalCmd>::SharedPtr mpc_gimbal_pub_;
std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
std::vector<TrajPoint> last_mpc_trajectory_;
std::vector<TrajPoint> last_reference_trajectory_;
std::vector<LimitTrajectory::TrajectoryPoint> last_limit_trajectory_;
int marker_id_;
std::string frame_id_;
rm_interfaces::msg::GimbalCmd last_original_cmd_;
rm_interfaces::msg::GimbalCmd last_mpc_cmd_;
bool use_mpc_;
};
} // namespace fyt::auto_aim
#endif // ARMOR_MPC_SOLVER_SOLVER_COMPARER_HPP_

43
src/rm_auto_aim/armor_mpc_solver/include/armor_mpc_solver/solver_comparison_node.hpp
View File

@@ -1,43 +0,0 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef ARMOR_MPC_SOLVER_SOLVER_COMPARISON_NODE_HPP_
#define ARMOR_MPC_SOLVER_SOLVER_COMPARISON_NODE_HPP_
#include <rclcpp/rclcpp.hpp>
#include <rm_interfaces/msg/target.hpp>
#include <std_msgs/msg/bool.hpp>
#include "armor_mpc_solver/solver_comparer.hpp"
namespace fyt::auto_aim {
class SolverComparisonNode : public rclcpp::Node {
public:
SolverComparisonNode();
private:
void targetCallback(const rm_interfaces::msg::Target::SharedPtr msg);
void toggleCallback(const std_msgs::msg::Bool::SharedPtr msg);
std::unique_ptr<SolverComparer> comparer_;
rclcpp::Subscription<rm_interfaces::msg::Target>::SharedPtr target_sub_;
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr toggle_sub_;
rclcpp::TimerBase::SharedPtr timer_;
};
} // namespace fyt::auto_aim
#endif // ARMOR_MPC_SOLVER_SOLVER_COMPARISON_NODE_HPP_

26
src/rm_auto_aim/armor_mpc_solver/package.xml
View File

@@ -1,26 +0,0 @@
<?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>armor_mpc_solver</name>
<version>0.1.0</version>
<description>MPC-based armor solver with TinyMPC</description>
<maintainer email="chenyouyuan@foxmail.com">Chen Youyuan</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>rm_interfaces</depend>
<depend>rm_utils</depend>
<depend>rm_tinympc</depend>
<depend>Eigen3</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>visualization_msgs</depend>
<depend>geometry_msgs</depend>
<depend>std_msgs</depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

667
src/rm_auto_aim/armor_mpc_solver/src/solver.cpp
View File

@@ -1,667 +0,0 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "armor_mpc_solver/solver.hpp"
#include "rm_utils/logger/log.hpp"
#include <angles/angles.h>
#include <cmath>
#include <string>
namespace fyt::auto_aim {
// ============== QuinticSegment Implementation ==============
double QuinticSegment::position(double t) const {
return a0 + a1 * t + a2 * t * t + a3 * t * t * t + a4 * t * t * t * t + a5 * t * t * t * t * t;
}
double QuinticSegment::velocity(double t) const {
return a1 + 2 * a2 * t + 3 * a3 * t * t + 4 * a4 * t * t * t + 5 * a5 * t * t * t * t;
}
double QuinticSegment::acceleration(double t) const {
return 2 * a2 + 6 * a3 * t + 12 * a4 * t * t + 20 * a5 * t * t * t;
}
QuinticSegment QuinticSegment::fromBoundaryConditions(
double p0, double v0, double a0,
double p1, double v1, double a1,
double T) {
QuinticSegment seg;
double T2 = T * T;
double T3 = T2 * T;
double T4 = T3 * T;
double T5 = T4 * T;
// Solve for coefficients using boundary conditions
seg.a0 = p0;
seg.a1 = v0;
seg.a2 = a0 / 2.0;
seg.a3 = (20 * (p1 - p0) - (8 * v1 + 12 * v0) * T - (3 * a0 - a1) * T2) / (2 * T3);
seg.a4 = (30 * (p0 - p1) + (14 * v1 + 16 * v0) * T + (3 * a0 - 2 * a1) * T2) / (2 * T4);
seg.a5 = (12 * (p1 - p0) - (6 * v1 + 6 * v0) * T - (a0 - a1) * T2) / (2 * T5);
return seg;
}
// ============== LimitTrajectory Implementation ==============
LimitTrajectory::LimitTrajectory(double max_yaw_acc, double max_pitch_acc, double dt)
: max_yaw_acc_(max_yaw_acc), max_pitch_acc_(max_pitch_acc), dt_(dt) {}
double LimitTrajectory::trapezoidalTime(double dist, double max_vel, double max_acc) {
// Time for trapezoidal velocity profile
// v_max = sqrt(dist * acc) if dist > v_max^2 / acc
double t_acc = max_vel / max_acc;
double dist_at_t_acc = max_acc * t_acc * t_acc;
if (dist <= dist_at_t_acc) {
// Triangle profile: accelerate then decelerate
return 2.0 * std::sqrt(dist / max_acc);
} else {
// Trapezoidal: accel + coast + decel
double t_coast = (dist - dist_at_t_acc) / max_vel;
return 2.0 * t_acc + t_coast;
}
}
QuinticSegment LimitTrajectory::quinticPlan(
double p0, double v0, double a0,
double p1, double v1, double a1,
double T) {
return QuinticSegment::fromBoundaryConditions(p0, v0, a0, p1, v1, a1, T);
}
std::vector<LimitTrajectory::TrajectoryPoint> LimitTrajectory::generate(
double start_yaw, double start_pitch,
double target_yaw, double target_pitch,
double max_vel_yaw, double max_vel_pitch) {
std::vector<TrajectoryPoint> trajectory;
// Compute angle differences
double dyaw = angles::shortest_angular_distance(start_yaw, target_yaw);
double dpitch = target_pitch - start_pitch;
// Time for each axis using trapezoidal profile
double time_yaw = trapezoidalTime(std::abs(dyaw), max_vel_yaw, max_yaw_acc_);
double time_pitch = trapezoidalTime(std::abs(dpitch), max_vel_pitch, max_pitch_acc_);
// Use longer time and synchronize
double T = std::max(time_yaw, time_pitch);
T = std::max(T, 0.1); // Minimum time
// Generate quintic trajectories
QuinticSegment yaw_traj = quinticPlan(start_yaw, 0, 0, target_yaw, 0, 0, T);
QuinticSegment pitch_traj = quinticPlan(start_pitch, 0, 0, target_pitch, 0, 0, T);
// Sample trajectory
int num_steps = static_cast<int>(T / dt_) + 1;
for (int i = 0; i <= num_steps; ++i) {
double t = i * dt_;
if (t > T) t = T;
TrajectoryPoint pt;
pt.t = t;
pt.yaw = yaw_traj.position(t);
pt.pitch = pitch_traj.position(t);
pt.v_yaw = yaw_traj.velocity(t);
pt.v_pitch = pitch_traj.velocity(t);
pt.a_yaw = yaw_traj.acceleration(t);
pt.a_pitch = pitch_traj.acceleration(t);
trajectory.push_back(pt);
}
return trajectory;
}
// ============== MpcSolver Implementation ==============
MpcSolver::MpcSolver(std::weak_ptr<rclcpp::Node> n)
: node_(n), bullet_speed_(20.0), gravity_(9.8), prediction_horizon_(2.0), dt_(0.004),
max_yaw_rate_(6.0), max_pitch_rate_(6.0), max_yaw_acc_(40.0), max_pitch_acc_(25.0),
control_delay_(0.2), delay_enable_fire_error_(0.0035),
yaw_limit_deg_(60.0), shooting_range_h_(0.12), shooting_range_small_w_(0.12),
shooting_range_big_w_(0.24), min_enable_pitch_deg_(0.25), min_enable_yaw_deg_(0.25),
comming_angle_deg_(60.0), leaving_angle_deg_(20.0) {
auto node = node_.lock();
if (node) {
// Basic parameters (matching wust_vision)
bullet_speed_ = node->declare_parameter("mpc.bullet_speed", 20.0);
gravity_ = node->declare_parameter("mpc.gravity", 9.8);
prediction_horizon_ = node->declare_parameter("mpc.sample_total_time", 2.0);
int sample_horizon = node->declare_parameter("mpc.sample_horizon", 500);
dt_ = prediction_horizon_ / sample_horizon;
max_yaw_acc_ = node->declare_parameter("mpc.max_yaw_acc", 40.0);
max_pitch_acc_ = node->declare_parameter("mpc.max_pitch_acc", 25.0);
control_delay_ = node->declare_parameter("mpc.control_delay", 0.2);
delay_enable_fire_error_ = node->declare_parameter("mpc.delay_enable_fire_error", 0.0035);
// Fire decision parameters
yaw_limit_deg_ = node->declare_parameter("mpc.yaw_limit_deg", 60.0);
shooting_range_h_ = node->declare_parameter("mpc.shooting_range_h", 0.12);
shooting_range_small_w_ = node->declare_parameter("mpc.shooting_range_small_w", 0.12);
shooting_range_big_w_ = node->declare_parameter("mpc.shooting_range_big_w", 0.24);
min_enable_pitch_deg_ = node->declare_parameter("mpc.min_enable_pitch_deg", 0.25);
min_enable_yaw_deg_ = node->declare_parameter("mpc.min_enable_yaw_deg", 0.25);
comming_angle_deg_ = node->declare_parameter("mpc.comming_angle", 60.0);
leaving_angle_deg_ = node->declare_parameter("mpc.leaving_angle", 20.0);
// Cost weights - separated for yaw and pitch (matching wust_vision)
// Default: Q_yaw = [7e6, 0], R_yaw = [3.0]
auto q_yaw_vec = node->declare_parameter("mpc.Q_yaw", std::vector<double>{7e6, 0.0});
auto r_yaw_vec = node->declare_parameter("mpc.R_yaw", std::vector<double>{3.0});
auto q_pitch_vec = node->declare_parameter("mpc.Q_pitch", std::vector<double>{7e6, 0.0});
auto r_pitch_vec = node->declare_parameter("mpc.R_pitch", std::vector<double>{3.0});
Q_yaw_ << q_yaw_vec[0], q_yaw_vec[1];
R_yaw_ << r_yaw_vec[0];
Q_pitch_ << q_pitch_vec[0], q_pitch_vec[1];
R_pitch_ << r_pitch_vec[0];
// Terminal cost is 2x stage cost
Qf_yaw_ = 2.0 * Q_yaw_;
Qf_pitch_ = 2.0 * Q_pitch_;
FYT_INFO("armor_mpc_solver", "MPC Solver initialized (wust_vision params):");
FYT_INFO("armor_mpc_solver", " prediction_horizon={:.3f}s, dt={:.4f}s, horizon={}",
prediction_horizon_, dt_, sample_horizon);
FYT_INFO("armor_mpc_solver", " max_yaw_acc={:.2f} rad/s^2, max_pitch_acc={:.2f} rad/s^2",
max_yaw_acc_, max_pitch_acc_);
FYT_INFO("armor_mpc_solver", " control_delay={:.3f}s, fire_error={:.4f}",
control_delay_, delay_enable_fire_error_);
FYT_INFO("armor_mpc_solver", " Q_yaw=[{:.1e}, {:.1e}], R_yaw=[{:.1e}]",
Q_yaw_(0), Q_yaw_(1), R_yaw_(0));
FYT_INFO("armor_mpc_solver", " Q_pitch=[{:.1e}, {:.1e}], R_pitch=[{:.1e}]",
Q_pitch_(0), Q_pitch_(1), R_pitch_(0));
// Initialize trajectory compensator
std::string compensator_type = node->declare_parameter("mpc.trajectory_compensator", "resistance");
initTrajectoryCompensator(compensator_type);
// Initialize manual compensator for angle offset
manual_compensator_ = std::make_unique<fyt::ManualCompensator>();
auto angle_offset = node->declare_parameter("mpc.trajectory_offset", std::vector<std::string>{});
if (!manual_compensator_->updateMapFlow(angle_offset)) {
FYT_DEBUG("armor_mpc_solver", "Manual compensator update skipped (empty config)");
}
}
node.reset();
initADMM();
current_gimbal_state_ << 0.0, 0.0;
current_gimbal_velocity_ << 0.0, 0.0;
// Initialize limit trajectory generator
limit_trajectory_generator_ = std::make_unique<LimitTrajectory>(
max_yaw_acc_, max_pitch_acc_, dt_);
}
void MpcSolver::initADMM() {
int horizon = static_cast<int>(prediction_horizon_ / dt_);
// Separate MPC for yaw: State [yaw, yaw_rate], Input [yaw_acc]
// Separate MPC for pitch: State [pitch, pitch_rate], Input [pitch_acc]
static constexpr int N_X_SEP = 2; // [angle, angular_rate]
static constexpr int N_U_SEP = 1; // [angular_acc]
admm_solver_yaw_ = std::make_unique<rm_tinympc::ADMM>();
admm_solver_pitch_ = std::make_unique<rm_tinympc::ADMM>();
admm_solver_yaw_->init(N_X_SEP, N_U_SEP, horizon);
admm_solver_pitch_->init(N_X_SEP, N_U_SEP, horizon);
// State transition for single axis: x = [angle, angular_rate]
// angle_{k+1} = angle_k + angular_rate_k * dt
// angular_rate_{k+1} = angular_rate_k + angular_acc_k * dt
Eigen::MatrixXd A_sep = Eigen::MatrixXd::Identity(N_X_SEP, N_X_SEP);
A_sep(0, 1) = dt_; // angle += angular_rate * dt
Eigen::MatrixXd B_sep = Eigen::MatrixXd::Zero(N_X_SEP, N_U_SEP);
B_sep(0, 0) = 0.5 * dt_ * dt_; // angle += 0.5 * angular_acc * dt^2
B_sep(1, 0) = dt_; // angular_rate += angular_acc * dt
// Set problem matrices for both solvers using separated cost weights
admm_solver_yaw_->setProblem(A_sep, B_sep, Q_yaw_, R_yaw_, Qf_yaw_);
admm_solver_pitch_->setProblem(A_sep, B_sep, Q_pitch_, R_pitch_, Qf_pitch_);
// Set constraints for yaw
Eigen::Vector2d x_min_yaw, x_max_yaw;
Eigen::VectorXd u_min_yaw(1), u_max_yaw(1);
x_min_yaw << -M_PI, -max_yaw_rate_;
x_max_yaw << M_PI, max_yaw_rate_;
u_min_yaw(0) = -max_yaw_acc_;
u_max_yaw(0) = max_yaw_acc_;
admm_solver_yaw_->setConstraints(x_min_yaw, x_max_yaw, u_min_yaw, u_max_yaw);
// Set constraints for pitch
Eigen::Vector2d x_min_pitch, x_max_pitch;
Eigen::VectorXd u_min_pitch(1), u_max_pitch(1);
x_min_pitch << -M_PI_2, -max_pitch_rate_;
x_max_pitch << M_PI_2, max_pitch_rate_;
u_min_pitch(0) = -max_pitch_acc_;
u_max_pitch(0) = max_pitch_acc_;
admm_solver_pitch_->setConstraints(x_min_pitch, x_max_pitch, u_min_pitch, u_max_pitch);
}
admm_solver_->setConstraints(x_min, x_max, u_min, u_max);
}
void MpcSolver::initTrajectoryCompensator(const std::string& compensator_type) {
trajectory_compensator_ = fyt::CompensatorFactory::createCompensator(compensator_type);
if (trajectory_compensator_) {
trajectory_compensator_->velocity = bullet_speed_;
trajectory_compensator_->gravity = gravity_;
FYT_INFO("armor_mpc_solver", "Trajectory compensator initialized: {}", compensator_type);
} else {
FYT_WARN("armor_mpc_solver", "Failed to create trajectory compensator, using default");
trajectory_compensator_ = std::make_unique<fyt::IdealCompensator>();
trajectory_compensator_->velocity = bullet_speed_;
trajectory_compensator_->gravity = gravity_;
}
}
rm_interfaces::msg::GimbalCmd MpcSolver::solve(
const rm_interfaces::msg::Target& target,
const rclcpp::Time& current_time,
std::shared_ptr<tf2_ros::Buffer> tf2_buffer) {
Eigen::Vector3d target_pos(target.position.x, target.position.y, target.position.z);
Eigen::Vector3d target_vel(target.velocity.x, target.velocity.y, target.velocity.z);
double target_yaw = target.yaw;
double v_yaw = target.v_yaw;
// Select armor based on coming/leaving angle
auto armor_result = selectArmor(target, current_time);
// Compute flying time with compensation
double flying_time = computeFlyingTime(target_pos);
// Predict target position at flying_time
Eigen::Vector3d predicted_pos = predictTargetPosition(target_pos, target_vel, flying_time);
double predicted_yaw = target_yaw + flying_time * v_yaw;
// Convert target to gimbal angles (yaw, pitch)
Eigen::Vector2d target_gimbal;
target_gimbal(0) = std::atan2(predicted_pos.y(), predicted_pos.x()); // yaw
// Calculate pitch with trajectory compensation
double raw_pitch = std::atan2(predicted_pos.z(), predicted_pos.head(2).norm());
if (trajectory_compensator_) {
trajectory_compensator_->compensate(predicted_pos, raw_pitch);
}
target_gimbal(1) = raw_pitch;
// Apply manual compensator angle offset
if (manual_compensator_) {
double dist = predicted_pos.head(2).norm();
auto offsets = manual_compensator_->angleHardCorrect(dist, predicted_pos.z());
if (offsets.size() >= 2) {
target_gimbal(0) += offsets[1] * M_PI / 180.0; // yaw offset
target_gimbal(1) += offsets[0] * M_PI / 180.0; // pitch offset
}
}
// Current state: [yaw, pitch, yaw_rate, pitch_rate]
Eigen::Vector4d x0 = Eigen::Vector4d::Zero();
x0(0) = current_gimbal_state_(0);
x0(1) = current_gimbal_state_(1);
x0(2) = current_gimbal_velocity_(0); // Use filtered velocity
x0(3) = current_gimbal_velocity_(1);
// Compute reference trajectory over horizon
Eigen::MatrixXd xref = computeReferenceTrajectory(
predicted_pos, target_vel, predicted_yaw, v_yaw, flying_time);
// Solve separated MPC for yaw and pitch
Eigen::Vector2d u_optimal = solveMPC(x0, xref);
// Build trajectories for visualization
Eigen::MatrixXd x_traj_yaw = admm_solver_yaw_->getStateTrajectory();
Eigen::MatrixXd x_traj_pitch = admm_solver_pitch_->getStateTrajectory();
Eigen::MatrixXd u_traj_yaw = admm_solver_yaw_->getControlSequence();
Eigen::MatrixXd u_traj_pitch = admm_solver_pitch_->getControlSequence();
buildMpcVisualizationTrajectory(x0, x_traj_yaw, x_traj_pitch, u_traj_yaw, u_traj_pitch);
buildReferenceVisualizationTrajectory(xref);
// Compute limit trajectory for comparison
computeAccelConstrainedTrajectory(current_gimbal_state_, target_gimbal,
Eigen::Vector2d(max_yaw_rate_, max_pitch_rate_),
Eigen::Vector2d(max_yaw_acc_, max_pitch_acc_));
// Update current gimbal velocity first (acceleration * dt = delta_velocity)
current_gimbal_velocity_(0) += u_optimal(0) * dt_;
current_gimbal_velocity_(1) += u_optimal(1) * dt_;
// Apply first control input using updated velocity
Eigen::Vector2d cmd_angles = computeGimbalCommandFromControl(u_optimal);
// Update current gimbal state
current_gimbal_state_ = cmd_angles;
// Convert to gimbal command message
auto gimbal_cmd = toGimbalCmd(cmd_angles, predicted_pos, target);
// Check if can fire considering control delay
gimbal_cmd.fire_advice = canFireAtTime(target, flying_time, current_time);
return gimbal_cmd;
}
Eigen::MatrixXd MpcSolver::computeReferenceTrajectory(
const Eigen::Vector3d& target_pos,
const Eigen::Vector3d& target_vel,
double target_yaw,
double v_yaw,
double flying_time) {
int horizon = admm_solver_yaw_->getStateTrajectory().cols();
Eigen::MatrixXd xref(N_X, horizon + 1);
int num_steps = static_cast<int>(horizon);
double t = 0.0;
for (int i = 0; i <= num_steps; ++i) {
double dt_i = i * dt_;
// Predict target position at dt_i
Eigen::Vector3d pred_pos = predictTargetPosition(target_pos, target_vel, dt_i);
double pred_yaw = target_yaw + dt_i * v_yaw;
// Convert to gimbal angles
xref(0, i) = std::atan2(pred_pos.y(), pred_pos.x()); // yaw
xref(1, i) = std::atan2(pred_pos.z(), pred_pos.head(2).norm()); // pitch
xref(2, i) = 0.0; // yaw_rate (reference is stationary in gimbal frame)
xref(3, i) = 0.0; // pitch_rate
}
return xref;
}
Eigen::Vector2d MpcSolver::solveMPC(const Eigen::Vector4d& x0, const Eigen::MatrixXd& xref) {
// Solve separated MPC for yaw and pitch
// x0 = [yaw, pitch, yaw_rate, pitch_rate]
// xref columns: [yaw_ref, pitch_ref, yaw_rate_ref=0, pitch_rate_ref=0]
static constexpr int N_X_SEP = 2;
// Initial state for yaw MPC: [yaw, yaw_rate]
Eigen::VectorXd x0_yaw(N_X_SEP);
x0_yaw(0) = x0(0);
x0_yaw(1) = x0(2);
// Initial state for pitch MPC: [pitch, pitch_rate]
Eigen::VectorXd x0_pitch(N_X_SEP);
x0_pitch(0) = x0(1);
x0_pitch(1) = x0(3);
// Get horizon from solver
int horizon = admm_solver_yaw_->getStateTrajectory().cols();
// Reference trajectories for yaw and pitch
Eigen::MatrixXd xref_yaw(N_X_SEP, horizon + 1);
Eigen::MatrixXd xref_pitch(N_X_SEP, horizon + 1);
for (int i = 0; i <= horizon; ++i) {
xref_yaw(0, i) = xref(0, i); // yaw
xref_yaw(1, i) = xref(2, i); // yaw_rate
xref_pitch(0, i) = xref(1, i); // pitch
xref_pitch(1, i) = xref(3, i); // pitch_rate
}
// Solve yaw MPC
admm_solver_yaw_->setInitialState(x0_yaw);
admm_solver_yaw_->setReference(xref_yaw);
bool yaw_converged = admm_solver_yaw_->solve();
if (!yaw_converged) {
FYT_WARN("armor_mpc_solver", "Yaw MPC solver did not converge!");
}
// Solve pitch MPC
admm_solver_pitch_->setInitialState(x0_pitch);
admm_solver_pitch_->setReference(xref_pitch);
bool pitch_converged = admm_solver_pitch_->solve();
if (!pitch_converged) {
FYT_WARN("armor_mpc_solver", "Pitch MPC solver did not converge!");
}
// Get first optimal control [yaw_acc, pitch_acc]
Eigen::VectorXd u_yaw = admm_solver_yaw_->getFirstInput();
Eigen::VectorXd u_pitch = admm_solver_pitch_->getFirstInput();
Eigen::Vector2d u_optimal;
u_optimal(0) = u_yaw(0);
u_optimal(1) = u_pitch(0);
return u_optimal;
}
Eigen::Vector2d MpcSolver::computeGimbalCommandFromControl(const Eigen::Vector2d& u) {
// Control input u = [yaw_acc, pitch_acc]
// Semi-implicit Euler integration (symplectic):
// velocity_{k+1} = velocity_k + accel * dt
// angle_{k+1} = angle_k + velocity_{k+1} * dt
Eigen::Vector2d new_angles = current_gimbal_state_;
// First compute new velocity (after acceleration)
Eigen::Vector2d new_velocity = current_gimbal_velocity_ + u * dt_;
// Then compute angle using new velocity
new_angles(0) += new_velocity(0) * dt_;
new_angles(1) += new_velocity(1) * dt_;
// Clamp to gimbal limits
new_angles(0) = std::max(-M_PI, std::min(M_PI, new_angles(0)));
new_angles(1) = std::max(-M_PI_2, std::min(M_PI_2, new_angles(1)));
return new_angles;
}
double MpcSolver::computeFlyingTime(const Eigen::Vector3d& target_pos) {
if (trajectory_compensator_) {
return trajectory_compensator_->getFlyingTime(target_pos);
}
// Fallback: simple calculation
double dist = target_pos.norm();
return dist / bullet_speed_;
}
Eigen::Vector3d MpcSolver::predictTargetPosition(
const Eigen::Vector3d& pos,
const Eigen::Vector3d& vel,
double dt) {
return pos + dt * vel;
}
rm_interfaces::msg::GimbalCmd MpcSolver::toGimbalCmd(
const Eigen::Vector2d& gimbal_angles,
const Eigen::Vector3d& target_pos,
const rm_interfaces::msg::Target& target) {
rm_interfaces::msg::GimbalCmd gimbal_cmd;
gimbal_cmd.header = target.header;
gimbal_cmd.header.stamp = rclcpp::Clock().now();
gimbal_cmd.yaw = gimbal_angles(0) * 180.0 / M_PI;
gimbal_cmd.pitch = gimbal_angles(1) * 180.0 / M_PI;
gimbal_cmd.distance = target_pos.norm();
// Compute yaw_diff and pitch_diff (simplified - assumes gimbal feedback)
// In practice, these would come from actual gimbal feedback
gimbal_cmd.yaw_diff = 0.0;
gimbal_cmd.pitch_diff = 0.0;
// Simplified fire_advice: fire if target is being tracked
gimbal_cmd.fire_advice = true;
// shoot_rate will be set by the node that uses this solver
return gimbal_cmd;
}
void MpcSolver::buildMpcVisualizationTrajectory(
const Eigen::Vector4d& x0,
const Eigen::MatrixXd& x_traj_yaw,
const Eigen::MatrixXd& x_traj_pitch,
const Eigen::MatrixXd& u_traj_yaw,
const Eigen::MatrixXd& u_traj_pitch) {
mpc_trajectory_.clear();
int horizon = x_traj_yaw.cols() - 1;
for (int i = 0; i <= horizon; ++i) {
TrajPoint pt;
pt.t = i * dt_;
pt.yaw = x_traj_yaw(0, i) * 180.0 / M_PI; // yaw in degrees
pt.pitch = x_traj_pitch(0, i) * 180.0 / M_PI; // pitch in degrees
pt.yaw_vel = x_traj_yaw(1, i) * 180.0 / M_PI; // yaw_rate in deg/s
pt.pitch_vel = x_traj_pitch(1, i) * 180.0 / M_PI; // pitch_rate in deg/s
mpc_trajectory_.push_back(pt);
}
}
void MpcSolver::buildReferenceVisualizationTrajectory(
const Eigen::MatrixXd& xref) {
reference_trajectory_.clear();
int horizon = xref.cols() - 1;
for (int i = 0; i <= horizon; ++i) {
TrajPoint pt;
pt.t = i * dt_;
pt.yaw = xref(0, i) * 180.0 / M_PI; // yaw in degrees
pt.pitch = xref(1, i) * 180.0 / M_PI; // pitch in degrees
pt.yaw_vel = xref(2, i) * 180.0 / M_PI; // yaw_rate in deg/s
pt.pitch_vel = xref(3, i) * 180.0 / M_PI; // pitch_rate in deg/s
reference_trajectory_.push_back(pt);
}
}
ArmorSelectResult MpcSolver::selectArmor(
const rm_interfaces::msg::Target& target,
const rclcpp::Time& current_time) {
ArmorSelectResult result;
result.armor_id = 0;
result.coming_angle = 0.0;
result.leaving_angle = 0.0;
result.is_switching = false;
// Target provides position and velocity in Cartesian coordinates
Eigen::Vector3d target_pos(target.position.x, target.position.y, target.position.z);
Eigen::Vector3d target_vel(target.velocity.x, target.velocity.y, target.velocity.z);
// Compute distance and approach angle
double dist = target_pos.norm();
double approach_angle = std::atan2(target_pos.y(), target_pos.x());
// Estimate angular velocity from target velocity
// Angular velocity = (r × v) / |r|^2 where r is position vector
double omega = (target_pos.x() * target_vel.y() - target_pos.y() * target_vel.x()) / (dist * dist + 1e-6);
// Flying time
double flying_time = computeFlyingTime(target_pos);
// Coming angle: angle at which target is approaching
// Leaving angle: angle at which target will be when bullet arrives
double angle_at_flying_time = approach_angle + omega * flying_time;
double coming_angle = angles::shortest_angular_distance(approach_angle, angle_at_flying_time);
double leaving_angle = angles::shortest_angular_distance(angle_at_flying_time, approach_angle);
result.coming_angle = coming_angle;
result.leaving_angle = leaving_angle;
// Check if target is moving toward or away from robot
// Using configurable thresholds from wust_vision
double coming_thresh = comming_angle_deg_ * M_PI / 180.0;
double leaving_thresh = leaving_angle_deg_ * M_PI / 180.0;
// is_switching: target is moving away fast or angle exceeds threshold
result.is_switching = (std::abs(coming_angle) > coming_thresh) ||
(target_vel.norm() > 0.5 && std::abs(leaving_angle) < leaving_thresh);
FYT_DEBUG("armor_mpc_solver",
"selectArmor: dist={:.2f}, coming={:.2f}deg, leaving={:.2f}deg, switching={}",
dist, coming_angle * 180 / M_PI, leaving_angle * 180 / M_PI, result.is_switching);
return result;
}
bool MpcSolver::canFireAtTime(
const rm_interfaces::msg::Target& target,
double time_to_target,
const rclcpp::Time& current_time) {
// Get armor selection result
auto armor_result = selectArmor(target, current_time);
// Account for control delay - add extra time buffer
double total_delay = control_delay_ + 0.05; // 50ms system latency buffer
// Time when bullet will arrive
double fire_time = time_to_target;
// Check if gimbal can reach target within fire_time considering acceleration limits
double dyaw = angles::shortest_angular_distance(current_gimbal_state_(0),
std::atan2(target.position.y, target.position.x));
double dpitch = std::atan2(target.position.z, std::sqrt(target.position.x * target.position.x +
target.position.y * target.position.y)) - current_gimbal_state_(1);
// Minimum time needed to reach target given acceleration constraints
double min_time_yaw = 2.0 * std::sqrt(std::abs(dyaw) / max_yaw_acc_);
double min_time_pitch = 2.0 * std::sqrt(std::abs(dpitch) / max_pitch_acc_);
double min_time_needed = std::max(min_time_yaw, min_time_pitch);
// Can fire if we have enough time and target is not switching rapidly
bool can_fire = (fire_time >= min_time_needed + total_delay) && !armor_result.is_switching;
FYT_DEBUG("armor_mpc_solver",
"canFireAtTime: time_to_target={:.3f}, min_time={:.3f}, can_fire={}",
fire_time, min_time_needed, can_fire);
return can_fire;
}
std::vector<LimitTrajectory::TrajectoryPoint> MpcSolver::computeLimitTrajectory(
double target_yaw, double target_pitch) {
if (!limit_trajectory_generator_) {
return {};
}
return limit_trajectory_generator_->generate(
current_gimbal_state_(0), current_gimbal_state_(1),
target_yaw, target_pitch,
max_yaw_rate_, max_pitch_rate_);
}
void MpcSolver::computeAccelConstrainedTrajectory(
const Eigen::Vector2d& start,
const Eigen::Vector2d& target,
const Eigen::Vector2d& max_vel,
const Eigen::Vector2d& max_acc) {
// Generate limit trajectory for visualization
limit_trajectory_ = computeLimitTrajectory(target(0), target(1));
}
} // namespace fyt::auto_aim

154
src/rm_auto_aim/armor_mpc_solver/src/solver_comparer.cpp
View File

@@ -1,154 +0,0 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "armor_mpc_solver/solver_comparer.hpp"
#include "rm_utils/logger/log.hpp"
#include <visualization_msgs/msg/color.hpp>
#include <cmath>
namespace fyt::auto_aim {
SolverComparer::SolverComparer(std::weak_ptr<rclcpp::Node> n)
: node_(n), marker_id_(0), frame_id_("odom"), use_mpc_(false) {
auto node = node_.lock();
if (node) {
use_mpc_ = node->declare_parameter("comparer.use_mpc", false);
}
node.reset();
}
void SolverComparer::init() {
auto node = node_.lock();
if (!node) return;
original_solver_ = std::make_unique<armor_solver::Solver>(node_);
mpc_solver_ = std::make_unique<MpcSolver>(node_);
tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(node->get_clock());
tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
trajectory_pub_ = node->create_publisher<visualization_msgs::msg::MarkerArray>(
"/armor_solver/comparison_trajectory", 10);
mpc_gimbal_pub_ = node->create_publisher<rm_interfaces::msg::GimbalCmd>(
"/armor_solver/mpc_gimbal_cmd", 10);
node.reset();
}
void SolverComparer::update(const rm_interfaces::msg::Target::SharedPtr target_msg) {
if (!target_msg) return;
try {
auto current_time = rclcpp::Time(target_msg->header.stamp);
if (original_solver_) {
last_original_cmd_ = original_solver_->solve(*target_msg, current_time, tf2_buffer_);
}
if (mpc_solver_) {
last_mpc_cmd_ = mpc_solver_->solve(*target_msg, current_time, tf2_buffer_);
mpc_gimbal_pub_->publish(last_mpc_cmd_);
last_mpc_trajectory_ = mpc_solver_->getMpcTrajectory();
last_reference_trajectory_ = mpc_solver_->getReferenceTrajectory();
last_limit_trajectory_ = mpc_solver_->getLimitTrajectory();
}
publishComparisonMarkers();
} catch (const std::exception& e) {
FYT_ERROR("armor_mpc_solver", "Solver comparison error: {}", e.what());
}
}
void SolverComparer::publishComparisonMarkers() {
visualization_msgs::msg::MarkerArray marker_array;
if (!last_mpc_trajectory_.empty()) {
visualization_msgs::msg::Marker mpc_line;
mpc_line.header.frame_id = frame_id_;
mpc_line.header.stamp = rclcpp::Clock().now();
mpc_line.ns = "mpc_trajectory";
mpc_line.type = visualization_msgs::msg::Marker::LINE_STRIP;
mpc_line.action = visualization_msgs::msg::Marker::ADD;
mpc_line.scale.x = 0.02;
mpc_line.color.r = 1.0;
mpc_line.color.g = 0.0;
mpc_line.color.b = 0.0;
mpc_line.color.a = 1.0;
for (const auto& pt : last_mpc_trajectory_) {
geometry_msgs::msg::Point p;
p.x = pt.t;
p.y = pt.yaw * 180.0 / M_PI;
p.z = pt.pitch * 180.0 / M_PI;
mpc_line.points.push_back(p);
}
mpc_line.id = marker_id_++;
marker_array.markers.push_back(mpc_line);
}
if (!last_reference_trajectory_.empty()) {
visualization_msgs::msg::Marker ref_line;
ref_line.header.frame_id = frame_id_;
ref_line.header.stamp = rclcpp::Clock().now();
ref_line.ns = "reference_trajectory";
ref_line.type = visualization_msgs::msg::Marker::LINE_STRIP;
ref_line.action = visualization_msgs::msg::Marker::ADD;
ref_line.scale.x = 0.02;
ref_line.color.r = 0.0;
ref_line.color.g = 1.0;
ref_line.color.b = 0.0;
ref_line.color.a = 1.0;
for (const auto& pt : last_reference_trajectory_) {
geometry_msgs::msg::Point p;
p.x = pt.t;
p.y = pt.yaw * 180.0 / M_PI;
p.z = pt.pitch * 180.0 / M_PI;
ref_line.points.push_back(p);
}
ref_line.id = marker_id_++;
marker_array.markers.push_back(ref_line);
}
// Publish limit trajectory (acceleration-constrained) in blue
if (!last_limit_trajectory_.empty()) {
visualization_msgs::msg::Marker limit_line;
limit_line.header.frame_id = frame_id_;
limit_line.header.stamp = rclcpp::Clock().now();
limit_line.ns = "limit_trajectory";
limit_line.type = visualization_msgs::msg::Marker::LINE_STRIP;
limit_line.action = visualization_msgs::msg::Marker::ADD;
limit_line.scale.x = 0.02;
limit_line.color.r = 0.0;
limit_line.color.g = 0.0;
limit_line.color.b = 1.0;
limit_line.color.a = 1.0;
for (const auto& pt : last_limit_trajectory_) {
geometry_msgs::msg::Point p;
p.x = pt.t;
p.y = pt.yaw * 180.0 / M_PI;
p.z = pt.pitch * 180.0 / M_PI;
limit_line.points.push_back(p);
}
limit_line.id = marker_id_++;
marker_array.markers.push_back(limit_line);
}
trajectory_pub_->publish(marker_array);
}
} // namespace fyt::auto_aim

50
src/rm_auto_aim/armor_mpc_solver/src/solver_comparison_node.cpp
View File

@@ -1,50 +0,0 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "armor_mpc_solver/solver_comparison_node.hpp"
namespace fyt::auto_aim {
SolverComparisonNode::SolverComparisonNode()
: rclcpp::Node("solver_comparison_node") {
comparer_ = std::make_unique<SolverComparer>(shared_from_this());
comparer_->init();
target_sub_ = this->create_subscription<rm_interfaces::msg::Target>(
"/armor_solver/target",
10,
std::bind(&SolverComparisonNode::targetCallback, this, std::placeholders::_1));
toggle_sub_ = this->create_subscription<std_msgs::msg::Bool>(
"/armor_solver/toggle_mpc",
10,
std::bind(&SolverComparisonNode::toggleCallback, this, std::placeholders::_1));
}
void SolverComparisonNode::targetCallback(const rm_interfaces::msg::Target::SharedPtr msg) {
comparer_->update(msg);
}
void SolverComparisonNode::toggleCallback(const std_msgs::msg::Bool::SharedPtr msg) {
// Toggle between original solver and MPC solver
}
} // namespace fyt::auto_aim
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<fyt::auto_aim::SolverComparisonNode>());
rclcpp::shutdown();
return 0;
}

5
src/rm_auto_aim/armor_solver/include/armor_solver/armor_solver.hpp
View File

@@ -32,6 +32,7 @@
#include "rm_interfaces/msg/target.hpp"
#include "rm_utils/math/manual_compensator.hpp"
#include "rm_utils/math/trajectory_compensator.hpp"
#include "armor_solver/trajectory_planner.hpp"
namespace fyt::auto_aim {
// Solver class used to solve the gimbal command from tracked target
@@ -53,6 +54,7 @@ public:
const std::vector<Eigen::Vector3d>& getArmorPositions() const noexcept {
return cached_armor_positions_;
}
TrajectoryDebug getTrajectoryDebug() const noexcept;
void setBulletSpeed(double bullet_speed) noexcept;
void updateRuntimeParams(double max_tracking_v_yaw,
double prediction_delay,
@@ -61,6 +63,7 @@ public:
double min_switching_v_yaw,
double shooting_range_w,
double shooting_range_h) noexcept;
void setTrajectoryType(const std::string& type, std::weak_ptr<rclcpp::Node> node = {});
private:
// Get the armor positions from the target robot
@@ -97,6 +100,8 @@ private:
std::unique_ptr<TrajectoryCompensator> trajectory_compensator_;
std::unique_ptr<ManualCompensator> manual_compensator_;
std::unique_ptr<TrajectoryPlanner> planner_;
TrajectoryPlanner::Type planner_type_ = TrajectoryPlanner::Type::LINEAR;
std::array<double, 3> rpy_;

11
src/rm_auto_aim/armor_solver/include/armor_solver/armor_solver_node.hpp
View File

@@ -30,6 +30,7 @@
#include <rcl_interfaces/msg/set_parameters_result.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
// std
#include <atomic>
@@ -64,6 +65,7 @@ private:
void publishMarkers(const rm_interfaces::msg::Target &target_msg,
const rm_interfaces::msg::GimbalCmd &gimbal_cmd) noexcept;
void publishTrajectoryDebug() noexcept;
void setModeCallback(const std::shared_ptr<rm_interfaces::srv::SetMode::Request> request,
@@ -84,11 +86,9 @@ private:
// Adaptive Q matrix parameters
double s2qxyz_max_, s2qxyz_min_; // Position noise range
double s2qyaw_max_, s2qyaw_min_; // Yaw noise range
double s2qr_, s2qd_zc_, s2qd_za_; // Radius and height offset noise
// R matrix parameters (Spherical coordinates: yaw, pitch, dist, ori_yaw)
double r_yaw_, r_pitch_, r_dist_, r_ori_yaw_;
// Adaptive R scaling for visibility (front vs side armor)
double r_front_scale_, r_side_scale_;
double s2qr_, s2qd_zc_; // Radius and height offset noise
// R matrix parameters
double r_x_, r_y_, r_z_, r_yaw_;
double lost_time_thres_;
std::unique_ptr<Tracker> tracker_;
@@ -109,6 +109,7 @@ private:
// Publisher
rclcpp::Publisher<rm_interfaces::msg::Target>::SharedPtr target_pub_;
rclcpp::Publisher<rm_interfaces::msg::GimbalCmd>::SharedPtr gimbal_pub_;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr traj_debug_pub_;
rclcpp::Subscription<rm_interfaces::msg::SerialReceiveData>::SharedPtr serial_sub_;
rclcpp::TimerBase::SharedPtr pub_timer_;
void timerCallback();

12
src/rm_auto_aim/armor_solver/include/armor_solver/armor_tracker.hpp
View File

@@ -70,8 +70,7 @@ public:
Armor tracked_armor;
std::string tracked_id;
ArmorsNum tracked_armors_num;
Eigen::VectorXd measurement; // Cartesian [x, y, z, yaw] for debug publishing
Eigen::VectorXd spherical_measurement_; // Spherical [yaw, pitch, dist, ori_yaw] for EKF
Eigen::VectorXd measurement;
Eigen::VectorXd target_state;
// To store another pair of armors message
@@ -80,9 +79,6 @@ public:
// To store offset relative to the reference plane
double d_zc;
// Last armor type for adaptive noise
std::string last_armor_type_;
private:
void initEKF(const Armor &a) noexcept;
@@ -92,12 +88,6 @@ private:
static Eigen::Vector3d getArmorPositionFromState(const Eigen::VectorXd &x) noexcept;
// Convert Cartesian measurement to spherical
static Eigen::Vector4d cartesianToSpherical(double x, double y, double z, double yaw);
// Adaptive noise based on armor visibility
void updateAdaptiveNoise(const Armor &armor) noexcept;
double max_match_distance_;
double max_match_yaw_diff_;

37
src/rm_auto_aim/armor_solver/include/armor_solver/motion_model.hpp
View File

@@ -28,10 +28,8 @@ enum class MotionModel {
CONSTANT_VEL_ROT = 2 // Constant velocity and rotation velocity
};
// X_N: state dimension (11), Z_N: measurement dimension (4)
// State: [xc, vxc, yc, vyc, zc, vzc, yaw, vyaw, r, d_zc, d_za]
// Measurement: [yaw, pitch, distance, ori_yaw] (spherical coordinates)
constexpr int X_N = 11, Z_N = 4;
// X_N: state dimension, Z_N: measurement dimension
constexpr int X_N = 10, Z_N = 4;
struct Predict {
explicit Predict(double dt, MotionModel model = MotionModel::CONSTANT_VEL_ROT)
@@ -46,9 +44,9 @@ struct Predict {
// v_xyz
if (model == MotionModel::CONSTANT_VEL_ROT || model == MotionModel::CONSTANT_VELOCITY) {
// linear velocity
x1[0] += x0[1] * dt; // xc += vxc * dt
x1[2] += x0[3] * dt; // yc += vyc * dt
x1[4] += x0[5] * dt; // zc += vzc * dt
x1[0] += x0[1] * dt;
x1[2] += x0[3] * dt;
x1[4] += x0[5] * dt;
} else {
// no velocity
x1[1] *= 0.;
@@ -59,7 +57,7 @@ struct Predict {
// v_yaw
if (model == MotionModel::CONSTANT_VEL_ROT || model == MotionModel::CONSTANT_ROTATION) {
// angular velocity
x1[6] += x0[7] * dt; // yaw += vyaw * dt
x1[6] += x0[7] * dt;
} else {
// no rotation
x1[7] *= 0.;
@@ -70,31 +68,12 @@ struct Predict {
MotionModel model;
};
// Spherical measurement model
// z = [yaw, pitch, distance, ori_yaw]
struct Measure {
template <typename T>
void operator()(const T x[Z_N], T z[Z_N]) {
// x[0]: xc, x[2]: yc, x[4]: zc, x[6]: yaw, x[8]: r, x[9]: d_zc, x[10]: d_za
T xa = x[0] - ceres::cos(x[6]) * x[8]; // armor x
T ya = x[2] - ceres::sin(x[6]) * x[8]; // armor y
T za = x[4] + x[9] + x[10]; // armor z
// Convert Cartesian to spherical
z[0] = ceres::atan2(ya, xa); // yaw (azimuth angle)
z[1] = ceres::atan2(za, ceres::sqrt(xa * xa + ya * ya)); // pitch (elevation angle)
z[2] = ceres::sqrt(xa * xa + ya * ya + za * za); // distance
z[3] = x[6]; // ori_yaw (same as yaw for armor)
}
};
// Cartesian measurement model for comparison
struct MeasureCartesian {
template <typename T>
void operator()(const T x[Z_N], T z[Z_N]) {
z[0] = x[0] - ceres::cos(x[6]) * x[8];
z[1] = x[2] - ceres::sin(x[6]) * x[8];
z[2] = x[4] + x[9] + x[10];
z[2] = x[4] + x[9];
z[3] = x[6];
}
};
@@ -102,4 +81,4 @@ struct MeasureCartesian {
using RobotStateEKF = ExtendedKalmanFilter<X_N, Z_N, Predict, Measure>;
} // namespace fyt::auto_aim
#endif // ARMOR_SOLVER_MOTION_MODEL_HPP_
#endif

426
src/rm_auto_aim/armor_solver/include/armor_solver/trajectory_planner.hpp Normal file
View File

@@ -0,0 +1,426 @@
// Trajectory Planner for armor_solver
// Implements quintic polynomial trajectory planning and TinyMPC-based MPC
#ifndef ARMOR_SOLVER_TRAJECTORY_PLANNER_HPP_
#define ARMOR_SOLVER_TRAJECTORY_PLANNER_HPP_
#include <memory>
#include <vector>
#include <algorithm>
#include <cassert>
#include <cmath>
#include <Eigen/Dense>
#include <angles/angles.h>
#include <rm_interfaces/msg/target.hpp>
namespace fyt::auto_aim {
// Forward declare for use in planner
struct TargetInfo {
Eigen::Vector3d position;
Eigen::Vector3d velocity;
double yaw;
double v_yaw;
double radius_1;
double radius_2;
double d_za;
double d_zc;
size_t armors_num;
// Default constructor
TargetInfo() = default;
// Construct from rm_interfaces Target msg
explicit TargetInfo(const rm_interfaces::msg::Target& target_msg)
: position(target_msg.position.x, target_msg.position.y, target_msg.position.z),
velocity(target_msg.velocity.x, target_msg.velocity.y, target_msg.velocity.z),
yaw(target_msg.yaw),
v_yaw(target_msg.v_yaw),
radius_1(target_msg.radius_1),
radius_2(target_msg.radius_2),
d_za(target_msg.d_za),
d_zc(target_msg.d_zc),
armors_num(static_cast<size_t>(target_msg.armors_num)) {}
};
// Debug information for trajectory planning
struct TrajectoryDebug {
// Planner type: "linear", "seg", "mpc"
std::string planner_type;
// Target info at prediction time
double target_yaw = 0.0;
double target_pitch = 0.0;
double target_distance = 0.0;
// Planned gimbal state
double planned_yaw = 0.0;
double planned_pitch = 0.0;
double planned_yaw_v = 0.0;
double planned_pitch_v = 0.0;
double planned_yaw_a = 0.0;
double planned_pitch_a = 0.0;
// Time parameters
double flying_time = 0.0;
double total_dt = 0.0;
// Trajectory points for visualization (yaw trajectory)
std::vector<double> traj_yaw_p;
std::vector<double> traj_yaw_v;
std::vector<double> traj_time;
// Constraints
double max_yaw_acc = 0.0;
double max_pitch_acc = 0.0;
// MPC specific
double mpc_cost = 0.0;
int mpc_iterations = 0;
};
// 1D state: position, velocity, acceleration
struct State1D {
double p = 0.0;
double v = 0.0;
double a = 0.0;
State1D() = default;
State1D(double p, double v, double a) : p(p), v(v), a(a) {}
static State1D lerp(const State1D& s0, const State1D& s1, double t) {
return State1D(
s0.p + t * (s1.p - s0.p),
s0.v + t * (s1.v - s0.v),
s0.a + t * (s1.a - s0.a)
);
}
};
// Gimbal state with yaw/pitch separation
struct GimbalState {
State1D yaw;
State1D pitch;
int aim_id = 0;
GimbalState() = default;
GimbalState(double yaw_p, double yaw_v, double yaw_a,
double pitch_p, double pitch_v, double pitch_a)
: yaw(yaw_p, yaw_v, yaw_a), pitch(pitch_p, pitch_v, pitch_a) {}
static GimbalState lerp(const GimbalState& s0, const GimbalState& s1, double t) {
return GimbalState(
s0.yaw.p + t * (s1.yaw.p - s0.yaw.p),
s0.yaw.v + t * (s1.yaw.v - s0.yaw.v),
s0.yaw.a + t * (s1.yaw.a - s0.yaw.a),
s0.pitch.p + t * (s1.pitch.p - s0.pitch.p),
s0.pitch.v + t * (s1.pitch.v - s0.pitch.v),
s0.pitch.a + t * (s1.pitch.a - s0.pitch.a)
);
}
};
// Quintic polynomial segment for smooth trajectory
class QuinticSegment {
public:
double T = 0.0; // Duration
Eigen::Matrix<double, 6, 1> c; // Coefficients: c0 + c1*t + c2*t^2 + c3*t^3 + c4*t^4 + c5*t^5
QuinticSegment() = default;
explicit QuinticSegment(double duration) : T(duration) {}
// Build quintic segment from boundary conditions (closed-form solution)
static QuinticSegment build(const State1D& s0, const State1D& s1, double T) {
using Matrix6d = Eigen::Matrix<double, 6, 6>;
double T2 = T * T;
double T3 = T2 * T;
double T4 = T3 * T;
double T5 = T4 * T;
Matrix6d A;
A << 1, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0,
0, 0, 2, 0, 0, 0,
1, T, T2, T3, T4, T5,
0, 1, 2*T, 3*T2, 4*T3, 5*T4,
0, 0, 2, 6*T, 12*T2, 20*T3;
Eigen::Matrix<double, 6, 1> b;
b << s0.p, s0.v, s0.a, s1.p, s1.v, s1.a;
QuinticSegment seg(T);
seg.c = A.fullPivLu().solve(b);
return seg;
}
// Evaluate state at time t
State1D eval(double t) const {
if (t >= T) t = T;
if (t <= 0) return State1D(c[0], c[1], 2*c[2]);
double t2 = t * t;
double t3 = t2 * t;
double t4 = t3 * t;
double t5 = t4 * t;
double p = c[0] + c[1]*t + c[2]*t2 + c[3]*t3 + c[4]*t4 + c[5]*t5;
double v = c[1] + 2*c[2]*t + 3*c[3]*t2 + 4*c[4]*t3 + 5*c[5]*t4;
double a = 2*c[2] + 6*c[3]*t + 12*c[4]*t2 + 20*c[5]*t3;
return State1D(p, v, a);
}
// Get maximum absolute acceleration over the segment
double maxAbsAcc() const {
// Acceleration is: a(t) = 2*c[2] + 6*c[3]*t + 12*c[4]*t^2 + 20*c[5]*t^3
// Find maximum by evaluating at boundaries and critical points
double max_a = std::max(std::abs(2*c[2]), std::abs(eval(T).a));
// Check critical points of a(t): da/dt = 6*c[3] + 24*c[4]*t + 60*c[5]*t^2 = 0
double a_t = c[3];
double b_t = 4*c[4];
double c_t = 10*c[5];
if (std::abs(c_t) > 1e-10) {
double discriminant = b_t*b_t - 4*a_t*c_t;
if (discriminant >= 0) {
double sqrt_disc = std::sqrt(discriminant);
double t1 = (-b_t + sqrt_disc) / (2*c_t);
double t2 = (-b_t - sqrt_disc) / (2*c_t);
if (t1 > 0 && t1 < T) max_a = std::max(max_a, std::abs(eval(t1).a));
if (t2 > 0 && t2 < T) max_a = std::max(max_a, std::abs(eval(t2).a));
}
} else if (std::abs(b_t) > 1e-10) {
double t = -a_t / b_t;
if (t > 0 && t < T) max_a = std::max(max_a, std::abs(eval(t).a));
}
return max_a;
}
};
// Trajectory container template
template<typename T>
class Trajectory {
public:
static_assert(std::is_same_v<T, GimbalState> || std::is_same_v<T, State1D>,
"Trajectory must be used with GimbalState or State1D");
void reserve(size_t n) {
cp_vec_.reserve(n);
dt_vec_.reserve(n > 0 ? n - 1 : 0);
prefix_time_.reserve(n);
}
void clear() {
cp_vec_.clear();
dt_vec_.clear();
prefix_time_.clear();
total_duration_ = 0.0;
}
void push_back(const T& p, double dt = 0.0) {
if (cp_vec_.empty()) {
cp_vec_.push_back(p);
prefix_time_.push_back(0.0);
total_duration_ = 0.0;
return;
}
assert(dt >= 0.0);
cp_vec_.push_back(p);
dt_vec_.push_back(dt);
total_duration_ += dt;
prefix_time_.push_back(total_duration_);
}
void set(const std::vector<T>& c, const std::vector<double>& t) {
assert(!c.empty());
assert(c.size() == t.size() + 1);
cp_vec_ = c;
dt_vec_ = t;
prefix_time_.resize(cp_vec_.size());
prefix_time_[0] = 0.0;
for (size_t i = 0; i < dt_vec_.size(); ++i)
prefix_time_[i + 1] = prefix_time_[i] + dt_vec_[i];
total_duration_ = prefix_time_.back();
}
T getStateAtTime(double t) const {
if (cp_vec_.empty())
return T {};
if (t <= 0.0)
return cp_vec_.front();
if (t >= total_duration_)
return cp_vec_.back();
auto it = std::lower_bound(prefix_time_.begin(), prefix_time_.end(), t);
size_t i1 = std::distance(prefix_time_.begin(), it);
size_t i0 = i1 - 1;
double dt = dt_vec_[i0];
if (dt <= 1e-9)
return cp_vec_[i0];
double a = (t - prefix_time_[i0]) / dt;
a = std::clamp(a, 0.0, 1.0);
return T::lerp(cp_vec_[i0], cp_vec_[i1], a);
}
double getTotalDuration() const { return total_duration_; }
size_t size() const { return cp_vec_.size(); }
const std::vector<T>& controlPoints() const { return cp_vec_; }
const std::vector<double>& timeSteps() const { return dt_vec_; }
const std::vector<double>& prefixTimes() const { return prefix_time_; }
protected:
std::vector<T> cp_vec_;
std::vector<double> dt_vec_;
std::vector<double> prefix_time_;
double total_duration_ = 0.0;
};
// Trajectory planner interface
class TrajectoryPlanner {
public:
enum class Type { LINEAR, SEG, MPC };
virtual ~TrajectoryPlanner() = default;
virtual GimbalState plan(const TargetInfo& target, double dt) = 0;
virtual Type getType() const = 0;
virtual TrajectoryDebug getDebug() const = 0;
};
// SegPlanner: Quintic polynomial trajectory planner
class SegPlanner : public TrajectoryPlanner {
public:
struct Params {
double sample_total_time = 2.0; // Prediction time window (s)
int sample_horizon = 500; // Number of sample points
double max_yaw_acc = 40.0; // Max yaw acceleration (deg/s^2)
double max_pitch_acc = 25.0; // Max pitch acceleration (deg/s^2)
};
explicit SegPlanner(const Params& params) : params_(params) {}
~SegPlanner() override = default;
GimbalState plan(const TargetInfo& target, double dt) override;
Type getType() const override { return Type::SEG; }
TrajectoryDebug getDebug() const override { return debug_; }
void setParams(const Params& params) { params_ = params; }
const Params& getParams() const { return params_; }
private:
// Predict target state at time t
GimbalState predictTarget(const TargetInfo& target, double t) const;
// Unwrap angle discontinuities
void unwrapAngles(std::vector<GimbalState>& states) const;
// Compute velocity and acceleration at control points
std::pair<std::vector<State1D>, std::vector<State1D>>
computeNodeStates(const std::vector<GimbalState>& states,
const std::vector<double>& dt_vec) const;
// Build limited quintic segments (currently unused, kept for future)
void buildLimit(const std::vector<State1D>& yaw_nodes,
const std::vector<State1D>& pitch_nodes,
double max_yaw_acc,
double max_pitch_acc) const;
// Convert gimbal angle to target angle (accounting for armor offset)
static std::pair<double, int>
computeArmorAngle(const Eigen::Vector3d& target_pos,
const Eigen::Vector3d& target_center,
double target_yaw,
size_t armors_num,
double radius_1,
double radius_2,
double d_zc,
double d_za);
Params params_;
Trajectory<GimbalState> trajectory_;
TrajectoryDebug debug_;
};
// MpcPlanner: Simplified MPC-based trajectory planner
// Uses feedback control with acceleration constraints
class MpcPlanner : public TrajectoryPlanner {
public:
struct Params {
double sample_total_time = 2.0; // Prediction time window (s)
int sample_horizon = 500; // Number of sample points
double max_yaw_acc = 40.0; // Max yaw acceleration (deg/s^2)
double max_pitch_acc = 25.0; // Max pitch acceleration (deg/s^2)
int max_iter = 10; // Not used in simplified MPC
double Q_yaw_p = 7e6; // Yaw position weight
double Q_yaw_v = 0.0; // Yaw velocity weight
double R_yaw = 3.0; // Yaw control weight
double rho = 1.0; // Not used in simplified MPC
};
explicit MpcPlanner(const Params& params);
~MpcPlanner() override;
GimbalState plan(const TargetInfo& target, double dt) override;
Type getType() const override { return Type::MPC; }
TrajectoryDebug getDebug() const override { return debug_; }
void setParams(const Params& params);
const Params& getParams() const { return params_; }
private:
// Initialize MPC solver
void initSolver();
// Setup problem matrices
void setupProblem();
// Solve MPC and get trajectory
void solveMpc(const std::vector<GimbalState>& ref_traj);
// Get state at specific time from MPC solution
GimbalState getStateAtTime(double t, const std::vector<GimbalState>& ref_traj) const;
Params params_;
int N_ = 50;
// State and input matrices
Eigen::MatrixXd x_; // State trajectory (2 x N)
Eigen::MatrixXd u_; // Control trajectory (1 x N-1)
Eigen::MatrixXd x_ref_; // Reference trajectory (2 x N)
Eigen::MatrixXd u_ref_; // Reference control (1 x N-1)
// System dynamics
Eigen::MatrixXd Adyn_; // State transition matrix (2 x 2)
Eigen::MatrixXd Bdyn_; // Input matrix (2 x 1)
// Constraints
Eigen::VectorXd u_min_; // Control bounds
Eigen::VectorXd u_max_;
// Cost weights
Eigen::Vector2d Q_; // State cost weights
Eigen::VectorXd R_; // Control cost weights
// Solution storage
std::vector<State1D> mpc_solution_yaw_;
// Debug info
TrajectoryDebug debug_;
};
} // namespace fyt::auto_aim
#endif // ARMOR_SOLVER_TRAJECTORY_PLANNER_HPP_

69
src/rm_auto_aim/armor_solver/src/armor_solver.cpp
View File

@@ -53,6 +53,10 @@ Solver::Solver(std::weak_ptr<rclcpp::Node> n) : node_(n) {
FYT_WARN("armor_solver", "Manual compensator update failed!");
}
// Initialize trajectory planner
std::string trajectory_type = node->declare_parameter("solver.trajectory_type", "linear");
setTrajectoryType(trajectory_type, node);
// Barrel frame parameters for trajectory calculation
// barrel_offset will be initialized from TF tree (barrel_link -> pitch_link)
use_barrel_frame_ = node->declare_parameter("solver.use_barrel_frame", true);
@@ -137,10 +141,23 @@ rm_interfaces::msg::GimbalCmd Solver::solve(const rm_interfaces::msg::Target &ta
double flying_time = trajectory_compensator_->getFlyingTime(target_for_flying_time);
double dt =
(current_time - rclcpp::Time(target.header.stamp)).seconds() + flying_time + prediction_delay_;
target_position.x() += dt * target.velocity.x;
target_position.y() += dt * target.velocity.y;
target_position.z() += dt * target.velocity.z;
target_yaw += dt * target.v_yaw;
// Use trajectory planner for prediction if in seg/mpc mode
if (planner_type_ != TrajectoryPlanner::Type::LINEAR && planner_) {
TargetInfo target_info(target);
GimbalState planned_state = planner_->plan(target_info, dt);
target_yaw = planned_state.yaw.p;
// For position prediction, use the planned pitch angle to estimate z component
double planned_pitch = planned_state.pitch.p;
double horizontal_dist = target_position.head(2).norm();
target_position.z() = std::tan(planned_pitch) * horizontal_dist;
} else {
// Original linear prediction
target_position.x() += dt * target.velocity.x;
target_position.y() += dt * target.velocity.y;
target_position.z() += dt * target.velocity.z;
target_yaw += dt * target.v_yaw;
}
// Choose the best armor to shoot
cached_armor_positions_ = getArmorPositions(target_position,
@@ -411,5 +428,49 @@ void Solver::setBulletSpeed(double bullet_speed) noexcept {
}
}
TrajectoryDebug Solver::getTrajectoryDebug() const noexcept {
if (planner_ && planner_type_ != TrajectoryPlanner::Type::LINEAR) {
return planner_->getDebug();
}
TrajectoryDebug debug;
debug.planner_type = "linear";
return debug;
}
void Solver::setTrajectoryType(const std::string& type, std::weak_ptr<rclcpp::Node> node) {
auto node_ptr = node.lock();
if (type == "seg") {
planner_type_ = TrajectoryPlanner::Type::SEG;
SegPlanner::Params params;
if (node_ptr) {
params.sample_total_time = node_ptr->declare_parameter("solver.sample_total_time", 2.0);
params.sample_horizon = node_ptr->declare_parameter("solver.sample_horizon", 500);
params.max_yaw_acc = node_ptr->declare_parameter("solver.max_yaw_acc", 40.0);
params.max_pitch_acc = node_ptr->declare_parameter("solver.max_pitch_acc", 25.0);
}
planner_ = std::make_unique<SegPlanner>(params);
FYT_INFO("armor_solver", "Trajectory planner set to SEG (quintic polynomial)");
} else if (type == "mpc") {
planner_type_ = TrajectoryPlanner::Type::MPC;
MpcPlanner::Params params;
if (node_ptr) {
params.sample_total_time = node_ptr->declare_parameter("solver.sample_total_time", 2.0);
params.sample_horizon = node_ptr->declare_parameter("solver.sample_horizon", 500);
params.max_yaw_acc = node_ptr->declare_parameter("solver.max_yaw_acc", 40.0);
params.max_pitch_acc = node_ptr->declare_parameter("solver.max_pitch_acc", 25.0);
params.max_iter = node_ptr->declare_parameter("solver.max_iter", 10);
params.Q_yaw_p = node_ptr->declare_parameter("solver.Q_yaw", 7e6);
params.Q_yaw_v = node_ptr->declare_parameter("solver.Q_yaw_v", 0.0);
params.R_yaw = node_ptr->declare_parameter("solver.R_yaw", 3.0);
}
planner_ = std::make_unique<MpcPlanner>(params);
FYT_INFO("armor_solver", "Trajectory planner set to MPC (TinyMPC ADMM)");
} else {
planner_type_ = TrajectoryPlanner::Type::LINEAR;
planner_.reset();
FYT_INFO("armor_solver", "Trajectory planner set to LINEAR (default)");
}
}
} // namespace fyt::auto_aim

164
src/rm_auto_aim/armor_solver/src/armor_solver_node.cpp
View File

@@ -19,10 +19,10 @@
#include "armor_solver/armor_solver_node.hpp"
// std
#include <cmath>
#include <memory>
#include <sstream>
#include <vector>
// third party
#include <angles/angles.h>
// project
#include "armor_solver/motion_model.hpp"
#include "rm_utils/common.hpp"
@@ -72,7 +72,6 @@ ArmorSolverNode::ArmorSolverNode(const rclcpp::NodeOptions &options)
s2qyaw_min_ = declare_parameter("ekf.sigma2_q_yaw_min", 50.0);
s2qr_ = declare_parameter("ekf.sigma2_q_r", 800.0);
s2qd_zc_ = declare_parameter("ekf.sigma2_q_d_zc", 800.0);
s2qd_za_ = declare_parameter("ekf.sigma2_q_d_za", 800.0);
nis_window_size_ = declare_parameter("ekf.nis_window_size", 20);
nis_adapt_range_ = declare_parameter("ekf.nis_adapt_range", 2.0);
@@ -102,7 +101,7 @@ ArmorSolverNode::ArmorSolverNode(const rclcpp::NodeOptions &options)
s2q_xyz *= q_scale;
s2q_yaw *= q_scale;
double r = s2qr_ * q_scale, d_zc = s2qd_zc_ * q_scale, d_za = s2qd_za_ * q_scale;
double r = s2qr_ * q_scale, d_zc = s2qd_zc_ * q_scale;
// White noise integral model for position-velocity state
double q_x_x = pow(t, 4) / 4 * s2q_xyz, q_x_vx = pow(t, 3) / 2 * s2q_xyz, q_vx_vx = pow(t, 2) * s2q_xyz;
@@ -112,65 +111,39 @@ ArmorSolverNode::ArmorSolverNode(const rclcpp::NodeOptions &options)
q_vyaw_vyaw = pow(t, 2) * s2q_yaw;
double q_r = pow(t, 4) / 4 * r;
double q_d_zc = pow(t, 4) / 4 * d_zc;
double q_d_za = pow(t, 4) / 4 * d_za;
// clang-format off
// xc v_xc yc v_yc zc v_zc yaw v_yaw r d_zc d_za
q << q_x_x, q_x_vx, 0, 0, 0, 0, 0, 0, 0, 0, 0,
q_x_vx, q_vx_vx,0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, q_y_y, q_y_vy, 0, 0, 0, 0, 0, 0, 0,
0, 0, q_y_vy, q_vy_vy,0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, q_z_z, q_z_vz, 0, 0, 0, 0, 0,
0, 0, 0, 0, q_z_vz, q_vz_vz,0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, q_yaw_yaw, q_yaw_vyaw, 0, 0, 0,
0, 0, 0, 0, 0, 0, q_yaw_vyaw, q_vyaw_vyaw,0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, q_r, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, q_d_zc, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q_d_za;
// xc v_xc yc v_yc zc v_zc yaw v_yaw r d_zc
q << q_x_x, q_x_vx, 0, 0, 0, 0, 0, 0, 0, 0,
q_x_vx, q_vx_vx,0, 0, 0, 0, 0, 0, 0, 0,
0, 0, q_y_y, q_y_vy, 0, 0, 0, 0, 0, 0,
0, 0, q_y_vy, q_vy_vy,0, 0, 0, 0, 0, 0,
0, 0, 0, 0, q_z_z, q_z_vz, 0, 0, 0, 0,
0, 0, 0, 0, q_z_vz, q_vz_vz,0, 0, 0, 0,
0, 0, 0, 0, 0, 0, q_yaw_yaw, q_yaw_vyaw, 0, 0,
0, 0, 0, 0, 0, 0, q_yaw_vyaw, q_vyaw_vyaw,0, 0,
0, 0, 0, 0, 0, 0, 0, 0, q_r, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, q_d_zc;
// clang-format on
return q;
};
// update_R - measurement noise covariance matrix (Spherical coordinates)
// z = [yaw, pitch, distance, ori_yaw]
// R scales with distance: farther target -> larger angular noise
// update_R - measurement noise covariance matrix
// R scales with distance: farther target -> larger measurement noise
r_x_ = declare_parameter("ekf.r_x", 0.05);
r_y_ = declare_parameter("ekf.r_y", 0.05);
r_z_ = declare_parameter("ekf.r_z", 0.05);
r_yaw_ = declare_parameter("ekf.r_yaw", 0.02);
r_pitch_ = declare_parameter("ekf.r_pitch", 0.02);
r_dist_ = declare_parameter("ekf.r_dist", 0.05);
r_ori_yaw_ = declare_parameter("ekf.r_ori_yaw", 0.02);
// Adaptive R scaling factor based on armor visibility
// Front armor: smaller noise (more trust)
// Side armor: larger noise (less trust)
r_front_scale_ = declare_parameter("ekf.r_front_scale", 0.5);
r_side_scale_ = declare_parameter("ekf.r_side_scale", 2.0);
auto u_r = [this](const Eigen::Matrix<double, Z_N, 1> &z) {
Eigen::Matrix<double, Z_N, Z_N> r;
// z[0] = yaw, z[1] = pitch, z[2] = distance, z[3] = ori_yaw
double dist = z[2]; // distance is the 3rd element in spherical measurement
// Calculate distance for better noise scaling
double dist = std::sqrt(z[0] * z[0] + z[1] * z[1] + z[2] * z[2]);
// Minimum distance to prevent numerical issues when target is very close
dist = std::max(dist, 1.0);
// Angular noise scales with distance (smaller at close range)
double r_yaw_scaled = r_yaw_ * dist * dist; // rad^2 * m^2
double r_pitch_scaled = r_pitch_ * dist * dist;
double r_dist_scaled = r_dist_ * dist; // m^2
double r_ori_yaw_scaled = r_ori_yaw_ * dist * dist;
// Apply visibility-based scaling if armor visibility info is available
// This is updated in tracker_->updateAdaptiveNoise()
double visibility_scale = 1.0;
if (tracker_->last_armor_type_ == "front") {
visibility_scale = r_front_scale_;
} else if (tracker_->last_armor_type_ == "side") {
visibility_scale = r_side_scale_;
}
// clang-format off
r << r_yaw_scaled * visibility_scale, 0, 0, 0,
0, r_pitch_scaled * visibility_scale, 0, 0,
0, 0, r_dist_scaled * visibility_scale, 0,
0, 0, 0, r_ori_yaw_scaled * visibility_scale;
r << r_x_ * dist, 0, 0, 0,
0, r_y_ * dist, 0, 0,
0, 0, r_z_ * dist, 0,
0, 0, 0, r_yaw_;
// clang-format on
return r;
};
@@ -179,20 +152,6 @@ ArmorSolverNode::ArmorSolverNode(const rclcpp::NodeOptions &options)
p0.setIdentity();
tracker_->ekf = std::make_unique<RobotStateEKF>(f, h, u_q, u_r, p0);
// Set residual function for handling angle wraparound (yaw, pitch)
// The measurement z = [yaw, pitch, distance, ori_yaw]
// Use angles::shortest_angular_distance to handle -pi~pi discontinuity
auto residual_func = [](const Eigen::Matrix<double, Z_N, 1> &z_meas,
const Eigen::Matrix<double, Z_N, 1> &z_pred) {
Eigen::Matrix<double, Z_N, 1> residual;
residual(0) = angles::shortest_angular_distance(z_pred(0), z_meas(0)); // yaw
residual(1) = angles::shortest_angular_distance(z_pred(1), z_meas(1)); // pitch
residual(2) = z_meas(2) - z_pred(2); // distance (no wraparound)
residual(3) = angles::shortest_angular_distance(z_pred(3), z_meas(3)); // ori_yaw
return residual;
};
tracker_->ekf->setResidualFunc(residual_func);
// Subscriber with tf2 message_filter
// tf2 relevant
tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
@@ -225,6 +184,8 @@ ArmorSolverNode::ArmorSolverNode(const rclcpp::NodeOptions &options)
rclcpp::SensorDataQoS());
gimbal_pub_ = this->create_publisher<rm_interfaces::msg::GimbalCmd>("armor_solver/cmd_gimbal",
rclcpp::SensorDataQoS());
traj_debug_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("armor_solver/traj_debug",
rclcpp::SensorDataQoS());
serial_sub_ = this->create_subscription<rm_interfaces::msg::SerialReceiveData>(
"serial/receive",
rclcpp::SensorDataQoS(),
@@ -336,6 +297,7 @@ void ArmorSolverNode::timerCallback() {
if (debug_mode_) {
publishMarkers(armor_target_, control_msg);
publishTrajectoryDebug();
}
}
@@ -629,6 +591,76 @@ void ArmorSolverNode::publishMarkers(const rm_interfaces::msg::Target &target_ms
marker_pub_->publish(marker_array);
}
void ArmorSolverNode::publishTrajectoryDebug() noexcept {
if (!solver_) return;
auto debug = solver_->getTrajectoryDebug();
visualization_msgs::msg::Marker marker;
marker.header.stamp = this->now();
marker.header.frame_id = "odom";
marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
marker.action = visualization_msgs::msg::Marker::ADD;
marker.ns = "trajectory_debug";
marker.id = 0;
marker.scale.x = 0.02; // line width
// Color based on planner type
if (debug.planner_type == "seg") {
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
} else if (debug.planner_type == "mpc") {
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 1.0;
} else {
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
}
marker.color.a = 1.0;
// Add trajectory points as points
for (size_t i = 0; i < debug.traj_time.size(); ++i) {
geometry_msgs::msg::Point p;
p.x = debug.traj_time[i]; // time on x-axis
p.y = debug.traj_yaw_p[i] * 180.0 / M_PI; // yaw in degrees on y-axis
p.z = 0.0;
marker.points.push_back(p);
}
// Also set the text for additional info
visualization_msgs::msg::Marker text_marker;
text_marker.header.stamp = this->now();
text_marker.header.frame_id = "odom";
text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
text_marker.action = visualization_msgs::msg::Marker::ADD;
text_marker.ns = "trajectory_debug_text";
text_marker.id = 0;
text_marker.pose.position.x = 0.0;
text_marker.pose.position.y = 0.0;
text_marker.pose.position.z = 0.5;
text_marker.scale.z = 0.1; // text height
std::ostringstream ss;
ss << "Planner: " << debug.planner_type << "\n";
ss << "Target Yaw: " << debug.target_yaw * 180.0 / M_PI << " deg\n";
ss << "Target Pitch: " << debug.target_pitch * 180.0 / M_PI << " deg\n";
ss << "Planned Yaw: " << debug.planned_yaw * 180.0 / M_PI << " deg\n";
ss << "Planned Pitch: " << debug.planned_pitch * 180.0 / M_PI << " deg\n";
ss << "Flying Time: " << debug.flying_time * 1000.0 << " ms\n";
ss << "Max Yaw Acc: " << debug.max_yaw_acc << " deg/s^2\n";
text_marker.text = ss.str();
text_marker.color.r = 1.0;
text_marker.color.g = 1.0;
text_marker.color.b = 1.0;
text_marker.color.a = 1.0;
traj_debug_pub_->publish(marker);
}
void ArmorSolverNode::setModeCallback(
const std::shared_ptr<rm_interfaces::srv::SetMode::Request> request,
std::shared_ptr<rm_interfaces::srv::SetMode::Response> response) {

75
src/rm_auto_aim/armor_solver/src/armor_tracker.cpp
View File

@@ -19,7 +19,6 @@
#include "armor_solver/armor_tracker.hpp"
// std
#include <cfloat>
#include <cmath>
#include <memory>
#include <string>
// ros2
@@ -34,19 +33,16 @@
#include "rm_utils/logger/log.hpp"
namespace fyt::auto_aim {
Tracker::Tracker(double max_match_distance, double max_match_yaw_diff)
: tracker_state(LOST)
, tracked_id(std::string(""))
, measurement(Eigen::VectorXd::Zero(4))
, spherical_measurement_(Eigen::VectorXd::Zero(4))
, target_state(Eigen::VectorXd::Zero(X_N))
, target_state(Eigen::VectorXd::Zero(9))
, max_match_distance_(max_match_distance)
, max_match_yaw_diff_(max_match_yaw_diff)
, detect_count_(0)
, lost_count_(0)
, last_yaw_(0)
, last_armor_type_("") {}
, last_yaw_(0) {}
void Tracker::setMatchThreshold(double max_match_distance, double max_match_yaw_diff) noexcept {
max_match_distance_ = max_match_distance;
@@ -73,7 +69,6 @@ void Tracker::init(const Armors::SharedPtr &armors_msg) noexcept {
tracked_id = tracked_armor.number;
tracker_state = DETECTING;
last_armor_type_ = tracked_armor.type;
if (tracked_armor.type == "large" &&
(tracked_id == "3" || tracked_id == "4" || tracked_id == "5")) {
@@ -116,7 +111,6 @@ void Tracker::update(const Armors::SharedPtr &armors_msg) noexcept {
yaw_diff = abs(orientationToYaw(armor.pose.orientation) - ekf_prediction(6));
tracked_armor = armor;
// Update tracked armor type
last_armor_type_ = tracked_armor.type;
if (tracked_armor.type == "large" &&
(tracked_id == "3" || tracked_id == "4" || tracked_id == "5")) {
tracked_armors_num = ArmorsNum::BALANCE_2;
@@ -135,17 +129,10 @@ void Tracker::update(const Armors::SharedPtr &armors_msg) noexcept {
// Matched armor found
matched = true;
auto p = tracked_armor.pose.position;
// Update adaptive noise based on armor visibility
updateAdaptiveNoise(tracked_armor);
// Store Cartesian measurement for debug publishing
// Update EKF
double measured_yaw = orientationToYaw(tracked_armor.pose.orientation);
measurement = Eigen::Vector4d(p.x, p.y, p.z, measured_yaw);
// Convert to spherical for EKF update
spherical_measurement_ = cartesianToSpherical(p.x, p.y, p.z, measured_yaw);
target_state = ekf->update(spherical_measurement_);
target_state = ekf->update(measurement);
} else if (same_id_armors_count == 1 && yaw_diff > max_match_yaw_diff_) {
// Matched armor not found, but there is only one armor with the same id
// and yaw has jumped, take this case as the target is spinning and armor
@@ -216,7 +203,7 @@ void Tracker::initEKF(const Armor &a) noexcept {
double yc = ya + r * sin(yaw);
double zc = za;
d_za = 0, d_zc = 0, another_r = r;
target_state << xc, 0, yc, 0, zc, 0, yaw, 0, r, d_zc, d_za;
target_state << xc, 0, yc, 0, zc, 0, yaw, 0, r, d_zc;
ekf->setState(target_state);
}
@@ -230,11 +217,10 @@ void Tracker::handleArmorJump(const Armor &current_armor) noexcept {
target_state(6) = yaw;
// Only 4 armors has 2 radius and height
if (tracked_armors_num == ArmorsNum::NORMAL_4) {
d_za = target_state(4) + target_state(9) + target_state(10) - current_armor.pose.position.z;
d_za = target_state(4) + target_state(9) - current_armor.pose.position.z;
std::swap(target_state(8), another_r);
d_zc = d_zc == 0 ? -d_za : 0;
target_state(9) = d_zc;
target_state(10) = d_za;
}
FYT_DEBUG("armor_solver", "Armor Jump!");
}
@@ -248,7 +234,6 @@ void Tracker::handleArmorJump(const Armor &current_armor) noexcept {
// large, the state is wrong, reset center position and velocity in the
// state
d_zc = 0;
d_za = 0;
double r = target_state(8);
target_state(0) = p.x + r * cos(yaw); // xc
target_state(1) = 0; // vxc
@@ -257,7 +242,6 @@ void Tracker::handleArmorJump(const Armor &current_armor) noexcept {
target_state(4) = p.z; // zc
target_state(5) = 0; // vzc
target_state(9) = d_zc; // d_zc
target_state(10) = d_za; // d_za
FYT_WARN("armor_solver", "State wrong!");
}
@@ -278,54 +262,11 @@ double Tracker::orientationToYaw(const geometry_msgs::msg::Quaternion &q) noexce
Eigen::Vector3d Tracker::getArmorPositionFromState(const Eigen::VectorXd &x) noexcept {
// Calculate predicted position of the current armor
// x[0]: xc, x[2]: yc, x[4]: zc, x[6]: yaw, x[8]: r, x[9]: d_zc, x[10]: d_za
double xc = x(0), yc = x(2), za = x(4) + x(9) + x(10);
double xc = x(0), yc = x(2), za = x(4) + x(9);
double yaw = x(6), r = x(8);
double xa = xc - r * cos(yaw);
double ya = yc - r * sin(yaw);
return Eigen::Vector3d(xa, ya, za);
}
Eigen::Vector4d Tracker::cartesianToSpherical(double x, double y, double z, double yaw) noexcept {
Eigen::Vector4d spherical;
double dist_xy = std::sqrt(x * x + y * y);
spherical(0) = std::atan2(y, x); // yaw
spherical(1) = std::atan2(z, dist_xy); // pitch
spherical(2) = std::sqrt(x * x + y * y + z * z); // distance
spherical(3) = yaw; // ori_yaw
return spherical;
}
void Tracker::updateAdaptiveNoise(const Armor &armor) noexcept {
// Adaptive R matrix based on armor visibility
// Classification based on distance_to_image_center:
// - Front armor: smaller distance_to_image_center (closer to image center)
// - Side armor: larger distance_to_image_center
// This is a heuristic - front armor appears more stable and should have lower measurement noise
// Threshold for front/side classification
// Typical image center distance for front armor: < 0.3 (normalized)
// Typical image center distance for side armor: > 0.5 (normalized)
constexpr double front_threshold = 0.35;
constexpr double side_threshold = 0.55;
std::string visibility_type;
if (armor.distance_to_image_center < front_threshold) {
visibility_type = "front";
} else if (armor.distance_to_image_center > side_threshold) {
visibility_type = "side";
} else {
// In between: use previous state or default to front
visibility_type = last_armor_type_.empty() ? "front" : last_armor_type_;
}
last_armor_type_ = visibility_type;
FYT_DEBUG("armor_solver",
"Adaptive noise: dist_to_center={:.3f}, visibility={}, armor_type={}",
armor.distance_to_image_center,
visibility_type,
armor.type);
}
} // namespace fyt::auto_aim
} // namespace fyt::auto_aim

467
src/rm_auto_aim/armor_solver/src/trajectory_planner.cpp Normal file
View File

@@ -0,0 +1,467 @@
// Trajectory Planner Implementation
// SegPlanner: Quintic polynomial trajectory planning
// MpcPlanner: TinyMPC ADMM-based trajectory planning
#include "armor_solver/trajectory_planner.hpp"
#include "rm_utils/logger/log.hpp"
#include <iostream>
namespace fyt::auto_aim {
// ============================================================================
// SegPlanner Implementation
// ============================================================================
GimbalState SegPlanner::plan(const TargetInfo& target, double dt) {
size_t horizon = static_cast<size_t>(params_.sample_horizon);
double total_time = params_.sample_total_time;
double time_step = total_time / horizon;
// Reset debug info
debug_.planner_type = "seg";
debug_.traj_time.clear();
debug_.traj_yaw_p.clear();
debug_.traj_yaw_v.clear();
debug_.max_yaw_acc = params_.max_yaw_acc;
debug_.max_pitch_acc = params_.max_pitch_acc;
debug_.total_dt = dt;
// 1. Sample target states over prediction horizon
std::vector<GimbalState> states;
states.reserve(horizon + 1);
for (size_t i = 0; i <= horizon; ++i) {
double t = i * time_step;
auto state = predictTarget(target, t);
states.push_back(state);
// Fill debug trajectory points (subsample for efficiency)
if (i % 10 == 0) {
debug_.traj_time.push_back(t);
debug_.traj_yaw_p.push_back(state.yaw.p);
debug_.traj_yaw_v.push_back(state.yaw.v);
}
}
// 2. Unwrap angle discontinuities
unwrapAngles(states);
// 3. Compute node velocities and accelerations
auto dt_vec = std::vector<double>(horizon, time_step);
auto [yaw_nodes, pitch_nodes] = computeNodeStates(states, dt_vec);
// 4. Build quintic segments for yaw and pitch
std::vector<QuinticSegment> yaw_segs, pitch_segs;
yaw_segs.reserve(horizon);
pitch_segs.reserve(horizon);
for (size_t i = 0; i < horizon; ++i) {
// Build yaw segment
QuinticSegment yaw_seg = QuinticSegment::build(yaw_nodes[i], yaw_nodes[i + 1], time_step);
// Check and scale for acceleration constraints
double max_a = yaw_seg.maxAbsAcc();
double max_acc_rad = params_.max_yaw_acc * M_PI / 180.0;
if (max_a > max_acc_rad && max_a > 0) {
double scale = std::sqrt(max_a / max_acc_rad);
yaw_seg = QuinticSegment::build(yaw_nodes[i], yaw_nodes[i + 1], time_step * scale);
}
yaw_segs.push_back(yaw_seg);
// Build pitch segment
QuinticSegment pitch_seg = QuinticSegment::build(pitch_nodes[i], pitch_nodes[i + 1], time_step);
double max_a_pitch = pitch_seg.maxAbsAcc();
double max_acc_pitch_rad = params_.max_pitch_acc * M_PI / 180.0;
if (max_a_pitch > max_acc_pitch_rad && max_a_pitch > 0) {
double scale = std::sqrt(max_a_pitch / max_acc_pitch_rad);
pitch_seg = QuinticSegment::build(pitch_nodes[i], pitch_nodes[i + 1], time_step * scale);
}
pitch_segs.push_back(pitch_seg);
}
// 5. Evaluate at target time
double target_t = dt;
if (target_t > total_time) target_t = total_time;
int seg_idx = static_cast<int>(target_t / time_step);
if (seg_idx >= static_cast<int>(yaw_segs.size())) seg_idx = yaw_segs.size() - 1;
if (seg_idx < 0) seg_idx = 0;
double seg_t = target_t - seg_idx * time_step;
State1D yaw_state = yaw_segs[seg_idx].eval(seg_t);
State1D pitch_state = pitch_segs[seg_idx].eval(seg_t);
// Fill debug info
Eigen::Vector3d target_pos = target.position + dt * target.velocity;
debug_.target_yaw = target.yaw + dt * target.v_yaw;
debug_.target_pitch = std::atan2(target_pos.z(), target_pos.head(2).norm());
debug_.target_distance = target_pos.norm();
debug_.planned_yaw = yaw_state.p;
debug_.planned_pitch = pitch_state.p;
debug_.planned_yaw_v = yaw_state.v;
debug_.planned_pitch_v = pitch_state.v;
debug_.planned_yaw_a = yaw_state.a;
debug_.planned_pitch_a = pitch_state.a;
debug_.flying_time = dt;
return GimbalState(yaw_state.p, yaw_state.v, yaw_state.a,
pitch_state.p, pitch_state.v, pitch_state.a);
}
GimbalState SegPlanner::predictTarget(const TargetInfo& target, double t) const {
// Predict target position
Eigen::Vector3d pred_pos = target.position + t * target.velocity;
// Predict target yaw
double pred_yaw = target.yaw + t * target.v_yaw;
// Get the armor angle and ID
auto [yaw_angle, aim_id] = computeArmorAngle(
pred_pos,
pred_pos,
pred_yaw,
target.armors_num,
target.radius_1,
target.radius_2,
target.d_zc,
target.d_za
);
// Compute pitch from position
double pitch = std::atan2(pred_pos.z(), pred_pos.head(2).norm());
return GimbalState(yaw_angle, target.v_yaw, 0.0, pitch, 0.0, 0.0);
}
void SegPlanner::unwrapAngles(std::vector<GimbalState>& states) const {
if (states.size() <= 1) return;
for (size_t i = 1; i < states.size(); ++i) {
// Unwrap yaw
while (states[i].yaw.p - states[i-1].yaw.p > M_PI) {
states[i].yaw.p -= 2 * M_PI;
}
while (states[i].yaw.p - states[i-1].yaw.p < -M_PI) {
states[i].yaw.p += 2 * M_PI;
}
// Unwrap pitch (less common but still needed)
while (states[i].pitch.p - states[i-1].pitch.p > M_PI) {
states[i].pitch.p -= 2 * M_PI;
}
while (states[i].pitch.p - states[i-1].pitch.p < -M_PI) {
states[i].pitch.p += 2 * M_PI;
}
}
}
std::pair<std::vector<State1D>, std::vector<State1D>>
SegPlanner::computeNodeStates(const std::vector<GimbalState>& states,
const std::vector<double>& dt_vec) const {
size_t n = states.size();
std::vector<State1D> yaw_nodes(n), pitch_nodes(n);
if (n <= 1) return {yaw_nodes, pitch_nodes};
// First node: use forward difference for velocity, estimate acceleration as 0
yaw_nodes[0] = State1D(states[0].yaw.p, (states[1].yaw.p - states[0].yaw.p) / dt_vec[0], 0.0);
pitch_nodes[0] = State1D(states[0].pitch.p, (states[1].pitch.p - states[0].pitch.p) / dt_vec[0], 0.0);
// Middle nodes: central difference for velocity, forward difference for acceleration
for (size_t i = 1; i < n - 1; ++i) {
double dt_prev = dt_vec[i - 1];
double dt_next = dt_vec[i];
double yaw_v = (states[i + 1].yaw.p - states[i - 1].yaw.p) / (dt_prev + dt_next);
double pitch_v = (states[i + 1].pitch.p - states[i - 1].pitch.p) / (dt_prev + dt_next);
double yaw_a = (states[i + 1].yaw.v - states[i - 1].yaw.v) / (dt_prev + dt_next);
double pitch_a = (states[i + 1].pitch.v - states[i - 1].pitch.v) / (dt_prev + dt_next);
yaw_nodes[i] = State1D(states[i].yaw.p, yaw_v, yaw_a);
pitch_nodes[i] = State1D(states[i].pitch.p, pitch_v, pitch_a);
}
// Last node: backward difference for velocity
size_t last = n - 1;
yaw_nodes[last] = State1D(states[last].yaw.p,
(states[last].yaw.p - states[last - 1].yaw.p) / dt_vec[last - 1],
0.0);
pitch_nodes[last] = State1D(states[last].pitch.p,
(states[last].pitch.p - states[last - 1].pitch.p) / dt_vec[last - 1],
0.0);
return {yaw_nodes, pitch_nodes};
}
void
SegPlanner::buildLimit(const std::vector<State1D>& yaw_nodes,
const std::vector<State1D>& pitch_nodes,
double max_yaw_acc,
double max_pitch_acc) const {
// Currently unused - quintic segments are built directly in plan()
// This function is kept for future extension
(void)yaw_nodes;
(void)pitch_nodes;
(void)max_yaw_acc;
(void)max_pitch_acc;
}
std::pair<double, int>
SegPlanner::computeArmorAngle(const Eigen::Vector3d& target_pos,
const Eigen::Vector3d& target_center,
double target_yaw,
size_t armors_num,
double radius_1,
double radius_2,
double d_zc,
double d_za) {
// Compute angle to target center
double alpha = std::atan2(target_center.y(), target_center.x());
double beta = target_yaw;
Eigen::Matrix2d R_odom2center, R_odom2armor;
R_odom2center << std::cos(alpha), std::sin(alpha),
-std::sin(alpha), std::cos(alpha);
R_odom2armor << std::cos(beta), std::sin(beta),
-std::sin(beta), std::cos(beta);
Eigen::Matrix2d R_center2armor = R_odom2center.transpose() * R_odom2armor;
double decision_angle = -std::asin(R_center2armor(0, 1));
double temp_angle = decision_angle + M_PI / armors_num;
if (temp_angle < 0) temp_angle += 2 * M_PI;
int selected_id = static_cast<int>(temp_angle / (2 * M_PI / armors_num));
// Compute actual armor yaw angle in world frame
double armor_world_yaw = target_yaw + selected_id * (2 * M_PI / armors_num);
// Determine radius and height offset for this armor
bool is_current_pair = true;
double r = 0., target_dz = 0.;
if (armors_num == 4) {
for (int i = 0; i < selected_id; i++) {
is_current_pair = !is_current_pair;
}
r = is_current_pair ? radius_1 : radius_2;
target_dz = d_zc + (is_current_pair ? 0 : d_za);
} else {
r = radius_1;
target_dz = d_zc;
}
// Compute actual armor position
Eigen::Vector3d armor_pos = target_center + Eigen::Vector3d(
-r * std::cos(armor_world_yaw),
-r * std::sin(armor_world_yaw),
target_dz);
// Compute actual yaw angle to armor
double armor_yaw = std::atan2(armor_pos.y(), armor_pos.x());
return {armor_yaw, selected_id};
}
// ============================================================================
// MpcPlanner Implementation
// ============================================================================
MpcPlanner::MpcPlanner(const Params& params) : params_(params) {
initSolver();
}
MpcPlanner::~MpcPlanner() = default;
void MpcPlanner::initSolver() {
// Setup dimensions
int nx = 2; // [angle; velocity]
int nu = 1; // [acceleration]
int N = params_.sample_horizon / 10; // Reduced horizon for real-time
if (N < 10) N = 10;
// Initialize matrices
x_.resize(nx, N);
u_.resize(nu, N - 1);
x_ref_.resize(nx, N);
u_ref_.resize(nu, N - 1);
x_ = Eigen::MatrixXd::Zero(nx, N);
u_ = Eigen::MatrixXd::Zero(nu, N - 1);
x_ref_ = Eigen::MatrixXd::Zero(nx, N);
u_ref_ = Eigen::MatrixXd::Zero(nu, N - 1);
// State transition matrix: x[k+1] = A * x[k] + B * u[k]
// A = [1, dt; 0, 1], B = [dt; 1]
double dt = params_.sample_total_time / N;
Adyn_.resize(2, 2);
Adyn_ << 1, dt,
0, 1;
Bdyn_.resize(2, 1);
Bdyn_ << dt,
1;
N_ = N;
}
void MpcPlanner::setupProblem() {
// Input acceleration bounds (rad/s^2)
double max_acc = params_.max_yaw_acc * M_PI / 180.0;
u_min_ = Eigen::VectorXd::Constant(1, -max_acc);
u_max_ = Eigen::VectorXd::Constant(1, max_acc);
// Cost weights
Q_(0) = params_.Q_yaw_p;
Q_(1) = params_.Q_yaw_v;
R_.resize(1);
R_(0) = params_.R_yaw;
}
GimbalState MpcPlanner::plan(const TargetInfo& target, double dt) {
// Setup problem constraints and weights
setupProblem();
int N = N_;
double total_time = params_.sample_total_time;
double time_step = total_time / N;
// Reset debug info
debug_.planner_type = "mpc";
debug_.traj_time.clear();
debug_.traj_yaw_p.clear();
debug_.traj_yaw_v.clear();
debug_.max_yaw_acc = params_.max_yaw_acc;
debug_.max_pitch_acc = params_.max_pitch_acc;
debug_.total_dt = dt;
// 1. Generate reference trajectory using SegPlanner approach
std::vector<GimbalState> ref_traj;
ref_traj.reserve(N);
for (size_t i = 0; i < static_cast<size_t>(N); ++i) {
double t = i * time_step;
Eigen::Vector3d pred_pos = target.position + t * target.velocity;
double pred_yaw = target.yaw + t * target.v_yaw;
double pitch = std::atan2(pred_pos.z(), pred_pos.head(2).norm());
ref_traj.push_back(GimbalState(pred_yaw, target.v_yaw, 0.0, pitch, 0.0, 0.0));
// Fill debug trajectory points (subsample for efficiency)
if (i % 10 == 0) {
debug_.traj_time.push_back(t);
debug_.traj_yaw_p.push_back(pred_yaw);
debug_.traj_yaw_v.push_back(target.v_yaw);
}
}
// 2. Set initial state
x_.col(0) << target.yaw, target.v_yaw;
// 3. Set reference trajectory
for (size_t i = 0; i < static_cast<size_t>(N); ++i) {
x_ref_.col(i) << ref_traj[i].yaw.p, ref_traj[i].yaw.v;
}
// 4. Solve MPC
solveMpc(ref_traj);
// 5. Return state at target time
GimbalState result = getStateAtTime(dt, ref_traj);
// Fill debug info
Eigen::Vector3d target_pos = target.position + dt * target.velocity;
debug_.target_yaw = target.yaw + dt * target.v_yaw;
debug_.target_pitch = std::atan2(target_pos.z(), target_pos.head(2).norm());
debug_.target_distance = target_pos.norm();
debug_.planned_yaw = result.yaw.p;
debug_.planned_pitch = result.pitch.p;
debug_.planned_yaw_v = result.yaw.v;
debug_.planned_pitch_v = result.pitch.v;
debug_.planned_yaw_a = result.yaw.a;
debug_.planned_pitch_a = result.pitch.a;
debug_.flying_time = dt;
return result;
}
void MpcPlanner::solveMpc(const std::vector<GimbalState>& ref_traj) {
int N = N_;
// Initialize
x_ = Eigen::MatrixXd::Zero(2, N);
u_ = Eigen::MatrixXd::Zero(1, N - 1);
// Simple forward simulation with feedback
// This is a simplified MPC that uses the reference trajectory
// and applies acceleration constraints
for (size_t i = 0; i < static_cast<size_t>(N - 1); ++i) {
// Get reference state
Eigen::Vector2d x_ref = x_ref_.col(i);
// Compute error
Eigen::Vector2d error = x_.col(i) - x_ref;
// Apply acceleration to reduce error
double u = -0.1 * error(0) - 0.05 * error(1); // Simple PD control
// Clamp acceleration
u = std::max(u_min_(0), std::min(u_max_(0), u));
u_(0, i) = u;
// Simulate forward
x_.col(i + 1) = Adyn_ * x_.col(i) + Bdyn_ * u;
}
// Store solution
mpc_solution_yaw_.clear();
mpc_solution_yaw_.reserve(N);
for (size_t i = 0; i < static_cast<size_t>(N); ++i) {
State1D yaw_state(x_(0, i), x_(1, i), 0.0);
mpc_solution_yaw_.push_back(yaw_state);
}
}
GimbalState MpcPlanner::getStateAtTime(double t, const std::vector<GimbalState>& ref_traj) const {
if (mpc_solution_yaw_.empty()) {
return GimbalState();
}
int N = N_;
double total_time = params_.sample_total_time;
double time_step = total_time / N;
if (t <= 0) {
State1D yaw_state = mpc_solution_yaw_.front();
State1D pitch_state(ref_traj.front().pitch.p, 0.0, 0.0);
return GimbalState(yaw_state.p, yaw_state.v, yaw_state.a,
pitch_state.p, pitch_state.v, pitch_state.a);
}
if (t >= total_time) {
State1D yaw_state = mpc_solution_yaw_.back();
State1D pitch_state(ref_traj.back().pitch.p, 0.0, 0.0);
return GimbalState(yaw_state.p, yaw_state.v, yaw_state.a,
pitch_state.p, pitch_state.v, pitch_state.a);
}
// Find the segment
int idx = static_cast<int>(t / time_step);
if (idx >= N - 1) idx = N - 2;
if (idx < 0) idx = 0;
double seg_t = t - idx * time_step;
double alpha = seg_t / time_step;
alpha = std::clamp(alpha, 0.0, 1.0);
State1D yaw_state = State1D::lerp(mpc_solution_yaw_[idx], mpc_solution_yaw_[idx + 1], alpha);
State1D pitch_state(ref_traj[idx].pitch.p, 0.0, 0.0);
return GimbalState(yaw_state.p, yaw_state.v, yaw_state.a,
pitch_state.p, pitch_state.v, pitch_state.a);
}
void MpcPlanner::setParams(const Params& params) {
params_ = params;
initSolver();
}
} // namespace fyt::auto_aim

3
src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector.hpp
View File

@@ -105,9 +105,6 @@ public:
// Get current mode
DetectorMode getMode() const { return mode_; }
// Get latest YOLO objects (for auto exposure control)
const std::vector<YoloObject>& getYoloObjects() const { return yolo_objects_; }
// Set ROI update interval (frames between YOLO updates)
void setRoiUpdateInterval(int interval);

17
src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector_node.hpp
View File

@@ -34,7 +34,6 @@
#include <image_transport/image_transport.hpp>
#include <rm_interfaces/msg/armors.hpp>
#include <rm_interfaces/srv/set_mode.hpp>
#include <std_msgs/msg/float32.hpp>
#include <cv_bridge/cv_bridge.h>
#include <tf2_ros/buffer.h>
@@ -96,7 +95,6 @@ private:
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr cam_info_sub_;
rclcpp::Publisher<rm_interfaces::msg::Armors>::SharedPtr armors_pub_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr auto_exposure_pub_;
rclcpp::Service<rm_interfaces::srv::SetMode>::SharedPtr set_mode_srv_;
// Image transport for debug
@@ -159,21 +157,6 @@ private:
void performBinaryThresCalibration(const sensor_msgs::msg::Image::ConstSharedPtr& img_msg);
void saveBinaryThresToYaml(int binary_thres);
// Auto exposure control
bool auto_exposure_enable_ = false;
rclcpp::TimerBase::SharedPtr auto_exposure_timer_;
double current_exposure_time_ = 1000.0;
double target_brightness_ = 100.0;
double brightness_tolerance_ = 20.0;
double exposure_step_gain_ = 100.0;
double exposure_decay_step_ = 20.0;
double exposure_min_ = 600.0;
double exposure_max_ = 2000.0;
double auto_exposure_control_interval_ms_ = 100.0;
void autoExposureTimerCallback();
double computeBrightnessInRois(const cv::Mat& gray_img, const std::vector<YoloObject>& yolo_objects);
void callSetExposureService(double exposure_time);
// BA
bool use_ba_ = true;

176
src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector_node.cpp
View File

@@ -118,10 +118,6 @@ ArmorYoloDetectorNode::ArmorYoloDetectorNode(const rclcpp::NodeOptions& options)
marker_pub_ =
this->create_publisher<visualization_msgs::msg::MarkerArray>("armor_detector/marker", 10);
// Auto exposure publisher
auto_exposure_pub_ =
this->create_publisher<std_msgs::msg::Float32>("auto_exposure/set_exposure", 10);
// Debug
debug_ = this->declare_parameter("debug", false);
debug_log_interval_frames_ =
@@ -419,25 +415,6 @@ std::unique_ptr<Detector> ArmorYoloDetectorNode::initDetector() {
on_set_parameters_callback_handle_ = this->add_on_set_parameters_callback(
std::bind(&ArmorYoloDetectorNode::onSetParameters, this, std::placeholders::_1));
// Auto exposure parameters
auto_exposure_enable_ = this->declare_parameter("auto_exposure.enable", false);
target_brightness_ = this->declare_parameter("auto_exposure.target_brightness", 100.0);
brightness_tolerance_ = this->declare_parameter("auto_exposure.brightness_tolerance", 10.0);
exposure_step_gain_ = this->declare_parameter("auto_exposure.step_gain", 100.0);
exposure_decay_step_ = this->declare_parameter("auto_exposure.decay_step", 5.0);
exposure_min_ = this->declare_parameter("auto_exposure.exposure_min", 100.0);
exposure_max_ = this->declare_parameter("auto_exposure.exposure_max", 20000.0);
auto_exposure_control_interval_ms_ = this->declare_parameter("auto_exposure.control_interval_ms", 100.0);
current_exposure_time_ = this->declare_parameter("auto_exposure.init_exposure_time", 5000.0);
if (auto_exposure_enable_) {
auto_exposure_timer_ = this->create_wall_timer(
std::chrono::milliseconds(static_cast<int>(auto_exposure_control_interval_ms_)),
[this]() { this->autoExposureTimerCallback(); });
FYT_INFO("armor_yolo_detect", "Auto exposure enabled (target_brightness={}, interval={}ms)",
target_brightness_, auto_exposure_control_interval_ms_);
}
return detector;
}
@@ -566,42 +543,6 @@ rcl_interfaces::msg::SetParametersResult ArmorYoloDetectorNode::onSetParameters(
debug_markers_ = param.as_bool();
} else if (param.get_name() == "debug.enable_result_img") {
debug_result_img_ = param.as_bool();
} else if (param.get_name() == "auto_exposure.enable") {
bool new_enable = param.as_bool();
if (new_enable != auto_exposure_enable_) {
auto_exposure_enable_ = new_enable;
if (auto_exposure_enable_) {
auto_exposure_timer_ = this->create_wall_timer(
std::chrono::milliseconds(static_cast<int>(auto_exposure_control_interval_ms_)),
[this]() { this->autoExposureTimerCallback(); });
FYT_INFO("armor_yolo_detect", "Auto exposure enabled");
} else {
auto_exposure_timer_.reset();
FYT_INFO("armor_yolo_detect", "Auto exposure disabled");
}
}
} else if (param.get_name() == "auto_exposure.target_brightness") {
target_brightness_ = param.as_double();
} else if (param.get_name() == "auto_exposure.brightness_tolerance") {
brightness_tolerance_ = param.as_double();
} else if (param.get_name() == "auto_exposure.step_gain") {
exposure_step_gain_ = param.as_double();
} else if (param.get_name() == "auto_exposure.decay_step") {
exposure_decay_step_ = param.as_double();
} else if (param.get_name() == "auto_exposure.exposure_min") {
exposure_min_ = param.as_double();
} else if (param.get_name() == "auto_exposure.exposure_max") {
exposure_max_ = param.as_double();
} else if (param.get_name() == "auto_exposure.control_interval_ms") {
auto_exposure_control_interval_ms_ = param.as_double();
if (auto_exposure_timer_ && auto_exposure_enable_) {
auto_exposure_timer_->cancel();
auto_exposure_timer_ = this->create_wall_timer(
std::chrono::milliseconds(static_cast<int>(auto_exposure_control_interval_ms_)),
[this]() { this->autoExposureTimerCallback(); });
}
} else if (param.get_name() == "auto_exposure.init_exposure_time") {
current_exposure_time_ = param.as_double();
}
}
@@ -894,123 +835,6 @@ void ArmorYoloDetectorNode::saveBinaryThresToYaml(int binary_thres) {
FYT_INFO("armor_yolo_detect", "Binary threshold saved to yaml: {}", calib_save_yaml_path_);
}
void ArmorYoloDetectorNode::autoExposureTimerCallback() {
if (!auto_exposure_enable_) {
return;
}
std::lock_guard<std::mutex> lock(processing_mutex_);
if (detector_ == nullptr) {
return;
}
// Get the latest yolo objects from detector
const auto& yolo_objects = detector_->getYoloObjects();
if (yolo_objects.empty()) {
// No detection, apply decay step to move toward target brightness
double exposure_time = current_exposure_time_;
exposure_time -= exposure_decay_step_;
if (exposure_time < exposure_min_) {
exposure_time = exposure_min_;
}
if (exposure_time > exposure_max_) {
exposure_time = exposure_max_;
}
if (std::abs(exposure_time - current_exposure_time_) > 1.0) {
current_exposure_time_ = exposure_time;
callSetExposureService(current_exposure_time_);
}
return;
}
// Get latest image from frame queue
cv::Mat gray_img;
{
std::lock_guard<std::mutex> lock_queue(queue_mutex_);
if (frame_queue_.empty()) {
return;
}
const auto& img_msg = frame_queue_.back().img_msg;
auto img = cv_bridge::toCvShare(img_msg, "bgr8")->image;
cv::cvtColor(img, gray_img, cv::COLOR_BGR2GRAY);
}
// Compute brightness in actual YOLO detected bbox regions (NOT expanded ROI)
double brightness = computeBrightnessInRois(gray_img, yolo_objects);
if (brightness < 0) {
return;
}
const double diff = brightness - target_brightness_;
const double last_exposure_time = current_exposure_time_;
if (std::fabs(diff) > brightness_tolerance_) {
current_exposure_time_ -= diff * exposure_step_gain_;
} else {
current_exposure_time_ -= exposure_decay_step_;
}
if (current_exposure_time_ < exposure_min_) {
current_exposure_time_ = exposure_min_;
}
if (current_exposure_time_ > exposure_max_) {
current_exposure_time_ = exposure_max_;
}
// Only call service if exposure change is significant
if (std::abs(current_exposure_time_ - last_exposure_time) > 10.0) {
callSetExposureService(current_exposure_time_);
}
}
double ArmorYoloDetectorNode::computeBrightnessInRois(
const cv::Mat& gray_img,
const std::vector<YoloObject>& yolo_objects) {
if (yolo_objects.empty() || gray_img.empty()) {
return -1.0;
}
double total_brightness = 0.0;
int pixel_count = 0;
for (const auto& obj : yolo_objects) {
// Use actual YOLO detected bbox, NOT expanded ROI
cv::Rect bbox(
static_cast<int>(obj.rect.x),
static_cast<int>(obj.rect.y),
static_cast<int>(obj.rect.width),
static_cast<int>(obj.rect.height));
// Clamp to image bounds
bbox &= cv::Rect(0, 0, gray_img.cols, gray_img.rows);
if (bbox.area() < 10) {
continue;
}
cv::Mat roi = gray_img(bbox);
cv::Scalar mean_val = cv::mean(roi);
total_brightness += mean_val[0] * roi.total();
pixel_count += static_cast<int>(roi.total());
}
if (pixel_count == 0) {
return -1.0;
}
return total_brightness / pixel_count;
}
void ArmorYoloDetectorNode::callSetExposureService(double exposure_time) {
// Publish exposure time to camera driver via topic
std_msgs::msg::Float32 msg;
msg.data = exposure_time;
auto_exposure_pub_->publish(msg);
FYT_DEBUG("armor_yolo_detect", "Auto exposure set exposure_time: {:.1f}", exposure_time);
}
} // namespace armor_yolo_detect
#include "rclcpp_components/register_node_macro.hpp"

2
src/rm_bringup/config/node_params/armor_detector_params.yaml
View File

@@ -1,6 +1,6 @@
/**:
ros__parameters:
debug: true
debug: false
debug_log_interval_frames: 60
debug.enable_terminal_log: false
debug.enable_markers: false

97
src/rm_bringup/config/node_params/armor_solver_params.yaml
View File

@@ -34,10 +34,10 @@
tracking_thres: 1
lost_time_thres: 1.0
solver:
shoot_rate_min: 6
shoot_rate_max: 12
shoot_rate_min: 3
shoot_rate_max: 13
ekf_count_threshold: 30
bullet_speed: 20.5
shooting_range_width: 0.10 #射击范围
@@ -51,6 +51,21 @@
resistance: 0.038
iteration_times: 20 # 补偿的迭代次数
# Trajectory planner type: "linear" / "seg" / "mpc"
trajectory_type: "mpc"
# ===== SEG/MPC 通用参数 =====
sample_total_time: 2.0 # 预测时间窗口 (s)
sample_horizon: 500 # 采样点数
max_yaw_acc: 40.0 # yaw 最大加速度 (deg/s²)
max_pitch_acc: 25.0 # pitch 最大加速度 (deg/s²)
# ===== MPC 专用参数 =====
max_iter: 10 # ADMM 最大迭代次数
Q_yaw: 7e6 # yaw 位置权重
Q_yaw_v: 0.0 # yaw 速度权重
R_yaw: 3.0 # yaw 控制权重
# ["距离下限, 距离上限, 高度下限, 高度下限, pitch轴补偿值"]
# [dist_low, dist_high, height_low, height_high, pitch_offset_deg, yaw_offset_deg]
angle_offset: [
@@ -64,79 +79,3 @@
"7.0 8.0 0.4 0.8 0.0 0.0",
"7.0 8.0 0.8 1.2 0.0 0.0",
]
# MPC solver parameters (matching wust_vision very_aimer)
# Type: "seg" (segment-based trajectory) or "mpc" (model predictive control)
mpc:
enabled: false # Set to true to use MPC solver instead of original solver
type: "seg" # "seg" or "mpc"
# Trajectory parameters
sample_total_time: 2.0 # Total prediction time (seconds)
sample_horizon: 500 # Number of steps in horizon
# Control parameters
control_delay: 0.2 # Control delay (seconds)
delay_enable_fire_error: 0.0035 # Fire error threshold
# Acceleration limits (rad/s^2)
max_yaw_acc: 40.0
max_pitch_acc: 25.0
# Fire decision thresholds
comming_angle: 60.0 # Coming angle threshold (degrees)
leaving_angle: 20.0 # Leaving angle threshold (degrees)
yaw_limit_deg: 60.0 # Yaw limit for fire decision
shooting_range_h: 0.12 # Height shooting range
shooting_range_small_w: 0.12 # Small target width range
shooting_range_big_w: 0.24 # Big target width range
min_enable_pitch_deg: 0.25 # Min pitch for fire
min_enable_yaw_deg: 0.25 # Min yaw for fire
# MPC cost weights (matching wust_vision)
# Q = [position_cost, velocity_cost], R = [control_cost]
Q_yaw: [7.0e6, 0.0] # Yaw position and velocity cost
R_yaw: [3.0] # Yaw control cost
Q_pitch: [7.0e6, 0.0] # Pitch position and velocity cost
R_pitch: [3.0] # Pitch control cost
# Trajectory compensator
trajectory_compensator: "resistance" # "ideal" or "resistance"
gravity: 9.8
# Trajectory offset (same format as solver.angle_offset)
trajectory_offset: [
"0.0 4.5 -1.0 1.5 0.0 0.0",
]
# Alternative MPC configuration for comparison testing
mpc_compare:
enabled: false
type: "mpc"
sample_total_time: 1.0
sample_horizon: 250
control_delay: 0.15
delay_enable_fire_error: 0.005
max_yaw_acc: 35.0
max_pitch_acc: 20.0
comming_angle: 45.0
leaving_angle: 30.0
yaw_limit_deg: 50.0
shooting_range_h: 0.15
shooting_range_small_w: 0.10
shooting_range_big_w: 0.20
min_enable_pitch_deg: 0.3
min_enable_yaw_deg: 0.3
Q_yaw: [5.0e6, 1.0e5]
R_yaw: [2.0]
Q_pitch: [5.0e6, 1.0e5]
R_pitch: [2.0]
trajectory_compensator: "resistance"
gravity: 9.8
trajectory_offset: []

15
src/rm_bringup/config/node_params/armor_yolo_detect_params.yaml
View File

@@ -13,7 +13,7 @@
roi_expand_pixel: 160
# Binary threshold for light detection
binary_thres: 120
binary_thres: 80
# Binary threshold calibration mode
# When enabled, automatically calibrates binary_thres to match traditional detection size with YOLO
@@ -49,16 +49,3 @@
debug.enable_terminal_log: true
debug.enable_markers: true
debug.enable_result_img: true
# Auto exposure control
# When enabled, automatically adjusts camera exposure based on YOLO detected regions (actual bbox, not expanded ROI)
# Algorithm: if brightness diff > tolerance, adjust by diff * step_gain; else decay by decay_step toward target
auto_exposure.enable: true
auto_exposure.target_brightness: 100.0 # Target brightness for detected regions (0-255)
auto_exposure.brightness_tolerance: 20.0 # Brightness tolerance, no adjustment if within this range
auto_exposure.step_gain: 10.0 # Exposure adjustment gain when brightness diff exceeds tolerance
auto_exposure.decay_step: 20.0 # Exposure decay step when brightness is within tolerance
auto_exposure.exposure_min: 600.0 # Minimum exposure time (us)
auto_exposure.exposure_max: 2000.0 # Maximum exposure time (us)
auto_exposure.control_interval_ms: 100.0 # Auto exposure control loop interval (ms)
auto_exposure.init_exposure_time: 1000.0 # Initial exposure time for auto exposure mode (us)

3
src/rm_bringup/config/node_params/camera_driver_params.yaml
View File

@@ -1,7 +1,7 @@
/**:
ros__parameters:
camera_info_url: package://rm_bringup/config/camera_info.yaml
exposure_time: 800
exposure_time: 1000
gain: 18.3
resolution_width: 1440
resolution_height: 1080
@@ -9,4 +9,3 @@
offsetY: 0
camera_name: "hik"
recording: false #是否录制
open_delay_ms: 2000 # Camera open delay after previous instance closed (ms)

58
src/rm_hardware_driver/ros2_hik_camera/src/hik_camera_node.cpp
View File

@@ -11,7 +11,6 @@
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <std_msgs/msg/float32.hpp>
#include <string>
#include <thread>
@@ -66,20 +65,6 @@ public:
// Heartbeat
heartbeat_ = fyt::HeartBeatPublisher::create(this);
// Auto exposure subscriber - receives target exposure time from armor detector
auto_exposure_sub_ = this->create_subscription<std_msgs::msg::Float32>(
"auto_exposure/set_exposure", rclcpp::SensorDataQoS(),
[this](const std_msgs::msg::Float32::SharedPtr msg) {
if (msg->data > 0) {
std::lock_guard<std::mutex> lock(camera_mutex_);
exposure_time_ = static_cast<int>(msg->data);
if (camera_handle_ != nullptr) {
(void)MV_CC_SetFloatValue(camera_handle_, "ExposureTime", static_cast<float>(exposure_time_));
FYT_DEBUG("camera_driver", "Auto exposure set exposure_time: {}", exposure_time_);
}
}
});
// Param set callback
params_callback_handle_ = this->add_on_set_parameters_callback(
std::bind(&HikCameraNode::parametersCallback, this, std::placeholders::_1));
@@ -98,10 +83,8 @@ public:
}
// Watch dog timer (also handles initial open)
// Add initial delay to allow previous camera instance to fully release resources
int open_delay_ms = this->declare_parameter("open_delay_ms", 1000);
timer_ = this->create_wall_timer(
std::chrono::milliseconds(open_delay_ms), std::bind(&HikCameraNode::timerCallback, this));
std::chrono::milliseconds(1000), std::bind(&HikCameraNode::timerCallback, this));
// Start capture thread
capture_thread_ = std::thread([this]() { this->captureLoop(); });
@@ -210,29 +193,20 @@ private:
void timerCallback()
{
// Open camera with retry delay
if (!is_open_) {
static int retry_count = 0;
// Open camera
while (!is_open_ && rclcpp::ok()) {
if (!open()) {
retry_count++;
if (retry_count >= 3) {
FYT_ERROR("camera_driver", "open() failed {} times, will keep retrying", retry_count);
}
FYT_ERROR("camera_driver", "open() failed");
close();
std::this_thread::sleep_for(std::chrono::milliseconds(500));
return;
}
retry_count = 0;
is_open_ = true;
}
// Watchdog: no frames for too long => reconnect
double dt = (this->now() - latest_frame_stamp_).seconds();
if (dt > 5.0) {
FYT_WARN("camera_driver", "Camera is not alive! lost frame for {:.2f} seconds, reconnecting...", dt);
FYT_WARN("camera_driver", "Camera is not alive! lost frame for {:.2f} seconds", dt);
close();
is_open_ = false;
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
}
@@ -275,15 +249,6 @@ private:
// Apply ROI/offset before querying image info
applyRoiLocked();
// Force set pixel format to RGB8 every time camera opens
// This ensures camera doesn't fall back to default Mono8
int set_fmt_ret = MV_CC_SetEnumValue(camera_handle_, "PixelFormat", PixelType_Gvsp_RGB8_Packed);
if (set_fmt_ret != MV_OK) {
FYT_WARN("camera_driver", "Failed to set PixelFormat to RGB8: [0x{:x}], will use SDK conversion", set_fmt_ret);
} else {
FYT_INFO("camera_driver", "PixelFormat forced to RGB8");
}
// Set exposure / gain
(void)MV_CC_SetFloatValue(camera_handle_, "ExposureTime", static_cast<float>(exposure_time_));
(void)MV_CC_SetFloatValue(camera_handle_, "Gain", static_cast<float>(gain_));
@@ -378,16 +343,6 @@ private:
continue;
}
// Debug: log pixel format periodically (every 60 frames)
static int frame_count = 0;
frame_count++;
if (frame_count % 60 == 0) {
FYT_INFO("camera_driver", "Frame pixel_type=0x{:x}, size={}x{}",
out_frame.stFrameInfo.enPixelType,
out_frame.stFrameInfo.nWidth,
out_frame.stFrameInfo.nHeight);
}
// Ensure output buffer
const size_t need_size =
static_cast<size_t>(out_frame.stFrameInfo.nWidth) * out_frame.stFrameInfo.nHeight * 3;
@@ -527,9 +482,6 @@ private:
// Heartbeat
fyt::HeartBeatPublisher::SharedPtr heartbeat_;
// Auto exposure subscriber
rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr auto_exposure_sub_;
// Params
int frame_rate_{210};
int exposure_time_{5000};

61
src/rm_tinympc/CMakeLists.txt
View File

@@ -1,61 +0,0 @@
cmake_minimum_required(VERSION 3.8)
project(rm_tinympc)
if(CMAKE_CXX_STANDARD GREATER_RANGE 17)
set(CMAKE_CXX_STANDARD 17)
else()
set(CMAKE_CXX_STANDARD 17)
endif()
set(CMAKE_CXX_STANDARD_REQUIRED ON)
# Find dependencies
find_package(ament_cmake REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(tf2 REQUIRED)
include_directories(
include
${EIGEN3_INCLUDE_DIRS}
)
set(${PROJECT_NAME}_HEADERS
include/rm_tinympc/types.hpp
include/rm_tinympc/admm.hpp
include/rm_tinympc/tiny_api.hpp
include/rm_tinympc/codegen.hpp
include/rm_tinympc/error.hpp
include/rm_tinympc/trajectory.hpp
)
add_library(${PROJECT_NAME} SHARED
src/admm.cpp
src/tiny_api.cpp
src/codegen.cpp
src/trajectory.cpp
)
ament_target_dependencies(${PROJECT_NAME}
Eigen3
tf2
)
target_include_directories(${PROJECT_NAME} PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
install(DIRECTORY include/
DESTINATION include
)
ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME})
ament_package()

162
src/rm_tinympc/include/rm_tinympc/admm.hpp
View File

@@ -1,162 +0,0 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_TINYMPC_ADMM_HPP_
#define RM_TINYMPC_ADMM_HPP_
#include <Eigen/Eigen>
#include <vector>
namespace rm_tinympc {
/**
* ADMM-based MPC Solver (TinyMPC port)
*
* Minimizes: sum_{k=0}^{T-1} (x_k - xref_k)' Q (x_k - xref_k) + u_k' R u_k
* Subject to: x_{k+1} = A x_k + B u_k (dynamics)
* x_min <= x_k <= x_max (state constraints)
* u_min <= u_k <= u_max (input constraints)
*/
class ADMM {
public:
ADMM() = default;
~ADMM() = default;
/**
* Initialize the solver
* @param n State dimension
* @param m Input dimension
* @param T Prediction horizon
*/
void init(int n, int m, int T);
/**
* Set the problem matrices
* @param A State transition matrix (n x n)
* @param B Input matrix (n x m)
* @param Q State cost weight (n x n)
* @param R Input cost weight (m x m)
* @param Qf Terminal cost weight (n x n)
*/
void setProblem(const Eigen::MatrixXd& A,
const Eigen::MatrixXd& B,
const Eigen::VectorXd& Q,
const Eigen::VectorXd& R,
const Eigen::VectorXd& Qf);
/**
* Set state and input constraints
*/
void setConstraints(const Eigen::VectorXd& x_min,
const Eigen::VectorXd& x_max,
const Eigen::VectorXd& u_min,
const Eigen::VectorXd& u_max);
/**
* Set reference trajectory
*/
void setReference(const Eigen::MatrixXd& xref);
/**
* Set initial state
*/
void setInitialState(const Eigen::VectorXd& x0);
/**
* Solve the MPC problem
* @return true if converged, false otherwise
*/
bool solve();
/**
* Get the optimal control input (first input of the sequence)
*/
const Eigen::VectorXd& getFirstInput() const { return u_solution_.col(0); }
/**
* Get the full state trajectory
*/
const Eigen::MatrixXd& getStateTrajectory() const { return x_solution_; }
/**
* Get the full control sequence
*/
const Eigen::MatrixXd& getControlSequence() const { return u_solution_; }
/**
* Configure solver parameters
*/
void setMaxIterations(int max_iter) { max_iterations_ = max_iter; }
void setRho(double rho) { rho_ = rho; }
void setAbsTol(double tol) { abs_tol_ = tol; }
void setRelTol(double tol) { rel_tol_ = tol; }
void setVerbose(bool verbose) { verbose_ = verbose; }
private:
int n_; // State dimension
int m_; // Input dimension
int T_; // Horizon
int max_iterations_ = 100;
double rho_ = 0.1;
double abs_tol_ = 1e-4;
double rel_tol_ = 1e-3;
bool verbose_ = false;
// System matrices
Eigen::MatrixXd A_;
Eigen::MatrixXd B_;
Eigen::VectorXd Q_;
Eigen::VectorXd R_;
Eigen::VectorXd Qf_;
// Constraints
Eigen::VectorXd x_min_;
Eigen::VectorXd x_max_;
Eigen::VectorXd u_min_;
Eigen::VectorXd u_max_;
bool has_constraints_ = false;
// Reference trajectory
Eigen::MatrixXd xref_;
// Initial state
Eigen::VectorXd x0_;
// Solution
Eigen::MatrixXd x_solution_; // n x (T+1)
Eigen::MatrixXd u_solution_; // m x T
// ADMM variables
Eigen::MatrixXd z_; // n x (T+1) - state consensus
Eigen::MatrixXd u_; // m x T - control consensus
Eigen::MatrixXd z_old_; // n x (T+1)
Eigen::MatrixXd u_old_; // m x T
// Lagrange multipliers
Eigen::MatrixXd lambda_x_; // n x (T+1)
Eigen::MatrixXd lambda_u_; // m x T
// Check convergence
bool checkConvergence(const Eigen::MatrixXd& x, const Eigen::MatrixXd& u);
// Proximal operators
Eigen::VectorXd proxGradient(const Eigen::VectorXd& x, const Eigen::VectorXd& grad,
double step, const Eigen::VectorXd& x_min,
const Eigen::VectorXd& x_max);
};
} // namespace rm_tinympc
#endif // RM_TINYMPC_ADMM_HPP_

53
src/rm_tinympc/include/rm_tinympc/codegen.hpp
View File

@@ -1,53 +0,0 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_TINYMPC_CODEGEN_HPP_
#define RM_TINYMPC_CODEGEN_HPP_
#include <Eigen/Eigen>
namespace rm_tinympc {
class CodeGen {
public:
CodeGen() = default;
~CodeGen() = default;
void generateSolverCode(const std::string& output_dir);
static Eigen::Vector2d computeGimbalCommand(
const Eigen::Vector3d& target_pos,
const Eigen::Vector3d& target_vel,
double target_yaw,
double v_yaw,
double bullet_speed,
double gravity);
static Eigen::Vector3d predictTargetPosition(
const Eigen::Vector3d& pos,
const Eigen::Vector3d& vel,
double dt);
static double computeFlyingTime(
const Eigen::Vector3d& target_pos,
double bullet_speed,
double gravity = 9.8);
private:
static constexpr double kPi = 3.14159265358979323846;
};
} // namespace rm_tinympc
#endif // RM_TINYMPC_CODEGEN_HPP_

40
src/rm_tinympc/include/rm_tinympc/error.hpp
View File

@@ -1,40 +0,0 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_TINYMPC_ERROR_HPP_
#define RM_TINYMPC_ERROR_HPP_
#include <stdexcept>
#include <string>
namespace rm_tinympc {
class TinyMPCException : public std::runtime_error {
public:
explicit TinyMPCException(const std::string& msg) : std::runtime_error(msg) {}
};
class ConvergenceError : public TinyMPCException {
public:
explicit ConvergenceError(const std::string& msg) : TinyMPCException(msg) {}
};
class InvalidSizeError : public TinyMPCException {
public:
explicit InvalidSizeError(const std::string& msg) : TinyMPCException(msg) {}
};
} // namespace rm_tinympc
#endif // RM_TINYMPC_ERROR_HPP_

71
src/rm_tinympc/include/rm_tinympc/tiny_api.hpp
View File

@@ -1,71 +0,0 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_TINYMPC_TINY_API_HPP_
#define RM_TINYMPC_TINY_API_HPP_
#include <Eigen/Eigen>
#include "types.hpp"
namespace rm_tinympc {
class TinyMPC {
public:
TinyMPC() = default;
~TinyMPC() = default;
void init(int n, int m, int T);
void setProblem(
const Eigen::MatrixXd& A,
const Eigen::MatrixXd& B,
const Eigen::VectorXd& x0,
const Eigen::VectorXd& xref,
const Eigen::VectorXd& uref);
void setWeights(const Eigen::VectorXd& Q, const Eigen::VectorXd& R, const Eigen::VectorXd& Qf);
void solve();
const Eigen::VectorXd& getSolution() const { return u_solution_; }
const Eigen::MatrixXd& getFullSolution() const { return x_solution_; }
void setMaxIterations(int max_iter) { max_iterations_ = max_iter; }
void setRho(double rho) { rho_ = rho; }
private:
int n_; // state dimension
int m_; // input dimension
int T_; // horizon
int max_iterations_ = 100;
double rho_ = 0.1;
Eigen::MatrixXd A_;
Eigen::MatrixXd B_;
Eigen::VectorXd x0_;
Eigen::VectorXd xref_;
Eigen::VectorXd uref_;
Eigen::VectorXd Q_;
Eigen::VectorXd R_;
Eigen::VectorXd Qf_;
Eigen::MatrixXd x_solution_;
Eigen::VectorXd u_solution_;
};
} // namespace rm_tinympc
#endif // RM_TINYMPC_TINY_API_HPP_

165
src/rm_tinympc/include/rm_tinympc/trajectory.hpp
View File

@@ -1,165 +0,0 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_TINYMPC_TRAJECTORY_HPP_
#define RM_TINYMPC_TRAJECTORY_HPP_
#include <Eigen/Eigen>
namespace rm_tinympc {
/**
* Quintic Polynomial Trajectory Generator
*
* Generates smooth trajectories from start state to goal state
* with specified boundary velocities and accelerations.
*
* Coefficients: a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5
*/
class QuinticPolynomial {
public:
QuinticPolynomial() = default;
~QuinticPolynomial() = default;
/**
* Compute quintic polynomial coefficients
* @param p0 Initial position
* @param v0 Initial velocity
* @param a0 Initial acceleration
* @param pf Final position
* @param vf Final velocity
* @param af Final acceleration
* @param T Trajectory duration
*/
void compute(double p0, double v0, double a0,
double pf, double vf, double af, double T);
/**
* Evaluate position at time t
*/
double evaluate(double t) const;
/**
* Evaluate velocity at time t
*/
double evaluateVelocity(double t) const;
/**
* Evaluate acceleration at time t
*/
double evaluateAcceleration(double t) const;
/**
* Get polynomial coefficients
*/
const Eigen::Vector6d& getCoefficients() const { return coef_; }
private:
Eigen::Vector6d coef_; // a0, a1, a2, a3, a4, a5
};
/**
* Trajectory Point with position, velocity, acceleration
*/
struct TrajectoryPoint1D {
double t; // Time
double p; // Position
double v; // Velocity
double a; // Acceleration
};
/**
* 2D Trajectory Point (yaw, pitch)
*/
struct TrajectoryPoint2D {
double t;
double yaw;
double pitch;
double yaw_vel;
double pitch_vel;
double yaw_acc;
double pitch_acc;
};
/**
* Generate smooth 1D trajectory with quintic polynomial
*/
class TrajectoryGenerator1D {
public:
TrajectoryGenerator1D() = default;
~TrajectoryGenerator1D() = default;
/**
* Generate trajectory from start to goal
* @param p0 Initial position
* @param pf Final position
* @param max_v Maximum velocity (for constraint)
* @param max_a Maximum acceleration (for constraint)
* @param T Total duration
*/
std::vector<TrajectoryPoint1D> generate(double p0, double pf,
double max_v, double max_a, double T);
/**
* Generate minimum time trajectory with velocity/acceleration constraints
*/
double generateMinTime(double p0, double pf, double max_v, double max_a);
private:
double T_ = 0.0;
QuinticPolynomial poly_;
};
/**
* 2D Gimbal Trajectory Generator (yaw and pitch simultaneously)
*/
class GimbalTrajectoryGenerator {
public:
GimbalTrajectoryGenerator() = default;
~GimbalTrajectoryGenerator() = default;
/**
* Generate 2D gimbal trajectory
* @param yaw0 Initial yaw
* @param pitch0 Initial pitch
* @param yawf Target yaw
* @param pitchf Target pitch
* @param max_yaw_rate Maximum yaw rate (rad/s)
* @param max_pitch_rate Maximum pitch rate (rad/s)
* @param max_yaw_acc Maximum yaw acceleration (rad/s^2)
* @param max_pitch_acc Maximum pitch acceleration (rad/s^2)
* @param T Trajectory duration
*/
std::vector<TrajectoryPoint2D> generate(double yaw0, double pitch0,
double yawf, double pitchf,
double max_yaw_rate, double max_pitch_rate,
double max_yaw_acc, double max_pitch_acc,
double T);
/**
* Minimum time trajectory with constraints
*/
double generateMinTime(double yaw0, double pitch0,
double yawf, double pitchf,
double max_yaw_rate, double max_pitch_rate,
double max_yaw_acc, double max_pitch_acc);
private:
QuinticPolynomial poly_yaw_;
QuinticPolynomial poly_pitch_;
};
} // namespace rm_tinympc
#endif // RM_TINYMPC_TRAJECTORY_HPP_

37
src/rm_tinympc/include/rm_tinympc/types.hpp
View File

@@ -1,37 +0,0 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_TINYMPC_TYPES_HPP_
#define RM_TINYMPC_TYPES_HPP_
#include <Eigen/Eigen>
namespace rm_tinympc {
using vec2 = Eigen::Vector2d;
using vec3 = Eigen::Vector3d;
using vec4 = Eigen::Vector4d;
using mat2 = Eigen::Matrix2d;
using mat3 = Eigen::Matrix3d;
using mat4 = Eigen::Matrix4d;
struct TrajectoryPoint {
double yaw;
double pitch;
double t;
};
} // namespace rm_tinympc
#endif // RM_TINYMPC_TYPES_HPP_

19
src/rm_tinympc/package.xml
View File

@@ -1,19 +0,0 @@
<?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>rm_tinympc</name>
<version>0.1.0</version>
<description>TinyMPC - ADMM-based small MPC solver</description>
<maintainer email="chenyouyuan@foxmail.com">Chen Youyuan</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>ament_cmake_python</buildtool_depend>
<depend>Eigen3</depend>
<depend>tf2</depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

204
src/rm_tinympc/src/admm.cpp
View File

@@ -1,204 +0,0 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "rm_tinympc/admm.hpp"
#include <iostream>
#include <cmath>
namespace rm_tinympc {
void ADMM::init(int n, int m, int T) {
n_ = n;
m_ = m;
T_ = T;
// Allocate solution matrices
x_solution_.resize(n_, T_ + 1);
u_solution_.resize(m_, T_);
// ADMM variables
z_.resize(n_, T_ + 1);
u_.resize(m_, T_);
z_old_.resize(n_, T_ + 1);
u_old_.resize(m_, T_);
lambda_x_.resize(n_, T_ + 1);
lambda_u_.resize(m_, T_);
x_solution_.setZero();
u_solution_.setZero();
z_.setZero();
u_.setZero();
lambda_x_.setZero();
lambda_u_.setZero();
}
void ADMM::setProblem(const Eigen::MatrixXd& A,
const Eigen::MatrixXd& B,
const Eigen::VectorXd& Q,
const Eigen::VectorXd& R,
const Eigen::VectorXd& Qf) {
A_ = A;
B_ = B;
Q_ = Q;
R_ = R;
Qf_ = Qf;
}
void ADMM::setConstraints(const Eigen::VectorXd& x_min,
const Eigen::VectorXd& x_max,
const Eigen::VectorXd& u_min,
const Eigen::VectorXd& u_max) {
x_min_ = x_min;
x_max_ = x_max;
u_min_ = u_min;
u_max_ = u_max;
has_constraints_ = true;
}
void ADMM::setReference(const Eigen::MatrixXd& xref) {
xref_ = xref;
}
void ADMM::setInitialState(const Eigen::VectorXd& x0) {
x0_ = x0;
x_solution_.col(0) = x0;
z_.col(0) = x0;
}
bool ADMM::solve() {
if (xref_.rows() != n_ || xref_.cols() != T_ + 1) {
return false;
}
// Initialize state trajectory by forward simulation
x_solution_.col(0) = x0_;
for (int t = 0; t < T_; ++t) {
x_solution_.col(t + 1) = A_ * x_solution_.col(t) + B_ * u_solution_.col(t);
}
double primal_res_x = 0.0, primal_res_u = 0.0;
double dual_res_x = 0.0, dual_res_u = 0.0;
for (int iter = 0; iter < max_iterations_; ++iter) {
z_old_ = z_;
u_old_ = u_;
// === X-update (state update) ===
// Solve: min_{x} rho/2 * ||x - z + lambda||^2 + sum (x_k - xref_k)' Q (x_k - xref_k)
// Subject to: x_{k+1} = A x_k + B u_k
for (int t = 0; t <= T_; ++t) {
Eigen::VectorXd x_t = z_.col(t) - lambda_x_.col(t) / rho_;
if (t == 0) {
// Initial state constraint
x_t = x0_;
} else if (t <= T_) {
// Cost gradient: 2 * Q * (x_t - xref_t)
Eigen::VectorXd grad = Q_.asDiagonal() * (x_t - xref_.col(t));
x_t = x_t - (1.0 / (rho_ + 2.0 * Q_(t % Q_.size()))) * grad;
}
// Apply state constraints
if (has_constraints_ && t > 0) {
x_t = x_t.cwiseMax(x_min_).cwiseMin(x_max_);
}
z_.col(t) = x_t;
}
// === U-update (control update) ===
// Solve: min_{u} rho/2 * ||u - v + lambda||^2 + sum u_k' R u_k
for (int t = 0; t < T_; ++t) {
// Compute the implied state from previous state and control
Eigen::VectorXd x_implied = A_ * x_solution_.col(t) + B_ * u_solution_.col(t);
// Gradient of cost w.r.t. u: 2 * R * u_k + B' * lambda_u
Eigen::VectorXd grad_u = (2.0 * R_.asDiagonal() * u_solution_.col(t)
+ B_.transpose() * lambda_u_.col(t)) / rho_;
Eigen::VectorXd u_t = u_solution_.col(t) - grad_u;
// Apply control constraints
if (has_constraints_) {
u_t = u_t.cwiseMax(u_min_).cwiseMin(u_max_);
}
u_.col(t) = u_t;
// Update state trajectory using new control
if (t < T_) {
x_solution_.col(t + 1) = A_ * x_solution_.col(t) + B_ * u_t;
}
}
// === Lagrange multiplier update ===
// lambda_x += rho * (x_solution - z_)
// lambda_u += rho * (u_solution - u_)
lambda_x_ += rho_ * (x_solution_ - z_);
lambda_u_ += rho_ * (u_solution_ - u_);
// === Check convergence ===
if (checkConvergence(x_solution_, u_solution_)) {
if (verbose_) {
std::cout << "ADMM converged at iteration " << iter << std::endl;
}
return true;
}
// === Adaptive rho adjustment (optional) ===
// Increase rho if primal residual is large, decrease if dual residual is large
// This is simplified for TinyMPC
// Update solution for next iteration
z_ = x_solution_;
u_ = u_solution_;
}
if (verbose_) {
std::cout << "ADMM reached max iterations " << max_iterations_ << std::endl;
}
return false;
}
bool ADMM::checkConvergence(const Eigen::MatrixXd& x, const Eigen::MatrixXd& u) {
// Compute primal residuals
double eps_pri_x = 0.0, eps_pri_u = 0.0;
double eps_dual_x = 0.0, eps_dual_u = 0.0;
// Primal residual for x: ||x - z||
for (int t = 0; t <= T_; ++t) {
eps_pri_x += (x.col(t) - z_.col(t)).squaredNorm();
eps_dual_x += (rho_ * (z_.col(t) - z_old_.col(t))).squaredNorm();
}
// Primal residual for u: ||u - u_old||
for (int t = 0; t < T_; ++t) {
eps_pri_u += (u.col(t) - u_.col(t)).squaredNorm();
eps_dual_u += (rho_ * (u_.col(t) - u_old_.col(t))).squaredNorm();
}
double eps_pri = std::sqrt(eps_pri_x + eps_pri_u);
double eps_dual = std::sqrt(eps_dual_x + eps_dual_u);
// Compute tolerance
double tol_pri = std::sqrt(static_cast<double>((T_ + 1) * n_ + T_ * m_)) * abs_tol_
+ rel_tol_ * std::max(std::sqrt(eps_pri_x), std::sqrt(eps_dual_x));
double tol_dual = std::sqrt(static_cast<double>((T_ + 1) * n_ + T_ * m_)) * abs_tol_
+ rel_tol_ * std::sqrt(eps_dual_x + eps_dual_u);
return (eps_pri < tol_pri * tol_pri) && (eps_dual < tol_dual * tol_dual);
}
} // namespace rm_tinympc

56
src/rm_tinympc/src/codegen.cpp
View File

@@ -1,56 +0,0 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "rm_tinympc/codegen.hpp"
#include <cmath>
namespace rm_tinympc {
Eigen::Vector2d CodeGen::computeGimbalCommand(
const Eigen::Vector3d& target_pos,
const Eigen::Vector3d& target_vel,
double target_yaw,
double v_yaw,
double bullet_speed,
double gravity) {
// Simplified trajectory compensation
double dist = target_pos.norm();
double flying_time = dist / bullet_speed;
// Predict target position
Eigen::Vector3d predicted_pos = target_pos + flying_time * target_vel;
// Compute gimbal angles
double yaw = std::atan2(predicted_pos.y(), predicted_pos.x());
double pitch = std::atan2(predicted_pos.z(), predicted_pos.head(2).norm());
return Eigen::Vector2d(yaw, pitch);
}
Eigen::Vector3d CodeGen::predictTargetPosition(
const Eigen::Vector3d& pos,
const Eigen::Vector3d& vel,
double dt) {
return pos + dt * vel;
}
double CodeGen::computeFlyingTime(
const Eigen::Vector3d& target_pos,
double bullet_speed,
double gravity) {
double dist = target_pos.norm();
return dist / bullet_speed;
}
} // namespace rm_tinympc

71
src/rm_tinympc/src/tiny_api.cpp
View File

@@ -1,71 +0,0 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "rm_tinympc/tiny_api.hpp"
namespace rm_tinympc {
void TinyMPC::init(int n, int m, int T) {
n_ = n;
m_ = m;
T_ = T;
x_solution_.resize((T + 1) * n);
u_solution_.resize(T * m);
x_solution_.setZero();
u_solution_.setZero();
}
void TinyMPC::setProblem(
const Eigen::MatrixXd& A,
const Eigen::MatrixXd& B,
const Eigen::VectorXd& x0,
const Eigen::VectorXd& xref,
const Eigen::VectorXd& uref) {
A_ = A;
B_ = B;
x0_ = x0;
xref_ = xref;
uref_ = uref;
}
void TinyMPC::setWeights(const Eigen::VectorXd& Q, const Eigen::VectorXd& R, const Eigen::VectorXd& Qf) {
Q_ = Q;
R_ = R;
Qf_ = Qf;
}
void TinyMPC::solve() {
// Initialize with reference trajectory
x_solution_.segment(0, n_) = x0_;
// Simplified forward simulation using the dynamics
for (int t = 0; t < T_; ++t) {
int x_idx = t * n_;
int u_idx = t * m_;
// Use reference control if not set
if (t * m_ < uref_.size()) {
u_solution_.segment(u_idx, m_) = uref_.segment(u_idx, m_);
}
// Simulate dynamics: x_{t+1} = A * x_t + B * u_t
if ((t + 1) * n_ < x_solution_.size()) {
x_solution_.segment((t + 1) * n_, n_) = A_ * x_solution_.segment(x_idx, n_) +
B_ * u_solution_.segment(u_idx, m_);
}
}
}
} // namespace rm_tinympc

163
src/rm_tinympc/src/trajectory.cpp
View File

@@ -1,163 +0,0 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "rm_tinympc/trajectory.hpp"
#include <cmath>
namespace rm_tinympc {
void QuinticPolynomial::compute(double p0, double v0, double a0,
double pf, double vf, double af, double T) {
T_ = T;
double T2 = T * T;
double T3 = T2 * T;
double T4 = T3 * T;
double T5 = T4 * T;
// Solve for coefficients using boundary conditions
// p(0) = p0, p(T) = pf
// v(0) = v0, v(T) = vf
// a(0) = a0, a(T) = af
Eigen::Matrix3d A;
A << T3, T4, T5,
3 * T2, 4 * T3, 5 * T4,
6 * T, 12 * T2, 20 * T3;
Eigen::Vector3d b;
b << pf - p0 - v0 * T - 0.5 * a0 * T2,
vf - v0 - a0 * T,
af - a0;
Eigen::Vector3d x = A.colPivHouseholderQr().solve(b);
coef_ << p0, v0, 0.5 * a0, x[0], x[1], x[2];
}
double QuinticPolynomial::evaluate(double t) const {
double t2 = t * t;
double t3 = t2 * t;
double t4 = t3 * t;
double t5 = t4 * t;
return coef_[0] + coef_[1] * t + coef_[2] * t2 + coef_[3] * t3 + coef_[4] * t4 + coef_[5] * t5;
}
double QuinticPolynomial::evaluateVelocity(double t) const {
double t2 = t * t;
double t3 = t2 * t;
double t4 = t3 * t;
return coef_[1] + 2 * coef_[2] * t + 3 * coef_[3] * t2 + 4 * coef_[4] * t3 + 5 * coef_[5] * t4;
}
double QuinticPolynomial::evaluateAcceleration(double t) const {
double t2 = t * t;
double t3 = t2 * t;
return 2 * coef_[2] + 6 * coef_[3] * t + 12 * coef_[4] * t2 + 20 * coef_[5] * t3;
}
std::vector<TrajectoryPoint1D> TrajectoryGenerator1D::generate(
double p0, double pf, double max_v, double max_a, double T) {
std::vector<TrajectoryPoint1D> trajectory;
// Compute quintic polynomial
poly_.compute(p0, 0, 0, pf, 0, 0, T);
// Sample trajectory
const int num_samples = static_cast<int>(T * 100); // 100 Hz
for (int i = 0; i <= num_samples; ++i) {
double t = static_cast<double>(i) / num_samples * T;
TrajectoryPoint1D pt;
pt.t = t;
pt.p = poly_.evaluate(t);
pt.v = poly_.evaluateVelocity(t);
pt.a = poly_.evaluateAcceleration(t);
trajectory.push_back(pt);
}
return trajectory;
}
double TrajectoryGenerator1D::generateMinTime(
double p0, double pf, double max_v, double max_a) {
double d = std::abs(pf - p0);
// Minimum time for bang-bang acceleration profile
// t_acc = max_v / max_a
// d_acc = 0.5 * max_a * t_acc^2 = max_v^2 / (2 * max_a)
// If distance < 2 * d_acc, we're limited by acceleration
// If distance >= 2 * d_acc, we can reach max_v
double t_acc = max_v / max_a;
double d_acc = 0.5 * max_a * t_acc * t_acc; // Distance for acceleration
double d_total = 2 * d_acc; // Distance for accel + decel
if (d <= d_total) {
// Triangle profile: can't reach max_v
// d = 0.5 * max_a * t^2 for triangle
T_ = std::sqrt(4 * d / max_a);
} else {
// Trapezoidal profile: reach max_v
double d_cruise = d - d_total;
double t_cruise = d_cruise / max_v;
T_ = 2 * t_acc + t_cruise;
}
return T_;
}
std::vector<TrajectoryPoint2D> GimbalTrajectoryGenerator::generate(
double yaw0, double pitch0, double yawf, double pitchf,
double max_yaw_rate, double max_pitch_rate,
double max_yaw_acc, double max_pitch_acc, double T) {
std::vector<TrajectoryPoint2D> trajectory;
// Compute quintic polynomials for yaw and pitch
poly_yaw_.compute(yaw0, 0, 0, yawf, 0, 0, T);
poly_pitch_.compute(pitch0, 0, 0, pitchf, 0, 0, T);
// Sample trajectory
const int num_samples = static_cast<int>(T * 100); // 100 Hz
for (int i = 0; i <= num_samples; ++i) {
double t = static_cast<double>(i) / num_samples * T;
TrajectoryPoint2D pt;
pt.t = t;
pt.yaw = poly_yaw_.evaluate(t);
pt.yaw_vel = poly_yaw_.evaluateVelocity(t);
pt.yaw_acc = poly_yaw_.evaluateAcceleration(t);
pt.pitch = poly_pitch_.evaluate(t);
pt.pitch_vel = poly_pitch_.evaluateVelocity(t);
pt.pitch_acc = poly_pitch_.evaluateAcceleration(t);
trajectory.push_back(pt);
}
return trajectory;
}
double GimbalTrajectoryGenerator::generateMinTime(
double yaw0, double pitch0, double yawf, double pitchf,
double max_yaw_rate, double max_pitch_rate,
double max_yaw_acc, double max_pitch_acc) {
TrajectoryGenerator1D yaw_gen, pitch_gen;
double T_yaw = yaw_gen.generateMinTime(yaw0, yawf, max_yaw_rate, max_yaw_acc);
double T_pitch = pitch_gen.generateMinTime(pitch0, pitchf, max_pitch_rate, max_pitch_acc);
// Use the maximum of the two times to ensure both constraints are satisfied
T_ = std::max(T_yaw, T_pitch);
return T_;
}
} // namespace rm_tinympc

2
src/rm_upstart/rm_clean_up.sh
View File

@@ -6,7 +6,7 @@ export ROS_LOG_DIR="/tmp"
source /opt/ros/humble/setup.bash
pkill rm_watch_dog.sh # 杀掉看门狗
PID_FILE="$HOME/yq_2026/.rm_bringup.pid"
PID_FILE="$HOME/yq_2026_duo/.rm_bringup.pid"
kill_process_tree() {
local pid="$1"

2
src/rm_upstart/rm_watch_dog.sh
View File

@@ -8,7 +8,7 @@ CAMERA_NODE_NAME="hik_camera" # 相机节点默认名（视频模式会改为 v
NODE_NAMES=()
USER="$(whoami)" #用户名
HOME_DIR=$(eval echo ~$USER)
WORKING_DIR="$HOME_DIR/yq_2026" # 代码目录
WORKING_DIR="$HOME_DIR/yq_2026_duo" # 代码目录
LAUNCH_FILE="rm_bringup bringup.launch.py" # launch 文件
OUTPUT_FILE="$WORKING_DIR/screen.output" # 终端输出记录文件
PID_FILE="$WORKING_DIR/.rm_bringup.pid" # 记录由看门狗启动的 bringup 主进程 PID

179
src/rm_utils/include/rm_utils/math/error_state_kalman_filter.hpp
View File

@@ -1,179 +0,0 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef ERROR_STATE_KALMAN_FILTER_HPP_
#define ERROR_STATE_KALMAN_FILTER_HPP_
#include <Eigen/Eigen>
#include <functional>
namespace fyt {
template <int X_N, int Z_N, typename PredictModel, typename MeasureModel>
class ErrorStateKalmanFilter {
public:
using StateVector = Eigen::Vector<double, X_N>;
using MeasurementVector = Eigen::Vector<double, Z_N>;
using StateMatrix = Eigen::Matrix<double, X_N, X_N>;
using MeasurementMatrix = Eigen::Matrix<double, Z_N, Z_N>;
using CrossCovarianceMatrix = Eigen::Matrix<double, X_N, Z_N>;
using JacobianMatrix = Eigen::Matrix<double, X_N, X_N>;
ErrorStateKalmanFilter() = default;
void init(const StateVector& initial_state,
const StateMatrix& initial_covariance,
const MeasurementMatrix& measurement_noise,
const StateMatrix& process_noise) {
state_ = initial_state;
covariance_ = initial_covariance;
R_ = measurement_noise;
Q_ = process_noise;
}
void setNoiseMatrices(const MeasurementMatrix& R, const StateMatrix& Q) {
R_ = R;
Q_ = Q;
}
void setResidualFunc(std::function<MeasurementVector(const MeasurementVector&,
const MeasurementVector&)> func) {
residual_func_ = func;
}
void setInjectFunc(std::function<void(StateVector&, const StateVector&)> func) {
inject_func_ = func;
}
StateVector predict(double dt) {
PredictModel predict_model(dt);
// State prediction using the motion model
StateVector predicted_state = state_;
predict_model(state_.data(), predicted_state.data());
// Jacobian of the motion model w.r.t. state
JacobianMatrix F = JacobianMatrix::Identity();
// This is a simplified Jacobian - for complex models, numerical differentiation
// or analytical Jacobians should be used
// For now, we use identity as the motion is linear in velocity terms
// Covariance prediction
covariance_ = F * covariance_ * F.transpose() + Q_;
state_ = predicted_state;
return state_;
}
StateVector update(const MeasurementVector& measurement) {
MeasurementVector z_predicted;
MeasureModel measure_model;
measure_model(state_.data(), z_predicted.data());
// Innovation (measurement residual)
MeasurementVector y;
if (residual_func_) {
y = residual_func_(measurement, z_predicted);
} else {
y = measurement - z_predicted;
}
// Innovation covariance
MeasurementMatrix S = measurement_jacobian_ * covariance_ * measurement_jacobian_.transpose() + R_;
// Kalman gain
CrossCovarianceMatrix Pxz = covariance_ * measurement_jacobian_.transpose();
Eigen::Matrix<double, X_N, Z_N> K = Pxz * S.inverse();
// State update
StateVector delta_x = K * y;
state_ = state_ + delta_x;
// Covariance update (Joseph form for numerical stability)
Eigen::Matrix<double, X_N, X_N> I_KH = JacobianMatrix::Identity() - K * measurement_jacobian_;
covariance_ = I_KH * covariance_ * I_KH.transpose() + K * R_ * K.transpose();
return state_;
}
StateVector updateIterative(const MeasurementVector& measurement, int max_iterations = 10) {
StateVector updated_state = state_;
MeasurementMatrix H = measurement_jacobian_;
for (int i = 0; i < max_iterations; ++i) {
MeasurementVector z_predicted;
MeasureModel measure_model;
measure_model(updated_state.data(), z_predicted.data());
MeasurementVector y;
if (residual_func_) {
y = residual_func_(measurement, z_predicted);
} else {
y = measurement - z_predicted;
}
MeasurementMatrix S = H * covariance_ * H.transpose() + R_;
Eigen::Matrix<double, X_N, Z_N> K = covariance_ * H.transpose() * S.inverse();
StateVector delta_x = K * y;
updated_state = updated_state + delta_x;
if (delta_x.norm() < 1e-6) {
break;
}
}
if (inject_func_) {
inject_func_(state_, updated_state - state_);
}
state_ = updated_state;
return state_;
}
void setState(const StateVector& state) {
state_ = state;
}
StateVector getState() const {
return state_;
}
StateMatrix getCovariance() const {
return covariance_;
}
double getNIS() const {
return last_nis_;
}
void setMeasurementJacobian(const MeasurementMatrix& H) {
measurement_jacobian_ = H;
}
private:
StateVector state_;
StateMatrix covariance_;
MeasurementMatrix R_; // Measurement noise covariance
StateMatrix Q_; // Process noise covariance
MeasurementMatrix measurement_jacobian_;
double last_nis_ = 0;
std::function<MeasurementVector(const MeasurementVector&, const MeasurementVector&)> residual_func_;
std::function<void(StateVector&, const StateVector&)> inject_func_;
};
} // namespace fyt
#endif // ERROR_STATE_KALMAN_FILTER_HPP_

75
src/rm_utils/include/rm_utils/math/extended_kalman_filter.hpp
View File

@@ -48,8 +48,6 @@ public:
using UpdateQFunc = std::function<MatrixXX()>;
using UpdateRFunc = std::function<MatrixZZ(const MatrixZ1 &z)>;
// Residual function for handling angle wraparound (e.g., -pi~pi)
using ResidualFunc = std::function<MatrixZ1(const MatrixZ1 &z_meas, const MatrixZ1 &z_pred)>;
explicit ExtendedKalmanFilter(const PredicFunc &f,
const MeasureFunc &h,
@@ -68,14 +66,6 @@ public:
void setMeasureFunc(const MeasureFunc &h) noexcept { this->h = h; }
// Set residual function for handling angle wraparound
void setResidualFunc(const ResidualFunc &residual_func) noexcept {
residual_func_ = residual_func;
}
// Check if residual function is set
bool hasResidualFunc() const noexcept { return residual_func_ != nullptr; }
// Compute a predicted state
MatrixX1 predict() noexcept {
ceres::Jet<double, N_X> x_e_jet[N_X];
@@ -116,66 +106,14 @@ public:
}
R = update_R(z);
// Compute innovation (measurement residual)
// Use residual_func if set to handle angle wraparound
if (residual_func_ != nullptr) {
innovation_ = residual_func_(z, z_pri_);
} else {
innovation_ = z - z_pri_;
}
S_ = H * P_pri * H.transpose() + R;
K = P_pri * H.transpose() * S_.inverse();
innovation_ = z - z_pri_;
x_post = x_post + K * innovation_;
P_post = (MatrixXX::Identity() - K * H) * P_pri;
return x_post;
}
// Update with iterative measurement update for better angle handling
MatrixX1 updateIterative(const MatrixZ1 &z, int max_iterations = 3) noexcept {
MatrixX1 updated_state = x_post;
for (int iter = 0; iter < max_iterations; ++iter) {
ceres::Jet<double, N_X> x_jet[N_X];
for (int i = 0; i < N_X; i++) {
x_jet[i].a = updated_state[i];
x_jet[i].v[i] = 1;
}
ceres::Jet<double, N_X> z_jet[N_Z];
h(x_jet, z_jet);
MatrixZ1 z_pred;
for (int i = 0; i < N_Z; i++) {
z_pred[i] = z_jet[i].a;
}
R = update_R(z);
MatrixZ1 innovation;
if (residual_func_ != nullptr) {
innovation = residual_func_(z, z_pred);
} else {
innovation = z - z_pred;
}
MatrixZZ S = H * P_pri * H.transpose() + R;
MatrixXZ K = P_pri * H.transpose() * S.inverse();
MatrixX1 delta_x = K * innovation;
updated_state = updated_state + delta_x;
// Check convergence
if (delta_x.norm() < 1e-6) {
break;
}
}
x_post = updated_state;
return x_post;
}
// Get innovation (z - z_pri)
const MatrixZ1 & getInnovation() const { return innovation_; }
@@ -188,15 +126,6 @@ public:
return innovation_.transpose() * S_.inverse() * innovation_;
}
// Get Kalman gain
const MatrixXZ & getKalmanGain() const { return K; }
// Get posterior state
const MatrixX1 & getState() const { return x_post; }
// Get posterior covariance
const MatrixXX & getCovariance() const { return P_post; }
private:
// Process nonlinear vector function
PredicFunc f;
@@ -210,8 +139,6 @@ private:
// Measurement noise covariance matrix
UpdateRFunc update_R;
MatrixZZ R;
// Residual function for angle wraparound handling
ResidualFunc residual_func_;
// Priori error estimate covariance matrix
MatrixXX P_pri;

72
wust_vision-main/.clang-format Normal file
View File

@@ -0,0 +1,72 @@
AccessModifierOffset: -4
AlignAfterOpenBracket: BlockIndent
AlignConsecutiveMacros: false
AlignConsecutiveAssignments: false
AlignConsecutiveDeclarations: false
AlignEscapedNewlines: DontAlign
AlignOperands: false
AlignTrailingComments: false
AllowAllArgumentsOnNextLine: false
AllowAllConstructorInitializersOnNextLine: false
AllowAllParametersOfDeclarationOnNextLine: false
AllowShortBlocksOnASingleLine: Empty
AllowShortCaseLabelsOnASingleLine: false
AllowShortFunctionsOnASingleLine: Empty
AllowShortIfStatementsOnASingleLine: Never
AllowShortLambdasOnASingleLine: All
AllowShortLoopsOnASingleLine: false
AlwaysBreakAfterReturnType: None
AlwaysBreakBeforeMultilineStrings: true
AlwaysBreakTemplateDeclarations: Yes
BinPackArguments: false
BinPackParameters: false
BreakBeforeBinaryOperators: NonAssignment
BreakBeforeBraces: Custom
BreakBeforeTernaryOperators: true
BraceWrapping:
AfterControlStatement: MultiLine
AfterEnum: false
AfterStruct: false
SplitEmptyFunction: false
BreakConstructorInitializers: AfterColon
BreakInheritanceList: AfterColon
BreakStringLiterals: false
ColumnLimit: 100
CompactNamespaces: false
ConstructorInitializerAllOnOneLineOrOnePerLine: true
ConstructorInitializerIndentWidth: 4
ContinuationIndentWidth: 4
Cpp11BracedListStyle: false
DerivePointerAlignment: false
FixNamespaceComments: true
IncludeBlocks: Preserve
IncludeIsMainRegex: '([-_](test|unittest))?$'
IndentCaseLabels: true
IndentPPDirectives: BeforeHash
IndentWidth: 4
IndentWrappedFunctionNames: false
KeepEmptyLinesAtTheStartOfBlocks: false
MaxEmptyLinesToKeep: 1
NamespaceIndentation: Inner
PointerAlignment: Left
ReflowComments: false
SortIncludes: true
SortUsingDeclarations: true
SpaceAfterCStyleCast: false
SpaceAfterLogicalNot: false
SpaceAfterTemplateKeyword: false
SpaceBeforeAssignmentOperators: true
SpaceBeforeCpp11BracedList: true
SpaceBeforeCtorInitializerColon: false
SpaceBeforeInheritanceColon: false
SpaceBeforeParens: ControlStatements
SpaceBeforeRangeBasedForLoopColon: false
SpaceInEmptyParentheses: false
SpacesBeforeTrailingComments: 1
SpacesInAngles: false
SpacesInCStyleCastParentheses: false
SpacesInContainerLiterals: false
SpacesInParentheses: false
SpacesInSquareBrackets: false
Standard: Cpp11
UseTab: Never

6
wust_vision-main/.clangd Normal file
View File

@@ -0,0 +1,6 @@
Diagnostics:
Suppress:
- drv_unknown_argument
CompileFlags:
Remove: [-forward-unknown-to-host-compiler, --generate-code=*, -Xcompiler=*]

31
wust_vision-main/.gitignore vendored Normal file
View File

@@ -0,0 +1,31 @@
build
devel
install
log/*
.catkin_workspace
.vscode
.cache
__pycache__
*~
.DS_Store
*.pcd
*.gv
*.pdf
bin
model/at.onnx
model/at.engine

6
wust_vision-main/.gitmodules vendored Normal file
View File

@@ -0,0 +1,6 @@
[submodule "KalmanHyLib"]
path = KalmanHyLib
url = https://github.com/hyheiyue/KalmanHyLib.git
[submodule "3rdparty/backward-cpp"]
path = 3rdparty/backward-cpp
url = https://github.com/bombela/backward-cpp.git

373
wust_vision-main/3rdparty/angles.h vendored Normal file
View File

@@ -0,0 +1,373 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef GEOMETRY_ANGLES_UTILS_H
#define GEOMETRY_ANGLES_UTILS_H
#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif
#include <algorithm>
#include <cmath>
namespace angles {
/*!
* \brief Convert degrees to radians
*/
static inline double from_degrees(double degrees) {
return degrees * M_PI / 180.0;
}
/*!
* \brief Convert radians to degrees
*/
static inline double to_degrees(double radians) {
return radians * 180.0 / M_PI;
}
/*!
* \brief normalize_angle_positive
*
* Normalizes the angle to be 0 to 2*M_PI
* It takes and returns radians.
*/
static inline double normalize_angle_positive(double angle) {
const double result = fmod(angle, 2.0 * M_PI);
if (result < 0)
return result + 2.0 * M_PI;
return result;
}
/*!
* \brief normalize
*
* Normalizes the angle to be -M_PI circle to +M_PI circle
* It takes and returns radians.
*
*/
static inline double normalize_angle(double angle) {
const double result = fmod(angle + M_PI, 2.0 * M_PI);
if (result <= 0.0)
return result + M_PI;
return result - M_PI;
}
/*!
* \function
* \brief shortest_angular_distance
*
* Given 2 angles, this returns the shortest angular
* difference. The inputs and ouputs are of course radians.
*
* The result
* would always be -pi <= result <= pi. Adding the result
* to "from" will always get you an equivelent angle to "to".
*/
static inline double shortest_angular_distance(double from, double to) {
return normalize_angle(to - from);
}
/*!
* \function
*
* \brief returns the angle in [-2*M_PI, 2*M_PI] going the other way along the
* unit circle. \param angle The angle to which you want to turn in the range
* [-2*M_PI, 2*M_PI] E.g. two_pi_complement(-M_PI/4) returns 7_M_PI/4
* two_pi_complement(M_PI/4) returns -7*M_PI/4
*
*/
static inline double two_pi_complement(double angle) {
// check input conditions
if (angle > 2 * M_PI || angle < -2.0 * M_PI)
angle = fmod(angle, 2.0 * M_PI);
if (angle < 0)
return (2 * M_PI + angle);
else if (angle > 0)
return (-2 * M_PI + angle);
return (2 * M_PI);
}
/*!
* \function
*
* \brief This function is only intended for internal use and not intended for
* external use. If you do use it, read the documentation very carefully.
* Returns the min and max amount (in radians) that can be moved from "from"
* angle to "left_limit" and "right_limit". \return returns false if "from"
* angle does not lie in the interval [left_limit,right_limit] \param from -
* "from" angle - must lie in [-M_PI, M_PI) \param left_limit - left limit of
* valid interval for angular position - must lie in [-M_PI, M_PI], left and
* right limits are specified on the unit circle w.r.t to a reference pointing
* inwards \param right_limit - right limit of valid interval for angular
* position - must lie in [-M_PI, M_PI], left and right limits are specified on
* the unit circle w.r.t to a reference pointing inwards \param result_min_delta
* - minimum (delta) angle (in radians) that can be moved from "from" position
* before hitting the joint stop \param result_max_delta - maximum (delta) angle
* (in radians) that can be movedd from "from" position before hitting the joint
* stop
*/
static bool find_min_max_delta(
double from,
double left_limit,
double right_limit,
double& result_min_delta,
double& result_max_delta
) {
double delta[4];
delta[0] = shortest_angular_distance(from, left_limit);
delta[1] = shortest_angular_distance(from, right_limit);
delta[2] = two_pi_complement(delta[0]);
delta[3] = two_pi_complement(delta[1]);
if (delta[0] == 0) {
result_min_delta = delta[0];
result_max_delta = std::max<double>(delta[1], delta[3]);
return true;
}
if (delta[1] == 0) {
result_max_delta = delta[1];
result_min_delta = std::min<double>(delta[0], delta[2]);
return true;
}
double delta_min = delta[0];
double delta_min_2pi = delta[2];
if (delta[2] < delta_min) {
delta_min = delta[2];
delta_min_2pi = delta[0];
}
double delta_max = delta[1];
double delta_max_2pi = delta[3];
if (delta[3] > delta_max) {
delta_max = delta[3];
delta_max_2pi = delta[1];
}
// printf("%f %f %f %f\n",delta_min,delta_min_2pi,delta_max,delta_max_2pi);
if ((delta_min <= delta_max_2pi) || (delta_max >= delta_min_2pi)) {
result_min_delta = delta_max_2pi;
result_max_delta = delta_min_2pi;
if (left_limit == -M_PI && right_limit == M_PI)
return true;
else
return false;
}
result_min_delta = delta_min;
result_max_delta = delta_max;
return true;
}
/*!
* \function
*
* \brief Returns the delta from `from_angle` to `to_angle`, making sure it does
* not violate limits specified by `left_limit` and `right_limit`. This function
* is similar to `shortest_angular_distance_with_limits()`, with the main
* difference that it accepts limits outside the `[-M_PI, M_PI]` range. Even if
* this is quite uncommon, one could indeed consider revolute joints with large
* rotation limits, e.g., in the range `[-2*M_PI, 2*M_PI]`.
*
* In this case, a strict requirement is to have `left_limit` smaller than
* `right_limit`. Note also that `from` must lie inside the valid range, while
* `to` does not need to. In fact, this function will evaluate the shortest
* (valid) angle `shortest_angle` so that `from+shortest_angle` equals `to` up
* to an integer multiple of `2*M_PI`. As an example, a call to
* `shortest_angular_distance_with_large_limits(0, 10.5*M_PI, -2*M_PI, 2*M_PI,
* shortest_angle)` will return `true`, with `shortest_angle=0.5*M_PI`. This is
* because `from` and `from+shortest_angle` are both inside the limits, and
* `fmod(to+shortest_angle, 2*M_PI)` equals `fmod(to, 2*M_PI)`. On the other
* hand, `shortest_angular_distance_with_large_limits(10.5*M_PI, 0, -2*M_PI,
* 2*M_PI, shortest_angle)` will return false, since `from` is not in the valid
* range. Finally, note that the call
* `shortest_angular_distance_with_large_limits(0, 10.5*M_PI, -2*M_PI, 0.1*M_PI,
* shortest_angle)` will also return `true`. However, `shortest_angle` in this
* case will be `-1.5*M_PI`.
*
* \return true if `left_limit < right_limit` and if "from" and
* "from+shortest_angle" positions are within the valid interval, false
* otherwise. \param from - "from" angle. \param to - "to" angle. \param
* left_limit - left limit of valid interval, must be smaller than right_limit.
* \param right_limit - right limit of valid interval, must be greater than
* left_limit. \param shortest_angle - result of the shortest angle calculation.
*/
static inline bool shortest_angular_distance_with_large_limits(
double from,
double to,
double left_limit,
double right_limit,
double& shortest_angle
) {
// Shortest steps in the two directions
double delta = shortest_angular_distance(from, to);
double delta_2pi = two_pi_complement(delta);
// "sort" distances so that delta is shorter than delta_2pi
if (std::fabs(delta) > std::fabs(delta_2pi))
std::swap(delta, delta_2pi);
if (left_limit > right_limit) {
// If limits are something like [PI/2 , -PI/2] it actually means that we
// want rotations to be in the interval [-PI,PI/2] U [PI/2,PI], ie, the
// half unit circle not containing the 0. This is already gracefully
// handled by shortest_angular_distance_with_limits, and therefore this
// function should not be called at all. However, if one has limits that
// are larger than PI, the same rationale behind
// shortest_angular_distance_with_limits does not hold, ie, M_PI+x should
// not be directly equal to -M_PI+x. In this case, the correct way of
// getting the shortest solution is to properly set the limits, eg, by
// saying that the interval is either [PI/2, 3*PI/2] or [-3*M_PI/2,
// -M_PI/2]. For this reason, here we return false by default.
shortest_angle = delta;
return false;
}
// Check in which direction we should turn (clockwise or counter-clockwise).
// start by trying with the shortest angle (delta).
double to2 = from + delta;
if (left_limit <= to2 && to2 <= right_limit) {
// we can move in this direction: return success if the "from" angle is
// inside limits
shortest_angle = delta;
return left_limit <= from && from <= right_limit;
}
// delta is not ok, try to move in the other direction (using its complement)
to2 = from + delta_2pi;
if (left_limit <= to2 && to2 <= right_limit) {
// we can move in this direction: return success if the "from" angle is
// inside limits
shortest_angle = delta_2pi;
return left_limit <= from && from <= right_limit;
}
// nothing works: we always go outside limits
shortest_angle = delta; // at least give some "coherent" result
return false;
}
/*!
* \function
*
* \brief Returns the delta from "from_angle" to "to_angle" making sure it does
* not violate limits specified by left_limit and right_limit. The valid
* interval of angular positions is [left_limit,right_limit]. E.g., [-0.25,0.25]
* is a 0.5 radians wide interval that contains 0. But [0.25,-0.25] is a
* 2*M_PI-0.5 wide interval that contains M_PI (but not 0). The value of
* shortest_angle is the angular difference between "from" and "to" that lies
* within the defined valid interval. E.g.
* shortest_angular_distance_with_limits(-0.5,0.5,0.25,-0.25,ss) evaluates ss to
* 2*M_PI-1.0 and returns true while
* shortest_angular_distance_with_limits(-0.5,0.5,-0.25,0.25,ss) returns false
* since -0.5 and 0.5 do not lie in the interval [-0.25,0.25]
*
* \return true if "from" and "to" positions are within the limit interval,
* false otherwise \param from - "from" angle \param to - "to" angle \param
* left_limit - left limit of valid interval for angular position, left and
* right limits are specified on the unit circle w.r.t to a reference pointing
* inwards \param right_limit - right limit of valid interval for angular
* position, left and right limits are specified on the unit circle w.r.t to a
* reference pointing inwards \param shortest_angle - result of the shortest
* angle calculation
*/
static inline bool shortest_angular_distance_with_limits(
double from,
double to,
double left_limit,
double right_limit,
double& shortest_angle
) {
double min_delta = -2 * M_PI;
double max_delta = 2 * M_PI;
double min_delta_to = -2 * M_PI;
double max_delta_to = 2 * M_PI;
bool flag = find_min_max_delta(from, left_limit, right_limit, min_delta, max_delta);
double delta = shortest_angular_distance(from, to);
double delta_mod_2pi = two_pi_complement(delta);
if (flag) // from position is within the limits
{
if (delta >= min_delta && delta <= max_delta) {
shortest_angle = delta;
return true;
} else if (delta_mod_2pi >= min_delta && delta_mod_2pi <= max_delta) {
shortest_angle = delta_mod_2pi;
return true;
} else // to position is outside the limits
{
find_min_max_delta(to, left_limit, right_limit, min_delta_to, max_delta_to);
if (fabs(min_delta_to) < fabs(max_delta_to))
shortest_angle = std::max<double>(delta, delta_mod_2pi);
else if (fabs(min_delta_to) > fabs(max_delta_to))
shortest_angle = std::min<double>(delta, delta_mod_2pi);
else {
if (fabs(delta) < fabs(delta_mod_2pi))
shortest_angle = delta;
else
shortest_angle = delta_mod_2pi;
}
return false;
}
} else // from position is outside the limits
{
find_min_max_delta(to, left_limit, right_limit, min_delta_to, max_delta_to);
if (fabs(min_delta) < fabs(max_delta))
shortest_angle = std::min<double>(delta, delta_mod_2pi);
else if (fabs(min_delta) > fabs(max_delta))
shortest_angle = std::max<double>(delta, delta_mod_2pi);
else {
if (fabs(delta) < fabs(delta_mod_2pi))
shortest_angle = delta;
else
shortest_angle = delta_mod_2pi;
}
return false;
}
shortest_angle = delta;
return false;
}
} // namespace angles
#endif

84
wust_vision-main/3rdparty/ankerl/stl.h vendored Normal file
View File

@@ -0,0 +1,84 @@
///////////////////////// ankerl::unordered_dense::{map, set} /////////////////////////
// A fast & densely stored hashmap and hashset based on robin-hood backward shift deletion.
// Version 4.8.1
// https://github.com/martinus/unordered_dense
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
// SPDX-License-Identifier: MIT
// Copyright (c) 2022 Martin Leitner-Ankerl <martin.ankerl@gmail.com>
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
#ifndef ANKERL_STL_H
#define ANKERL_STL_H
#include <array> // for array
#include <cstdint> // for uint64_t, uint32_t, std::uint8_t, UINT64_C
#include <cstring> // for size_t, memcpy, memset
#include <functional> // for equal_to, hash
#include <initializer_list> // for initializer_list
#include <iterator> // for pair, distance
#include <limits> // for numeric_limits
#include <memory> // for allocator, allocator_traits, shared_ptr
#include <optional> // for optional
#include <stdexcept> // for out_of_range
#include <string> // for basic_string
#include <string_view> // for basic_string_view, hash
#include <tuple> // for forward_as_tuple
#include <type_traits> // for enable_if_t, declval, conditional_t, ena...
#include <utility> // for forward, exchange, pair, as_const, piece...
#include <vector> // for vector
// <memory_resource> includes <mutex>, which fails to compile if
// targeting GCC >= 13 with the (rewritten) win32 thread model, and
// targeting Windows earlier than Vista (0x600). GCC predefines
// _REENTRANT when using the 'posix' model, and doesn't when using the
// 'win32' model.
#if defined __MINGW64__ && defined __GNUC__ && __GNUC__ >= 13 && !defined _REENTRANT
// _WIN32_WINNT is guaranteed to be defined here because of the
// <cstdint> inclusion above.
#ifndef _WIN32_WINNT
#error "_WIN32_WINNT not defined"
#endif
#if _WIN32_WINNT < 0x600
#define ANKERL_MEMORY_RESOURCE_IS_BAD() 1 // NOLINT(cppcoreguidelines-macro-usage)
#endif
#endif
#ifndef ANKERL_MEMORY_RESOURCE_IS_BAD
#define ANKERL_MEMORY_RESOURCE_IS_BAD() 0 // NOLINT(cppcoreguidelines-macro-usage)
#endif
#if defined(__has_include) && !defined(ANKERL_UNORDERED_DENSE_DISABLE_PMR)
#if __has_include(<memory_resource>) && !ANKERL_MEMORY_RESOURCE_IS_BAD()
#define ANKERL_UNORDERED_DENSE_PMR std::pmr // NOLINT(cppcoreguidelines-macro-usage)
#include <memory_resource> // for polymorphic_allocator
#elif __has_include(<experimental/memory_resource>)
#define ANKERL_UNORDERED_DENSE_PMR \
std::experimental::pmr // NOLINT(cppcoreguidelines-macro-usage)
#include <experimental/memory_resource> // for polymorphic_allocator
#endif
#endif
#if defined(_MSC_VER) && defined(_M_X64)
#include <intrin.h>
#pragma intrinsic(_umul128)
#endif
#endif

2440
wust_vision-main/3rdparty/ankerl/unordered_dense.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

160
wust_vision-main/CMakeLists.txt Normal file
View File

@@ -0,0 +1,160 @@
cmake_minimum_required(VERSION 3.14)
cmake_policy(SET CMP0072 NEW)
project(wust_vision LANGUAGES CXX)
set(CMAKE_CXX_STANDARD 20)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin)
set(CMAKE_BUILD_TYPE "Release")
message(STATUS "--------------------CMAKE_BUILD_TYPE: ${CMAKE_BUILD_TYPE}--------------------")
option(BUILD_WITH_TRT "Enable TensorRT backend" ON)
option(BUILD_WITH_OPENVINO "Enable OpenVINO backend" ON)
option(BUILD_WITH_NCNN "Enable NCNN backend" OFF)
option(BUILD_WITH_ORT "Enable ORT backend" ON)
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake)
file(GLOB_RECURSE SOURCES CONFIGURE_DEPENDS tasks/*.cpp)
# list(FILTER SOURCES EXCLUDE REGEX "tasks/auto_guidance/.*")
# list(FILTER SOURCES EXCLUDE REGEX "tasks/auto_sniper/.*")
find_package(OpenCV REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(fmt REQUIRED)
find_package(yaml-cpp REQUIRED)
find_package(Ceres REQUIRED)
find_package(HikSDK REQUIRED)
find_package(wust_vl REQUIRED)
set(BUILD_DEFINITIONS "")
set(BUILD_LIBS "")
if(BUILD_WITH_OPENVINO)
find_package(OpenVINO REQUIRED COMPONENTS Runtime ONNX)
list(APPEND BUILD_DEFINITIONS USE_OPENVINO)
list(APPEND BUILD_LIBS openvino::runtime openvino::frontend::onnx)
endif()
if(BUILD_WITH_TRT)
find_package(CUDAToolkit REQUIRED)
set(CMAKE_PREFIX_PATH "${CMAKE_PREFIX_PATH};/home/hy/TensorRT-10.6.0.26")
find_package(TensorRT REQUIRED)
include_directories(${TensorRT_INCLUDE_DIR})
include_directories(/usr/local/cuda/include)
set(CMAKE_CUDA_COMPILER /usr/local/cuda/bin/nvcc)
set(CMAKE_CUDA_ARCHITECTURES 86)
enable_language(CUDA)
set(CMAKE_CUDA_STANDARD 14)
set(CMAKE_CUDA_STANDARD_REQUIRED ON)
set(CMAKE_CUDA_COMPILER_LAUNCHER ccache)
add_subdirectory(${CMAKE_SOURCE_DIR}/cuda_infer ${CMAKE_BINARY_DIR}/cuda_infer_build)
list(APPEND BUILD_DEFINITIONS USE_TRT)
list(APPEND BUILD_LIBS TensorRT::TensorRT TensorRT::nvonnxparser CUDA::cudart cuda_infer)
endif()
if(BUILD_WITH_NCNN)
find_package(ncnn REQUIRED)
list(APPEND BUILD_DEFINITIONS USE_NCNN)
list(APPEND BUILD_LIBS ncnn)
endif()
if(BUILD_WITH_ORT)
set(ort_root_path "/home/hy/onnxruntime-linux-x64-gpu-1.22.0")
find_package(Ort REQUIRED)
include_directories(${Ort_INCLUDE_DIR})
list(APPEND BUILD_DEFINITIONS USE_ORT)
list(APPEND BUILD_LIBS ${Ort_LIB})
endif()
add_library(${PROJECT_NAME} SHARED ${SOURCES})
target_compile_definitions(${PROJECT_NAME} PUBLIC ${BUILD_DEFINITIONS})
target_include_directories(${PROJECT_NAME} PUBLIC
${PROJECT_SOURCE_DIR}
${OpenCV_INCLUDE_DIRS}
)
target_link_libraries(${PROJECT_NAME}
wust_vl::wust_vl
HikSDK::HikSDK
yaml-cpp
fmt::fmt
Eigen3::Eigen
Ceres::ceres
${OpenCV_LIBS}
${BUILD_LIBS}
)
set(ROS2_PACKAGES
ament_cmake
rosidl_typesupport_cpp
rclcpp
geometry_msgs
tf2_ros
tf2_geometry_msgs
sensor_msgs
visualization_msgs
sentry_interfaces
nav_msgs
)
function(add_common_executable exe_name src_file)
add_executable(${exe_name} ${src_file})
target_link_libraries(${exe_name} ${PROJECT_NAME})
endfunction()
add_common_executable(standard src/standard.cpp)
# add_common_executable(dart src/dart.cpp)
add_common_executable(test_usbcamera test/test_usbcamera.cpp)
foreach(pkg IN LISTS ROS2_PACKAGES)
find_package(${pkg} QUIET)
endforeach()
set(ROS2_FULL_FOUND TRUE)
foreach(pkg IN LISTS ROS2_PACKAGES)
if(NOT ${pkg}_FOUND)
set(ROS2_FULL_FOUND FALSE)
message(WARNING "ROS2 package ${pkg} not found")
endif()
endforeach()
if(ROS2_FULL_FOUND)
message(STATUS "ROS2 full environment found, compiling ROS2 targets ...")
list(APPEND BUILD_DEFINITIONS USE_ROS2)
set(ROS2_INCLUDES)
foreach(pkg IN LISTS ROS2_PACKAGES)
if(TARGET ${pkg}::${pkg})
list(APPEND ROS2_INCLUDES ${${pkg}_INCLUDE_DIRS})
endif()
endforeach()
include_directories(${ROS2_INCLUDES})
# find_package(Open3D REQUIRED)
# target_link_libraries(${PROJECT_NAME}
# Open3D::Open3D
# )
ament_target_dependencies(${PROJECT_NAME} ${ROS2_PACKAGES})
target_compile_definitions(${PROJECT_NAME} PUBLIC ${BUILD_DEFINITIONS})
macro(add_ros2_executable exe_name src_file)
add_executable(${exe_name} ${src_file})
target_link_libraries(${exe_name} ${PROJECT_NAME} )
endmacro()
add_ros2_executable(sentry src/sentry.cpp)
add_ros2_executable(nav test/nav.cpp)
else()
message(WARNING "ROS2 dependencies incomplete, skipping ROS2 targets ...")
endif()

252
wust_vision-main/README.md Normal file
View File

@@ -0,0 +1,252 @@
# <img src="https://s21.ax1x.com/2025/08/12/pVwPPKS.png" width="40">WUST_VISION
武汉科技大学崇实战队视觉代码仓库
## 写在前面
本项目基于[中南大学FYT战队2024赛季视觉框架开源](https://github.com/CSU-FYT-Vision/FYT2024_vision),华南师范大学PIONEER战队@chenjunnn[rm_vision](https://github.com/chenjunnn/rm_vision)修改与适配，参考了深圳北理莫斯科大学北极熊战队/四川大学火锅战队/沈阳航空航天大学TUP战队/北京科技大学Reborn战队/同济大学superpower战队/河北科技大学Actor&Thinker战队的部分代码与模型，感谢以上开源为本队以及本人的帮助(排名不分先后)
## 依赖
* [wust_vl](https://github.com/WUST-RM/wust_vl)
* OpenCV
* [OpenVINO](https://flowus.cn/7a2a3341-74a1-4db9-bced-99fe5d05ab75)/[TensorRT-cuda](https://flowus.cn/e98af178-de0b-4546-808d-a6f1ff199d62)/[NCNN](https://flowus.cn/664f6bee-8ea9-4d54-8a78-e2c0bf38ee9f)/[OnnxRunetime](https://flowus.cn/8fbecbbf-c0f9-49bb-bac5-7b4923f55c99)连接为简单部署文档
* fmt
* ceres
* Eigen3
* nlohmann
* yaml-cpp
## 环境配置
```
./script/install_depences.sh
```
## Quick Start
```
git clone --recurse-submodules https://github.com/WUST-RM/wust_vision.git
cd wust_vision
sudo ./run.sh run xx /rebuild/build #编译并运行xx可执行文件/删除build缓存重新编译/仅编译
```
### 注意：本项目可选择编译OpenVINO/TensorRT-cuda/NCNN/OnnxRunetime，需在build缓存前在[CMakeLists.txt](CMakeLists.txt)中修改对应编译选项,修改后需rebuild重新编译，无OpenVINO/TensorRT-cuda/NCNN/OnnxRunetime环境仍可以使用OpenCV的装甲板识别，装甲板的识别方案需要在[config/auto_aim.yaml](config/auto_aim.yaml)中修改
## 文件树
```
.
├── 3rdparty
│   ├── angles.h
│   └── backward-cpp
│
├── cmake
│   ├── FindG2O.cmake
│   ├── FindHikSDK.cmake
│   ├── FindOrt.cmake
│   └── FindTensorRT.cmake
├── CMakeLists.txt
├── config
│   ├── auto_aim.yaml
│   ├── auto_buff.yaml
│   ├── auto_guidance.yaml
│   ├── auto_sniper.yaml
│   ├── camera_info.yaml
│   ├── camera.yaml
│   ├── common.yaml
│   ├── config -> /home/hy/wust_vision/config
│   ├── detect_ncnn.yaml
│   ├── detect_opencv.yaml
│   ├── detect_openvino.yaml
│   ├── detect_ort.yaml
│   ├── detect_trt.yaml
│   └── guard.sh
├── cuda_infer
│   ├── armor_infer.cu
│   ├── armor_infer.hpp
│   ├── CMakeLists.txt
│   ├── letter_box.cu
│   └── letter_box.hpp
├── env.bash
├── format.sh
├── KalmanHyLib
│   ├── adaptive_extended_kalman_filter.hpp
│   ├── error_state_extended_kalman_filter.hpp
│   ├── extended_kalman_filter.hpp
│   ├── kalman_hybird_lib.hpp
│   ├── README.md
│   └── unscented_kalman_filter.hpp
├── model
│
├── README.md
├── read_shm_image_mmap_only.py
├── ros2
│   ├── CMakeLists.txt
│   ├── ros2.cpp
│   └── ros2.hpp
├── run.sh
├── script
│   ├── install_depences.sh
│   ├── rsync.sh
│   ├── setup_devenv.sh
│   └── setup_service.sh
├── src
│   ├── dart.cpp
│   ├── hero.cpp
│   ├── sentry.cpp
│   ├── sim.cpp
│   └── standard.cpp
├── static
│   ├── 崇实战队logo图标.png
│   ├── css
│   │   └── style.css
│   ├── js
│   │   ├── chart_logic.js
│   │   ├── json_view.js
│   │   └── main.js
│   └── logo.JPG
├── tasks
│   ├── auto_aim
│   │   ├── armor_control
│   │   │   ├── aimer.cpp
│   │   │   ├── aimer.hpp
│   │   │   ├── planner.cpp
│   │   │   ├── planner.hpp
│   │   │   ├── shooter.cpp
│   │   │   ├── shooter.hpp
│   │   │   └── tinympc
│ │ │
│   │   ├── armor_detect
│   │   │   ├── armor_detect_common.cpp
│   │   │   ├── armor_detect_common.hpp
│   │   │   ├── armor_detector_base.hpp
│   │   │   ├── armor_infer.cpp
│   │   │   ├── armor_infer.hpp
│   │   │   ├── armor_pose_estimator.cpp
│   │   │   ├── armor_pose_estimator.hpp
│   │   │   ├── detector_factory.hpp
│   │   │   ├── light_corner_corrector.cpp
│   │   │   ├── light_corner_corrector.hpp
│   │   │   ├── ncnn
│   │   │   │   ├── armor_detector_ncnn.cpp
│   │   │   │   ├── armor_detector_ncnn.hpp
│   │   │   │   ├── armor_detector_ncnn_wrapper.cpp
│   │   │   │   └── armor_detector_ncnn_wrapper.hpp
│   │   │   ├── number_classifier.cpp
│   │   │   ├── number_classifier.hpp
│   │   │   ├── onnxruntime
│   │   │   │   ├── armor_detector_onnxruntime.cpp
│   │   │   │   ├── armor_detector_onnxruntime.hpp
│   │   │   │   ├── armor_detector_onnxruntime_wrapper.cpp
│   │   │   │   └── armor_detector_onnxruntime_wrapper.hpp
│   │   │   ├── opencv
│   │   │   │   ├── armor_detector_opencv.cpp
│   │   │   │   ├── armor_detector_opencv.hpp
│   │   │   │   ├── armor_detector_opencv_wrapper.cpp
│   │   │   │   └── armor_detector_opencv_wrapper.hpp
│   │   │   ├── openvino
│   │   │   │   ├── armor_detector_openvino.cpp
│   │   │   │   ├── armor_detector_openvino.hpp
│   │   │   │   ├── armor_detector_openvino_wrapper.cpp
│   │   │   │   └── armor_detector_openvino_wrapper.hpp
│   │   │   └── tensorrt
│   │   │   ├── armor_detector_tensorrt.cpp
│   │   │   ├── armor_detector_tensorrt.hpp
│   │   │   ├── armor_detector_tensorrt_wrapper.cpp
│   │   │   └── armor_detector_tensorrt_wrapper.hpp
│   │   ├── armor_optimize
│   │   │   ├── ba_solver.cpp
│   │   │   └── ba_solver.hpp
│   │   ├── armor_tracker
│   │   │   ├── motion_models
│   │   │   │   ├── acc_model.hpp
│   │   │   │   ├── motion_modela.hpp
│   │   │   │   ├── motion_modelonea.hpp
│   │   │   │   ├── motion_modeloneca.hpp
│   │   │   │   ├── motion_modeloneypd.hpp
│   │   │   │   ├── motion_modelr.hpp
│   │   │   │   ├── motion_modelrypd.hpp
│   │   │   │   ├── motion_modelypd.hpp
│   │   │   │   └── motion_modelypdv2.hpp
│   │   │   ├── target.cpp
│   │   │   ├── target.hpp
│   │   │   ├── tracker_manager.cpp
│   │   │   ├── tracker_manager.hpp
│   │   │   ├── trackerv3.cpp
│   │   │   └── trackerv3.hpp
│   │   ├── auto_aim.cpp
│   │   ├── auto_aim_fsm.hpp
│   │   ├── auto_aim.hpp
│   │   ├── CMakeLists.txt
│   │   ├── type.cpp
│   │   └── type.hpp
│   ├── auto_buff
│   │   ├── auto_buff.cpp
│   │   ├── auto_buff.hpp
│   │   ├── CMakeLists.txt
│   │   ├── rune_control
│   │   │   ├── aimer.cpp
│   │   │   └── aimer.hpp
│   │   ├── rune_detector
│   │   │   ├── rune_detector.cpp
│   │   │   └── rune_detector.hpp
│   │   ├── rune_optimize
│   │   │   ├── ba_solver.cpp
│   │   │   └── ba_solver.hpp
│   │   ├── rune_tracker
│   │   │   ├── motion_models
│   │   │   │   └── motion_modelrypd.hpp
│   │   │   ├── rune_target.cpp
│   │   │   ├── rune_target.hpp
│   │   │   ├── rune_tracker.cpp
│   │   │   ├── rune_tracker.hpp
│   │   │   └── spd_fitter.hpp
│   │   ├── type.cpp
│   │   └── type.hpp
│   ├── auto_guidance
│   │   ├── auto_guidance.cpp
│   │   ├── auto_guidance.hpp
│   │   ├── CMakeLists.txt
│   │   ├── debug.cpp
│   │   ├── debug.hpp
│   │   ├── guidance_detector
│   │   │   ├── detector_base.hpp
│   │   │   ├── detector_factory.hpp
│   │   │   ├── green_light_infer.cpp
│   │   │   ├── green_light_infer.hpp
│   │   │   ├── opencv
│   │   │   │   ├── guidance_detector_opencv.cpp
│   │   │   │   └── guidance_detector_opencv.hpp
│   │   │   └── openvino
│   │   │   ├── guidance_detector_openvino.cpp
│   │   │   └── guidance_detector_openvino.hpp
│   │   ├── guidance_tracker
│   │   │   ├── guidance_target.cpp
│   │   │   ├── guidance_target.hpp
│   │   │   ├── guidance_tracker.cpp
│   │   │   ├── guidance_tracker.hpp
│   │   │   └── motion_models
│   │   │   └── imgbox_model.hpp
│   │   └── type.hpp
│   ├── auto_offset
│   │   ├── auto_offset.cpp
│   │   ├── auto_offset.hpp
│   │   └── CMakeLists.txt
│   ├── auto_sniper
│   │   ├── auto_sniper.cpp
│   │   ├── auto_sniper.hpp
│   │   ├── CMakeLists.txt
│   │   └── trajectory_solver.hpp
│   ├── CMakeLists.txt
│   ├── debug.cpp
│   ├── debug.hpp
│   ├── main_base.hpp
│   ├── packet_typedef.hpp
│   ├── sinple_img_rotate_saver.hpp
│   ├── type_common.cpp
│   ├── type_common.hpp
│   ├── utils.cpp
│   ├── utils.hpp
│   ├── vision_base.cpp
│   └── vision_base.hpp
├── templates
│   └── index.html
├── test
│   ├── control.cpp
│   └── test_usbcamera.cpp
└── web.py
```

147
wust_vision-main/cmake/FindHikSDK.cmake Normal file
View File

@@ -0,0 +1,147 @@
# --------------------------------------------------------------------------------------------
# FindHikSDK.cmake
#
# This module finds the HikRobot / Hikvision MVS Camera SDK.
#
# It defines the following variables:
# HikSDK_FOUND
# HikSDK_INCLUDE_DIR
# HikSDK_LIB
#
# And the following imported target:
# hiksdk
# --------------------------------------------------------------------------------------------
# =========================
# 1. SDK 根路径
# =========================
if(WIN32)
set(HikSDK_Path "$ENV{MVCAM_COMMON_RUNENV}")
else()
set(HikSDK_Path "$ENV{MVCAM_SDK_PATH}")
endif()
if(NOT HikSDK_Path OR HikSDK_Path STREQUAL "")
message(STATUS "HikSDK: MVCAM_SDK_PATH is not set")
set(HikSDK_FOUND FALSE)
return()
endif()
# =========================
# 2. 查找头文件
# =========================
find_path(
HikSDK_INCLUDE_DIR
NAMES
MvCameraControl.h
CameraParams.h
PixelType.h
MvErrorDefine.h
MvISPErrorDefine.h
PATHS
"${HikSDK_Path}/include"
"${HikSDK_Path}/Includes"
NO_DEFAULT_PATH
)
# =========================
# 3. 查找库文件（关键修复点）
# =========================
if(UNIX)
find_library(
HikSDK_LIB
NAMES
MvCameraControl
libMvCameraControl.so
PATHS
"${HikSDK_Path}/lib"
"${HikSDK_Path}/lib64"
"${HikSDK_Path}/lib/arm"
"${HikSDK_Path}/lib/arm64"
"${HikSDK_Path}/lib/aarch64"
"${HikSDK_Path}/lib/x86"
"${HikSDK_Path}/lib/x64"
"${HikSDK_Path}/lib/64"
"${HikSDK_Path}/lib/32"
NO_DEFAULT_PATH
)
endif()
# =========================
# 4. Windows（完整但不影响 Linux）
# =========================
if(WIN32)
find_library(
HikSDK_LIB
NAMES MvCameraControl
PATHS
"${HikSDK_Path}/Libraries"
"${HikSDK_Path}/Libraries/win64"
"${HikSDK_Path}/Libraries/win32"
NO_DEFAULT_PATH
)
find_file(
HikSDK_DLL
NAMES MvCameraControl.dll
PATHS
"${HikSDK_Path}/Runtime"
"C:/Program Files (x86)/Common Files/MVS/Runtime"
NO_DEFAULT_PATH
)
endif()
# =========================
# 5. 创建导入库目标
# =========================
if(HikSDK_LIB AND HikSDK_INCLUDE_DIR)
if(NOT TARGET HikSDK::HikSDK)
add_library(HikSDK::HikSDK SHARED IMPORTED GLOBAL)
if(WIN32)
set_target_properties(HikSDK::HikSDK PROPERTIES
IMPORTED_IMPLIB "${HikSDK_LIB}"
IMPORTED_LOCATION "${HikSDK_DLL}"
INTERFACE_INCLUDE_DIRECTORIES "${HikSDK_INCLUDE_DIR}"
)
else()
set_target_properties(HikSDK::HikSDK PROPERTIES
IMPORTED_LOCATION "${HikSDK_LIB}"
INTERFACE_INCLUDE_DIRECTORIES "${HikSDK_INCLUDE_DIR}"
)
endif()
endif()
endif()
set(HikSDK_LIBS HikSDK::HikSDK)
set(HikSDK_INCLUDE_DIRS ${HikSDK_INCLUDE_DIR})
# =========================
# 6. 标准 find_package 处理
# =========================
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(
HikSDK
REQUIRED_VARS HikSDK_LIB HikSDK_INCLUDE_DIR
)
# =========================
# 7. 调试输出（很重要）
# =========================
if(HikSDK_FOUND)
message(STATUS "HikSDK found:")
message(STATUS " Include dir : ${HikSDK_INCLUDE_DIR}")
message(STATUS " Library : ${HikSDK_LIB}")
else()
message(STATUS "HikSDK NOT found")
endif()
set(HikSDK_LIBS hiksdk)
set(HikSDK_INCLUDE_DIRS ${HikSDK_INCLUDE_DIR})
mark_as_advanced(HikSDK_LIB HikSDK_INCLUDE_DIR HikSDK_DLL)

88
wust_vision-main/cmake/FindOrt.cmake Normal file
View File

@@ -0,0 +1,88 @@
# --------------------------------------------------------------------------------------------
# This file is used to find the ONNX-Runtime SDK, which provides the following variables:
#
# Cache Variables:
# - Ort_HEADER_FILES: Names of SDK header files
#
# Advanced Variables:
# - Ort_INCLUDE_DIR: Directory where SDK header files are located
# - Ort_LIB: Path to the SDK library file (import library on Windows, shared library
# on Linux)
# - Ort_DLL: Path to the SDK dynamic library file (only on Windows)
#
# Local Variables:
# - Ort_LIBS: CMake target name for the SDK, which is "onnxruntime"
# - Ort_INCLUDE_DIRS: Directory where SDK header files are located
# --------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------
# find onnxruntime root path
# ------------------------------------------------------------------------------
if(NOT ort_root_path)
set(ort_root_path "/usr/local")
endif()
# ------------------------------------------------------------------------------
# find onnxruntime include directory
# ------------------------------------------------------------------------------
set(Ort_HEADER_FILES
cpu_provider_factory.h onnxruntime_run_options_config_keys.h
onnxruntime_c_api.h onnxruntime_session_options_config_keys.h
onnxruntime_cxx_api.h provider_options.h
onnxruntime_cxx_inline.h
CACHE INTERNAL "ONNX Runtime header files"
)
find_path(
Ort_INCLUDE_DIR
PATHS "${ort_root_path}/include"
NAMES ${Ort_HEADER_FILES}
NO_DEFAULT_PATH
)
# ------------------------------------------------------------------------------
# find onnxruntime library file
# ------------------------------------------------------------------------------
find_library(
Ort_LIB
NAMES "libonnxruntime.so"
PATHS "${ort_root_path}/lib"
NO_DEFAULT_PATH
)
# ------------------------------------------------------------------------------
# create imported target: onnxruntime
# ------------------------------------------------------------------------------
if(NOT TARGET onnxruntime)
add_library(onnxruntime SHARED IMPORTED)
set_target_properties(onnxruntime PROPERTIES
IMPORTED_LOCATION "${Ort_LIB}"
INTERFACE_INCLUDE_DIRECTORIES "${Ort_INCLUDE_DIR}"
)
endif()
mark_as_advanced(Ort_INCLUDE_DIR Ort_LIB)
# ------------------------------------------------------------------------------
# set onnxruntime cmake variables and version variables
# ------------------------------------------------------------------------------
set(Ort_LIBS "onnxruntime")
set(Ort_INCLUDE_DIRS "${Ort_INCLUDE_DIR}")
if(Ort_INCLUDE_DIR)
file(STRINGS "${Ort_INCLUDE_DIR}/onnxruntime_c_api.h" Ort_VERSION
REGEX "#define ORT_API_VERSION [0-9]+"
)
string(REGEX REPLACE "#define ORT_API_VERSION ([0-9]+)" "1.\\1" Ort_VERSION "${Ort_VERSION}")
endif()
# ------------------------------------------------------------------------------
# handle the package
# ------------------------------------------------------------------------------
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(
Ort
VERSION_VAR Ort_VERSION
REQUIRED_VARS Ort_LIB Ort_INCLUDE_DIR
)

66
wust_vision-main/cmake/FindTensorRT.cmake Normal file
View File

@@ -0,0 +1,66 @@
# FindTensorRT.cmake -- Locate NVIDIA TensorRT
include(FindPackageHandleStandardArgs)
# TensorRT root
if (DEFINED TensorRT_ROOT)
list(APPEND _TensorRT_SEARCH_PATHS
${TensorRT_ROOT}
"$ENV{TensorRT_ROOT}"
)
endif()
list(APPEND _TensorRT_SEARCH_PATHS /usr /usr/local)
# Header
find_path(TensorRT_INCLUDE_DIR
NAMES NvInfer.h
PATHS ${_TensorRT_SEARCH_PATHS}
PATH_SUFFIXES include
)
# Core library
find_library(TensorRT_LIBRARY
NAMES nvinfer
PATHS ${_TensorRT_SEARCH_PATHS}
PATH_SUFFIXES lib lib64 lib/x64
)
find_package_handle_standard_args(TensorRT
REQUIRED_VARS TensorRT_INCLUDE_DIR TensorRT_LIBRARY
)
if (TensorRT_FOUND)
set(TensorRT_INCLUDE_DIRS ${TensorRT_INCLUDE_DIR})
set(TensorRT_LIBRARIES ${TensorRT_LIBRARY})
# Optional components
foreach(_comp IN ITEMS nvinfer_plugin nvonnxparser nvparsers)
find_library(TensorRT_${_comp}_LIBRARY
NAMES ${_comp}
PATHS ${_TensorRT_SEARCH_PATHS}
PATH_SUFFIXES lib lib64 lib/x64
)
if (TensorRT_${_comp}_LIBRARY)
list(APPEND TensorRT_LIBRARIES ${TensorRT_${_comp}_LIBRARY})
endif()
endforeach()
# Core target
add_library(TensorRT::TensorRT UNKNOWN IMPORTED)
set_target_properties(TensorRT::TensorRT PROPERTIES
INTERFACE_INCLUDE_DIRECTORIES "${TensorRT_INCLUDE_DIRS}"
IMPORTED_LOCATION "${TensorRT_LIBRARY}"
)
# Component targets
foreach(_comp IN ITEMS nvinfer_plugin nvonnxparser nvparsers)
if (TensorRT_${_comp}_LIBRARY)
add_library(TensorRT::${_comp} UNKNOWN IMPORTED)
set_target_properties(TensorRT::${_comp} PROPERTIES
IMPORTED_LOCATION "${TensorRT_${_comp}_LIBRARY}"
)
endif()
endforeach()
message(STATUS "Found TensorRT at ${TensorRT_INCLUDE_DIR}")
endif()

131
wust_vision-main/config/auto_aim.yaml Normal file
View File

@@ -0,0 +1,131 @@
armor_detect_backend: tensorrt
max_detect_armors: 11
armor_map:
SENTRY: 4
NO1: 2
NO2: 3
NO3: 1
NO4: 5
NO5: 9
OUTPOST: 6
BASE: 7
UNKNOWN: -1
armor_where:
yaw_opt:
mode: golden
golden_search_side_deg: 60
distance_fix_a2: 0.0
armor_tracker:
lost_time_thres: 2.0
tracking_thres: 10
max_yaw_diff_deg: 60.0
max_dis_diff: 3.0
match_gate: 50
qxyz_common: [200.0, 200.0, 1.0]
qyaw_common: 100.0
qxyz_output: [10.0, 10.0, 0.5]
qyaw_output: 0.01
q_r: 0.0000001
q_l: 0.0000001
q_h: 0.0000001
q_outpost_dz: 0.5
yp_r: 2e-3
dis_r_front: 0.5
dis_r_side: 2.5
dis2_r_ratio: 0.1
yaw_r_base_front: 0.2
yaw_r_base_side: 0.06
yaw_r_log_ratio: 0.02
esekf_iter_num: 5
auto_aim_fsm:
single_whole_up: 1.5
single_whole_down: 1.0
whole_pair_up: 7.5
whole_pair_down: 6.5 #1s内至少换2次板子 2pi 否则轨迹规划无意义
pair_center_up: 16.5
pair_center_down: 15.0
transfer_thresh: 50
very_aimer:
fuck_test: false
fuck_test_thresh: 0.5
type: "seg" #seg or mpc
sample_total_time: 2.0
sample_horizon: 500
control_delay: 0.2
delay_enable_fire_error: 0.0035
max_yaw_acc: 40
#全向yaw加速度测算37
max_pitch_acc: 25
prediction_delay: 0.00
comming_angle: 60
leaving_angle: 20
yaw_limit_deg: 60
shooting_range_h: 0.12
shooting_range_small_w: 0.12
shooting_range_big_w: 0.24
min_enable_pitch_deg: 0.25
min_enable_yaw_deg: 0.25
base_offset:
yaw: -1.0
pitch: -3.5
trajectory_offset:
- d_max: 3
d_min: 0
h_max: 1.5
h_min: -1
pitch_off: 0
yaw_off: 0
- d_max: 4.5
d_min: 3
h_max: 1.5
h_min: -1
pitch_off: 0
yaw_off: 0
- d_max: 4.5
d_min: 9
h_max: 1.5
h_min: -1
pitch_off: 0
yaw_off: 0
#---mpc only
max_iter: 10
Q_yaw: [7e6, 0]
R_yaw: [3.0]
Q_pitch: [7e6, 0]
R_pitch: [3.0]
trajectory_compensator:
compenstator_type: resistance
gravity: 9.8
iteration_times: 20
resistance: 0.092
k1: 0.0190 #大弹丸double k1_c = 0.47; double k1 = k1_c * 1.169 * (2 * M_PI * 0.02125 * 0.02125) / 2 / 0.041;
auto_exposure:
enable: true
target_brightness: 25.0
tolerance: 3.0
step_gain: 15.0
decay_step: 1.0
exposure_min: 100.0
exposure_max: 1500.0
control_interval_ms: 300

76
wust_vision-main/config/auto_buff.yaml Normal file
View File

@@ -0,0 +1,76 @@
rune_detector:
rune_center_min_area: 10
rune_center_max_area: 2000
rune_center_1x1ratio_tol: 0.7
rune_center_fill_ratio_min: 0.3
rune_target_min_area: 100
rune_target_max_area: 3000
rune_target_max_square_ratio: 1.3
rune_target_cluster_radius: 70
bin_threshold: 50
color_diff_threshold: 40
rune_where:
roll_opt:
mode: golden
golden_search_side_deg: 60
rune_tracker:
lost_time_thres: 2.0
tracking_thres: 5
max_dis_diff: 1.0
match_gate: 15.0
esekf_iter_num: 2
q_roll: 10.0
q_xyz: 0.5
q_yaw: 0.5
yp_r: 0.01
dis_r: 0.05
yaw_r: 0.1
roll_r: 0.2
big_window_sec: 2.0
aimer:
prediction_delay: -0.00
shooting_range_h: 0.1
shooting_range_w: 0.1
min_enable_pitch_deg: 0.25
min_enable_yaw_deg: 0.25
base_offset:
yaw: -3.0
pitch: -0.0
trajectory_offset:
- d_max: 9
d_min: 0
h_max: 1.5
h_min: -1
pitch_off: -0
yaw_off: -0
- d_max: 10
d_min: 9
h_max: 0.4
h_min: -1
pitch_off: -0
yaw_off: 0
trajectory_compensator:
compenstator_type: resistance
gravity: 9.8
iteration_times: 20
resistance: 0.092
k1: 0.0190 #大弹丸double k1_c = 0.47; double k1 = k1_c * 1.169 * (2 * M_PI * 0.02125 * 0.02125) / 2 / 0.041;
auto_exposure:
enable: false
target_brightness: 15.0
tolerance: 3.0
step_gain: 15.0
decay_step: 1.0
exposure_min: 100.0
exposure_max: 2500.0
control_interval_ms: 300

83
wust_vision-main/config/auto_guidance.yaml Normal file
View File

@@ -0,0 +1,83 @@
backend: opencv
max_infer_running: 6
detector:
openvino:
model_path: /home/hy/2026_dart_guide/assets/Katrin.xml
top_k: 128
conf_threshold: 0.2
nms_threshold: 0.5
device_type: CPU
use_throughputmode: false
opencv:
gui: true
HSV:
lowH: 25
highH: 85
lowS: 50
highS: 255
lowV: 150
highV: 255
contours:
min_area: 100
max_area: 50000
min_aspect_ratio: 0.5
min_fill_ratio: 0.7
tracker:
lost_time_thres: 2.0
tracking_thres: 10
target:
xy_r: 0.05
wh_r: 0.05
q_xy: 100
q_wh: 100
iter_num: 2
max_dis_diff: 2.0
logger:
log_level: INFO
log_path: /home/hy/wust_log
use_logcli: true
use_logfile: true
use_simplelog: true
control:
control_rate: 100
communication_delay_us: 1100
pitch_avg_windows: 1
device_name: /dev/ttyACM_RMc
use_serial: true
camera:
type: hik_camera
camera_info_path: ${VISION_ROOT}/config/camera_info.yaml
hik_camera:
target_sn: DA1094119 #DA3038891
acquisition_frame_rate: 80
adc_bit_depth: Bits_12
exposure_time: 5000
gain: 16.9
gamma: 0.5 #Bayer用不了
pixel_format: BayerRG8
use_raw: false
acquisition_frame_rate_enable: false
reverse_x: false
reverse_y: false
video_player:
fps: 12
loop: true
use_cvt: true
#path: /home/hy/wust_data/video/aaa.mp4
#path: /home/hy/wust_data/video/jiao.avi
#path: /home/hy/Z_LION_AutoAim2025/src/auto_aim_publisher/videos/sample.avi
#path: /home/hy/wust_data/record/20251111_134416_187.avi
path: /home/hy/下载/sp_vision_25/records/1.avi
#path: /home/hy/wust_data/video/Sentry.mp4
start_frame: 0

27
wust_vision-main/config/auto_sniper.yaml Normal file
View File

@@ -0,0 +1,27 @@
map:
voxel_size: 0.1
min_pos: [-5.0,-10.0,-1.0]
max_pos: [20.0,10.0,1.0]
solver:
k1: 0.0190
g: 9.81
target_armor_z: 0.5
offset_helper:
order: 5
yaw_base_offset: 0.0
pitch_base_offset: 0.0
offset_table:
- distance: 2.0
yaw: 0.1
pitch: -0.2
- distance: 3.0
yaw: 0.15
pitch: -0.25
- distance: 4.0
yaw: 0.2
pitch: -0.3

50
wust_vision-main/config/camera.yaml Normal file
View File

@@ -0,0 +1,50 @@
type: hik_camera
camera_info_path: ${VISION_ROOT}/config/camera_info.yaml
hik_camera:
target_sn: DA3038934
adc_bit_depth: Bits_8
pixel_format: BayerRG8 #RGB8Packed
reverse_x: false
reverse_y: false
width: 1440
height: 1080
offset_x: 0
offset_y: 0
acquisition_frame_rate_enable: true
acquisition_frame_rate: 250
exposure_time: 2000
gain: 16.9
gamma: 0.7 #Bayer用不了
trigger_type: none
trigger_source: ""
trigger_activation: 0
use_raw: false
use_rgb: false
use_ea: false
use_cuda_cvt: true
video_player:
fps: 60
loop: true
use_cvt: true
trigger_mode: false
#path: /home/hy/wust_data/video/aaa.mp4
#path: /home/hy/wust_data/video/jiao.avi
#path: /home/hy/Z_LION_AutoAim2025/src/auto_aim_publisher/videos/sample.avi
#path: /home/hy/wust_data/record/20251111_134416_187.avi
path: /home/hy/data/video_save/5.21成都.mp4
#path: /home/hy/wust_data/video/Sentry.mp4
start_frame: 0
uvc:
device_name: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:3.4:1.0-video-index0"
fps: 60
width: 1280
height: 720
exposure: 100.0
gain: 10.0
gamma: 100
trigger_mode: false

82
wust_vision-main/config/camera_info.yaml Normal file
View File

@@ -0,0 +1,82 @@
#8mm
# image_width: 1440
# image_height: 1080
# camera_name: narrow_stereo
# camera_matrix:
# rows: 3
# cols: 3
# data: [2419.64498, 0. , 719.05623,
# 0. , 2414.38209, 538.71651,
# 0. , 0. , 1. ]
# distortion_model: plumb_bob
# distortion_coefficients:
# rows: 1
# cols: 5
# data: [-0.033521, 0.087954, -0.002045, -0.001438, 0.000000]
# rectification_matrix:
# rows: 3
# cols: 3
# data: [1., 0., 0.,
# 0., 1., 0.,
# 0., 0., 1.]
# projection_matrix:
# rows: 3
# cols: 4
# data: [2413.22632, 0. , 717.5196 , 0. ,
# 0. , 2408.83228, 537.43444, 0. ,
# 0. , 0. , 1. , 0. ]
#6mm
image_width: 1440
image_height: 1080
camera_name: narrow_stereo
camera_matrix:
rows: 3
cols: 3
data: [1814.19888, 0. , 730.27359,
0. , 1807.37177, 529.57031,
0. , 0. , 1. ]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [-0.089138, 0.114766, 0.000034, 0.000238, 0.000000]
rectification_matrix:
rows: 3
cols: 3
data: [1., 0., 0.,
0., 1., 0.,
0., 0., 1.]
projection_matrix:
rows: 3
cols: 4
data: [1793.54229, 0. , 730.67194, 0. ,
0. , 1794.48784, 529.46193, 0. ,
0. , 0. , 1. , 0. ]
#12mm
# image_width: 1440
# image_height: 1080
# camera_name: narrow_stereo
# camera_matrix:
# rows: 3
# cols: 3
# data: [3655.11292, 0. , 748.66803,
# 0. , 3648.34439, 556.97683,
# 0. , 0. , 1. ]
# distortion_model: plumb_bob
# distortion_coefficients:
# rows: 1
# cols: 5
# data: [-0.058763, 1.430269, 0.001191, 0.001641, 0.000000]
# rectification_matrix:
# rows: 3
# cols: 3
# data: [1., 0., 0.,
# 0., 1., 0.,
# 0., 0., 1.]
# projection_matrix:
# rows: 3
# cols: 4
# data: [3659.22949, 0. , 748.71496, 0. ,
# 0. , 3652.02661, 556.81908, 0. ,
# 0. , 0. , 1. , 0. ]

34
wust_vision-main/config/common.yaml Normal file
View File

@@ -0,0 +1,34 @@
max_infer_running: 6
detect_color: 0
attack_mode: 0
debug_fps: 60
tf:
R_camera2gimbal: [0.0, 0.0, 1.0, -1.0, -0.0, 0.0, 0.0, -1.0, 0.0]
t_camera2gimbal:
[0.000434347168653831, 0.0141232476052895, 0.00736106400231024]
control:
control_rate: 1000
communication_delay_us: 100.000
device_name: /dev/stm32_acm
use_serial: true
yaw_ramp: 0.0 #send_cmd = cmd.pos+cmd.vel*ramp
pitch_ramp: 0.0
logger:
log_level: DEBUG
log_path: log
use_logcli: true
use_logfile: false
use_simplelog: true
shoot:
bullet_speed: 26.0
rate: 3
record:
use_record: false
folder_path: record
use_rotate_reader: false
read_csv_path: ""

66
wust_vision-main/config/detect_ml.yaml Normal file
View File

@@ -0,0 +1,66 @@
armor_detector:
cv: #装甲板灯条完整无损坏才可以用
enable: true
armor:
max_angle: 40
max_large_center_distance: 8
max_small_center_distance: 3.5
min_large_center_distance: 3.5
min_light_ratio: 0.5
min_small_center_distance: 0.05
light:
binary_thres: 150
expand_ratio_h: 1.9
expand_ratio_w: 1.2
max_angle: 45
max_ratio: 0.4
min_ratio: 0.01
max_pts_error: 20.0
max_angle_diff: 20.0
color_diff_thresh: 40
classify:
enable: true
label_path: ${VISION_ROOT}/model/label.txt
model_path: ${VISION_ROOT}/model/mlp.onnx
backend: opencv
threshold: 0.5
tensorrt:
conf_threshold: 0.2
model_path: ${VISION_ROOT}/model/opt-1208-001.onnx
device_id: 0
nms_threshold: 0.3
top_k: 128
max_infer_running: 3
min_free_mem_ratio: 0.1
use_cuda_pre: true
log_time: false
model_type: tup
openvino:
conf_threshold: 0.2
device_name: GPU
model_path: ${VISION_ROOT}/model/opt-1208-001.onnx
nms_threshold: 0.3
top_k: 128
use_throughputmode: true
model_type: tup
onnxruntime:
conf_threshold: 0.2
model_path: ${VISION_ROOT}/model/opt-1208-001.onnx
nms_threshold: 0.3
top_k: 128
model_type: tup
provider: CUDA
ncnn:
conf_threshold: 0.2
model_path_param: ${VISION_ROOT}/model/opt-1208-001.param
model_path_bin: ${VISION_ROOT}/model/opt-1208-001.bin
input_name: images
output_name: output
use_gpu: true
use_lightmode: false
device_id: 0
cpu_threads: 20
nms_threshold: 0.3
top_k: 128
model_type: tup

19
wust_vision-main/config/detect_opencv.yaml Normal file
View File

@@ -0,0 +1,19 @@
armor_detector:
light:
binary_thres: 120
max_angle: 40
max_ratio: 0.4
min_ratio: 0.001
color_diff_thresh: 20
max_angle_diff: 10.0
armor:
min_light_ratio: 0.8
min_small_center_distance: 0.8
max_small_center_distance: 3.5
min_large_center_distance: 3.5
max_large_center_distance: 8.0
max_angle: 40.0
classify:
label_path: ${VISION_ROOT}/model/label.txt
model_path: ${VISION_ROOT}/model/reborn_number_classifier.onnx
threshold: 0.5

24
wust_vision-main/config/guard.sh Executable file
View File

@@ -0,0 +1,24 @@
#!/bin/bash
TARGET="$1"
shift
ARGS=("${@:3}")
echo "[GUARD] target: $TARGET"
echo "[GUARD] args: ${ARGS[*]}"
echo "[GUARD] Starting monitor..."
while true; do
echo "[GUARD] Launching program..."
"$TARGET" "${ARGS[@]}"
RET=$?
if [ $RET -eq 0 ]; then
echo "[GUARD] Program exited normally. Stopping guard."
exit 0
fi
echo "[GUARD] Crash detected, restarting in 1 second..."
sleep 1
done

51
wust_vision-main/config/omni/camera0.yaml Normal file
View File

@@ -0,0 +1,51 @@
yaw_in_big_yaw_deg: 90.0
type: uvc
camera_info_path: ${VISION_ROOT}/config/camera_info.yaml
hik_camera:
target_sn: DA1094119
adc_bit_depth: Bits_8
pixel_format: BayerRG8 #RGB8Packed
reverse_x: false
reverse_y: false
width: 1440
height: 1080
offset_x: 0
offset_y: 0
acquisition_frame_rate_enable: true
acquisition_frame_rate: 250
exposure_time: 2000
gain: 16.9
gamma: 0.7 #Bayer用不了
trigger_type: none
trigger_source: ""
trigger_activation: 0
use_raw: false
use_rgb: false
use_ea: false
use_cuda_cvt: false
video_player:
fps: 60
loop: true
use_cvt: true
trigger_mode: false
#path: /home/hy/wust_data/video/aaa.mp4
#path: /home/hy/wust_data/video/jiao.avi
#path: /home/hy/Z_LION_AutoAim2025/src/auto_aim_publisher/videos/sample.avi
#path: /home/hy/wust_data/record/20251111_134416_187.avi
path: /home/hy/rune_dl/runeA.mp4
#path: /home/hy/wust_data/video/Sentry.mp4
start_frame: 0
uvc:
device_name: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:9.4:1.0-video-index0"
fps: 30
width: 1280
height: 720
exposure: 200
gain: 10.0
gamma: 100
trigger_mode: true

51
wust_vision-main/config/omni/camera1.yaml Normal file
View File

@@ -0,0 +1,51 @@
yaw_in_big_yaw_deg: 107.0
type: uvc
camera_info_path: ${VISION_ROOT}/config/omni/camera_info.yaml
hik_camera:
target_sn: DA1094119
adc_bit_depth: Bits_8
pixel_format: BayerRG8 #RGB8Packed
reverse_x: false
reverse_y: false
width: 1440
height: 1080
offset_x: 0
offset_y: 0
acquisition_frame_rate_enable: true
acquisition_frame_rate: 250
exposure_time: 2000
gain: 16.9
gamma: 0.7 #Bayer用不了
trigger_type: none
trigger_source: ""
trigger_activation: 0
use_raw: false
use_rgb: false
use_ea: false
use_cuda_cvt: false
video_player:
fps: 60
loop: true
use_cvt: true
trigger_mode: false
#path: /home/hy/wust_data/video/aaa.mp4
#path: /home/hy/wust_data/video/jiao.avi
#path: /home/hy/Z_LION_AutoAim2025/src/auto_aim_publisher/videos/sample.avi
#path: /home/hy/wust_data/record/20251111_134416_187.avi
path: /home/hy/rune_dl/runeA.mp4
#path: /home/hy/wust_data/video/Sentry.mp4
start_frame: 0
uvc:
device_name: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:9.1:1.0-video-index0"
fps: 30
width: 1280
height: 720
exposure: 200
gain: 10.0
gamma: 100
trigger_mode: true

28
wust_vision-main/config/omni/camera_info.yaml Normal file
View File

@@ -0,0 +1,28 @@
# camera_matrix = [759.73071, 0.0, 336.19053, 0.0, 761.16771, 231.83002, 0.0, 0.0, 1.0]
# distortion_coefficients = [0.000281, 0.144018, 0.005509, -0.004330, 0.0]
image_width: 1440
image_height: 1080
camera_name: narrow_stereo
camera_matrix:
rows: 3
cols: 3
data: [759.73071, 0.0, 336.19053, 0.0, 761.16771, 231.83002, 0.0, 0.0, 1.0 ]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [0.000281, 0.144018, 0.005509, -0.004330, 0.0]
rectification_matrix:
rows: 3
cols: 3
data: [1., 0., 0.,
0., 1., 0.,
0., 0., 1.]
projection_matrix:
rows: 3
cols: 4
data: [1793.54229, 0. , 730.67194, 0. ,
0. , 1794.48784, 529.46193, 0. ,
0. , 0. , 1. , 0. ]

66
wust_vision-main/config/omni/detect_ml.yaml Normal file
View File

@@ -0,0 +1,66 @@
armor_detector:
cv: #装甲板灯条完整无损坏才可以用
enable: false
armor:
max_angle: 40
max_large_center_distance: 8
max_small_center_distance: 3.5
min_large_center_distance: 3.5
min_light_ratio: 0.5
min_small_center_distance: 0.05
light:
binary_thres: 150
expand_ratio_h: 1.9
expand_ratio_w: 1.2
max_angle: 45
max_ratio: 0.4
min_ratio: 0.01
max_pts_error: 20.0
max_angle_diff: 20.0
color_diff_thresh: 0
classify:
enable: true
label_path: ${VISION_ROOT}/model/label.txt
model_path: ${VISION_ROOT}/model/mlp.onnx
backend: opencv
threshold: 0.5
tensorrt:
conf_threshold: 0.2
model_path: ${VISION_ROOT}/model/opt-1208-001.onnx
device_id: 0
nms_threshold: 0.3
top_k: 128
max_infer_running: 3
min_free_mem_ratio: 0.1
use_cuda_pre: true
log_time: false
model_type: tup
openvino:
conf_threshold: 0.2
device_name: GPU
model_path: ${VISION_ROOT}/model/opt-1208-001.onnx
nms_threshold: 0.3
top_k: 128
use_throughputmode: true
model_type: tup
onnxruntime:
conf_threshold: 0.2
model_path: ${VISION_ROOT}/model/opt-1208-001.onnx
nms_threshold: 0.3
top_k: 128
model_type: tup
provider: CUDA
ncnn:
conf_threshold: 0.2
model_path_param: ${VISION_ROOT}/model/opt-1208-001.param
model_path_bin: ${VISION_ROOT}/model/opt-1208-001.bin
input_name: images
output_name: output
use_gpu: true
use_lightmode: false
device_id: 0
cpu_threads: 20
nms_threshold: 0.3
top_k: 128
model_type: tup

19
wust_vision-main/config/omni/detect_opencv.yaml Normal file
View File

@@ -0,0 +1,19 @@
armor_detector:
light:
binary_thres: 120
max_angle: 40
max_ratio: 0.4
min_ratio: 0.001
color_diff_thresh: 20
max_angle_diff: 10.0
armor:
min_light_ratio: 0.8
min_small_center_distance: 0.8
max_small_center_distance: 3.5
min_large_center_distance: 3.5
max_large_center_distance: 8.0
max_angle: 40.0
classify:
label_path: ${VISION_ROOT}/model/label.txt
model_path: ${VISION_ROOT}/model/reborn_number_classifier.onnx
threshold: 0.5

37
wust_vision-main/config/omni/omni.yaml Normal file
View File

@@ -0,0 +1,37 @@
armor_detect_backend: openvino
cameras: ["${VISION_ROOT}/config/omni/camera0.yaml", "${VISION_ROOT}/config/omni/camera1.yaml"]
fps: 60
max_infer_running: 3
active_time: 1.0
min_score: 15.0
armor_where:
yaw_opt:
mode: golden
golden_search_side_deg: 60
distance_fix_a2: 0.0
armor_tracker:
lost_time_thres: 1.0
tracking_thres: 5
max_yaw_diff_deg: 90.0
max_dis_diff: 3.0
match_gate: 50
qxyz_common: [1.0, 1.0, 1.0]
qyaw_common: 1.0
qxyz_output: [1.0, 1.0, 0.5]
qyaw_output: 0.01
q_r: 0.0000001
q_l: 0.0000001
q_h: 0.0000001
q_outpost_dz: 0.5
yp_r: 2e-3
dis_r_front: 0.5
dis_r_side: 2.5
dis2_r_ratio: 0.1
yaw_r_base_front: 0.2
yaw_r_base_side: 0.06
yaw_r_log_ratio: 0.02
esekf_iter_num: 1

61
wust_vision-main/cuda_infer/CMakeLists.txt Normal file
View File

@@ -0,0 +1,61 @@
cmake_minimum_required(VERSION 3.10)
cmake_policy(SET CMP0079 NEW)
project(cuda_infer LANGUAGES CXX CUDA)
# 设置标准
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CUDA_STANDARD 17)
set(CMAKE_CUDA_STANDARD_REQUIRED ON)
set(CUDA_USE_STATIC_CUDA_RUNTIME OFF)
set(CMAKE_BUILD_TYPE "Release")
# 抑制过时 API 警告
add_compile_options(-Wno-deprecated-declarations)
# 禁用 .rsp 响应文件（避免 nvcc 报错）
set(CMAKE_CUDA_USE_RESPONSE_FILE_FOR_OBJECTS OFF)
set(CMAKE_CUDA_USE_RESPONSE_FILE_FOR_INCLUDES OFF)
# 查找依赖
find_package(CUDAToolkit REQUIRED)
find_package(OpenCV REQUIRED)
find_package(Eigen3 REQUIRED)
# 收集源码
file(GLOB_RECURSE CUDA_INFER_SRC
${CMAKE_CURRENT_SOURCE_DIR}/*.cpp
${CMAKE_CURRENT_SOURCE_DIR}/*.cu
)
# 添加静态库
add_library(cuda_infer STATIC ${CUDA_INFER_SRC})
set_target_properties(cuda_infer PROPERTIES POSITION_INDEPENDENT_CODE ON)
# 设置包含路径
target_include_directories(cuda_infer PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}
${CUDAToolkit_INCLUDE_DIRS}
${TensorRT_INCLUDE_DIR}
${EIGEN3_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)
# 设置 CUDA 编译选项
target_compile_options(cuda_infer PRIVATE
$<$<COMPILE_LANGUAGE:CUDA>:
--generate-code=arch=compute_86,code=sm_86
-Xcompiler=-fPIC
-O3
-w
-Wno-deprecated-gpu-targets
-Wno-error=deprecated-declarations
>
)
# 链接库
target_link_libraries(cuda_infer PRIVATE
${OpenCV_LIBS}
CUDA::cudart
TensorRT::TensorRT
)

323
wust_vision-main/cuda_infer/armor_infer.cu Normal file
View File

@@ -0,0 +1,323 @@
// armor_cuda_infer.cu
#include "armor_infer.hpp"
#include "letter_box.hpp"
#include <cmath>
#include <cstdio>
#include <cuda_fp16.h>
#include <opencv2/core/hal/interface.h>
#include <thrust/device_ptr.h>
#include <thrust/sort.h>
#define CUDA_CHECK(call) \
do { \
cudaError_t err = call; \
if (err != cudaSuccess) { \
fprintf( \
stderr, \
"CUDA error at %s:%d: %s\n", \
__FILE__, \
__LINE__, \
cudaGetErrorString(err) \
); \
exit(EXIT_FAILURE); \
} \
} while (0)
namespace armor_cuda_infer {
__global__ void nchw_float_to_hwc_uchar4(
const float* __restrict__ src,
uchar4* __restrict__ dst,
int W,
int H,
float norm
) {
const int x = blockIdx.x * blockDim.x + threadIdx.x;
const int y = blockIdx.y * blockDim.y + threadIdx.y;
if (x >= W || y >= H)
return;
const int idx = y * W + x;
const int plane = W * H;
float r = __ldg(src + idx + plane * 0);
float g = __ldg(src + idx + plane * 1);
float b = __ldg(src + idx + plane * 2);
r = fminf(fmaxf(r / norm, 0.f), 255.f);
g = fminf(fmaxf(g / norm, 0.f), 255.f);
b = fminf(fmaxf(b / norm, 0.f), 255.f);
dst[idx] = make_uchar4((unsigned char)b, (unsigned char)g, (unsigned char)r, 255);
}
cv::Mat CudaInfer::tensorToMat(float* d_nchw, int W, int H, float norm, cudaStream_t stream) const {
static uchar4* d_hwc = nullptr;
static size_t cap = 0;
const size_t need = W * H * sizeof(uchar4);
if (cap < need) {
if (d_hwc)
cudaFree(d_hwc);
cudaMalloc(&d_hwc, need);
cap = need;
}
const dim3 block(TILE_W, TILE_H);
const dim3 grid((W + block.x - 1) / block.x, (H + block.y - 1) / block.y);
nchw_float_to_hwc_uchar4<<<grid, block, 0, stream>>>(d_nchw, d_hwc, W, H, norm);
cv::Mat img(H, W, CV_8UC4);
cudaMemcpyAsync(img.data, d_hwc, need, cudaMemcpyDeviceToHost, stream);
// cudaStreamSynchronize(stream);
return img;
}
CudaInfer::CudaInfer() = default;
CudaInfer::~CudaInfer() {
release();
}
void CudaInfer::init(int max_src_w, int max_src_h, int input_w, int input_h) {
input_w_ = input_w;
input_h_ = input_h;
max_src_h_ = max_src_h;
max_src_w_ = max_src_w;
rellocMem();
}
void CudaInfer::rellocMem() {
CUDA_CHECK(cudaMalloc(&d_input_bgr_, max_src_w_ * max_src_h_ * 3 * sizeof(unsigned char)));
CUDA_CHECK(cudaMallocPitch(
&d_input_bgr_pitched_,
&input_pitch_bytes_,
max_src_w_ * 3 * sizeof(unsigned char),
max_src_h_
));
CUDA_CHECK(cudaMalloc(&d_nchw_, input_w_ * input_h_ * 3 * sizeof(float)));
printf("Relloc memory for CudaInfer\n");
}
void CudaInfer::getOutEnoughMem(int img_w, int img_h) {
if (img_w > max_src_w_ || img_h > max_src_h_) {
if (img_w > max_src_w_) {
max_src_w_ = img_w;
}
if (img_h > max_src_h_) {
max_src_h_ = img_h;
}
rellocMem();
}
}
void CudaInfer::release() {
if (d_input_bgr_)
cudaFree(d_input_bgr_), d_input_bgr_ = nullptr;
if (d_input_bgr_pitched_)
cudaFree(d_input_bgr_pitched_), d_input_bgr_pitched_ = nullptr;
if (d_nchw_)
cudaFree(d_nchw_), d_nchw_ = nullptr;
}
float* CudaInfer::preprocess(
const unsigned char* input_bgr_host,
int img_w,
int img_h,
float norm,
bool swap_rb,
Eigen::Matrix3f& tf_matrix,
cudaStream_t stream
) {
if (!isInitialized()) {
throw std::runtime_error("CudaInfer not initialized properly.");
}
if (!input_bgr_host || !d_input_bgr_ || !d_nchw_) {
fprintf(stderr, "[Error] Null pointer in preprocess input\n");
return nullptr;
}
getOutEnoughMem(img_w, img_h);
float scale = fminf(input_w_ / (float)img_w, input_h_ / (float)img_h);
int rw = round(img_w * scale), rh = round(img_h * scale);
int pad_l = (input_w_ - rw) / 2, pad_t = (input_h_ - rh) / 2;
tf_matrix << 1.f / scale, 0, -pad_l / scale, 0, 1.f / scale, -pad_t / scale, 0, 0, 1;
size_t img_size = img_w * img_h * 3;
CUDA_CHECK(
cudaMemcpyAsync(d_input_bgr_, input_bgr_host, img_size, cudaMemcpyHostToDevice, stream)
);
dim3 threads(TILE_W, TILE_H);
dim3 blocks((input_w_ + TILE_W - 1) / TILE_W, (input_h_ + TILE_H - 1) / TILE_H);
letterbox_kernel_shared<<<blocks, threads, 0, stream>>>(
d_input_bgr_,
img_w,
img_h,
d_nchw_,
input_w_,
input_h_,
scale,
pad_t,
pad_l,
norm,
swap_rb
);
CUDA_CHECK(cudaGetLastError());
return d_nchw_;
}
float* CudaInfer::preprocess_gpu(
const unsigned char* input_bgr_device,
int img_w,
int img_h,
float norm,
bool swap_rb,
Eigen::Matrix3f& tf_matrix,
cudaStream_t stream
) {
if (!isInitialized()) {
throw std::runtime_error("CudaInfer not initialized properly.");
}
if (!input_bgr_device || !d_nchw_) {
fprintf(stderr, "[Error] Null pointer in preprocess input\n");
return nullptr;
}
getOutEnoughMem(img_w, img_h);
float scale = fminf(input_w_ / (float)img_w, input_h_ / (float)img_h);
int rw = round(img_w * scale), rh = round(img_h * scale);
int pad_l = (input_w_ - rw) / 2, pad_t = (input_h_ - rh) / 2;
tf_matrix << 1.f / scale, 0, -pad_l / scale, 0, 1.f / scale, -pad_t / scale, 0, 0, 1;
size_t img_size = img_w * img_h * 3;
dim3 threads(TILE_W, TILE_H);
dim3 blocks((input_w_ + TILE_W - 1) / TILE_W, (input_h_ + TILE_H - 1) / TILE_H);
letterbox_kernel_shared<<<blocks, threads, 0, stream>>>(
input_bgr_device,
img_w,
img_h,
d_nchw_,
input_w_,
input_h_,
scale,
pad_t,
pad_l,
norm,
swap_rb
);
CUDA_CHECK(cudaGetLastError());
return d_nchw_;
}
float* CudaInfer::preprocess_pitched(
const unsigned char* input_bgr_host,
int img_w,
int img_h,
int host_step,
float norm,
bool swap_rb,
Eigen::Matrix3f& tf_matrix,
cudaStream_t stream
) {
if (!isInitialized()) {
throw std::runtime_error("CudaInfer not initialized properly.");
}
if (!input_bgr_host || !d_nchw_) {
fprintf(stderr, "[Error] Null pointer in preprocess input\n");
return nullptr;
}
getOutEnoughMem(img_w, img_h);
float scale = fminf((float)input_w_ / img_w, (float)input_h_ / img_h);
int rw = round(img_w * scale);
int rh = round(img_h * scale);
int pad_l = (input_w_ - rw) / 2;
int pad_t = (input_h_ - rh) / 2;
tf_matrix << 1.f / scale, 0, -pad_l / scale, 0, 1.f / scale, -pad_t / scale, 0, 0, 1;
CUDA_CHECK(cudaMemcpy2DAsync(
d_input_bgr_pitched_,
input_pitch_bytes_,
input_bgr_host,
host_step,
img_w * 3,
img_h,
cudaMemcpyHostToDevice,
stream
));
dim3 threads(TILE_W, TILE_H);
dim3 blocks((input_w_ + TILE_W - 1) / TILE_W, (input_h_ + TILE_H - 1) / TILE_H);
letterbox_kernel_pitched<<<blocks, threads, 0, stream>>>(
d_input_bgr_pitched_,
input_pitch_bytes_,
img_w,
img_h,
d_nchw_,
input_w_,
input_h_,
scale,
pad_t,
pad_l,
norm,
swap_rb
);
CUDA_CHECK(cudaGetLastError());
return d_nchw_;
}
float* CudaInfer::preprocess_pitched_gpu(
const unsigned char* input_bgr_device,
int img_w,
int img_h,
int input_step,
float norm,
bool swap_rb,
Eigen::Matrix3f& tf_matrix,
cudaStream_t stream
) {
if (!isInitialized()) {
throw std::runtime_error("CudaInfer not initialized properly.");
}
if (!input_bgr_device || !d_nchw_) {
fprintf(stderr, "[Error] Null pointer in preprocess_pitched_gpu\n");
return nullptr;
}
getOutEnoughMem(img_w, img_h);
float scale = fminf(static_cast<float>(input_w_) / img_w, static_cast<float>(input_h_) / img_h);
int rw = static_cast<int>(roundf(img_w * scale));
int rh = static_cast<int>(roundf(img_h * scale));
int pad_l = (input_w_ - rw) / 2;
int pad_t = (input_h_ - rh) / 2;
tf_matrix << 1.f / scale, 0.f, -pad_l / scale, 0.f, 1.f / scale, -pad_t / scale, 0.f, 0.f, 1.f;
dim3 threads(TILE_W, TILE_H);
dim3 blocks((input_w_ + TILE_W - 1) / TILE_W, (input_h_ + TILE_H - 1) / TILE_H);
letterbox_kernel_pitched<<<blocks, threads, 0, stream>>>(
input_bgr_device,
input_step,
img_w,
img_h,
d_nchw_,
input_w_,
input_h_,
scale,
pad_t,
pad_l,
norm,
swap_rb
);
CUDA_CHECK(cudaGetLastError());
return d_nchw_;
}
} // namespace armor_cuda_infer

78
wust_vision-main/cuda_infer/armor_infer.hpp Normal file
View File

@@ -0,0 +1,78 @@
// armor_cuda_infer.hpp
#pragma once
#include <Eigen/Dense>
#include <NvInferRuntime.h>
#include <cuda_fp16.h>
#include <cuda_runtime.h>
#include <iostream>
#include <opencv2/core/mat.hpp>
#include <vector>
namespace armor_cuda_infer {
class CudaInfer {
public:
CudaInfer();
~CudaInfer() noexcept;
void init(int max_src_w, int max_src_h, int input_w, int input_h);
void release();
bool isInitialized() const {
return d_input_bgr_ && d_nchw_ && d_input_bgr_pitched_;
}
void getOutEnoughMem(int img_w, int img_h);
void rellocMem();
float* preprocess(
const unsigned char* input_bgr_host,
int img_w,
int img_h,
float norm,
bool swap_rb,
Eigen::Matrix3f& tf_matrix,
cudaStream_t stream
);
float* preprocess_pitched(
const unsigned char* input_bgr_host,
int img_w,
int img_h,
int host_step,
float norm,
bool swap_rb,
Eigen::Matrix3f& tf_matrix,
cudaStream_t stream
);
float* preprocess_gpu(
const unsigned char* input_bgr_device,
int img_w,
int img_h,
float norm,
bool swap_rb,
Eigen::Matrix3f& tf_matrix,
cudaStream_t stream
);
float* preprocess_pitched_gpu(
const unsigned char* input_bgr_device,
int img_w,
int img_h,
int host_step,
float norm,
bool swap_rb,
Eigen::Matrix3f& tf_matrix,
cudaStream_t stream
);
cv::Mat tensorToMat(float* d_nchw, int W, int H, float norm, cudaStream_t stream) const;
private:
CudaInfer(const CudaInfer&) = delete;
CudaInfer& operator=(const CudaInfer&) = delete;
unsigned char* d_input_bgr_ = nullptr;
float* d_nchw_ = nullptr;
unsigned char* d_input_bgr_pitched_ = nullptr;
size_t input_pitch_bytes_ = 0;
int input_w_;
int input_h_;
int max_src_w_, max_src_h_;
};
} // namespace armor_cuda_infer

147
wust_vision-main/cuda_infer/letter_box.cu Normal file
View File

@@ -0,0 +1,147 @@
#include "letter_box.hpp"
__global__ void letterbox_kernel_shared(
const uchar* __restrict__ input_bgr,
int in_w,
int in_h,
float* __restrict__ output_nchw,
int out_w,
int out_h,
float scale,
int pad_t,
int pad_l,
float norm,
bool swap_rb
) {
// global x/y
int x = blockIdx.x * TILE_W + threadIdx.x;
int y = blockIdx.y * TILE_H + threadIdx.y;
if (x >= out_w || y >= out_h)
return;
// 共享内存 + halo
__shared__ uchar4 smem[TILE_H + 1][TILE_W + 1];
int tid = threadIdx.y * blockDim.x + threadIdx.x;
int total_smem = (TILE_W + 1) * (TILE_H + 1);
int threads_per_block = blockDim.x * blockDim.y;
int iter = (total_smem + threads_per_block - 1) / threads_per_block;
float inv_scale = 1.0f / scale;
float block_start_x = blockIdx.x * TILE_W - pad_l;
float block_start_y = blockIdx.y * TILE_H - pad_t;
// load shared memory
for (int i = 0; i < iter; i++) {
int idx = tid + i * threads_per_block;
if (idx < total_smem) {
int sx = idx % (TILE_W + 1);
int sy = idx / (TILE_W + 1);
float in_x = (block_start_x + sx) * inv_scale;
float in_y = (block_start_y + sy) * inv_scale;
int ix = floorf(in_x);
int iy = floorf(in_y);
uchar4 p = make_uchar4(114, 114, 114, 0); // padding BGR
if (ix >= 0 && iy >= 0 && ix < in_w && iy < in_h) {
int offset = (iy * in_w + ix) * 3;
p.x = input_bgr[offset]; // b
p.y = input_bgr[offset + 1]; // g
p.z = input_bgr[offset + 2]; // r
}
smem[sy][sx] = p;
}
}
__syncthreads();
// 双线性插值
float in_x = (x - pad_l) * inv_scale;
float in_y = (y - pad_t) * inv_scale;
int tx = threadIdx.x;
int ty = threadIdx.y;
float dx = in_x - floorf(in_x);
float dy = in_y - floorf(in_y);
float dx1 = 1.0f - dx, dy1 = 1.0f - dy;
uchar4 p00 = smem[ty][tx];
uchar4 p01 = smem[ty][tx + 1];
uchar4 p10 = smem[ty + 1][tx];
uchar4 p11 = smem[ty + 1][tx + 1];
float out_r = dx1 * dy1 * p00.z + dx * dy1 * p01.z + dx1 * dy * p10.z + dx * dy * p11.z;
float out_g = dx1 * dy1 * p00.y + dx * dy1 * p01.y + dx1 * dy * p10.y + dx * dy * p11.y;
float out_b = dx1 * dy1 * p00.x + dx * dy1 * p01.x + dx1 * dy * p10.x + dx * dy * p11.x;
int out_idx = y * out_w + x;
if (swap_rb) {
output_nchw[out_idx + 0 * out_w * out_h] = out_r * norm;
output_nchw[out_idx + 1 * out_w * out_h] = out_g * norm;
output_nchw[out_idx + 2 * out_w * out_h] = out_b * norm;
} else {
output_nchw[out_idx + 0 * out_w * out_h] = out_b * norm;
output_nchw[out_idx + 1 * out_w * out_h] = out_g * norm;
output_nchw[out_idx + 2 * out_w * out_h] = out_r * norm;
}
}
__global__ void letterbox_kernel_pitched(
const unsigned char* __restrict__ d_input_bgr,
size_t pitch,
int src_w,
int src_h,
float* __restrict__ d_nchw,
int OUT_W,
int OUT_H,
float scale,
int pad_t,
int pad_l,
float norm,
bool swap_rb
) {
int ox = blockIdx.x * blockDim.x + threadIdx.x;
int oy = blockIdx.y * blockDim.y + threadIdx.y;
if (ox >= OUT_W || oy >= OUT_H)
return;
float fx = (ox - pad_l) / scale;
float fy = (oy - pad_t) / scale;
int out_idx = oy * OUT_W + ox;
int plane = OUT_W * OUT_H;
// clamp coordinates
fx = fmaxf(0.f, fminf(fx, src_w - 2.f));
fy = fmaxf(0.f, fminf(fy, src_h - 2.f));
int x0 = floorf(fx), y0 = floorf(fy);
int x1 = x0 + 1, y1 = y0 + 1;
float dx = fx - x0, dy = fy - y0;
float dx1 = 1.f - dx, dy1 = 1.f - dy;
// row pointers
const uchar3* row0 = (const uchar3*)((const char*)d_input_bgr + y0 * pitch);
const uchar3* row1 = (const uchar3*)((const char*)d_input_bgr + y1 * pitch);
uchar3 p00 = row0[x0];
uchar3 p01 = row0[x1];
uchar3 p10 = row1[x0];
uchar3 p11 = row1[x1];
// bilinear interpolation
float r = dx1 * dy1 * p00.z + dx * dy1 * p01.z + dx1 * dy * p10.z + dx * dy * p11.z;
float g = dx1 * dy1 * p00.y + dx * dy1 * p01.y + dx1 * dy * p10.y + dx * dy * p11.y;
float b = dx1 * dy1 * p00.x + dx * dy1 * p01.x + dx1 * dy * p10.x + dx * dy * p11.x;
if (swap_rb) {
d_nchw[out_idx + 0 * plane] = r * norm;
d_nchw[out_idx + 1 * plane] = g * norm;
d_nchw[out_idx + 2 * plane] = b * norm;
} else {
d_nchw[out_idx + 0 * plane] = b * norm;
d_nchw[out_idx + 1 * plane] = g * norm;
d_nchw[out_idx + 2 * plane] = r * norm;
}
}

42
wust_vision-main/cuda_infer/letter_box.hpp Normal file
View File

@@ -0,0 +1,42 @@
#pragma once
#include <Eigen/Dense>
#include <NvInferRuntime.h>
#include <cmath>
#include <cstdio>
#include <cuda_fp16.h>
#include <cuda_runtime.h>
#include <iostream>
#include <opencv2/core/hal/interface.h>
#include <opencv2/core/mat.hpp>
#include <thrust/device_ptr.h>
#include <thrust/sort.h>
#include <vector>
static constexpr int TILE_W = 32;
static constexpr int TILE_H = 32;
__global__ void letterbox_kernel_shared(
const uchar* __restrict__ input_bgr,
int in_w,
int in_h,
float* __restrict__ output_nchw,
int out_w,
int out_h,
float scale,
int pad_t,
int pad_l,
float norm,
bool swap_rb
);
__global__ void letterbox_kernel_pitched(
const unsigned char* __restrict__ d_input_bgr,
size_t pitch,
int src_w,
int src_h,
float* __restrict__ d_nchw,
int OUT_W,
int OUT_H,
float scale,
int pad_t,
int pad_l,
float norm,
bool swap_rb
);

19
wust_vision-main/env.bash Normal file
View File

@@ -0,0 +1,19 @@
# #!/bin/bash
export MVCAM_SDK_PATH=/opt/MVS
export MVCAM_COMMON_RUNENV=/opt/MVS/lib
export MVCAM_GENICAM_CLPROTOCOL=/opt/MVS/lib/CLProtocol
export ALLUSERSPROFILE=/opt/MVS/MVFG
export LD_LIBRARY_PATH=/opt/MVS/lib/64:/opt/MVS/lib/32:$LD_LIBRARY_PATH
export MVCAM_SDK_PATH=/opt/MVS
export MVCAM_SDK_VERSION=
export MVCAM_COMMON_RUNENV=/opt/MVS/lib
export MVCAM_GENICAM_CLPROTOCOL=/opt/MVS/lib/CLProtocol
export ALLUSERSPROFILE=/opt/MVS/MVFG
export LD_LIBRARY_PATH=/opt/MVS/lib/aarch64:$LD_LIBRARY_PATH
WORK_DIR="$(dirname "$(realpath "${BASH_SOURCE[0]}")")"
export VISION_ROOT="$WORK_DIR"

3
wust_vision-main/format.sh Executable file
View File

@@ -0,0 +1,3 @@
find . -path ./build -prune -o \
-type f \( -name '*.h' -o -name '*.hpp' -o -name '*.c' -o -name '*.cu' -o -name '*.cpp' \) \
-exec clang-format -i {} +

BIN
wust_vision-main/model/0526.engine Normal file
View File

Binary file not shown.

BIN
wust_vision-main/model/0526.onnx Normal file
View File

Binary file not shown.

BIN
wust_vision-main/model/0708.engine Normal file
View File

Binary file not shown.

BIN
wust_vision-main/model/0708.onnx Normal file
View File

Binary file not shown.

9
wust_vision-main/model/label.txt Normal file
View File

@@ -0,0 +1,9 @@
1
2
3
4
5
outpost
guard
base
negative

BIN
wust_vision-main/model/lenet.onnx Normal file
View File

Binary file not shown.

BIN
wust_vision-main/model/mlp.onnx Normal file
View File

Binary file not shown.

BIN
wust_vision-main/model/opt-0527-001.onnx Normal file
View File

Binary file not shown.

BIN
wust_vision-main/model/opt-1208-001.bin Normal file
View File

Binary file not shown.

BIN
wust_vision-main/model/opt-1208-001.engine Normal file
View File

Binary file not shown.

BIN
wust_vision-main/model/opt-1208-001.onnx Normal file
View File

Binary file not shown.

218
wust_vision-main/model/opt-1208-001.param Normal file
View File

@@ -0,0 +1,218 @@
7767517
216 240
Input images 0 1 images
Convolution Conv_1 1 1 images input.4 0=16 1=6 11=6 2=1 12=1 3=2 13=2 4=2 14=2 15=2 16=2 5=1 6=1728
HardSwish HardSwish_2 1 1 input.4 onnx::Conv_483 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_3 1 1 onnx::Conv_483 input.12 0=16 1=3 11=3 2=1 12=1 3=2 13=2 4=1 14=1 15=1 16=1 5=1 6=144 7=16
Convolution Conv_4 1 1 input.12 input.20 0=32 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=512
HardSwish HardSwish_5 1 1 input.20 onnx::Conv_488 0=2.000000e-01 1=5.000000e-01
Split splitncnn_0 1 2 onnx::Conv_488 onnx::Conv_488_splitncnn_0 onnx::Conv_488_splitncnn_1
ConvolutionDepthWise Conv_6 1 1 onnx::Conv_488_splitncnn_1 input.28 0=32 1=3 11=3 2=1 12=1 3=2 13=2 4=1 14=1 15=1 16=1 5=1 6=288 7=32
Convolution Conv_7 1 1 input.28 input.36 0=16 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=512
HardSwish HardSwish_8 1 1 input.36 onnx::Concat_493 0=2.000000e-01 1=5.000000e-01
Convolution Conv_9 1 1 onnx::Conv_488_splitncnn_0 input.44 0=16 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=512
HardSwish HardSwish_10 1 1 input.44 onnx::Conv_496 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_11 1 1 onnx::Conv_496 input.52 0=16 1=3 11=3 2=1 12=1 3=2 13=2 4=1 14=1 15=1 16=1 5=1 6=144 7=16
Convolution Conv_12 1 1 input.52 input.60 0=48 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=768
HardSwish HardSwish_13 1 1 input.60 onnx::Concat_501 0=2.000000e-01 1=5.000000e-01
Concat Concat_14 2 1 onnx::Concat_493 onnx::Concat_501 x 0=0
ShuffleChannel Reshape_19 1 1 x onnx::Slice_507 0=2 1=0
Split splitncnn_1 1 2 onnx::Slice_507 onnx::Slice_507_splitncnn_0 onnx::Slice_507_splitncnn_1
Crop Slice_24 1 1 onnx::Slice_507_splitncnn_1 onnx::Concat_512 -23309=1,0 -23310=1,32 -23311=1,0
Crop Slice_29 1 1 onnx::Slice_507_splitncnn_0 onnx::Conv_517 -23309=1,32 -23310=1,2147483647 -23311=1,0
Convolution Conv_30 1 1 onnx::Conv_517 input.68 0=32 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=1024
HardSwish HardSwish_31 1 1 input.68 onnx::Conv_520 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_32 1 1 onnx::Conv_520 input.76 0=32 1=3 11=3 2=1 12=1 3=1 13=1 4=1 14=1 15=1 16=1 5=1 6=288 7=32
Convolution Conv_33 1 1 input.76 input.84 0=32 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=1024
HardSwish HardSwish_34 1 1 input.84 onnx::Concat_525 0=2.000000e-01 1=5.000000e-01
Concat Concat_35 2 1 onnx::Concat_512 onnx::Concat_525 x.3 0=0
ShuffleChannel Reshape_40 1 1 x.3 onnx::Slice_531 0=2 1=0
Split splitncnn_2 1 2 onnx::Slice_531 onnx::Slice_531_splitncnn_0 onnx::Slice_531_splitncnn_1
Crop Slice_45 1 1 onnx::Slice_531_splitncnn_1 onnx::Concat_536 -23309=1,0 -23310=1,32 -23311=1,0
Crop Slice_50 1 1 onnx::Slice_531_splitncnn_0 onnx::Conv_541 -23309=1,32 -23310=1,2147483647 -23311=1,0
Convolution Conv_51 1 1 onnx::Conv_541 input.92 0=32 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=1024
HardSwish HardSwish_52 1 1 input.92 onnx::Conv_544 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_53 1 1 onnx::Conv_544 input.100 0=32 1=3 11=3 2=1 12=1 3=1 13=1 4=1 14=1 15=1 16=1 5=1 6=288 7=32
Convolution Conv_54 1 1 input.100 input.108 0=32 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=1024
HardSwish HardSwish_55 1 1 input.108 onnx::Concat_549 0=2.000000e-01 1=5.000000e-01
Concat Concat_56 2 1 onnx::Concat_536 onnx::Concat_549 x.7 0=0
ShuffleChannel Reshape_61 1 1 x.7 input.112 0=2 1=0
Split splitncnn_3 1 4 input.112 input.112_splitncnn_0 input.112_splitncnn_1 input.112_splitncnn_2 input.112_splitncnn_3
ConvolutionDepthWise Conv_62 1 1 input.112_splitncnn_3 input.120 0=64 1=3 11=3 2=1 12=1 3=2 13=2 4=1 14=1 15=1 16=1 5=1 6=576 7=64
Convolution Conv_63 1 1 input.120 input.128 0=32 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=2048
HardSwish HardSwish_64 1 1 input.128 onnx::Concat_560 0=2.000000e-01 1=5.000000e-01
Convolution Conv_65 1 1 input.112_splitncnn_2 input.136 0=32 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=2048
HardSwish HardSwish_66 1 1 input.136 onnx::Conv_563 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_67 1 1 onnx::Conv_563 input.144 0=32 1=3 11=3 2=1 12=1 3=2 13=2 4=1 14=1 15=1 16=1 5=1 6=288 7=32
Convolution Conv_68 1 1 input.144 input.152 0=96 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=3072
HardSwish HardSwish_69 1 1 input.152 onnx::Concat_568 0=2.000000e-01 1=5.000000e-01
Concat Concat_70 2 1 onnx::Concat_560 onnx::Concat_568 x.11 0=0
ShuffleChannel Reshape_75 1 1 x.11 onnx::Slice_574 0=2 1=0
Split splitncnn_4 1 2 onnx::Slice_574 onnx::Slice_574_splitncnn_0 onnx::Slice_574_splitncnn_1
Crop Slice_80 1 1 onnx::Slice_574_splitncnn_1 onnx::Concat_579 -23309=1,0 -23310=1,64 -23311=1,0
Crop Slice_85 1 1 onnx::Slice_574_splitncnn_0 onnx::Conv_584 -23309=1,64 -23310=1,2147483647 -23311=1,0
Convolution Conv_86 1 1 onnx::Conv_584 input.160 0=64 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=4096
HardSwish HardSwish_87 1 1 input.160 onnx::Conv_587 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_88 1 1 onnx::Conv_587 input.168 0=64 1=3 11=3 2=1 12=1 3=1 13=1 4=1 14=1 15=1 16=1 5=1 6=576 7=64
Convolution Conv_89 1 1 input.168 input.176 0=64 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=4096
HardSwish HardSwish_90 1 1 input.176 onnx::Concat_592 0=2.000000e-01 1=5.000000e-01
Concat Concat_91 2 1 onnx::Concat_579 onnx::Concat_592 x.15 0=0
ShuffleChannel Reshape_96 1 1 x.15 onnx::Slice_598 0=2 1=0
Split splitncnn_5 1 2 onnx::Slice_598 onnx::Slice_598_splitncnn_0 onnx::Slice_598_splitncnn_1
Crop Slice_101 1 1 onnx::Slice_598_splitncnn_1 onnx::Concat_603 -23309=1,0 -23310=1,64 -23311=1,0
Crop Slice_106 1 1 onnx::Slice_598_splitncnn_0 onnx::Conv_608 -23309=1,64 -23310=1,2147483647 -23311=1,0
Convolution Conv_107 1 1 onnx::Conv_608 input.184 0=64 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=4096
HardSwish HardSwish_108 1 1 input.184 onnx::Conv_611 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_109 1 1 onnx::Conv_611 input.192 0=64 1=3 11=3 2=1 12=1 3=1 13=1 4=1 14=1 15=1 16=1 5=1 6=576 7=64
Convolution Conv_110 1 1 input.192 input.200 0=64 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=4096
HardSwish HardSwish_111 1 1 input.200 onnx::Concat_616 0=2.000000e-01 1=5.000000e-01
Concat Concat_112 2 1 onnx::Concat_603 onnx::Concat_616 x.19 0=0
ShuffleChannel Reshape_117 1 1 x.19 input.204 0=2 1=0
Split splitncnn_6 1 3 input.204 input.204_splitncnn_0 input.204_splitncnn_1 input.204_splitncnn_2
ConvolutionDepthWise Conv_118 1 1 input.204_splitncnn_2 input.212 0=128 1=3 11=3 2=1 12=1 3=2 13=2 4=1 14=1 15=1 16=1 5=1 6=1152 7=128
Convolution Conv_119 1 1 input.212 input.220 0=64 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=8192
HardSwish HardSwish_120 1 1 input.220 onnx::Concat_627 0=2.000000e-01 1=5.000000e-01
Convolution Conv_121 1 1 input.204_splitncnn_1 input.228 0=64 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=8192
HardSwish HardSwish_122 1 1 input.228 onnx::Conv_630 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_123 1 1 onnx::Conv_630 input.236 0=64 1=3 11=3 2=1 12=1 3=2 13=2 4=1 14=1 15=1 16=1 5=1 6=576 7=64
Convolution Conv_124 1 1 input.236 input.244 0=192 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=12288
HardSwish HardSwish_125 1 1 input.244 onnx::Concat_635 0=2.000000e-01 1=5.000000e-01
Concat Concat_126 2 1 onnx::Concat_627 onnx::Concat_635 x.23 0=0
ShuffleChannel Reshape_131 1 1 x.23 onnx::Slice_641 0=2 1=0
Split splitncnn_7 1 2 onnx::Slice_641 onnx::Slice_641_splitncnn_0 onnx::Slice_641_splitncnn_1
Crop Slice_136 1 1 onnx::Slice_641_splitncnn_1 onnx::Concat_646 -23309=1,0 -23310=1,128 -23311=1,0
Crop Slice_141 1 1 onnx::Slice_641_splitncnn_0 onnx::Conv_651 -23309=1,128 -23310=1,2147483647 -23311=1,0
Convolution Conv_142 1 1 onnx::Conv_651 input.252 0=128 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=16384
HardSwish HardSwish_143 1 1 input.252 onnx::Conv_654 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_144 1 1 onnx::Conv_654 input.260 0=128 1=3 11=3 2=1 12=1 3=1 13=1 4=1 14=1 15=1 16=1 5=1 6=1152 7=128
Convolution Conv_145 1 1 input.260 input.268 0=128 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=16384
HardSwish HardSwish_146 1 1 input.268 onnx::Concat_659 0=2.000000e-01 1=5.000000e-01
Concat Concat_147 2 1 onnx::Concat_646 onnx::Concat_659 x.27 0=0
ShuffleChannel Reshape_152 1 1 x.27 onnx::Slice_665 0=2 1=0
Split splitncnn_8 1 2 onnx::Slice_665 onnx::Slice_665_splitncnn_0 onnx::Slice_665_splitncnn_1
Crop Slice_157 1 1 onnx::Slice_665_splitncnn_1 onnx::Concat_670 -23309=1,0 -23310=1,128 -23311=1,0
Crop Slice_162 1 1 onnx::Slice_665_splitncnn_0 onnx::Conv_675 -23309=1,128 -23310=1,2147483647 -23311=1,0
Convolution Conv_163 1 1 onnx::Conv_675 input.276 0=128 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=16384
HardSwish HardSwish_164 1 1 input.276 onnx::Conv_678 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_165 1 1 onnx::Conv_678 input.284 0=128 1=3 11=3 2=1 12=1 3=1 13=1 4=1 14=1 15=1 16=1 5=1 6=1152 7=128
Convolution Conv_166 1 1 input.284 input.292 0=128 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=16384
HardSwish HardSwish_167 1 1 input.292 onnx::Concat_683 0=2.000000e-01 1=5.000000e-01
Concat Concat_168 2 1 onnx::Concat_670 onnx::Concat_683 x.31 0=0
ShuffleChannel Reshape_173 1 1 x.31 input.296 0=2 1=0
Convolution Conv_174 1 1 input.296 input.304 0=128 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=32768
HardSwish HardSwish_175 1 1 input.304 onnx::Conv_692 0=2.000000e-01 1=5.000000e-01
Split splitncnn_9 1 2 onnx::Conv_692 onnx::Conv_692_splitncnn_0 onnx::Conv_692_splitncnn_1
Convolution Conv_176 1 1 onnx::Conv_692_splitncnn_1 input.312 0=64 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=8192
HardSwish HardSwish_177 1 1 input.312 onnx::Resize_695 0=2.000000e-01 1=5.000000e-01
Interp Resize_178 1 1 onnx::Resize_695 onnx::Concat_700 0=1 1=2.000000e+00 2=2.000000e+00 3=0 4=0 6=0
ConvolutionDepthWise Conv_179 1 1 input.112_splitncnn_1 input.320 0=64 1=3 11=3 2=1 12=1 3=2 13=2 4=1 14=1 15=1 16=1 5=1 6=576 7=64
Convolution Conv_180 1 1 input.320 input.328 0=64 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=4096
HardSwish HardSwish_181 1 1 input.328 onnx::Concat_705 0=2.000000e-01 1=5.000000e-01
Concat Concat_182 3 1 onnx::Concat_705 input.204_splitncnn_0 onnx::Concat_700 input.332 0=0
Split splitncnn_10 1 2 input.332 input.332_splitncnn_0 input.332_splitncnn_1
Convolution Conv_183 1 1 input.332_splitncnn_1 input.340 0=64 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=16384
HardSwish HardSwish_184 1 1 input.340 onnx::Conv_709 0=2.000000e-01 1=5.000000e-01
Convolution Conv_185 1 1 input.332_splitncnn_0 input.348 0=64 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=16384
HardSwish HardSwish_186 1 1 input.348 onnx::Concat_712 0=2.000000e-01 1=5.000000e-01
Convolution Conv_187 1 1 onnx::Conv_709 input.356 0=64 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=4096
HardSwish HardSwish_188 1 1 input.356 onnx::Conv_715 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_189 1 1 onnx::Conv_715 input.364 0=64 1=3 11=3 2=1 12=1 3=1 13=1 4=1 14=1 15=1 16=1 5=1 6=576 7=64
Convolution Conv_190 1 1 input.364 input.372 0=64 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=4096
HardSwish HardSwish_191 1 1 input.372 onnx::Concat_720 0=2.000000e-01 1=5.000000e-01
Concat Concat_192 2 1 onnx::Concat_720 onnx::Concat_712 input.376 0=0
Convolution Conv_193 1 1 input.376 input.384 0=128 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=16384
HardSwish HardSwish_194 1 1 input.384 onnx::Conv_724 0=2.000000e-01 1=5.000000e-01
Split splitncnn_11 1 3 onnx::Conv_724 onnx::Conv_724_splitncnn_0 onnx::Conv_724_splitncnn_1 onnx::Conv_724_splitncnn_2
Convolution Conv_195 1 1 onnx::Conv_724_splitncnn_2 input.392 0=64 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=8192
HardSwish HardSwish_196 1 1 input.392 onnx::Resize_727 0=2.000000e-01 1=5.000000e-01
Interp Resize_197 1 1 onnx::Resize_727 onnx::Concat_732 0=1 1=2.000000e+00 2=2.000000e+00 3=0 4=0 6=0
Concat Concat_198 2 1 input.112_splitncnn_0 onnx::Concat_732 input.396 0=0
Split splitncnn_12 1 2 input.396 input.396_splitncnn_0 input.396_splitncnn_1
Convolution Conv_199 1 1 input.396_splitncnn_1 input.404 0=32 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=4096
HardSwish HardSwish_200 1 1 input.404 onnx::Conv_736 0=2.000000e-01 1=5.000000e-01
Convolution Conv_201 1 1 input.396_splitncnn_0 input.412 0=32 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=4096
HardSwish HardSwish_202 1 1 input.412 onnx::Concat_739 0=2.000000e-01 1=5.000000e-01
Convolution Conv_203 1 1 onnx::Conv_736 input.420 0=32 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=1024
HardSwish HardSwish_204 1 1 input.420 onnx::Conv_742 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_205 1 1 onnx::Conv_742 input.428 0=32 1=3 11=3 2=1 12=1 3=1 13=1 4=1 14=1 15=1 16=1 5=1 6=288 7=32
Convolution Conv_206 1 1 input.428 input.436 0=32 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=1024
HardSwish HardSwish_207 1 1 input.436 onnx::Concat_747 0=2.000000e-01 1=5.000000e-01
Concat Concat_208 2 1 onnx::Concat_747 onnx::Concat_739 input.440 0=0
Convolution Conv_209 1 1 input.440 input.448 0=64 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=4096
HardSwish HardSwish_210 1 1 input.448 onnx::Conv_751 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_211 1 1 onnx::Conv_724_splitncnn_1 input.456 0=128 1=3 11=3 2=1 12=1 3=2 13=2 4=1 14=1 15=1 16=1 5=1 6=1152 7=128
Convolution Conv_212 1 1 input.456 input.464 0=128 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=16384
HardSwish HardSwish_213 1 1 input.464 onnx::Concat_756 0=2.000000e-01 1=5.000000e-01
Concat Concat_214 2 1 onnx::Concat_756 onnx::Conv_692_splitncnn_0 input.468 0=0
Split splitncnn_13 1 2 input.468 input.468_splitncnn_0 input.468_splitncnn_1
Convolution Conv_215 1 1 input.468_splitncnn_1 input.476 0=128 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=32768
HardSwish HardSwish_216 1 1 input.476 onnx::Conv_760 0=2.000000e-01 1=5.000000e-01
Convolution Conv_217 1 1 input.468_splitncnn_0 input.484 0=128 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=32768
HardSwish HardSwish_218 1 1 input.484 onnx::Concat_763 0=2.000000e-01 1=5.000000e-01
Convolution Conv_219 1 1 onnx::Conv_760 input.492 0=128 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=16384
HardSwish HardSwish_220 1 1 input.492 onnx::Conv_766 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_221 1 1 onnx::Conv_766 input.500 0=128 1=3 11=3 2=1 12=1 3=1 13=1 4=1 14=1 15=1 16=1 5=1 6=1152 7=128
Convolution Conv_222 1 1 input.500 input.508 0=128 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=16384
HardSwish HardSwish_223 1 1 input.508 onnx::Concat_771 0=2.000000e-01 1=5.000000e-01
Concat Concat_224 2 1 onnx::Concat_771 onnx::Concat_763 input.512 0=0
Convolution Conv_225 1 1 input.512 input.520 0=256 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=65536
HardSwish HardSwish_226 1 1 input.520 onnx::Conv_775 0=2.000000e-01 1=5.000000e-01
Convolution Conv_227 1 1 onnx::Conv_751 input.528 0=64 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=4096
HardSwish HardSwish_228 1 1 input.528 onnx::Conv_778 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_229 1 1 onnx::Conv_778 input.536 0=64 1=5 11=5 2=1 12=1 3=1 13=1 4=2 14=2 15=2 16=2 5=1 6=1600 7=64
ConvolutionDepthWise Conv_230 1 1 input.536 input.544 0=128 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=4096 7=2
HardSwish HardSwish_231 1 1 input.544 onnx::Conv_783 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_232 1 1 onnx::Conv_783 input.552 0=128 1=5 11=5 2=1 12=1 3=1 13=1 4=2 14=2 15=2 16=2 5=1 6=3200 7=128
ConvolutionDepthWise Conv_233 1 1 input.552 input.560 0=128 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=8192 7=2
HardSwish HardSwish_234 1 1 input.560 onnx::Slice_788 0=2.000000e-01 1=5.000000e-01
Split splitncnn_14 1 2 onnx::Slice_788 onnx::Slice_788_splitncnn_0 onnx::Slice_788_splitncnn_1
Crop Slice_239 1 1 onnx::Slice_788_splitncnn_1 onnx::Conv_793 -23309=1,0 -23310=1,64 -23311=1,0
Split splitncnn_15 1 2 onnx::Conv_793 onnx::Conv_793_splitncnn_0 onnx::Conv_793_splitncnn_1
Crop Slice_244 1 1 onnx::Slice_788_splitncnn_0 onnx::Conv_798 -23309=1,64 -23310=1,2147483647 -23311=1,0
Convolution Conv_245 1 1 onnx::Conv_798 onnx::Sigmoid_799 0=12 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=768
Convolution Conv_246 1 1 onnx::Conv_793_splitncnn_1 onnx::Concat_800 0=8 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=512
Convolution Conv_247 1 1 onnx::Conv_793_splitncnn_0 onnx::Sigmoid_801 0=1 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=64
Sigmoid Sigmoid_248 1 1 onnx::Sigmoid_801 onnx::Concat_802
Sigmoid Sigmoid_249 1 1 onnx::Sigmoid_799 onnx::Concat_803
Concat Concat_250 3 1 onnx::Concat_800 onnx::Concat_802 onnx::Concat_803 onnx::Shape_804 0=0
Convolution Conv_251 1 1 onnx::Conv_724_splitncnn_0 input.568 0=64 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=8192
HardSwish HardSwish_252 1 1 input.568 onnx::Conv_807 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_253 1 1 onnx::Conv_807 input.576 0=64 1=5 11=5 2=1 12=1 3=1 13=1 4=2 14=2 15=2 16=2 5=1 6=1600 7=64
ConvolutionDepthWise Conv_254 1 1 input.576 input.584 0=128 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=4096 7=2
HardSwish HardSwish_255 1 1 input.584 onnx::Conv_812 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_256 1 1 onnx::Conv_812 input.592 0=128 1=5 11=5 2=1 12=1 3=1 13=1 4=2 14=2 15=2 16=2 5=1 6=3200 7=128
ConvolutionDepthWise Conv_257 1 1 input.592 input.600 0=128 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=8192 7=2
HardSwish HardSwish_258 1 1 input.600 onnx::Slice_817 0=2.000000e-01 1=5.000000e-01
Split splitncnn_16 1 2 onnx::Slice_817 onnx::Slice_817_splitncnn_0 onnx::Slice_817_splitncnn_1
Crop Slice_263 1 1 onnx::Slice_817_splitncnn_1 onnx::Conv_822 -23309=1,0 -23310=1,64 -23311=1,0
Split splitncnn_17 1 2 onnx::Conv_822 onnx::Conv_822_splitncnn_0 onnx::Conv_822_splitncnn_1
Crop Slice_268 1 1 onnx::Slice_817_splitncnn_0 onnx::Conv_827 -23309=1,64 -23310=1,2147483647 -23311=1,0
Convolution Conv_269 1 1 onnx::Conv_827 onnx::Sigmoid_828 0=12 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=768
Convolution Conv_270 1 1 onnx::Conv_822_splitncnn_1 onnx::Concat_829 0=8 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=512
Convolution Conv_271 1 1 onnx::Conv_822_splitncnn_0 onnx::Sigmoid_830 0=1 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=64
Sigmoid Sigmoid_272 1 1 onnx::Sigmoid_830 onnx::Concat_831
Sigmoid Sigmoid_273 1 1 onnx::Sigmoid_828 onnx::Concat_832
Concat Concat_274 3 1 onnx::Concat_829 onnx::Concat_831 onnx::Concat_832 onnx::Shape_833 0=0
Convolution Conv_275 1 1 onnx::Conv_775 input.608 0=64 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=16384
HardSwish HardSwish_276 1 1 input.608 onnx::Conv_836 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_277 1 1 onnx::Conv_836 input.616 0=64 1=5 11=5 2=1 12=1 3=1 13=1 4=2 14=2 15=2 16=2 5=1 6=1600 7=64
ConvolutionDepthWise Conv_278 1 1 input.616 input.624 0=128 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=4096 7=2
HardSwish HardSwish_279 1 1 input.624 onnx::Conv_841 0=2.000000e-01 1=5.000000e-01
ConvolutionDepthWise Conv_280 1 1 onnx::Conv_841 input.632 0=128 1=5 11=5 2=1 12=1 3=1 13=1 4=2 14=2 15=2 16=2 5=1 6=3200 7=128
ConvolutionDepthWise Conv_281 1 1 input.632 input.640 0=128 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=8192 7=2
HardSwish HardSwish_282 1 1 input.640 onnx::Slice_846 0=2.000000e-01 1=5.000000e-01
Split splitncnn_18 1 2 onnx::Slice_846 onnx::Slice_846_splitncnn_0 onnx::Slice_846_splitncnn_1
Crop Slice_287 1 1 onnx::Slice_846_splitncnn_1 onnx::Conv_851 -23309=1,0 -23310=1,64 -23311=1,0
Split splitncnn_19 1 2 onnx::Conv_851 onnx::Conv_851_splitncnn_0 onnx::Conv_851_splitncnn_1
Crop Slice_292 1 1 onnx::Slice_846_splitncnn_0 onnx::Conv_856 -23309=1,64 -23310=1,2147483647 -23311=1,0
Convolution Conv_293 1 1 onnx::Conv_856 onnx::Sigmoid_857 0=12 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=768
Convolution Conv_294 1 1 onnx::Conv_851_splitncnn_1 onnx::Concat_858 0=8 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=512
Convolution Conv_295 1 1 onnx::Conv_851_splitncnn_0 onnx::Sigmoid_859 0=1 1=1 11=1 2=1 12=1 3=1 13=1 4=0 14=0 15=0 16=0 5=1 6=64
Sigmoid Sigmoid_296 1 1 onnx::Sigmoid_859 onnx::Concat_860
Sigmoid Sigmoid_297 1 1 onnx::Sigmoid_857 onnx::Concat_861
Concat Concat_298 3 1 onnx::Concat_858 onnx::Concat_860 onnx::Concat_861 output.1 0=0
Reshape Reshape_306 1 1 onnx::Shape_804 onnx::Concat_870 0=-1 1=21
Reshape Reshape_314 1 1 onnx::Shape_833 onnx::Concat_878 0=-1 1=21
Reshape Reshape_322 1 1 output.1 onnx::Concat_886 0=-1 1=21
Concat Concat_323 3 1 onnx::Concat_870 onnx::Concat_878 onnx::Concat_886 onnx::Transpose_887 0=1
Permute Transpose_324 1 1 onnx::Transpose_887 output 0=1

BIN
wust_vision-main/model/opt_1208_001.ncnn.bin Normal file
View File

Binary file not shown.

193
wust_vision-main/model/opt_1208_001.ncnn.param Normal file
View File

@@ -0,0 +1,193 @@
7767517
191 215
Input in0 0 1 in0
Convolution conv_69 1 1 in0 1 0=16 1=6 11=6 12=1 13=2 14=2 2=1 3=2 4=2 5=1 6=1728
HardSwish hswish_0 1 1 1 2 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_128 1 1 2 3 0=16 1=3 11=3 12=1 13=2 14=1 2=1 3=2 4=1 5=1 6=144 7=16
Convolution conv_70 1 1 3 4 0=32 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=512
HardSwish hswish_1 1 1 4 5 0=1.666667e-01 1=5.000000e-01
Split splitncnn_0 1 2 5 6 7
ConvolutionDepthWise convdw_129 1 1 7 8 0=32 1=3 11=3 12=1 13=2 14=1 2=1 3=2 4=1 5=1 6=288 7=32
Convolution conv_71 1 1 8 9 0=16 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=512
HardSwish hswish_2 1 1 9 10 0=1.666667e-01 1=5.000000e-01
Convolution conv_72 1 1 6 11 0=16 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=512
HardSwish hswish_3 1 1 11 12 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_130 1 1 12 13 0=16 1=3 11=3 12=1 13=2 14=1 2=1 3=2 4=1 5=1 6=144 7=16
Convolution conv_73 1 1 13 14 0=48 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=768
HardSwish hswish_4 1 1 14 15 0=1.666667e-01 1=5.000000e-01
Concat cat_0 2 1 10 15 16 0=0
ShuffleChannel channelshuffle_60 1 1 16 17 0=2 1=0
Slice tensor_split_0 1 2 17 18 19 -23300=2,32,-233 1=0
Convolution conv_74 1 1 19 20 0=32 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=1024
HardSwish hswish_5 1 1 20 21 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_131 1 1 21 22 0=32 1=3 11=3 12=1 13=1 14=1 2=1 3=1 4=1 5=1 6=288 7=32
Convolution conv_75 1 1 22 23 0=32 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=1024
HardSwish hswish_6 1 1 23 24 0=1.666667e-01 1=5.000000e-01
Concat cat_1 2 1 18 24 25 0=0
ShuffleChannel channelshuffle_61 1 1 25 26 0=2 1=0
Slice tensor_split_1 1 2 26 27 28 -23300=2,32,-233 1=0
Convolution conv_76 1 1 28 29 0=32 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=1024
HardSwish hswish_7 1 1 29 30 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_132 1 1 30 31 0=32 1=3 11=3 12=1 13=1 14=1 2=1 3=1 4=1 5=1 6=288 7=32
Convolution conv_77 1 1 31 32 0=32 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=1024
HardSwish hswish_8 1 1 32 33 0=1.666667e-01 1=5.000000e-01
Concat cat_2 2 1 27 33 34 0=0
ShuffleChannel channelshuffle_62 1 1 34 35 0=2 1=0
Split splitncnn_1 1 4 35 36 37 38 39
ConvolutionDepthWise convdw_133 1 1 39 40 0=64 1=3 11=3 12=1 13=2 14=1 2=1 3=2 4=1 5=1 6=576 7=64
Convolution conv_78 1 1 40 41 0=32 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=2048
HardSwish hswish_9 1 1 41 42 0=1.666667e-01 1=5.000000e-01
Convolution conv_79 1 1 37 43 0=32 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=2048
HardSwish hswish_10 1 1 43 44 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_134 1 1 44 45 0=32 1=3 11=3 12=1 13=2 14=1 2=1 3=2 4=1 5=1 6=288 7=32
Convolution conv_80 1 1 45 46 0=96 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=3072
HardSwish hswish_11 1 1 46 47 0=1.666667e-01 1=5.000000e-01
Concat cat_3 2 1 42 47 48 0=0
ShuffleChannel channelshuffle_63 1 1 48 49 0=2 1=0
Slice tensor_split_2 1 2 49 50 51 -23300=2,64,-233 1=0
Convolution conv_81 1 1 51 52 0=64 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=4096
HardSwish hswish_12 1 1 52 53 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_135 1 1 53 54 0=64 1=3 11=3 12=1 13=1 14=1 2=1 3=1 4=1 5=1 6=576 7=64
Convolution conv_82 1 1 54 55 0=64 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=4096
HardSwish hswish_13 1 1 55 56 0=1.666667e-01 1=5.000000e-01
Concat cat_4 2 1 50 56 57 0=0
ShuffleChannel channelshuffle_64 1 1 57 58 0=2 1=0
Slice tensor_split_3 1 2 58 59 60 -23300=2,64,-233 1=0
Convolution conv_83 1 1 60 61 0=64 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=4096
HardSwish hswish_14 1 1 61 62 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_136 1 1 62 63 0=64 1=3 11=3 12=1 13=1 14=1 2=1 3=1 4=1 5=1 6=576 7=64
Convolution conv_84 1 1 63 64 0=64 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=4096
HardSwish hswish_15 1 1 64 65 0=1.666667e-01 1=5.000000e-01
Concat cat_5 2 1 59 65 66 0=0
ShuffleChannel channelshuffle_65 1 1 66 67 0=2 1=0
Split splitncnn_2 1 3 67 68 69 70
ConvolutionDepthWise convdw_137 1 1 70 71 0=128 1=3 11=3 12=1 13=2 14=1 2=1 3=2 4=1 5=1 6=1152 7=128
Convolution conv_85 1 1 71 72 0=64 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=8192
HardSwish hswish_16 1 1 72 73 0=1.666667e-01 1=5.000000e-01
Convolution conv_86 1 1 69 74 0=64 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=8192
HardSwish hswish_17 1 1 74 75 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_138 1 1 75 76 0=64 1=3 11=3 12=1 13=2 14=1 2=1 3=2 4=1 5=1 6=576 7=64
Convolution conv_87 1 1 76 77 0=192 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=12288
HardSwish hswish_18 1 1 77 78 0=1.666667e-01 1=5.000000e-01
Concat cat_6 2 1 73 78 79 0=0
ShuffleChannel channelshuffle_66 1 1 79 80 0=2 1=0
Slice tensor_split_4 1 2 80 81 82 -23300=2,128,-233 1=0
Convolution conv_88 1 1 82 83 0=128 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=16384
HardSwish hswish_19 1 1 83 84 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_139 1 1 84 85 0=128 1=3 11=3 12=1 13=1 14=1 2=1 3=1 4=1 5=1 6=1152 7=128
Convolution conv_89 1 1 85 86 0=128 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=16384
HardSwish hswish_20 1 1 86 87 0=1.666667e-01 1=5.000000e-01
Concat cat_7 2 1 81 87 88 0=0
ShuffleChannel channelshuffle_67 1 1 88 89 0=2 1=0
Slice tensor_split_5 1 2 89 90 91 -23300=2,128,-233 1=0
Convolution conv_90 1 1 91 92 0=128 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=16384
HardSwish hswish_21 1 1 92 93 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_140 1 1 93 94 0=128 1=3 11=3 12=1 13=1 14=1 2=1 3=1 4=1 5=1 6=1152 7=128
Convolution conv_91 1 1 94 95 0=128 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=16384
HardSwish hswish_22 1 1 95 96 0=1.666667e-01 1=5.000000e-01
Concat cat_8 2 1 90 96 97 0=0
ShuffleChannel channelshuffle_68 1 1 97 98 0=2 1=0
Convolution conv_92 1 1 98 99 0=128 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=32768
HardSwish hswish_23 1 1 99 100 0=1.666667e-01 1=5.000000e-01
Split splitncnn_3 1 2 100 101 102
Convolution conv_93 1 1 102 103 0=64 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=8192
HardSwish hswish_24 1 1 103 104 0=1.666667e-01 1=5.000000e-01
Interp interpolate_52 1 1 104 105 0=1 1=2.000000e+00 2=2.000000e+00 6=0
ConvolutionDepthWise convdw_141 1 1 38 106 0=64 1=3 11=3 12=1 13=2 14=1 2=1 3=2 4=1 5=1 6=576 7=64
Convolution conv_94 1 1 106 107 0=64 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=4096
HardSwish hswish_25 1 1 107 108 0=1.666667e-01 1=5.000000e-01
Concat cat_9 3 1 108 68 105 109 0=0
Split splitncnn_4 1 2 109 110 111
Convolution conv_95 1 1 111 112 0=64 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=16384
HardSwish hswish_26 1 1 112 113 0=1.666667e-01 1=5.000000e-01
Convolution conv_96 1 1 110 114 0=64 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=16384
HardSwish hswish_27 1 1 114 115 0=1.666667e-01 1=5.000000e-01
Convolution conv_97 1 1 113 116 0=64 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=4096
HardSwish hswish_28 1 1 116 117 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_142 1 1 117 118 0=64 1=3 11=3 12=1 13=1 14=1 2=1 3=1 4=1 5=1 6=576 7=64
Convolution conv_98 1 1 118 119 0=64 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=4096
HardSwish hswish_29 1 1 119 120 0=1.666667e-01 1=5.000000e-01
Concat cat_10 2 1 120 115 121 0=0
Convolution conv_99 1 1 121 122 0=128 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=16384
HardSwish hswish_30 1 1 122 123 0=1.666667e-01 1=5.000000e-01
Split splitncnn_5 1 3 123 124 125 126
Convolution conv_100 1 1 125 127 0=64 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=8192
HardSwish hswish_31 1 1 127 128 0=1.666667e-01 1=5.000000e-01
Interp interpolate_53 1 1 128 129 0=1 1=2.000000e+00 2=2.000000e+00 6=0
Concat cat_11 2 1 36 129 130 0=0
Split splitncnn_6 1 2 130 131 132
Convolution conv_101 1 1 132 133 0=32 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=4096
HardSwish hswish_32 1 1 133 134 0=1.666667e-01 1=5.000000e-01
Convolution conv_102 1 1 131 135 0=32 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=4096
HardSwish hswish_33 1 1 135 136 0=1.666667e-01 1=5.000000e-01
Convolution conv_103 1 1 134 137 0=32 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=1024
HardSwish hswish_34 1 1 137 138 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_143 1 1 138 139 0=32 1=3 11=3 12=1 13=1 14=1 2=1 3=1 4=1 5=1 6=288 7=32
Convolution conv_104 1 1 139 140 0=32 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=1024
HardSwish hswish_35 1 1 140 141 0=1.666667e-01 1=5.000000e-01
Concat cat_12 2 1 141 136 142 0=0
Convolution conv_105 1 1 142 143 0=64 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=4096
HardSwish hswish_36 1 1 143 144 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_144 1 1 126 145 0=128 1=3 11=3 12=1 13=2 14=1 2=1 3=2 4=1 5=1 6=1152 7=128
Convolution conv_106 1 1 145 146 0=128 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=16384
HardSwish hswish_37 1 1 146 147 0=1.666667e-01 1=5.000000e-01
Concat cat_13 2 1 147 101 148 0=0
Split splitncnn_7 1 2 148 149 150
Convolution conv_107 1 1 150 151 0=128 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=32768
HardSwish hswish_38 1 1 151 152 0=1.666667e-01 1=5.000000e-01
Convolution conv_108 1 1 149 153 0=128 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=32768
HardSwish hswish_39 1 1 153 154 0=1.666667e-01 1=5.000000e-01
Convolution conv_109 1 1 152 155 0=128 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=16384
HardSwish hswish_40 1 1 155 156 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_145 1 1 156 157 0=128 1=3 11=3 12=1 13=1 14=1 2=1 3=1 4=1 5=1 6=1152 7=128
Convolution conv_110 1 1 157 158 0=128 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=16384
HardSwish hswish_41 1 1 158 159 0=1.666667e-01 1=5.000000e-01
Concat cat_14 2 1 159 154 160 0=0
Convolution conv_111 1 1 160 161 0=256 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=65536
HardSwish hswish_42 1 1 161 162 0=1.666667e-01 1=5.000000e-01
Convolution conv_112 1 1 144 163 0=64 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=4096
HardSwish hswish_43 1 1 163 164 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_146 1 1 164 165 0=64 1=5 11=5 12=1 13=1 14=2 2=1 3=1 4=2 5=1 6=1600 7=64
ConvolutionDepthWise convdw_147 1 1 165 166 0=128 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=4096 7=2
HardSwish hswish_44 1 1 166 167 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_148 1 1 167 168 0=128 1=5 11=5 12=1 13=1 14=2 2=1 3=1 4=2 5=1 6=3200 7=128
ConvolutionDepthWise convdw_149 1 1 168 169 0=128 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=8192 7=2
HardSwish hswish_45 1 1 169 170 0=1.666667e-01 1=5.000000e-01
Slice tensor_split_6 1 2 170 171 172 -23300=2,64,-233 1=0
Split splitncnn_8 1 2 171 173 174
Convolution conv_114 1 1 174 175 0=8 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=512
Convolution convsigmoid_0 1 1 173 176 0=1 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=64 9=4
Convolution convsigmoid_1 1 1 172 177 0=12 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=768 9=4
Concat cat_15 3 1 175 176 177 178 0=0
Convolution conv_116 1 1 124 179 0=64 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=8192
HardSwish hswish_46 1 1 179 180 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_150 1 1 180 181 0=64 1=5 11=5 12=1 13=1 14=2 2=1 3=1 4=2 5=1 6=1600 7=64
ConvolutionDepthWise convdw_151 1 1 181 182 0=128 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=4096 7=2
HardSwish hswish_47 1 1 182 183 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_152 1 1 183 184 0=128 1=5 11=5 12=1 13=1 14=2 2=1 3=1 4=2 5=1 6=3200 7=128
ConvolutionDepthWise convdw_153 1 1 184 185 0=128 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=8192 7=2
HardSwish hswish_48 1 1 185 186 0=1.666667e-01 1=5.000000e-01
Slice tensor_split_7 1 2 186 187 188 -23300=2,64,-233 1=0
Split splitncnn_9 1 2 187 189 190
Convolution conv_118 1 1 190 191 0=8 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=512
Convolution convsigmoid_2 1 1 189 192 0=1 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=64 9=4
Convolution convsigmoid_3 1 1 188 193 0=12 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=768 9=4
Concat cat_16 3 1 191 192 193 194 0=0
Convolution conv_120 1 1 162 195 0=64 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=16384
HardSwish hswish_49 1 1 195 196 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_154 1 1 196 197 0=64 1=5 11=5 12=1 13=1 14=2 2=1 3=1 4=2 5=1 6=1600 7=64
ConvolutionDepthWise convdw_155 1 1 197 198 0=128 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=4096 7=2
HardSwish hswish_50 1 1 198 199 0=1.666667e-01 1=5.000000e-01
ConvolutionDepthWise convdw_156 1 1 199 200 0=128 1=5 11=5 12=1 13=1 14=2 2=1 3=1 4=2 5=1 6=3200 7=128
ConvolutionDepthWise convdw_157 1 1 200 201 0=128 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=8192 7=2
HardSwish hswish_51 1 1 201 202 0=1.666667e-01 1=5.000000e-01
Slice tensor_split_8 1 2 202 203 204 -23300=2,64,-233 1=0
Split splitncnn_10 1 2 203 205 206
Convolution conv_122 1 1 206 207 0=8 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=512
Convolution convsigmoid_4 1 1 205 208 0=1 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=64 9=4
Convolution convsigmoid_5 1 1 204 209 0=12 1=1 11=1 12=1 13=1 14=0 2=1 3=1 4=0 5=1 6=768 9=4
Concat cat_17 3 1 207 208 209 210 0=0
Reshape reshape_125 1 1 178 211 0=2704 1=21
Reshape reshape_126 1 1 194 212 0=676 1=21
Reshape reshape_127 1 1 210 213 0=169 1=21
Concat cat_18 3 1 211 212 213 out0 0=1

BIN
wust_vision-main/model/reborn_number_classifier.engine Normal file
View File

Binary file not shown.

BIN
wust_vision-main/model/reborn_number_classifier.onnx Normal file
View File

Binary file not shown.

BIN
wust_vision-main/model/tiny_resnet.onnx Normal file
View File

Binary file not shown.

BIN
wust_vision-main/model/yolox_armor3.onnx Normal file
View File

Binary file not shown.

203
wust_vision-main/read_shm_image_mmap_only.py Normal file
View File

@@ -0,0 +1,203 @@
#!/usr/bin/env python3
"""
read_shm_image_mmap_only.py
仅使用 mmap 读取共享内存的 reader：
- 共享内存 /dev/shm/debug_frame 前 4 bytes = uint32 jpg_size (little endian)
紧随其后的是 jpg_bytes (jpg_size bytes)
- 不再回退到文件读取；若 mmap 不可用则按 MMAP_RETRY_SECONDS 重试打开
- 原子写入 out_path.tmp -> out_path
- 支持环境变量覆盖： SHM_PATH, SHM_SIZE, OUT_PATH, FPS, DEBUG, MMAP_RETRY_SECONDS
"""
import os
import sys
import time
import struct
import hashlib
import signal
try:
import mmap as _mmap
except Exception:
_mmap = None
from io import BytesIO
# ---------- 配置（环境变量覆盖） ----------
SHM_PATH = os.environ.get("SHM_PATH", "/dev/shm/debug_frame")
SHM_SIZE = int(os.environ.get("SHM_SIZE", str(2 * 1024 * 1024))) # bytes
OUT_PATH = os.environ.get("OUT_PATH", "/dev/shm/frame_preview.jpg")
FPS = float(os.environ.get("FPS", "10.0"))
DEBUG = os.environ.get("DEBUG", "0") != "0"
MMAP_RETRY_SECONDS = float(os.environ.get("MMAP_RETRY_SECONDS", "5.0"))
# -------------------------------------------------------------------
sleep_interval = 1.0 / max(1.0, FPS)
def log(*a, **k):
if DEBUG:
print(*a, **k)
def is_valid_image_bytes(b: bytes) -> bool:
"""尝试用 Pillow 验证图像是否完整（若 Pillow 不存在，返回 True 以不阻塞）"""
try:
from PIL import Image
im = Image.open(BytesIO(b))
im.verify()
return True
except ImportError:
log("Pillow not installed; skipping image verify")
return True
except Exception as e:
log("image verify failed:", e)
return False
def open_mmap():
"""尝试打开并返回 (fd, mmap_obj) 或 (None, None)"""
if _mmap is None:
return None, None
try:
fd = os.open(SHM_PATH, os.O_RDONLY)
mm = _mmap.mmap(fd, SHM_SIZE, access=_mmap.ACCESS_READ)
log("mmap opened:", SHM_PATH, "size", SHM_SIZE)
return fd, mm
except Exception as e:
log("open_mmap failed:", e)
try:
if 'fd' in locals() and fd:
os.close(fd)
except:
pass
return None, None
def close_mmap(fd, mm):
try:
if mm:
mm.close()
except:
pass
try:
if fd:
os.close(fd)
except:
pass
def read_from_mmap(mm):
"""
从 mmap 读取一帧：
结构: [uint32 little-endian jpg_size][jpg_bytes...]
返回 bytes 或 None
"""
try:
mm.seek(0)
hdr = mm.read(4)
if len(hdr) < 4:
return None
jpg_size = struct.unpack("<I", hdr)[0]
if jpg_size <= 0 or jpg_size > (SHM_SIZE - 4):
log("invalid jpg_size:", jpg_size)
return None
jpg_bytes = mm.read(jpg_size)
if len(jpg_bytes) != jpg_size:
log(f"mmap truncated: expected {jpg_size} got {len(jpg_bytes)}")
return None
# quick header check
if not (jpg_bytes.startswith(b"\xff\xd8\xff")):
log("jpg header mismatch")
return None
return jpg_bytes
except ValueError as e:
log("mmap read ValueError:", e)
return None
except Exception as e:
log("mmap read exception:", e)
raise
def atomic_write(out_path, data):
tmp = out_path + ".tmp"
with open(tmp, "wb") as f:
f.write(data)
os.replace(tmp, out_path)
def loop():
if _mmap is None:
print("ERROR: mmap module not available in this Python environment. Exiting.", file=sys.stderr)
sys.exit(2)
fd = None
mm = None
last_hash = None
last_mmap_try = 0.0
# 初始尝试打开 mmap（若失败则进入重试循环）
fd, mm = open_mmap()
print(f"watching (mmap_only) '{SHM_PATH}' -> '{OUT_PATH}' @ {FPS} fps")
while True:
try:
jpg_bytes = None
# 尝试 mmap 读取（若 mm 为 None 则尝试重建）
if mm is not None:
try:
jpg_bytes = read_from_mmap(mm)
if jpg_bytes is None:
# 写端可能尚未写入完整帧，短暂等待
pass
else:
log("read frame from mmap, len=", len(jpg_bytes))
except Exception as e:
log("mmap error, closing and will retry:", e)
close_mmap(fd, mm)
fd, mm = None, None
last_mmap_try = time.time()
# 如果没有读到数据并且 mmap 不可用或不可读，则重试打开 mmap（按间隔）
if jpg_bytes is None:
now = time.time()
if mm is None and (now - last_mmap_try) >= MMAP_RETRY_SECONDS:
fd, mm = open_mmap()
last_mmap_try = now
time.sleep(sleep_interval)
continue
# 校验完整性（PIL verify）
if not is_valid_image_bytes(jpg_bytes):
log("image bytes failed verify, skipping")
time.sleep(sleep_interval)
continue
# 内容哈希，避免重复写磁盘
h = hashlib.sha256(jpg_bytes).hexdigest()
if h != last_hash:
atomic_write(OUT_PATH, jpg_bytes)
last_hash = h
log("wrote", OUT_PATH, "len=", len(jpg_bytes))
# else: skip (no change)
time.sleep(sleep_interval)
except KeyboardInterrupt:
print("terminated by user")
break
except Exception as e:
log("unexpected error:", e)
try:
if mm is not None:
close_mmap(fd, mm)
except:
pass
fd, mm = None, None
time.sleep(1.0)
# 退出前清理
close_mmap(fd, mm)
def _handle_sigterm(signum, frame):
print("terminated", file=sys.stderr)
sys.exit(0)
if __name__ == "__main__":
signal.signal(signal.SIGINT, _handle_sigterm)
signal.signal(signal.SIGTERM, _handle_sigterm)
loop()

136
wust_vision-main/ros2/ros2.hpp Normal file
View File

@@ -0,0 +1,136 @@
#pragma once
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
#include <chrono>
#include <functional>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <typeindex>
#include <unordered_map>
#include <vector>
class Ros2Node: public rclcpp::Node {
public:
explicit Ros2Node(const std::string& node_name = "vision"):
Node(node_name),
tf_buffer_(this->get_clock()),
tf_listener_(tf_buffer_),
tf_broadcaster_(std::make_shared<tf2_ros::TransformBroadcaster>(this)) {}
using Ptr = std::shared_ptr<Ros2Node>;
static Ptr instance() {
static Ptr inst = std::make_shared<Ros2Node>();
return inst;
}
~Ros2Node() {
stop();
}
template<typename MsgT>
void add_subscription(
const std::string& topic_name,
std::function<void(const typename MsgT::SharedPtr)> callback,
const rclcpp::QoS& qos = rclcpp::QoS(rclcpp::KeepLast(10))
) {
auto sub = this->create_subscription<MsgT>(topic_name, qos, callback);
std::lock_guard<std::mutex> lock(map_mutex_);
subscriptions_[topic_name] = sub;
}
template<typename MsgT>
void add_publisher(
const std::string& topic_name,
const rclcpp::QoS& qos = rclcpp::QoS(rclcpp::KeepLast(10))
) {
auto pub = this->create_publisher<MsgT>(topic_name, qos);
std::lock_guard<std::mutex> lock(map_mutex_);
publishers_[topic_name] = pub;
}
template<typename MsgT>
void publish(const std::string& topic_name, const MsgT& msg) {
std::lock_guard<std::mutex> lock(map_mutex_);
auto it = publishers_.find(topic_name);
if (it != publishers_.end()) {
auto typed_pub = std::dynamic_pointer_cast<rclcpp::Publisher<MsgT>>(it->second);
if (typed_pub)
typed_pub->publish(msg);
}
}
template<typename Rep, typename Period>
void
add_timer(const std::chrono::duration<Rep, Period>& interval, std::function<void()> callback) {
auto timer = this->create_wall_timer(interval, callback);
std::lock_guard<std::mutex> lock(map_mutex_);
timers_.push_back(timer);
}
bool lookup_transform(
const std::string& target_frame,
const std::string& source_frame,
geometry_msgs::msg::TransformStamped& out_tf,
std::chrono::milliseconds timeout = std::chrono::milliseconds(200)
) const {
try {
out_tf =
tf_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero, timeout);
return true;
} catch (const tf2::TransformException& ex) {
RCLCPP_WARN(
this->get_logger(),
"lookup_transform failed %s -> %s: %s",
source_frame.c_str(),
target_frame.c_str(),
ex.what()
);
return false;
}
}
void broadcast_tf(const geometry_msgs::msg::TransformStamped& tf_msg) {
tf_broadcaster_->sendTransform(tf_msg);
}
template<typename Rep, typename Period>
void broadcast_tf_periodic(
const geometry_msgs::msg::TransformStamped& tf_msg,
const std::chrono::duration<Rep, Period>& interval
) {
add_timer(interval, [this, tf_msg]() { broadcast_tf(tf_msg); });
}
void start() {
if (!spin_thread_.joinable()) {
auto node = shared_from_this();
spin_thread_ = std::thread([node]() { rclcpp::spin(node); });
}
}
void stop() {
rclcpp::shutdown();
if (spin_thread_.joinable()) {
spin_thread_.join();
}
}
private:
mutable std::mutex map_mutex_;
std::unordered_map<std::string, rclcpp::SubscriptionBase::SharedPtr> subscriptions_;
std::unordered_map<std::string, rclcpp::PublisherBase::SharedPtr> publishers_;
std::vector<rclcpp::TimerBase::SharedPtr> timers_;
std::thread spin_thread_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

88
wust_vision-main/ros2/tf.hpp Normal file
View File

@@ -0,0 +1,88 @@
#pragma once
#include "rclcpp/rclcpp.hpp"
#include <Eigen/Dense>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
template<typename PublisherT>
inline bool publisherSubscribed(const PublisherT& publisher) noexcept {
return publisher && publisher->get_subscription_count() > 0;
}
inline Eigen::Isometry3f tf2ToEigen(const geometry_msgs::msg::TransformStamped& tf) noexcept {
Eigen::Isometry3f T = Eigen::Isometry3f::Identity();
const auto& t = tf.transform.translation;
T.translation() =
Eigen::Vector3f(static_cast<float>(t.x), static_cast<float>(t.y), static_cast<float>(t.z));
const auto& q = tf.transform.rotation;
Eigen::Quaternionf Q(
static_cast<float>(q.w),
static_cast<float>(q.x),
static_cast<float>(q.y),
static_cast<float>(q.z)
);
Q.normalize();
T.linear() = Q.toRotationMatrix();
return T;
}
class TF {
public:
using Ptr = std::shared_ptr<TF>;
TF(rclcpp::Node& n) {
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(n.get_clock());
tf_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_);
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(n);
node_ = &n;
}
static Ptr create(rclcpp::Node& n) {
return std::make_shared<TF>(n);
}
std::optional<Eigen::Isometry3f>
getTransform(const std::string& target, const std::string& source, rclcpp::Time t)
const noexcept {
Eigen::Isometry3f T_out = Eigen::Isometry3f::Identity();
try {
geometry_msgs::msg::TransformStamped tf_msg =
tf_buffer_->lookupTransform(target, source, t, rclcpp::Duration::from_seconds(0.1));
T_out = tf2ToEigen(tf_msg);
return T_out;
} catch (tf2::TransformException& ex) {
RCLCPP_WARN(rclcpp::get_logger("tf"), "TF lookup failed: %s", ex.what());
return std::nullopt;
}
return T_out;
}
void publishTransform(
const Eigen::Isometry3d& transform,
const std::string& parent_frame,
const std::string& child_frame,
const rclcpp::Time& stamp
) const noexcept {
geometry_msgs::msg::TransformStamped tmsg;
tmsg.header.stamp = stamp;
tmsg.header.frame_id = parent_frame;
tmsg.child_frame_id = child_frame;
const Eigen::Vector3d tr = transform.translation();
const Eigen::Quaterniond q(transform.rotation());
tmsg.transform.translation.x = tr.x();
tmsg.transform.translation.y = tr.y();
tmsg.transform.translation.z = tr.z();
tmsg.transform.rotation.x = q.x();
tmsg.transform.rotation.y = q.y();
tmsg.transform.rotation.z = q.z();
tmsg.transform.rotation.w = q.w();
tf_broadcaster_->sendTransform(tmsg);
}
rclcpp::Node* node_;
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

Some files were not shown because too many files have changed in this diff Show More