forked from ros-navigation/navigation2
-
Notifications
You must be signed in to change notification settings - Fork 2
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- obstacle_layer and voxel_layer add new plugins - global_obstacle_layer and global_voxel_layer inherited from obstacle_layer, voxel_layer
- Loading branch information
1 parent
5536e4d
commit 38cb346
Showing
11 changed files
with
621 additions
and
204 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
134 changes: 134 additions & 0 deletions
134
nav2_costmap_2d/include/nav2_costmap_2d/global_obstacle_layer.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,134 @@ | ||
| /********************************************************************* | ||
| * | ||
| * Software License Agreement (BSD License) | ||
| * | ||
| * Copyright (c) 2008, 2013, Willow Garage, Inc. | ||
| * All rights reserved. | ||
| * | ||
| * Redistribution and use in source and binary forms, with or without | ||
| * modification, are permitted provided that the following conditions | ||
| * are met: | ||
| * | ||
| * * Redistributions of source code must retain the above copyright | ||
| * notice, this list of conditions and the following disclaimer. | ||
| * * Redistributions in binary form must reproduce the above | ||
| * copyright notice, this list of conditions and the following | ||
| * disclaimer in the documentation and/or other materials provided | ||
| * with the distribution. | ||
| * * Neither the name of Willow Garage, Inc. nor the names of its | ||
| * contributors may be used to endorse or promote products derived | ||
| * from this software without specific prior written permission. | ||
| * | ||
| * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| * POSSIBILITY OF SUCH DAMAGE. | ||
| * | ||
| * Author: Eitan Marder-Eppstein | ||
| * David V. Lu!! | ||
| *********************************************************************/ | ||
| #ifndef NAV2_COSTMAP_2D__GLOBAL_OBSTACLE_LAYER_HPP_ | ||
| #define NAV2_COSTMAP_2D__GLOBAL_OBSTACLE_LAYER_HPP_ | ||
|
|
||
| #include "nav2_costmap_2d/obstacle_layer.hpp" | ||
|
|
||
| namespace nav2_costmap_2d | ||
| { | ||
|
|
||
| /** | ||
| * @class ObstacleLayer | ||
| * @brief Takes in laser and pointcloud data to populate into 2D costmap | ||
| */ | ||
| class GlobalObstacleLayer : public ObstacleLayer | ||
| { | ||
| public: | ||
| /** | ||
| * @brief A constructor | ||
| */ | ||
| GlobalObstacleLayer() | ||
| : ObstacleLayer() | ||
| {} | ||
|
|
||
| /** | ||
| * @brief Initialization process of layer on startup | ||
| */ | ||
| virtual void onInitialize(); | ||
| /** | ||
| * @brief Update the bounds of the master costmap by this layer's update dimensions | ||
| * @param robot_x X pose of robot | ||
| * @param robot_y Y pose of robot | ||
| * @param robot_yaw Robot orientation | ||
| * @param min_x X min map coord of the window to update | ||
| * @param min_y Y min map coord of the window to update | ||
| * @param max_x X max map coord of the window to update | ||
| * @param max_y Y max map coord of the window to update | ||
| */ | ||
| virtual void updateBounds( | ||
| double robot_x, double robot_y, double robot_yaw, double * min_x, | ||
| double * min_y, | ||
| double * max_x, | ||
| double * max_y); | ||
| /** | ||
| * @brief Update the costs in the master costmap in the window | ||
| * @param master_grid The master costmap grid to update | ||
| * @param min_x X min map coord of the window to update | ||
| * @param min_y Y min map coord of the window to update | ||
| * @param max_x X max map coord of the window to update | ||
| * @param max_y Y max map coord of the window to update | ||
| */ | ||
| virtual void updateCosts( | ||
| nav2_costmap_2d::Costmap2D & master_grid, | ||
| int min_i, int min_j, int max_i, int max_j); | ||
|
|
||
| /** | ||
| * @brief A callback to handle buffering LaserScan messages | ||
| * @param message The message returned from a message notifier | ||
| * @param buffer A pointer to the observation buffer to update | ||
| */ | ||
| void laserScanCallback( | ||
| sensor_msgs::msg::LaserScan::ConstSharedPtr message, | ||
| const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer); | ||
|
|
||
| /** | ||
| * @brief A callback to handle buffering LaserScan messages which need filtering to turn Inf values into range_max. | ||
| * @param message The message returned from a message notifier | ||
| * @param buffer A pointer to the observation buffer to update | ||
| */ | ||
| void laserScanValidInfCallback( | ||
| sensor_msgs::msg::LaserScan::ConstSharedPtr message, | ||
| const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer); | ||
|
|
||
| protected: | ||
|
|
||
| rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr obstacle_grid_pub_; | ||
| std::unique_ptr<nav_msgs::msg::OccupancyGrid> grid_; | ||
|
|
||
| bool is_init_scan_angle_; | ||
| bool use_init_scan_angle_; | ||
| double scan_start_angle_; | ||
| double scan_end_angle_; | ||
| float scan_link_offset_; | ||
| double current_robot_yaw_; | ||
| static char * cost_translation_table_; | ||
| rclcpp::Time last_publish_{0, 0, RCL_ROS_TIME}; | ||
| rclcpp::Duration publish_cycle_{1, 0}; | ||
|
|
||
| protected: | ||
| void updateRobotYaw(const double robot_yaw); | ||
|
|
||
| private: | ||
| void initializeScanAngle(sensor_msgs::msg::LaserScan::ConstSharedPtr message); | ||
| void prepareGrid(nav2_costmap_2d::Costmap2D costmap); | ||
| }; | ||
|
|
||
| } // namespace nav2_costmap_2d | ||
|
|
||
| #endif // NAV2_COSTMAP_2D__OBSTACLE_LAYER_HPP_ |
80 changes: 80 additions & 0 deletions
80
nav2_costmap_2d/include/nav2_costmap_2d/global_voxel_layer.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,80 @@ | ||
| /********************************************************************* | ||
| * | ||
| * Software License Agreement (BSD License) | ||
| * | ||
| * Copyright (c) 2008, 2013, Willow Garage, Inc. | ||
| * All rights reserved. | ||
| * | ||
| * Redistribution and use in source and binary forms, with or without | ||
| * modification, are permitted provided that the following conditions | ||
| * are met: | ||
| * | ||
| * * Redistributions of source code must retain the above copyright | ||
| * notice, this list of conditions and the following disclaimer. | ||
| * * Redistributions in binary form must reproduce the above | ||
| * copyright notice, this list of conditions and the following | ||
| * disclaimer in the documentation and/or other materials provided | ||
| * with the distribution. | ||
| * * Neither the name of Willow Garage, Inc. nor the names of its | ||
| * contributors may be used to endorse or promote products derived | ||
| * from this software without specific prior written permission. | ||
| * | ||
| * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| * POSSIBILITY OF SUCH DAMAGE. | ||
| * | ||
| * Author: Eitan Marder-Eppstein | ||
| * David V. Lu!! | ||
| *********************************************************************/ | ||
| #ifndef NAV2_COSTMAP_2D__GLOBAL_VOXEL_LAYER_HPP_ | ||
| #define NAV2_COSTMAP_2D__GLOBAL_VOXEL_LAYER_HPP_ | ||
|
|
||
| #include "nav2_costmap_2d/voxel_layer.hpp" | ||
|
|
||
| namespace nav2_costmap_2d | ||
| { | ||
| /** | ||
| * @class GlobalVoxelLayer | ||
| * @brief Takes laser and pointcloud data to populate a 3D voxel representation of the environment | ||
| */ | ||
| class GlobalVoxelLayer : public VoxelLayer | ||
| { | ||
| public: | ||
| /** | ||
| * @brief Voxel Layer constructor | ||
| */ | ||
| GlobalVoxelLayer() | ||
| : VoxelLayer() | ||
| {}; | ||
|
|
||
| /** | ||
| * @brief Update the bounds of the master costmap by this layer's update dimensions | ||
| * @param robot_x X pose of robot | ||
| * @param robot_y Y pose of robot | ||
| * @param robot_yaw Robot orientation | ||
| * @param min_x X min map coord of the window to update | ||
| * @param min_y Y min map coord of the window to update | ||
| * @param max_x X max map coord of the window to update | ||
| * @param max_y Y max map coord of the window to update | ||
| */ | ||
| virtual void updateBounds( | ||
| double robot_x, double robot_y, double robot_yaw, | ||
| double * min_x, double * min_y, | ||
| double * max_x, double * max_y); | ||
|
|
||
| protected: | ||
| double current_robot_yaw_; | ||
| }; | ||
|
|
||
| } // namespace nav2_costmap_2d | ||
|
|
||
| #endif // NAV2_COSTMAP_2D__VOXEL_LAYER_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.