gazebo_mavlink_interface.h
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  *
00008  * Licensed under the Apache License, Version 2.0 (the "License");
00009  * you may not use this file except in compliance with the License.
00010  * You may obtain a copy of the License at
00011  *
00012  *     http://www.apache.org/licenses/LICENSE-2.0
00013 
00014  * Unless required by applicable law or agreed to in writing, software
00015  * distributed under the License is distributed on an "AS IS" BASIS,
00016  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00017  * See the License for the specific language governing permissions and
00018  * limitations under the License.
00019  */
00020 
00021 #include <iostream>
00022 #include <math.h>
00023 #include <deque>
00024 #include <random>
00025 #include <sys/socket.h>
00026 #include <netinet/in.h>
00027 #include <sdf/sdf.hh>
00028 #include <stdio.h>
00029 
00030 #include <boost/bind.hpp>
00031 #include <Eigen/Eigen>
00032 #include <gazebo/common/common.hh>
00033 #include <gazebo/common/Plugin.hh>
00034 #include <gazebo/gazebo.hh>
00035 #include <gazebo/physics/physics.hh>
00036 #include "ignition/math/Vector3.hh"
00037 #include "gazebo/transport/transport.hh"
00038 #include "gazebo/msgs/msgs.hh"
00039 #include "common/mavlink.h"     // Either provided by ROS or as CMake argument MAVLINK_HEADER_DIR
00040 
00041 #include "common.h"
00042 //#include "mavlink/v1.0/common/mavlink.h"
00043 
00044 #include "CommandMotorSpeed.pb.h"
00045 #include "Imu.pb.h"
00046 #include "OpticalFlow.pb.h"
00047 #include "Lidar.pb.h"
00048 
00049 static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
00050 static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
00051 
00052 static const uint32_t kDefaultMavlinkUdpPort = 14560;
00053 
00054 namespace gazebo {
00055 
00056 typedef const boost::shared_ptr<const gz_mav_msgs::CommandMotorSpeed> CommandMotorSpeedPtr;
00057 typedef const boost::shared_ptr<const gz_sensor_msgs::Imu> ImuPtr;
00058 typedef const boost::shared_ptr<const lidar_msgs::msgs::lidar> LidarPtr;
00059 typedef const boost::shared_ptr<const opticalFlow_msgs::msgs::opticalFlow> OpticalFlowPtr;
00060 
00061 // Default values
00062 // static const std::string kDefaultNamespace = "";
00063 
00064 // This just proxies the motor commands from command/motor_speed to the single motors via internal
00065 // ConsPtr passing, such that the original commands don't have to go n_motors-times over the wire.
00066 static const std::string kDefaultMotorVelocityReferencePubTopic = "/gazebo/command/motor_speed";
00067 
00068 static const std::string kDefaultImuTopic = "/imu";
00069 static const std::string kDefaultLidarTopic = "/lidar/link/lidar";
00070 static const std::string kDefaultOpticalFlowTopic = "/camera/link/opticalFlow";
00071 
00072 class GazeboMavlinkInterface : public ModelPlugin {
00073  public:
00074   GazeboMavlinkInterface()
00075       : ModelPlugin(),
00076 
00077         received_first_reference_(false),
00078         namespace_(kDefaultNamespace),
00079         motor_velocity_reference_pub_topic_(kDefaultMotorVelocityReferencePubTopic),
00080         imu_sub_topic_(kDefaultImuTopic),
00081         opticalFlow_sub_topic_(kDefaultOpticalFlowTopic),
00082         lidar_sub_topic_(kDefaultLidarTopic),
00083         model_{},
00084         world_(nullptr),
00085         left_elevon_joint_(nullptr),
00086         right_elevon_joint_(nullptr),
00087         elevator_joint_(nullptr),
00088         propeller_joint_(nullptr),
00089         gimbal_yaw_joint_(nullptr),
00090         gimbal_pitch_joint_(nullptr),
00091         gimbal_roll_joint_(nullptr),
00092         input_offset_{},
00093         input_scaling_{},
00094         zero_position_disarmed_{},
00095         zero_position_armed_{},
00096         input_index_{},
00097         lat_rad_(0.0),
00098         lon_rad_(0.0),
00099         mavlink_udp_port_(kDefaultMavlinkUdpPort)
00100         {}
00101   ~GazeboMavlinkInterface();
00102 
00103   void Publish();
00104 
00105  protected:
00106   void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
00107   void OnUpdate(const common::UpdateInfo& /*_info*/);
00108 
00109  private:
00110 
00111   bool received_first_reference_;
00112   Eigen::VectorXd input_reference_;
00113 
00114   std::string namespace_;
00115   std::string motor_velocity_reference_pub_topic_;
00116   std::string mavlink_control_sub_topic_;
00117   std::string link_name_;
00118 
00119   transport::NodePtr node_handle_;
00120   transport::PublisherPtr motor_velocity_reference_pub_;
00121   transport::SubscriberPtr mav_control_sub_;
00122 
00123   physics::ModelPtr model_;
00124   physics::WorldPtr world_;
00125   physics::JointPtr left_elevon_joint_;
00126   physics::JointPtr right_elevon_joint_;
00127   physics::JointPtr elevator_joint_;
00128   physics::JointPtr propeller_joint_;
00129   physics::JointPtr gimbal_yaw_joint_;
00130   physics::JointPtr gimbal_pitch_joint_;
00131   physics::JointPtr gimbal_roll_joint_;
00132   common::PID propeller_pid_;
00133   common::PID elevator_pid_;
00134   common::PID left_elevon_pid_;
00135   common::PID right_elevon_pid_;
00136   bool use_propeller_pid_;
00137   bool use_elevator_pid_;
00138   bool use_left_elevon_pid_;
00139   bool use_right_elevon_pid_;
00140 
00141   std::vector<physics::JointPtr> joints_;
00142   std::vector<common::PID> pids_;
00143 
00145   event::ConnectionPtr updateConnection_;
00146 
00147   boost::thread callback_queue_thread_;
00148   void QueueThread();
00149   void ImuCallback(ImuPtr& imu_msg);
00150   void LidarCallback(LidarPtr& lidar_msg);
00151   void OpticalFlowCallback(OpticalFlowPtr& opticalFlow_msg);
00152   void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID);
00153   void handle_message(mavlink_message_t *msg);
00154   void pollForMAVLinkMessages(double _dt, uint32_t _timeoutMs);
00155 
00156   static const unsigned kNOutMax = 16;
00157 
00158   unsigned rotor_count_;
00159 
00160   double input_offset_[kNOutMax];
00161   double input_scaling_[kNOutMax];
00162   std::string joint_control_type_[kNOutMax];
00163   std::string gztopic_[kNOutMax];
00164   double zero_position_disarmed_[kNOutMax];
00165   double zero_position_armed_[kNOutMax];
00166   int input_index_[kNOutMax];
00167   transport::PublisherPtr joint_control_pub_[kNOutMax];
00168 
00169   transport::SubscriberPtr imu_sub_;
00170   transport::SubscriberPtr lidar_sub_;
00171   transport::SubscriberPtr opticalFlow_sub_;
00172   transport::PublisherPtr gps_pub_;
00173   std::string imu_sub_topic_;
00174   std::string lidar_sub_topic_;
00175   std::string opticalFlow_sub_topic_;
00176 
00177   common::Time last_time_;
00178   common::Time last_gps_time_;
00179   common::Time last_actuator_time_;
00180   double gps_update_interval_;
00181   double lat_rad_;
00182   double lon_rad_;
00183   void handle_control(double _dt);
00184 
00185   ignition::math::Vector3d gravity_W_;
00186   ignition::math::Vector3d velocity_prev_W_;
00187   ignition::math::Vector3d mag_d_;
00188 
00189   std::default_random_engine random_generator_;
00190   std::normal_distribution<float> standard_normal_distribution_;
00191 
00192   int fd_;
00193   struct sockaddr_in myaddr_;  
00194   struct sockaddr_in srcaddr_;  
00195   socklen_t addrlen_;
00196   unsigned char buf_[65535];
00197   struct pollfd fds_[1];
00198 
00199   struct sockaddr_in srcaddr_2_;  
00200 
00201   //so we dont have to do extra callbacks
00202   double optflow_xgyro_;
00203   double optflow_ygyro_;
00204   double optflow_zgyro_;
00205   double optflow_distance_;
00206 
00207   in_addr_t mavlink_addr_;
00208   int mavlink_udp_port_;
00209 
00210   };
00211 }


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