Mavros obstacle distance for collision prevention

Hello,
I’m trying to test collision prevention by publishing fake LaserScan messages on /mavros/obstacle/send. I’m using PX4 SITL Gazebo to test the drone behaviour.

My message is received as I can see an OBSTACLE_DISTANCE message in QGC with the expected distances values. One thing to notice is that the component id for this message is 240 whereas most of the other mavlink messages have id 1. I’ve tried to change but I didn’t found the way to do it from ROS. I don’t know if this really is the problem?

The collision prevention does not seems to work as I can still move the quadcopter in all directions even though obstacle messages are published for all directions.
I think I’m not creating my LaserScan message properly. I’ve read Question on distances array in OBSTACLE_DISTANCE but it is not about mavros messages.
This is how I do it :

int num_readings = 36;
ros::Time scan_time = ros::Time::now(); //use_sim_time=true

//populate the LaserScan message
sensor_msgs::LaserScan scan;

 scan.header.stamp = scan_time;
 scan.header.frame_id = "base_link";
 // scan.angle_min = 0.;
 // scan.angle_max = 0.;
 scan.angle_increment = 10.;
 // scan.time_increment = 0.;
 // scan.range_min = 0.0;
 // scan.range_max = 20.0;

 scan.ranges.resize(num_readings);
 scan.ranges[0] = 2.5;

 for(unsigned int i = 0; i < num_readings; ++i){
    scan.ranges[i] = 2.5; //obstacle at 2.5m
 }

Any help would be appreciated.

Seems you need to set 2 parameters :

  • MPC_POS_MODE = Smooth position control (Jerk optimized)
  • EKF2_RNG_AID = Disabled