Implement general compute_vel method for OmniPID controller

[erc-bpgc]

Implement general compute_vel method for OmniPID controller

[SuhrudhSarathy]

As mentioned by @hardesh , a loop will be a good way to follow. However we must also keep in mind that we should be able to break the loop to carry out another trajectory also. We could do something in these lines.

current_goal_index = 0
while request_to_move:
    start = trajectory[current_goal_index]
    goal = trajectory[current_goal_index + 1]
    error, need = PID.init(start, goal)
    if need:
        repeat - > PID.goto(end, current_pose)
    if current_goal_index == len(trajectory-1) or new_trajectory_found:
        break
    elif:
        current_goal_index += 1

I think we have to figure out how to update current_pose, into the function and also incorporate new_trajectory_found function. PID.goto will be a loop again, we should have more control over how we break it.

[threewisemonkeys-as]

@SuhrudhSarathy, @hardesh for this, how do you think we should proceed? Since we were thinking of passing a complete trajectory to each controller, but how would PID decide which is the current target point?

[hardesh]

I think here you'll have to use the base PID object which has the goto method.

You can follow something like this:

for i in range(len(trajectory)-1):
  start = trjectory[i]
  end = trajectory[i+1]
  PID.init(start, end)
  repeat -> PID.goto(end, current_pose)

You'll have to figure out how to implement that repeat function without ROS.

[hardesh]

Instead of breaking out of the loop after the controller gets a new trajectory, you can reinitialize the main controller with the new trajectory.

[threewisemonkeys-as]

@pranavgo is this addressed? If so feel free to close the issue