feat(control-messages): add control messages

[xmfcx]

Signed-off-by: M. Fatih Cırıt [email protected]

Description

This PR adds the message guidelines and first set of messages: autoware_control_msgs

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

[isamu-takagi]

How about adding a guide for enums? I considered enums here because I'm often confused by message containing multiple enums. For example:

uint8 LEFT=1
uint8 RIGHT=2
uint8 UP=3
uint8 DOWN=4
uint8 horizontal
uint8 vertical

[xmfcx]

@kenji-miyake is there a simple way to not have dependency_ws in build process? Do we need to edit the workflow?

https://github.com/autowarefoundation/autoware_msgs/runs/6098081703?check_suite_focus=true#step:7:29

[xmfcx]

@isamu-takagi

How about adding a guide for enums? I considered enums here because I'm often confused by message containing multiple enums.

Thanks for reminding, we can add from official ros2 messages for an example, will add it to the guidelines.

https://github.com/ros2/common_interfaces/blob/f3cb4848560e91596e7688e8ac1816828fa460cb/shape_msgs/msg/SolidPrimitive.msg#L4-L11

uint8 BOX=1
uint8 SPHERE=2
uint8 CYLINDER=3
uint8 CONE=4
uint8 PRISM=5

# The type of the shape
uint8 type

[kenji-miyake]

@kenji-miyake is there a simple way to not have dependency_ws in build process? Do we need to edit the workflow?

@xmfcx There are two ways.

1. Remove build_depends.repos from the workflow.
   Then, we have to stop sync-files and maintain the CI script for this repository.

   autoware_msgs/.github/workflows/build-and-test-differential.yaml

   Line 41 in cf30f44

   build-depends-repos: build_depends.repos

2. Put an empty build_depends.repos that only has repository:.
   We can keep syncing build-and-test-differencial.yaml.

Either one is okay for me, but I personally prefer the No.2 because it's easier to maintain.

[xmfcx]

@xmfcx create a message for velocity, acceleration, jerk, steering, steering_rate limits.

[TakaHoribe]

@xmfcx @mitsudome-r Thanks for the great work.

I have a few thoughts after mtg and make some additional comments.

First,

• Dynamic limits_msg should be defined NOT as an output of the control module, BUT as an input of vehicle interface. Thus, the limits_msg should be defined in autoware_vehicle_msgs.

The reason for this is that the jerk_limit (and other limits) will be published not only by the control module, but also by system and planning modules, for example, in emergency cases. Of course, the control module can publish limits_msgs, but it should be optional. The vehicle should move even if there are no limits_msgs published.

Secondly. (Sorry for a long comment, but I think it is essential.)

In the mtg, it is mentioned that the target acceleration is the derivative of the target velocity. But it may be misleading.

For example, let's say a vehicle is driving at 50 km/h and Autoware wants the vehicle to be 60 km/h.

The velocity profile will then look like this.

In this figure, the derivative of the target velocity array is 0, but obviously, the acceleration we want to achieve is not zero, should be some positive value.
Thus, "the derivative of the target velocity" is not the target acceleration. These are different concepts and there is no low_level_controller using both target velocity and target acceleration (i think).

In the current implementation of .universe, the longitudinal_controller node calculates the target acceleration by feedback control of the velocity tracking deviation.

Some people may think it is strange to suddenly give a target speed of 60 km/h when the vehicle's velocity is 50 km/h. The velocity profile in that case is probably like this.

In this case, the target acceleration matches the derivative of the target velocity. But if the low_level_controller is working only with pure velocity FB control like PID (setpoint control), it would never accelerate because the velocity error is zero. Similarly, when the vehicle velocity is 0, it will never start accelerating for a velocity array that increases from a zero target velocity.

Someone may also think, what if we increase the target speed a little so that the FB control works? Actually, this idea is often used in practice.

However, in this case, if the vehicle moves according to the derivative of the target velocity array, the vehicle velocity will never catch up to the target velocity, as shown by the blue dots.

Supporting multiple interfaces (target speed, acceleration) causes this kind of confusion.

The current implementation of .universe follows the first policy (first picture). The planning module calculates a kinematically feasible velocity profile, but the same situation as in the first figure can occur if the low_level_controller's velocity control performance is very poor.

The current implementation is based on the idea that "when the low_level_controller supports the targe velocity interface, it should have the responsibility for achieving the target velocity. It is to clarify the role and responsibility.
Of course, it is technically possible to compensate for the poor performance of the low_level_controller with Autoware, but I do not recommend it because the responsibility of modules becomes vague.

In summary, "the derivative of the target velocity is not the target acceleration", or I can say, "the target acceleration cannot be calculated by simply differentiating the target velocity". It may cause a confusion for users.

In addition, the target jerk can not be calculated by just differentiating the target acceleration as well. To calculate the target jerk, it is needed to add a new feedback control logic. In that case, is the target_jerk information really needed for the control interface? Maybe the benefits are small for the burden of supporting this.

So, the discussion point would be:

• Is the target_jerk really necessary for control IF?
• Would it make confusion for users that the target acceleration is not the derivative of the target velocity?

My opinion is:

• A1. we can have the target_jerk because someone may want a desired_jerk and will implement such a controller. Although the is_defined_jerk flag will be false for a while.
• A2. it is a little complicated, but ok as long as the target velocity/acceleration/jerk is well defined in the documentation.

On the other hand, there is no concern with having control_command_arrays. It will make a room for a low_level_controller to improve its performance. Although the current universe implementation will always publish with size=1 since the longitudinal_controller does not support the target acceleration array calculation.

[xmfcx]

@TakaHoribe wrote:

First,

Dynamic limits_msg should be defined NOT as an output of the control module, BUT as an input of vehicle interface. Thus, the limits_msg should be defined in autoware_vehicle_msgs.

The reason for this is that the jerk_limit (and other limits) will be published not only by the control module, but also by system and planning modules, for example, in emergency cases. Of course, the control module can publish limits_msgs, but it should be optional. The vehicle should move even if there are no limits_msgs published.

I agree, I will add this to autoware_vehicle_msgs, thanks!

[xmfcx]

@TakaHoribe I think we need to clarify the output of control and planning nodes and the input of actual vehicle controller.

I want to share the ideal case first:

Here, the control stack outputted the blue velocity/time curve.

The facts we and the all the nodes know:

• We are at t_0, vehicle velocity is v_0.
• v_1 = v_0 + Δv_0
• Vehicle can only have one velocity at a certain time.
• Various Δv_x (x being time) values are constrained by:
  • vehicle's velocity at the previous time step,
  • gas/brake pedal levels at that time
  • gas/brake pedal speeds
  • real world friction values,
  • vehicle's internal mechanical frictions and latencies
• Vehicle's acceleration change at a time Δa_x (x being time) has also limits.

For consistency and conveying the right information, following should be done:

For planning stack:

• Planning stack should be strict and pessimistic to follow the road rules.
• Planning stack should assume control stack to fall behind and be on the safe side when generating crude/rough velocity curves.

For control stack:

• Control stack should know the vehicle constraints well and refine the velocity curve accordingly.
• It should know the current acceleration of the vehicle and the possible limits of the acceleration change per time.
• Output the expected velocity and acceleration curve for the time ahead.
• Control stack doesn't know the pedal levels, voltage/torque relationships.
• But it knows vehicle should have certain acceleration values at certain times.
• Vehicle can actually achieve these accelerations and velocities.

For low level controller / vehicle controller:

• It receives velocity values it can realistically achieve.
• It knows it has v_0 and a_0 and in order to achieve v_1 and a_1 and the curve ahead, it calculates the necessary torque/voltage necessary for the gas/brake pedals, steering controller and applies them.

Here is a bad control example.

If with this kind of curve being output from the controller (along with acceleration values I didn't draw to achieve these) the vehicle achieves that kind of real velocity curve in the end, there is something wrong going on.

Also we shouldn't forget that these curves are being generated very frequently, about 50hz generally.

If the vehicle ends up with that velocity error at time 100ms, then it has already failed. This shouldn't be possible if the control stack and vehicle controller functioned as expected.

Also of course if we feed the first element of these to a PID controller naively, the vehicle will never move at all too. Picking the right input of a PID is the job of the controller that is utilizing the algorithm.

In the end, the controller outputs a velocity/time curve which vehicle is expected to follow (and can follow) and also an acceleration/time curve which vehicle is expected to follow (and can follow) and if vehicle achieves those acceleration curve, it will end up with the same velocity curve.

The arrays we are sending are just discretized versions of these curves.

[xmfcx]

@TakaHoribe wrote:

For example, let's say a vehicle is driving at 50 km/h and Autoware wants the vehicle to be 60 km/h.

The velocity profile will then look like this.

In this figure, the derivative of the target velocity array is 0, but obviously, the acceleration we want to achieve is not zero, should be some positive value.

This gray curve is an unrealistic expected velocity curve.

At t_0, the vehicle has v_0 which is 50 km/h and it cannot reach to 60 km/h in an instant.

Even if target was 51 km/h, at t_0 the vehicle cannot reach there, it needs time, even if it's a small amount of time.

For the acceleration, assuming vehicle was going at constant velocity 50 km/h, initial acceleration of the vehicle is 0 m/s².

As the time progresses, the vehicle might press the gas pedal more and more and achieve a positive acceleration and reduce it at some point to 0 m/s² again.

So expected acceleration array would be like: [0, 1, 2, 3, 2, 1, 0] m/s² (I am making up numbers).

[xmfcx]

@TakaHoribe wrote:

Some people may think it is strange to suddenly give a target speed of 60 km/h when the vehicle's velocity is 50 km/h. The velocity profile in that case is probably like this.

In this case, the target acceleration matches the derivative of the target velocity. But if the low_level_controller is working only with pure velocity FB control like PID (setpoint control), it would never accelerate because the velocity error is zero. Similarly, when the vehicle velocity is 0, it will never start accelerating for a velocity array that increases from a zero target velocity.

This gray expected velocity curve is more realistic.

But if you put the first value in this curve as the input of the PID controller (assuming error input is delta velocity, output is gas pedal level / throttle), the vehicle won't move. You should at least enter the v_1 - v_0 or something latter as error input.

[xmfcx]

@TakaHoribe wrote:

Someone may also think, what if we increase the target speed a little so that the FB control works? Actually, this idea is often used in practice.

However, in this case, if the vehicle moves according to the derivative of the target velocity array, the vehicle velocity will never catch up to the target velocity, as shown by the blue dots.

Here the gray expected velocity curve is not realistic again. The vehicle cannot jump to 50 to 53 km/h at t_0.

It might be what you directly feeding into the PID but that's an implementation detail. You are basically shifting the expected velocity curve from right to left to feed into PID. It is ok but we should pass the expected velocity curve and let the node use it as it likes to.

If you take the derivative of the actual expected velocity curve, you will end up with expected acceleration curve.

Supporting multiple interfaces (target speed, acceleration) causes this kind of confusion.

Unifying the outputs of the planning and control stacks as being expected velocity/acceleration arrays from v_0 to v_n should lead to common understanding and clarification, as opposed to confusion.

The unrealistic expectations from the nodes with hardcoded assumptions causes more confusion in my opinion.

[TakaHoribe]

@xmfcx thank you for your reply. I think we should first clarify the role of planning/control/low_leverl_controller.
(btw, why PR page doesn't support a thread reply function.)

I think it is useful to discuss:

• Who is responsible for correcting tracking errors? (maybe we should define the "tracking error" clearly at first.)

Let's think about the velocity deviation caused by disturbances. Generally, the control module should be responsible for the suppression and correction of the error.
In this case, it is important to determine whether planning calculates the target speed from the self-velocity. Actually, "calculating the target velocity profile from the self-velocity" contains a part of the velocity feedback and thus conflicts with the role of the control module. This is undesirable because it causes a burden on the design and tuning of planning/control modules.

For this problem, there are two popular approaches:

• combine the control and planning into one big module so that the big module is responsible (like written in the CMU paper)
• planning should plan in a no-error situation, and the only control should have the role of the error correction (like written in the Stanford paper)

The current implementation of .universe uses the latter method from the standpoint of modularity, and the planning module does not perform calculations from self-position or velocity. Thus, this case arises frequently (10km/h deviation is too much though).

My opinion is that the planning should not calculate from the ego-position/velocity as implemented in the current .universe. @xmfcx what do you think?

[xmfcx]

@TakaHoribe wrote:

Actually, "calculating the target velocity profile from the self-velocity" contains a part of the velocity feedback and thus conflicts with the role of the control module. This is undesirable because it causes a burden on the design and tuning of planning/control modules.

I agree, having multiple facilities responding to an error makes it hard to tune and might cause weird oscillations due to multiple feedback mechanisms responding to an error.

My thoughts on the planner

Planner

It should plan trajectories that can be followed by the vehicle.

A trajectory is made of poses from where the vehicle is and where it will be in future, discretized with a fixed time step.

A trajectory is very deeply attached to the planned velocity profile/curve for the vehicle. Because trajectory includes time information and there is only a single velocity profile possible to a matched trajectory.

The trajectory should be planned with the current steering angle, steering rate, velocity, acceleration in mind. Otherwise the vehicle might not be able to follow the plan.

Also the planner should be consistent, pessimistic and strict.

• Consistent, because it updates very frequently (up to 50 to 100 Hz) and as the vehicle follows the route, not change the plan too frequently in the re-calculation of the routes. (Unless there is really a reason to do so like emergency maneuvers) to allow a smooth, reliable and safe ride.
• Pessimistic in the sense of assuming the vehicle to be prone to errors and not leave the maneuvers to last second. Like, order to vehicle to slow down slightly more aggressive than necessary to leave room for controller error. Or not get too close to nearby obstacles to leave room for steering errors as much as possible.
• Strict, similar to consistent, if the vehicle falls behind the required constraints and previously planned trajectories, it should get more aggressive and at worst case, order an emergency braking.

Roles of controller and vehicle controller

I’ve stated my opinions on these on previous post.

What approach should we use?

Reference to architecture design discussion: https://github.com/orgs/autowarefoundation/discussions/4

graph TD
    subgraph motion planning & control
    subgraph motion planning
    oa[obstacle avoidance] --- pp(path planning)
    pp --- tp(trajectory planning + velocity optimization)
    end
    tp --- c(control)
    c --- vc(vehicle control)
    end

Loading

@TakaHoribe wrote:

combine the control and planning into one big module so that the big module is responsible (like written in the CMU paper)

TEB (Timed Elastic Bands) approach combines all of these in a single optimization problem. Here is a comparison of TEB and MPC approaches. Although it would require a lot of changes to be used in our stack.

Is this similar to the paper you sent? I’ve skimmed through the CMU paper but it seemed like it was similar to our modular approach. Maybe I misunderstood it. Could you explain?

flowchart TD
    subgraph current autoware motion planning & control
    oa[obstacle avoidance] --- pp(path planning)
    pp --> tp(trajectory planning + velocity optimization)
    tp --> c(control)
    c --> vc(vehicle control)
    end

Loading

My concerns with our current approach is similar to yours:

• Planner generates an unfeasible trajectory
• Controller tries to follow it but will fail to some extent
• If the trajectory was a critical one, might cause catastrophic failures

My opinion is that the planning should not calculate from the ego-position/velocity as implemented in the current .universe.

What should be done then?

Ideally, I'd expect some feedback pathways on the stack:

flowchart TD
    subgraph maybe like this?
    
    subgraph pp[path planner]
    bp[behavior planner] --- oa[obstacle avoidance]
    oa --- pg(path generator)
    end

    pp --> tp(trajectory planning + velocity optimization)
    tp --> c(control)
    c --> vc(vehicle control)

    vc -. can't follow, replan .-> c
    c -. can't follow, replan .-> tp
    tp -. can't follow, replan .-> pp

    end

Loading

But even then I see so many problems arising.

[TakaHoribe]

@xmfcx Just a quick reply:

TEB (Timed Elastic Bands) approach combines all of these in a single optimization problem. Here is a comparison of TEB and MPC approaches. Although it would require a lot of changes to be used in our stack.
Is this similar to the paper you sent?

Usually, the path of the teb_planner is followed by pure_pursuit type controller, right? In that case, it is not a similar approach to the paper I sent. The role of ego-position feedback exists in two modules, teb and pure_pursuit.

If the output of the teb_planner is directly used to drive the robot, then it is the same approach as I mentioned as the CMU paper. The planning and control is in the one big module.

[codecov]

Codecov Report

❗ No coverage uploaded for pull request base (main@2ce4d9c). Click here to learn what that means.
The diff coverage is n/a.

❗ Current head 20cd819 differs from pull request most recent head 68d13df. Consider uploading reports for the commit 68d13df to get more accurate results

@@         Coverage Diff          @@
##             main   #11   +/-   ##
====================================
  Coverage        ?     0           
====================================
  Files           ?     0           
  Lines           ?     0           
  Branches        ?     0           
====================================
  Hits            ?     0           
  Misses          ?     0           
  Partials        ?     0           

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 2ce4d9c...68d13df. Read the comment docs.

[xmfcx]

@TakaHoribe @maxime-clem @kenji-miyake could you review it please?

[maxime-clem]

The messages look good to me and I only have some minor comments.

[maxime-clem]

In universe this is changed to autoware_cmake.

• https://github.com/autowarefoundation/autoware_common/tree/main/autoware_cmake
• refactor: use autoware cmake autoware.universe#849

I am not sure if we want to change it here as well.

[xmfcx]

@kenji-miyake do you have opinions on this?

[kenji-miyake]

Actually, I'm not so familiar with the dependency type, but I think we don't have to remove ament_cmake_auto. We can just add another dependency of autoware_cmake (if the package uses autoware_cmake).

[xmfcx]

I will keep this one as ament_cmake_auto. If it becomes necessary, we can do it later on.

[maxime-clem]

Is apex.ai still involved ?

[esteve]

@maxime-clem they are not, as fas as I know

[xmfcx]

Hmm maybe it is a better idea to add them with a NOTICE file since they've contributed to the _auto messages, which I am partially making use of.

Like in https://github.com/autowarefoundation/autoware/blob/main/NOTICE

What do you think about it?

cc. @kenji-miyake @mitsudome-r

[kenji-miyake]

Yes, it's a good idea to write in the NOTICE file like These messages are based on _auto messages that are mainly developed by Apex.AI. 👍

[xmfcx]

Added in 8c5e4f5

[kenji-miyake]

Regarding the message formats, I believe TIER IV is okay if it's okay for Taka and Maxime.
cc @yukkysaito @mitsudome-r

[TakaHoribe]

We've discussed in the AWF architecture mtg. I totally agree with this message definition.

[xmfcx]

@TakaHoribe could you review it again please? I've separated stamp and control_time and added definitions.

[TakaHoribe]

@xmfcx Great description. LGTM!