小车仿真环境.zip

# The obstacle_detector package The `obstacle_detector` package provides utilities to detect and track obstacles from data provided by 2D laser scanners. Detected obstacles come in a form of line segments or circles. The package was designed for a robot equipped with two laser scanners therefore it contains several additional utilities. The working principles of the method are described in an article provided in the `resources` folder. The package requires [Armadillo C++](http://arma.sourceforge.net) library for compilation and runtime. ----------------------- <p align="center"> <img src="https://user-images.githubusercontent.com/1482514/27595825-0abe4338-5b5e-11e7-8438-ffdeec4e9cef.png" alt="Visual example of obstacle detector output."/> <br/> Fig. 1. Visual example of obstacle detector output. </p> ----------------------- 1. The nodes and nodelets 1.1 The scans_merger 1.2 The obstacle_extractor 1.3 The obstacle_tracker 1.4 The obstacle_publisher 2. The messages 3. Launch files 4. The displays ## 1. The nodes and nodelets The package provides separate nodes/nodelets to perform separate tasks. In general solution the data is processed in a following manner: `two laser scans` -> `scans merger` -> `merged scan or pcl` -> `obstacle extractor` -> `obstacles` -> `obstacle tracker` -> `refined obstacles` For some scenarios the pure obstacle extraction directly from a laser scan (without tracking) might be sufficient. The nodes are configurable with the use of ROS parameter server. All of the nodes provide a private `params` service, which allows the process to get the latest parameters from the parameter server. All of the nodes can be in either active or sleep mode, triggered by setting the appropriate variable in the parameter server and calling `params` service. In the sleep mode, any subscribers or publishers are shut down and the node does nothing until activated again. For the ease of use it is recomended to use appropriate Rviz panels provided for the nodes with the package. The Rviz panels communicate via parameter server and service-client calls, therefore the names of the nodes must be preserved unchanged (cf. launch files for examples). ### 1.1. The scans_merger node This node converts two messages of type `sensor_msgs/LaserScan` from topics `front_scan` and `rear_scan` into a single laser scan of the same type, published under topic `scan` and/or a point cloud of type `sensor_msgs/PointCloud`, published under topic `pcl`. The difference between both is that the resulting laser scan divides the area into finite number of circular sectors and put one point (or actually one range value) in each section occupied by some measured points, whereas the resulting point cloud simply copies all of the points obtained from sensors. The input laser scans are firstly rectified to incorporate the motion of the scanner in time (see `laser_geometry` package). Next, two PCLs obtained from the previous step are synchronized and transformed into the target coordinate frame at the current point in time. ----------------------- <p align="center"> <img src="https://user-images.githubusercontent.com/1482514/27595821-0aa3519a-5b5e-11e7-9ce0-8f48db4592e4.gif" alt="Visual example of scans_merger output"/> <br/> Fig. 2. Visual example of `scans_merger` output. </p> ----------------------- The resulting messages contain geometric data described with respect to a specific coordinate frame (e.g. `robot`). Assuming that the coordinate frames attached to two laser scanners are called `front_scanner` and `rear_scanner`, both transformation from `robot` frame to `front_scanner` frame and from `robot` frame to `rear_scanner` frame must be provided. The node allows to artificially restrict measured points to some rectangular region around the `robot` frame as well as to limit the resulting laser scan range. The points falling behind this region will be discarded. Even if only one laser scanner is used, the node can be useful for simple data pre-processing, e.g. rectification, range restriction or recalculation of points to a different coordinate frame. The node uses the following set of local parameters: * `~active` (`bool`, default: `true`) - active/sleep mode, * `~publish_scan` (`bool`, default: `false`) - publish the merged laser scan message, * `~publish_pcl` (`bool`, default: `true`) - publish the merged point cloud message, * `~ranges_num` (`int`, default: `1000`) - number of ranges (circular sectors) contained in the 360 deg laser scan message, * `~min_scanner_range` (`double`, default: `0.05`) - minimal allowable range value for produced laser scan message, * `~max_scanner_range` (`double`, default: `10.0`) - maximal allowable range value for produced laser scan message, * `~min_x_range` (`double`, default: `-10.0`) - limitation for points coordinates (points with coordinates behind these limitations will be discarded), * `~max_x_range` (`double`, default: `10.0`) - as above, * `~min_y_range` (`double`, default: `-10.0`) - as above, * `~max_y_range` (`double`, default: `10.0`) - as above, * `~fixed_frame_id` (`string`, default: `map`) - name of the fixed coordinate frame used for scan rectification in time, * `~target_frame_id` (`string`, default: `robot`) - name of the coordinate frame used as the origin for the produced laser scan or point cloud. The package comes with Rviz panel for this node. ----------------------- <p align="center"> <img src="https://user-images.githubusercontent.com/1482514/28522255-97e2c210-7077-11e7-8ca1-556790b043e6.png" alt="Rviz panel for the scans_merger node."/> <br/> Fig. 3. Rviz panel for the `scans_merger` node. </p> ----------------------- ### 1.2. The obstacle_extractor node This node converts messages of type `sensor_msgs/LaserScan` from topic `scan` or messages of type `sensor_msgs/PointCloud` from topic `pcl` into obstacles which are published as messages of custom type `obstacles_detector/Obstacles` under topic `raw_obstacles`. The PCL message must be ordered in the angular fashion, because the algorithm exploits the polar nature of laser scanners. ----------------------- <p align="center"> <img src="https://user-images.githubusercontent.com/1482514/27595822-0aa50ab2-5b5e-11e7-8061-1da4b947b617.gif" alt="Visual example of obstacle_extractor output."/> <br/> Fig. 4. Visual example of `obstacle_extractor` output. </p> ----------------------- The input points are firstly grouped into subsets and marked as visible or not (if a group is in front of neighbouring groups, it is visible. Otherwise it is assumed to be occluded). The algorithm extracts segments from each points subset. Next, the segments are checked for possible merging between each other. The circular obstacles are then extracted from segments and also merged if possible. Resulting set of obstacles can be transformed to a dedicated coordinate frame. The node is configurable with the following set of local parameters: * `~active` (`bool`, default: `true`) - active/sleep mode, * `~use_scan` (`bool`, default: `false`) - use laser scan messages, * `~use_pcl` (`bool`, default: `true`) - use point cloud messages (if both scan and pcl are chosen, scans will have priority), * `~use_split_and_merge` (`bool`, default: `true`) - choose wether to use Iterative End Point Fit (false) or Split And Merge (true) algorithm to detect segments, * `~circles_from_visibles` (`bool`, default: `true`) - detect circular obstacles only from fully visible (not occluded) segments, * `~discard_converted_segments` (`bool`, default: `true`) - do not publish segments, from which the circles were spawned, * `~transform_coordinates` (`bool`, default: `true`) - transform the coordinates of obstacles to a frame described with `frame_id` parameter, * `~min_group_points` (`int`, default: `5`) - minimum number of points comprising a group to be further processed, * `~max_group_distance` (`double`, default: `0.1`) - if the distance between two poi