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

simulation/gz_bridge: Add support for lidar altitude sensors #23755

Closed
wants to merge 2 commits into from
Closed

simulation/gz_bridge: Add support for lidar altitude sensors #23755

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
8c37e45
simulation/gz_bridge: add support for lidar altitude sensors
Myzhe Oct 1, 2024
61df80a
Merge branch 'main' into gz-lidar-altitude-sensor
Myzhe Oct 1, 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
50 changes: 45 additions & 5 deletions src/modules/simulation/gz_bridge/GZBridge.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -211,11 +211,20 @@ int GZBridge::init()
return PX4_ERROR;
}

// Laser Scan: optional
std::string laser_scan_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/link/sensor/lidar_2d_v2/scan";
// Lidar 1D Distance Scan: optional
std::string lidar_distance_scan_topic = "/world/" + _world_name + "/model/" + _model_name +
"/link/base_link/sensor/distance_sensor/scan";

if (!_node.Subscribe(laser_scan_topic, &GZBridge::laserScanCallback, this)) {
PX4_WARN("failed to subscribe to %s", laser_scan_topic.c_str());
if (!_node.Subscribe(lidar_distance_scan_topic, &GZBridge::lidarDistanceScanCallback, this)) {
PX4_WARN("failed to subscribe to %s", lidar_distance_scan_topic.c_str());
}

// Lidar 2D Obstacle Scan: optional
std::string lidar_obstacle_scan_topic = "/world/" + _world_name + "/model/" + _model_name +
"/link/link/sensor/lidar_2d_v2/scan";

if (!_node.Subscribe(lidar_obstacle_scan_topic, &GZBridge::lidarObstacleScanCallback, this)) {
PX4_WARN("failed to subscribe to %s", lidar_obstacle_scan_topic.c_str());
}

#if 0
Expand Down Expand Up @@ -763,7 +772,38 @@ void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat)
pthread_mutex_unlock(&_node_mutex);
}

void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan)
void GZBridge::lidarDistanceScanCallback(const gz::msgs::LaserScan &scan)
{
// Publish to uORB
distance_sensor_s distance_sensor {};

distance_sensor.timestamp = hrt_absolute_time();
distance_sensor.device_id = 0x20629a; // 0x276270: DRV_DIST_DEVTYPE_SIM, BUS: 0, ADDR: 0x62, TYPE: SIMULATION
distance_sensor.min_distance = scan.range_min();
distance_sensor.max_distance = scan.range_max();
distance_sensor.current_distance = 0;
distance_sensor.variance = 0;
distance_sensor.signal_quality = -1;
distance_sensor.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
distance_sensor.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
distance_sensor.mode = distance_sensor_s::MODE_ENABLED;

if (!scan.ranges().empty()) {
float distance = scan.ranges()[0];

if (!isinf(distance)) {
distance_sensor.current_distance = distance;
distance_sensor.signal_quality = 100;

} else {
distance_sensor.signal_quality = 0;
}
}

_distance_sensor_pub.publish(distance_sensor);
}

void GZBridge::lidarObstacleScanCallback(const gz::msgs::LaserScan &scan)
{
static constexpr int SECTOR_SIZE_DEG = 10; // PX4 Collision Prevention only has 36 sectors of 10 degrees each

Expand Down
5 changes: 4 additions & 1 deletion src/modules/simulation/gz_bridge/GZBridge.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/wheel_encoders.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/obstacle_distance.h>

#include <gz/math.hh>
Expand Down Expand Up @@ -110,7 +111,8 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
void poseInfoCallback(const gz::msgs::Pose_V &pose);
void odometryCallback(const gz::msgs::OdometryWithCovariance &odometry);
void navSatCallback(const gz::msgs::NavSat &nav_sat);
void laserScanCallback(const gz::msgs::LaserScan &scan);
void lidarDistanceScanCallback(const gz::msgs::LaserScan &scan);
void lidarObstacleScanCallback(const gz::msgs::LaserScan &scan);

/**
* @brief Call Entityfactory service
Expand Down Expand Up @@ -165,6 +167,7 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

//uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::Publication<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::Publication<obstacle_distance_s> _obstacle_distance_pub{ORB_ID(obstacle_distance)};
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 Down
Loading