Feature/respawn entity

[pawellech1]

Description

Abstract

This PR introduces the possibility of changing the position of ego manually using RViz GUI during the simulation in cpp mock scenarios.

Example scenario presenting the feature is included.

Background

https://tier4.atlassian.net/browse/RJD-933)](https://tier4.atlassian.net/browse/RJD-933

Details

• API::respawn method is implemented in this PR.
  • inside the function UpdateEntityStatusRequest is used to change entity position in simple sensor simulator
  • after the relocation the route planned for this entity is canceled and replanned starting from the new position
  • finally concealer for the respawned entity is reengaged.
• For the purpose of API::respawn function:
  • FieldOperatorApplicationFor<AutowareUniverse>::clearRoute() method was implemented which triggers /api/routing/clear_route service
  • EntityBase::getVehicleParameters pure virtual method was implemented. For the entities other than VehicleEntity calling this method causes an exception.
  • new function getParameterFromExistingNode was implemented and used in EgoEntitySimulation::makeSimulationModel instead of getParameter function.
    • the reason for this change is the fact that during the respawn, multiple calls of getParameter function inside EgoEntitySimulation::makeSimulationModel were very time consuming (because of the need to construct and deconstruct multiple ROS2 nodes).
• respawn_ego.cpp scenario is implemented to present the future
  • in the scenario subscriber of /initialpose topic is implemented
    • after receiveing the message from the topic, subscriber calls API::respawn passing newly received position and the initial ego goal as the arguments
  • using the /initialpose topic inside this scenario is just an example
    • some other topic or service could be used to trigger API::respawn call
    • using the /initialpose topic makes it possible to use RViz GUI “2d Pose Estimate” built in plugin

Known Limitations

Currently only respawning of the ego entity is supported. Passing any other entity name to the function ends up with an exception.

[github-actions]

Checklist for reviewers ☑️

All references to "You" in the following text refer to the code reviewer.

• Is this pull request written in a way that is easy to read from a third-party perspective?
• Is there sufficient information (background, purpose, specification, algorithm description, list of disruptive changes, and migration guide) in the description of this pull request?
• If this pull request contains a destructive change, does this pull request contain the migration guide?
• Labels of this pull request are valid?
• All unit tests/integration tests are included in this pull request? If you think adding test cases is unnecessary, please describe why and cross out this line.
• The documentation for this pull request is enough? If you think adding documents for this pull request is unnecessary, please describe why and cross out this line.

[dmoszynski]

@yamacir-kit
I've merged the master to the branch from this task and adapted to the changes.

• However, I encountered issues - after merge respawn feature stopped working.
• After a deep analysis, I found that it is necessary to publish the clock (ROS time) to Autoware during loop respawn.
• I added this publication, and by the way I refactored the code - 1b9639f.

I'm also thinking whether it would be better to develop the respawn in such a way that it would be performed inside the updateFrame() in the API. More precisely, for example, as a Job or in EgoEntity::onUpdate(), so that ss2 modules update will be available during the respawn. The current solution is blocking and during the respawn the modules are not updated. I'm not sure if it is essential that the time required for respawn not cause the elapse of simulation time (iteration of frames).

Let us know if this approach (of the original author) is as expected?
If so, I will run regression tests again for the current version.

[yamacir-kit]

@dmoszynski
The problem is that you are trying to perform a task in a single function that obviously cannot be completed in an instant.
traffic_simulator basically assumes that all operations can be performed without consuming simulation time. In other words, the functions other than updateFrame are the staging operations for the changes to occur in the next simulation frame, and updateFrame is the commit of the staged changes.

The concealer is usually responsible for the actual work, since the setup of Autoware is obviously not finished in an instant.
The traffic_simulator only instructs the concealer on the setup, while the actual work of the setup is performed by the concealer in an independent thread. (See the use of task_queue.delay in field_opeartor_application_for_autoware_universe.cpp)

I'm not sure if it is essential that the time required for respawn not cause the elapse of simulation time (iteration of frames).

The simulation start is simultaneous with the engage, but respawn is an event during the simulation, so it should be accompanied by a simulation time elapse.

[dmoszynski]

@yamacir-kit

The problem is that you are trying to perform a task in a single function that obviously cannot be completed in an instant.
traffic_simulator basically assumes that all operations can be performed without consuming simulation time. In other words, the functions other than updateFrame are the staging operations for the changes to occur in the next simulation frame, and updateFrame is the commit of the staged changes.

I understand this completely, I'm asking because the current solution is not of my invention and is a bit specific - does not cause elapsed time, that is, it blocks updateFrame() and I'm not sure if this is expected. I think we should avoid such blocking of updateFrame(), and some of the operations (engageable() check) should be done during updateFrame().

Although I must admit that now I've concluded that this check engageble() - the whole while loop, is unnecessary. We can assume that the concealer is responsible for the execution of engage() - that is, add the task to its queue and assume that it will be executed when possible.

What do you think about this idea - commit (video below)? Unfortunately, this way we don't know if the "respawn" was successful/unsuccessful - we don't have a check on the traffic_simulator side to see if the Ego is engaged().

The simulation start is simultaneous with the engage, but respawn is an event during the simulation, so it should be accompanied by a simulation time elapse.

From this I conclude that the check engageble() (the whole while loop) should be moved or removed.

Video

Video after change commit is added:

vokoscreenNG-2024-04-04_09-20-18.mp4

[yamacir-kit]

@dmoszynski
The engageable is there to "engage Autoware and start the simulation as soon as Autoware is ready to engage". Therefore, when re-engaging during simulation, there is no problem to engage without checking engageable.

The concealers initialize, plan, and engage have built-in confirmation that the execution of those functions will correctly transition the Autoware state. In other words, the initialize process confirms that the result will be a transition to WaitingForRoute, the plan process confirms that the result will be a transition to WaitingForEngage, and the engage process confirms that the result will be a transition to Driving. If the transition cannot be confirmed after a certain period of time, the concealer notifies an error and the simulation is shifted to shutdown as a failure. And initialize, plan and engage use task_queue.delay.

Therefore, there should be no problem to run initialize, plan and engage in succession without any confirmation. The transitions are automatically monitored by an independent thread, which is notified if a problem occurs, and the simulation proceeds as is if not. The success of respawn is checked consequently by the fact that no errors result.

I have not been able to check the video, but for the reasons above, the commit should be fine.
(It may be an issue with my environment, but I can't view the video on GitHub. It also fails to download.)

[dmoszynski]

@yamacir-kit
Thank you so much for the review.

• I've introduced all the requested changes.
• I've performed local tests, github CI is green.

Taking a look at the commit with the changes I suppose that it is not necessary to re-run the regression tests - let me know what you think about it.

[yamacir-kit]

There were still a few things that bothered me and I'd like to fix them before merging.
There is no need to do another regression test on this change.

[dmoszynski]

:)