std::vector<mavros_msgs::Mavlink> HilSensorLevelInterface::CollectData() {
boost::mutex::scoped_lock lock(mtx_);
ros::Time current_time = ros::Time::now();
uint64_t time_usec = RosTimeToMicroseconds(current_time);
mavlink_message_t mmsg;
std::vector<mavros_msgs::Mavlink> hil_msgs;
// Rotate gyroscope, accelerometer, and magnetometer data into NED frame
Eigen::Vector3f gyro = R_S_B_ * hil_data_.gyro_rad_per_s;
Eigen::Vector3f acc = R_S_B_ * hil_data_.acc_m_per_s2;
Eigen::Vector3f mag = R_S_B_ * hil_data_.mag_G;
// Check if we need to publish a HIL_GPS message.
if ((current_time.nsec - last_gps_pub_time_nsec_) >= gps_interval_nsec_) {
last_gps_pub_time_nsec_ = current_time.nsec;
// Rotate ground speed data into NED frame
Eigen::Vector3i gps_vel = (R_S_B_ * hil_data_.gps_vel_cm_per_s.cast<float>()).cast<int>();
// Fill in a MAVLINK HIL_GPS message and convert it to MAVROS format.
hil_gps_msg_.time_usec = time_usec;
hil_gps_msg_.fix_type = hil_data_.fix_type;
hil_gps_msg_.lat = hil_data_.lat_1e7deg;
hil_gps_msg_.lon = hil_data_.lon_1e7deg;
hil_gps_msg_.alt = hil_data_.alt_mm;
hil_gps_msg_.eph = hil_data_.eph_cm;
hil_gps_msg_.epv = hil_data_.epv_cm;
hil_gps_msg_.vel = hil_data_.vel_1e2m_per_s;
hil_gps_msg_.vn = gps_vel.x();
hil_gps_msg_.ve = gps_vel.y();
hil_gps_msg_.vd = gps_vel.z();
hil_gps_msg_.cog = hil_data_.cog_1e2deg;
hil_gps_msg_.satellites_visible = hil_data_.satellites_visible;
mavlink_hil_gps_t* hil_gps_msg_ptr = &hil_gps_msg_;
mavlink_msg_hil_gps_encode(1, 0, &mmsg, hil_gps_msg_ptr);
mavros_msgs::MavlinkPtr rmsg_hil_gps = boost::make_shared<mavros_msgs::Mavlink>();
rmsg_hil_gps->header.stamp.sec = current_time.sec;
rmsg_hil_gps->header.stamp.nsec = current_time.nsec;
mavros_msgs::mavlink::convert(mmsg, *rmsg_hil_gps);
hil_msgs.push_back(*rmsg_hil_gps);
}
// Fill in a MAVLINK HIL_SENSOR message and convert it to MAVROS format.
hil_sensor_msg_.time_usec = time_usec;
hil_sensor_msg_.xacc = acc.x();
hil_sensor_msg_.yacc = acc.y();
hil_sensor_msg_.zacc = acc.z();
hil_sensor_msg_.xgyro = gyro.x();
hil_sensor_msg_.ygyro = gyro.y();
hil_sensor_msg_.zgyro = gyro.z();
hil_sensor_msg_.xmag = mag.x();
hil_sensor_msg_.ymag = mag.y();
hil_sensor_msg_.zmag = mag.z();
hil_sensor_msg_.abs_pressure = hil_data_.pressure_abs_mBar;
hil_sensor_msg_.diff_pressure = hil_data_.pressure_diff_mBar;
hil_sensor_msg_.pressure_alt = hil_data_.pressure_alt;
hil_sensor_msg_.temperature = hil_data_.temperature_degC;
hil_sensor_msg_.fields_updated = kAllFieldsUpdated;
mavlink_hil_sensor_t* hil_sensor_msg_ptr = &hil_sensor_msg_;
mavlink_msg_hil_sensor_encode(1, 0, &mmsg, hil_sensor_msg_ptr);
mavros_msgs::MavlinkPtr rmsg_hil_sensor = boost::make_shared<mavros_msgs::Mavlink>();
rmsg_hil_sensor->header.stamp.sec = current_time.sec;
rmsg_hil_sensor->header.stamp.nsec = current_time.nsec;
mavros_msgs::mavlink::convert(mmsg, *rmsg_hil_sensor);
hil_msgs.push_back(*rmsg_hil_sensor);
return hil_msgs;
}
// This gets called by the world update start event.
void GazeboMavlinkInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {
gzerr << "[gazebo_mavlink_interface] Please specify a robotNamespace.\n";
if(!received_first_referenc_)
return;
common::Time now = world_->GetSimTime();
mav_msgs::CommandMotorSpeedPtr turning_velocities_msg(new mav_msgs::CommandMotorSpeed);
for (int i = 0; i < input_reference_.size(); i++)
turning_velocities_msg->motor_speed.push_back(input_reference_[i]);
turning_velocities_msg->header.stamp.sec = now.sec;
turning_velocities_msg->header.stamp.nsec = now.nsec;
motor_velocity_reference_pub_.publish(turning_velocities_msg);
turning_velocities_msg.reset();
//send gps
common::Time current_time = now;
double dt = (current_time - last_time_).Double();
last_time_ = current_time;
double t = current_time.Double();
math::Pose T_W_I = model_->GetWorldPose(); //TODO(burrimi): Check tf.
math::Vector3 pos_W_I = T_W_I.pos; // Use the models' world position for GPS and pressure alt.
math::Vector3 velocity_current_W = model_->GetWorldLinearVel(); // Use the models' world position for GPS velocity.
math::Vector3 velocity_current_W_xy = velocity_current_W;
velocity_current_W_xy.z = 0.0;
// TODO: Remove GPS message from IMU plugin. Added gazebo GPS plugin. This is temp here.
float lat_zurich = 47.3667; // deg
float long_zurich = 8.5500; // deg
float earth_radius = 6353000; // m
common::Time gps_update(gps_update_interval_);
if(current_time - last_gps_time_ > gps_update){ // 5Hz
mavlink_message_t gps_mmsg;
hil_gps_msg_.time_usec = current_time.nsec*1000;
hil_gps_msg_.fix_type = 3;
hil_gps_msg_.lat = (lat_zurich + (pos_W_I.x/earth_radius)*180/3.1416) * 10000000;
hil_gps_msg_.lon = (long_zurich + (-pos_W_I.y/earth_radius)*180/3.1416) * 10000000;
hil_gps_msg_.alt = pos_W_I.z * 1000;
hil_gps_msg_.eph = 100;
hil_gps_msg_.epv = 100;
hil_gps_msg_.vel = velocity_current_W_xy.GetLength() * 100;
hil_gps_msg_.vn = velocity_current_W.x * 100;
hil_gps_msg_.ve = -velocity_current_W.y * 100;
hil_gps_msg_.vd = -velocity_current_W.z * 100;
hil_gps_msg_.cog = atan2(hil_gps_msg_.ve, hil_gps_msg_.vn) * 180.0/3.1416 * 100.0;
hil_gps_msg_.satellites_visible = 10;
mavlink_hil_gps_t* hil_gps_msg = &hil_gps_msg_;
mavlink_msg_hil_gps_encode(1, 240, &gps_mmsg, hil_gps_msg);
mavlink_message_t* gps_msg = &gps_mmsg;
mavros::MavlinkPtr gps_rmsg = boost::make_shared<mavros::Mavlink>();
gps_rmsg->header.stamp = ros::Time::now();
mavutils::copy_mavlink_to_ros(gps_msg, gps_rmsg);
hil_sensor_pub_.publish(gps_rmsg);
last_gps_time_ = current_time;
}
}
// This gets called by the world update start event.
void GazeboMavlinkInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {
if(!received_first_referenc_)
return;
common::Time now = world_->GetSimTime();
mav_msgs::ActuatorsPtr turning_velocities_msg(new mav_msgs::Actuators);
for (int i = 0; i < input_reference_.size(); i++)
turning_velocities_msg->angular_velocities.push_back(input_reference_[i]);
turning_velocities_msg->header.stamp.sec = now.sec;
turning_velocities_msg->header.stamp.nsec = now.nsec;
motor_velocity_reference_pub_.publish(turning_velocities_msg);
turning_velocities_msg.reset();
//send gps
common::Time current_time = now;
double dt = (current_time - last_time_).Double();
last_time_ = current_time;
double t = current_time.Double();
math::Pose T_W_I = model_->GetWorldPose(); //TODO(burrimi): Check tf.
math::Vector3 pos_W_I = T_W_I.pos; // Use the models' world position for GPS and pressure alt.
math::Vector3 velocity_current_W = model_->GetWorldLinearVel(); // Use the models' world position for GPS velocity.
math::Vector3 velocity_current_W_xy = velocity_current_W;
velocity_current_W_xy.z = 0.0;
// TODO: Remove GPS message from IMU plugin. Added gazebo GPS plugin. This is temp here.
double lat_zurich = 47.3667 * M_PI / 180 ; // rad
double lon_zurich = 8.5500 * M_PI / 180; // rad
float earth_radius = 6353000; // m
// reproject local position to gps coordinates
double x_rad = pos_W_I.x / earth_radius;
double y_rad = -pos_W_I.y / earth_radius;
double c = sqrt(x_rad * x_rad + y_rad * y_rad);
double sin_c = sin(c);
double cos_c = cos(c);
double lat_rad;
double lon_rad;
if (c != 0.0) {
lat_rad = asin(cos_c * sin(lat_zurich) + (x_rad * sin_c * cos(lat_zurich)) / c);
lon_rad = (lon_zurich + atan2(y_rad * sin_c, c * cos(lat_zurich) * cos_c - x_rad * sin(lat_zurich) * sin_c));
} else {
lat_rad = lat_zurich;
lon_rad = lon_zurich;
}
common::Time gps_update(gps_update_interval_);
if(current_time - last_gps_time_ > gps_update){ // 5Hz
mavlink_message_t gps_mmsg;
hil_gps_msg_.time_usec = current_time.nsec*1000;
hil_gps_msg_.fix_type = 3;
hil_gps_msg_.lat = lat_rad * 180 / M_PI * 1e7;
hil_gps_msg_.lon = lon_rad * 180 / M_PI * 1e7;
hil_gps_msg_.alt = pos_W_I.z * 1000;
hil_gps_msg_.eph = 100;
hil_gps_msg_.epv = 100;
hil_gps_msg_.vel = velocity_current_W_xy.GetLength() * 100;
hil_gps_msg_.vn = velocity_current_W.x * 100;
hil_gps_msg_.ve = -velocity_current_W.y * 100;
hil_gps_msg_.vd = -velocity_current_W.z * 100;
hil_gps_msg_.cog = atan2(hil_gps_msg_.ve, hil_gps_msg_.vn) * 180.0/3.1416 * 100.0;
hil_gps_msg_.satellites_visible = 10;
mavlink_hil_gps_t* hil_gps_msg = &hil_gps_msg_;
mavlink_msg_hil_gps_encode(1, 240, &gps_mmsg, hil_gps_msg);
mavlink_message_t* gps_msg = &gps_mmsg;
mavros_msgs::MavlinkPtr gps_rmsg = boost::make_shared<mavros_msgs::Mavlink>();
gps_rmsg->header.stamp = ros::Time::now();
mavros_msgs::mavlink::convert(*gps_msg, *gps_rmsg);
hil_sensor_pub_.publish(gps_rmsg);
last_gps_time_ = current_time;
}
}
