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);
}
void GazeboMavlinkInterface::MavlinkControlCallback(const mavros::Mavlink::ConstPtr &rmsg) {
mavlink_message_t mmsg;
if(mavutils::copy_ros_to_mavlink(rmsg, mmsg)){
mavlink_hil_controls_t act_msg;
mavlink_message_t* msg = &mmsg;
mavlink_msg_hil_controls_decode(msg, &act_msg);
inputs.control[0] =(double)act_msg.roll_ailerons;
inputs.control[1] =(double)act_msg.pitch_elevator;
inputs.control[2] =(double)act_msg.yaw_rudder;
inputs.control[3] =(double)act_msg.throttle;
inputs.control[4] =(double)act_msg.aux1;
inputs.control[5] =(double)act_msg.aux2;
inputs.control[6] =(double)act_msg.aux3;
inputs.control[7] =(double)act_msg.aux4;
// publish message
double scaling = 150;
double offset = 600;
mav_msgs::CommandMotorSpeedPtr turning_velocities_msg(new mav_msgs::CommandMotorSpeed);
for (int i = 0; i < _rotor_count; i++) {
turning_velocities_msg->motor_speed.push_back(inputs.control[i] * scaling + offset);
}
CommandMotorMavros(turning_velocities_msg);
turning_velocities_msg.reset();
} else{
std::cout << "incorrect mavlink data" <<"\n";
}
}
// 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;
}
}
