Add mros_contingencies_sim

[jginesclavero]

MROS contingencies simulator

This PR adds two functionalities to simulate some contingencies, following @chcorbato suggestions in #46:

• I have added a node that is a subscriber of AMCL (to know how many meters the robot has moved) and takes the battery consumption per meter as input. With this information, the robot's battery is calculated and published in a topic (/robot/battery).

• I have added an RVIz panel to easily the simulation of the laser driver failure. This panel has a button to switch the laser driver node into a DEACTIVATE state.

[chcorbato]

• I have added an RVIz panel to easily the simulation of the laser driver failure. This panel has a button to switch the laser driver node into a DEACTIVATE state.

Cool idea, but what do you refer to with the DEACTIVATE state? Do you mean "Inactive in the node lifecycle state machine?
Is that the best option to simulate a laser failure? Or should we trigger the transition to ErrorProcessing?

[jginesclavero]

Hi @chcorbato! You are right. I was fixing it, and I found an open PR about this. It seems like the diagram and the implementation differ a bit, and it is not possible to transit from de Activate state to ErrorProcessing, at least for now. We could compile this PR, but this means compile ROS2... Any idea? @fmrico

[fmrico]

I don't like the idea of depending on compiling ROS2, or something similar. We depend on the acceptance of that PR.

Meanwhile, is it possible to do the transitions active->inactive->Unconfigured and the Error?

[jginesclavero]

Meanwhile, is it possible to do the transitions active->inactive->Unconfigured and the Error?

I have tried it, and not...
[laser_resender_node-4] [WARN] [] [rcl_lifecycle]: No transition matching 12 found for current state unconfigured [laser_resender_node-4] [ERROR] [] []: Unable to start transition 12 from current state unconfigured: Transition is not registered
I have used the TRANSITION_ON_CONFIGURE_ERROR msgs.

No TRANSITION_ERROR appears here . Seems like ErrorProcessing is an internal state.

[chcorbato]

I don't like the idea of depending on compiling ROS2, or something similar. We depend on the acceptance of that PR.

Meanwhile, is it possible to do the transitions active->inactive->Unconfigured and the Error?

Rather than work arounds, I think a valuable contribution from MROS would be to fix this so that ROS2 complies with its own formal models and improves quality. What do you think?

[jginesclavero]

I have updated the contingency simulator. Now, it injects all-zero laser_scan msgs in the network and the laser_driver wrapper (the lifecyclenode) detects this messages and self-transits to ErrorProcessing.

[chcorbato]

I am trying to test the branch contingencies-sim, but when building the ws I get the following error:

Starting >>> pilot_urjc_bringup
--- stderr: pilot_urjc_bringup                                                                     
Traceback (most recent call last):
  File "/opt/ros/foxy/share/ament_cmake_core/cmake/package_templates/templates_2_cmake.py", line 21, in <module>
    from ament_package.templates import get_environment_hook_template_path
ModuleNotFoundError: No module named 'ament_package'
CMake Error at /opt/ros/foxy/share/ament_cmake_core/cmake/ament_cmake_package_templates-extras.cmake:41 (message):
  execute_process(/usr/bin/python3
  /opt/ros/foxy/share/ament_cmake_core/cmake/package_templates/templates_2_cmake.py
  /home/parallels/pilot_urjc_ws/build/pilot_urjc_bringup/ament_cmake_package_templates/templates.cmake)
  returned error code 1
Call Stack (most recent call first):
  /opt/ros/foxy/share/ament_cmake_core/cmake/ament_cmake_coreConfig.cmake:41 (include)
  /opt/ros/foxy/share/ament_cmake/cmake/ament_cmake_export_dependencies-extras.cmake:15 (find_package)
  /opt/ros/foxy/share/ament_cmake/cmake/ament_cmakeConfig.cmake:41 (include)
  CMakeLists.txt:7 (find_package)


make: *** [Makefile:222: cmake_check_build_system] Error 1
---
Failed   <<< pilot_urjc_bringup [0.22s, exited with code 2]
Aborted  <<< nav2_gazebo_spawner [1.08s]                        

Summary: 46 packages finished [16.7s]
  1 package failed: pilot_urjc_bringup
  1 package aborted: nav2_gazebo_spawner
  1 package had stderr output: pilot_urjc_bringup
  9 packages not processed

help is welcome!

[jginesclavero]

Are you building in /home/USER/MROS_WS? Did you source ROS2 and the ws?

[chcorbato]

...
help is welcome!

Something weird with my ws, now it is working, thanks @jginesclavero

[chcorbato]

I have updated the contingency simulator. Now, it injects all-zero laser_scan msgs in the network and the laser_driver wrapper (the lifecyclenode) detects this messages and self-transits to ErrorProcessing.

This is the behavior I observe in the first terminal (tb3_sim_launch.py):

[laser_resender_node-4] [WARN] [1601452186.621478064] [laser_resender]: [laser_resender] ALL-ZEROS. It has to go to error processing state
[laser_resender_node-4] [WARN] [1601452186.621572066] []: Error occurred while doing error handling.
[laser_resender_node-4] [ERROR] [1601452186.621582193] [laser_resender]: [laser_resender] Error processing from [active] state...

LGTM, please go ahead and merge if @fmrico is ok too.

[fmrico]

LGTM. Merging!!