Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "rotors_hil_interface/hil_interface.h"
00018
00019 namespace rotors_hil {
00020
00021 HilStateLevelInterface::HilStateLevelInterface(const Eigen::Quaterniond& q_S_B) {
00022 ros::NodeHandle pnh("~");
00023
00024
00025 std::string air_speed_sub_topic;
00026 std::string gps_sub_topic;
00027 std::string ground_speed_sub_topic;
00028 std::string imu_sub_topic;
00029
00030 pnh.param("air_speed_topic", air_speed_sub_topic, std::string(mav_msgs::default_topics::AIR_SPEED));
00031 pnh.param("gps_topic", gps_sub_topic, std::string(mav_msgs::default_topics::GPS));
00032 pnh.param("ground_speed_topic", ground_speed_sub_topic, std::string(mav_msgs::default_topics::GROUND_SPEED));
00033 pnh.param("imu_topic", imu_sub_topic, std::string(mav_msgs::default_topics::IMU));
00034
00035
00036 q_S_B_ = q_S_B;
00037 R_S_B_ = q_S_B_.matrix().cast<float>();
00038
00039
00040 air_speed_sub_ =
00041 nh_.subscribe<geometry_msgs::TwistStamped>(
00042 air_speed_sub_topic, 1, boost::bind(
00043 &HilListeners::AirSpeedCallback, &hil_listeners_, _1, &hil_data_));
00044
00045 gps_sub_ =
00046 nh_.subscribe<sensor_msgs::NavSatFix>(
00047 gps_sub_topic, 1, boost::bind(
00048 &HilListeners::GpsCallback, &hil_listeners_, _1, &hil_data_));
00049
00050 ground_speed_sub_ =
00051 nh_.subscribe<geometry_msgs::TwistStamped>(
00052 ground_speed_sub_topic, 1, boost::bind(
00053 &HilListeners::GroundSpeedCallback, &hil_listeners_, _1, &hil_data_));
00054
00055 imu_sub_ =
00056 nh_.subscribe<sensor_msgs::Imu>(
00057 imu_sub_topic, 1, boost::bind(
00058 &HilListeners::ImuCallback, &hil_listeners_, _1, &hil_data_));
00059 }
00060
00061 HilStateLevelInterface::~HilStateLevelInterface() {
00062 }
00063
00064 std::vector<mavros_msgs::Mavlink> HilStateLevelInterface::CollectData() {
00065 boost::mutex::scoped_lock lock(mtx_);
00066
00067 ros::Time current_time = ros::Time::now();
00068 uint64_t time_usec = RosTimeToMicroseconds(current_time);
00069
00070 mavlink_message_t mmsg;
00071 std::vector<mavros_msgs::Mavlink> hil_msgs;
00072
00073
00074 Eigen::Quaterniond att = q_S_B_ * hil_data_.att;
00075
00076
00077 Eigen::Vector3f gyro = R_S_B_ * hil_data_.gyro_rad_per_s;
00078 Eigen::Vector3f acc = R_S_B_ * hil_data_.acc_m_per_s2;
00079 Eigen::Vector3i gps_vel = (R_S_B_ * hil_data_.gps_vel_cm_per_s.cast<float>()).cast<int>();
00080
00081
00082 hil_state_qtrn_msg_.time_usec = time_usec;
00083 hil_state_qtrn_msg_.attitude_quaternion[0] = att.w();
00084 hil_state_qtrn_msg_.attitude_quaternion[1] = att.x();
00085 hil_state_qtrn_msg_.attitude_quaternion[2] = att.y();
00086 hil_state_qtrn_msg_.attitude_quaternion[3] = att.z();
00087 hil_state_qtrn_msg_.rollspeed = gyro.x();
00088 hil_state_qtrn_msg_.pitchspeed = gyro.y();
00089 hil_state_qtrn_msg_.yawspeed = gyro.z();
00090 hil_state_qtrn_msg_.lat = hil_data_.lat_1e7deg;
00091 hil_state_qtrn_msg_.lon = hil_data_.lon_1e7deg;
00092 hil_state_qtrn_msg_.alt = hil_data_.alt_mm;
00093 hil_state_qtrn_msg_.vx = gps_vel.x();
00094 hil_state_qtrn_msg_.vy = gps_vel.y();
00095 hil_state_qtrn_msg_.vz = gps_vel.z();
00096 hil_state_qtrn_msg_.ind_airspeed = hil_data_.ind_airspeed_1e2m_per_s;
00097 hil_state_qtrn_msg_.true_airspeed = hil_data_.true_airspeed_1e2m_per_s;
00098 hil_state_qtrn_msg_.xacc = acc.x() * kMetersToMm / kGravityMagnitude_m_per_s2;
00099 hil_state_qtrn_msg_.yacc = acc.y() * kMetersToMm / kGravityMagnitude_m_per_s2;
00100 hil_state_qtrn_msg_.zacc = acc.z() * kMetersToMm / kGravityMagnitude_m_per_s2;
00101
00102 mavlink_hil_state_quaternion_t* hil_state_qtrn_msg_ptr = &hil_state_qtrn_msg_;
00103 mavlink_msg_hil_state_quaternion_encode(1, 0, &mmsg, hil_state_qtrn_msg_ptr);
00104
00105 mavros_msgs::MavlinkPtr rmsg_hil_state_qtrn = boost::make_shared<mavros_msgs::Mavlink>();
00106 rmsg_hil_state_qtrn->header.stamp.sec = current_time.sec;
00107 rmsg_hil_state_qtrn->header.stamp.nsec = current_time.nsec;
00108 mavros_msgs::mavlink::convert(mmsg, *rmsg_hil_state_qtrn);
00109
00110 hil_msgs.push_back(*rmsg_hil_state_qtrn);
00111
00112 return hil_msgs;
00113 }
00114
00115 }