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); }