Skip to content

Commit

Permalink
Add support for cmd_vel control for rover
Browse files Browse the repository at this point in the history
  • Loading branch information
@haitomatic
haitomatic committed Aug 21, 2024
1 parent a22d53e commit cc79840
Show file tree
Hide file tree
Showing 4 changed files with 62 additions and 21 deletions.
6 changes: 3 additions & 3 deletions include/common.h
View file
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_
7 changes: 6 additions & 1 deletion include/gazebo_mavlink_interface.h
View file
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 Down Expand Up @@ -147,6 +148,7 @@ namespace mavlink_interface
void PublishMotorVelocities(gz::sim::EntityComponentManager &_ecm,
const Eigen::VectorXd &_vels);
void PublishServoVelocities(const Eigen::VectorXd &_vels);
void PublishCmdVelocities(const Eigen::VectorXd &_vels);
void handle_actuator_controls(const gz::sim::UpdateInfo &_info);
void handle_control(double _dt);
void onSigInt();
Expand All @@ -166,10 +168,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};
Expand All @@ -179,6 +183,7 @@ 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_ {};

Expand Down
64 changes: 50 additions & 14 deletions src/gazebo_mavlink_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,6 +80,7 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity,
gazebo::getSdfParam<std::string>(_sdf, "imuSubTopic", imu_sub_topic_, imu_sub_topic_);
gazebo::getSdfParam<std::string>(_sdf, "magSubTopic", mag_sub_topic_, mag_sub_topic_);
gazebo::getSdfParam<std::string>(_sdf, "baroSubTopic", baro_sub_topic_, baro_sub_topic_);
gazebo::getSdfParam<std::string>(_sdf, "cmdVelSubTopic", cmd_vel_sub_topic_, cmd_vel_sub_topic_);

// set motor and servo input_reference_ from inputs.control
motor_input_reference_.resize(n_out_max);
Expand All @@ -98,7 +99,7 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity,
tcp_client_mode = _sdf->Get<bool>("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"))
{
Expand Down Expand Up @@ -140,6 +141,10 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity,
servo_control_pub_[i] = node.Advertise<gz::msgs::Double>(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<gz::msgs::Twist>(cmd_vel_topic);

// Subscribe to messages of sensors.
auto imu_topic = vehicle_scope_prefix + imu_sub_topic_;
node.Subscribe(imu_topic, &GazeboMavlinkInterface::ImuCallback, this);
Expand Down Expand Up @@ -244,8 +249,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(motor_input_reference_);
} else {
PublishMotorVelocities(_ecm, motor_input_reference_);
PublishServoVelocities(servo_input_reference_);
}
}
}

Expand Down Expand Up @@ -424,14 +433,28 @@ 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 Cmd vel input for rover
if ((actuator_controls[n_out_max - 1] - 0.0) > 1e-6 || (actuator_controls[n_out_max - 2] - 0.0) > 1e-6) {
motor_input_reference_[n_out_max - 1] = actuator_controls[n_out_max - 1];
motor_input_reference_[n_out_max - 2] = actuator_controls[n_out_max - 2];
input_is_cmd_vel_ = true;
received_first_actuator_ = mavlink_interface_->GetReceivedFirstActuator();
return;
} else {
input_is_cmd_vel_ = false;
}

// 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;
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);
}
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]] * 1000;
Expand All @@ -442,13 +465,16 @@ void GazeboMavlinkInterface::handle_actuator_controls(const gz::sim::UpdateInfo
}

// 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;
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);
}
servo_input_reference_.resize(n_servos);

for (int i = 0; i < servo_input_reference_.size(); i++) {
if (armed) {
servo_input_reference_[i] = actuator_controls[servo_input_index_[i]];
Expand Down Expand Up @@ -515,7 +541,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++) {
Expand All @@ -525,6 +550,17 @@ void GazeboMavlinkInterface::PublishServoVelocities(const Eigen::VectorXd &_vels
}
}

void GazeboMavlinkInterface::PublishCmdVelocities(const Eigen::VectorXd &_vels)
{
gz::msgs::Twist cmd_vel_message;
cmd_vel_message.mutable_linear()->set_x(_vels[n_out_max - 1]);
cmd_vel_message.mutable_angular()->set_z(_vels[n_out_max - 2]);

if (cmd_vel_pub_.Valid()) {
cmd_vel_pub_.Publish(cmd_vel_message);
}
}

bool GazeboMavlinkInterface::resolveHostName()
{
if (!mavlink_hostname_str_.empty()) {
Expand Down
6 changes: 3 additions & 3 deletions src/mavlink_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit cc79840

Please sign in to comment.