00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "rotors_gazebo_plugins/gazebo_mavlink_interface.h"
00024
00025
00026 #include "rotors_gazebo_plugins/geo_mag_declination.h"
00027
00028 namespace gazebo {
00029
00030
00031
00032
00033
00034
00035
00036 static const double kLatZurich_rad = 47.397742 * M_PI / 180;
00037 static const double kLonZurich_rad = 8.545594 * M_PI / 180;
00038 static const double kAltZurich_m = 488.0;
00039
00040
00041
00042
00043 static const float kEarthRadius_m = 6353000;
00044
00045
00046 GZ_REGISTER_MODEL_PLUGIN(GazeboMavlinkInterface);
00047
00048 GazeboMavlinkInterface::~GazeboMavlinkInterface() {
00049 }
00050
00051 void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
00052
00053 if(kPrintOnPluginLoad) {
00054 gzdbg << __FUNCTION__ << "() called." << std::endl;
00055 }
00056
00057
00058 model_ = _model;
00059
00060 world_ = model_->GetWorld();
00061
00062 namespace_.clear();
00063 if (_sdf->HasElement("robotNamespace")) {
00064 namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
00065 gzdbg << "namespace_ = \"" << namespace_ << "\"." << std::endl;
00066 } else {
00067 gzerr << "[gazebo_mavlink_interface] Please specify a robotNamespace.\n";
00068 }
00069
00070 node_handle_ = transport::NodePtr(new transport::Node());
00071 node_handle_->Init();
00072
00073 getSdfParam<std::string>(_sdf, "motorSpeedCommandPubTopic", motor_velocity_reference_pub_topic_,
00074 motor_velocity_reference_pub_topic_);
00075 gzdbg << "motorSpeedCommandPubTopic = \"" << motor_velocity_reference_pub_topic_ << "\"." << std::endl;
00076
00077 getSdfParam<std::string>(_sdf, "imuSubTopic", imu_sub_topic_, imu_sub_topic_);
00078 getSdfParam<std::string>(_sdf, "lidarSubTopic", lidar_sub_topic_, lidar_sub_topic_);
00079 getSdfParam<std::string>(_sdf, "opticalFlowSubTopic",
00080 opticalFlow_sub_topic_, opticalFlow_sub_topic_);
00081
00082
00083 input_reference_.resize(kNOutMax);
00084 joints_.resize(kNOutMax);
00085 pids_.resize(kNOutMax);
00086 for(int i = 0; i < kNOutMax; ++i)
00087 {
00088 pids_[i].Init(0, 0, 0, 0, 0, 0, 0);
00089 input_reference_[i] = 0;
00090 }
00091
00092 if (_sdf->HasElement("control_channels")) {
00093 sdf::ElementPtr control_channels = _sdf->GetElement("control_channels");
00094 sdf::ElementPtr channel = control_channels->GetElement("channel");
00095 while (channel)
00096 {
00097 if (channel->HasElement("input_index"))
00098 {
00099 int index = channel->Get<int>("input_index");
00100 if (index < kNOutMax)
00101 {
00102 input_offset_[index] = channel->Get<double>("input_offset");
00103 input_scaling_[index] = channel->Get<double>("input_scaling");
00104 zero_position_disarmed_[index] = channel->Get<double>("zero_position_disarmed");
00105 zero_position_armed_[index] = channel->Get<double>("zero_position_armed");
00106 if (channel->HasElement("joint_control_type"))
00107 {
00108 joint_control_type_[index] = channel->Get<std::string>("joint_control_type");
00109 }
00110 else
00111 {
00112 gzwarn << "joint_control_type[" << index << "] not specified, using velocity.\n";
00113 joint_control_type_[index] = "velocity";
00114 }
00115
00116
00117 if (joint_control_type_[index] == "position_gztopic")
00118 {
00119
00120 if (channel->HasElement("gztopic"))
00121 gztopic_[index] = "~/"+ model_->GetName() + channel->Get<std::string>("gztopic");
00122 else
00123 gztopic_[index] = "control_position_gztopic_" + std::to_string(index);
00124 #if (GAZEBO_MAJOR_VERSION >= 7 && GAZEBO_MINOR_VERSION >= 4) || GAZEBO_MAJOR_VERSION >= 8
00125
00126 joint_control_pub_[index] = node_handle_->Advertise<gazebo::msgs::Any>(
00127 gztopic_[index]);
00128 #else
00129 joint_control_pub_[index] = node_handle_->Advertise<gazebo::msgs::GzString>(
00130 gztopic_[index]);
00131 #endif
00132 }
00133
00134 if (channel->HasElement("joint_name"))
00135 {
00136 std::string joint_name = channel->Get<std::string>("joint_name");
00137 joints_[index] = model_->GetJoint(joint_name);
00138 if (joints_[index] == nullptr)
00139 {
00140 gzwarn << "joint [" << joint_name << "] not found for channel["
00141 << index << "] no joint control for this channel.\n";
00142 }
00143 else
00144 {
00145 gzdbg << "joint [" << joint_name << "] found for channel["
00146 << index << "] joint control active for this channel.\n";
00147 }
00148 }
00149 else
00150 {
00151 gzdbg << "<joint_name> not found for channel[" << index
00152 << "] no joint control will be performed for this channel.\n";
00153 }
00154
00155
00156 if (channel->HasElement("joint_control_pid"))
00157 {
00158 sdf::ElementPtr pid = channel->GetElement("joint_control_pid");
00159 double p = 0;
00160 if (pid->HasElement("p"))
00161 p = pid->Get<double>("p");
00162 double i = 0;
00163 if (pid->HasElement("i"))
00164 i = pid->Get<double>("i");
00165 double d = 0;
00166 if (pid->HasElement("d"))
00167 d = pid->Get<double>("d");
00168 double iMax = 0;
00169 if (pid->HasElement("iMax"))
00170 iMax = pid->Get<double>("iMax");
00171 double iMin = 0;
00172 if (pid->HasElement("iMin"))
00173 iMin = pid->Get<double>("iMin");
00174 double cmdMax = 0;
00175 if (pid->HasElement("cmdMax"))
00176 cmdMax = pid->Get<double>("cmdMax");
00177 double cmdMin = 0;
00178 if (pid->HasElement("cmdMin"))
00179 cmdMin = pid->Get<double>("cmdMin");
00180 pids_[index].Init(p, i, d, iMax, iMin, cmdMax, cmdMin);
00181 }
00182 }
00183 else
00184 {
00185 gzerr << "input_index[" << index << "] out of range, not parsing.\n";
00186 }
00187 }
00188 else
00189 {
00190 gzerr << "no input_index, not parsing.\n";
00191 break;
00192 }
00193 channel = channel->GetNextElement("channel");
00194 }
00195 }
00196
00197 if (_sdf->HasElement("left_elevon_joint")) {
00198 sdf::ElementPtr left_elevon = _sdf->GetElement("left_elevon_joint");
00199 std::string left_elevon_joint_name = left_elevon->Get<std::string>();
00200 left_elevon_joint_ = model_->GetJoint(left_elevon_joint_name);
00201 int control_index;
00202 getSdfParam<int>(left_elevon, "input_index", control_index, -1);
00203 if (control_index >= 0) {
00204 joints_.at(control_index) = left_elevon_joint_;
00205 }
00206
00207 use_left_elevon_pid_ = false;
00208 if (left_elevon->HasElement("joint_control_pid")) {
00209
00210 sdf::ElementPtr pid = _sdf->GetElement("joint_control_pid");
00211 double p = 0.1;
00212 if (pid->HasElement("p"))
00213 p = pid->Get<double>("p");
00214 double i = 0;
00215 if (pid->HasElement("i"))
00216 i = pid->Get<double>("i");
00217 double d = 0;
00218 if (pid->HasElement("d"))
00219 d = pid->Get<double>("d");
00220 double iMax = 0;
00221 if (pid->HasElement("iMax"))
00222 iMax = pid->Get<double>("iMax");
00223 double iMin = 0;
00224 if (pid->HasElement("iMin"))
00225 iMin = pid->Get<double>("iMin");
00226 double cmdMax = 3;
00227 if (pid->HasElement("cmdMax"))
00228 cmdMax = pid->Get<double>("cmdMax");
00229 double cmdMin = -3;
00230 if (pid->HasElement("cmdMin"))
00231 cmdMin = pid->Get<double>("cmdMin");
00232 left_elevon_pid_.Init(p, i, d, iMax, iMin, cmdMax, cmdMin);
00233 use_left_elevon_pid_ = true;
00234 }
00235 }
00236
00237 if (_sdf->HasElement("left_aileron_joint")) {
00238 std::string left_elevon_joint_name = _sdf->GetElement("left_aileron_joint")->Get<std::string>();
00239 left_elevon_joint_ = model_->GetJoint(left_elevon_joint_name);
00240 int control_index;
00241 getSdfParam<int>(_sdf->GetElement("left_aileron_joint"), "input_index", control_index, -1);
00242 if (control_index >= 0) {
00243 joints_.at(control_index) = left_elevon_joint_;
00244 }
00245 }
00246
00247 if (_sdf->HasElement("right_elevon_joint")) {
00248 sdf::ElementPtr right_elevon = _sdf->GetElement("right_elevon_joint");
00249 std::string right_elevon_joint_name = right_elevon->Get<std::string>();
00250 right_elevon_joint_ = model_->GetJoint(right_elevon_joint_name);
00251 int control_index;
00252 getSdfParam<int>(right_elevon, "input_index", control_index, -1);
00253 if (control_index >= 0) {
00254 joints_.at(control_index) = right_elevon_joint_;
00255 }
00256
00257 use_right_elevon_pid_ = false;
00258 if (right_elevon->HasElement("joint_control_pid")) {
00259
00260 sdf::ElementPtr pid = _sdf->GetElement("joint_control_pid");
00261 double p = 0.1;
00262 if (pid->HasElement("p"))
00263 p = pid->Get<double>("p");
00264 double i = 0;
00265 if (pid->HasElement("i"))
00266 i = pid->Get<double>("i");
00267 double d = 0;
00268 if (pid->HasElement("d"))
00269 d = pid->Get<double>("d");
00270 double iMax = 0;
00271 if (pid->HasElement("iMax"))
00272 iMax = pid->Get<double>("iMax");
00273 double iMin = 0;
00274 if (pid->HasElement("iMin"))
00275 iMin = pid->Get<double>("iMin");
00276 double cmdMax = 3;
00277 if (pid->HasElement("cmdMax"))
00278 cmdMax = pid->Get<double>("cmdMax");
00279 double cmdMin = -3;
00280 if (pid->HasElement("cmdMin"))
00281 cmdMin = pid->Get<double>("cmdMin");
00282 right_elevon_pid_.Init(p, i, d, iMax, iMin, cmdMax, cmdMin);
00283 use_right_elevon_pid_ = true;
00284 }
00285 }
00286
00287 if (_sdf->HasElement("right_aileron_joint")) {
00288 std::string right_elevon_joint_name = _sdf->GetElement("right_aileron_joint")->Get<std::string>();
00289 right_elevon_joint_ = model_->GetJoint(right_elevon_joint_name);
00290 int control_index;
00291 getSdfParam<int>(_sdf->GetElement("right_aileron_joint"), "input_index", control_index, -1);
00292 if (control_index >= 0) {
00293 joints_.at(control_index) = right_elevon_joint_;
00294 }
00295 }
00296
00297 if (_sdf->HasElement("elevator_joint")) {
00298 sdf::ElementPtr elevator = _sdf->GetElement("elevator_joint");
00299 std::string elevator_joint_name = elevator->Get<std::string>();
00300 elevator_joint_ = model_->GetJoint(elevator_joint_name);
00301 int control_index;
00302 getSdfParam<int>(elevator, "input_index", control_index, -1);
00303 if (control_index >= 0) {
00304 joints_.at(control_index) = elevator_joint_;
00305 }
00306
00307 use_elevator_pid_ = false;
00308 if (elevator->HasElement("joint_control_pid")) {
00309
00310 sdf::ElementPtr pid = _sdf->GetElement("joint_control_pid");
00311 double p = 0.1;
00312 if (pid->HasElement("p"))
00313 p = pid->Get<double>("p");
00314 double i = 0;
00315 if (pid->HasElement("i"))
00316 i = pid->Get<double>("i");
00317 double d = 0;
00318 if (pid->HasElement("d"))
00319 d = pid->Get<double>("d");
00320 double iMax = 0;
00321 if (pid->HasElement("iMax"))
00322 iMax = pid->Get<double>("iMax");
00323 double iMin = 0;
00324 if (pid->HasElement("iMin"))
00325 iMin = pid->Get<double>("iMin");
00326 double cmdMax = 3;
00327 if (pid->HasElement("cmdMax"))
00328 cmdMax = pid->Get<double>("cmdMax");
00329 double cmdMin = -3;
00330 if (pid->HasElement("cmdMin"))
00331 cmdMin = pid->Get<double>("cmdMin");
00332 elevator_pid_.Init(p, i, d, iMax, iMin, cmdMax, cmdMin);
00333 use_elevator_pid_ = true;
00334 }
00335 }
00336
00337 if (_sdf->HasElement("propeller_joint")) {
00338 sdf::ElementPtr propeller = _sdf->GetElement("propeller_joint");
00339 std::string propeller_joint_name = propeller->Get<std::string>();
00340 propeller_joint_ = model_->GetJoint(propeller_joint_name);
00341 int control_index;
00342 getSdfParam<int>(propeller, "input_index", control_index, -1);
00343 if (control_index >= 0) {
00344 joints_.at(control_index) = propeller_joint_;
00345 }
00346 use_propeller_pid_ = false;
00347
00348 if (propeller->HasElement("joint_control_pid"))
00349 {
00350 sdf::ElementPtr pid = _sdf->GetElement("joint_control_pid");
00351 double p = 0.1;
00352 if (pid->HasElement("p"))
00353 p = pid->Get<double>("p");
00354 double i = 0;
00355 if (pid->HasElement("i"))
00356 i = pid->Get<double>("i");
00357 double d = 0;
00358 if (pid->HasElement("d"))
00359 d = pid->Get<double>("d");
00360 double iMax = 0;
00361 if (pid->HasElement("iMax"))
00362 iMax = pid->Get<double>("iMax");
00363 double iMin = 0;
00364 if (pid->HasElement("iMin"))
00365 iMin = pid->Get<double>("iMin");
00366 double cmdMax = 3;
00367 if (pid->HasElement("cmdMax"))
00368 cmdMax = pid->Get<double>("cmdMax");
00369 double cmdMin = -3;
00370 if (pid->HasElement("cmdMin"))
00371 cmdMin = pid->Get<double>("cmdMin");
00372 propeller_pid_.Init(p, i, d, iMax, iMin, cmdMax, cmdMin);
00373 use_propeller_pid_ = true;
00374 }
00375 }
00376
00377 if (_sdf->HasElement("cgo3_mount_joint")) {
00378 std::string gimbal_yaw_joint_name = _sdf->GetElement("cgo3_mount_joint")->Get<std::string>();
00379 gimbal_yaw_joint_ = model_->GetJoint(gimbal_yaw_joint_name);
00380 int control_index;
00381 getSdfParam<int>(_sdf->GetElement("cgo3_mount_joint"), "input_index", control_index, -1);
00382 if (control_index >= 0) {
00383 joints_.at(control_index) = gimbal_yaw_joint_;
00384 }
00385 }
00386
00387 if (_sdf->HasElement("cgo3_vertical_arm_joint")) {
00388 std::string gimbal_roll_joint_name = _sdf->GetElement("cgo3_vertical_arm_joint")->Get<std::string>();
00389 gimbal_roll_joint_ = model_->GetJoint(gimbal_roll_joint_name);
00390 int control_index;
00391 getSdfParam<int>(_sdf->GetElement("cgo3_vertical_arm_joint"), "input_index", control_index, -1);
00392 if (control_index >= 0) {
00393 joints_.at(control_index) = gimbal_roll_joint_;
00394 }
00395 }
00396
00397 if (_sdf->HasElement("cgo3_horizontal_arm_joint")) {
00398 std::string gimbal_pitch_joint_name = _sdf->GetElement("cgo3_horizontal_arm_joint")->Get<std::string>();
00399 gimbal_pitch_joint_ = model_->GetJoint(gimbal_pitch_joint_name);
00400 int control_index;
00401 getSdfParam<int>(_sdf->GetElement("cgo3_horizontal_arm_joint"), "input_index", control_index, -1);
00402 if (control_index >= 0) {
00403 joints_.at(control_index) = gimbal_pitch_joint_;
00404 }
00405 }
00406
00407
00408
00409 updateConnection_ = event::Events::ConnectWorldUpdateBegin(
00410 boost::bind(&GazeboMavlinkInterface::OnUpdate, this, _1));
00411
00412
00413
00414
00415
00416 gzdbg << "Creating Gazebo subscriber on topic \"" << "~/" + namespace_ + "/" + imu_sub_topic_ << "\"." << std::endl;
00417 imu_sub_ = node_handle_->Subscribe("~/" + namespace_ + "/" + imu_sub_topic_, &GazeboMavlinkInterface::ImuCallback, this);
00418
00419 gzdbg << "Creating Gazebo subscriber on topic \"" << "~/" + namespace_ + "/" + lidar_sub_topic_ << "\"." << std::endl;
00420 lidar_sub_ = node_handle_->Subscribe("~/" + namespace_ + "/" + lidar_sub_topic_, &GazeboMavlinkInterface::LidarCallback, this);
00421
00422 gzdbg << "Creating Gazebo subscriber on topic \"" << "~/" + namespace_ + "/" + opticalFlow_sub_topic_ << "\"." << std::endl;
00423 opticalFlow_sub_ = node_handle_->Subscribe("~/" + namespace_ + "/" + opticalFlow_sub_topic_, &GazeboMavlinkInterface::OpticalFlowCallback, this);
00424
00425
00426 gzdbg << "Creating Gazebo publisher on topic \"" << "~/" + namespace_ + "/" + motor_velocity_reference_pub_topic_ << "\"." << std::endl;
00427 motor_velocity_reference_pub_ = node_handle_->Advertise<gz_mav_msgs::CommandMotorSpeed>("~/" + namespace_ + "/" + motor_velocity_reference_pub_topic_, 1);
00428
00429
00431 gzdbg << "Creating Gazebo publisher on topic \"" << "~/gps_position" << "\"." << std::endl;
00432 gps_pub_ = node_handle_->Advertise<msgs::Vector3d>("~/gps_position");
00433
00434
00435 rotor_count_ = 5;
00436 last_time_ = world_->SimTime();
00437 last_gps_time_ = world_->SimTime();
00438 gps_update_interval_ = 0.2;
00439
00440 gravity_W_ = world_->Gravity();
00441
00442
00443
00444
00445
00446
00447
00448
00449 mag_d_.X() = 0.21523;
00450 mag_d_.Y() = 0;
00451 mag_d_.Z() = -0.42741;
00452
00453
00454
00455 mavlink_addr_ = htonl(INADDR_ANY);
00456 if (_sdf->HasElement("mavlink_addr")) {
00457 std::string mavlink_addr = _sdf->GetElement("mavlink_addr")->Get<std::string>();
00458 if (mavlink_addr != "INADDR_ANY") {
00459 mavlink_addr_ = inet_addr(mavlink_addr.c_str());
00460 if (mavlink_addr_ == INADDR_NONE) {
00461 fprintf(stderr, "invalid mavlink_addr \"%s\"\n", mavlink_addr.c_str());
00462 return;
00463 }
00464 }
00465 }
00466 if (_sdf->HasElement("mavlink_udp_port")) {
00467 mavlink_udp_port_ = _sdf->GetElement("mavlink_udp_port")->Get<int>();
00468 }
00469
00470
00471 if ((fd_ = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
00472 printf("create socket failed\n");
00473 return;
00474 }
00475
00476 memset((char *)&myaddr_, 0, sizeof(myaddr_));
00477 myaddr_.sin_family = AF_INET;
00478 myaddr_.sin_addr.s_addr = htonl(INADDR_ANY);
00479
00480 myaddr_.sin_port = htons(0);
00481
00482 if (bind(fd_, (struct sockaddr *)&myaddr_, sizeof(myaddr_)) < 0) {
00483 printf("bind failed\n");
00484 return;
00485 }
00486
00487 srcaddr_.sin_family = AF_INET;
00488 srcaddr_.sin_addr.s_addr = mavlink_addr_;
00489 srcaddr_.sin_port = htons(mavlink_udp_port_);
00490 addrlen_ = sizeof(srcaddr_);
00491
00492 fds_[0].fd = fd_;
00493 fds_[0].events = POLLIN;
00494
00495
00496 }
00497
00498
00499 void GazeboMavlinkInterface::OnUpdate(const common::UpdateInfo& ) {
00500
00501 common::Time current_time = world_->SimTime();
00502 double dt = (current_time - last_time_).Double();
00503
00504 pollForMAVLinkMessages(dt, 1000);
00505
00506 handle_control(dt);
00507
00508 if(received_first_reference_) {
00509
00510 gz_mav_msgs::CommandMotorSpeed turning_velocities_msg;
00511
00512 for (int i = 0; i < input_reference_.size(); i++){
00513 if (last_actuator_time_ == 0 || (current_time - last_actuator_time_).Double() > 0.2) {
00514 turning_velocities_msg.add_motor_speed(0);
00515 } else {
00516 turning_velocities_msg.add_motor_speed(input_reference_[i]);
00517 }
00518 }
00519
00520
00521
00522
00523
00524
00525 motor_velocity_reference_pub_->Publish(turning_velocities_msg);
00526 }
00527
00528 last_time_ = current_time;
00529
00530
00531 ignition::math::Pose3d T_W_I = model_->WorldPose();
00532 ignition::math::Vector3d pos_W_I = T_W_I.Pos();
00533
00534 ignition::math::Vector3d velocity_current_W = model_->WorldLinearVel();
00535
00536 ignition::math::Vector3d velocity_current_W_xy = velocity_current_W;
00537 velocity_current_W_xy.Z() = 0;
00538
00539
00540
00541 double x_rad = pos_W_I.Y() / kEarthRadius_m;
00542 double y_rad = pos_W_I.X() / kEarthRadius_m;
00543 double c = sqrt(x_rad * x_rad + y_rad * y_rad);
00544 double sin_c = sin(c);
00545 double cos_c = cos(c);
00546 if (c != 0.0) {
00547 lat_rad_ = asin(cos_c * sin(kLatZurich_rad) + (x_rad * sin_c * cos(kLatZurich_rad)) / c);
00548 lon_rad_ = (kLonZurich_rad + atan2(y_rad * sin_c, c * cos(kLatZurich_rad) * cos_c - x_rad * sin(kLatZurich_rad) * sin_c));
00549 } else {
00550 lat_rad_ = kLatZurich_rad;
00551 lon_rad_ = kLonZurich_rad;
00552 }
00553
00554 if (current_time.Double() - last_gps_time_.Double() > gps_update_interval_) {
00555
00556 mavlink_hil_gps_t hil_gps_msg;
00557 hil_gps_msg.time_usec = current_time.nsec/1000;
00558 hil_gps_msg.fix_type = 3;
00559 hil_gps_msg.lat = lat_rad_ * 180 / M_PI * 1e7;
00560 hil_gps_msg.lon = lon_rad_ * 180 / M_PI * 1e7;
00561 hil_gps_msg.alt = (pos_W_I.Z() + kAltZurich_m) * 1000;
00562 hil_gps_msg.eph = 100;
00563 hil_gps_msg.epv = 100;
00564 hil_gps_msg.vel = velocity_current_W_xy.Length() * 100;
00565 hil_gps_msg.vn = velocity_current_W.Y() * 100;
00566 hil_gps_msg.ve = velocity_current_W.X() * 100;
00567 hil_gps_msg.vd = -velocity_current_W.Z() * 100;
00568 hil_gps_msg.cog = atan2(hil_gps_msg.ve, hil_gps_msg.vn) * 180.0/3.1416 * 100.0;
00569 hil_gps_msg.satellites_visible = 10;
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592 send_mavlink_message(MAVLINK_MSG_ID_HIL_GPS, &hil_gps_msg, 200);
00593
00594
00595 msgs::Vector3d gps_msg;
00596 gps_msg.set_x(lat_rad_ * 180. / M_PI);
00597 gps_msg.set_y(lon_rad_ * 180. / M_PI);
00598 gps_msg.set_z(hil_gps_msg.alt / 1000.f);
00599 gps_pub_->Publish(gps_msg);
00600
00601 last_gps_time_ = current_time;
00602 }
00603 }
00604
00605 void GazeboMavlinkInterface::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) {
00606
00607 component_ID = 0;
00608 uint8_t payload_len = mavlink_message_lengths[msgid];
00609 unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
00610
00611 uint8_t buf[MAVLINK_MAX_PACKET_LEN];
00612
00613
00614 buf[0] = MAVLINK_STX;
00615 buf[1] = payload_len;
00616
00617 buf[2] = 100;
00618 buf[3] = 0;
00619 buf[4] = component_ID;
00620 buf[5] = msgid;
00621
00622
00623 memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],msg, payload_len);
00624
00625
00626 uint16_t checksum;
00627 crc_init(&checksum);
00628 crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
00629 crc_accumulate(mavlink_message_crcs[msgid], &checksum);
00630
00631 buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
00632 buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
00633
00634 ssize_t len;
00635
00636 len = sendto(fd_, buf, packet_len, 0, (struct sockaddr *)&srcaddr_, sizeof(srcaddr_));
00637
00638 if (len <= 0) {
00639 printf("Failed sending mavlink message\n");
00640 }
00641 }
00642
00643 void GazeboMavlinkInterface::ImuCallback(ImuPtr& imu_message) {
00644
00645
00646
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661
00662
00663
00664 ignition::math::Quaterniond q_gr = ignition::math::Quaterniond(
00665 imu_message->orientation().w(),
00666 imu_message->orientation().x(),
00667 imu_message->orientation().y(),
00668 imu_message->orientation().z());
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681 ignition::math::Quaterniond q_br(0, 1, 0, 0);
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694 ignition::math::Quaterniond q_ng(0, 0.70711, 0.70711, 0);
00695
00696 ignition::math::Quaterniond q_gb = q_gr*q_br.Inverse();
00697 ignition::math::Quaterniond q_nb = q_ng*q_gb;
00698
00699 ignition::math::Vector3d pos_g = model_->WorldPose().Pos();
00700 ignition::math::Vector3d pos_n = q_ng.RotateVector(pos_g);
00701
00702
00703
00704 float declination = get_mag_declination(lat_rad_, lon_rad_);
00705
00706 ignition::math::Quaterniond q_dn(0.0, 0.0, declination);
00707 ignition::math::Vector3d mag_n = q_dn.RotateVectorReverse(mag_d_);
00708
00709 ignition::math::Vector3d vel_b = q_br.RotateVector(model_->RelativeLinearVel());
00710 ignition::math::Vector3d vel_n = q_ng.RotateVector(model_->WorldLinearVel());
00711 ignition::math::Vector3d omega_nb_b = q_br.RotateVector(model_->RelativeAngularVel());
00712
00713 standard_normal_distribution_ = std::normal_distribution<float>(0, 0.01f);
00714 ignition::math::Vector3d mag_noise_b(
00715 standard_normal_distribution_(random_generator_),
00716 standard_normal_distribution_(random_generator_),
00717 standard_normal_distribution_(random_generator_));
00718
00719 ignition::math::Vector3d accel_b = q_br.RotateVector(ignition::math::Vector3d (
00720 imu_message->linear_acceleration().x(),
00721 imu_message->linear_acceleration().y(),
00722 imu_message->linear_acceleration().z()));
00723 ignition::math::Vector3d gyro_b = q_br.RotateVector(ignition::math::Vector3d (
00724 imu_message->angular_velocity().x(),
00725 imu_message->angular_velocity().y(),
00726 imu_message->angular_velocity().z()));
00727 ignition::math::Vector3d mag_b = q_nb.RotateVectorReverse(mag_n) + mag_noise_b;
00728
00729 mavlink_hil_sensor_t sensor_msg;
00730 sensor_msg.time_usec = world_->SimTime().nsec/1000;
00731 sensor_msg.xacc = accel_b.X();
00732 sensor_msg.yacc = accel_b.Y();
00733 sensor_msg.zacc = accel_b.Z();
00734 sensor_msg.xgyro = gyro_b.X();
00735 sensor_msg.ygyro = gyro_b.Y();
00736 sensor_msg.zgyro = gyro_b.Z();
00737 sensor_msg.xmag = mag_b.X();
00738 sensor_msg.ymag = mag_b.Y();
00739 sensor_msg.zmag = mag_b.Z();
00740 sensor_msg.abs_pressure = 0.0;
00741 float rho = 1.2754f;
00742 sensor_msg.diff_pressure = 0.5f*rho*vel_b.X()*vel_b.X() / 100;
00743
00744 float p1, p2;
00745
00746
00747 do {
00748 p1 = rand() * (1.0 / RAND_MAX);
00749 p2 = rand() * (1.0 / RAND_MAX);
00750 } while (p1 <= __FLT_EPSILON__);
00751
00752 float n = sqrtf(-2.0 * logf(p1)) * cosf(2.0f * M_PI * p2);
00753 float alt_n = -pos_n.Z() + n * sqrtf(0.006f);
00754
00755 sensor_msg.pressure_alt = (std::isfinite(alt_n)) ? alt_n : -pos_n.Z();
00756 sensor_msg.temperature = 0.0;
00757 sensor_msg.fields_updated = 4095;
00758
00759
00760 optflow_xgyro_ = gyro_b.X();
00761 optflow_ygyro_ = gyro_b.Y();
00762 optflow_zgyro_ = gyro_b.Z();
00763
00764
00765
00766
00767
00768
00769
00770
00771
00772
00773
00774
00775
00776
00777
00778
00779
00780
00781
00782
00783
00784
00785
00786
00787 send_mavlink_message(MAVLINK_MSG_ID_HIL_SENSOR, &sensor_msg, 200);
00788
00789
00790 ignition::math::Vector3d accel_true_b = q_br.RotateVector(model_->RelativeLinearAccel());
00791
00792
00793 mavlink_hil_state_quaternion_t hil_state_quat;
00794 hil_state_quat.time_usec = world_->SimTime().nsec/1000;
00795 hil_state_quat.attitude_quaternion[0] = q_nb.W();
00796 hil_state_quat.attitude_quaternion[1] = q_nb.X();
00797 hil_state_quat.attitude_quaternion[2] = q_nb.Y();
00798 hil_state_quat.attitude_quaternion[3] = q_nb.Z();
00799
00800 hil_state_quat.rollspeed = omega_nb_b.X();
00801 hil_state_quat.pitchspeed = omega_nb_b.Y();
00802 hil_state_quat.yawspeed = omega_nb_b.Z();
00803
00804 hil_state_quat.lat = lat_rad_ * 180 / M_PI * 1e7;
00805 hil_state_quat.lon = lon_rad_ * 180 / M_PI * 1e7;
00806 hil_state_quat.alt = (-pos_n.Z() + kAltZurich_m) * 1000;
00807
00808 hil_state_quat.vx = vel_n.X() * 100;
00809 hil_state_quat.vy = vel_n.Y() * 100;
00810 hil_state_quat.vz = vel_n.Z() * 100;
00811
00812
00813 hil_state_quat.ind_airspeed = vel_b.X();
00814 hil_state_quat.true_airspeed = model_->WorldLinearVel().Length() * 100;
00815
00816 hil_state_quat.xacc = accel_true_b.X() * 1000;
00817 hil_state_quat.yacc = accel_true_b.Y() * 1000;
00818 hil_state_quat.zacc = accel_true_b.Z() * 1000;
00819
00820
00821
00822
00823
00824
00825
00826
00827
00828
00829
00830
00831
00832
00833
00834
00835
00836
00837
00838
00839
00840
00841
00842
00843
00844
00845
00846
00847 send_mavlink_message(MAVLINK_MSG_ID_HIL_STATE_QUATERNION, &hil_state_quat, 200);
00848 }
00849
00850 void GazeboMavlinkInterface::LidarCallback(LidarPtr& lidar_message) {
00851
00852 gzdbg << __FUNCTION__ << "() called." << std::endl;
00853
00854 mavlink_distance_sensor_t sensor_msg;
00855 sensor_msg.time_boot_ms = lidar_message->time_msec();
00856 sensor_msg.min_distance = lidar_message->min_distance() * 100.0;
00857 sensor_msg.max_distance = lidar_message->max_distance() * 100.0;
00858 sensor_msg.current_distance = lidar_message->current_distance() * 100.0;
00859 sensor_msg.type = 0;
00860 sensor_msg.id = 0;
00861
00862 sensor_msg.orientation = 8;
00863 sensor_msg.covariance = 0;
00864
00865
00866 optflow_distance_ = lidar_message->current_distance();
00867
00868 send_mavlink_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &sensor_msg, 200);
00869
00870 }
00871
00872 void GazeboMavlinkInterface::OpticalFlowCallback(OpticalFlowPtr& opticalFlow_message) {
00873
00874 gzdbg << __FUNCTION__ << "() called." << std::endl;
00875
00876 mavlink_hil_optical_flow_t sensor_msg;
00877 sensor_msg.time_usec = opticalFlow_message->time_usec();
00878 sensor_msg.sensor_id = opticalFlow_message->sensor_id();
00879 sensor_msg.integration_time_us = opticalFlow_message->integration_time_us();
00880 sensor_msg.integrated_x = opticalFlow_message->integrated_x();
00881 sensor_msg.integrated_y = opticalFlow_message->integrated_y();
00882 sensor_msg.integrated_xgyro = -optflow_ygyro_ * opticalFlow_message->integration_time_us() / 1000000.0;
00883 sensor_msg.integrated_ygyro = optflow_xgyro_ * opticalFlow_message->integration_time_us() / 1000000.0;
00884 sensor_msg.integrated_zgyro = -optflow_zgyro_ * opticalFlow_message->integration_time_us() / 1000000.0;
00885 sensor_msg.temperature = opticalFlow_message->temperature();
00886 sensor_msg.quality = opticalFlow_message->quality();
00887 sensor_msg.time_delta_distance_us = opticalFlow_message->time_delta_distance_us();
00888 sensor_msg.distance = optflow_distance_;
00889
00890 send_mavlink_message(MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, &sensor_msg, 200);
00891 }
00892
00893
00894
00895
00896
00897
00898
00899
00900
00901
00902
00903
00904
00905
00906
00907
00908
00909
00910
00911
00912 void GazeboMavlinkInterface::pollForMAVLinkMessages(double _dt, uint32_t _timeoutMs)
00913 {
00914
00915 struct timeval tv;
00916 tv.tv_sec = _timeoutMs / 1000;
00917 tv.tv_usec = (_timeoutMs % 1000) * 1000UL;
00918
00919
00920 ::poll(&fds_[0], (sizeof(fds_[0])/sizeof(fds_[0])), 0);
00921
00922 if (fds_[0].revents & POLLIN) {
00923 int len = recvfrom(fd_, buf_, sizeof(buf_), 0, (struct sockaddr *)&srcaddr_, &addrlen_);
00924 if (len > 0) {
00925 mavlink_message_t msg;
00926 mavlink_status_t status;
00927 for (unsigned i = 0; i < len; ++i)
00928 {
00929 if (mavlink_parse_char(MAVLINK_COMM_0, buf_[i], &msg, &status))
00930 {
00931
00932 handle_message(&msg);
00933 }
00934 }
00935 }
00936 }
00937 }
00938
00939 void GazeboMavlinkInterface::handle_message(mavlink_message_t *msg)
00940 {
00941
00942
00943
00944 switch(msg->msgid) {
00945 case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS:
00946 mavlink_hil_actuator_controls_t controls;
00947 mavlink_msg_hil_actuator_controls_decode(msg, &controls);
00948 bool armed = false;
00949
00950 if ((controls.mode & MAV_MODE_FLAG_SAFETY_ARMED) > 0) {
00951 armed = true;
00952 }
00953
00954 last_actuator_time_ = world_->SimTime();
00955
00956 for (unsigned i = 0; i < kNOutMax; i++) {
00957 input_index_[i] = i;
00958 }
00959
00960
00961 input_reference_.resize(kNOutMax);
00962 for (int i = 0; i < input_reference_.size(); i++) {
00963 if (armed) {
00964 input_reference_[i] = (controls.controls[input_index_[i]] + input_offset_[i])
00965 * input_scaling_[i] + zero_position_armed_[i];
00966
00967
00968 } else {
00969 input_reference_[i] = zero_position_disarmed_[i];
00970 }
00971 }
00972
00973 received_first_reference_ = true;
00974 break;
00975 }
00976 }
00977
00978 void GazeboMavlinkInterface::handle_control(double _dt)
00979 {
00980
00981 for (int i = 0; i < input_reference_.size(); i++) {
00982 if (joints_[i]) {
00983 double target = input_reference_[i];
00984 if (joint_control_type_[i] == "velocity")
00985 {
00986 double current = joints_[i]->GetVelocity(0);
00987 double err = current - target;
00988 double force = pids_[i].Update(err, _dt);
00989 joints_[i]->SetForce(0, force);
00990
00991
00992
00993 }
00994 else if (joint_control_type_[i] == "position")
00995 {
00996 double current = joints_[i]->Position(0);
00997 double err = current - target;
00998 double force = pids_[i].Update(err, _dt);
00999 joints_[i]->SetForce(0, force);
01000
01001
01002
01003 }
01004 else if (joint_control_type_[i] == "position_gztopic")
01005 {
01006 #if (GAZEBO_MAJOR_VERSION >= 7 && GAZEBO_MINOR_VERSION >= 4) || GAZEBO_MAJOR_VERSION >= 8
01007
01008 gazebo::msgs::Any m;
01009 m.set_type(gazebo::msgs::Any_ValueType_DOUBLE);
01010 m.set_double_value(target);
01011 #else
01012 std::stringstream ss;
01013 gazebo::msgs::GzString m;
01014 ss << target;
01015 m.set_data(ss.str());
01016 #endif
01017
01018 joint_control_pub_[i]->Publish(m);
01019 }
01020 else if (joint_control_type_[i] == "position_kinematic")
01021 {
01025 joints_[i]->SetPosition(0, input_reference_[i]);
01026 }
01027 else
01028 {
01029 gzerr << "joint_control_type[" << joint_control_type_[i] << "] undefined.\n";
01030 }
01031 }
01032 }
01033 }
01034
01035 }