Skip to content

Commit

Permalink
simplify to only transfer needed sensor and groundtruth msg; support …
Browse files Browse the repository at this point in the history 
…only TCP and UDP; clean up unused features
  • Loading branch information
@haitomatic
haitomatic committed Oct 16, 2023
1 parent d889d93 commit 330314f
Show file tree
Hide file tree
Showing 6 changed files with 246 additions and 985 deletions.
13 changes: 12 additions & 1 deletion build.sh
View file
Original file line number Diff line number Diff line change
@@ -1,10 +1,21 @@
#!/bin/bash

# present working directory
PWD=$(pwd)

# Clone c_library_v2 commit matching with current px4-firmware mavlink commit
# => mavlink/c_library_v2:fbdb7c29 is built from mavlink/mavlink:08112084
git clone -q https://github.com/mavlink/c_library_v2.git ${PWD}/mavlink && \
cd ${PWD}/mavlink && git checkout -q fbdb7c29e47902d44eeaa58b4395678a9b78f3ae && \
rm -rf ${PWD}/mavlink/.git && cd ${PWD}

export _MAVLINK_INCLUDE_DIR=${PWD}/mavlink


if [ ! -e build ]; then
mkdir build
fi
cd build
cmake ..
make
cpack -G DEB

30 changes: 7 additions & 23 deletions include/gazebo_mavlink_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,8 @@
#include <gz/sim/components/Pose.hh>

#include <gz/transport/Node.hh>
#include <gz/msgs/imu.pb.h>
#include <gz/msgs.hh>
#include <gz/math.hh>

#include <common.h>

Expand All @@ -86,6 +87,7 @@ static const std::string kDefaultNamespace = "";
// ConsPtr passing, such that the original commands don't have to go n_motors-times over the wire.
static const std::string kDefaultMotorVelocityReferencePubTopic = "/gazebo/command/motor_speed";

static const std::string kDefaultPoseTopic = "/pose";
static const std::string kDefaultImuTopic = "/imu";
static const std::string kDefaultOpticalFlowTopic = "/px4flow/link/opticalFlow";
static const std::string kDefaultIRLockTopic = "/camera/link/irlock";
Expand All @@ -94,11 +96,6 @@ static const std::string kDefaultVisionTopic = "/vision_odom";
static const std::string kDefaultMagTopic = "/magnetometer";
static const std::string kDefaultBarometerTopic = "/air_pressure";

static const std::string kDefaultImuSensorName = "imu_sensor";
static const std::string kDefaultGPSSensorName= "gps";
static const std::string kDefaultMagSensorName = "mag_sensor";
static const std::string kDefaultBarometerSensorName = "air_pressure_sensor";

namespace mavlink_interface
{
class GZ_SIM_VISIBLE GazeboMavlinkInterface:
Expand Down Expand Up @@ -132,7 +129,6 @@ namespace mavlink_interface
float protocol_version_{2.0};

std::string namespace_{kDefaultNamespace};
std::string motor_velocity_reference_pub_topic_{kDefaultMotorVelocityReferencePubTopic};
std::string mavlink_control_sub_topic_;
std::string link_name_;

Expand All @@ -141,12 +137,12 @@ namespace mavlink_interface
bool use_left_elevon_pid_{false};
bool use_right_elevon_pid_{false};

void PoseCallback(const gz::msgs::Pose_V &_msg);
void ImuCallback(const gz::msgs::IMU &_msg);
void BarometerCallback(const gz::msgs::FluidPressure &_msg);
void MagnetometerCallback(const gz::msgs::Magnetometer &_msg);
void GpsCallback(const gz::msgs::NavSat &_msg);
void SendSensorMessages(const gz::sim::UpdateInfo &_info);
void SendGroundTruth();
void PublishRotorVelocities(gz::sim::EntityComponentManager &_ecm,
const Eigen::VectorXd &_vels);
void handle_actuator_controls(const gz::sim::UpdateInfo &_info);
Expand All @@ -156,6 +152,8 @@ namespace mavlink_interface
bool resolveHostName();
void ResolveWorker();
float AddSimpleNoise(float value, float mean, float stddev);
void RotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED,
const gz::math::Quaterniond q_FLU_to_ENU);

static const unsigned n_out_max = 16;

Expand All @@ -170,20 +168,15 @@ namespace mavlink_interface
/// \brief gz communication node.
gz::transport::Node node;

std::string pose_sub_topic_{kDefaultPoseTopic};
std::string imu_sub_topic_{kDefaultImuTopic};
std::string opticalFlow_sub_topic_{kDefaultOpticalFlowTopic};
std::string irlock_sub_topic_{kDefaultIRLockTopic};
std::string gps_sub_topic_{kDefaultGPSTopic};
std::string groundtruth_sub_topic_;
std::string vision_sub_topic_{kDefaultVisionTopic};
std::string mag_sub_topic_{kDefaultMagTopic};
std::string baro_sub_topic_{kDefaultBarometerTopic};

std::string imu_sensor_name_{kDefaultImuSensorName};
std::string gps_sensor_name_{kDefaultGPSSensorName};
std::string mag_sensor_name_{kDefaultMagSensorName};
std::string baro_sensor_name_{kDefaultBarometerSensorName};

std::mutex last_imu_message_mutex_ {};

gz::msgs::IMU last_imu_message_;
Expand All @@ -192,16 +185,11 @@ namespace mavlink_interface
std::chrono::steady_clock::duration last_imu_time_{0};
std::chrono::steady_clock::duration lastControllerUpdateTime{0};
std::chrono::steady_clock::duration last_actuator_time_{0};
std::chrono::steady_clock::duration last_heartbeat_sent_time_{0};

bool mag_updated_{false};
bool baro_updated_;
bool diff_press_updated_;

double groundtruth_lat_rad{0.0};
double groundtruth_lon_rad{0.0};
double groundtruth_altitude{0.0};

double imu_update_interval_ = 0.004; ///< Used for non-lockstep

gz::math::Vector3d gravity_W_{gz::math::Vector3d(0.0, 0.0, -9.8)};
Expand All @@ -218,14 +206,10 @@ namespace mavlink_interface
double sonar_distance;

bool enable_lockstep_ = false;
bool serial_enabled_;
double speed_factor_ = 1.0;
uint8_t previous_imu_seq_ = 0;
uint8_t update_skip_factor_ = 1;

bool hil_mode_{false};
bool hil_state_level_{false};

std::string mavlink_hostname_str_;
struct hostent *hostptr_{nullptr};
bool mavlink_loaded_{false};
Expand Down
45 changes: 0 additions & 45 deletions include/mavlink_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,8 +58,6 @@
static const uint32_t kDefaultMavlinkUdpRemotePort = 14560;
static const uint32_t kDefaultMavlinkUdpLocalPort = 0;
static const uint32_t kDefaultMavlinkTcpPort = 4560;
static const uint32_t kDefaultQGCUdpPort = 14550;
static const uint32_t kDefaultSDKUdpPort = 14540;

static const size_t kMaxRecvBufferSize = 20;
static const size_t kMaxSendBufferSize = 30;
Expand Down Expand Up @@ -144,7 +142,6 @@ class MavlinkInterface {
void open();
void close();
void Load();
void SendHeartbeat();
void SendSensorMessages(const uint64_t time_usec);
void UpdateBarometer(const SensorData::Barometer &data);
void UpdateAirspeed(const SensorData::Airspeed &data);
Expand All @@ -157,7 +154,6 @@ class MavlinkInterface {
uint8_t min_length, uint8_t length, uint8_t crc_extra);
bool GetReceivedFirstActuator() {return received_first_actuator_;}
void SetBaudrate(int baudrate) {baudrate_ = baudrate;}
void SetSerialEnabled(bool serial_enabled) {serial_enabled_ = serial_enabled;}
void SetUseTcp(bool use_tcp) {use_tcp_ = use_tcp;}
void SetUseTcpClientMode(bool tcp_client_mode) {tcp_client_mode_ = tcp_client_mode;}
void SetDevice(std::string device) {device_ = device;}
Expand All @@ -166,15 +162,8 @@ class MavlinkInterface {
void SetMavlinkTcpPort(int mavlink_tcp_port) {mavlink_tcp_port_ = mavlink_tcp_port;}
void SetMavlinkUdpRemotePort(int mavlink_udp_port) {mavlink_udp_remote_port_ = mavlink_udp_port;}
void SetMavlinkUdpLocalPort(int mavlink_udp_port) {mavlink_udp_local_port_ = mavlink_udp_port;}
void SetQgcAddr(std::string qgc_addr) {qgc_addr_ = qgc_addr;}
void SetQgcUdpPort(int qgc_udp_port) {qgc_udp_port_ = qgc_udp_port;}
void SetSdkAddr(std::string sdk_addr) {sdk_addr_ = sdk_addr;}
void SetSdkUdpPort(int sdk_udp_port) {sdk_udp_port_ = sdk_udp_port;}
void SetHILMode(bool hil_mode) {hil_mode_ = hil_mode;}
void SetHILStateLevel(bool hil_state_level) {hil_state_level_ = hil_state_level;}
bool IsRecvBuffEmpty() {return receiver_buffer_.empty();}

bool SerialEnabled() const { return serial_enabled_; }
bool ReceivedHeartbeats() const { return received_heartbeats_; }

private:
Expand All @@ -191,15 +180,6 @@ class MavlinkInterface {
void RegisterNewHILSensorInstance(int id);
bool tryConnect();

// Serial interface
void open_serial();
void do_serial_read();
void parse_serial_buffer(const boost::system::error_code& err, std::size_t bytes_t);
inline bool is_serial_open(){
return serial_dev_.is_open();
}
void do_serial_write(bool check_tx_state);

// UDP/TCP send/receive thread workers
void ReceiveWorker();
void SendWorker();
Expand All @@ -213,20 +193,6 @@ class MavlinkInterface {
struct sockaddr_in remote_simulator_addr_;
socklen_t remote_simulator_addr_len_;

int qgc_udp_port_{kDefaultQGCUdpPort};
struct sockaddr_in remote_qgc_addr_;
socklen_t remote_qgc_addr_len_;
struct sockaddr_in local_qgc_addr_;
std::string qgc_addr_{"INADDR_ANY"};
socklen_t local_qgc_addr_len_;

int sdk_udp_port_{kDefaultSDKUdpPort};
struct sockaddr_in remote_sdk_addr_;
socklen_t remote_sdk_addr_len_;
struct sockaddr_in local_sdk_addr_;
socklen_t local_sdk_addr_len_;
std::string sdk_addr_{"INADDR_ANY"};

unsigned char buf_[65535];
enum FD_TYPES {
LISTEN_FD,
Expand All @@ -248,16 +214,8 @@ class MavlinkInterface {
int simulator_socket_fd_{0};
int simulator_tcp_client_fd_{0};

int qgc_socket_fd_{0};
int sdk_socket_fd_{0};

bool enable_lockstep_{false};

// Serial interface
boost::asio::io_service io_service_{};
boost::asio::serial_port serial_dev_;
bool serial_enabled_{false};

mavlink_status_t m_status_{};
mavlink_message_t m_buffer_{};
std::thread io_thread_;
Expand All @@ -272,9 +230,6 @@ class MavlinkInterface {
std::atomic<bool> tx_in_progress_;
std::deque<MsgBuffer> tx_q_{};

bool hil_mode_;
bool hil_state_level_;

bool baro_updated_;
bool diff_press_updated_;
bool mag_updated_;
Expand Down
Loading

0 comments on commit 330314f

Please sign in to comment.