diff --git a/include/common.h b/include/common.h index eb84b26..e63e8ce 100644 --- a/include/common.h +++ b/include/common.h @@ -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 @@ -218,4 +218,4 @@ inline std::pair reproject(gz::math::Vector3d& pos, return std::make_pair (lat_rad, lon_rad); } -#endif // SITL_GAZEBO_COMMON_H_ +#endif // HITL_GAZEBO_COMMON_H_ diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h index ee94224..36890b6 100644 --- a/include/gazebo_mavlink_interface.h +++ b/include/gazebo_mavlink_interface.h @@ -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 { @@ -119,7 +120,10 @@ namespace mavlink_interface gz::common::ConnectionPtr sigIntConnection_; std::shared_ptr 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}; @@ -143,10 +147,11 @@ 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(); @@ -154,19 +159,24 @@ namespace mavlink_interface 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}; @@ -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}; diff --git a/include/mavlink_interface.h b/include/mavlink_interface.h index 5eb90a9..ec8d8f6 100644 --- a/include/mavlink_interface.h +++ b/include/mavlink_interface.h @@ -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, @@ -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_; diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp index a3163e2..227670d 100644 --- a/src/gazebo_mavlink_interface.cpp +++ b/src/gazebo_mavlink_interface.cpp @@ -30,6 +30,7 @@ #include #include #include +#include GZ_ADD_PLUGIN( mavlink_interface::GazeboMavlinkInterface, @@ -40,11 +41,11 @@ GZ_ADD_PLUGIN( using namespace mavlink_interface; GazeboMavlinkInterface::GazeboMavlinkInterface() : - input_scaling_ {}, - input_index_ {} - { - mavlink_interface_ = std::make_shared(); - } + motor_input_index_ {}, + servo_input_index_ {} +{ + mavlink_interface_ = std::make_shared(); +} GazeboMavlinkInterface::~GazeboMavlinkInterface() { mavlink_interface_->close(); @@ -70,7 +71,6 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity, protocol_version_ = _sdf->Get("protocol_version"); } - gazebo::getSdfParam(_sdf, "poseSubTopic", pose_sub_topic_, pose_sub_topic_); gazebo::getSdfParam(_sdf, "gpsSubTopic", gps_sub_topic_, gps_sub_topic_); gazebo::getSdfParam(_sdf, "visionSubTopic", vision_sub_topic_, vision_sub_topic_); @@ -78,17 +78,15 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity, gazebo::getSdfParam(_sdf, "irlockSubTopic", irlock_sub_topic_, irlock_sub_topic_); gazebo::getSdfParam(_sdf, "imuSubTopic", imu_sub_topic_, imu_sub_topic_); gazebo::getSdfParam(_sdf, "magSubTopic", mag_sub_topic_, mag_sub_topic_); + gazebo::getSdfParam(_sdf, "cmdVelSubTopic", cmd_vel_sub_topic_, cmd_vel_sub_topic_); gazebo::getSdfParam(_sdf, "baroSubTopic", baro_sub_topic_, baro_sub_topic_); - // set input_reference_ from inputs.control - input_reference_.resize(n_out_max); + // Set motor and servo input_reference_ from inputs.control + motor_input_reference_.resize(n_out_max); + servo_input_reference_.resize(n_out_max); - ///TODO: Parse input reference - input_scaling_.resize(n_out_max); - input_scaling_(0) = 1000; - input_scaling_(1) = 1000; - input_scaling_(2) = 1000; - input_scaling_(3) = 1000; + // Parse the MulticopterMotorModel plugins to get the motor velocity scalings + ParseMulticopterMotorModelPlugins(model_.SourceFilePath(_ecm)); bool use_tcp = false; if (_sdf->HasElement("use_tcp")) @@ -103,7 +101,7 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity, tcp_client_mode = _sdf->Get("tcp_client_mode"); mavlink_interface_->SetUseTcpClientMode(tcp_client_mode); } - gzmsg << "Connecting to PX4 SITL using " << (use_tcp ? (tcp_client_mode ? "TCP (client mode)" : "TCP (server mode)") : "UDP") << std::endl; + gzmsg << "Connecting to PX4 HITL using " << (use_tcp ? (tcp_client_mode ? "TCP (client mode)" : "TCP (server mode)") : "UDP") << std::endl; if (_sdf->HasElement("enable_lockstep")) { @@ -129,14 +127,26 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity, gzmsg << "Speed factor set to: " << speed_factor_ << std::endl; } - // // Listen to Ctrl+C / SIGINT. + // Listen to Ctrl+C / SIGINT. sigIntConnection_ = _em.Connect(std::bind(&GazeboMavlinkInterface::onSigInt, this)); auto world_name = "/" + gz::sim::scopedName(gz::sim::worldEntity(_ecm), _ecm); - auto vehicle_scope_prefix = world_name + gz::sim::topicFromScopedName( + auto model_name = gz::sim::topicFromScopedName( _ecm.EntityByComponents(gz::sim::components::Name(model_name_)), _ecm, false); + auto vehicle_scope_prefix = world_name + model_name; + + // Publish to servo control + auto servo_control_topic = model_name + "/servo_"; + for (int i = 0; i < servo_input_reference_.size(); i++) { + servo_control_pub_[i] = node.Advertise(servo_control_topic + std::to_string(i)); + } + + // Publish to cmd vel (for rover control) + auto cmd_vel_topic = model_name + cmd_vel_sub_topic_; + cmd_vel_pub_ = node.Advertise(cmd_vel_topic); + // Subscribe to messages of sensors. auto imu_topic = vehicle_scope_prefix + imu_sub_topic_; node.Subscribe(imu_topic, &GazeboMavlinkInterface::ImuCallback, this); @@ -238,10 +248,13 @@ void GazeboMavlinkInterface::PreUpdate(const gz::sim::UpdateInfo &_info, handle_actuator_controls(_info); - handle_control(dt); - if (received_first_actuator_) { - PublishRotorVelocities(_ecm, input_reference_); + if (input_is_cmd_vel_) { + PublishCmdVelocities(cmd_vel_thrust_, cmd_vel_torque_); + } else { + PublishMotorVelocities(_ecm, motor_input_reference_); + PublishServoVelocities(servo_input_reference_); + } } } @@ -417,31 +430,59 @@ void GazeboMavlinkInterface::handle_actuator_controls(const gz::sim::UpdateInfo last_actuator_time_ = _info.simTime; - for (unsigned i = 0; i < n_out_max; i++) { - input_index_[i] = i; - } - // Read Input References - input_reference_.resize(n_out_max); - Eigen::VectorXd actuator_controls = mavlink_interface_->GetActuatorControls(); if (actuator_controls.size() < n_out_max) return; //TODO: Handle this properly - for (int i = 0; i < input_reference_.size(); i++) { + // Read Cmd vel input for rover + if (actuator_controls[n_out_max - 1] != 0.0 || actuator_controls[n_out_max - 2] != 0.0) { + cmd_vel_thrust_ = armed ? actuator_controls[n_out_max - 1] : 0.0; + cmd_vel_torque_ = armed ? actuator_controls[n_out_max - 2] : 0.0; + input_is_cmd_vel_ = true; + received_first_actuator_ = mavlink_interface_->GetReceivedFirstActuator(); + return; + } else { + input_is_cmd_vel_ = false; + } + + // Read Input References for servos + if (servo_input_reference_.size() == n_out_max) { + unsigned n_servos = 0; + for (unsigned i = 0; i < n_out_max; i++) { + if (!mavlink_interface_->IsInputMotorAtIndex(i)) { + servo_input_index_[n_servos++] = i; + } + } + servo_input_reference_.resize(n_servos); + } + + for (int i = 0; i < servo_input_reference_.size(); i++) { if (armed) { - input_reference_[i] = actuator_controls[input_index_[i]] * input_scaling_(i); + servo_input_reference_[i] = actuator_controls[servo_input_index_[i]]; } else { - input_reference_[i] = 0; - // std::cout << input_reference_ << ", "; + servo_input_reference_[i] = 0; } } - received_first_actuator_ = mavlink_interface_->GetReceivedFirstActuator(); -} -void GazeboMavlinkInterface::handle_control(double _dt) -{ - // set joint positions - // for (int i = 0; i < input_reference_.size(); i++) { - // } + // Read Input References for motors + if (motor_input_reference_.size() == n_out_max) { + unsigned n_motors = 0; + for (unsigned i = 0; i < n_out_max; i++) { + if (mavlink_interface_->IsInputMotorAtIndex(i)) { + motor_input_index_[n_motors++] = i; + } + } + motor_input_reference_.resize(n_motors); + } + + for (int i = 0; i < motor_input_reference_.size(); i++) { + if (armed) { + motor_input_reference_[i] = actuator_controls[motor_input_index_[i]] * motor_vel_scalings_[i]; + } else { + motor_input_reference_[i] = 0; + } + } + + received_first_actuator_ = mavlink_interface_->GetReceivedFirstActuator(); } bool GazeboMavlinkInterface::IsRunning() @@ -454,17 +495,17 @@ void GazeboMavlinkInterface::onSigInt() { } // The following snippet was copied from https://github.com/gzrobotics/ign-gazebo/blob/ign-gazebo4/src/systems/multicopter_control/MulticopterVelocityControl.cc -void GazeboMavlinkInterface::PublishRotorVelocities( +void GazeboMavlinkInterface::PublishMotorVelocities( gz::sim::EntityComponentManager &_ecm, const Eigen::VectorXd &_vels) { - if (_vels.size() != rotor_velocity_message_.velocity_size()) + if (_vels.size() != motor_velocity_message_.velocity_size()) { - rotor_velocity_message_.mutable_velocity()->Resize(_vels.size(), 0); + motor_velocity_message_.mutable_velocity()->Resize(_vels.size(), 0); } for (int i = 0; i < _vels.size(); ++i) { - rotor_velocity_message_.set_velocity(i, _vels(i)); + motor_velocity_message_.set_velocity(i, _vels(i)); } // Publish the message by setting the Actuators component on the model entity. // This assumes that the MulticopterMotorModel system is attached to this @@ -479,7 +520,7 @@ void GazeboMavlinkInterface::PublishRotorVelocities( return std::equal(_a.velocity().begin(), _a.velocity().end(), _b.velocity().begin()); }; - auto state = actuatorMsgComp->SetData(this->rotor_velocity_message_, compFunc) + auto state = actuatorMsgComp->SetData(this->motor_velocity_message_, compFunc) ? gz::sim::ComponentState::PeriodicChange : gz::sim::ComponentState::NoChange; _ecm.SetChanged(model_.Entity(), gz::sim::components::Actuators::typeId, state); @@ -487,7 +528,27 @@ void GazeboMavlinkInterface::PublishRotorVelocities( else { _ecm.CreateComponent(model_.Entity(), - gz::sim::components::Actuators(this->rotor_velocity_message_)); + gz::sim::components::Actuators(this->motor_velocity_message_)); + } +} + +void GazeboMavlinkInterface::PublishServoVelocities(const Eigen::VectorXd &_vels) +{ + for (int i = 0; i < _vels.size(); i++) { + gz::msgs::Double servo_input; + servo_input.set_data(_vels(i)); + servo_control_pub_[i].Publish(servo_input); + } +} + +void GazeboMavlinkInterface::PublishCmdVelocities(const float _thrust, const float _torque) +{ + gz::msgs::Twist cmd_vel_message; + cmd_vel_message.mutable_linear()->set_x(_thrust); + cmd_vel_message.mutable_angular()->set_z(_torque); + + if (cmd_vel_pub_.Valid()) { + cmd_vel_pub_.Publish(cmd_vel_message); } } @@ -545,4 +606,49 @@ void GazeboMavlinkInterface::RotateQuaternion(gz::math::Quaterniond &q_FRD_to_NE // final rotation composition q_FRD_to_NED = q_ENU_to_NED * q_FLU_to_ENU * q_FLU_to_FRD.Inverse(); -} \ No newline at end of file +} + +void GazeboMavlinkInterface::ParseMulticopterMotorModelPlugins(const std::string &sdfFilePath) +{ + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(sdfFilePath); + if (!errors.empty()) + { + for (const auto &error : errors) + { + gzerr << "[gazebo_mavlink_interface] Error: " << error.Message() << std::endl; + } + return; + } + + // Load the model + const sdf::Model *model = root.Model(); + if (!model) + { + gzerr << "[gazebo_mavlink_interface] No models found in SDF file." << std::endl; + return; + } + + // Iterate through all plugins in the model + for (const sdf::Plugin plugin : model->Plugins()) + { + // Check if the plugin is a MulticopterMotorModel + if (plugin.Name() == "gz::sim::systems::MulticopterMotorModel") { + if (plugin.Element()->HasElement("motorNumber")) + { + const int motorNumber = plugin.Element()->Get("motorNumber"); + if (motorNumber >= n_out_max) + { + gzerr << "[gazebo_mavlink_interface] Motor number " << motorNumber + << " exceeds maximum number of motors " << n_out_max << std::endl; + continue; + } + if (plugin.Element()->HasElement("motorNumber")) + { + motor_vel_scalings_[motorNumber] = plugin.Element()->Get("maxRotVelocity"); + } + } + } + } +} diff --git a/src/mavlink_interface.cpp b/src/mavlink_interface.cpp index f87bca3..d76b947 100644 --- a/src/mavlink_interface.cpp +++ b/src/mavlink_interface.cpp @@ -97,13 +97,13 @@ void MavlinkInterface::Load() fds_[LISTEN_FD].events = POLLIN; // only listens for new connections on tcp } } else { - // When connecting to SITL, we specify the port where the mavlink traffic originates from. + // When connecting to HITL, we specify the port where the mavlink traffic originates from. remote_simulator_addr_.sin_addr.s_addr = mavlink_addr_; remote_simulator_addr_.sin_port = htons(mavlink_udp_remote_port_); local_simulator_addr_.sin_addr.s_addr = htonl(INADDR_ANY); local_simulator_addr_.sin_port = htons(mavlink_udp_local_port_); - std::cout << "Creating UDP socket for SITL input on local port : " << mavlink_udp_local_port_ << " and remote port " << mavlink_udp_remote_port_ << std::endl; + std::cout << "Creating UDP socket for HITL input on local port : " << mavlink_udp_local_port_ << " and remote port " << mavlink_udp_remote_port_ << std::endl; if ((simulator_socket_fd_ = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { std::cerr << "Creating UDP socket failed: " << strerror(errno) << ", aborting" << std::endl; @@ -474,15 +474,12 @@ void MavlinkInterface::handle_actuator_controls(mavlink_message_t *msg) mavlink_hil_actuator_controls_t controls; mavlink_msg_hil_actuator_controls_decode(msg, &controls); - armed_ = (controls.mode & MAV_MODE_FLAG_SAFETY_ARMED); + armed_ = (controls.mode & MAV_MODE_FLAG_SAFETY_ARMED || controls.mode & MAV_MODE_FLAG_TEST_ENABLED); - for (unsigned i = 0; i < n_out_max; i++) { - input_index_[i] = i; - } - - // set rotor speeds, controller targets + // set rotor and servo speeds, controller targets input_reference_.resize(n_out_max); for (int i = 0; i < input_reference_.size(); i++) { + input_is_motor_[i] = controls.flags & (1 << i); input_reference_[i] = controls.controls[i]; } received_actuator_ = true; @@ -553,6 +550,11 @@ Eigen::VectorXd MavlinkInterface::GetActuatorControls() { return input_reference_; } +bool MavlinkInterface::IsInputMotorAtIndex(int index) { + const std::lock_guard lock(actuator_mutex_); + return input_is_motor_[index]; +} + bool MavlinkInterface::GetArmedState() { const std::lock_guard lock(actuator_mutex_); return armed_;