gazebo_mavlink_interface.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
00003  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
00004  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
00005  * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
00006  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
00007  * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
00008  *
00009  * Licensed under the Apache License, Version 2.0 (the "License");
00010  * you may not use this file except in compliance with the License.
00011  * You may obtain a copy of the License at
00012  *
00013  *     http://www.apache.org/licenses/LICENSE-2.0
00014 
00015  * Unless required by applicable law or agreed to in writing, software
00016  * distributed under the License is distributed on an "AS IS" BASIS,
00017  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00018  * See the License for the specific language governing permissions and
00019  * limitations under the License.
00020  */
00021 
00022 // MODULE
00023 #include "rotors_gazebo_plugins/gazebo_mavlink_interface.h"
00024 
00025 // USER
00026 #include "rotors_gazebo_plugins/geo_mag_declination.h"
00027 
00028 namespace gazebo {
00029 
00030 // Set global reference point
00031 // Zurich Irchel Park: 47.397742, 8.545594, 488m
00032 // Seattle downtown (15 deg declination): 47.592182, -122.316031, 86m
00033 // Moscow downtown: 55.753395, 37.625427, 155m
00034 
00035 // Zurich Irchel Park
00036 static const double kLatZurich_rad = 47.397742 * M_PI / 180;  // rad
00037 static const double kLonZurich_rad = 8.545594 * M_PI / 180;  // rad
00038 static const double kAltZurich_m = 488.0; // meters
00039 // Seattle downtown (15 deg declination): 47.592182, -122.316031
00040 // static const double lat_zurich = 47.592182 * M_PI / 180;  // rad
00041 // static const double lon_zurich = -122.316031 * M_PI / 180;  // rad
00042 // static const double alt_zurich = 86.0; // meters
00043 static const float kEarthRadius_m = 6353000;  // m
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   // Store the pointer to the model.
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   // set input_reference_ from inputs.control
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           // start gz transport node handle
00117           if (joint_control_type_[index] == "position_gztopic")
00118           {
00119             // setup publisher handle to topic
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           // setup joint control pid to control joint
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     // setup pid to control joint
00207     use_left_elevon_pid_ = false;
00208     if (left_elevon->HasElement("joint_control_pid")) {
00209       // setup joint control pid to control joint
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     // setup pid to control joint
00257     use_right_elevon_pid_ = false;
00258     if (right_elevon->HasElement("joint_control_pid")) {
00259       // setup joint control pid to control joint
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     // setup pid to control joint
00307     use_elevator_pid_ = false;
00308     if (elevator->HasElement("joint_control_pid")) {
00309       // setup joint control pid to control joint
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     // setup joint control pid to control joint
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   // Listen to the update event. This event is broadcast every
00408   // simulation iteration.
00409   updateConnection_ = event::Events::ConnectWorldUpdateBegin(
00410       boost::bind(&GazeboMavlinkInterface::OnUpdate, this, _1));
00411 
00412   //==============================================//
00413   //============ GAZEBO PUB/SUB SETUP ============//
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   // Publish gazebo's motor_speed message
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   // This topic is subscribed to by gazebo_geotagged_images_plugin.cpp
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;  // in seconds for 5Hz
00439 
00440   gravity_W_ = world_->Gravity();
00441 
00442   // Magnetic field data for Zurich from WMM2015 (10^5xnanoTesla (N, E D) n-frame )
00443   // mag_n_ = {0.21523, 0.00771, -0.42741};
00444   // We set the world Y component to zero because we apply
00445   // the declination based on the global position,
00446   // and so we need to start without any offsets.
00447   // The real value for Zurich would be 0.00771
00448   // frame d is the magnetic north frame
00449   mag_d_.X() = 0.21523;
00450   mag_d_.Y() = 0;
00451   mag_d_.Z() = -0.42741;
00452 
00453   //Create socket
00454   // udp socket data
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   // try to setup udp socket for communcation with simulator
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   // Let the OS pick the port
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& /*_info*/) {
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     // TODO Add timestamp and Header
00521     // turning_velocities_msg->header.stamp.sec = current_time.sec;
00522     // turning_velocities_msg->header.stamp.nsec = current_time.nsec;
00523 
00524     // gzerr << turning_velocities_msg.motor_speed(0) << "\n";
00525     motor_velocity_reference_pub_->Publish(turning_velocities_msg);
00526   }
00527 
00528   last_time_ = current_time;
00529 
00530   //send gps
00531   ignition::math::Pose3d T_W_I = model_->WorldPose(); //TODO(burrimi): Check tf.
00532   ignition::math::Vector3d pos_W_I = T_W_I.Pos();  // Use the models' world position for GPS and pressure alt.
00533 
00534   ignition::math::Vector3d velocity_current_W = model_->WorldLinearVel();  // Use the models' world position for GPS velocity.
00535 
00536   ignition::math::Vector3d velocity_current_W_xy = velocity_current_W;
00537   velocity_current_W_xy.Z() = 0;
00538 
00539   // TODO: Remove GPS message from IMU plugin. Added gazebo GPS plugin. This is temp here.
00540   // reproject local position to gps coordinates
00541   double x_rad = pos_W_I.Y() / kEarthRadius_m; // north
00542   double y_rad = pos_W_I.X() / kEarthRadius_m; // east
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_) {  // 5Hz
00555     // Raw UDP mavlink
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     /*static int gps_debug_msg_count = 0;
00572     if(gps_debug_msg_count >= 0) {
00573       gzmsg << "{ "
00574       "time_usec: " << hil_gps_msg.time_usec <<
00575       ", fix_type: " << hil_gps_msg.fix_type <<
00576       ", lat: " << hil_gps_msg.lat <<
00577       ", lon: " << hil_gps_msg.lon <<
00578       ", alt: " << hil_gps_msg.alt <<
00579       ", eph: " << hil_gps_msg.eph <<
00580       ", epv: " << hil_gps_msg.epv <<
00581       ", vel: " << hil_gps_msg.vel <<
00582       ", vv: " << hil_gps_msg.vn <<
00583       ", ve: " << hil_gps_msg.ve <<
00584       ", vd: " << hil_gps_msg.vd <<
00585       ", cog: " << hil_gps_msg.cog <<
00586       ", satellites_visible: " << static_cast<int>(hil_gps_msg.satellites_visible) <<
00587       " }" << std::endl;
00588       gps_debug_msg_count = 0;
00589     }
00590     gps_debug_msg_count++;*/
00591 
00592     send_mavlink_message(MAVLINK_MSG_ID_HIL_GPS, &hil_gps_msg, 200);
00593 
00594     // Also publish GPS info on Gazebo topic
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   // Header
00614   buf[0] = MAVLINK_STX;
00615   buf[1] = payload_len;
00616   // no idea which numbers should be here
00617   buf[2] = 100;
00618   buf[3] = 0;
00619   buf[4] = component_ID;
00620   buf[5] = msgid;
00621 
00622   // Payload
00623   memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],msg, payload_len);
00624 
00625   // Checksym
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   //gzdbg << __FUNCTION__ << "() called." << std::endl;
00646 
00647   /*static int imu_debug_msg_count = 0;
00648   if(imu_debug_msg_count >= 100) {
00649     gzmsg << "{ "
00650     "w: " << imu_message->orientation().w() <<
00651     ", x: " << imu_message->orientation().x() <<
00652     ", y: " << imu_message->orientation().y() <<
00653     ", z: " << imu_message->orientation().z() <<
00654     " }" << std::endl;
00655     imu_debug_msg_count = 0;
00656   }
00657   imu_debug_msg_count++;*/
00658 
00659   // frames
00660   // g - gazebo (ENU), east, north, up
00661   // r - rotors imu frame (FLU), forward, left, up
00662   // b - px4 (FRD) forward, right down
00663   // n - px4 (NED) north, east, down
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   // q_br
00672   /*
00673   tf.euler2quat(*tf.mat2euler([
00674   #        F  L  U
00675           [1, 0, 0],  # F
00676           [0, -1, 0], # R
00677           [0, 0, -1]  # D
00678       ]
00679   )).round(5)
00680   */
00681   ignition::math::Quaterniond q_br(0, 1, 0, 0);
00682 
00683 
00684   // q_ng
00685   /*
00686   tf.euler2quat(*tf.mat2euler([
00687   #        N  E  D
00688           [0, 1, 0],  # E
00689           [1, 0, 0],  # N
00690           [0, 0, -1]  # U
00691       ]
00692   )).round(5)
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   //gzerr << "got imu: " << C_W_I << "\n";
00703   //gzerr << "got pose: " << T_W_I.rot << "\n";
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; // density of air, TODO why is this not 1.225 as given by std. atmos.
00742   sensor_msg.diff_pressure = 0.5f*rho*vel_b.X()*vel_b.X() / 100;
00743 
00744   float p1, p2;
00745 
00746   // need to add noise to pressure alt
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   //gyro needed for optical flow message
00760   optflow_xgyro_ = gyro_b.X();
00761   optflow_ygyro_ = gyro_b.Y();
00762   optflow_zgyro_ = gyro_b.Z();
00763 
00764   /*static int imu_msg_count = 0;
00765   if(imu_msg_count >= 100) {
00766     gzmsg << "{ "
00767     "time_usec: " << sensor_msg.time_usec <<
00768     ", xacc: " << sensor_msg.xacc <<
00769     ", yacc: " << sensor_msg.yacc <<
00770     ", zacc: " << sensor_msg.zacc <<
00771     ", xgyro: " << sensor_msg.xgyro <<
00772     ", ygyro: " << sensor_msg.ygyro <<
00773     ", zgyro: " << sensor_msg.zgyro <<
00774     ", xmag: " << sensor_msg.xmag <<
00775     ", ymag: " << sensor_msg.ymag <<
00776     ", zmag: " << sensor_msg.zmag <<
00777     ", abs_pressure: " << sensor_msg.abs_pressure <<
00778     ", diff_pressure: " << sensor_msg.diff_pressure <<
00779     ", pressure_alt: " << sensor_msg.pressure_alt <<
00780     ", temperature: " << sensor_msg.temperature <<
00781     ", fields_updated: " << sensor_msg.fields_updated <<
00782     " }" << std::endl;
00783     imu_msg_count = 0;
00784   }
00785   imu_msg_count++;*/
00786 
00787   send_mavlink_message(MAVLINK_MSG_ID_HIL_SENSOR, &sensor_msg, 200);
00788 
00789   // ground truth
00790   ignition::math::Vector3d accel_true_b = q_br.RotateVector(model_->RelativeLinearAccel());
00791 
00792   // send ground truth
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   // assumed indicated airspeed due to flow aligned with pitot (body x)
00813   hil_state_quat.ind_airspeed = vel_b.X();
00814   hil_state_quat.true_airspeed = model_->WorldLinearVel().Length() * 100; //no wind simulated
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   /*static int quat_msg_count = 0;
00821   if(quat_msg_count >= 100) {
00822     gzmsg << "{ "
00823     "time_usec: " << hil_state_quat.time_usec <<
00824     ", attitude_quaternion[0]: " << hil_state_quat.attitude_quaternion[0] <<
00825     ", attitude_quaternion[1]: " << hil_state_quat.attitude_quaternion[1] <<
00826     ", attitude_quaternion[2]: " << hil_state_quat.attitude_quaternion[2] <<
00827     ", attitude_quaternion[3]: " << hil_state_quat.attitude_quaternion[3] <<
00828     ", rollspeed: " << hil_state_quat.rollspeed <<
00829     ", pitchspeed: " << hil_state_quat.pitchspeed <<
00830     ", yawspeed: " << hil_state_quat.yawspeed <<
00831     ", lat: " << hil_state_quat.lat <<
00832     ", lon: " << hil_state_quat.lon <<
00833     ", alt: " << hil_state_quat.alt <<
00834     ", vx: " << hil_state_quat.vx <<
00835     ", vy: " << hil_state_quat.vy <<
00836     ", vz: " << hil_state_quat.vz <<
00837     ", ind_airspeed: " << hil_state_quat.ind_airspeed <<
00838     ", true_airspeed: " << hil_state_quat.true_airspeed <<
00839     ", xacc: " << hil_state_quat.xacc <<
00840     ", yacc: " << hil_state_quat.yacc <<
00841     ", zacc: " << hil_state_quat.zacc <<
00842     " }" << std::endl;
00843     quat_msg_count = 0;
00844   }
00845   quat_msg_count++;*/
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   // to to roll 180 (downward facing for agl measurement)
00862   sensor_msg.orientation = 8;
00863   sensor_msg.covariance = 0;
00864 
00865   //distance needed for optical flow message
00866   optflow_distance_ = lidar_message->current_distance(); //[m]
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; //xy switched
00883   sensor_msg.integrated_ygyro = optflow_xgyro_ * opticalFlow_message->integration_time_us() / 1000000.0; //xy switched
00884   sensor_msg.integrated_zgyro = -optflow_zgyro_ * opticalFlow_message->integration_time_us() / 1000000.0; //change direction
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 /*ssize_t GazeboMavlinkInterface::receive(void *_buf, const size_t _size, uint32_t _timeoutMs)
00894 {
00895   fd_set fds;
00896   struct timeval tv;
00897 
00898   FD_ZERO(&fds);
00899   FD_SET(this->handle, &fds);
00900 
00901   tv.tv_sec = _timeoutMs / 1000;
00902   tv.tv_usec = (_timeoutMs % 1000) * 1000UL;
00903 
00904   if (select(this->handle+1, &fds, NULL, NULL, &tv) != 1)
00905   {
00906       return -1;
00907   }
00908 
00909   return recv(this->handle, _buf, _size, 0);
00910 }*/
00911 
00912 void GazeboMavlinkInterface::pollForMAVLinkMessages(double _dt, uint32_t _timeoutMs)
00913 {
00914   // convert timeout in ms to timeval
00915   struct timeval tv;
00916   tv.tv_sec = _timeoutMs / 1000;
00917   tv.tv_usec = (_timeoutMs % 1000) * 1000UL;
00918 
00919   // poll
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           // have a message, handle it
00932           handle_message(&msg);
00933         }
00934       }
00935     }
00936   }
00937 }
00938 
00939 void GazeboMavlinkInterface::handle_message(mavlink_message_t *msg)
00940 {
00941 
00942   //gzdbg << __FUNCTION__ << "() called, msg->msgid = " << msg->msgid << "." << std::endl;
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     // set rotor speeds, controller targets
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         // if (joints_[i])
00967         //   gzerr << i << " : " << input_index_[i] << " : " << controls.controls[input_index_[i]] << " : " << input_reference_[i] << "\n";
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     // set joint positions
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           // gzerr << "chan[" << i << "] curr[" << current
00991           //       << "] cmd[" << target << "] f[" << force
00992           //       << "] scale[" << input_scaling_[i] << "]\n";
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           // gzerr << "chan[" << i << "] curr[" << current
01001           //       << "] cmd[" << target << "] f[" << force
01002           //       << "] scale[" << input_scaling_[i] << "]\n";
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 }


rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:43