Hi,
I have modified px4’s local_planner avoidance node to run with arducopter. Using a depth sensor attached to the iris_with_arducopter gazebo model, I’m publishing a pointcloud2 topic. The node converts this pointcloud data into a laser scan message and publishes it to /mavros/obstacle/send. I was wondering if there was a simple way for arducopter to receive this laser scan message in the form of OBSTACLE_DISTANCE
When mavros starts up it shows that the obstacle distance plugin is loaded and the planner node is constantly posting messages like this to /mavros/obstacle/send
header:
seq: 1886
stamp:
secs: 736
nsecs: 890000000
frame_id: "map"
angle_min: 0.0
angle_max: 0.0
angle_increment: 0.104719758034
time_increment: 0.0
scan_time: 0.0
range_min: 0.20000000298
range_max: 15.0
ranges: [2.345651865005493, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 15.010000228881836, 2.429025411605835, 2.3743903636932373, 2.3546435832977295, 2.3510959148406982]
but i dont see it on the arducopter side. The following is from mavproxy:
LOITER> status
LOITER> Counters: Slave:5954 MasterIn:[73523] FGearOut:0 FGearIn:0 MasterOut:643
MAV Errors: 280
None
2504: AHRS {omegaIx : -0.0020479937084, omegaIy : -0.00200670282356, omegaIz : -0.00229100580327, accel_weight : 0.0, renorm_val : 0.0, error_rp : 0.00674330536276, error_yaw : 0.0139547735453}
2497: AHRS2 {roll : 0.013744388707, pitch : -0.00169652188197, yaw : 0.0215266048908, altitude : 0.0, lat : 0, lng : 0}
2499: AHRS3 {roll : 0.00734772719443, pitch : -0.00905097275972, yaw : 0.0214533731341, altitude : 594.609985352, lat : -353632601, lng : 1491652319, v1 : 0.0, v2 : 0.0, v3 : 0.0, v4 : 0.0}
2504: ATTITUDE {time_boot_ms : 604918, roll : 0.00734772719443, pitch : -0.00905097275972, yaw : 0.0214533731341, rollspeed : 0.12227319181, pitchspeed : 0.00014108675532, yawspeed : 0.00210963538848}
1: AUTOPILOT_VERSION {capabilities : 15311, flight_sw_version : 50728191, middleware_sw_version : 0, os_sw_version : 0, board_version : 0, flight_custom_version : [99, 98, 53, 55, 48, 99, 48, 0], middleware_custom_version : [0, 0, 0, 0, 0, 0, 0, 0], os_custom_version : [0, 0, 0, 0, 0, 0, 0, 0], vendor_id : 0, product_id : 0, uid : 0, uid2 : [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]}
280: BAD_DATA {Bad prefix, data:['20']}
2504: BATTERY_STATUS {id : 0, battery_function : 0, type : 0, temperature : 32767, voltages : [65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535], current_battery : 2949, current_consumed : 106, energy_consumed : 46, battery_remaining : 96, time_remaining : 0, charge_state : 0}
4: COMMAND_ACK {command : 400, result : 0}
2503: DISTANCE_SENSOR {time_boot_ms : 604918, min_distance : 20, max_distance : 5000, current_distance : 1026, type : 4, id : 0, orientation : 25, covariance : 0, horizontal_fov : 0.0, vertical_fov : 0.0, quaternion : [0.0, 0.0, 0.0, 0.0]}
2499: EKF_STATUS_REPORT {flags : 895, velocity_variance : 0.0301504377276, pos_horiz_variance : 0.0170443598181, pos_vert_variance : 0.00241008261219, compass_variance : 0.0301219541579, terrain_alt_variance : 0.0, airspeed_variance : 0.0}
2504: GLOBAL_POSITION_INT {time_boot_ms : 604918, lat : -353632601, lon : 1491652319, alt : 594610, relative_alt : 10715, vx : 15, vy : 0, vz : 0, hdg : 122}
3: GPS_GLOBAL_ORIGIN {latitude : -353632608, longitude : 1491652351, altitude : 583920, time_usec : 588571573}
2504: GPS_RAW_INT {time_usec : 604818000, fix_type : 6, lat : -353632613, lon : 1491652320, alt : 594150, eph : 121, epv : 200, vel : 4, cog : 27674, satellites_visible : 10, alt_ellipsoid : 0, h_acc : 200, v_acc : 200, vel_acc : 40, hdg_acc : 0, yaw : 0}
604: HEARTBEAT {type : 2, autopilot : 3, base_mode : 217, custom_mode : 5, system_status : 4, mavlink_version : 3}
3: HOME_POSITION {latitude : -353632596, longitude : 1491652334, altitude : 583900, x : 0.0, y : 0.0, z : 0.0, q : [1.0, 0.0, 0.0, 0.0], approach_x : 0.0, approach_y : 0.0, approach_z : 0.0, time_usec : 588571573}
2504: HWSTATUS {Vcc : 5000, I2Cerr : 0}
2349: LOCAL_POSITION_NED {time_boot_ms : 604918, x : -0.0452577769756, y : -0.139647156, z : -10.7157745361, vx : 0.150410622358, vy : 0.00105662841816, vz : -0.00598604604602}
2504: MEMINFO {brkval : 0, freemem : 65535, freemem32 : 131072}
1: MISSION_COUNT {target_system : 1, target_component : 240, count : 0, mission_type : 0}
2504: MISSION_CURRENT {seq : 0}
2504: NAV_CONTROLLER_OUTPUT {nav_roll : 0.337485611439, nav_pitch : -0.53586524725, nav_bearing : 1, target_bearing : 8, wp_dist : 0, alt_error : 0.016173094511, aspd_error : 0.0, xtrack_error : 0.0}
2380: PARAM_VALUE {param_id : STAT_RUNTIME, param_value : 8250.0, param_type : 6, param_count : 931, param_index : 65535}
2504: POWER_STATUS {Vcc : 5000, Vservo : 0, flags : 0}
2504: RANGEFINDER {distance : 10.2600002289, voltage : 1.02600002289}
2504: RAW_IMU {time_usec : 604918616, xacc : 0, yacc : -3, zacc : -1010, xgyro : 124, ygyro : 2, zgyro : 4, xmag : 223, ymag : 38, zmag : -539, id : 0, temperature : 0}
2504: RC_CHANNELS {time_boot_ms : 604918, chancount : 16, chan1_raw : 1500, chan2_raw : 1500, chan3_raw : 1500, chan4_raw : 1500, chan5_raw : 1800, chan6_raw : 1000, chan7_raw : 1000, chan8_raw : 1800, chan9_raw : 0, chan10_raw : 0, chan11_raw : 0, chan12_raw : 0, chan13_raw : 0, chan14_raw : 0, chan15_raw : 0, chan16_raw : 0, chan17_raw : 0, chan18_raw : 0, rssi : 0}
2504: SCALED_IMU2 {time_boot_ms : 604918, xacc : 0, yacc : -3, zacc : -1008, xgyro : 135, ygyro : 2, zgyro : 4, xmag : 223, ymag : 38, zmag : -539, temperature : 0}
2504: SCALED_IMU3 {time_boot_ms : 604918, xacc : 0, yacc : 0, zacc : 0, xgyro : 0, ygyro : 0, zgyro : 0, xmag : 223, ymag : 38, zmag : -539, temperature : 0}
2504: SCALED_PRESSURE {time_boot_ms : 604918, press_abs : 943.88671875, press_diff : 0.0, temperature : 3500}
227: SENSOR_OFFSETS {mag_ofs_x : 5, mag_ofs_y : 13, mag_ofs_z : -18, mag_declination : 0.20736888051, raw_press : 94388, raw_temp : 3500, gyro_cal_x : -1.99827190954e-05, gyro_cal_y : 1.63406966749e-05, gyro_cal_z : 3.15265824611e-05, accel_cal_x : 0.0010000000475, accel_cal_y : 0.0010000000475, accel_cal_z : 0.0010000000475}
2504: SERVO_OUTPUT_RAW {time_usec : 604918616, port : 0, servo1_raw : 1608, servo2_raw : 1529, servo3_raw : 1528, servo4_raw : 1608, servo5_raw : 0, servo6_raw : 0, servo7_raw : 0, servo8_raw : 0, servo9_raw : 0, servo10_raw : 0, servo11_raw : 0, servo12_raw : 0, servo13_raw : 0, servo14_raw : 0, servo15_raw : 0, servo16_raw : 0}
2504: SIMSTATE {roll : 0.0063869128935, pitch : -0.000106758692709, yaw : 0.0256327688694, xacc : -0.00157096842304, yacc : 0.0220022741705, zacc : -9.88020801544, xgyro : 0.302790105343, ygyro : -0.00146722164936, zgyro : 0.00715992366895, lat : -353632611, lng : 1491652318}
22: STATUSTEXT {severity : 6, text : EKF2 IMU1 in-flight yaw alignment complete, id : 0, chunk_seq : 0}
2504: SYSTEM_TIME {time_unix_usec : 1589752763879769, time_boot_ms : 604918}
2504: SYS_STATUS {onboard_control_sensors_present : 56753583, onboard_control_sensors_enabled : 56753583, onboard_control_sensors_health : 56753583, load : 0, voltage_battery : 12193, current_battery : 2949, battery_remaining : 96, drop_rate_comm : 0, errors_comm : 0, errors_count1 : 0, errors_count2 : 0, errors_count3 : 0, errors_count4 : 0}
2503: TERRAIN_REPORT {lat : -353632601, lon : 1491652319, spacing : 100, terrain_height : 583.884643555, current_height : 10.7098169327, pending : 0, loaded : 504}
60: TIMESYNC {tc1 : 0, ts1 : 600136406001}
2504: VFR_HUD {airspeed : 0.0460000038147, groundspeed : 0.150414332747, heading : 1, throttle : 35, alt : 594.609985352, climb : 0.00598604604602}
2504: VIBRATION {time_usec : 604918616, vibration_x : 0.00687937019393, vibration_y : 0.0425000488758, vibration_z : 0.157216966152, clipping_0 : 0, clipping_1 : 0, clipping_2 : 0}
These are my parameters:
LOITER> param show *AVOID*
LOITER> AVOID_ANGLE_MAX 1000.000000
AVOID_BEHAVE 0.000000
AVOID_DIST_MAX 5.000000
AVOID_ENABLE 3.000000
AVOID_MARGIN 2.000000
