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

1. 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;
   	}

2. 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