276 lines
8.6 KiB
Bash
Executable File
276 lines
8.6 KiB
Bash
Executable File
#!/bin/bash
|
|
#===============================================================================
|
|
# 加减速映射表标定启动脚本
|
|
# 位置: /home/nvidia/FaRui/v3.1/calibrate_accel_brake_map.sh
|
|
#===============================================================================
|
|
|
|
set -e # 遇到错误立即退出
|
|
|
|
# 颜色定义
|
|
RED='\033[0;31m'
|
|
GREEN='\033[0;32m'
|
|
YELLOW='\033[1;33m'
|
|
BLUE='\033[0;34m'
|
|
NC='\033[0m' # No Color
|
|
|
|
# 默认参数
|
|
VEHICLE_MODEL="ecar_vehicle"
|
|
USE_SIM_TIME="false"
|
|
RVIZ="true"
|
|
RECORD_BAG="false"
|
|
UPDATE_METHOD="update_offset_four_cell_around"
|
|
|
|
# 帮助信息
|
|
show_help() {
|
|
echo "============================================================"
|
|
echo " 加减速映射表标定工具"
|
|
echo "============================================================"
|
|
echo ""
|
|
echo "用法: ./calibrate_accel_brake_map.sh [选项]"
|
|
echo ""
|
|
echo "选项:"
|
|
echo " -m, --model <车型> 车辆模型名称 (默认: ecar404)"
|
|
echo " -s, --sim-time 使用仿真时间 (rosbag回放时使用)"
|
|
echo " -n, --no-rviz 不启动RViz"
|
|
echo " -r, --record 录制数据到rosbag"
|
|
echo " -c, --cell 使用逐网格标定算法 (默认: 四邻域)"
|
|
echo " -h, --help 显示此帮助信息"
|
|
echo ""
|
|
echo "标定模式:"
|
|
echo " 1. 实车标定: ./calibrate_accel_brake_map.sh"
|
|
echo " 2. rosbag标定: ./calibrate_accel_brake_map.sh -s"
|
|
echo " 3. 高精度标定: ./calibrate_accel_brake_map.sh -c"
|
|
echo ""
|
|
echo "============================================================"
|
|
}
|
|
|
|
# 解析参数
|
|
while [[ $# -gt 0 ]]; do
|
|
case $1 in
|
|
-m|--model)
|
|
VEHICLE_MODEL="$2"
|
|
shift 2
|
|
;;
|
|
-s|--sim-time)
|
|
USE_SIM_TIME="true"
|
|
shift
|
|
;;
|
|
-n|--no-rviz)
|
|
RVIZ="false"
|
|
shift
|
|
;;
|
|
-r|--record)
|
|
RECORD_BAG="true"
|
|
shift
|
|
;;
|
|
-c|--cell)
|
|
UPDATE_METHOD="update_offset_each_cell"
|
|
shift
|
|
;;
|
|
-h|--help)
|
|
show_help
|
|
exit 0
|
|
;;
|
|
*)
|
|
echo -e "${RED}错误: 未知参数 $1${NC}"
|
|
show_help
|
|
exit 1
|
|
;;
|
|
esac
|
|
done
|
|
|
|
# 获取脚本所在目录
|
|
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
|
cd "$SCRIPT_DIR"
|
|
|
|
echo "============================================================"
|
|
echo -e "${BLUE} 加减速映射表标定工具${NC}"
|
|
echo "============================================================"
|
|
echo ""
|
|
|
|
# 检查工作空间
|
|
if [ ! -f "install/setup.bash" ]; then
|
|
echo -e "${RED}错误: 未找到 install/setup.bash${NC}"
|
|
echo "请先编译工作空间: colcon build"
|
|
exit 1
|
|
fi
|
|
|
|
# Source 环境
|
|
echo -e "${BLUE}[1/6] 加载 ROS2 环境...${NC}"
|
|
source /opt/ros/humble/setup.bash
|
|
source install/setup.bash
|
|
|
|
# 检查 launch 文件是否存在
|
|
LAUNCH_FILE="src/Launcher/farui_launch/launch/calibration/accel_brake_map_calibration.launch.xml"
|
|
if [ ! -f "$LAUNCH_FILE" ]; then
|
|
echo -e "${RED}错误: 未找到标定 launch 文件${NC}"
|
|
echo "路径: $LAUNCH_FILE"
|
|
exit 1
|
|
fi
|
|
|
|
# 创建标定结果目录
|
|
CALIBRATION_DIR="$SCRIPT_DIR/src/Launcher/farui_launch/config/vehicle/calibrated_maps"
|
|
mkdir -p "$CALIBRATION_DIR"
|
|
|
|
echo -e "${GREEN}✓ 环境加载完成${NC}"
|
|
echo ""
|
|
|
|
# 显示配置信息
|
|
echo -e "${BLUE}[2/6] 标定配置信息:${NC}"
|
|
echo " 车辆模型: $VEHICLE_MODEL"
|
|
echo " 仿真时间: $USE_SIM_TIME"
|
|
echo " RViz可视化: $RVIZ"
|
|
echo " 录制数据: $RECORD_BAG"
|
|
echo " 标定算法: $UPDATE_METHOD"
|
|
echo " 结果保存路径: $CALIBRATION_DIR"
|
|
echo ""
|
|
|
|
# 检查车辆模型是否存在
|
|
VEHICLE_PKG="${VEHICLE_MODEL}_description"
|
|
if ! ros2 pkg list | grep "^$VEHICLE_PKG$" > /dev/null 2>&1; then
|
|
echo -e "${YELLOW}警告: 未找到车辆模型包 $VEHICLE_PKG${NC}"
|
|
echo "可用的车辆模型:"
|
|
ros2 pkg list | grep "_description$" | sed 's/_description$//' | head -10
|
|
echo ""
|
|
read -p "是否继续? (y/n) " -n 1 -r
|
|
echo
|
|
if [[ ! $REPLY =~ ^[Yy]$ ]]; then
|
|
exit 1
|
|
fi
|
|
fi
|
|
|
|
# 如果录制数据，创建bag目录
|
|
if [ "$RECORD_BAG" = "true" ]; then
|
|
BAG_DIR="$SCRIPT_DIR/calibration_bags"
|
|
mkdir -p "$BAG_DIR"
|
|
BAG_NAME="calibration_$(date +%Y%m%d_%H%M%S)"
|
|
fi
|
|
|
|
# 启动标定
|
|
echo -e "${BLUE}[3/6] 启动标定节点...${NC}"
|
|
echo ""
|
|
echo -e "${YELLOW}提示: 标定过程中请进行以下操作:${NC}"
|
|
echo " 1. 确保车辆在安全开阔场地"
|
|
echo " 2. 使用遥控器控制车辆"
|
|
echo " 3. 平稳加速、减速，覆盖各种速度"
|
|
echo " 4. 观察RViz中单元格变红表示数据充足"
|
|
echo " 5. 重点关注 0-6m/s 低速区间"
|
|
echo ""
|
|
read -p "按回车键开始标定，或按 Ctrl+C 取消..."
|
|
echo ""
|
|
|
|
# 构建 launch 参数
|
|
LAUNCH_ARGS="vehicle_model:=$VEHICLE_MODEL"
|
|
LAUNCH_ARGS="$LAUNCH_ARGS use_sim_time:=$USE_SIM_TIME"
|
|
LAUNCH_ARGS="$LAUNCH_ARGS rviz:=$RVIZ"
|
|
LAUNCH_ARGS="$LAUNCH_ARGS update_method:=$UPDATE_METHOD"
|
|
LAUNCH_ARGS="$LAUNCH_ARGS progress_file_output:=true"
|
|
LAUNCH_ARGS="$LAUNCH_ARGS pedal_accel_graph_output:=true"
|
|
|
|
# 启动 rosbag 录制（后台）
|
|
if [ "$RECORD_BAG" = "true" ]; then
|
|
echo -e "${BLUE}[4/6] 启动数据录制...${NC}"
|
|
ros2 bag record -o "$BAG_DIR/$BAG_NAME" \
|
|
/vehicle/status/velocity_status \
|
|
/vehicle/status/steering_status \
|
|
/vehicle/status/actuation_status \
|
|
/control/command/actuation_cmd \
|
|
/tf \
|
|
/tf_static \
|
|
&
|
|
BAG_PID=$!
|
|
echo -e "${GREEN}✓ 数据录制已启动，PID: $BAG_PID${NC}"
|
|
echo " 保存路径: $BAG_DIR/$BAG_NAME"
|
|
echo ""
|
|
fi
|
|
|
|
# 启动标定
|
|
echo -e "${BLUE}[5/6] 启动标定器...${NC}"
|
|
echo " 命令: ros2 launch farui_launch accel_brake_map_calibration.launch.xml $LAUNCH_ARGS"
|
|
echo ""
|
|
|
|
# 捕获 Ctrl+C 信号
|
|
cleanup() {
|
|
echo ""
|
|
echo -e "${YELLOW}接收到中断信号，正在清理...${NC}"
|
|
if [ "$RECORD_BAG" = "true" ] && [ -n "$BAG_PID" ]; then
|
|
echo "停止数据录制..."
|
|
kill $BAG_PID 2>/dev/null || true
|
|
wait $BAG_PID 2>/dev/null || true
|
|
fi
|
|
echo -e "${GREEN}✓ 已清理${NC}"
|
|
exit 0
|
|
}
|
|
trap cleanup SIGINT SIGTERM
|
|
|
|
# 启动标定节点
|
|
ros2 launch farui_launch accel_brake_map_calibration.launch.xml $LAUNCH_ARGS &
|
|
LAUNCH_PID=$!
|
|
|
|
# 等待标定节点启动
|
|
sleep 3
|
|
|
|
# 检查节点是否正常运行
|
|
if ! ps -p $LAUNCH_PID > /dev/null; then
|
|
echo -e "${RED}错误: 标定节点启动失败${NC}"
|
|
exit 1
|
|
fi
|
|
|
|
echo -e "${GREEN}✓ 标定节点已启动${NC}"
|
|
echo ""
|
|
|
|
# 显示操作提示
|
|
echo "============================================================"
|
|
echo -e "${BLUE} 标定操作指南${NC}"
|
|
echo "============================================================"
|
|
echo ""
|
|
echo "1. ${YELLOW}驾驶车辆${NC}: 使用遥控器控制车辆"
|
|
echo " - 平稳加速、减速"
|
|
echo " - 覆盖 0-最大车速 范围"
|
|
echo " - 覆盖 0-最大踏板开度 范围"
|
|
echo ""
|
|
echo "2. ${YELLOW}观察进度${NC}: 查看RViz可视化"
|
|
echo " - 灰色: 无数据"
|
|
echo " - 蓝色→红色: 数据逐渐充足"
|
|
echo " - 目标: 所有单元格变红"
|
|
echo ""
|
|
echo "3. ${YELLOW}保存结果${NC}: 新开终端执行"
|
|
echo " ros2 service call /accel_brake_map_calibrator/update_map_dir \\"
|
|
echo " tier4_vehicle_msgs/srv/UpdateAccelBrakeMap \\"
|
|
echo " \"path: '$CALIBRATION_DIR'\""
|
|
echo ""
|
|
echo "4. ${YELLOW}查看误差${NC}:"
|
|
echo " ros2 topic echo /accel_brake_map_calibrator/output/map_error_ratio"
|
|
echo " (比值 < 0.7 表示标定有效)"
|
|
echo ""
|
|
echo "5. ${YELLOW}结束标定${NC}: 在此窗口按 Ctrl+C"
|
|
echo ""
|
|
echo "============================================================"
|
|
echo ""
|
|
|
|
# 等待用户结束
|
|
wait $LAUNCH_PID
|
|
|
|
# 标定结束后的提示
|
|
echo ""
|
|
echo "============================================================"
|
|
echo -e "${BLUE} 标定已结束${NC}"
|
|
echo "============================================================"
|
|
echo ""
|
|
echo -e "${GREEN}标定结果保存在:${NC}"
|
|
echo " $CALIBRATION_DIR/"
|
|
echo ""
|
|
echo "生成的文件:"
|
|
ls -lh "$CALIBRATION_DIR/" 2>/dev/null || echo " (目录为空，请检查是否已调用保存服务)"
|
|
echo ""
|
|
echo "下一步:"
|
|
echo " 1. 检查 accel_map.csv 和 brake_map.csv 是否生成"
|
|
echo " 2. 修改 raw_vehicle_cmd_converter 配置使用新表"
|
|
echo " 3. 重启 Autoware 验证效果"
|
|
echo ""
|
|
echo "============================================================"
|
|
|
|
|
|
# ros2 service call /accel_brake_map_calibrator/input/update_map_dir tier4_vehicle_msgs/srv/UpdateAccelBrakeMap "path: '/home/nvidia/FaRui-test/v3.2/src/Launcher/farui_launch/config/vehicle/calibrated_maps'"
|
|
|