void GazeboMavlinkInterface::ImuCallback(ImuPtr& imu_message) {
math::Pose T_W_I = model_->GetWorldPose();
math::Vector3 pos_W_I = T_W_I.pos; // Use the models'world position for GPS and pressure alt.
math::Quaternion C_W_I;
C_W_I.w = imu_message->orientation().w();
C_W_I.x = imu_message->orientation().x();
C_W_I.y = imu_message->orientation().y();
C_W_I.z = imu_message->orientation().z();
math::Vector3 mag_I = C_W_I.RotateVectorReverse(mag_W_); // TODO: Add noise based on bais and variance like for imu and gyro
math::Vector3 body_vel = C_W_I.RotateVectorReverse(model_->GetWorldLinearVel());
standard_normal_distribution_ = std::normal_distribution<float>(0, 0.01f);
float mag_noise = standard_normal_distribution_(random_generator_);
if(use_mavlink_udp){
mavlink_hil_sensor_t sensor_msg;
sensor_msg.time_usec = world_->GetSimTime().nsec*1000;
sensor_msg.xacc = imu_message->linear_acceleration().x();
sensor_msg.yacc = imu_message->linear_acceleration().y();
sensor_msg.zacc = imu_message->linear_acceleration().z();
sensor_msg.xgyro = imu_message->angular_velocity().x();
sensor_msg.ygyro = imu_message->angular_velocity().y();
sensor_msg.zgyro = imu_message->angular_velocity().z();
sensor_msg.xmag = mag_I.x + mag_noise;
sensor_msg.ymag = mag_I.y + mag_noise;
sensor_msg.zmag = mag_I.z + mag_noise;
sensor_msg.abs_pressure = 0.0;
sensor_msg.diff_pressure = 0.5*1.2754*body_vel.x*body_vel.x;
sensor_msg.pressure_alt = pos_W_I.z;
sensor_msg.temperature = 0.0;
sensor_msg.fields_updated = 4095;
send_mavlink_message(MAVLINK_MSG_ID_HIL_SENSOR, &sensor_msg, 200);
} else{
hil_sensor_msg_.set_time_usec(world_->GetSimTime().nsec*1000);
hil_sensor_msg_.set_xacc(imu_message->linear_acceleration().x());
hil_sensor_msg_.set_yacc(imu_message->linear_acceleration().y());
hil_sensor_msg_.set_zacc(imu_message->linear_acceleration().z());
hil_sensor_msg_.set_xgyro(imu_message->angular_velocity().x());
hil_sensor_msg_.set_ygyro(imu_message->angular_velocity().y());
hil_sensor_msg_.set_zgyro(imu_message->angular_velocity().z());
hil_sensor_msg_.set_xmag(mag_I.x);
hil_sensor_msg_.set_ymag(mag_I.y);
hil_sensor_msg_.set_zmag(mag_I.z);
hil_sensor_msg_.set_abs_pressure(0.0);
hil_sensor_msg_.set_diff_pressure(0.5*1.2754*body_vel.x*body_vel.x);
hil_sensor_msg_.set_pressure_alt(pos_W_I.z);
hil_sensor_msg_.set_temperature(0.0);
hil_sensor_msg_.set_fields_updated(4095); // 0b1111111111111 (All updated since new data with new noise added always)
hil_sensor_pub_->Publish(hil_sensor_msg_);
}
}
void GazeboMavlinkInterface::ImuCallback(ImuPtr& imu_message) {
math::Pose T_W_I = model_->GetWorldPose();
math::Vector3 pos_W_I = T_W_I.pos; // Use the models'world position for GPS and pressure alt.
math::Quaternion C_W_I;
C_W_I.w = imu_message->orientation().w();
C_W_I.x = imu_message->orientation().x();
C_W_I.y = imu_message->orientation().y();
C_W_I.z = imu_message->orientation().z();
// gzerr << "got imu: " << C_W_I << "\n";
float declination = get_mag_declination(lat_rad, lon_rad);
math::Quaternion C_D_I(0.0, 0.0, declination);
math::Vector3 mag_decl = C_D_I.RotateVectorReverse(mag_W_);
// TODO replace mag_W_ in the line below with mag_decl
math::Vector3 mag_I = C_W_I.RotateVectorReverse(mag_decl); // TODO: Add noise based on bais and variance like for imu and gyro
math::Vector3 body_vel = C_W_I.RotateVectorReverse(model_->GetWorldLinearVel());
standard_normal_distribution_ = std::normal_distribution<float>(0, 0.01f);
float mag_noise = standard_normal_distribution_(random_generator_);
mavlink_hil_sensor_t sensor_msg;
sensor_msg.time_usec = world_->GetSimTime().nsec*1000;
sensor_msg.xacc = imu_message->linear_acceleration().x();
sensor_msg.yacc = imu_message->linear_acceleration().y();
sensor_msg.zacc = imu_message->linear_acceleration().z();
sensor_msg.xgyro = imu_message->angular_velocity().x();
sensor_msg.ygyro = imu_message->angular_velocity().y();
sensor_msg.zgyro = imu_message->angular_velocity().z();
sensor_msg.xmag = mag_I.x + mag_noise;
sensor_msg.ymag = mag_I.y + mag_noise;
sensor_msg.zmag = mag_I.z + mag_noise;
sensor_msg.abs_pressure = 0.0;
sensor_msg.diff_pressure = 0.5*1.2754*(body_vel.z + body_vel.x)*(body_vel.z + body_vel.x) / 100;
sensor_msg.pressure_alt = pos_W_I.z;
sensor_msg.temperature = 0.0;
sensor_msg.fields_updated = 4095;
//gyro needed for optical flow message
optflow_xgyro = imu_message->angular_velocity().x();
optflow_ygyro = imu_message->angular_velocity().y();
optflow_zgyro = imu_message->angular_velocity().z();
send_mavlink_message(MAVLINK_MSG_ID_HIL_SENSOR, &sensor_msg, 200);
}
void GazeboMavlinkInterface::OpticalFlowCallback(OpticalFlowPtr& opticalFlow_message) {
mavlink_hil_optical_flow_t sensor_msg;
sensor_msg.time_usec = opticalFlow_message->time_usec();
sensor_msg.sensor_id = opticalFlow_message->sensor_id();
sensor_msg.integration_time_us = opticalFlow_message->integration_time_us();
sensor_msg.integrated_x = opticalFlow_message->integrated_x();
sensor_msg.integrated_y = opticalFlow_message->integrated_y();
sensor_msg.integrated_xgyro = optflow_ygyro * opticalFlow_message->integration_time_us() / 1000000.0; //xy switched
sensor_msg.integrated_ygyro = optflow_xgyro * opticalFlow_message->integration_time_us() / 1000000.0; //xy switched
sensor_msg.integrated_zgyro = -optflow_zgyro * opticalFlow_message->integration_time_us() / 1000000.0; //change direction
sensor_msg.temperature = opticalFlow_message->temperature();
sensor_msg.quality = opticalFlow_message->quality();
sensor_msg.time_delta_distance_us = opticalFlow_message->time_delta_distance_us();
sensor_msg.distance = optflow_distance;
send_mavlink_message(MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, &sensor_msg, 200);
}
void GazeboMavlinkInterface::LidarCallback(LidarPtr& lidar_message) {
mavlink_distance_sensor_t sensor_msg;
sensor_msg.time_boot_ms = lidar_message->time_msec();
sensor_msg.min_distance = lidar_message->min_distance() * 100.0;
sensor_msg.max_distance = lidar_message->max_distance() * 100.0;
sensor_msg.current_distance = lidar_message->current_distance() * 100.0;
sensor_msg.type = 0;
sensor_msg.id = 0;
sensor_msg.orientation = 0;
sensor_msg.covariance = 0;
//distance needed for optical flow message
optflow_distance = lidar_message->current_distance(); //[m]
send_mavlink_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &sensor_msg, 200);
}
// This gets called by the world update start event.
void GazeboMavlinkInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {
pollForMAVLinkMessages();
common::Time now = world_->GetSimTime();
if(received_first_referenc_) {
mav_msgs::msgs::CommandMotorSpeed turning_velocities_msg;
for (int i = 0; i < input_reference_.size(); i++){
if (last_actuator_time_ == 0 || (now - last_actuator_time_).Double() > 0.2) {
turning_velocities_msg.add_motor_speed(0);
} else {
turning_velocities_msg.add_motor_speed(input_reference_[i]);
}
}
// TODO Add timestamp and Header
// turning_velocities_msg->header.stamp.sec = now.sec;
// turning_velocities_msg->header.stamp.nsec = now.nsec;
// gzerr << turning_velocities_msg.motor_speed(0) << "\n";
motor_velocity_reference_pub_->Publish(turning_velocities_msg);
}
//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;
// Set global reference point
// Zurich Irchel Park: 47.397742, 8.545594, 488m
// Seattle downtown (15 deg declination): 47.592182, -122.316031, 86m
// Moscow downtown: 55.753395, 37.625427, 155m
// TODO: Remove GPS message from IMU plugin. Added gazebo GPS plugin. This is temp here.
// Zurich Irchel Park
const double lat_zurich = 47.397742 * M_PI / 180; // rad
const double lon_zurich = 8.545594 * M_PI / 180; // rad
const double alt_zurich = 488.0; // meters
// Seattle downtown (15 deg declination): 47.592182, -122.316031
// const double lat_zurich = 47.592182 * M_PI / 180; // rad
// const double lon_zurich = -122.316031 * M_PI / 180; // rad
// const double alt_zurich = 86.0; // meters
const 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);
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;
}
if(current_time.Double() - last_gps_time_.Double() > gps_update_interval_){ // 5Hz
if(use_mavlink_udp){
// Raw UDP mavlink
mavlink_hil_gps_t hil_gps_msg;
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 + alt_zurich) * 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;
send_mavlink_message(MAVLINK_MSG_ID_HIL_GPS, &hil_gps_msg, 200);
} else{
// Send via protobuf
hil_gps_msg_.set_time_usec(current_time.nsec*1000);
hil_gps_msg_.set_fix_type(3);
hil_gps_msg_.set_lat(lat_rad * 180 / M_PI * 1e7);
hil_gps_msg_.set_lon(lon_rad * 180 / M_PI * 1e7);
hil_gps_msg_.set_alt((pos_W_I.z + alt_zurich) * 1000);
hil_gps_msg_.set_eph(100);
hil_gps_msg_.set_epv(100);
hil_gps_msg_.set_vel(velocity_current_W_xy.GetLength() * 100);
hil_gps_msg_.set_vn(velocity_current_W.x * 100);
hil_gps_msg_.set_ve(-velocity_current_W.y * 100);
hil_gps_msg_.set_vd(-velocity_current_W.z * 100);
hil_gps_msg_.set_cog(atan2(-velocity_current_W.y * 100, velocity_current_W.x * 100) * 180.0/3.1416 * 100.0);
hil_gps_msg_.set_satellites_visible(10);
hil_gps_pub_->Publish(hil_gps_msg_);
}
last_gps_time_ = current_time;
}
}
// This gets called by the world update start event.
void GazeboMavlinkInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {
pollForMAVLinkMessages();
common::Time now = world_->GetSimTime();
if(received_first_referenc_) {
mav_msgs::msgs::CommandMotorSpeed turning_velocities_msg;
for (int i = 0; i < input_reference_.size(); i++){
turning_velocities_msg.add_motor_speed(input_reference_[i]);
}
// TODO Add timestamp and Header
// turning_velocities_msg->header.stamp.sec = now.sec;
// turning_velocities_msg->header.stamp.nsec = now.nsec;
motor_velocity_reference_pub_->Publish(turning_velocities_msg);
}
//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;
// Set default global reference point
// Zurich Irchel Park: 47.397742, 8.545594, 488m
const double lat_default = 47.397742 * M_PI / 180; // rad
const double lon_default = 8.545594 * M_PI / 180; // rad
const double alt_default = 488.0; // meters
const float radius_default = 6353000; // m
double lat_world = lat_default;
double lon_world = lon_default;
double alt_world = alt_default;
double radius_world = radius_default;
common::SphericalCoordinatesPtr spherical_coordinates = world_->GetSphericalCoordinates();
if (spherical_coordinates) {
lat_world = spherical_coordinates->LatitudeReference().Radian(); // rad
lon_world = spherical_coordinates->LongitudeReference().Radian(); // rad
alt_world = spherical_coordinates->GetElevationReference(); // m
switch (spherical_coordinates->GetSurfaceType()) {
case (common::SphericalCoordinates::EARTH_WGS84): {
radius_world = 6353000; // m
break;
}
}
}
// reproject local position to gps coordinates
double x_rad = pos_W_I.x / radius_world;
double y_rad = -pos_W_I.y / radius_world;
double c = sqrt(x_rad * x_rad + y_rad * y_rad);
double sin_c = sin(c);
double cos_c = cos(c);
if (c != 0.0) {
lat_rad = asin(cos_c * sin(lat_world) + (x_rad * sin_c * cos(lat_world)) / c);
lon_rad = (lon_world + atan2(y_rad * sin_c, c * cos(lat_world) * cos_c - x_rad * sin(lat_world) * sin_c));
} else {
lat_rad = lat_world;
lon_rad = lon_world;
}
if(current_time.Double() - last_gps_time_.Double() > gps_update_interval_){ // 5Hz
if(use_mavlink_udp){
// Raw UDP mavlink
mavlink_hil_gps_t hil_gps_msg;
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 + alt_world) * 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;
send_mavlink_message(MAVLINK_MSG_ID_HIL_GPS, &hil_gps_msg, 200);
} else{
// Send via protobuf
hil_gps_msg_.set_time_usec(current_time.nsec*1000);
hil_gps_msg_.set_fix_type(3);
hil_gps_msg_.set_lat(lat_rad * 180 / M_PI * 1e7);
hil_gps_msg_.set_lon(lon_rad * 180 / M_PI * 1e7);
hil_gps_msg_.set_alt((pos_W_I.z + alt_world) * 1000);
hil_gps_msg_.set_eph(100);
hil_gps_msg_.set_epv(100);
hil_gps_msg_.set_vel(velocity_current_W_xy.GetLength() * 100);
hil_gps_msg_.set_vn(velocity_current_W.x * 100);
hil_gps_msg_.set_ve(-velocity_current_W.y * 100);
hil_gps_msg_.set_vd(-velocity_current_W.z * 100);
hil_gps_msg_.set_cog(atan2(-velocity_current_W.y * 100, velocity_current_W.x * 100) * 180.0/3.1416 * 100.0);
hil_gps_msg_.set_satellites_visible(10);
hil_gps_pub_->Publish(hil_gps_msg_);
}
last_gps_time_ = current_time;
}
}
// This gets called by the world update start event.
void GazeboMavlinkInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {
pollForMAVLinkMessages();
if(!received_first_referenc_)
return;
common::Time now = world_->GetSimTime();
mav_msgs::msgs::CommandMotorSpeed turning_velocities_msg;
for (int i = 0; i < input_reference_.size(); i++){
turning_velocities_msg.add_motor_speed(input_reference_[i]);
}
// TODO Add timestamp and Header
// turning_velocities_msg->header.stamp.sec = now.sec;
// turning_velocities_msg->header.stamp.nsec = now.nsec;
motor_velocity_reference_pub_->Publish(turning_velocities_msg);
//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
if(use_mavlink_udp){
// Raw UDP mavlink
mavlink_hil_gps_t hil_gps_msg;
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;
send_mavlink_message(MAVLINK_MSG_ID_HIL_GPS, &hil_gps_msg, 200);
} else{
// Send via protobuf
hil_gps_msg_.set_time_usec(current_time.nsec*1000);
hil_gps_msg_.set_fix_type(3);
hil_gps_msg_.set_lat(lat_rad * 180 / M_PI * 1e7);
hil_gps_msg_.set_lon(lon_rad * 180 / M_PI * 1e7);
hil_gps_msg_.set_alt(pos_W_I.z * 1000);
hil_gps_msg_.set_eph(100);
hil_gps_msg_.set_epv(100);
hil_gps_msg_.set_vel(velocity_current_W_xy.GetLength() * 100);
hil_gps_msg_.set_vn(velocity_current_W.x * 100);
hil_gps_msg_.set_ve(-velocity_current_W.y * 100);
hil_gps_msg_.set_vd(-velocity_current_W.z * 100);
hil_gps_msg_.set_cog(atan2(-velocity_current_W.y * 100, velocity_current_W.x * 100) * 180.0/3.1416 * 100.0);
hil_gps_msg_.set_satellites_visible(10);
hil_gps_pub_->Publish(hil_gps_msg_);
}
last_gps_time_ = current_time;
}
}
