Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Changes for gzsim HITL support #5

Merged
merged 4 commits into from
Jan 23, 2024
Merged

Changes for gzsim HITL support #5

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
330314f
simplify to only transfer needed sensor and groundtruth msg; support …
haitomatic Oct 16, 2023
f5813c6
Fix build.sh to build .so lib only
haitomatic Oct 16, 2023
181e4e3
cleanup unused models and world
haitomatic Jan 19, 2024
5356001
Change docker image artifact to only include .so file
haitomatic Jan 19, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions Dockerfile
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
FROM ubuntu:22.04 AS builder

ENV DEBIAN_FRONTEND=noninteractive
ENV DEBIAN_FRONTEND=noninteractive

RUN apt-get update -y \
&& apt install -y \
Expand Down Expand Up @@ -32,5 +32,5 @@ RUN ./build.sh
FROM busybox

WORKDIR /artifacts
COPY --from=builder /px4-plugins/build/*.deb /artifacts
COPY --from=builder /px4-plugins/build/*.so /artifacts

2 changes: 0 additions & 2 deletions build.sh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,5 +6,3 @@ fi
cd build
cmake ..
make
cpack -G DEB

30 changes: 7 additions & 23 deletions include/gazebo_mavlink_interface.h
View file
Open in desktop
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
47 changes: 1 addition & 46 deletions include/mavlink_interface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,10 +58,8 @@
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 kMaxRecvBufferSize = 30;
static const size_t kMaxSendBufferSize = 30;

using lock_guard = std::lock_guard<std::recursive_mutex>;
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