Add get the current control mode using ROS service
Add get the current control mode using ROS service