lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: lyq:27829447c03f459f892c0090bfb15756ccc2ba80
Branches Tags
lyq:master
lyq:humble
lyq:main
..
compare: lyq:fa8907ca6a1234677390b3b596151455f7abad91
Branches Tags
lyq:humble
lyq:main
lyq:master

No commits in common. "27829447c03f459f892c0090bfb15756ccc2ba80" and "fa8907ca6a1234677390b3b596151455f7abad91" have entirely different histories.
27829447c0 ... fa8907ca6a

3 changed files with 138 additions and 194 deletions
Show Stats Expand all files Collapse all files

8
src/autonomy/fu/src/fu_node.cpp
View File

@ -669,7 +669,6 @@ class fu_node : public rclcpp::Node
is_start = msg->is_start; is_start = msg->is_start;
LOG_INFO("[规划层消息回调] 目标点位置： x -- %.2f , y -- %.2f", pl_x_, pl_y_); LOG_INFO("[规划层消息回调] 目标点位置： x -- %.2f , y -- %.2f", pl_x_, pl_y_);
LOG_INFO("[规划层消息回调] pl_speed_： %d ", pl_speed_);
} }
// ======== 绕障路点规划 ======== // ======== 绕障路点规划 ========
@ -764,7 +763,7 @@ class fu_node : public rclcpp::Node
auto now = system_clock::now(); auto now = system_clock::now();
duration<double> time_since_last = now - last_rtk_time; duration<double> time_since_last = now - last_rtk_time;
if (g_rtk.reliability == 1 && time_since_last <= rtk_timeout) if (g_rtk.reliability != 0 && time_since_last <= rtk_timeout)
{ {
// RTK信号良好：使用绝对坐标判断 // RTK信号良好：使用绝对坐标判断
double distance = ntzx_GPS_length(g_rtk.lon, g_rtk.lat, target.lon, target.lat); double distance = ntzx_GPS_length(g_rtk.lon, g_rtk.lat, target.lon, target.lat);
@ -935,8 +934,7 @@ class fu_node : public rclcpp::Node
// 检查RTK信号 // 检查RTK信号
auto rtk_now = system_clock::now(); auto rtk_now = system_clock::now();
duration<double> time_since_last_rtk = rtk_now - last_rtk_time; duration<double> time_since_last_rtk = rtk_now - last_rtk_time;
bool rtk_signal_good = (g_rtk.reliability == 1 && time_since_last_rtk <= rtk_timeout); bool rtk_signal_good = (g_rtk.reliability != 0 && time_since_last_rtk <= rtk_timeout);
// LOG_INFO("rtk_signal_good ? %d",rtk_signal_good);
// 状态3: 雷达数据超时，停车 // 状态3: 雷达数据超时，停车
if (grid_age > 3.0) if (grid_age > 3.0)
@ -1036,7 +1034,7 @@ class fu_node : public rclcpp::Node
// 若之前处于WAITING状态，且现在没有障碍物且RTK信号良好，恢复到NORMAL状态 // 若之前处于WAITING状态，且现在没有障碍物且RTK信号良好，恢复到NORMAL状态
auto now = system_clock::now(); auto now = system_clock::now();
duration<double> time_since_last_rtk = now - last_rtk_time; duration<double> time_since_last_rtk = now - last_rtk_time;
bool rtk_signal_good = (g_rtk.reliability == 1 && time_since_last_rtk <= rtk_timeout); bool rtk_signal_good = (g_rtk.reliability != 0 && time_since_last_rtk <= rtk_timeout);
if (state_machine_->getCurrentState() == StateMachine::WAITING && if (state_machine_->getCurrentState() == StateMachine::WAITING &&
!state_machine_->obstacle_analysis.has_front_critical && !state_machine_->obstacle_analysis.has_front_critical &&

299
src/common/launch_system/launch/start_all.launch.py
View File

@ -1,6 +1,6 @@
from launch import LaunchDescription from launch import LaunchDescription
from launch_ros.actions import Node from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription, TimerAction from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory from ament_index_python.packages import get_package_share_directory
import os import os
@ -11,193 +11,116 @@ def generate_launch_description():
rslidar_sdk_path = get_package_share_directory("rslidar_sdk") rslidar_sdk_path = get_package_share_directory("rslidar_sdk")
fu_path = get_package_share_directory("fu") fu_path = get_package_share_directory("fu")
# ===================== 按顺序延时启动所有节点 =====================
# 1. 车辆参数节点 (立即启动)
vehicle_params_node = Node(
package="vehicle_params",
executable="vehicle_params_node",
name="vehicle_params_node",
output="screen",
respawn=True,
respawn_delay=2,
)
# 2. CAN遥控节点 (延时0.2s)
can_radio_ctrl_node = TimerAction(
period=0.2,
actions=[Node(
package="can_radio_ctrl",
executable="can_radio_ctrl_node",
name="can_radio_ctrl_node",
output="screen",
respawn=True,
respawn_delay=2,
)]
)
# Node(
# package="radio_ctrl",
# executable="radio_ctrl_node",
# name="radio_ctrl_node",
# output="screen",
# respawn=True,
# respawn_delay=2,
# ),
# 3. 远程控制节点 (延时0.4s)
remote_ctrl_node = TimerAction(
period=0.4,
actions=[Node(
package="remote_ctrl",
executable="remote_ctrl_node",
name="remote_ctrl_node",
output="screen",
respawn=True,
respawn_delay=2,
)]
)
# 4. MC电机控制节点 (延时0.6s)
mc_node = TimerAction(
period=0.6,
actions=[Node(
package="mc",
executable="mc_node",
name="mc_node",
output="screen",
respawn=True,
respawn_delay=2,
)]
)
# 5. 控制仲裁节点 (延时0.8s)
ctrl_arbiter_node = TimerAction(
period=0.8,
actions=[Node(
package="ctrl_arbiter",
executable="ctrl_arbiter_node",
name="ctrl_arbiter_node",
output="screen",
respawn=True,
respawn_delay=2,
)]
)
# 6. MQTT上报节点 (延时1.0s) —— 【关键：必须最后启动】
mqtt_report_node = TimerAction(
period=1.0,
actions=[Node(
package="mqtt_report",
executable="mqtt_report_node",
name="mqtt_report_node",
output="screen",
respawn=True,
respawn_delay=2,
)]
)
# 7. 雷达启动 (延时1.5s)
lidar_launch = TimerAction(
period=1.5,
actions=[IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(rslidar_sdk_path, "launch", "start_double.launch.py")
)
)]
)
# 8. RTK定位节点 (延时2.0s)
rtk_node = TimerAction(
period=2.0,
actions=[Node(
package="rtk",
executable="rtk_node",
name="rtk_node",
parameters=[
os.path.join(
get_package_share_directory("rtk"), "config", "rtk_params.yaml"
)
],
output="screen",
)]
)
# 9. 路径节点 (延时2.5s)
route_node = TimerAction(
period=2.5,
actions=[Node(
package="route",
executable="route_node",
name="route_node",
output="screen",
respawn=True,
respawn_delay=2,
)]
)
# 10. 订阅节点 (延时3.0s)
sub_node = TimerAction(
period=3.0,
actions=[Node(
package="sub",
executable="sub_node",
name="sub_node",
output="screen",
respawn=True,
respawn_delay=2,
)]
)
# 11. 任务管理节点 (延时3.5s)
task_manager_node = TimerAction(
period=3.5,
actions=[Node(
package="task_manager",
executable="task_manager_node",
name="task_manager_node",
output="screen",
respawn=True,
respawn_delay=2,
)]
)
# 12. 规划控制节点 (延时4.0s)
pl_node = TimerAction(
period=4.0,
actions=[Node(
package="pl",
executable="pl_node",
name="pl_node",
output="screen",
respawn=True,
respawn_delay=2,
)]
)
# 13. FU功能节点 (延时4.5s)
fu_launch = TimerAction(
period=4.5,
actions=[IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(fu_path, "launch", "fu.launch.py")
)
)]
)
# ===================== 组装启动列表 =====================
return LaunchDescription( return LaunchDescription(
[ [
vehicle_params_node, Node(
can_radio_ctrl_node, package="vehicle_params",
remote_ctrl_node, executable="vehicle_params_node",
mc_node, name="vehicle_params_node",
ctrl_arbiter_node, output="screen",
mqtt_report_node, respawn=True,
lidar_launch, respawn_delay=2,
rtk_node, ),
route_node, # Node(
sub_node, # package="radio_ctrl",
task_manager_node, # executable="radio_ctrl_node",
pl_node, # name="radio_ctrl_node",
fu_launch, # output="screen",
# respawn=True,
# respawn_delay=2,
# ),
Node(
package="can_radio_ctrl",
executable="can_radio_ctrl_node",
name="can_radio_ctrl_node",
output="screen",
respawn=True,
respawn_delay=2,
),
Node(
package="remote_ctrl",
executable="remote_ctrl_node",
name="remote_ctrl_node",
output="screen",
respawn=True,
respawn_delay=2,
),
Node(
package="mc",
executable="mc_node",
name="mc_node",
output="screen",
respawn=True,
respawn_delay=2,
),
Node(
package="ctrl_arbiter",
executable="ctrl_arbiter_node",
name="ctrl_arbiter_node",
output="screen",
respawn=True,
respawn_delay=2,
),
Node(
package="mqtt_report",
executable="mqtt_report_node",
name="mqtt_report_node",
output="screen",
respawn=True,
respawn_delay=2,
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(rslidar_sdk_path, "launch", "start_double.launch.py")
)
),
Node(
package="rtk",
executable="rtk_node",
name="rtk_node",
parameters=[
os.path.join(
get_package_share_directory("rtk"), "config", "rtk_params.yaml"
)
], # 从YAML文件加载参数
output="screen",
),
Node(
package="route",
executable="route_node",
name="route_node",
output="screen",
respawn=True,
respawn_delay=2,
),
Node(
package="sub",
executable="sub_node",
name="sub_node",
output="screen",
respawn=True,
respawn_delay=2,
),
Node(
package="task_manager",
executable="task_manager_node",
name="task_manager_node",
output="screen",
respawn=True,
respawn_delay=2,
),
Node(
package="pl",
executable="pl_node",
name="pl_node",
output="screen",
respawn=True,
respawn_delay=2,
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(fu_path, "launch", "fu.launch.py")
)
),
] ]
) )

25
src/common/logger/src/logger.cpp
View File

@ -231,7 +231,30 @@ void Logger::WriteLogThread()
} }
} }
std::cerr << formatted_msg; // 同时输出到控制台（带颜色，INFO级别无颜色）
std::string colored_msg;
switch (msg.level)
{
case LogLevel::DEBUG:
colored_msg = "\033[34m" + formatted_msg + "\033[0m"; // 蓝色
break;
case LogLevel::INFO:
colored_msg = formatted_msg; // INFO级别无颜色
break;
case LogLevel::WARN:
colored_msg = "\033[33m" + formatted_msg + "\033[0m"; // 黄色
break;
case LogLevel::ERROR:
colored_msg = "\033[31m" + formatted_msg + "\033[0m"; // 红色
break;
case LogLevel::FATAL:
colored_msg = "\033[35m" + formatted_msg + "\033[0m"; // 紫色
break;
default:
colored_msg = formatted_msg;
break;
}
std::cerr << colored_msg;
} }
} }