gazebo_mavlink_interface.h
Go to the documentation of this file.
1 /*
2  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
3  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
4  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
5  * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
6  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
7  * Copyright 2015-2018 PX4 Pro Development Team
8  *
9  * Licensed under the Apache License, Version 2.0 (the "License");
10  * you may not use this file except in compliance with the License.
11  * You may obtain a copy of the License at
12  *
13  * http://www.apache.org/licenses/LICENSE-2.0
14 
15  * Unless required by applicable law or agreed to in writing, software
16  * distributed under the License is distributed on an "AS IS" BASIS,
17  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
18  * See the License for the specific language governing permissions and
19  * limitations under the License.
20  */
21 #include <atomic>
22 #include <boost/asio.hpp>
23 #include <boost/bind.hpp>
24 #include <boost/shared_array.hpp>
25 #include <boost/system/system_error.hpp>
26 #include <cassert>
27 #include <chrono>
28 #include <deque>
29 #include <memory>
30 #include <mutex>
31 #include <sstream>
32 #include <stdexcept>
33 #include <thread>
34 #include <vector>
35 
36 #include <cstdlib>
37 #include <iostream>
38 #include <math.h>
39 #include <netinet/in.h>
40 #include <random>
41 #include <stdio.h>
42 #include <string>
43 #include <sys/socket.h>
44 
45 #include <Eigen/Eigen>
46 
47 #include <gazebo/common/Plugin.hh>
48 #include <gazebo/common/common.hh>
49 #include <gazebo/gazebo.hh>
50 #include <gazebo/msgs/msgs.hh>
51 #include <gazebo/physics/physics.hh>
52 #include <gazebo/transport/transport.hh>
53 
54 #include <CommandMotorSpeed.pb.h>
55 #include <Imu.pb.h>
56 #include <Lidar.pb.h>
57 #include <OpticalFlow.pb.h>
58 #include <ignition/math.hh>
59 #include <sdf/sdf.hh>
60 #include "common.h"
61 
62 #include <mavlink/v2.0/common/mavlink.h>
63 #include "msgbuffer.h"
64 
66 
67 static const uint32_t kDefaultMavlinkUdpPort = 14560;
68 static const uint32_t kDefaultQGCUdpPort = 14550;
69 
70 using lock_guard = std::lock_guard<std::recursive_mutex>;
71 static constexpr auto kDefaultDevice = "/dev/ttyACM0";
72 static constexpr auto kDefaultBaudRate = 921600;
73 static constexpr double kDefaultImuInterval = 0.004;
74 
76 static constexpr ssize_t MAX_SIZE = MAVLINK_MAX_PACKET_LEN + 16;
77 static constexpr size_t MAX_TXQ_SIZE = 1000;
78 
79 namespace gazebo {
80 
87 
88 // Default values
89 // static const std::string kDefaultNamespace = "";
90 
91 // This just proxies the motor commands from command/motor_speed to the single
92 // motors via internal ConsPtr passing, such that the original commands don't
93 // have to go n_motors-times over the wire.
94 static const std::string kDefaultMotorVelocityReferencePubTopic =
95  "/gazebo/command/motor_speed";
96 
97 static const std::string kDefaultImuTopic = "/imu";
98 static const std::string kDefaultLidarTopic = "/link/lidar";
99 static const std::string kDefaultOpticalFlowTopic = "/px4flow/link/opticalFlow";
100 
102 enum class Framing : uint8_t {
107 };
108 
109 class GazeboMavlinkInterface : public ModelPlugin {
110  public:
112  : ModelPlugin(),
113  received_first_reference_(false),
114  namespace_(kDefaultNamespace),
115  protocol_version_(2.0),
116  motor_velocity_reference_pub_topic_(
117  kDefaultMotorVelocityReferencePubTopic),
118  use_propeller_pid_(false),
119  use_elevator_pid_(false),
120  use_left_elevon_pid_(false),
121  use_right_elevon_pid_(false),
122  imu_sub_topic_(kDefaultImuTopic),
123  opticalFlow_sub_topic_(kDefaultOpticalFlowTopic),
124  lidar_sub_topic_(kDefaultLidarTopic),
125  model_{},
126  world_(nullptr),
127  left_elevon_joint_(nullptr),
128  right_elevon_joint_(nullptr),
129  elevator_joint_(nullptr),
130  propeller_joint_(nullptr),
131  gimbal_yaw_joint_(nullptr),
132  gimbal_pitch_joint_(nullptr),
133  gimbal_roll_joint_(nullptr),
134  input_offset_{},
135  input_scaling_{},
136  zero_position_disarmed_{},
137  zero_position_armed_{},
138  input_index_{},
139  lat_rad_(0.0),
140  lon_rad_(0.0),
141  mavlink_udp_port_(kDefaultMavlinkUdpPort),
142  qgc_udp_port_(kDefaultMavlinkUdpPort),
143  serial_enabled_(false),
144  tx_q_{},
145  rx_buf_{},
146  m_status_{},
147  m_buffer_{},
148  io_service_(),
149  serial_dev_(io_service_),
150  device_(kDefaultDevice),
151  baudrate_(kDefaultBaudRate),
152  hil_mode_(false),
153  hil_state_level_(false) {}
154 
156 
157  void Publish();
158 
159  protected:
160  void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
161  void OnUpdate(const common::UpdateInfo& /*_info*/);
162 
163  private:
165  Eigen::VectorXd input_reference_;
166 
168 
169  std::string namespace_;
172  std::string link_name_;
173 
174  transport::NodePtr node_handle_;
175  transport::PublisherPtr motor_velocity_reference_pub_;
176  transport::SubscriberPtr mav_control_sub_;
177 
178  physics::ModelPtr model_;
179  physics::WorldPtr world_;
180  physics::JointPtr left_elevon_joint_;
181  physics::JointPtr right_elevon_joint_;
182  physics::JointPtr elevator_joint_;
183  physics::JointPtr propeller_joint_;
184  physics::JointPtr gimbal_yaw_joint_;
185  physics::JointPtr gimbal_pitch_joint_;
186  physics::JointPtr gimbal_roll_joint_;
187  common::PID propeller_pid_;
188  common::PID elevator_pid_;
189  common::PID left_elevon_pid_;
190  common::PID right_elevon_pid_;
195 
196  std::vector<physics::JointPtr> joints_;
197  std::vector<common::PID> pids_;
198 
200  event::ConnectionPtr updateConnection_;
201 
202  boost::thread callback_queue_thread_;
203  void QueueThread();
204  void ImuCallback(ImuPtr& imu_msg);
205  void LidarCallback(LidarPtr& lidar_msg);
206  void OpticalFlowCallback(OpticalFlowPtr& opticalFlow_msg);
207  void send_mavlink_message(
208  const mavlink_message_t* message, const int destination_port = 0);
210  void pollForMAVLinkMessages(double _dt, uint32_t _timeoutMs);
211 
212  // Serial interface
213  void open();
214  void close();
215  void do_read();
216  void parse_buffer(const boost::system::error_code& err, std::size_t bytes_t);
217  void do_write(bool check_tx_state);
218  inline bool is_open() {
219  return serial_dev_.is_open();
220  }
221 
222  // Set the number of BLDC motors and tilting servos,
223  // if using flags to differentiate.
224  static const size_t kNOutMax = 18u;
225  static const size_t kNumMotors = 12u;
226  static const size_t kNumServos = 6u;
227 
228  static const u_int32_t kMotorSpeedFlag = 1;
229  static const u_int32_t kServoPositionFlag = 2;
230 
231  double alt_home = 488.0; // meters
232 
233  unsigned _rotor_count;
234 
235  double input_offset_[kNOutMax];
236  double input_scaling_[kNOutMax];
237  std::string joint_control_type_[kNOutMax];
238  std::string gztopic_[kNOutMax];
239  double zero_position_disarmed_[kNOutMax];
240  double zero_position_armed_[kNOutMax];
241  int input_index_[kNOutMax];
242  transport::PublisherPtr joint_control_pub_[kNOutMax];
243 
244  transport::SubscriberPtr imu_sub_;
245  transport::SubscriberPtr lidar_sub_;
246  transport::SubscriberPtr sonar_sub_;
247  transport::SubscriberPtr opticalFlow_sub_;
248 
249  std::string imu_sub_topic_;
250  std::string lidar_sub_topic_;
252 
253  common::Time last_time_;
254  common::Time last_imu_time_;
255  common::Time last_actuator_time_;
256  double imu_update_interval_ = kDefaultImuInterval;
257  double lat_rad_;
258  double lon_rad_;
259 
260  void handle_control(double _dt);
261 
262  ignition::math::Vector3d gravity_W_;
263  ignition::math::Vector3d velocity_prev_W_;
264  ignition::math::Vector3d mag_d_;
265 
266  std::default_random_engine rand_;
267  std::normal_distribution<float> randn_;
268 
269  int _fd;
270  struct sockaddr_in myaddr_;
271  struct sockaddr_in srcaddr_;
272  socklen_t addrlen_;
273  unsigned char buf_[65535];
274  struct pollfd fds[1];
275 
276  // so we dont have to do extra callbacks
277  ignition::math::Vector3d optflow_gyro_{};
279 
280  in_addr_t mavlink_addr_;
282 
283  in_addr_t qgc_addr_;
285 
286  // Serial interface
290  std::thread io_thread_;
291  std::string device_;
292  std::array<uint8_t, MAX_SIZE> rx_buf_;
293  std::recursive_mutex mutex_;
294  unsigned int baudrate_;
295  std::atomic<bool> tx_in_progress_;
296  std::deque<MsgBuffer> tx_q_;
298  boost::asio::serial_port serial_dev_;
299 
300  bool hil_mode_;
302 };
303 } // namespace gazebo
transport::SubscriberPtr opticalFlow_sub_
void parse_buffer(const char *pfx, uint8_t *buf, const size_t bufsize, size_t bytes_received)
std::lock_guard< std::recursive_mutex > lock_guard
const boost::shared_ptr< const gz_mav_msgs::CommandMotorSpeed > CommandMotorSpeedPtr
boost::asio::io_service io_service
void handle_message(const mavlink::mavlink_message_t *mmsg, msgT &rst)
transport::PublisherPtr motor_velocity_reference_pub_
void close() override
static const std::string kDefaultMotorVelocityReferencePubTopic
const boost::shared_ptr< const lidar_msgs::msgs::lidar > LidarPtr
const boost::shared_ptr< const gz_sensor_msgs::Imu > ImuPtr
static const std::string kDefaultNamespace
Definition: common.h:48
ignition::math::Vector3d velocity_prev_W_
static const std::string kDefaultLidarTopic
static const std::string kDefaultImuTopic
def open(path, mode)
Framing
Rx packer framing status. (same as mavlink::mavlink_framing_t)
const boost::shared_ptr< const opticalFlow_msgs::msgs::opticalFlow > OpticalFlowPtr
transport::SubscriberPtr mav_control_sub_
void do_read()
event::ConnectionPtr updateConnection_
Pointer to the update event connection.
std::array< uint8_t, MAX_SIZE > rx_buf_
static const std::string kDefaultOpticalFlowTopic
std::normal_distribution< float > randn_
std::vector< physics::JointPtr > joints_
void do_write(bool check_tx_state)


rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Mon Feb 28 2022 23:39:03