// function to get the motor speed
void GazeboUUVPlugin::CommandCallback(CommandMotorSpeedPtr &command) {
for (int i = 0; i < 4; i++) {
command_[i] = command->motor_speed(i);
}
/*std::cout << "UUV Command Callback:"
<< command_[0] << ","
<< command_[1] << ","
<< command_[2] << ","
<< command_[3] << ","
<< "\n"; */
}
void GazeboMotorModel::VelocityCallback(CommandMotorSpeedPtr &rot_velocities) {
if(rot_velocities->motor_speed_size() < motor_number_) {
std::cout << "You tried to access index " << motor_number_
<< " of the MotorSpeed message array which is of size " << rot_velocities->motor_speed_size() << "." << std::endl;
} else ref_motor_rot_vel_ = std::min(static_cast<double>(rot_velocities->motor_speed(motor_number_)), static_cast<double>(max_rot_velocity_));
}
