How to set up multiple distance sensors?

ROS / MAVROS / MicroDDS
1

Hello,

I want to set the group of distance sensors, so every of them has his own publishing topic; this is my current px4_config.yaml setup

  sonar_fwd_pub:
    id: 0
#    frame_id: "hrlv_ez4_sonar"
    frame_id: "sonar_fwd"
    orientation: NONE #PITCH_270 # RPY:{0.0, 270.0, 0.0} - downward-facing
    field_of_view: 0.0  # XXX TODO
    send_tf: true
    sensor_position: {x:  0.0, y:  0.0, z:  -0.1}
  sonar_bwd_pub:
    id: 1
    frame_id: "sonar_bwd"
    orientation: YAW_180 #PITCH_270 # RPY:{0.0, 270.0, 0.0} - downward-facing
    field_of_view: 0.0  # XXX TODO
    send_tf: true
    sensor_position: {x:  0.0, y:  0.0, z:  -0.1}
  sonar_right_pub:
    id: 2
    frame_id: "sonar_right"
    orientation: YAW_90 #PITCH_270 # RPY:{0.0, 270.0, 0.0} - downward-facing
    field_of_view: 0.0  # XXX TODO
    send_tf: true
    sensor_position: {x:  0.0, y:  0.0, z:  -0.1}
  sonar_left_pub:
    id: 3
    frame_id: "sonar_left"
    orientation: YAW_270 #PITCH_270 # RPY:{0.0, 270.0, 0.0} - downward-facing
    field_of_view: 0.0  # XXX TODO
    send_tf: true
    sensor_position: {x:  0.0, y:  0.0, z:  -0.1}
#  lidarlite_pub:
#    id: 1
#    frame_id: "lidarlite_laser"
#    orientation: PITCH_270
#    field_of_view: 0.0  # XXX TODO
#    send_tf: true
#    sensor_position: {x:  0.0, y:  0.0, z:  -0.1}
  sonar_1_sub:
    subscriber: true
    id: 4
    orientation: NONE #PITCH_270
    horizontal_fov_ratio: 1.0   # horizontal_fov = horizontal_fov_ratio * msg.field_of_view
    vertical_fov_ratio: 1.0     # vertical_fov = vertical_fov_ratio * msg.field_of_view
    # custom_orientation:       # Used for orientation == CUSTOM
    #   roll: 0
    #   pitch: 270
    #   yaw: 0
#  laser_1_sub:
#    subscriber: true
#    id: 3
#    orientation: PITCH_270

the problem is that sonars other then forward (sonar_fwd) are not publishing to their topics (they asking for clock source, which in my case is provided by simulator); I assume I need to setup something more?

2

when I checked uORB message, it revealed that there is only one topic of distance sensor (as shown below)

pxh> listener distance_sensor

TOPIC: distance_sensor
 distance_sensor_s
	timestamp: 74596000  (0.144000 seconds ago)
	device_id: 10118148 (Type: 0x9A, SIMULATION:0 (0x64)) 
	min_distance: 0.0100
	max_distance: 5.0000
	current_distance: 0.2900
	variance: 0.0000
	h_fov: 0.5236
	v_fov: 0.5236
	q: [1.0000, -0.0000, -0.0000, -0.0000]  (Roll: 0.0 deg, Pitch: -0.0 deg, Yaw: 0.0 deg)
	signal_quality: -1
	type: 1
	orientation: 0
3

I had a similar problem with PX4 SITL Gazebo and just got it working, so what you might try is the following:

  • Make sure, you use the latest PX4 version. I got it working with 1.12.3
  • Select a proper model to start in mavros_posix_sitl.launch. E.g. the iris_obs_avoid which has 4 lidar distance sensors. The sensors need to be defined in the appropriate <model>.sdf.
  • plugin distance_sensor must not be blacklisted in px4_pluginlists.yaml.
  • check uORB message with pxh> listener distance_sensor. It should give you an instance for each of the sensors
  • Then you can edit px4_config.yaml like you already did, to map the uORB distance messages to ROS topics. For setting the correct orientation, check the orientation in the uORB distance messages. Sensor position should be taken from the model sdf.
4

@alenz-68
the thing is, iris_obs_avoid has setup flag COM_OBS_AVOID, which could broke my ROS program (since it launching their own avoidance algorithms)

5

Do you know where the COM_OBS_AVOID Flag is set? I think it is the same for me, at least QGroundControl could not arm the copter as collsion avoidance was reported not ready. I disabled the collision avoidance and prevention in QGC saftey feature section.

6

You can also disable the internal obstacle avoidance by setting the COM_OBS_AVOID flag to 0.
This is found in the model parameter file:
PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes/1015_iris_obs_avoid
It might be necessary to remove the corresponding file in ~/.ros/eeprom

7

I recreated model parameter file in my model, but still I got only one instance of distance_sensor…

anyway, I’ll try to disable that parameter on original model