Skip to content

Commit

Permalink
experiment with 2 drones spawning
Browse files Browse the repository at this point in the history
  • Loading branch information
haitomatic committed Oct 16, 2023
1 parent d889d93 commit 34d3ebe
Show file tree
Hide file tree
Showing 27 changed files with 294,648 additions and 594 deletions.
13 changes: 12 additions & 1 deletion build.sh
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

29 changes: 7 additions & 22 deletions include/gazebo_mavlink_interface.h
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 @@ -223,9 +211,6 @@ namespace mavlink_interface
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
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
Binary file added launch/__pycache__/model.launch.cpython-310.pyc
Binary file not shown.
87 changes: 87 additions & 0 deletions launch/jinja_gen.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,87 @@
#!/usr/bin/env python3


import jinja2
import argparse
import os
import shutil
import fnmatch
import numpy as np


def get_file_contents(filepath):
with open(filepath, 'rb') as f:
return f.read()

def str2bool(v):
if v.lower() in ('yes', 'true', 't', 'y', '1'):
return True
elif v.lower() in ('no', 'false', 'f', 'n', '0'):
return False
else:
raise argparse.ArgumentTypeError('Boolean value expected.')

if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument('filename', help="file that the sdf file should be generated from")
parser.add_argument('env_dir')
parser.add_argument('--output-file', help="sdf output file")
parser.add_argument('--stdout', action='store_true', default=False, help="dump to stdout instead of file")
parser.add_argument('--vehicle_name', default="ssrc_fog_x", help="Name of the vehicle")
parser.add_argument('--mavlink_addr', default="127.0.0.1", help='Address of PX4 mavlink receiver')
parser.add_argument('--mavlink_udp_local_port', default="14540", help='listening UDP port of mavlink plugin')
parser.add_argument('--mavlink_udp_remote_port', default="14541", help='UDP port of PX4 mavlink receiver')
parser.add_argument('--mavlink_tcp_port', default="4560", help='TCP port of PX4 mavlink receiver')

args = parser.parse_args()
env = jinja2.Environment(loader=jinja2.FileSystemLoader(args.env_dir))
template = env.get_template(os.path.relpath(args.filename, args.env_dir))

# create dictionary with useful modules etc.
try:
import rospkg
rospack = rospkg.RosPack()
except ImportError:
pass
rospack = None

d = {'np': np, 'rospack': rospack, \
'vehicle_name': args.vehicle_name, \
'mavlink_addr': args.mavlink_addr, \
'mavlink_udp_local_port': args.mavlink_udp_local_port, \
'mavlink_udp_remote_port': args.mavlink_udp_remote_port, \
'mavlink_tcp_port': args.mavlink_tcp_port}

result = template.render(d)

if args.stdout:
print(result)

else:
if args.output_file:
filename_out = args.output_file
else:
if not args.filename.endswith('.sdf.jinja'):
raise Exception("ERROR: Output file can only be determined automatically for " + \
"input files with the .sdf.jinja extension")
filename_out = args.filename.replace('.sdf.jinja', '.sdf')
assert filename_out != args.filename, "Not allowed to overwrite template"

# Overwrite protection mechanism: after generation, the file will be copied to a "last_generated" file.
# In the next run, we can check whether the target file is still unmodified.
filename_out_last_generated = filename_out + '.last_generated'

if os.path.exists(filename_out) and os.path.exists(filename_out_last_generated):
# Check whether the target file is still unmodified.
if get_file_contents(filename_out).strip() != get_file_contents(filename_out_last_generated).strip():
raise Exception("ERROR: generation would overwrite changes to `{}`. ".format(filename_out) + \
"Changes should only be made to the template file `{}`. ".format(args.filename) + \
"Remove `{}` ".format(os.path.basename(filename_out)) + \
"(after extracting your changes) to disable this overwrite protection.")

with open(filename_out, 'w') as f_out:
print(('{:s} -> {:s}'.format(args.filename, filename_out)))
f_out.write(result)

# Copy the contents to a "last_generated" file for overwrite protection check next time.
shutil.copy(filename_out, filename_out_last_generated)
Loading

0 comments on commit 34d3ebe

Please sign in to comment.