search

ArduPilot / ardupilot

ArduPlane, ArduCopter, ArduRover, ArduSub source
http://ardupilot.org/
GNU General Public License v3.0
10.17k stars 16.73k forks source link

Copter: object avoidance with ROS #5608

Open rmackay9 opened 7 years ago

rmackay9 commented 7 years ago

Copter's current object avoidance works either through directly connecting a Lightware SF40C 360 lidar to the flight controller or by using a mavlink enabled sensor that provides the DISTANCE_SENSOR messages. We should extend support so that we can work with ROS's SLAM point clouds

patrickpoirier51 commented 6 years ago

Randy, is this being adressed with new slam release?

rmackay9 commented 6 years ago

@patrickpoirier51, i'm afraid not yet. What we need to do to get the object avoidance working is to somehow modify the ROS side so that it sends DISTANCE_SENSOR messages based on the distance to the closest points in the point cloud. I actually have almost no idea how to do this because I know so little about ROS.

auturgy commented 6 years ago

@vooon would you be interested in helping with this?

vooon commented 6 years ago

Unfortunately I don't have bandwidth, and point-cloud sources.

patrickpoirier51 commented 6 years ago

I implemented this on my wheeled robot, its reading KINECT pointcloud and transform to 2d laser scan http://wiki.ros.org/pointcloud_to_laserscan

So the message can be used for avoidance http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html

bromeara commented 5 years ago

This looks very similar to a problem I'm currently trying to solve since we are using a 3D lidar and while we are using the point cloud for processing on the companion computer I wanted to pass a couple of channels through to the autopilot for obstacle avoidance.

I was going to avoid converting from the point cloud due to latency concerns, @patrickpoirier51 I realize you are using a wheeled robot but were you able to measure the performance of your technique?

@rmackay9 My intention was originally to write a ROS node that sends DISTANCE_SENSOR messages I wasn't planning on converting from the point cloud, due to the latency concerns which may or may not be valid, however I feel like I could probably do some templating so that the source of the DISTANCE_SENSOR data doesn't matter as much. I saw the conversation in #8630 and was wondering if it would have an effect on this approach, or if that was more of a conversation on how proximity data is handled internally in general.

patrickpoirier51 commented 5 years ago

I since updated to a RealSense D435 and it’s quite performant, I used a python script to generate MAVLink avoidance but did not tried yet in a quadcopter

I posted result in gitter vision room end November

rmackay9 commented 5 years ago

Using the point cloud info would be great if possible. The latency might be OK if we move to a method in which we can somehow get AP to know the barrier information in "earth frame" (i.e. relative to some point instead of being relative to the body of the vehicle). I might be possible if the time on the DISTANCE_SENSOR message was the time it started the calculations instead of the time it's sending the message. By the way, AP also supports the OBSTACLE_DISTANCE message now so up to 72 distances can be sent at once.

bromeara commented 5 years ago

The latency I mentioned was the time that it takes for ROS to translate a lidar packet into a point cloud and then to a mavlink message, I was planning on going from lidar packet to mavlink message. Although the earth frame would be an interesting optimization.

You'll have to forgive me as I am still new to the ArduPilot community but when you are mentioning putting the barrier information in the "earth frame" where would this be implemented, AP_Avoidance? or would it be more of a custom AP_Proximity driver?

rmackay9 commented 5 years ago

@bromeara, the lidar packet to mavlink message should already work for an RPLidarA2 (and probably A3) using the OBSTACLE_DISTANCE message (recent mavros PR to fix issues). This is what I've been using to allow low-level obstacle avoidance to be done in the vehicle code (for Copter and Rover)

bromeara commented 5 years ago

@rmackay9 thanks for the response, the obstacle_distance plugin looks like it will work for my initial use case. We are using Ouster's OS1 Lidar which outputs 3D point clouds that utilize ROS' sensor_msg/PointCloud2 message, which is why I was really interested in the previously mentioned SLAM point cloud support. Was the sensor_msg/PointCloud2 message what you were referring to? Or was it more of the sensor_msg/LaserScan message that's now supported by the obstacle_distance plugin?

rmackay9 commented 5 years ago

@bromeara, it's the LaserScan message that is now supported (mavros code is here).

We have some ideas/plans to maintain a simplified occupancy grid of a big list of obstacles within AP.. we just haven't quite gotten to it yet.

By the way, just want to make sure you're aware we have this ArduPilot/VisionProjects gitter chat as well.

nkquynh98 commented 5 years ago

@rmackay9

Firstly, thank you for your work. Now, I'm working under the project for making an obstacle avoiding drone. And I see that it can be done by the mode Alt Hold in Arducopter with sending distance_sensor Mavlink message to the Ardupilot. Therefore, I use RPLiar A2 to read the proximity distance and send it to Raspberry Pi, and then use Mavros to send the values to the DISTANCE_SENSOR message of Mavlink.

However, I can not figure how to do that. The service "/mavros/param/set" tend to send the Ardupilot-defined message (which is in here: http://ardupilot.org/copter/docs/parameters.html#rngfnd1-parameters) but not the RAW MAVLINK message. Hence, could you so me the way to use the Alt_hold with obstacle avoiding mode by mavros? Thank you.

rmackay9 commented 5 years ago

@nkquynh98,

If you're only looking to do object avoidance in AltHold with an RPLidarA2 it is also possible to directly connect the lidar to the flight controller avoiding ROS completely. Setup instructions are here.

Instead of using the DISTANCE_SENSOR messages, I think it's better to use the OBSTACLE_DISTANCE message to send the distances to ArduPilot. There are a couple of problems though:

Sorry, I haven't produced documentation on how to setup the OBSTACLE_DISTANCE support but I hope to once the bits have all been released.

nkquynh98 commented 5 years ago

@rmackay9

Oh, thank you for your supportive response. I will try connecting directly the Lidar to the Pixhawk. Also, I will use Obstacle_distance message. Btw, is there anyway to send the raw mavlink message from Mavros to FCU?

Moreover, I found out that in this site (http://ardupilot.org/copter/docs/parameters.html), there is some parameter call Rangfinder. Could I feed the values of the obstacle distance into these parameters and will the drone use these values to avoid the obstacle?

rmackay9 commented 5 years ago

@nkquynh98,

I'm not really sure about sending raw mavlink messages from mavros. mavros is mostly a translation layer between ROS topics and mavlink messages.

AP parameters are for configuration, they're not meant to be used as a way to send or accept data from external systems. mavlink messages are a better way to send/receive with external system.

It's probably best to ask some of these questions on the ArduPilot VisionProjects gitter chat channel..

ddomit commented 9 months ago

Hello Everyone,

So its almost 4 years later now. What is the recommended ROS object avoidance today?

Thank you!

HopeCollector commented 6 months ago

@rmackay9 hello! I'm currently dev an obstacle avoiding drone based on lidar in ROS2(Humble), from the comment above I think you may need somthing that can translate sensor_msgs/PointCloud2 -> OBSTACLE_DISTANCE?

here is my project laser_copilot, the node obj_dist is all you need.

there's also a simple collision prevention controller on ros side named safe_fly_controller in it, it can stop the drone before any potential collision.

currently this project is designed for px4, it you need it I can change it to ardupilot's mode