Fred/crowd sim plugin

[FloodShao]

Migrating the crowd simulation plugin into building_sim_plugins package.
Please include the import the menge lib to src directory when compiled the plugins, otherwise, the crowd simulation plugins will not be compiled.
The current using menge lib is as following.

repositories:
    meng_core:
        type: git
        url: https://github.com/FloodShao/menge_core.git
        version: master

To run the crowd simulation example.
For gazebo:

1. go to .../building_gazebo_plugins/test
2. ros2 launch office.crowd_simulation.launch.xml

For Ign:

1. go to ../building_ignition_plugins/test
2. in the launch file office.launch.xml, you might need to add the model path for common models in https://github.com/FloodShao/traffic_editor/blob/fred/crowd_sim_plugin/building_sim_plugins/building_ignition_plugins/test/office.launch.xml#L7.
3. ros2 launch office.launch.xml

There is an additional issue for ignition plugin:
Current test example for ignition plugin does not assign magni1 and magni2 as external agents, though they are imported in the world. The reason is when processing the crowd simulation initialization, the magni model (plugin) might not be imported in the world yet, which makes the crowd simulation plugin failed to be initialized. This situation isn't considered before because previous demo didn't import other plugin.

[luca-della-vedova]

As a high level comment I would remove all the testing files if we want to merge this. A strategy you could try is move all the testing files to a separate branch (i.e. called `crowd_sim_plugin_test).
We can do high level tests using that branch and merge this one when it looks good. This should make the diff much smaller and easier to manage

[luca-della-vedova]

OK got it up and running! I was pondering if we can do something to make the actors look more natural when they reach a waypoint, basically what happens now is that they stop playing an animation at whatever point in time when they reach their destination (example below):

I can see at least two ways to make it look better:

1. Probably easier, reset the animation time to a constant (i.e. 0?) where they look standing
2. Better but maybe harder, we can use a different animation for walking compared to "being at waypoint". For example I think the actor you are using has a stand.dae file with an animation for standing in place. The nice thing about this would be that in the future we can change it to arbitrary animations, for example we can have actors perform actions at their destinations.

Do you think this would be possible?

[FloodShao]

I think I have achieved using the first method, but not sure it's satisfied. as follows:

The stand.dae actually does not have a stand position keyframe. The whole loop walks 1m so I tried to get the key frame when the two legs are together at 0.25 m.

This code is pushed to fred/crowd_sim_plugin_test branch

[luca-della-vedova]

I think it's OK, it seems it's not so immediate to switch among different animations at runtime, we can leave this as a future feature.

[luca-della-vedova]

Just a quick review, I believe we can further reduce the size of this PR by moving as much logic as possible to the common library and reducing code duplication between the ignition and the gazebo plugins.
There might be some additional functions I missed but I pointed out the ones I could find from a quick glance.
Ideally we would reach something similar to the door plugin, which is super simple.

[aaronchongth]

The simulation is working like a charm! Thanks for all the hardwork! This is going to be awesome once we iron out all the changes.

[FloodShao]

Refactor the original crowd simulation plugin structure. Changes are follow:

1. Move the sdf loading part into 'common' interface
2. Move modeltype initialize part into 'common' interface
3. Use template function for passing actor pose between menge and plugin, to address the different ignition::math::Pose3d version problem.
4. Fixing original ignition plugin waiting agents loading problem.
5. Other code structure problems.

[FloodShao]

To test the refactored crowd_sim_plugin, a new test branch is created under fred/crowd_sim_plugin_refact_test
https://github.com/FloodShao/traffic_editor/tree/fred/crowd_sim_plugin_refact_test/building_sim_plugins/crowd_sim_plugin_test

The test files are tested under the latest rmf_core structure, with tinyRobot (under both gazebo and ignition).
Please use ignition version as https://github.com/osrf/rmf_demos/blob/feature/add_ignition_support/docs/ign-dome.yaml
Take note the previous bug in Actor.cc is locally fixed in for sdf10.

[luca-della-vedova]

Thanks for the fixes! The main issue I find now is in the code style, generally we have the rmf guidelines for contributing code that we try to stick to (for example function names should be snake_case). I think for consistency with the rest of the code base it would be good to follow the same guidelines, as well as applying the rmf linter.

[aaronchongth]

Is there a reason the assert is in this check? Also do you intend to have the code continue executing even if the pointer is invalid? My suggestion would be to check for nullptr and return,

if (!agentPtr)
{
  RCLCPP_ERROR(_crowdSimInterface->logger(), "Null agentPtr when update Object!");
  return;
}

Or if this check is really for debugging purposes, maybe you can get away with only using a single assert call instead of an if case,

assert(agentPtr);
assert(modelPtr);

[FloodShao]

Thanks for pointing out. Yep I remember I wrote this assert in the early stage to abort the plugin once an invalid pointer is found, which I think assert() is doing the check, and if found false, assert will call abort(). Just return from this function might just get a log and the rest code is continuing.
Is there any way to do it elegantly? maybe I should use exit() ?

[aaronchongth]

There is not that clear a solution I believe. It will ultimately depends on how you would like the plugin's behaviour to be, and how often would you expect these edge cases to show up. If it is something that is critical for the continual operation of the plugin, you might want to crash the plugin while providing error logs. Otherwise if this happens once in a while, as a result of dropping a sensor message for example, and you would like the plugin to continue, then an error log while calling return; on the function would be good.

[aaronchongth]

Just a few more minor changes and we will be ready to merge, thanks for incorporating most of the comments in the previous review!

2 more things you can do, is to add proper licensing information into all the source files, we use Apache 2.0, you can find them all throughout rmf_core, and might just need to change the year to 2020.

Linting will also be needed. You can check out how it is done through this github action, https://github.com/osrf/rmf_core/blob/feature/style_action/.github/workflows/style.yaml

[aaronchongth]

Thanks for making the changes for all the new features! LGTM!

[luca-della-vedova]

Great thanks for all the changes!