Skip to content

Commit

Permalink
gz bridge: cleanup sensor interface; use vehicle torque and thrust se…
Browse files Browse the repository at this point in the history 
…tpoint for rover control
  • Loading branch information
@haitomatic
haitomatic committed Oct 4, 2023
1 parent cb77e41 commit cfa6602
Show file tree
Hide file tree
Showing 2 changed files with 40 additions and 118 deletions.
146 changes: 31 additions & 115 deletions src/modules/simulation/gz_bridge/GZBridge.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -155,32 +155,14 @@ int GZBridge::init()
return PX4_ERROR;
}

// IMU: /world/$WORLD/model/$MODEL/link/base_link/sensor/imu_sensor/imu
// Odometry: /world/$WORLD/model/$MODEL/odometry_with_covariance
std::string odometry_topic = "/model/" + _model_name + "/odometry_with_covariance";

if (!_node.Subscribe(odometry_topic, &GZBridge::odometryCallback, this)) {
PX4_ERR("failed to subscribe to %s", odometry_topic.c_str());
return PX4_ERROR;
}

// GPS: /world/$WORLD/model/$MODEL/link/gps1_link/sensor/gps/navsat
std::string gps_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/gps1_link/sensor/gps/navsat";

if (!_node.Subscribe(gps_topic, &GZBridge::gpsCallback, this)) {
PX4_ERR("failed to subscribe to %s", gps_topic.c_str());
return PX4_ERROR;
}

// ESC feedback: /x500/command/motor_speed
if (_vehicle_type != "rover") {
std::string motor_speed_topic = "/" + _model_name + "/command/motor_speed";

if (!_node.Subscribe(motor_speed_topic, &GZBridge::motorSpeedCallback, this)) {
PX4_ERR("failed to subscribe to %s", motor_speed_topic.c_str());
return PX4_ERROR;
}
}

// output (rover vehicle type) eg /model/$MODEL_NAME/cmd_vel
if (_vehicle_type == "rover") {
std::string cmd_vel_topic = "/model/" + _model_name + "/cmd_vel";
Expand All @@ -191,15 +173,6 @@ int GZBridge::init()
return PX4_ERROR;
}

} else {
// output (other vehicle types) eg%5 /X500/command/motor_speed
std::string actuator_topic = "/" + _model_name + "/command/motor_speed";
_actuators_pub = _node.Advertise<gz::msgs::Actuators>(actuator_topic);

if (!_actuators_pub.Valid()) {
PX4_ERR("failed to advertise %s", actuator_topic.c_str());
return PX4_ERROR;
}
}

#if 0
Expand Down Expand Up @@ -515,81 +488,6 @@ void GZBridge::imuCallback(const gz::msgs::IMU &imu)
pthread_mutex_unlock(&_node_mutex);
}

void GZBridge::gpsCallback(const gz::msgs::NavSat &gps)
{
if (hrt_absolute_time() == 0) {
return;
}

pthread_mutex_lock(&_mutex);

const uint64_t time_us = (gps.header().stamp().sec() * 1000000) + (gps.header().stamp().nsec() / 1000);

if (time_us > _world_time_us.load()) {
updateClock(gps.header().stamp().sec(), gps.header().stamp().nsec());
}

// device id
device::Device::DeviceId device_id;
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
device_id.devid_s.bus = 0;
device_id.devid_s.address = 0;
device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM;

// publish gps
sensor_gps_s sensor_gps{};
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
sensor_gps.timestamp_sample = time_us;
sensor_gps.timestamp = time_us;
#else
sensor_gps.timestamp_sample = hrt_absolute_time();
sensor_gps.timestamp = hrt_absolute_time();
#endif

sensor_gps.device_id = device_id.devid;

sensor_gps.lat = roundf(gps.latitude_deg() * 1e7); // Latitude in 1E-7 degrees
sensor_gps.lon = roundf(gps.longitude_deg() * 1e7); // Longitude in 1E-7 degrees
sensor_gps.alt = roundf(static_cast<float>(gps.altitude()) *
1000.f); // Altitude in 1E-3 meters above MSL, (millimetres)
sensor_gps.alt_ellipsoid = sensor_gps.alt;

sensor_gps.s_variance_m_s = 0.5f;
sensor_gps.c_variance_rad = 0.1f;

sensor_gps.fix_type = 3; // 3D fix

sensor_gps.eph = 0.9f;
sensor_gps.epv = 1.78f;
sensor_gps.hdop = 0.7f;
sensor_gps.vdop = 1.1f;

sensor_gps.noise_per_ms = 0;
sensor_gps.automatic_gain_control = 0;
sensor_gps.jamming_state = 0;
sensor_gps.jamming_indicator = 0;

sensor_gps.vel_m_s = sqrtf(gps.velocity_east() * gps.velocity_east() +
gps.velocity_north() * gps.velocity_north());
sensor_gps.vel_n_m_s = gps.velocity_north();
sensor_gps.vel_e_m_s = gps.velocity_east();
sensor_gps.vel_d_m_s = gps.velocity_up();
sensor_gps.cog_rad = atan2(gps.velocity_east(), gps.velocity_north());
sensor_gps.vel_ned_valid = true;
sensor_gps.timestamp_time_relative = 0;
sensor_gps.time_utc_usec = 0;

sensor_gps.satellites_used = 8;

sensor_gps.heading = 0.0f;
sensor_gps.heading_offset = NAN;
sensor_gps.heading_accuracy = 0;

_sensor_gps_pub.publish(sensor_gps);

pthread_mutex_unlock(&_mutex);
}

void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose)
{
if (hrt_absolute_time() == 0) {
Expand Down Expand Up @@ -793,21 +691,39 @@ void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry

void GZBridge::updateCmdVel()
{
if (_actuator_controls_sub.updated()) {
actuator_controls_s actuator_controls_msg;
bool do_update = false;

if (_actuator_controls_sub.copy(&actuator_controls_msg)) {
auto throttle = actuator_controls_msg.control[actuator_controls_s::INDEX_THROTTLE];
auto steering = actuator_controls_msg.control[actuator_controls_s::INDEX_YAW];
// Check torque setppoint update
if (_vehicle_torque_setpoint_sub.updated()) {
vehicle_torque_setpoint_s vehicle_torque_setpoint_msg;

// publish cmd_vel
gz::msgs::Twist cmd_vel_message;
cmd_vel_message.mutable_linear()->set_x(throttle);
cmd_vel_message.mutable_angular()->set_z(steering);
if (_vehicle_torque_setpoint_sub.copy(&vehicle_torque_setpoint_msg)) {
_rover_yaw_control = vehicle_torque_setpoint_msg.xyz[2];
do_update = true;
}
}

if (_cmd_vel_pub.Valid()) {
_cmd_vel_pub.Publish(cmd_vel_message);
}
// Check thrust setpoint update
if (_vehicle_thrust_setpoint_sub.updated()) {
vehicle_thrust_setpoint_s vehicle_thrust_setpoint_msg;

if (_vehicle_thrust_setpoint_sub.copy(&vehicle_thrust_setpoint_msg)) {
_rover_throttle_control = vehicle_thrust_setpoint_msg.xyz[0];
do_update = true;
}
}

if (do_update) {
auto throttle = 1.0f * _rover_throttle_control;
auto steering = _rover_yaw_control;

// publish cmd_vel
gz::msgs::Twist cmd_vel_message;
cmd_vel_message.mutable_linear()->set_x(throttle);
cmd_vel_message.mutable_angular()->set_z(steering);

if (_cmd_vel_pub.Valid()) {
_cmd_vel_pub.Publish(cmd_vel_message);
}
}
}
Expand Down
12 changes: 9 additions & 3 deletions src/modules/simulation/gz_bridge/GZBridge.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,11 +50,12 @@
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/vehicle_odometry.h>

Expand Down Expand Up @@ -101,7 +102,6 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
// void airspeedCallback(const gz::msgs::AirSpeedSensor &air_pressure);
void barometerCallback(const gz::msgs::FluidPressure &air_pressure);
void imuCallback(const gz::msgs::IMU &imu);
void gpsCallback(const gz::msgs::NavSat &gps);
void poseInfoCallback(const gz::msgs::Pose_V &pose);
void odometryCallback(const gz::msgs::OdometryWithCovariance &odometry);
void updateCmdVel();
Expand All @@ -118,7 +118,10 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S

// Subscriptions
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::SubscriptionInterval _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint), 50_ms};
uORB::SubscriptionInterval _vehicle_torque_setpoint_sub{ORB_ID(vehicle_torque_setpoint), 50_ms};

// Publications
//uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::Publication<vehicle_angular_velocity_s> _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
uORB::Publication<vehicle_attitude_s> _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};
Expand All @@ -129,7 +132,6 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
uORB::PublicationMulti<sensor_accel_s> _sensor_accel_pub{ORB_ID(sensor_accel)};
uORB::PublicationMulti<sensor_gyro_s> _sensor_gyro_pub{ORB_ID(sensor_gyro)};
uORB::PublicationMulti<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};

GZMixingInterfaceESC _mixing_interface_esc{_node, _node_mutex};
GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex};
Expand All @@ -153,6 +155,10 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S

float _temperature{288.15}; // 15 degrees

float _rover_throttle_control{0.0f};

float _rover_yaw_control{0.0f};

gz::transport::Node _node;
gz::transport::Node::Publisher _cmd_vel_pub;

Expand Down

0 comments on commit cfa6602

Please sign in to comment.