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

Add HITL support for Fixed-wing, VTOL and Rover #9

Merged
merged 3 commits into from
Sep 26, 2024
Merged

Add HITL support for Fixed-wing, VTOL and Rover #9

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a22d53e
Add support for all motors and servos control for all UAV airframes
haitomatic Aug 19, 2024
7ae6aea
Add support for cmd_vel control for rover
haitomatic Aug 21, 2024
7c4bdeb
Fix motor vel scalings
haitomatic Sep 25, 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
6 changes: 3 additions & 3 deletions include/common.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
#ifndef SITL_GAZEBO_COMMON_H_
#define SITL_GAZEBO_COMMON_H_
#ifndef HITL_GAZEBO_COMMON_H_
#define HITL_GAZEBO_COMMON_H_
/*
* Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
* Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
Expand Down Expand Up @@ -218,4 +218,4 @@ inline std::pair<double, double> reproject(gz::math::Vector3d& pos,
return std::make_pair (lat_rad, lon_rad);
}

#endif // SITL_GAZEBO_COMMON_H_
#endif // HITL_GAZEBO_COMMON_H_
25 changes: 18 additions & 7 deletions include/gazebo_mavlink_interface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,7 @@ static const std::string kDefaultGPSTopic = "/gps";
static const std::string kDefaultVisionTopic = "/vision_odom";
static const std::string kDefaultMagTopic = "/magnetometer";
static const std::string kDefaultBarometerTopic = "/air_pressure";
static const std::string kDefaultCmdVelTopic = "/cmd_vel";

namespace mavlink_interface
{
Expand All @@ -119,7 +120,10 @@ namespace mavlink_interface
gz::common::ConnectionPtr sigIntConnection_;
std::shared_ptr<MavlinkInterface> mavlink_interface_;
bool received_first_actuator_{false};
Eigen::VectorXd input_reference_;
Eigen::VectorXd motor_input_reference_;
Eigen::VectorXd servo_input_reference_;
float cmd_vel_thrust_{0.0};
float cmd_vel_torque_{0.0};

gz::sim::Entity entity_{gz::sim::kNullEntity};
gz::sim::Model model_{gz::sim::kNullEntity};
Expand All @@ -143,30 +147,36 @@ namespace mavlink_interface
void MagnetometerCallback(const gz::msgs::Magnetometer &_msg);
void GpsCallback(const gz::msgs::NavSat &_msg);
void SendSensorMessages(const gz::sim::UpdateInfo &_info);
void PublishRotorVelocities(gz::sim::EntityComponentManager &_ecm,
void PublishMotorVelocities(gz::sim::EntityComponentManager &_ecm,
const Eigen::VectorXd &_vels);
void PublishServoVelocities(const Eigen::VectorXd &_vels);
void PublishCmdVelocities(const float _thrust, const float _torque);
void handle_actuator_controls(const gz::sim::UpdateInfo &_info);
void handle_control(double _dt);
void onSigInt();
bool IsRunning();
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);
void ParseMulticopterMotorModelPlugins(const std::string &sdfFilePath);

static const unsigned n_out_max = 16;

double input_offset_[n_out_max];
Eigen::VectorXd input_scaling_;
std::string joint_control_type_[n_out_max];
std::string gztopic_[n_out_max];
double zero_position_disarmed_[n_out_max];
double zero_position_armed_[n_out_max];
int input_index_[n_out_max];
int motor_input_index_[n_out_max];
double motor_vel_scalings_[n_out_max] {1.0};
int servo_input_index_[n_out_max];
bool input_is_cmd_vel_{false};

/// \brief gz communication node.
/// \brief gz communication node and publishers.
gz::transport::Node node;
gz::transport::Node::Publisher servo_control_pub_[n_out_max];
gz::transport::Node::Publisher cmd_vel_pub_;

std::string pose_sub_topic_{kDefaultPoseTopic};
std::string imu_sub_topic_{kDefaultImuTopic};
Expand All @@ -176,11 +186,12 @@ namespace mavlink_interface
std::string vision_sub_topic_{kDefaultVisionTopic};
std::string mag_sub_topic_{kDefaultMagTopic};
std::string baro_sub_topic_{kDefaultBarometerTopic};
std::string cmd_vel_sub_topic_{kDefaultCmdVelTopic};

std::mutex last_imu_message_mutex_ {};

gz::msgs::IMU last_imu_message_;
gz::msgs::Actuators rotor_velocity_message_;
gz::msgs::Actuators motor_velocity_message_;

std::chrono::steady_clock::duration last_imu_time_{0};
std::chrono::steady_clock::duration lastControllerUpdateTime{0};
Expand Down
3 changes: 2 additions & 1 deletion include/mavlink_interface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -148,6 +148,7 @@ class MavlinkInterface {
void UpdateIMU(const SensorData::Imu &data);
void UpdateMag(const SensorData::Magnetometer &data);
Eigen::VectorXd GetActuatorControls();
bool IsInputMotorAtIndex(int index);
bool GetArmedState();
void onSigInt();
uint16_t FinalizeOutgoingMessage(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
Expand Down Expand Up @@ -186,7 +187,7 @@ class MavlinkInterface {

static const unsigned n_out_max = 16;

int input_index_[n_out_max];
bool input_is_motor_[n_out_max];

struct sockaddr_in local_simulator_addr_;
socklen_t local_simulator_addr_len_;
Expand Down
Loading
Loading