# Feedback/Comments/Observations

## Table of Content

[ToC]

## 15JUN2020

### Winston's Journal

Clearly written and elaborates concisely on the tasks required in the first quarter plan for the FYP.

### Reuben's Journal

States clearly the lessons learned during the development of the tasks required in the first half of the FYP plan.

> Given the academic context of the reports, please refer to me as Dr. Amezquita-Semprun. [color=#C70039]

### circle_wp_gen_py

Some execution errors are displayed in the terminal after generating the waypoints.


Please, make sure the script runs as intended before pushing it to the repo.

Also, I would suggest fixing the radius of the paths and ask the user if the waypoints should be located in the inner lane or outer lane instead.

### navigation_py

Before digging into the code implementation, I would like to emphasize that a more modular approach was required in this phase. As illustrated in the flowchart below, ``navigation.py`` comprises the decision-maker/behavior module, the motion planner, the longitudinal controller, and the lateral controller. Given the simplicity of the current implementation, this approach may result advantageous in the short term. However, for more complex control strategies and decision-maker approaches, such a centralized architecture would hinder the development in terms of debugging, flexibility, and scalability.


Please, follow a more modular approach similar to the one illustrated below.

graph TB
    loc[localization] -- veh_status --> nav
    loc[localization] -- veh_status --> motion_planner
    subgraph navigation
    wp[(waypoints)] --> nav
    end
    subgraph decision-maker
    nav --closer mission waypoints-->motion_planner
    end
    motion_planner --speed ref--> speed_control
    motion_planner --ref path--> lateral_control
    speed_control-->veh_dynamics
    lateral_control --> veh_dynamics
    veh_dynamics --> loc

Now, let us review the code implementation:

- calc_target_index

def calc_target_index(cx, cy):
    """
    Compute index in the trajectory list of the target.
    :param state: (State object)
    :param cx: [float]
    :param cy: [float]
    :return: (int, float)
    """

    #recieves yaw
    yaw = get_yaw_rad()

    # Calc front axle position
    fx = state.pose.position.x + 1.483 * np.cos((yaw)+1.5708)
    fy = state.pose.position.y + 1.483 * np.sin((yaw)+1.5708)
    print('front axle (fx, fy): (' + str(fx) + ', ' + str(fy) + ')')

    # Search nearest point index
    dx = [fx - icx for icx in cx]
    dy = [fy - icy for icy in cy]
    d = np.hypot(dx, dy)
    target_idx = np.argmin(d)

    # Project RMS error onto front axle vector
    front_axle_vec = [-np.cos((yaw+1.5708)+ np.pi / 2), -np.sin((yaw+1.5708)+ np.pi / 2)]
    error_front_axle = np.dot([dx[target_idx], dy[target_idx]], front_axle_vec)

    print('Target (x,y): (' + str(cx[target_idx]) + ', ' + str(cy[target_idx]) + ')')
    print('Front axle error: ' + str(error_front_axle))

    return target_idx, error_front_axle

**Lines 124 ~ 125** The magic number ``1.483`` should be set as a configuration parameter with a proper description of the physical meaning.

Use a consistent notation, e.g., ``np.pi/2 or 1.5708``.

The search for the nearest point is done for all points every time even though we may have some points behind. To speed-up the search, use last closest point index (i.e., ``last_target_idx``) and search from there on.

**Lines 135 ~ 136** Please, explain mathematically the operation done in these lines. Use the trigonometric identities to simplify the calculation when a sine or cosine function is shifted $\pi/2$. Also, you need to describe the coordinate systems employ in your implementation.

* stanley_control

def stanley_control(cx, cy, cyaw, last_target_idx):
    """
    Stanley steering control.
    :param state: (State object)
    :param cx: ([float])
    :param cy: ([float])
    :param cyaw: ([float])
    :param last_target_idx: (int)
    :return: (float, int)
    """
    current_target_idx, error_front_axle = calc_target_index(cx, cy)

    if last_target_idx >= current_target_idx:
        current_target_idx = last_target_idx

    #recieves yaw
    yaw = get_yaw_rad()

    # theta_e corrects the heading error
    theta_e = normalize_angle((cyaw[current_target_idx]-1.5708) - yaw)
    print('Heading error = ' + str(cyaw[current_target_idx]) + ' - ' + str(yaw) + ' = ' + str(theta_e))

    # theta_d corrects the cross track error
    theta_d = np.arctan2(k * error_front_axle, target_vel)
    # Steering control
    delta = theta_e + theta_d

    if delta >= 0.95:
        delta = 0.95
    elif delta <= -0.95:
        delta = -0.95

    return delta, current_target_idx

According to the reference [paper](http://ai.stanford.edu/~gabeh/papers/hoffmann_stanley_control07.pdf), the steering command in the Stanley lateral control strategy is generated as follows:

$\delta(t) = (\text{head_err} - \text{head_ss}) + \arctan\left(k \frac{\text{crosstrack_err}} {k_{soft} + \text{ego_speed}}\right) + k_{yaw}(\text{yaw_ref - yaw_ego})$

In our case, we could ignore head_ss given that is related to the vehicle dynamics and we are not considering that yet. But, we do need to included the yaw_err = yaw_ref - yaw_ego, which helps to stabilize the ego vehicle when changing lanes. The yaw_ref is computed according to the curvature of the reference path and the ego vehicle speed.

Please note, you need to check carefully your sign convention when including the yaw_err to avoid instability.

Also, the parameter $k_{soft}$ is very important when the ego vehicle travels at very low speed to avoid sudden changes in the steering command. The fourth term in the paper is used to compensate for any steering mechanism delay but we are not considering that in this project.