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

此更改更改了imu数据处理方式，使得静止状态yaw不会发生偏移，后续应优化陀螺仪数据处理：低通滤波会导致相位延迟、滑动窗口会导致数据有微量误差。

Browse Source 
无法避免运动过程中的yaw偏移。
后续应在运动时也加上零漂偏移
This commit is contained in:
Orange
2026-06-23 22:34:15 +08:00
parent 630cb18277
commit b14befd1cf
29 changed files with 9433 additions and 391 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/gc_navigation2_slamtoolbox/config/slam_toolbox_mapping.yaml
View File

@@ -21,11 +21,11 @@ slam_toolbox:
debug_logging: false
throttle_scans: 2
transform_publish_period: 0.02
map_update_interval: 3.0
map_update_interval: 0.8
resolution: 0.05
min_laser_range: 0.15
max_laser_range: 10.0
minimum_time_interval: 0.5
minimum_time_interval: 0.7
transform_timeout: 0.2
tf_buffer_duration: 30.0
stack_size_to_use: 40000000

2
src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml
View File

@@ -205,7 +205,7 @@ planner_server:
downsampling_factor: 1
tolerance: 0.25
allow_unknown: True
max_iterations: 100000
max_iterations: 10000
max_on_approach_iterations: 1000
max_planning_time: 5.0
motion_model_for_search: REEDS_SHEPP

139
src/origincar_base/include/origincar_base/origincar_base.h
View File

@@ -1,9 +1,9 @@
#ifndef _ORIGINCAR_BASE_H_
#define _ORIGINCAR_BASE_H_
#include <memory>
#include <inttypes.h>
#include <array>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <csignal>
@@ -35,53 +35,52 @@
#include <tf2_ros/transform_broadcaster.h>
#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
#include "origincar_msg/msg/data.hpp"
#include "origincar_msg/msg/sign.hpp" // 匹配信号发送
#include "origincar_msg/msg/sign.hpp" // 匹配信号发送
#include <sensor_msgs/msg/imu.hpp>
#include <nav_msgs/msg/odometry.hpp>
using namespace std;
#define SEND_DATA_CHECK 1 // Send data check flag bits //发送数据校验标志位
#define READ_DATA_CHECK 0 // Receive data to check flag bits //接收数据校验标志位
#define FRAME_HEADER 0X7B // Frame head //帧头
#define FRAME_TAIL 0X7D // Frame tail //帧尾
#define RECEIVE_DATA_SIZE 24 // The length of the data sent by the lower computer //下位机发送过来的数据的长度
#define SEND_DATA_SIZE 11 // The length of data sent by ROS to the lower machine //ROS向下位机发送的数据的长度
#define PI 3.1415926f // PI //圆周率
#define SEND_DATA_CHECK 1 //Send data check flag bits //发送数据校验标志位
#define READ_DATA_CHECK 0 //Receive data to check flag bits //接收数据校验标志位
#define FRAME_HEADER 0X7B //Frame head //帧头
#define FRAME_TAIL 0X7D //Frame tail //帧尾
#define RECEIVE_DATA_SIZE 24 //The length of the data sent by the lower computer //下位机发送过来的数据的长度
#define SEND_DATA_SIZE 11 //The length of data sent by ROS to the lower machine //ROS向下位机发送的数据的长度
#define PI 3.1415926f //PI //圆周率
#define GYROSCOPE_RATIO 0.00026644f
#define GYROSCOPE_RATIO 0.00026644f
#define ACCEl_RATIO 1671.84f
#define ACCEl_RATIO 1671.84f
extern sensor_msgs::msg::Imu Mpu6050;
const double odom_pose_covariance[36] = {1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e3 };
const double odom_pose_covariance[36] = {1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e3};
const double odom_pose_covariance2[36] = {1e-9, 0, 0, 0, 0, 0,
0, 1e-3, 1e-9, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-9 };
const double odom_pose_covariance2[36] = {1e-9, 0, 0, 0, 0, 0,
0, 1e-3, 1e-9, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-9};
const double odom_twist_covariance[36] = {1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e3 };
const double odom_twist_covariance[36] = {1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e3};
const double odom_twist_covariance2[36] = {1e-9, 0, 0, 0, 0, 0,
0, 1e-3, 1e-9, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-9} ;
const double odom_twist_covariance2[36] = {1e-9, 0, 0, 0, 0, 0,
0, 1e-3, 1e-9, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-9};
typedef struct __Vel_Pos_Data_
{
@@ -96,7 +95,7 @@ typedef struct __MPU6050_DATA_
short accele_x_data;
short accele_y_data;
short accele_z_data;
short gyros_x_data;
short gyros_x_data;
short gyros_y_data;
short gyros_z_data;
@@ -132,14 +131,13 @@ public:
void Control();
void Publish_Odom();
public :
public:
serial::Serial Stm32_Serial;
private:
void declare_parameters();
void get_parameters();
void Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux);
void Akm_Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr akm_ctl);
@@ -148,39 +146,39 @@ private:
auto createQuaternionMsgFromYaw(double yaw);
bool Get_Sensor_Data();
unsigned char Check_Sum(unsigned char Count_Number,unsigned char mode);
short IMU_Trans(uint8_t Data_High,uint8_t Data_Low);
float Odom_Trans(uint8_t Data_High,uint8_t Data_Low);
unsigned char Check_Sum(unsigned char Count_Number, unsigned char mode);
short IMU_Trans(uint8_t Data_High, uint8_t Data_Low);
float Odom_Trans(uint8_t Data_High, uint8_t Data_Low);
void Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr sign_switch);
private:
void reset_filter()
{
auto msg = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
// 设置头信息
msg->header.stamp = this->now();
msg->header.frame_id = "odom_combined";
void reset_filter()
{
auto msg = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
// 重置位姿
msg->pose.pose.position.x = 0.54;
msg->pose.pose.position.y = 0.2;
msg->pose.pose.orientation.w = 1.0;
// 设置头信息
msg->header.stamp = this->now();
msg->header.frame_id = "odom_combined";
// 设置关键协方差
std::array<double, 36> cov = {0};
cov[0] = 0.5; // x
cov[7] = 0.5; // y
cov[35] = 0.5; // yaw
msg->pose.covariance = cov;
// 重置位姿
msg->pose.pose.position.x = 0.54;
msg->pose.pose.position.y = 0.2;
msg->pose.pose.orientation.w = 1.0;
// 发布重置指令
pose_pub_->publish(*msg);
RCLCPP_WARN(this->get_logger(), "FILTER RESET TRIGGERED!");
}
// 设置关键协方差
std::array<double, 36> cov = {0};
cov[0] = 0.5; // x
cov[7] = 0.5; // y
cov[35] = 0.5; // yaw
msg->pose.covariance = cov;
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_;
// 发布重置指令
pose_pub_->publish(*msg);
RCLCPP_WARN(this->get_logger(), "FILTER RESET TRIGGERED!");
}
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_;
rclcpp::Time _Now, _Last_Time;
float Sampling_Time;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr Cmd_Vel_Sub;
@@ -188,7 +186,7 @@ private:
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher;
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr voltage_publisher;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher;
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr test_publisher;
@@ -223,8 +221,15 @@ private:
Vel_Pos_Data Robot_Vel;
MPU6050_DATA Mpu6050_Data;
float Power_voltage;
size_t count_;
float stationary_velocity_threshold_;
float stationary_yaw_rate_threshold_;
float gyro_z_low_pass_alpha_;
float gyro_z_low_pass_;
std::array<float, 3> gyro_z_median_window_;
size_t gyro_z_median_index_;
size_t gyro_z_median_count_;
bool gyro_z_low_pass_initialized_;
size_t count_;
};
#endif //_ORIGINCAR_BASE_H_

721
src/origincar_base/src/origincar_base.cpp
View File

@@ -1,10 +1,11 @@
#include "origincar_base/origincar_base.h"
#include "rclcpp/rclcpp.hpp"
#include "origincar_base/Quaternion_Solution.h"
#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
#include "origincar_msg/msg/data.hpp"
#include "robot_localization/srv/set_pose.hpp"
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <algorithm>
using std::placeholders::_1;
using namespace std;
@@ -12,368 +13,459 @@ void sigintHandler(int sig);
sensor_msgs::msg::Imu Mpu6050;
rclcpp::Node::SharedPtr node_handle = nullptr;
int Init_imu_num = 0;
float gyro_z_sum=0;
float gyro_z_sum = 0;
namespace
{
constexpr float kDefaultStationaryVelocityThreshold = 0.01f;
constexpr float kDefaultStationaryYawRateThreshold = 0.003f;
constexpr float kDefaultGyroZLowPassAlpha = 0.6f;
float median3(float a, float b, float c)
{
if (a > b)
{
std::swap(a, b);
}
if (b > c)
{
std::swap(b, c);
}
if (a > b)
{
std::swap(a, b);
}
return b;
}
}
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
signal(SIGINT, sigintHandler);
origincar_base Robot_Control;
Robot_Control.Control();
rclcpp::shutdown();
return 0;
rclcpp::init(argc, argv);
signal(SIGINT, sigintHandler);
origincar_base Robot_Control;
Robot_Control.Control();
rclcpp::shutdown();
return 0;
}
short origincar_base::IMU_Trans(uint8_t Data_High,uint8_t Data_Low)
short origincar_base::IMU_Trans(uint8_t Data_High, uint8_t Data_Low)
{
short transition_16;
transition_16 = 0;
transition_16 |= Data_High<<8;
transition_16 |= Data_Low;
return transition_16;
short transition_16;
transition_16 = 0;
transition_16 |= Data_High << 8;
transition_16 |= Data_Low;
return transition_16;
}
float origincar_base::Odom_Trans(uint8_t Data_High,uint8_t Data_Low)
float origincar_base::Odom_Trans(uint8_t Data_High, uint8_t Data_Low)
{
float data_return;
short transition_16;
transition_16 = 0;
transition_16 |= Data_High<<8;
transition_16 |= Data_Low;
data_return = (transition_16 / 1000)+(transition_16 % 1000)*0.001;
return data_return;
}
float data_return;
short transition_16;
transition_16 = 0;
transition_16 |= Data_High << 8;
transition_16 |= Data_Low;
data_return = (transition_16 / 1000) + (transition_16 % 1000) * 0.001;
return data_return;
}
void origincar_base::Akm_Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr akm_ctl)
{
short transition;
std::cout<<"linerx"<<akm_ctl->drive.speed<<std::endl;
std::cout<<"angular"<<akm_ctl->drive.steering_angle<<std::endl;
Send_Data.tx[0]=FRAME_HEADER;
Send_Data.tx[1] = 0;
Send_Data.tx[2] = 0;
short transition;
std::cout << "linerx" << akm_ctl->drive.speed << std::endl;
std::cout << "angular" << akm_ctl->drive.steering_angle << std::endl;
transition=0;
transition = akm_ctl->drive.speed*1000;
Send_Data.tx[4] = transition;
Send_Data.tx[3] = transition>>8;
Send_Data.tx[0] = FRAME_HEADER;
Send_Data.tx[1] = 0;
Send_Data.tx[2] = 0;
transition=0;
transition = akm_ctl->drive.steering_angle*1000/2;
Send_Data.tx[8] = transition;
Send_Data.tx[7] = transition>>8;
transition = 0;
transition = akm_ctl->drive.speed * 1000;
Send_Data.tx[4] = transition;
Send_Data.tx[3] = transition >> 8;
Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK);
Send_Data.tx[10]=FRAME_TAIL;
transition = 0;
transition = akm_ctl->drive.steering_angle * 1000 / 2;
Send_Data.tx[8] = transition;
Send_Data.tx[7] = transition >> 8;
try {
Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx));
} catch (serial::IOException& e) {
RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port"));
}
Send_Data.tx[9] = Check_Sum(9, SEND_DATA_CHECK);
Send_Data.tx[10] = FRAME_TAIL;
try
{
Stm32_Serial.write(Send_Data.tx, sizeof(Send_Data.tx));
}
catch (serial::IOException &e)
{
RCLCPP_ERROR(this->get_logger(), ("Unable to send data through serial port"));
}
}
void origincar_base::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux)
{
RCLCPP_INFO(this->get_logger(),"linarx: %.2f, angularz: %.2f ", twist_aux->linear.x, twist_aux->angular.z);
std::cout<<"linerx"<<twist_aux->linear.x<<std::endl;
std::cout<<"angular"<<twist_aux->angular.z<<std::endl;
short transition;
Send_Data.tx[0]=FRAME_HEADER;
Send_Data.tx[1] = 0;
Send_Data.tx[2] = 0;
RCLCPP_INFO(this->get_logger(), "linarx: %.2f, angularz: %.2f ", twist_aux->linear.x, twist_aux->angular.z);
std::cout << "linerx" << twist_aux->linear.x << std::endl;
std::cout << "angular" << twist_aux->angular.z << std::endl;
short transition;
Send_Data.tx[0] = FRAME_HEADER;
Send_Data.tx[1] = 0;
Send_Data.tx[2] = 0;
transition=0;
transition = twist_aux->linear.x*1000;
Send_Data.tx[4] = transition;
Send_Data.tx[3] = transition>>8;
transition = 0;
transition = twist_aux->linear.x * 1000;
Send_Data.tx[4] = transition;
Send_Data.tx[3] = transition >> 8;
transition=0;
transition = twist_aux->linear.y*1000;
Send_Data.tx[6] = transition;
Send_Data.tx[5] = transition>>8;
transition = 0;
transition = twist_aux->linear.y * 1000;
Send_Data.tx[6] = transition;
Send_Data.tx[5] = transition >> 8;
transition=0;
transition = (twist_aux->angular.z)*1000;
Send_Data.tx[8] = transition;
Send_Data.tx[7] = transition>>8;
transition = 0;
transition = (twist_aux->angular.z) * 1000;
Send_Data.tx[8] = transition;
Send_Data.tx[7] = transition >> 8;
Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK);
Send_Data.tx[10]=FRAME_TAIL;
Send_Data.tx[9] = Check_Sum(9, SEND_DATA_CHECK);
Send_Data.tx[10] = FRAME_TAIL;
try
{
Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx));
} catch (serial::IOException& e) {
RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port"));
}
try
{
Stm32_Serial.write(Send_Data.tx, sizeof(Send_Data.tx));
}
catch (serial::IOException &e)
{
RCLCPP_ERROR(this->get_logger(), ("Unable to send data through serial port"));
}
}
void origincar_base::Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr sign_switch)
{
(void)sign_switch;
if (sign_switch->data == -1) {
memset(&Robot_Pos, 0, sizeof(Robot_Pos));
memset(&Mpu6050, 0, sizeof(Mpu6050));
q0 = 0;
q1 = 0;
q2 = 0;
q3 = 0;
Robot_Pos.X = 0.54;
Robot_Pos.Y = 0.2;
Robot_Pos.Z = 0;
Robot_Vel.X = 0;
Robot_Vel.Y = 0;
Robot_Vel.Z = 0;
memset(&Robot_Vel, 0, sizeof(Robot_Vel));
reset_filter();
}
else if(sign_switch->data == -2) {
memset(&Robot_Pos, 0, sizeof(Robot_Pos));
Robot_Pos.X = 2.54;
Robot_Pos.Y = 2.5;
Robot_Pos.Z = -1.5708;
Robot_Vel.X = 0;
Robot_Vel.Y = 0;
Robot_Vel.Z = 0;
memset(&Robot_Vel, 0, sizeof(Robot_Vel));
reset_filter();
}
/*
else if (sign_switch->data == 6) {
memset(&Robot_Pos, 0, sizeof(Robot_Pos));
Robot_Pos.X = 2;
Robot_Pos.Y = 2;
memset(&Robot_Vel, 0, sizeof(Robot_Vel));
}*/
if (sign_switch->data == -1)
{
memset(&Robot_Pos, 0, sizeof(Robot_Pos));
memset(&Mpu6050, 0, sizeof(Mpu6050));
q0 = 0;
q1 = 0;
q2 = 0;
q3 = 0;
Robot_Pos.X = 0.54;
Robot_Pos.Y = 0.2;
Robot_Pos.Z = 0;
Robot_Vel.X = 0;
Robot_Vel.Y = 0;
Robot_Vel.Z = 0;
memset(&Robot_Vel, 0, sizeof(Robot_Vel));
reset_filter();
}
else if (sign_switch->data == -2)
{
memset(&Robot_Pos, 0, sizeof(Robot_Pos));
Robot_Pos.X = 2.54;
Robot_Pos.Y = 2.5;
Robot_Pos.Z = -1.5708;
Robot_Vel.X = 0;
Robot_Vel.Y = 0;
Robot_Vel.Z = 0;
memset(&Robot_Vel, 0, sizeof(Robot_Vel));
reset_filter();
}
/*
else if (sign_switch->data == 6) {
memset(&Robot_Pos, 0, sizeof(Robot_Pos));
Robot_Pos.X = 2;
Robot_Pos.Y = 2;
memset(&Robot_Vel, 0, sizeof(Robot_Vel));
}*/
}
void origincar_base::Publish_ImuSensor()
{
sensor_msgs::msg::Imu Imu_Data_Pub;
Imu_Data_Pub.header.stamp = rclcpp::Node::now();
Imu_Data_Pub.header.frame_id = gyro_frame_id;
Imu_Data_Pub.orientation.x = Mpu6050.orientation.x;
Imu_Data_Pub.orientation.y = Mpu6050.orientation.y;
Imu_Data_Pub.orientation.z = Mpu6050.orientation.z;
Imu_Data_Pub.orientation.w = Mpu6050.orientation.w;
Imu_Data_Pub.orientation_covariance[0] = 1e6;
Imu_Data_Pub.orientation_covariance[4] = 1e6;
Imu_Data_Pub.orientation_covariance[8] = 1e-6;
Imu_Data_Pub.angular_velocity.x = Mpu6050.angular_velocity.x;
Imu_Data_Pub.angular_velocity.y = Mpu6050.angular_velocity.y;
Imu_Data_Pub.angular_velocity.z = Mpu6050.angular_velocity.z;
Imu_Data_Pub.angular_velocity_covariance[0] = 1e6;
Imu_Data_Pub.angular_velocity_covariance[4] = 1e6;
Imu_Data_Pub.angular_velocity_covariance[8] = 1e-6;
Imu_Data_Pub.linear_acceleration.x = Mpu6050.linear_acceleration.x;
Imu_Data_Pub.linear_acceleration.y = Mpu6050.linear_acceleration.y;
Imu_Data_Pub.linear_acceleration.z = Mpu6050.linear_acceleration.z;
sensor_msgs::msg::Imu Imu_Data_Pub;
Imu_Data_Pub.header.stamp = rclcpp::Node::now();
Imu_Data_Pub.header.frame_id = gyro_frame_id;
imu_publisher->publish(Imu_Data_Pub);
Imu_Data_Pub.orientation.x = Mpu6050.orientation.x;
Imu_Data_Pub.orientation.y = Mpu6050.orientation.y;
Imu_Data_Pub.orientation.z = Mpu6050.orientation.z;
Imu_Data_Pub.orientation.w = Mpu6050.orientation.w;
Imu_Data_Pub.orientation_covariance[0] = 1e6;
Imu_Data_Pub.orientation_covariance[4] = 1e6;
Imu_Data_Pub.orientation_covariance[8] = 1e-6;
Imu_Data_Pub.angular_velocity.x = Mpu6050.angular_velocity.x;
Imu_Data_Pub.angular_velocity.y = Mpu6050.angular_velocity.y;
Imu_Data_Pub.angular_velocity.z = Mpu6050.angular_velocity.z;
Imu_Data_Pub.angular_velocity_covariance[0] = 1e6;
Imu_Data_Pub.angular_velocity_covariance[4] = 1e6;
Imu_Data_Pub.angular_velocity_covariance[8] = 1e-6;
Imu_Data_Pub.linear_acceleration.x = Mpu6050.linear_acceleration.x;
Imu_Data_Pub.linear_acceleration.y = Mpu6050.linear_acceleration.y;
Imu_Data_Pub.linear_acceleration.z = Mpu6050.linear_acceleration.z;
imu_publisher->publish(Imu_Data_Pub);
}
void origincar_base::Publish_Odom()
{
tf2::Quaternion q;
q.setRPY(0,0,Robot_Pos.Z);
geometry_msgs::msg::Quaternion odom_quat=tf2::toMsg(q);
origincar_msg::msg::Data robotpose;
origincar_msg::msg::Data robotvel;
nav_msgs::msg::Odometry odom;
tf2::Quaternion q;
q.setRPY(0, 0, Robot_Pos.Z);
geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(q);
odom.header.stamp = rclcpp::Node::now();
odom.header.frame_id = odom_frame_id;
odom.child_frame_id = robot_frame_id;
origincar_msg::msg::Data robotpose;
origincar_msg::msg::Data robotvel;
nav_msgs::msg::Odometry odom;
odom.pose.pose.position.x = Robot_Pos.X;
odom.pose.pose.position.y = Robot_Pos.Y;
odom.header.stamp = rclcpp::Node::now();
odom.header.frame_id = odom_frame_id;
odom.child_frame_id = robot_frame_id;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
odom.pose.covariance[0] = odom_pose_cov_x_;
odom.pose.covariance[7] = odom_pose_cov_y_;
odom.pose.covariance[35] = odom_pose_cov_yaw_;
//odom_quat;
//odom_quat;
odom.pose.pose.position.x = Robot_Pos.X;
odom.pose.pose.position.y = Robot_Pos.Y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
odom.pose.covariance[0] = odom_pose_cov_x_;
odom.pose.covariance[7] = odom_pose_cov_y_;
odom.pose.covariance[35] = odom_pose_cov_yaw_;
// odom_quat;
// odom_quat;
odom.twist.twist.linear.x = Robot_Vel.X;
odom.twist.twist.linear.y = Robot_Vel.Y;
odom.twist.twist.angular.z = Robot_Vel.Z;
odom.twist.twist.linear.x = Robot_Vel.X;
odom.twist.twist.linear.y = Robot_Vel.Y;
odom.twist.twist.angular.z = Robot_Vel.Z;
robotpose.x = Robot_Pos.X;
robotpose.y = Robot_Pos.Y;
robotpose.z = Robot_Pos.Z;
robotpose.x = Robot_Pos.X;
robotpose.y = Robot_Pos.Y;
robotpose.z = Robot_Pos.Z;
robotvel.x = Robot_Vel.X;
robotvel.y = Robot_Vel.Y;
robotvel.z = Robot_Vel.Z;
robotvel.x = Robot_Vel.X;
robotvel.y = Robot_Vel.Y;
robotvel.z = Robot_Vel.Z;
geometry_msgs::msg::TransformStamped t;
geometry_msgs::msg::TransformStamped t;
t.header.stamp = rclcpp::Node::now();
t.header.frame_id = odom_frame_id;
t.child_frame_id = robot_frame_id;
t.header.stamp = rclcpp::Node::now();
t.header.frame_id = odom_frame_id;
t.child_frame_id = robot_frame_id;
t.transform.translation.x = Robot_Pos.X;
t.transform.translation.y = Robot_Pos.Y;
t.transform.translation.z = 0.0;
t.transform.translation.x = Robot_Pos.X;
t.transform.translation.y = Robot_Pos.Y;
t.transform.translation.z = 0.0;
t.transform.rotation = odom_quat;
if (publish_tf_) {
tf_broadcaster_->sendTransform(t);
}
odom_publisher->publish(odom);
robotpose_publisher->publish(robotpose);
robotvel_publisher->publish(robotvel);
t.transform.rotation = odom_quat;
if (publish_tf_)
{
tf_broadcaster_->sendTransform(t);
}
odom_publisher->publish(odom);
robotpose_publisher->publish(robotpose);
robotvel_publisher->publish(robotvel);
}
void origincar_base::Publish_Voltage()
{
std_msgs::msg::Float32 voltage_msgs;
static float Count_Voltage_Pub = 0;
std_msgs::msg::Float32 voltage_msgs;
static float Count_Voltage_Pub = 0;
if (Count_Voltage_Pub++ > 10) {
Count_Voltage_Pub = 0;
voltage_msgs.data = Power_voltage;
voltage_publisher->publish(voltage_msgs);
}
if (Count_Voltage_Pub++ > 10)
{
Count_Voltage_Pub = 0;
voltage_msgs.data = Power_voltage;
voltage_publisher->publish(voltage_msgs);
}
}
unsigned char origincar_base::Check_Sum(unsigned char Count_Number,unsigned char mode)
unsigned char origincar_base::Check_Sum(unsigned char Count_Number, unsigned char mode)
{
unsigned char check_sum = 0, k;
unsigned char check_sum = 0, k;
if (mode == 0) {
for(k=0; k < Count_Number; k++) {
check_sum = check_sum^Receive_Data.rx[k];
}
} else if (mode == 1) {
for (k=0; k < Count_Number; k++) {
check_sum = check_sum^Send_Data.tx[k];
}
if (mode == 0)
{
for (k = 0; k < Count_Number; k++)
{
check_sum = check_sum ^ Receive_Data.rx[k];
}
}
else if (mode == 1)
{
for (k = 0; k < Count_Number; k++)
{
check_sum = check_sum ^ Send_Data.tx[k];
}
}
return check_sum;
return check_sum;
}
bool origincar_base::Get_Sensor_Data()
{
short transition_16 = 0, j = 0, Header_Pos = 0, Tail_Pos = 0;
uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE] = {0};
try {
Stm32_Serial.read(Receive_Data_Pr,sizeof (Receive_Data_Pr));
} catch (const serial::SerialException& e) {
return false;
}
for (j = 0; j < 24; j++) {
if (Receive_Data_Pr[j] == FRAME_HEADER)
Header_Pos=j;
else if (Receive_Data_Pr[j] == FRAME_TAIL)
Tail_Pos = j;
}
if (Tail_Pos == (Header_Pos + 23)) {
memcpy(Receive_Data.rx, Receive_Data_Pr, sizeof(Receive_Data_Pr));
} else if (Header_Pos == (1 + Tail_Pos)) {
for (j = 0;j < 24; j++)
Receive_Data.rx[j] = Receive_Data_Pr[(j+Header_Pos) % 24];
} else {
short transition_16 = 0, j = 0, Header_Pos = 0, Tail_Pos = 0;
uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE] = {0};
try
{
Stm32_Serial.read(Receive_Data_Pr, sizeof(Receive_Data_Pr));
}
catch (const serial::SerialException &e)
{
return false;
}
}
for (j = 0; j < 24; j++)
{
if (Receive_Data_Pr[j] == FRAME_HEADER)
Header_Pos = j;
else if (Receive_Data_Pr[j] == FRAME_TAIL)
Tail_Pos = j;
}
Receive_Data.Frame_Header = Receive_Data.rx[0];
Receive_Data.Frame_Tail = Receive_Data.rx[23];
if (Receive_Data.Frame_Header == FRAME_HEADER) {
if (Receive_Data.Frame_Tail == FRAME_TAIL) {
if (Receive_Data.rx[22] == Check_Sum(22,READ_DATA_CHECK)||(Header_Pos == (1 + Tail_Pos))) {
Receive_Data.Flag_Stop=Receive_Data.rx[1];
Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]);
Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]);
//Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]);
if (Tail_Pos == (Header_Pos + 23))
{
memcpy(Receive_Data.rx, Receive_Data_Pr, sizeof(Receive_Data_Pr));
}
else if (Header_Pos == (1 + Tail_Pos))
{
for (j = 0; j < 24; j++)
Receive_Data.rx[j] = Receive_Data_Pr[(j + Header_Pos) % 24];
}
else
{
return false;
}
Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]);
Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]);
Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]);
Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]);
Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]);
Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]);
Receive_Data.Frame_Header = Receive_Data.rx[0];
Receive_Data.Frame_Tail = Receive_Data.rx[23];
if (Receive_Data.Frame_Header == FRAME_HEADER)
{
if (Receive_Data.Frame_Tail == FRAME_TAIL)
{
if (Receive_Data.rx[22] == Check_Sum(22, READ_DATA_CHECK) || (Header_Pos == (1 + Tail_Pos)))
{
Receive_Data.Flag_Stop = Receive_Data.rx[1];
Robot_Vel.X = Odom_Trans(Receive_Data.rx[2], Receive_Data.rx[3]);
Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data / ACCEl_RATIO;
Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data / ACCEl_RATIO;
Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data / ACCEl_RATIO;
Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4], Receive_Data.rx[5]);
Mpu6050.angular_velocity.x = Mpu6050_Data.gyros_x_data * GYROSCOPE_RATIO;
Mpu6050.angular_velocity.y = Mpu6050_Data.gyros_y_data * GYROSCOPE_RATIO;
if(Init_imu_num<100)
// Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]);
Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8], Receive_Data.rx[9]);
Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10], Receive_Data.rx[11]);
Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12], Receive_Data.rx[13]);
Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14], Receive_Data.rx[15]);
Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16], Receive_Data.rx[17]);
Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18], Receive_Data.rx[19]);
Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data / ACCEl_RATIO;
Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data / ACCEl_RATIO;
Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data / ACCEl_RATIO;
Mpu6050.angular_velocity.x = Mpu6050_Data.gyros_x_data * GYROSCOPE_RATIO;
Mpu6050.angular_velocity.y = Mpu6050_Data.gyros_y_data * GYROSCOPE_RATIO;
if (Init_imu_num < 100)
{
Init_imu_num += 1;
gyro_z_sum += Mpu6050_Data.gyros_z_data;
Mpu6050.angular_velocity.z = 0;
}
else
{
Mpu6050.angular_velocity.z = (Mpu6050_Data.gyros_z_data - (gyro_z_sum / (float)Init_imu_num)) * GYROSCOPE_RATIO;
gyro_z_median_window_[gyro_z_median_index_] = Mpu6050.angular_velocity.z;
gyro_z_median_index_ = (gyro_z_median_index_ + 1) % gyro_z_median_window_.size();
if (gyro_z_median_count_ < gyro_z_median_window_.size())
{
Init_imu_num += 1;
gyro_z_sum += Mpu6050_Data.gyros_z_data ;
Mpu6050.angular_velocity.z = 0;
gyro_z_median_count_ += 1;
}
float median_gyro_z = Mpu6050.angular_velocity.z;
if (gyro_z_median_count_ == gyro_z_median_window_.size())
{
median_gyro_z = median3(
gyro_z_median_window_[0],
gyro_z_median_window_[1],
gyro_z_median_window_[2]);
}
if (!gyro_z_low_pass_initialized_)
{
gyro_z_low_pass_ = median_gyro_z;
gyro_z_low_pass_initialized_ = true;
}
else
{
Mpu6050.angular_velocity.z = (Mpu6050_Data.gyros_z_data - (gyro_z_sum / (float)Init_imu_num))*GYROSCOPE_RATIO;
//RCLCPP_INFO(this->get_logger(),"gyro_z_sum: %.2f, err: %.2f, gyroz: %.2f ", gyro_z_sum ,(gyro_z_sum / (float)Init_imu_num), Mpu6050.angular_velocity.z);
gyro_z_low_pass_ =
gyro_z_low_pass_alpha_ * gyro_z_low_pass_ +
(1.0f - gyro_z_low_pass_alpha_) * median_gyro_z;
}
Robot_Vel.Z = Mpu6050.angular_velocity.z;
transition_16 = 0;
transition_16 |= Receive_Data.rx[20]<<8;
transition_16 |= Receive_Data.rx[21];
Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001;
return true;
Mpu6050.angular_velocity.z = gyro_z_low_pass_;
// RCLCPP_INFO(this->get_logger(),"gyro_z_sum: %.2f, err: %.2f, gyroz: %.2f ", gyro_z_sum ,(gyro_z_sum / (float)Init_imu_num), Mpu6050.angular_velocity.z);
}
Robot_Vel.Z = Mpu6050.angular_velocity.z;
transition_16 = 0;
transition_16 |= Receive_Data.rx[20] << 8;
transition_16 |= Receive_Data.rx[21];
Power_voltage = transition_16 / 1000 + (transition_16 % 1000) * 0.001;
return true;
}
}
}
return false;
return false;
}
void origincar_base::Control()
{
rclcpp::Time current_time, last_time;
rclcpp::Time current_time, last_time;
current_time = rclcpp::Node::now();
last_time = rclcpp::Node::now();
while (rclcpp::ok())
{
current_time = rclcpp::Node::now();
last_time = rclcpp::Node::now();
while(rclcpp::ok()) {
current_time = rclcpp::Node::now();
Sampling_Time = (current_time - last_time).seconds();
if (true == Get_Sensor_Data()) {
Robot_Pos.X+=1.03*(Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time;
Robot_Pos.Y+=1.01*(Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time;//1.125
Robot_Pos.Z+= Mpu6050.angular_velocity.z * Sampling_Time;
Quaternion_Solution(Mpu6050.angular_velocity.x, Mpu6050.angular_velocity.y, Mpu6050.angular_velocity.z,\
Mpu6050.linear_acceleration.x, Mpu6050.linear_acceleration.y, Mpu6050.linear_acceleration.z);
Publish_ImuSensor();
Publish_Voltage();
Publish_Odom();
rclcpp::spin_some(this->get_node_base_interface());
Sampling_Time = (current_time - last_time).seconds();
if (true == Get_Sensor_Data())
{
const bool is_stationary =
std::fabs(Robot_Vel.X) < stationary_velocity_threshold_ &&
std::fabs(Robot_Vel.Y) < stationary_velocity_threshold_ &&
std::fabs(Mpu6050.angular_velocity.z) < stationary_yaw_rate_threshold_;
if (is_stationary)
{
Mpu6050.angular_velocity.z = 0.0f;
Robot_Vel.Z = 0.0f;
}
last_time = current_time;
else
{
Robot_Vel.Z = Mpu6050.angular_velocity.z;
}
Robot_Pos.X += 1.03 * (Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time;
Robot_Pos.Y += 1.01 * (Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time; // 1.125
Robot_Pos.Z += Robot_Vel.Z * Sampling_Time;
Quaternion_Solution(Mpu6050.angular_velocity.x, Mpu6050.angular_velocity.y, Robot_Vel.Z,
Mpu6050.linear_acceleration.x, Mpu6050.linear_acceleration.y, Mpu6050.linear_acceleration.z);
Publish_ImuSensor();
Publish_Voltage();
Publish_Odom();
rclcpp::spin_some(this->get_node_base_interface());
}
last_time = current_time;
}
}
origincar_base::origincar_base()
: rclcpp::Node ("origincar_base")
: rclcpp::Node("origincar_base")
{
memset(&Robot_Pos, 0, sizeof(Robot_Pos));
memset(&Robot_Vel, 0, sizeof(Robot_Vel));
memset(&Receive_Data, 0, sizeof(Receive_Data));
memset(&Send_Data, 0, sizeof(Send_Data));
memset(&Mpu6050_Data, 0, sizeof(Mpu6050_Data));
gyro_z_median_window_.fill(0.0f);
gyro_z_median_index_ = 0;
gyro_z_median_count_ = 0;
gyro_z_low_pass_ = 0.0f;
gyro_z_low_pass_initialized_ = false;
Robot_Pos.X = 0.54;
Robot_Pos.Y = 0.2;
@@ -386,6 +478,9 @@ origincar_base::origincar_base()
this->declare_parameter<std::string>("robot_frame_id", "base_link");
this->declare_parameter<std::string>("gyro_frame_id", "gyro_link");
this->declare_parameter<bool>("publish_tf", false);
this->declare_parameter<double>("stationary_velocity_threshold", kDefaultStationaryVelocityThreshold);
this->declare_parameter<double>("stationary_yaw_rate_threshold", kDefaultStationaryYawRateThreshold);
this->declare_parameter<double>("gyro_z_low_pass_alpha", kDefaultGyroZLowPassAlpha);
// Odom covariance parameters (tunable via YAML)
this->declare_parameter<double>("odom_pose_cov_x", 0.01);
@@ -400,7 +495,20 @@ origincar_base::origincar_base()
this->get_parameter("robot_frame_id", robot_frame_id);
this->get_parameter("gyro_frame_id", gyro_frame_id);
this->get_parameter("publish_tf", publish_tf_);
stationary_velocity_threshold_ =
static_cast<float>(this->get_parameter("stationary_velocity_threshold").as_double());
stationary_yaw_rate_threshold_ =
static_cast<float>(this->get_parameter("stationary_yaw_rate_threshold").as_double());
gyro_z_low_pass_alpha_ =
static_cast<float>(this->get_parameter("gyro_z_low_pass_alpha").as_double());
if (gyro_z_low_pass_alpha_ < 0.0f)
{
gyro_z_low_pass_alpha_ = 0.0f;
}
else if (gyro_z_low_pass_alpha_ > 0.99f)
{
gyro_z_low_pass_alpha_ = 0.99f;
}
this->get_parameter("odom_pose_cov_x", odom_pose_cov_x_);
this->get_parameter("odom_pose_cov_y", odom_pose_cov_y_);
this->get_parameter("odom_pose_cov_yaw", odom_pose_cov_yaw_);
@@ -414,11 +522,11 @@ origincar_base::origincar_base()
robotpose_publisher = create_publisher<origincar_msg::msg::Data>("robotpose", 10);
robotvel_publisher = create_publisher<origincar_msg::msg::Data>("robotvel", 10);
pose_pub_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
"/set_pose",
rclcpp::SystemDefaultsQoS().reliable());
"/set_pose",
rclcpp::SystemDefaultsQoS().reliable());
tf_bro = std::make_shared<tf2_ros::TransformBroadcaster>(this);
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
Cmd_Vel_Sub = create_subscription<geometry_msgs::msg::Twist>(
@@ -427,35 +535,39 @@ origincar_base::origincar_base()
akm_cmd_vel, 1, std::bind(&origincar_base::Akm_Cmd_Vel_Callback, this, _1));
Sign_Switch_Sub = create_subscription<std_msgs::msg::Int32>(
"/sign4return", 1, std::bind(&origincar_base::Sign_Switch_Callback, this, _1));
try {
"/sign4return", 1, std::bind(&origincar_base::Sign_Switch_Callback, this, _1));
try
{
Stm32_Serial.setPort("/dev/ttyACM0");
Stm32_Serial.setBaudrate(serial_baud_rate);
serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
Stm32_Serial.setTimeout(_time);
Stm32_Serial.open();
} catch (serial::IOException& e) {
RCLCPP_ERROR(this->get_logger(),"origincar_base can not open serial port,Please check the serial port cable! ");
}
if(Stm32_Serial.isOpen()) {
RCLCPP_INFO(this->get_logger(),"origincar_base serial port opened");
catch (serial::IOException &e)
{
RCLCPP_ERROR(this->get_logger(), "origincar_base can not open serial port,Please check the serial port cable! ");
}
if (Stm32_Serial.isOpen())
{
RCLCPP_INFO(this->get_logger(), "origincar_base serial port opened");
}
}
void sigintHandler(int sig)
{
sig = sig;
printf("OriginBot shutdown...\n");
serial::Serial Stm32_Serial;
Stm32_Serial.setPort("/dev/ttyACM0");
Stm32_Serial.setBaudrate(115200);
serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
Stm32_Serial.setTimeout(_time);
Stm32_Serial.open();
SEND_DATA Send_Data;
if (Stm32_Serial.isOpen()) {
Send_Data.tx[0]=FRAME_HEADER;
sig = sig;
printf("OriginBot shutdown...\n");
serial::Serial Stm32_Serial;
Stm32_Serial.setPort("/dev/ttyACM0");
Stm32_Serial.setBaudrate(115200);
serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
Stm32_Serial.setTimeout(_time);
Stm32_Serial.open();
SEND_DATA Send_Data;
if (Stm32_Serial.isOpen())
{
Send_Data.tx[0] = FRAME_HEADER;
Send_Data.tx[1] = 0;
Send_Data.tx[2] = 0;
@@ -468,23 +580,26 @@ void sigintHandler(int sig)
Send_Data.tx[7] = 0;
Send_Data.tx[8] = 0;
int check_sum = 0;
for (int k = 0; k < 9; k++) {
check_sum = check_sum^Send_Data.tx[k];
}
Send_Data.tx[9]=check_sum;
Send_Data.tx[10]=FRAME_TAIL;
try {
Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx));
} catch (serial::IOException& e) {
for (int k = 0; k < 9; k++)
{
check_sum = check_sum ^ Send_Data.tx[k];
}
Send_Data.tx[9] = check_sum;
Send_Data.tx[10] = FRAME_TAIL;
try
{
Stm32_Serial.write(Send_Data.tx, sizeof(Send_Data.tx));
}
catch (serial::IOException &e)
{
}
}
// 关闭ROS2接口，清除资源
rclcpp::shutdown();
// 关闭ROS2接口，清除资源
rclcpp::shutdown();
}
origincar_base::~origincar_base()
{
RCLCPP_INFO(this->get_logger(),"Shutting down");
}
RCLCPP_INFO(this->get_logger(), "Shutting down");
}

197
src/planner/config/planner.yaml
View File

@@ -1,232 +1,399 @@
# ============================================================================
# planner.yaml — Nav2 minimal global planning stack
# planner.yaml - Nav2 minimal global planning stack
# 节点: slam_toolbox + planner_server + global_costmap + local_costmap
# 来源: gc_navigation2_slamtoolbox (slam + nav params) 剪裁
# 用途: 启动建图、全局规划、全局代价地图和局部代价地图所需的 ROS2 参数。
# 说明: 本文件只配置参数；实际由 launch 文件把这些参数加载到对应节点。
# ============================================================================
# ---------------------------------------------------------------------------
# slam_toolbox — online_async 实时建图 → 发布 /map + map→odom transform
# 来源: slam_toolbox_mapping.yaml
# slam_toolbox - online_async 实时建图
# 主要作用:
# 1. 订阅激光 /scan 和里程计 TF。
# 2. 生成 /map 占用栅格地图。
# 3. 发布 map -> odom 变换，使 map 坐标系和机器人 odom/base_link 连接起来。
# ---------------------------------------------------------------------------
slam_toolbox:
ros__parameters:
# 是否使用仿真时间 /clock。真实车通常为 False；Gazebo/rosbag 回放通常为 True。
use_sim_time: False
# Solver
# 后端图优化求解器插件。CeresSolver 用 Ceres 做位姿图优化，是 slam_toolbox 常用选择。
solver_plugin: solver_plugins::CeresSolver
# Ceres 线性求解器。SPARSE_NORMAL_CHOLESKY 适合稀疏 SLAM 图，速度和精度较均衡。
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
# Ceres 预条件器。SCHUR_JACOBI 常用于 bundle/pose graph 类问题，加速迭代收敛。
ceres_preconditioner: SCHUR_JACOBI
# 信赖域策略。LEVENBERG_MARQUARDT 稳定性好，适合非线性最小二乘优化。
ceres_trust_strategy: LEVENBERG_MARQUARDT
# dogleg 策略类型；仅在 dogleg 信赖域策略启用时影响优化路径，这里保留默认传统形式。
ceres_dogleg_type: TRADITIONAL_DOGLEG
# 鲁棒核函数。None 表示不额外压制离群约束；激光匹配质量差时可考虑鲁棒 loss。
ceres_loss_function: None
# ROS base
# 里程计坐标系名。slam_toolbox 需要 TF 中存在 odom -> base_link 或等价链路。
odom_frame: odom
# 地图坐标系名。/map 消息和 map -> odom TF 都以这个坐标系作为全局参考。
map_frame: map
# 机器人车体坐标系名。用于从 TF 读取机器人当前在 odom/map 下的位姿。
base_frame: base_link
# 激光雷达话题。slam_toolbox 从这里取 LaserScan 做 scan matching 和建图。
scan_topic: /scan
# SLAM 工作模式。mapping 表示在线建图；localization 表示基于已有地图定位。
mode: mapping
# 是否启用地图保存相关服务/能力，便于后续保存当前构建出的地图。
use_map_saver: true
# Debug & performance
# 是否输出详细调试日志。排查匹配/闭环问题时可开；平时关闭减少日志量。
debug_logging: false
throttle_scans: 1
# 激光降采样倍率。1 表示每帧都处理；增大可降负载但地图更新更慢。
throttle_scans: 2
# 发布 map -> odom TF 的周期，单位秒。0.02 表示约 50Hz，越小 TF 越实时但 CPU 占用更高。
transform_publish_period: 0.02
map_update_interval: 3.0
# /map 地图更新周期，单位秒。数值越小地图刷新越频繁，但计算/网络开销更高。
map_update_interval: 1.0
# 地图分辨率，单位 m/cell。0.05 表示每个栅格 5cm；越小越精细但地图更大。
resolution: 0.05
# 接受的最小激光距离，小于该距离的点会被过滤，避免近距离噪声/车体反射。
min_laser_range: 0.15
# 接受的最大激光距离，超过该距离的点会被过滤，避免远距离弱回波干扰。
max_laser_range: 20.0
# 两次处理激光之间的最小时间间隔，单位秒；用于限制处理频率。
minimum_time_interval: 0.5
# 查询 TF 的超时时间，单位秒；TF 延迟超过该值会导致当前帧无法处理。
transform_timeout: 0.2
tf_buffer_duration: 30.0
# TF 缓冲时间长度，单位秒；越大越能容忍延迟数据，但内存占用增加。
tf_buffer_duration: 10.0
# slam_toolbox 内部线程栈大小；地图/图优化规模大时需要足够栈空间。
stack_size_to_use: 40000000
# 是否启用交互模式，允许通过 RViz/服务交互式调整图节点或执行手动操作。
enable_interactive_mode: true
# Mapping
# 是否使用 scan matching。开启后根据激光匹配修正里程计漂移，是建图核心能力。
use_scan_matching: true
# 是否使用激光点云重心辅助匹配，可改善局部匹配初值稳定性。
use_scan_barycenter: true
# 机器人至少移动多少米才添加/处理新的位姿节点；越小图更密，越大负载更低。
minimum_travel_distance: 0.5
# 机器人至少转动多少弧度才添加/处理新的位姿节点；越小对转弯更敏感。
minimum_travel_heading: 0.1
# scan buffer 中保留的扫描帧数量，用于局部匹配和链路建立。
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10.0
# scan buffer 中扫描之间允许的最大距离，超过该距离的历史帧不再用于匹配。
scan_buffer_maximum_scan_distance: 5.0
# 精匹配响应最低阈值；低于该值的局部链路匹配会被认为不可靠。
link_match_minimum_response_fine: 0.1
# 建立相邻 scan 链路时允许的最大空间距离，超过则不尝试连接。
link_scan_maximum_distance: 1.5
# Loop closure
do_loop_closing: true
# 是否启用闭环检测。开启后回到旧区域时可修正累计漂移。
do_loop_closing: false
# 闭环候选链最小长度；较大可减少误闭环，较小更容易检测短回环。
loop_match_minimum_chain_size: 10
# 粗匹配允许的最大方差；越小越严格，误闭环少但可能漏闭环。
loop_match_maximum_variance_coarse: 3.0
# 闭环粗匹配最低响应阈值；候选低于该值会被丢弃。
loop_match_minimum_response_coarse: 0.35
# 闭环精匹配最低响应阈值；越高越保守，闭环质量更可靠。
loop_match_minimum_response_fine: 0.45
# 搜索闭环候选的最大距离，单位米；越大越容易找回环但计算量和误匹配风险增加。
loop_search_maximum_distance: 3.0
# Scan matching
# 局部 scan matching 搜索窗口尺寸，单位米；越大越能容忍初值误差但更耗时。
correlation_search_space_dimension: 0.5
# 局部搜索分辨率，单位米；越小搜索更精细但计算更多。
correlation_search_space_resolution: 0.01
# 相关性搜索的 smear 标准差，用于让匹配响应更平滑，提升抗噪性。
correlation_search_space_smear_deviation: 0.1
# 闭环搜索窗口尺寸，单位米；通常比局部匹配更大以覆盖漂移。
loop_search_space_dimension: 8.0
# 闭环搜索分辨率，单位米；控制闭环候选搜索精度和计算量。
loop_search_space_resolution: 0.05
# 闭环搜索 smear 标准差，用于平滑闭环匹配响应。
loop_search_space_smear_deviation: 0.03
# Matcher params
# 位移方差惩罚。越大越不愿接受与预测位移差异大的匹配结果。
distance_variance_penalty: 0.5
# 角度方差惩罚。越大越不愿接受与预测角度差异大的匹配结果。
angle_variance_penalty: 1.0
# 精搜索角度步进，单位弧度；越小角度搜索更细但更慢。
fine_search_angle_offset: 0.00349
# 粗搜索角度范围/步进相关参数，单位弧度；用于大范围初始角度搜索。
coarse_search_angle_offset: 0.349
# 粗搜索角分辨率，单位弧度；越小越精细但计算量更高。
coarse_angle_resolution: 0.0349
# 最小角度惩罚因子，限制角度惩罚不要过低，避免过度相信异常角度匹配。
minimum_angle_penalty: 0.9
# 最小距离惩罚因子，限制距离惩罚不要过低，避免过度相信异常位移匹配。
minimum_distance_penalty: 0.5
# 是否扩展匹配响应区域，有助于在响应较弱时找到可接受匹配。
use_response_expansion: true
# 激光穿过栅格的最小次数阈值，用于空闲/占用证据更新。
min_pass_through: 2
# 占用概率阈值。超过该阈值的栅格更倾向标为障碍。
occupancy_threshold: 0.1
# 输入 scan 队列大小；处理跟不上时队列过小会丢帧，过大可能增加延迟。
scan_queue_size: 20
# ---------------------------------------------------------------------------
# planner_server — SmacPlannerHybrid 全局路径规划 → /plan
# 来源: gc_navigation_slam.yaml planner_server section
# planner_server - SmacPlannerHybrid 全局路径规划
# 主要作用:
# 1. 接收起点/终点和全局代价地图。
# 2. 使用 Hybrid-A* / Reeds-Shepp 运动模型生成符合车体转弯约束的 /plan。
# ---------------------------------------------------------------------------
planner_server:
ros__parameters:
# 规划插件列表。每个名字对应下方同名配置块；这里仅启用 GridBased。
planner_plugins:
- GridBased
# 是否使用仿真时间。应与其它 Nav2 节点保持一致。
use_sim_time: False
GridBased:
# 插件类型。SmacPlannerHybrid 适合非完整约束车辆，能考虑转弯半径和倒车。
plugin: nav2_smac_planner/SmacPlannerHybrid
# 是否对 costmap 降采样后规划。False 表示用原始分辨率，路径更细但计算更多。
downsample_costmap: False
# 降采样倍率；只有 downsample_costmap=True 时生效。
downsampling_factor: 1
# 允许规划终点距离目标的容差，单位米；目标附近不可达时可在容差内结束。
tolerance: 0.25
# 是否允许路径经过未知区域。True 可穿过未探索区域，False 更保守。
allow_unknown: True
# 最大搜索迭代次数；越大越不容易提前失败，但规划耗时上限更高。
max_iterations: 100000
# 接近目标阶段的最大迭代次数，用于目标附近精细搜索。
max_on_approach_iterations: 1000
# 单次规划最大耗时，单位秒；超时后规划失败或返回当前可行结果。
max_planning_time: 5.0
# 搜索运动模型。REEDS_SHEPP 支持前进和倒车，适合可倒车小车。
motion_model_for_search: REEDS_SHEPP
# 朝向离散桶数量。72 表示 5 度一个方向；越大方向更细但搜索量更大。
angle_quantization_bins: 72
# 解析扩展触发比例，用于尝试直接连接目标，提高接近目标时的效率。
analytic_expansion_ratio: 2.0
# 解析扩展最大长度，单位米；太长可能穿障碍，太短接近目标效率降低。
analytic_expansion_max_length: 3.0
# 最小转弯半径，单位米。应接近实车运动学能力；过小会生成车转不过的路径。
minimum_turning_radius: 0.35
# 倒车惩罚。大于 1 会减少倒车段；值越大越偏好前进。
reverse_penalty: 1.3
# 前进/倒车切换惩罚。增大后路径会减少换向次数。
change_penalty: 0.0
# 非直线运动惩罚。增大后更偏好直线路径，可能牺牲可达性。
non_straight_penalty: 0.0
# 代价地图代价惩罚。越大越远离障碍/高代价区域，但可能绕路更多。
cost_penalty: 5.0
# 回溯惩罚。用于影响搜索顺序，通常小正数可提升搜索效率。
retrospective_penalty: 0.015
# 运动启发查表范围，单位米；越大预计算更多，可能提升规划质量但占内存。
lookup_table_size: 5.0
# 是否缓存障碍启发。静态环境可开以加速重复规划；动态环境关闭更安全。
cache_obstacle_heuristic: False
# 是否发布搜索扩展可视化。调试规划失败时可开，平时关闭减少开销。
viz_expansions: False
# 是否对生成路径做平滑。True 通常让路径更适合控制器跟踪。
smooth_path: True
smoother:
# 平滑器最大迭代次数；越大越可能收敛但耗时更长。
max_iterations: 1000
# 平滑权重。越大路径越平滑，但可能偏离原始安全路径。
w_smooth: 0.4
# 数据保持权重。越大越贴近原始规划路径，越不容易被平滑拉偏。
w_data: 0.2
# 平滑收敛阈值；变化小于该值时停止迭代。
tolerance: 1.0e-10
# 是否进行多轮细化平滑，改善最终路径质量。
do_refinement: True
# 细化轮数；轮数越多路径越平滑但耗时增加。
refinement_num: 4
planner_server_rclcpp_node:
ros__parameters:
# planner_server 辅助 rclcpp 节点的时间源设置，应与 planner_server 保持一致。
use_sim_time: False
# ---------------------------------------------------------------------------
# global_costmap — 全局代价地图（订阅 /scan + /map）
# 来源: gc_navigation_slam.yaml global_costmap section
# global_costmap - 全局代价地图
# 主要作用:
# 1. 在 map 坐标系下融合静态地图和激光障碍。
# 2. 给 planner_server 提供从起点到目标点的全局规划代价。
# ---------------------------------------------------------------------------
global_costmap:
global_costmap:
ros__parameters:
# 是否使用仿真时间。应与 SLAM、planner_server 和其它 Nav2 节点一致。
use_sim_time: False
# TF 允许的时间容差，单位秒；传感器/TF 稍有延迟时可避免频繁报错。
transform_tolerance: 2.0
# 代价地图更新频率，单位 Hz；越高越实时但 CPU 占用更高。
update_frequency: 1.0
# 代价地图发布频率，单位 Hz；影响 RViz/Foxglove 可视化刷新和下游接收频率。
publish_frequency: 1.0
# 全局代价地图所在坐标系。全局规划通常使用 map。
global_frame: map
# 机器人基坐标系，用于从 TF 获取机器人在 costmap 中的位置。
robot_base_frame: base_link
# 机器人二维足迹，多边形点按 base_link 坐标给出，单位米；用于碰撞检测。
footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
# 足迹外扩安全边距，单位米；越大规划离障碍越远但可通行空间变窄。
footprint_padding: 0.02
# 代价地图分辨率，单位 m/cell；应与地图精度和计算能力匹配。
resolution: 0.05
# 是否保留未知空间。True 时未知区域保持 unknown，配合 allow_unknown 决定能否规划穿过。
track_unknown_space: True
# 启用的 costmap layer 顺序。静态层提供地图，障碍层叠加实时障碍，膨胀层扩展安全距离。
plugins:
- static_layer
- obstacle_layer
- inflation_layer
obstacle_layer:
# 障碍层插件类型，负责把 LaserScan/PointCloud 转为障碍和清障信息。
plugin: nav2_costmap_2d::ObstacleLayer
# 是否启用障碍层。False 时该层不参与代价地图。
enabled: True
# 观测源名称列表；这里定义一个名为 scan 的传感器源。
observation_sources: scan
scan:
# 激光雷达话题名。障碍层从这里读取 LaserScan。
topic: /scan
# 处理障碍的最大高度，单位米；LaserScan 通常按 2D 使用，此参数保留兼容层配置。
max_obstacle_height: 2.0
# 是否用激光射线清除自由空间。True 会把射线经过区域标为空闲。
clearing: True
# 是否用激光命中点标记障碍。True 会把终点附近标为障碍。
marking: True
# 传感器数据类型。LaserScan 表示订阅 sensor_msgs/msg/LaserScan。
data_type: LaserScan
# 清障射线最大距离，单位米；越大清除范围越远。
raytrace_max_range: 3.0
# 清障射线最小距离，单位米；小于该距离不参与清障。
raytrace_min_range: 0.0
# 标记障碍最大距离，单位米；超过该距离的障碍点不写入 costmap。
obstacle_max_range: 2.5
# 标记障碍最小距离，单位米；小于该距离的点不写入 costmap。
obstacle_min_range: 0.0
static_layer:
# 静态地图层插件类型，订阅 /map 并把占用栅格转成全局代价。
plugin: nav2_costmap_2d::StaticLayer
# 是否用 transient local QoS 订阅地图。True 可收到已经发布过的 latched /map。
map_subscribe_transient_local: True
inflation_layer:
# 膨胀层插件类型，把障碍周围一定范围扩展为逐渐降低的代价。
plugin: nav2_costmap_2d::InflationLayer
# 膨胀代价衰减系数。越大衰减越快，障碍影响范围内高代价区域更窄。
cost_scaling_factor: 3.0
# 膨胀半径，单位米。障碍周围该半径内会增加代价。
inflation_radius: 0.2
# 是否每次发布完整 costmap。True 简化可视化/调试，但带宽占用更高。
always_send_full_costmap: True
global_costmap_client:
ros__parameters:
# global_costmap lifecycle client 的时间源设置，应与 global_costmap 保持一致。
use_sim_time: False
global_costmap_rclcpp_node:
ros__parameters:
# global_costmap 内部 rclcpp 节点的时间源设置，应与 global_costmap 保持一致。
use_sim_time: False
# ---------------------------------------------------------------------------
# local_costmap — 局部代价地图（obstacle_detector 可注入）
# 来源: gc_navigation_slam.yaml local_costmap section
# local_costmap - 局部代价地图
# 主要作用:
# 1. 在 odom 坐标系下围绕机器人滚动更新附近障碍。
# 2. 给局部控制器/避障模块提供短距离实时环境代价。
# ---------------------------------------------------------------------------
local_costmap:
local_costmap:
ros__parameters:
# 局部代价地图更新频率，单位 Hz；越高避障越实时但 CPU 占用更高。
update_frequency: 1.0
# 局部代价地图发布频率，单位 Hz；影响可视化和下游节点接收频率。
publish_frequency: 2.0
# TF 查询容差，单位秒；允许 odom/base_link TF 存在小幅延迟。
transform_tolerance: 2.0
# 局部地图所在坐标系。局部 costmap 通常用 odom，避免 map 闭环跳变影响局部控制。
global_frame: odom
# 机器人基坐标系，用于把机器人足迹放到局部代价地图中。
robot_base_frame: base_link
# 是否使用仿真时间。应与其它 Nav2 节点一致。
use_sim_time: False
# 是否启用滚动窗口。True 表示地图窗口跟随机器人移动。
rolling_window: True
# 局部窗口宽度，单位米；越大能看到更远障碍但计算更多。
width: 3
# 局部窗口高度，单位米；越大能看到更远障碍但计算更多。
height: 3
# 局部代价地图分辨率，单位 m/cell；越小越精细但计算更多。
resolution: 0.05
# 机器人二维足迹，单位米；用于局部碰撞检测。
footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
# 足迹外扩安全边距，单位米；越大越保守。
footprint_padding: 0.02
# 启用的局部 costmap layer。当前只启用 voxel_layer 和 inflation_layer。
plugins:
- voxel_layer
- inflation_layer
inflation_layer:
# 局部膨胀层插件类型，负责给附近障碍周围增加安全代价。
plugin: nav2_costmap_2d::InflationLayer
# 膨胀代价衰减系数。越大衰减越快，机器人更贴近障碍。
cost_scaling_factor: 3.0
# 膨胀半径，单位米；建议不小于机器人定位误差和控制误差。
inflation_radius: 0.2
voxel_layer:
# 体素层插件类型，可维护 3D 障碍体素并投影到 2D costmap。
plugin: nav2_costmap_2d::VoxelLayer
# 是否启用体素层。False 时不会把 scan 写入该层。
enabled: True
# 是否发布体素地图，便于 RViz/Foxglove 调试障碍高度栅格。
publish_voxel_map: True
# 体素网格 z 方向起点，单位米；通常从地面或激光参考高度附近开始。
origin_z: 0.0
# 单个体素 z 方向分辨率，单位米。
z_resolution: 0.05
# z 方向体素层数；与 z_resolution 相乘决定可表达的高度范围。
z_voxels: 16
# 最大障碍高度，单位米；高于该值的数据不作为障碍处理。
max_obstacle_height: 2.0
# 标记障碍所需的最小体素命中阈值。0 表示非常敏感，容易标障碍。
mark_threshold: 0
# 观测源名称列表；这里使用 scan。
observation_sources: scan
scan:
# 激光雷达话题名。体素层从这里读取 LaserScan。
topic: /scan
# 该观测源接受的最大障碍高度，单位米。
max_obstacle_height: 2.0
# 是否用激光射线清除自由空间。
clearing: True
# 是否用激光命中点标记障碍。
marking: True
# 传感器数据类型。LaserScan 表示 2D 激光。
data_type: LaserScan
# 清障射线最大距离，单位米。
raytrace_max_range: 3.0
# 清障射线最小距离，单位米。
raytrace_min_range: 0.0
# 标记障碍最大距离，单位米。
obstacle_max_range: 2.5
# 标记障碍最小距离，单位米。
obstacle_min_range: 0.0
static_layer:
# 注意: 当前 local_costmap.plugins 未包含 static_layer，因此这个配置块不会生效。
# 若以后把 static_layer 加入 plugins，该参数表示用 transient local QoS 订阅 /map。
map_subscribe_transient_local: True
# 是否每次发布完整局部 costmap。True 便于可视化，但占用更多带宽。
always_send_full_costmap: True
local_costmap_client:
ros__parameters:
# local_costmap lifecycle client 的时间源设置，应与 local_costmap 保持一致。
use_sim_time: False
local_costmap_rclcpp_node:
ros__parameters:
# local_costmap 内部 rclcpp 节点的时间源设置，应与 local_costmap 保持一致。
use_sim_time: False

232
src/planner/config/planner.yaml.bak- Normal file
View File

@@ -0,0 +1,232 @@
# ============================================================================
# planner.yaml — Nav2 minimal global planning stack
# 节点: slam_toolbox + planner_server + global_costmap + local_costmap
# 来源: gc_navigation2_slamtoolbox (slam + nav params) 剪裁
# ============================================================================
# ---------------------------------------------------------------------------
# slam_toolbox — online_async 实时建图 → 发布 /map + map→odom transform
# 来源: slam_toolbox_mapping.yaml
# ---------------------------------------------------------------------------
slam_toolbox:
ros__parameters:
use_sim_time: False
# Solver
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None
# ROS base
odom_frame: odom
map_frame: map
base_frame: base_link
scan_topic: /scan
mode: mapping
use_map_saver: true
# Debug & performance
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02
map_update_interval: 3.0
resolution: 0.05
min_laser_range: 0.15
max_laser_range: 20.0
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 30.0
stack_size_to_use: 40000000
enable_interactive_mode: true
# Mapping
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.5
minimum_travel_heading: 0.1
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10.0
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
# Loop closure
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
loop_match_minimum_response_fine: 0.45
loop_search_maximum_distance: 3.0
# Scan matching
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
# Matcher params
distance_variance_penalty: 0.5
angle_variance_penalty: 1.0
fine_search_angle_offset: 0.00349
coarse_search_angle_offset: 0.349
coarse_angle_resolution: 0.0349
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: true
min_pass_through: 2
occupancy_threshold: 0.1
scan_queue_size: 20
# ---------------------------------------------------------------------------
# planner_server — SmacPlannerHybrid 全局路径规划 → /plan
# 来源: gc_navigation_slam.yaml planner_server section
# ---------------------------------------------------------------------------
planner_server:
ros__parameters:
planner_plugins:
- GridBased
use_sim_time: False
GridBased:
plugin: nav2_smac_planner/SmacPlannerHybrid
downsample_costmap: False
downsampling_factor: 1
tolerance: 0.25
allow_unknown: True
max_iterations: 100000
max_on_approach_iterations: 1000
max_planning_time: 5.0
motion_model_for_search: REEDS_SHEPP
angle_quantization_bins: 72
analytic_expansion_ratio: 2.0
analytic_expansion_max_length: 3.0
minimum_turning_radius: 0.35
reverse_penalty: 1.3
change_penalty: 0.0
non_straight_penalty: 0.0
cost_penalty: 5.0
retrospective_penalty: 0.015
lookup_table_size: 5.0
cache_obstacle_heuristic: False
viz_expansions: False
smooth_path: True
smoother:
max_iterations: 1000
w_smooth: 0.4
w_data: 0.2
tolerance: 1.0e-10
do_refinement: True
refinement_num: 4
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: False
# ---------------------------------------------------------------------------
# global_costmap — 全局代价地图（订阅 /scan + /map）
# 来源: gc_navigation_slam.yaml global_costmap section
# ---------------------------------------------------------------------------
global_costmap:
global_costmap:
ros__parameters:
use_sim_time: False
transform_tolerance: 2.0
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
footprint_padding: 0.02
resolution: 0.05
track_unknown_space: True
plugins:
- static_layer
- obstacle_layer
- inflation_layer
obstacle_layer:
plugin: nav2_costmap_2d::ObstacleLayer
enabled: True
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: LaserScan
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
static_layer:
plugin: nav2_costmap_2d::StaticLayer
map_subscribe_transient_local: True
inflation_layer:
plugin: nav2_costmap_2d::InflationLayer
cost_scaling_factor: 3.0
inflation_radius: 0.2
always_send_full_costmap: True
global_costmap_client:
ros__parameters:
use_sim_time: False
global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: False
# ---------------------------------------------------------------------------
# local_costmap — 局部代价地图（obstacle_detector 可注入）
# 来源: gc_navigation_slam.yaml local_costmap section
# ---------------------------------------------------------------------------
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 1.0
publish_frequency: 2.0
transform_tolerance: 2.0
global_frame: odom
robot_base_frame: base_link
use_sim_time: False
rolling_window: True
width: 3
height: 3
resolution: 0.05
footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
footprint_padding: 0.02
plugins:
- voxel_layer
- inflation_layer
inflation_layer:
plugin: nav2_costmap_2d::InflationLayer
cost_scaling_factor: 3.0
inflation_radius: 0.2
voxel_layer:
plugin: nav2_costmap_2d::VoxelLayer
enabled: True
publish_voxel_map: True
origin_z: 0.0
z_resolution: 0.05
z_voxels: 16
max_obstacle_height: 2.0
mark_threshold: 0
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: LaserScan
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
static_layer:
map_subscribe_transient_local: True
always_send_full_costmap: True
local_costmap_client:
ros__parameters:
use_sim_time: False
local_costmap_rclcpp_node:
ros__parameters:
use_sim_time: False

232
src/planner/config/planner.yaml.bak-20260623-160129 Normal file
View File

@@ -0,0 +1,232 @@
# ============================================================================
# planner.yaml — Nav2 minimal global planning stack
# 节点: slam_toolbox + planner_server + global_costmap + local_costmap
# 来源: gc_navigation2_slamtoolbox (slam + nav params) 剪裁
# ============================================================================
# ---------------------------------------------------------------------------
# slam_toolbox — online_async 实时建图 → 发布 /map + map→odom transform
# 来源: slam_toolbox_mapping.yaml
# ---------------------------------------------------------------------------
slam_toolbox:
ros__parameters:
use_sim_time: False
# Solver
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None
# ROS base
odom_frame: odom
map_frame: map
base_frame: base_link
scan_topic: /scan
mode: mapping
use_map_saver: true
# Debug & performance
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02
map_update_interval: 3.0
resolution: 0.05
min_laser_range: 0.15
max_laser_range: 20.0
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 30.0
stack_size_to_use: 40000000
enable_interactive_mode: true
# Mapping
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.5
minimum_travel_heading: 0.1
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10.0
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
# Loop closure
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
loop_match_minimum_response_fine: 0.45
loop_search_maximum_distance: 3.0
# Scan matching
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
# Matcher params
distance_variance_penalty: 0.5
angle_variance_penalty: 1.0
fine_search_angle_offset: 0.00349
coarse_search_angle_offset: 0.349
coarse_angle_resolution: 0.0349
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: true
min_pass_through: 2
occupancy_threshold: 0.1
scan_queue_size: 20
# ---------------------------------------------------------------------------
# planner_server — SmacPlannerHybrid 全局路径规划 → /plan
# 来源: gc_navigation_slam.yaml planner_server section
# ---------------------------------------------------------------------------
planner_server:
ros__parameters:
planner_plugins:
- GridBased
use_sim_time: False
GridBased:
plugin: nav2_smac_planner/SmacPlannerHybrid
downsample_costmap: False
downsampling_factor: 1
tolerance: 0.25
allow_unknown: True
max_iterations: 100000
max_on_approach_iterations: 1000
max_planning_time: 5.0
motion_model_for_search: REEDS_SHEPP
angle_quantization_bins: 72
analytic_expansion_ratio: 2.0
analytic_expansion_max_length: 3.0
minimum_turning_radius: 0.35
reverse_penalty: 1.3
change_penalty: 0.0
non_straight_penalty: 0.0
cost_penalty: 5.0
retrospective_penalty: 0.015
lookup_table_size: 5.0
cache_obstacle_heuristic: False
viz_expansions: False
smooth_path: True
smoother:
max_iterations: 1000
w_smooth: 0.4
w_data: 0.2
tolerance: 1.0e-10
do_refinement: True
refinement_num: 4
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: False
# ---------------------------------------------------------------------------
# global_costmap — 全局代价地图（订阅 /scan + /map）
# 来源: gc_navigation_slam.yaml global_costmap section
# ---------------------------------------------------------------------------
global_costmap:
global_costmap:
ros__parameters:
use_sim_time: False
transform_tolerance: 2.0
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
footprint_padding: 0.02
resolution: 0.05
track_unknown_space: True
plugins:
- static_layer
- obstacle_layer
- inflation_layer
obstacle_layer:
plugin: nav2_costmap_2d::ObstacleLayer
enabled: True
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: LaserScan
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
static_layer:
plugin: nav2_costmap_2d::StaticLayer
map_subscribe_transient_local: True
inflation_layer:
plugin: nav2_costmap_2d::InflationLayer
cost_scaling_factor: 3.0
inflation_radius: 0.2
always_send_full_costmap: True
global_costmap_client:
ros__parameters:
use_sim_time: False
global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: False
# ---------------------------------------------------------------------------
# local_costmap — 局部代价地图（obstacle_detector 可注入）
# 来源: gc_navigation_slam.yaml local_costmap section
# ---------------------------------------------------------------------------
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 1.0
publish_frequency: 2.0
transform_tolerance: 2.0
global_frame: odom
robot_base_frame: base_link
use_sim_time: False
rolling_window: True
width: 3
height: 3
resolution: 0.05
footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
footprint_padding: 0.02
plugins:
- voxel_layer
- inflation_layer
inflation_layer:
plugin: nav2_costmap_2d::InflationLayer
cost_scaling_factor: 3.0
inflation_radius: 0.2
voxel_layer:
plugin: nav2_costmap_2d::VoxelLayer
enabled: True
publish_voxel_map: True
origin_z: 0.0
z_resolution: 0.05
z_voxels: 16
max_obstacle_height: 2.0
mark_threshold: 0
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: LaserScan
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
static_layer:
map_subscribe_transient_local: True
always_send_full_costmap: True
local_costmap_client:
ros__parameters:
use_sim_time: False
local_costmap_rclcpp_node:
ros__parameters:
use_sim_time: False

14
src/yolov8_launch/config/yolov8workconfig.json Normal file
View File

@@ -0,0 +1,14 @@
{
"model_file": "/home/sunrise/yiliao_ws/models/obstacle-133.bin",
"task_num": 4,
"dnn_Parser": "yolov8",
"model_output_count": 6,
"reg_max": 16,
"class_num": 1,
"cls_names_list": "/home/sunrise/yiliao_ws/src/yolov8_launch/class_list/cone.list",
"strides": [8, 16, 32],
"score_threshold": 0.5,
"nms_threshold": 0.7,
"nms_top_k": 300,
"output_order": [0,1,2,3,4,5]
}

6
src/yolov8_launch/launch/dnn_node_example.launch.py
View File

@@ -35,10 +35,10 @@ def generate_launch_description():
"dnn_example_dump_render_img", default_value=TextSubstitution(text="0")
)
image_width_launch_arg = DeclareLaunchArgument(
"dnn_example_image_width", default_value=TextSubstitution(text="640")
"dnn_example_image_width", default_value=TextSubstitution(text="1920")
)
image_height_launch_arg = DeclareLaunchArgument(
"dnn_example_image_height", default_value=TextSubstitution(text="480")
"dnn_example_image_height", default_value=TextSubstitution(text="1080")
)
msg_pub_topic_name_launch_arg = DeclareLaunchArgument(
"dnn_example_msg_pub_topic_name", default_value=TextSubstitution(text="hobot_dnn_detection")
@@ -56,7 +56,7 @@ def generate_launch_description():
# usb cam图片发布pkg
usb_cam_device_arg = DeclareLaunchArgument(
'device',
default_value='/dev/video8',
default_value='/dev/video0',
description='usb camera device')
usb_node = IncludeLaunchDescription(