Exemplos de eigenOdometryFromMsg em C++ (Cpp)

Exemplo n.º 1

Arquivo: lee_position_controller_node.cpp Projeto: jenke-gpfw/rotors_simulator

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

Exemplo n.º 2

Arquivo: roll_pitch_yawrate_thrust_controller_node.cpp Projeto: Aevena/rotors_simulator

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