Rebound: to spring back on or as if on collision or impact with another body

  ┌────────────────┐                                        ▬
  │ reboundReplan  │    .─────────.                     ▬▬▬▬▬
▬ └────────────────┘ ,─'███████████'─.                ▬▬▬
  ▬▬                ╱█████████████████╲             ▬▬
     ▬ ▬ ▬         ╱███████████████████╲        ▬▬▬▬
           ▬  ▬  ▬▬█▬▬██████████████████:    ▬ ▬▬
            ●●    :███▬▬█▬█▬██▬▬█▬▬▬▬█▬▬▬▬▬▬▬    ●
             ●     :███████████████████;        ●●
              ●●   :███████████████████;       ●●
               ●●   `.███████████████,'       ●●
                ●●    '─.█████████,─'       ●●●
                 ●●●     `───────'      ●●●●●
                   ●●●●●●●●      ●●●●●●●●
                           ●●●●●●

EGO-Planner 算法模块

  • ego_planner 调度算法
  • path_searching 前端路径搜索
  • bspline_opt 后端轨迹优化
  • traj_utils 前端轨迹表示
  • plan_env 后端Grid地图

EGO-Swarm 扩展模块

  • drone_detect 深度图目标检测
  • rosmsg_tcp_bridge 分布式通信
┌───────────────────────────────────┐
│                         ┌─────────┴────────┐
│                         │ BaseGlobalPlanner│
│                         │ vs path_searching│
│                         └─────────┬────────┘
│                                   │
│                                   │
│        move_base        ┌─────────┴────────┐      ┌──────────┐
│        vs               │ BaseLocalPlanner │      │ plan_env │
│        ego_planner      │ vs  bspline_opt  │◀─────│          │
│                         └─────────┬────────┘      └──────────┘
│                         ┌ ─ ─ ─ ─ ┴ ─ ─ ─ ─
│                           RecoveryBehavior │
│                         └ ─ ─ ─ ─ ┬ ─ ─ ─ ─
└───────────────────────────────────┘

编译设置

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")

编程规范

  • ROS 包模块化
  • typedef unique_ptr Ptr;
  • typedef std::shared_ptr Ptr;

ROS 用法

  • roslaunch XML
  • message_filters
  • Use TF or Topic?

初始化

  • EGOReplanFSM::init
    • EGOPlannerManager::initPlanModules
      • GridMap::initMap
      • BsplineOptimizer::setParam
      • BsplineOptimizer::setEnvironment
      • AStar::initGridMap
    • EGOPlannerManager::deliverTrajToOptimizer
    • EGOPlannerManager::setDroneIdtoOpt
    • createTimer(100Hz) -> execFSMCallback
      • planFromGlobalTraj
        • callReboundReplan
          • EGOPlannerManager::reboundReplan
            • STEP 1: INIT
              • PolynomialTraj::minSnapTraj
              • BsplineOptimizer::initControlPoints
                • 1 Segment the initial trajectory according to obstacles.
                • 2 A star search.
                • 3 Calculate bounds.
                • 4 Adjust segment length.
                • 5 Assign data to each segment.
            • STEP 2: OPTIMIZE
              • BsplineOptimizer::distinctiveTrajs (Topological Planning)
                • 1 Find the opposite vectors and base points for every segment.
                  • 1.1 Find the start occupied point id and the last occupied point id.
                  • 1.2 Reverse the vector and find new base points from occ_start_id to occ_end_id.
                  • 1.3 Assign the base points to control points within [0, occ_start_id) and (occ_end_id, RichInfoSegs[i].first.size()-1].
                • 2 Assemble each segment to make up the new control point sequence.
                  • 2.1 Calculate the selection table.
                  • 2.2 Assign params according to the selection table.
              • BsplineOptimizer::BsplineOptimizeTrajRebound
                • BsplineOptimizer::rebound_optimize
                  • BsplineOptimizer::costFunctionRebound
                    • BsplineOptimizer::combineCostRebound
                      • BsplineOptimizer::calcSmoothnessCost
                      • BsplineOptimizer::calcDistanceCostRebound
                        • BsplineOptimizer::check_collision_and_rebound
                      • BsplineOptimizer::calcFeasibilityCost
                      • BsplineOptimizer::calcSwarmCost
                      • BsplineOptimizer::calcTerminalCost
            • STEP 3: REFINE(RE-ALLOCATE TIME) IF NECESSARY
              • EGOPlannerManager::refineTrajAlgo
                • BsplineOptimizer::BsplineOptimizeTrajRefine
      • planFromCurrentTraj
        • callReboundReplan
          • EGOPlannerManager::reboundReplan
            • …
      • planNextWaypoint
        • EGOPlannerManager::planGlobalTraj
          • PolynomialTraj::minSnapTraj/PolynomialTraj::one_segment_traj_gen
    • createTimer(20Hz) -> checkCollisionCallback
      • planFromCurrentTraj
        • callReboundReplan
          • EGOPlannerManager::reboundReplan
            • …

状态机

                  ┌──────────────────────────────────────────────────┐
                  │                                                  │
                  ▼                   ┌───────────┐                  │
┌─────┐    ┌────────────┐             │           │                  │
│INIT │───▶│WAIT_TARGET │─┐           ▼           │                  │
└─────┘    └────────────┘ │  ┌─────────────────┐  │   ┌───────────┐  │
                          └─▶│SEQUENTIAL_START │──┴──▶│ EXEC_TRAJ │──┤
                             └─────────────────┘      └───────────┘  │
                                                            ▲        │
                                                            │        │
┌──────────────┐             ┌──────────────┐               │        │
│EMERGENCY_STOP│────────────▶│ GEN_NEW_TRAJ │──┬────────────┤        │
└──────────────┘             └──────────────┘  │            │        │
                                     ▲         │            │        │
                                     │         │            │        │
                                     └─────────┘            │        │
                                                            │        │
                                     ┌─────────┐            │        │
                                     │         │            │        │
                                     ▼         │            │        │
                             ┌──────────────┐  │            │        │
                             │ REPLAN_TRAJ  │──┴────────────┘        │
                             └──────────────┘                        │
                                     ▲                               │
                                     │                               │
                                     └───────────────────────────────┘

局部地图算法

  • GridMap::initMap
    • 视觉方案 w/ message_filters 同步
      • sensor_msgs::Image + geometry_msgs::PoseStamped
      • sensor_msgs::Image + nav_msgs::Odometry
    • 激光方案
      • sensor_msgs::PointCloud2 + nav_msgs::Odometry
    • createTimer(20Hz) -> updateOccupancyCallback
      • projectDepthImage
      • raycastProcess
      • clearAndInflateLocalMap
    • createTimer(20Hz) -> visCallback

深度相机