[ros2] Add check if rclcpp::init() has been called

[otamachan]

Hi,

I would like to use both gazebo_ros_init and gazebo_ros_factory plugins at the same time. But when I tried that, it failed.

tamaki@pc:~$ gzserver --verbose -s libgazebo_ros_init.so -s libgazebo_ros_factory.so
Gazebo multi-robot simulator, version 9.0.0
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

terminate called after throwing an instance of 'rclcpp::ContextAlreadyInitialized'
  what():  context is already initialized
Aborted (core dumped)

This PR adds check if rclcpp::init() has been called before calling it.

[chapulina]

Thanks for catching the problem and making a PR!

The new test failed on the build farm, it looks like the message is published too late:

19:54:56 12: �[0m/tmp/ws/src/gazebo_ros_pkgs/gazebo_ros/test/test_plugins.cpp:63: Failure
19:54:56 12: Expected: (msg) != (nullptr), actual: 16-byte object <00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00> vs 8-byte object <00-00 00-00 00-00 00-00>
19:54:56 12: test
19:54:57 12: [WARN] [hello_ros_world]: Publishing

[chapulina]

Good call! It may also be worth it adding a check to gazebo_ros_init, and in both cases, printing a warning if it's been initialized, so the user knows their arguments won't be loaded.

[otamachan]

OK. I'll add a check to gazebo_ros_init, too.
I think using both gazebo_ros_init and gazebo_ros_factory is a common use case for ROS users. In that case it might be annoying when a warning is always printed. Is that OK?

[chapulina]

Ok, let's go without the warning, thanks!

[otamachan]

Thank you for your reviewing.

1. Test failure
   I could reproduce the test failure locally, but after upgrading rclcpp to master branch it never happens. So it might not be related to this change. (Resolve startup race condition for sim time ros2/rclcpp#608 ?)
   The test is still failing but [ros2] Improve executor destruction #860 will solve the problem.
2. I removed calling rclcpp::init() from gazebo_ros_factory rather than checking and calling because gazebo_ros::Node::Get() will call rclcpp::init() automatically.
3. I added warning to gazebo_ros_init if rclcpp::init is already called as I think gazebo_ros_init should be the only way to call rclcpp::init with user defined arguments.

Thanks for your advice in advance.

[chapulina]

Thanks for iterating, this looks good to me.

[chapulina]

nitpick: this line can be moved out of the if/else