void LeePositionControllerNode::OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg) {
ROS_INFO_ONCE("LeePositionController got first odometry message.");
EigenOdometry odometry;
eigenOdometryFromMsg(odometry_msg, &odometry);
lee_position_controller_.SetOdometry(odometry);
Eigen::VectorXd ref_rotor_velocities;
lee_position_controller_.CalculateRotorVelocities(&ref_rotor_velocities);
// Todo(ffurrer): Do this in the conversions header.
mav_msgs::ActuatorsPtr actuator_msg(new mav_msgs::Actuators);
actuator_msg->angular_velocities.clear();
for (int i = 0; i < ref_rotor_velocities.size(); i++)
actuator_msg->angular_velocities.push_back(ref_rotor_velocities[i]);
actuator_msg->header.stamp = odometry_msg->header.stamp;
motor_velocity_reference_pub_.publish(actuator_msg);
}
void RollPitchYawrateThrustControllerNode::OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg) {
ROS_INFO_ONCE("RollPitchYawrateThrustController got first odometry message.");
EigenOdometry odometry;
eigenOdometryFromMsg(odometry_msg, &odometry);
roll_pitch_yawrate_thrust_controller_.SetOdometry(odometry);
Eigen::VectorXd ref_rotor_velocities;
roll_pitch_yawrate_thrust_controller_.CalculateRotorVelocities(&ref_rotor_velocities);
// Todo(ffurrer): Do this in the conversions header.
mav_msgs::CommandMotorSpeedPtr turning_velocities_msg(new mav_msgs::CommandMotorSpeed);
turning_velocities_msg->motor_speed.clear();
for (int i = 0; i < ref_rotor_velocities.size(); i++)
turning_velocities_msg->motor_speed.push_back(ref_rotor_velocities[i]);
turning_velocities_msg->header.stamp = odometry_msg->header.stamp;
motor_velocity_reference_pub_.publish(turning_velocities_msg);
}