The input to servo at c++ level can be :
- Eigen::Vector6f (twist)
- Eigen::VectorXf (joint jog)
- Eigen::Isometry3d (pose)
The output of servo will be an Eigen::Matrix with 3 rows (position, velocity, acceleration) and num_joints columns (one for each joint)
A user can use Servo by initializing it with the necessary parameters and then calling getNextJointState(). This function will accept an input of type std::variant which can be any of the allowed input types.
The respective input processor will be called from within getNextJointState() depending on the input type.
i.e processCommand(std::get<input_type> msg)
When getNextJointState() is called it will call the input processor, IK solver , output processor in sequence.
The input processor will include validations and necessary conversions needed for the kind of IK solver being used.
The output processor will include the computations for the requested output fields (pos, vel, acc) and their respective validations.
typedef std::variant<Eigen::VectorXf, Eigen::Isometry3d> InputTypes;
Eigen::Matrix getNextJointState(InputTypes command)
{
// input_type will have the info on type of command (int)
processCommand(std::get<input_type>(command);
}
At the ROS level there will be a subscriber for each type of available input message type. Each type of subscriber will convert the message to the C++ level representation (Eigen).
There can be an int type switch_input service that will enable subscription on the desired topic and stop the other subscribers.
This can be accomplished using unsubscribe() and subscribe() methods of the subscriber.
The ROS interface will inlcude the following services
- start
- stop
- switch_input_type
The start and stop services are only available when servo is run as an independent ROS node. (?)
The switch_input_type service is available even if using through the C++ API.
The node will subscribe to relevant input topics for twist, pose and joint jog commands and publish the specified output message type on the topic defined in the parameters.
The Servo ROS node will have 2 modes
- Real-Time
- Periodic
The real-time mode will wait on input (conditional variable) and respond as soon as it arrives. The periodic mode will run at the user specified frequency.