Autonomous driving via motion primitives-based planner.
After building this ROS package using 'catkin_make' in your catkin workspace, launch the motion primitives-based planner node:
roslaunch motion_primitives_planner run.launch
-
Utility functions
a. Collision Checking
motion_primitives_planner/src/motion_primitives_planner_node.cpp
Lines 243 to 266 in 831f817
bool MotionPlanner::CheckCollision(Node currentNodeMap, nav_msgs::OccupancyGrid localMap) { /* TODO: check collision of the current node - the position x of the node should be in a range of [0, map width] - the position y of the node should be in a range of [0, map height] - check all map values within the inflation area of the current node e.g., for loop i in range(0, inflation_size) for loop j in range(0, inflation_size) tmp_x := currentNodeMap.x + i - 0.5*inflation_size <- you need to check whether this tmp_x is in [0, map width] tmp_y := currentNodeMap.y + j - 0.5*inflation_size <- you need to check whether this tmp_x is in [0, map height] map_index := "index of the grid at the position (tmp_x, tmp_y)" <-- map_index should be int, not double! map_value = static_cast<int16_t>(localMap.data[map_index]) if (map_value > map_value_threshold) OR (map_value < 0) return true return false - use params in header file: INFLATION_SIZE, OCCUPANCY_THRES */ return false; } b. Local to Map Coordinate
motion_primitives_planner/src/motion_primitives_planner_node.cpp
Lines 279 to 296 in 831f817
Node MotionPlanner::LocalToMapCorrdinate(Node nodeLocal) { /* TODO: Transform from local to occupancy grid map coordinate - local coordinate ([m]): x [map min x, map max x], y [map min y, map max y] - map coordinate ([cell]): x [0, map width], y [map height] - convert [m] to [cell] using map resolution ([m]/[cell]) */ // Copy data nodeLocal to nodeMap Node nodeMap; memcpy(&nodeMap, &nodeLocal, sizeof(struct Node)); // Transform from local (min x, max x) [m] to map (0, map width) [grid] coordinate nodeMap.x = 0; // Transform from local (min y, max y) [m] to map (0, map height) [grid] coordinate nodeMap.y = 0; return nodeMap; } -
Main algorithms
a. Generate motion primitives
motion_primitives_planner/src/motion_primitives_planner_node.cpp
Lines 107 to 138 in 831f817
std::vector<std::vector<Node>> MotionPlanner::GenerateMotionPrimitives(nav_msgs::OccupancyGrid localMap) { /* TODO: Generate motion primitives - you can change the below process if you need. - you can calculate cost of each motion if you need. */ // initialize motion primitives std::vector<std::vector<Node>> motionPrimitives; // number of candidates int num_candidates = this->MAX_DELTA*2 / this->DELTA_RESOL; // *2 for considering both left/right direction // max progress of each motion double maxProgress = this->MAX_PROGRESS; for (int i=0; i<num_candidates+1; i++) { // current steering delta double angle_delta = this->MAX_DELTA - i * this->DELTA_RESOL; // init start node Node startNode(0, 0, 0, angle_delta, 0, 0, 0, -1, false); // rollout to generate motion std::vector<Node> motionPrimitive = RolloutMotion(startNode, maxProgress, localMap); // add current motionPrimitive motionPrimitives.push_back(motionPrimitive); } return motionPrimitives; } b. Compute rollout of a motion primitive
motion_primitives_planner/src/motion_primitives_planner_node.cpp
Lines 140 to 204 in 831f817
std::vector<Node> MotionPlanner::RolloutMotion(Node startNode, double maxProgress, nav_msgs::OccupancyGrid localMap) { /* TODO: rollout to generate a motion primitive based on the current steering angle - calculate cost terms here if you need - check collision / sensor range if you need 1. Update motion node using current steering angle delta based on the vehicle kinematics equation. 2. collision checking 3. range checking */ // Initialize motionPrimitive std::vector<Node> motionPrimitive; // Init current motion node Node currMotionNode(startNode.x, startNode.y, 0, startNode.delta, 0, 0, 0, -1, false); // Start from small progress value double progress = this->DIST_RESOL; // 1. Update motion node using current steering angle delta based on the vehicle kinematics equation // - while loop until maximum progress of a motion while (progress < maxProgress) { // - you can use params in header file (MOTION_VEL, WHEELBASE, TIME_RESOL) // - steering angle value is 'startNode.delta' or 'currMotionNode.delta' // x_t+1 := x_t + x_dot * dt // y_t+1 := y_t + y_dot * dt // yaw_t+1 := yaw_t + yaw_dot * dt currMotionNode.x += 0.0; currMotionNode.y += 0.0; currMotionNode.yaw += 0.0; // 2. collision checking // - local to map coordinate transform Node collisionPointNode(currMotionNode.x, currMotionNode.y, currMotionNode.yaw, currMotionNode.delta, 0, 0, 0, -1, false); Node collisionPointNodeMap = LocalToMapCorrdinate(collisionPointNode); if (CheckCollision(collisionPointNodeMap, localMap)) { // - do some process when collision occurs. // - you can save collision information & calculate collision cost here. // - you can break and return current motion primitive or keep generate rollout. } // 3. range checking // - if you want to filter out motion points out of the sensor range, calculate the line-of-sight (LOS) distance & yaw angle of the node // - LOS distance := sqrt(x^2 + y^2) // - LOS yaw := atan2(y, x) // - if LOS distance > MAX_SENSOR_RANGE or abs(LOS_yaw) > FOV*0.5 <-- outside of sensor range // - if LOS distance <= MAX_SENSOR_RANGE and abs(LOS_yaw) <= FOV*0.5 <-- inside of sensor range // - use params in header file (MAX_SENSOR_RANGE, FOV) double LOS_DIST = 0.0; double LOS_YAW = 0.0; if (LOS_DIST > this->MAX_SENSOR_RANGE || abs(LOS_YAW) > this->FOV*0.5) { // -- do some process when out-of-range occurs. // -- you can break and return current motion primitive or keep generate rollout. } // append collision-free motion in the current motionPrimitive motionPrimitive.push_back(currMotionNode); // update progress of motion progress += this->DIST_RESOL; } // return current motion return motionPrimitive; } c. Calculate the cost of motion primitives and select cost-minimum motion
motion_primitives_planner/src/motion_primitives_planner_node.cpp
Lines 206 to 238 in 831f817
std::vector<Node> MotionPlanner::SelectMotion(std::vector<std::vector<Node>> motionPrimitives) { /* TODO: select the minimum cost motion primitive 1. Calculate cost terms 2. Calculate total cost (weighted sum of all cost terms) 3. Compare & Find minimum cost (double minCost) & minimum cost motion (std::vector<Node> motionMinCost) 4. Return minimum cost motion */ // Initialization double minCost = 9999999; std::vector<Node> motionMinCost; // initialize as odom // check size of motion primitives if (motionPrimitives.size() != 0) { // Iterate all motion primitive (motionPrimitive) in motionPrimitives for (auto& motionPrimitive : motionPrimitives) { // 1. Calculate cost terms // 2. Calculate total cost double cost_total = 0.0; // 3. Compare & Find minimum cost & minimum cost motion if (cost_total < minCost) { motionMinCost = motionPrimitive; minCost = cost_total; } } } // 4. Return minimum cost motion return motionMinCost; } d. Publish control commands
motion_primitives_planner/src/motion_primitives_planner_node.cpp
Lines 77 to 91 in 831f817
void MotionPlanner::PublishCommand(std::vector<Node> motionMinCost) { /* TODO: Publish control commands - Two components in the AckermannDrive message is used: steering_angle and speed. - Compute steering angle using your controller or other method. - Calculate proper speed command - Publish data */ ackermann_msgs::AckermannDrive command; command.steering_angle = 0.0; command.speed = 0.0; pubCommand.publish(command); std::cout << "command steer/speed : " << command.steering_angle*180/M_PI << " " << command.speed << std::endl; }
-
Use the class "Node" to manage the position, cost, and collision status of nodes in motion primitives
motion_primitives_planner/include/motion_primitives_planner/motion_primitives_planner_node.hpp
Lines 58 to 96 in 831f817
class Node { public: // The default constructor for 3D array initialization Node(): Node(0, 0, 0, 0, 0, 0, 0, -1, false) {} // Constructor for a node with the given arguments Node(double x, double y, double yaw, double delta, double cost_dir, double cost_colli, double cost_total, int idx, bool collision) { this->x = x; this->y = y; this->yaw = yaw; this->delta = delta; this->cost_dir = cost_dir; this->cost_colli = cost_colli; this->cost_total = cost_total; this->idx = idx; this->collision = collision; } // the x position double x; // the y position double y; // the heading yaw double yaw; // the steering angle delta double delta; // the direction cost double cost_dir; // the collision cost double cost_colli; // the total cost double cost_total; // the index on path int idx; // flag for collision bool collision; }; -
Change (tune) parameters if you need
motion_primitives_planner/include/motion_primitives_planner/motion_primitives_planner_node.hpp
Lines 123 to 146 in 831f817
// Parameters double FOV = 85.2 * (M_PI / 180.0); // [rad] FOV of point cloud (realsense) double MAX_SENSOR_RANGE = 10.0; // [m] maximum sensor range (realsense) double WHEELBASE = 0.27; // [m] wheelbase of the vehicle (our RC vehicle is near 0.26 m) // TODO: Change (tune) below parameters if you need double DIST_RESOL = 0.2; // [m] distance resolution for rollout double TIME_RESOL = 0.05; // [sec] time resolution between each motion (for rollout) double MOTION_VEL = DIST_RESOL / TIME_RESOL; // [m/s] velocity between each motion (for rollout) double DELTA_RESOL = 0.5 * (M_PI / 180.0); // [rad] angle resolution for steering angle sampling double MAX_DELTA = 10.0 * (M_PI / 180.0); // [rad] maximum angle for steering angle sampling double MAX_PROGRESS = 5.0; // [m] max progress of motion double INFLATION_SIZE = 0.5 / DIST_RESOL; // [grid] inflation size [m] / grid_res [m/grid] // Map info // TODO: Match below parameters with your cost map double mapMinX = -5; // [m] minimum x position of the map double mapMaxX = 15; // [m] maximum x position of the map double mapMinY = -10; // [m] minimum y position of the map double mapMaxY = 10; // [m] maximum y position of the map double mapResol = 0.1; // [m/cell] The map resolution double origin_x = 0.0; // [m] x position of the map origin double origin_y = 0.0; // [m] y position of the map origin std::string frame_id = "base_link"; // frame id of the map int OCCUPANCY_THRES = 50; // occupancy value threshold
- /map/local_map/obstacle <-- local cost map
- /points/selected_motion <-- selected motion primitive for visualization
- /points/motion_primitives <-- motion primitives for visualization
- /car_1/command <-- control command