Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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"
00040
00041 #include "common.h"
00042
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
00062
00063
00064
00065
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& );
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
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