Changing servo group construction

[greenpepper123]

Servo parameters and pub/subs are formed by servo groups and each group is top-level namespace for now (i.e. parent or root doesn't exist).

https://github.com/JSKAerialRobot/aerial_robot/blob/master/robots/hydrus_xi/config/quad/Servo.yaml
https://github.com/JSKAerialRobot/aerial_robot/blob/master/aerial_robot_model/src/servo_bridge/servo_bridge.cpp

This causes subscribers of the groups to be registered separately, even if topic names are the same.
It had not been the actual problem up until now, because only joint servos are set to be send_data_flag = True.

Now we started to let all joints to return the data.
If we don't change the code and write the config like this,

servo_controller:
  joints:
    state_sub_topic: servo/states
  gimbals:
    state_sub_topic: servo/states

then the same two callback for joints and gimbals are called when a new servo/states comes in.

I tried it and at first it worked correctly, but sometimes aerial_robot_base_node crashed (SEGFAULT).
Then I launched aerial_robot_base_node with gdb.
I found out that double-free and something similar happens in the callback.

#0  0x00007ffff5895f55 in _int_free (av=0x7fffbc000020, p=<optimized out>, have_lock=0) at malloc.c:4012
#1  0x00007ffff589a58c in __GI___libc_free (mem=<optimized out>) at malloc.c:2975
#2  0x00007ffff50fe1eb in void Eigen::internal::call_assignment_no_alias<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::internal::assign_op<double
> >(Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, Eigen::internal::assign_op<double> const&) () from /opt/ros/kinetic/lib/liborocos-kdl.so.1.3
#3  0x00007fffe679cdc3 in Eigen::internal::call_assignment<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::internal::assign_op<double> >(Eigen::Mat
rix<double, -1, -1, 0, -1, -1>&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, Eigen::internal::assign_op<double> const&, Eigen::internal::enable_if<!Eigen::internal::evaluator_assume_ali
asing<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::internal::evaluator_traits<Eigen::Matrix<double, -1, -1, 0, -1, -1> >::Shape>::value, void*>::type) (func=..., src=...,
    dst=...) at /usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h:712
#4  Eigen::internal::call_assignment<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > (src=...,
    dst=...) at /usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h:693
#5  Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >::_set<Eigen::Matrix<double, -1, -1, 0, -1, -1> > (other=...,
    this=0x755320) at /usr/include/eigen3/Eigen/src/Core/PlainObjectBase.h:682
#6  Eigen::Matrix<double, -1, -1, 0, -1, -1>::operator= (other=..., this=0x755320) at /usr/include/eigen3/Eigen/src/Core/Matrix.h:208
#7  aerial_robot_model::RobotModel::setThrustWrenchMatrix (q_mat=..., this=0x754c80)
    at /home/leus/ros/aerial_robot_ws/src/aerial_robot/aerial_robot_model/include/aerial_robot_model/transformable_aerial_robot_model.h:347
#8  HydrusRobotModel::calcWrenchMatrixOnRoot (this=0x754c80)
    at /home/leus/ros/aerial_robot_ws/src/aerial_robot/robots/hydrus/src/hydrus_robot_model.cpp:103
#9  0x00007fffe67c43c7 in HydrusTiltedRobotModel::calcStaticThrust (this=0x754c80)
    at /home/leus/ros/aerial_robot_ws/src/aerial_robot/robots/hydrus/src/hydrus_tilted_robot_model.cpp:10
#10 0x00007ffff0733594 in aerial_robot_model::RobotModel::updateRobotModelImpl (this=this@entry=0x754c80, joint_positions=...)
    at /home/leus/ros/aerial_robot_ws/src/aerial_robot/aerial_robot_model/src/transformable_aerial_robot_model/robot_model.cpp:130
#11 0x00007fffe67c2ae5 in HydrusTiltedRobotModel::updateRobotModelImpl (this=0x754c80, joint_positions=...)
    at /home/leus/ros/aerial_robot_ws/src/aerial_robot/robots/hydrus/src/hydrus_tilted_robot_model.cpp:89
#12 0x00007ffff2cebb15 in aerial_robot_model::RobotModel::updateRobotModel (joint_positions=..., this=0x754c80)
    at /home/leus/ros/aerial_robot_ws/src/aerial_robot/aerial_robot_model/include/aerial_robot_model/transformable_aerial_robot_model.h:70
#13 aerial_robot_model::RobotModel::updateRobotModel (state=..., this=0x754c80)
    at /home/leus/ros/aerial_robot_ws/src/aerial_robot/aerial_robot_model/include/aerial_robot_model/transformable_aerial_robot_model.h:71
#14 aerial_robot_model::RobotModelRos::jointStateCallback (this=0xa13ac0, state=...)
    at /home/leus/ros/aerial_robot_ws/src/aerial_robot/aerial_robot_model/src/transformable_aerial_robot_model/robot_model_ros.cpp:41
#15 0x00007ffff2cf246e in boost::function1<void, boost::shared_ptr<sensor_msgs::JointState_<std::allocator<void> > const> const&>::operator() (a0=..., this=<optimized out>) at /usr/include/b
oost/function/function_template.hpp:773
#16 boost::detail::function::void_function_obj_invoker1<boost::function<void (boost::shared_ptr<sensor_msgs::JointState_<std::allocator<void> > const> const&)>, void, boost::shared_ptr<senso
r_msgs::JointState_<std::allocator<void> > const> >::invoke(boost::detail::function::function_buffer&, boost::shared_ptr<sensor_msgs::JointState_<std::allocator<void> > const>) (function_obj
_ptr=..., a0=...)
    at /usr/include/boost/function/function_template.hpp:159
#17 0x00007ffff2cffe32 in boost::function1<void, boost::shared_ptr<sensor_msgs::JointState_<std::allocator<void> > const> >::operator() (

jointStateCallback is called twice when a servo/states comes in because it is double-registered by joints and gimbals, and then it violated the multi-thread rule.

Actually, it happens even when I unsubscribed servo/states for gimbals (as usual), but the frequency of it is much higher when I double-subbed.
It is natural because the program is writing to the same object in the robot model from another thread in navigation without any mutex, but the frequency of SEGFAULT is greatly increased by double-register.

Is it blamed to the servo group implementation or not?

Possible solution: Create the parent of servo groups and put it to top-level. Write the common parameters to parent namespace and pub/subs are registered once as the parent-level.