Fix group states

[rhaschke]

As pointed out in #1780 (comment),

1. The robot model loader doesn't complain about malformed group states (having missing joints).
2. FakeControllers used those uninitialized joint values for publishing.

Both issues are fixed with this PR. This should fix the flaky moveit-cpp unit test.

[henningkayser]

Additionally to throwing a warning, default values should be 0 or the state might be invalid. Alternatively, we could also make this an error and don't add the state at all. Or do you think we should allow incomplete states in order to support values for unspecified "subgroups"?

[rhaschke]

The group state is essentially a map: joint name -> value. Hence, it is fine to leave some values "undefined" (not listed in the map) as long as setToDefaultState starts from a fully-defined state.
This should be the case anyway as the there might be joints outside the JMG, which will definitely not be modified by setToDefaultState. The fake controller manager takes care of this (now).

[rhaschke]

@henningkayser, @JafarAbdi: This fix doesn't resolve the failing unit test. Same for #1780.

[JafarAbdi]

@rhaschke @henningkayser I debugged this exception

terminate called after throwing an instance of 'boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<boost::lock_error> >'
  what():  boost: mutex lock failed in pthread_mutex_lock: Invalid argument

I think the reason is a dangling reference, the exception happens in this line

In the MoveItCpp constructor, we pass tf_buffer_ to the tf_listener which takes a reference and store it as reference, now when the MoveItCpp destructor is called it will reset tf_buffer_ which will cause the pointee to be destructed while there's another thread that uses the object when we default the destructor the tf_listener_ will be destructed before tf_buffer_ and the exception won't occur

This's the stack call for a reference

[rhaschke]

Just fix the order of resetting variables in the custom destructor:

1. tf_listener_
2. tf_buffer_

[codecov-io]

Codecov Report

Merging #1781 into master will increase coverage by 0.72%.
The diff coverage is 100%.

@@            Coverage Diff            @@
##           master   #1781      +/-   ##
=========================================
+ Coverage   47.58%   48.3%   +0.72%     
=========================================
  Files         290     294       +4     
  Lines       22950   23232     +282     
=========================================
+ Hits        10920   11223     +303     
+ Misses      12030   12009      -21

Impacted Files	Coverage Δ
...s/planning_interface/moveit_cpp/src/moveit_cpp.cpp	62.29% <ø> (ø)
moveit_core/robot_model/src/robot_model.cpp	77.2% <100%> (+0.97%)	⬆️
...ler_manager/src/moveit_fake_controller_manager.cpp	63.44% <100%> (+23.22%)	⬆️
...ng_interface/moveit_cpp/src/planning_component.cpp	48.43% <100%> (ø)
...cpp/include/moveit/moveit_cpp/planning_component.h	100% <0%> (ø)
.../moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h	100% <0%> (ø)
moveit_core/robot_state/src/conversions.cpp	30.94% <0%> (+0.44%)	⬆️
moveit_core/robot_model/src/joint_model_group.cpp	59.39% <0%> (+0.6%)	⬆️
... and 13 more

---

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 5fab740...601f550. Read the comment docs.