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 0db49ce..978ac4a 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 { @@ -121,6 +122,8 @@ namespace mavlink_interface bool received_first_actuator_{false}; 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}; @@ -147,6 +150,7 @@ namespace mavlink_interface 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(); @@ -166,10 +170,12 @@ namespace mavlink_interface double zero_position_armed_[n_out_max]; int motor_input_index_[n_out_max]; 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}; @@ -179,6 +185,9 @@ 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}; + int mc_motor_vel_scaling_{1000}; + int fw_motor_vel_scaling_{3500}; std::mutex last_imu_message_mutex_ {}; diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp index 41dedd7..55b1358 100644 --- a/src/gazebo_mavlink_interface.cpp +++ b/src/gazebo_mavlink_interface.cpp @@ -79,7 +79,10 @@ 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_); + gazebo::getSdfParam(_sdf, "mc_motor_vel_scaling", mc_motor_vel_scaling_, mc_motor_vel_scaling_); + gazebo::getSdfParam(_sdf, "fw_motor_vel_scaling", fw_motor_vel_scaling_, fw_motor_vel_scaling_); // set motor and servo input_reference_ from inputs.control motor_input_reference_.resize(n_out_max); @@ -98,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")) { @@ -140,6 +143,10 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity, 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); @@ -244,8 +251,12 @@ void GazeboMavlinkInterface::PreUpdate(const gz::sim::UpdateInfo &_info, handle_control(dt); if (received_first_actuator_) { - PublishMotorVelocities(_ecm, motor_input_reference_); - PublishServoVelocities(servo_input_reference_); + if (input_is_cmd_vel_) { + PublishCmdVelocities(cmd_vel_thrust_, cmd_vel_torque_); + } else { + PublishMotorVelocities(_ecm, motor_input_reference_); + PublishServoVelocities(servo_input_reference_); + } } } @@ -424,37 +435,64 @@ void GazeboMavlinkInterface::handle_actuator_controls(const gz::sim::UpdateInfo Eigen::VectorXd actuator_controls = mavlink_interface_->GetActuatorControls(); if (actuator_controls.size() < n_out_max) return; //TODO: Handle this properly - // Read Input References for motors - unsigned n_motors = 0; - for (unsigned i = 0; i < n_out_max; i++) { - if (mavlink_interface_->IsInputMotorAtIndex(i)) { - motor_input_index_[n_motors++] = 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); } - motor_input_reference_.resize(n_motors); - for (int i = 0; i < motor_input_reference_.size(); i++) { + + for (int i = 0; i < servo_input_reference_.size(); i++) { if (armed) { - motor_input_reference_[i] = actuator_controls[motor_input_index_[i]] * 1000; + servo_input_reference_[i] = actuator_controls[servo_input_index_[i]]; } else { - motor_input_reference_[i] = 0; - // std::cout << motor_input_reference_ << ", "; + servo_input_reference_[i] = 0; } } - // Read Input References for servos - unsigned n_servos = 0; - for (unsigned i = 0; i < n_out_max; i++) { - if (!mavlink_interface_->IsInputMotorAtIndex(i)) { - servo_input_index_[n_servos++] = 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); } - servo_input_reference_.resize(n_servos); - for (int i = 0; i < servo_input_reference_.size(); i++) { + + for (int i = 0; i < motor_input_reference_.size(); i++) { if (armed) { - servo_input_reference_[i] = actuator_controls[servo_input_index_[i]]; + if (i == motor_input_reference_.size() - 1) { + // Set pusher motor velocity scaling if airframe is either fw or vtol. This assumes the pusher motor is the last motor! + double scaling; + if (servo_input_reference_.size() > 0 && servo_input_reference_.size() != n_out_max) { + scaling = static_cast(fw_motor_vel_scaling_); + } else { + scaling = static_cast(mc_motor_vel_scaling_); + } + motor_input_reference_[i] = actuator_controls[motor_input_index_[i]] * scaling; + } + else { + motor_input_reference_[i] = actuator_controls[motor_input_index_[i]] * static_cast(mc_motor_vel_scaling_); + } } else { - servo_input_reference_[i] = 0; - // std::cout << servo_input_reference_ << ", "; + motor_input_reference_[i] = 0; } } @@ -515,7 +553,6 @@ void GazeboMavlinkInterface::PublishMotorVelocities( } } -// Publish to servo control topic (gz.msgs.Double) void GazeboMavlinkInterface::PublishServoVelocities(const Eigen::VectorXd &_vels) { for (int i = 0; i < _vels.size(); i++) { @@ -525,6 +562,17 @@ void GazeboMavlinkInterface::PublishServoVelocities(const Eigen::VectorXd &_vels } } +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); + } +} + bool GazeboMavlinkInterface::resolveHostName() { if (!mavlink_hostname_str_.empty()) { diff --git a/src/mavlink_interface.cpp b/src/mavlink_interface.cpp index 2f1d23e..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,7 +474,7 @@ 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); // set rotor and servo speeds, controller targets input_reference_.resize(n_out_max);