sforce auto

sforce auto的思维导图，内容有ConfigManager::init、ChassisManager::init、DecisionManager:init、CommManager::init、Sig Handle.

Adas Routing

• init
  • init refline, display_line, line_vec, state
  • Point A
  • Point B
  • work width
  • inter resolution
• get_path
  • if init err, return empty path;
  • straight state
    • if car is reversing, update line_vec
    • find nearest refline by pose when moving refline or not lock line
      • the new refline generated, let start point be the foot point of refline
    • let start point be refline's foot point of pose
    • clear old trajectory path and generate new a new one
  • turn state
    • consider car moved or not during system is paused
      • paused point is valid
        • calculate dist between cur pose and paused point
        • if dist > 0.8m, car moved and turn state to straight state
          • generate new straight trajectory path once state changed
        • dist < 0.8 and lock line, clear paused point
      • paused point not valid
        • update pause point when not lock line
    • find nearest point in trajectory path of cur pose
      • near_point function should verify state first
    • if nearest point's idx * inter_res > 2m, set idx to it's half
    • check turn finished
      • change state to straight`
      • clear target_line
      • generate new trajectory path
    • fill walked turn dist's striaght line into trajectory path, keep the trajectory path's length be 10m
• set_work_width
  • check wdith > 0.1, return 0, else return -1
• move_by_dist
  • dist is the dist of original refline
• make_turn
  • check line_vec is valid
  • update turn point
  • set need_warn flag false
  • check velocity > 0
  • deal with continous turn
  • clear straigth trajectory
  • calculate turn end point, generate turn
• need_warn
  • return false when need_warn flag is false
  • get turn_point by pose.yaw
  • calculate theta of(foot_cur, foot_target, foot_turn)
    • foot_target = foot_turn - line_vec*dist
• get_display_line
• 转弯状态暂停之后接着作业
  • 判断能否接着弧线作业
    • 能
      • 发剩余的弧线点
    • 不能
      • 清空弧线点
      • 上转弯之后的目标线

边界检测

• 初始化的参数
  • 车辆尺寸
  • 机具尺寸
  • 田块点UTM坐标
• 依赖项
  • 实时位姿
    • UTM坐标
    • 航向角
• 处理流程
  • 根据参数计算车子+机具所在的矩形框顶点
  • 根据位姿信息将所有的顶点旋转+平移到位姿处
  • 使用射线法判判断每一个顶点在是否在田块内部
    • 在内部返回0
    • 不在内部
      • 计算点到田块每一条边（线段）的最小距离，并将之储存到数组中
      • 返回数组的最大值

Sforce Auto

• ConfigManager::init
  • ParamDb::get_msg(planning)
  • setPlanningConfig
  • printfPlanningConfig
• ChassisManager::init
  • ChassisFactory::registerChassisFactory
  • ChassisFactory::createChassis(vehicleProtocal)
  • ChassisFactory::createChassisCore
  • ChassisCore::init
    • baseInit
      • controlInit
        • ControlLib::init
        • controlProcessThread
      • memberInit
        • None
      • cddsInit
        • None
      • nodeInit
        • m_nodePtr is a std::unordered_map<std::string, cdds::Node::SharedPtr> all node created in this function would add in m_nodePtr map
        • add Chassis Sub Node
        • add Chassis Pub Node
        • add Chassis Timer
        • add Chassis Server
        • add Chassis Client
      • publisherInit
        • control_basic
          • Motion->Planning
          • FLAGS_control_basic_topic
          • sforceauto::control::Control
        • chassis_basic
          • Chasiss->Planning
          • FLAGS_chassis_basic_topic
          • sforceauto::chassis::Chassis
        • mcu update
          • MCU->Planning
          • Flags_chassis_mcu_update_topic
          • sforceauto::acu::Message
        • control_basic_test
          • control_basic_test_topic
          • sforceauto::control::Control
        • chassis_angle_zero_topic
          • chassis_angle_zero_topic
          • sforceauto::acu::Message
      • subscriptionInit
        • 1.: source module->target module 2. topic name 3. topic message 4. topic callback function
        • planning_chassis_control
          • Planning->Chassis Control
          • FLAGS_planning_chassis_control_topic
          • sforceauto::planning::ChassisControl
          • suberChassisControlCallback
        • planning_local_control
          • Planning->Localization
          • FLAGS_planning_control_localization_topic
          • sforceauto::localization::Localization
          • suberLocalizationCallback
        • monitor_stop
          • Monitor->Chassis
          • FLAGS_monitor_basic_topic
          • sforceauto::monitor::Monitor
          • suberMonitorCallback
        • planning_trajectory
          • Planning->ChassisControl
          • FLAGS_planning_trajectory_topic
          • sforceauto::planning::PlanningTrajectory
          • suberPlanningTrajectoryCallback
        • control_panel
          • Comm->Chasiss
          • FLAGS_control_board_display_topic
          • sforceauto::acu::Message
          • suberControlBoardInfoCallback
        • planning_chaiss
          • Planning->Chassis
          • FLAGS_planning_basic_topic
          • sforceauto::planning::Planning
          • suberplanningInfoCallback
      • serverInit
        • FLAGS_planning_chassis_config_topic
          • sforceauto::acu::Message <-> sforceauto::acu::Message
          • serverChassisConfigCallback
        • FLAGS_planning_control_config_topic
          • sforceauto::acu::Message <-> sforceauto::acu::Message
          • serverControlConfigCallback
      • clientInit
        • None
      • timerInit
        • ControlInfoTimer
          • publishControlMessage()
    • chassisControllerInit
      • new SforceChassisController
      • ChassisStatusEvent bind SforceChassisCore::chassisStatusPollEvent
      • ControlBoardStatusEvent bind SforceChassisCore::controlBoardStatusPollEvent
      • steerMotorStatusEvent bind SforceChassisCore::steerMotorStatusPollEvent
      • steerMotorControlFeedbackStatusEvent bind SforceChassisCore::steerMotorControlFeedbackStatusPollEvent
      • steerMotorQueryFeedbackStatusEvent bind SforceChassisCore::steerMotorQueryFeedbackStatusPollEvent
      • mcuUpdateStatusEvent bind mcuUpdateStatusEventPollEvent
    • steerContorlInit
      • steerControlPthread
    • dfisekiMemberInit
      • m_steerMotorQueryFeedbackFlag memory_order_relaxed
    • dfisekiThreadInit
      • None
    • dfisekiNodeInit
      • None
    • dfisekiPublisherInit
      • FLAGS_steer_control_basic_topic
        • sforceauto::steer_control::SteerControl
      • FLAGS_none_wheel_angle_basic_topic
        • sforceauto::none_wheel_angle::NoneWheelAngle
      • FLAGS_control_board_command_topic
        • sforceauto::acu::Message
    • dfisekiSubscriptionInit
      • imu_basic_topic
        • sforceauto::sensor::ahrs::Ahrs
    • dfisekiServerInit
      • FLAGS_planning_steer_control_config_topic
        • sforceauto::acu::Message <-> sforceauto::acu::Message
    • dfisekiClientInit
      • None
    • dfisekiTimerInit
      • publishSteerControlMessage
      • publishChassisMessage
      • publishNoneWheelAngleMessage
• DecisionManager::init
• CommManager::init
  • memberInit
    • get dev sn from txt
    • set planning_config dev_sn
  • protoAdapterInit
    • ProtoAdapter::init
      • protoAdapterThread
        • Navigation Sensitive Config
          • assemble respond msg
          • farward responde msg by protoAdapterCallback
        • Configure Vehicle Param
        • Request Vehicle Param
          • assemble param msg
            • transplanter
              • localization reciver offset xyz
              • wheelbase
              • min turn_radius
              • max speed
              • steering_angle_ratio
              • front_edge_to_front_wheel
              • safe_width
              • safe_height
            • tractor
              • localization reciver offset xyz
              • wheelbase
              • min turn_radius
              • max speed
              • max_engine_speed
              • steering_angle_ratio
              • front_wheel_base
              • front_edge_to_front_wheel
              • safe_width
              • safe_height
              • back_wheel_hang
              • back_wheel_trac
          • farward param msg
      • protoCommonThread
        • assemble log msg
      • init protoAdapterCallback
        • WiFi
          • config navigation sensitive
          • config vehicle param
          • request vehicle param
          • command work detail info switch
          • command chassis info switch
        • Cloud
  • nodeInit
    • CommSubscription
    • CommPublisher
    • CommTimer
    • CommServer
    • CommClient
  • subInit
    • FLAGS_control_board_command_topic
      • chassis->comm
      • sforceauto::acu::Message
      • CommManager::subControlBoardCommandCallback
        • set hmi OFFLINE_CONTROL_BOARD
        • farward msg
    • FLAGS_planning_hmi_topic
      • decision->comm
      • sforceauto::acu::Message
      • subPlanningHmiCallback
        • SOCKET_COMM_MODE_PROTO
          • App Mode
          • socket wifi
          • socket ethernet
        • SOCKET_COMM_MODE_CLOUD
          • upload alarm msg(cloud, wifi, ethernet)
          • upload error msg(cloud, wifi, ethernet)
          • upload work_info/chassis_info (wifi, ethernet)
          • upload work(wifi,ethernet,cloud)
          • upload msg by hmi mode
            • ethernet
            • wifi
            • cloud
    • FLAGS_planning_work_task_req_topic
      • decision->comm
      • sforceauto::acu::Message
      • CommManager::subPlanningWorkTaskReqCallback
        • ProtoAdapter::handlePlanningWorkTaskReqMessage
          • 1. cloud sends all task to communication node, communication node stored it, and send frist one to planning node; 2. planning node request the next task if existed after finish current one
          • handle header
          • WorkTask.type
            • 1 manual task
            • 2 multi-task
              • repeated single-task
                • type 1: road task 2: field task
                • field_id: field task specified
                • points: road task specified
              • parse multitask from bytes
              • assemble task_package
                • property
                  • type 1:point 2:param
                  • total task count
                  • cur task id
                • param
                  • 1. road_task + field task: - specify road_task's path points - specify field task's field id + ABCD paramters 2. field task: - specify sub_task_type(SUB_TASK_FIELD) - specify task_package_property.package_type PACKAGE_PATH_PARAM - specify task_package_param.routing_param.rout_type ROUTING_TYPE_ABCD - get field point info by field's id(LL->UTM) - set task_package_param.farmland_param
        • foreach msg in msgs publish
  • pubInit
    • FLAGS_comm_basic_topic
      • sforceauto::comm::Comm
    • FLAGS_control_board_display_topic
      • sforceauto::acu::Message
    • FLAGS_comm_hmi_topic
      • sforceauto::acu::Message
    • FLAGS_comm_gsm_config_topic
      • sforceauto::acu::Message
    • comm_task_work_topic
      • sforceauto::acu::Message
  • timerInit
    • publishCommInfo()
  • serverInit
    • serverCommConfigCallback
  • clientInit
    • None
  • threadInit
    • None
  • mqttSdkInit
    • CommManager::mqttSdkInitCallback
      • CommManager::mqttSdkToCddsCallback
        • ProtoAdapter::handleDownMessage mqtt override
          • Parse Mqtt buff into iovproto::common::Message
  • socketInit
    • CommManager::ethernetToCddsCallback
      • SOCKET_COMM_MODE_PROTO
        • App Mode
        • farward sforceauto::acu::Message to planning_node
      • SOCKET_COMM_MODE_CLOUD
        • ProtoAdapter::handleDownMessage mqtt override
        • farward msg to Planning
          • set hmi_mode OFFLINE_RJ45_APP
          • CommandWorkTaskTopic
            • UploadWorkTaskTopic
          • CommandTaskPointTopicTopic
            • UploadTaskPointTopic
          • CommandTaskArgumentsTopic
            • UploadTaskArgumentsTopic
  • wifiInit
    • modify wifi bind ip
    • CommManager::wifiToCddsCallback
      • SOCKET_COMM_MODE_PROTO
        • farward sforceauto::acu::Message to planning_node
      • SOCKET_COMM_MODE_CLOUD
        • farward msg to Planning
          • set hmi_mode OFFLINE_RJ45_APP
          • CommandWorkTaskTopic
            • UploadWorkTaskTopic
          • CommandTaskPointTopicTopic
            • UploadTaskPointTopic
          • CommandTaskArgumentsTopic
            • UploadTaskArgumentsTopic
  • GsmInit
    • GsmManager::init
      • CommManager::gsmInfoCallback
      • CommManager::gsmConfigInfoCallback
• Sig Handle
  • register unexpected abort signal
    • SIGSEGV
    • SIGABRT
  • printf stack info
• ProtoAdapter::handleDownMessage Msg override
  • CommandWorkTaskTopic
  • CommandWorkStatusTopic
  • CommandTaskArgumentsTopic
  • CommandTaskPointTopicTopic
  • CommandLeftRightTurnTopic
  • CommandWorkWidthTopic
  • CommandFirmwareUpgradeTopic
  • CommandWakeSwitchTopic
  • CommandCtrlSwitchTopic
  • CommandLogSwitchTopic
  • CommandBaselineOffsetTopic
  • ConfigFarmImpleOffsetTopic
  • ConfigGnssDiffParamTopic
  • ConfigGnssParamTopic
  • ConfigGsmSimParamTopic
  • ConfigNavSenParamTopic
  • ConfigSteerAngleParamTopic
  • ConfigSystemParamTopic
  • ConfigVehicleParamTopic
  • RequestChassisParamTopic
  • RequestFarmImpleOffsetTopic
  • RequestGnssDiffParamTopic
  • RequestGnssParamTopic
  • RequestGsmSimInfoTopic
  • RequestNavSenParamTopic
  • RequestSteerAngleParamTopic
  • RequestSystemParamTopic
  • RequestVehicleParamTopic
  • RequestSacuVersionTopic
  • CommandWorkDetailInfoSwitchTopic
  • CommandChassisInfoSwitchTopic
  • CalibrationGnssYawOffsetTopic
  • CalibrationSteerAngleSensorTopic
  • CalibrationChassisParamTopic
  • CommandFunctionTestTopic
  • CommandFieldsDataTopic
  • ConfigAlarmParamTopic
  • RequestAlarmParamTopic

Parameters

• 前端
  • json editor
    • https://github.com/json-editor/json-editor
  • json schema
• 后端
  • cinatra
    • https://github.com/qicosmos/cinatra
• 数据库
  • sqlite3
    • https://runebook.dev/zh/docs/sqlite/json1
• 响应js的请求
  • jQuery
• js获取response并展示

obstacle avoidance

• worklines
• obstacle polygon
• 寻找每个障碍物影响的作业行
  • 构建map<Polygon, vector<Line>>
• 处理每一条被影响的作业行
  • 重构障碍物为矩形
    • 矩形的两条边（0，2）平行于作业行，边0距离工作行max_dist_left， 边2距离工作行max_dist_right 矩形的另外两条边（1，3）垂直于作业行，找出多边形障碍物的每个顶点在工作行上的垂足，存入数组foot_ps中，数组按照点的x坐标排序，边1垂直工作行且经过foot_ps.front，边3垂直于工作行切经过foot_ps.back 由此得到重构后的障碍物矩形的四个点
  • 膨胀矩形
    • 按照max{imple_width, vehicle_width}*0.5 对矩形进行膨胀
  • 判断2r与max_dist_left/right
    • 将作业行变为一个Turn，Turn中的obs包含 绕障前的作业行line1 绕障arc1, 绕障line2（max_dist > 2r时存在） 绕障arc2 绕障line3 绕障arc3 绕障arc4 绕障后的作业行line4 静态障碍物不能太大。。。 障碍物占据地头的情况： 完全在地头： 通过将转弯路径向地块中心靠拢的方式解决 占据了line1或line4，田块中间 占据了line1或line4， 接近左右田埂 需要枚举的情况太多。可以使用全局栅格化地图的方式，
    • 2r>max_dist
    • 2r<max_dist
      • 将作业行变为一个Turn,
• 主题