diff --git a/nav2_bringup/params/acloi00_nav2_params.yaml b/nav2_bringup/params/acloi00_nav2_params.yaml index 85977173b4c..4ac6ed05c42 100644 --- a/nav2_bringup/params/acloi00_nav2_params.yaml +++ b/nav2_bringup/params/acloi00_nav2_params.yaml @@ -196,15 +196,14 @@ local_costmap: height: 4 resolution: 0.1 robot_radius: 0.34 - # plugins: ["voxel_layer", "inflation_layer"] - plugins: ["voxel_layer", "inflation_layer"] + plugins: ["global_voxel_layer", "inflation_layer"] #filters: ["keepout_filter"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 6.5 inflation_radius: 1.75 - voxel_layer: - plugin: "nav2_costmap_2d::VoxelLayer" + global_voxel_layer: + plugin: "nav2_costmap_2d::GlobalVoxelLayer" enabled: True publish_voxel_map: True origin_z: 0.0 @@ -214,7 +213,7 @@ local_costmap: mark_threshold: 0 observation_sources: scan scan: - topic: /acloi00/scan + topic: /acloi01/scan max_obstacle_height: 5.0 clearing: True marking: True @@ -228,10 +227,6 @@ local_costmap: # init_scan_angle: False static_layer: map_subscribe_transient_local: True - global_obstacle_layer: - plugin: "nav2_global_obstacle_costmap_plugin::GlobalObstacleLayer" - enabled: True - topic: "/global_obstacle/obstacle_list" keepout_filter: plugin: "nav2_costmap_2d::KeepoutFilter" enabled: True @@ -249,7 +244,6 @@ global_costmap: robot_radius: 0.34 resolution: 0.1 track_unknown_space: true - #plugins: ["static_layer", "obstacle_layer", "global_obstacle_layer", "inflation_layer"] plugins: ["static_layer", "obstacle_layer", "inflation_layer"] #filters: ["keepout_filter"] obstacle_layer: diff --git a/nav2_bringup/params/acloi01_nav2_params.yaml b/nav2_bringup/params/acloi01_nav2_params.yaml index 6d46203a812..884e1ce953a 100644 --- a/nav2_bringup/params/acloi01_nav2_params.yaml +++ b/nav2_bringup/params/acloi01_nav2_params.yaml @@ -196,13 +196,33 @@ local_costmap: height: 4 resolution: 0.1 robot_radius: 0.34 - plugins: ["voxel_layer", "inflation_layer"] + plugins: ["global_obstacle_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 6.5 inflation_radius: 1.75 - voxel_layer: - plugin: "nav2_costmap_2d::VoxelLayer" + # voxel_layer: + # plugin: "nav2_costmap_2d::VoxelLayer" + # enabled: True + # publish_voxel_map: True + # origin_z: 0.0 + # z_resolution: 0.05 + # z_voxels: 16 + # max_obstacle_height: 2.0 + # mark_threshold: 0 + # observation_sources: scan + # scan: + # topic: /acloi01/scan + # max_obstacle_height: 5.0 + # clearing: True + # marking: True + # data_type: "LaserScan" + # raytrace_max_range: 15.0 + # raytrace_min_range: 0.0 + # obstacle_max_range: 5.0 + # obstacle_min_range: 0.0 + global_obstacle_layer: + plugin: "nav2_costmap_2d::GlobalObstacleLayer" enabled: True publish_voxel_map: True origin_z: 0.0 @@ -221,6 +241,9 @@ local_costmap: raytrace_min_range: 0.0 obstacle_max_range: 5.0 obstacle_min_range: 0.0 + scan_link_offset: 2.0 + publish_cycle: 1.0 + # init_scan_angle: False static_layer: map_subscribe_transient_local: True always_send_full_costmap: True diff --git a/nav2_bringup/params/acloi02_nav2_params.yaml b/nav2_bringup/params/acloi02_nav2_params.yaml index 04837463361..7758af912ac 100644 --- a/nav2_bringup/params/acloi02_nav2_params.yaml +++ b/nav2_bringup/params/acloi02_nav2_params.yaml @@ -196,13 +196,13 @@ local_costmap: height: 4 resolution: 0.1 robot_radius: 0.34 - plugins: ["voxel_layer", "inflation_layer"] + plugins: ["global_voxel_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 6.5 inflation_radius: 1.75 - voxel_layer: - plugin: "nav2_costmap_2d::VoxelLayer" + global_voxel_layer: + plugin: "nav2_costmap_2d::GlobalVoxelLayer" enabled: True publish_voxel_map: True origin_z: 0.0 @@ -212,7 +212,7 @@ local_costmap: mark_threshold: 0 observation_sources: scan scan: - topic: /acloi02/scan + topic: /acloi01/scan max_obstacle_height: 5.0 clearing: True marking: True @@ -221,6 +221,9 @@ local_costmap: raytrace_min_range: 0.0 obstacle_max_range: 5.0 obstacle_min_range: 0.0 + scan_link_offset: 2.0 + publish_cycle: 1.0 + # init_scan_angle: False static_layer: map_subscribe_transient_local: True always_send_full_costmap: True diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 683cb04fdd2..0f60c986791 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -84,8 +84,10 @@ add_library(layers SHARED plugins/inflation_layer.cpp plugins/static_layer.cpp plugins/obstacle_layer.cpp + plugins/global_obstacle_layer.cpp src/observation_buffer.cpp plugins/voxel_layer.cpp + plugins/global_voxel_layer.cpp plugins/range_sensor_layer.cpp ) ament_target_dependencies(layers diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml index e5867d51089..06d1857e59b 100644 --- a/nav2_costmap_2d/costmap_plugins.xml +++ b/nav2_costmap_2d/costmap_plugins.xml @@ -6,12 +6,18 @@ Listens to laser scan and point cloud messages and marks and clears grid cells. + + Listens to laser scan and point cloud messages and marks and clears grid cells. + Listens to OccupancyGrid messages and copies them in, like from map_server. Similar to obstacle costmap, but uses 3D voxel grid to store data. + + Similar to obstacle costmap, but uses 3D voxel grid to store data. + A range-sensor (sonar, IR) based obstacle layer for costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/global_obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/global_obstacle_layer.hpp new file mode 100644 index 00000000000..bf9536a9957 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/global_obstacle_layer.hpp @@ -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 & 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 & buffer); + +protected: + rclcpp::Publisher::SharedPtr obstacle_grid_pub_; + std::unique_ptr 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_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/global_voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/global_voxel_layer.hpp new file mode 100644 index 00000000000..23081f7a6a0 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/global_voxel_layer.hpp @@ -0,0 +1,132 @@ +/********************************************************************* + * + * 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 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 & 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 & buffer); + +protected: + rclcpp::Publisher::SharedPtr obstacle_grid_pub_; + std::unique_ptr 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__VOXEL_LAYER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp index c529fe3b162..754b434930c 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -250,22 +250,6 @@ class ObstacleLayer : public CostmapLayer std::vector static_clearing_observations_; std::vector static_marking_observations_; - rclcpp::Publisher::SharedPtr obstacle_grid_pub_; - std::unique_ptr grid_; - - bool is_init_scan_angle_; - bool use_init_scan_angle_; - double scan_start_angle_; - double scan_end_angle_; - float scan_link_offset_; - void initializeScanAngle(sensor_msgs::msg::LaserScan::ConstSharedPtr message); - double current_robot_yaw_; - void updateRobotYaw(double robot_yaw); - static char * cost_translation_table_; - rclcpp::Time last_publish_{0, 0, RCL_ROS_TIME}; - rclcpp::Duration publish_cycle_{1, 0}; - void prepareGrid(nav2_costmap_2d::Costmap2D costmap); - bool rolling_window_; bool was_reset_; int combination_method_; diff --git a/nav2_costmap_2d/plugins/global_obstacle_layer.cpp b/nav2_costmap_2d/plugins/global_obstacle_layer.cpp new file mode 100644 index 00000000000..69a8118eee3 --- /dev/null +++ b/nav2_costmap_2d/plugins/global_obstacle_layer.cpp @@ -0,0 +1,294 @@ +/********************************************************************* + * + * 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!! + * Steve Macenski + *********************************************************************/ +#include "nav2_costmap_2d/global_obstacle_layer.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_costmap_2d/observation_buffer.hpp" + +#include +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::GlobalObstacleLayer, nav2_costmap_2d::Layer) + +using nav2_costmap_2d::FREE_SPACE; +using nav2_costmap_2d::ObservationBuffer; + +namespace nav2_costmap_2d +{ +char * GlobalObstacleLayer::cost_translation_table_ = NULL; + +void GlobalObstacleLayer::onInitialize() +{ + ObstacleLayer::onInitialize(); + + RCLCPP_INFO(logger_, "[GlobalObstacleLayer] onInitialize with Obstacle Publisher"); + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // The topics that we'll subscribe to from the parameter server + std::string topics_string; + node->get_parameter(name_ + "." + "observation_sources", topics_string); + + // now we need to split the topics based on whitespace which we can use a stringstream for + std::stringstream ss(topics_string); + std::string source; + while (ss >> source) { + bool init_scan_angle; + std::string data_type; + bool inf_is_valid; + double publish_cycle; + declareParameter(source + "." + "init_scan_angle", rclcpp::ParameterValue(true)); + declareParameter(source + "." + "scan_link_offset", rclcpp::ParameterValue(2.0)); + declareParameter(source + "." + "publish_cycle", rclcpp::ParameterValue(1.0)); + + node->get_parameter(name_ + "." + source + "." + "data_type", data_type); + node->get_parameter(name_ + "." + source + "." + "inf_is_valid", inf_is_valid); + node->get_parameter(name_ + "." + source + "." + "init_scan_angle", init_scan_angle); + node->get_parameter(name_ + "." + source + "." + "scan_link_offset", scan_link_offset_); + node->get_parameter(name_ + "." + source + "." + "publish_cycle", publish_cycle); + + if (publish_cycle > 0) { + rclcpp::Duration::from_seconds(publish_cycle); + } else { + publish_cycle_ = rclcpp::Duration(0, 0); + } + + // create a callback for the topic + if (data_type == "LaserScan") { + auto filter = std::dynamic_pointer_cast>(observation_notifiers_.back()); + if (inf_is_valid) { + filter->registerCallback( + std::bind( + &GlobalObstacleLayer::laserScanValidInfCallback, this, std::placeholders::_1, + observation_buffers_.back())); + + } else { + filter->registerCallback( + std::bind( + &GlobalObstacleLayer::laserScanCallback, this, std::placeholders::_1, + observation_buffers_.back())); + } + } + + // [sungkyu.kang] initialize use_init_scan_angle_ parameter + use_init_scan_angle_ = init_scan_angle; + } + + // [sungkyu.kang] create for obstacle index + is_init_scan_angle_ = false; + + auto custom_qos2 = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(); + obstacle_grid_pub_ = node->create_publisher( + "obstacle_map", + custom_qos2); + + if (cost_translation_table_ == NULL) { + cost_translation_table_ = new char[256]; + + // special values: + cost_translation_table_[0] = FREE_SPACE; // NO obstacle + cost_translation_table_[253] = 99; // INSCRIBED obstacle + cost_translation_table_[254] = 100; // LETHAL obstacle + cost_translation_table_[255] = -1; // UNKNOWN + + // regular cost values scale the range 1 to 252 (inclusive) to fit + // into 1 to 98 (inclusive). + for (int i = 1; i < 253; i++) { + cost_translation_table_[i] = static_cast(1 + (97 * (i - 1)) / 251); + } + } +} + +void +GlobalObstacleLayer::initializeScanAngle(sensor_msgs::msg::LaserScan::ConstSharedPtr message) +{ + // [sungkyu.kang] initialize scan angles + RCLCPP_INFO(logger_, "GlobalObstacleLayer::initializeScanAngle"); + if (use_init_scan_angle_) { + float current_angle = message->angle_min; + float range = 0.0; + + for (size_t i = 0; i < message->ranges.size(); i++) { + range = message->ranges[i]; + if (range >= message->range_min && range <= message->range_max) { + scan_start_angle_ = current_angle + 0.05; + break; + } + current_angle = current_angle + message->angle_increment; + } + current_angle = message->angle_max; + for (size_t i = message->ranges.size(); i > 0; i--) { + range = message->ranges[i]; + if (range >= message->range_min && range <= message->range_max) { + scan_end_angle_ = current_angle - 0.05; + break; + } + current_angle = current_angle - message->angle_increment; + } + } else { + scan_start_angle_ = message->angle_min; + scan_end_angle_ = message->angle_max; + } + + RCLCPP_INFO(logger_, " scan_start_angle: %f, scan_end_angle_: %f", scan_start_angle_, scan_end_angle_); + is_init_scan_angle_ = true; +} + +void +GlobalObstacleLayer::laserScanCallback( + sensor_msgs::msg::LaserScan::ConstSharedPtr message, + const std::shared_ptr & buffer) +{ + if (!is_init_scan_angle_) { + initializeScanAngle(message); + } + ObstacleLayer::laserScanCallback(message, buffer); +} + +void +GlobalObstacleLayer::laserScanValidInfCallback( + sensor_msgs::msg::LaserScan::ConstSharedPtr raw_message, + const std::shared_ptr & buffer) +{ + if (!is_init_scan_angle_) { + initializeScanAngle(raw_message); + } + laserScanValidInfCallback(raw_message, buffer); +} + +void GlobalObstacleLayer::updateRobotYaw(const double robot_yaw) +{ + // [sungkyu.kang] set current_robot_yaw_ + current_robot_yaw_ = robot_yaw; + // RCLCPP_INFO(logger_, "updateBounds - robot_yaw: %f, current_robot_yaw_: %f", robot_yaw, current_robot_yaw_); +} + +void +GlobalObstacleLayer::updateBounds( + double robot_x, double robot_y, double robot_yaw, double * min_x, + double * min_y, double * max_x, double * max_y) +{ + { + std::lock_guard guard(*getMutex()); + updateRobotYaw(robot_yaw); + } + + ObstacleLayer::updateBounds(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); +} + +void GlobalObstacleLayer::updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, int min_j, + int max_i, int max_j) +{ + ObstacleLayer::updateCosts(master_grid, min_i, min_j, max_i, max_j); + + // [sungkyu.kang] publish current master grid + if (publish_cycle_ > rclcpp::Duration(0, 0) && obstacle_grid_pub_->get_subscription_count() > 0) { + const auto current_time = clock_->now(); + if ((last_publish_ + publish_cycle_ < current_time) || // publish_cycle_ is due + (current_time < last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT + { + prepareGrid(master_grid); + obstacle_grid_pub_->publish(std::move(grid_)); + last_publish_ = current_time; + } + } +} + +void GlobalObstacleLayer::prepareGrid(nav2_costmap_2d::Costmap2D costmap) +{ + std::unique_lock lock(*(costmap.getMutex())); + float grid_resolution = costmap.getResolution(); + unsigned int grid_width = costmap.getSizeInCellsX(); + unsigned int grid_height = costmap.getSizeInCellsY(); + + grid_ = std::make_unique(); + + grid_->header.frame_id = global_frame_; + grid_->header.stamp = clock_->now(); + + grid_->info.resolution = grid_resolution; + + grid_->info.width = grid_width; + grid_->info.height = grid_height; + + double wx, wy; + costmap.mapToWorld(0, 0, wx, wy); + grid_->info.origin.position.x = wx - grid_resolution / 2; + grid_->info.origin.position.y = wy - grid_resolution / 2; + grid_->info.origin.position.z = 0.0; + grid_->info.origin.orientation.w = 1.0; + + grid_->data.resize(grid_->info.width * grid_->info.height); + + unsigned char * data = costmap.getCharMap(); + + double normalLeftX = -1 * sin(current_robot_yaw_ - scan_start_angle_); + double normalLeftY = cos(current_robot_yaw_ - scan_start_angle_); + double normalRightX = sin(current_robot_yaw_ - scan_end_angle_); + double normalRightY = -1 * cos(current_robot_yaw_ - scan_end_angle_); + + // RCLCPP_INFO( + // logger_, + // "\nGlobalObstacleLayer::prepareGrid %f, %f, %f.\n %f, %f, %f, %f", current_robot_yaw_, current_robot_yaw_ - scan_start_angle_, current_robot_yaw_ - scan_end_angle_, normalLeftX, normalLeftY, normalRightX, normalRightY); + int x, y; + unsigned int row_index, col_index; + int scan_tf_offset_x = round(scan_link_offset_ * cos(current_robot_yaw_)); + int scan_tf_offset_y = round(scan_link_offset_ * sin(current_robot_yaw_)); + // RCLCPP_INFO(logger_, "scan_tf_offset_x: %d, scan_tf_offset_y: %d", scan_tf_offset_x, scan_tf_offset_y); + for (unsigned int i = 0; i < grid_->data.size(); i++) { + // [sungkyu.kang] check robot orientation. fill -1 back of robot. + row_index = i / grid_width; + col_index = i % grid_width; + x = col_index - grid_height / 2 - scan_tf_offset_x; + y = row_index - grid_width / 2 - scan_tf_offset_y; + // RCLCPP_INFO( + // logger_, + // " [%4d] row_index: %2d, col_index: %2d, (%2d, %2d)", i, row_index, col_index, x, y); + if (x * normalLeftX + y * normalLeftY > 0 && x * normalRightX + y * normalRightY > 0) { + // RCLCPP_INFO( logger_," hit!!"); + grid_->data[i] = -1; + } else { + grid_->data[i] = cost_translation_table_[data[i]]; + } + } + // RCLCPP_INFO(logger_, "GlobalObstacleLayer::prepareGrid finish "); +} +} // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/global_voxel_layer.cpp b/nav2_costmap_2d/plugins/global_voxel_layer.cpp new file mode 100644 index 00000000000..d04ec60b3fd --- /dev/null +++ b/nav2_costmap_2d/plugins/global_voxel_layer.cpp @@ -0,0 +1,289 @@ +/********************************************************************* + * + * 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!! + *********************************************************************/ + +#include "nav2_costmap_2d/global_voxel_layer.hpp" + +#include +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::GlobalVoxelLayer, nav2_costmap_2d::Layer) + +namespace nav2_costmap_2d +{ +char * GlobalVoxelLayer::cost_translation_table_ = NULL; + +void GlobalVoxelLayer::onInitialize() +{ + VoxelLayer::onInitialize(); + + RCLCPP_INFO(logger_, "[GlobalVoxelLayer] onInitialize with Obstacle Publisher"); + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // The topics that we'll subscribe to from the parameter server + std::string topics_string; + node->get_parameter(name_ + "." + "observation_sources", topics_string); + + // now we need to split the topics based on whitespace which we can use a stringstream for + std::stringstream ss(topics_string); + std::string source; + while (ss >> source) { + bool init_scan_angle; + std::string data_type; + bool inf_is_valid; + double publish_cycle; + declareParameter(source + "." + "init_scan_angle", rclcpp::ParameterValue(true)); + declareParameter(source + "." + "scan_link_offset", rclcpp::ParameterValue(2.0)); + declareParameter(source + "." + "publish_cycle", rclcpp::ParameterValue(1.0)); + + node->get_parameter(name_ + "." + source + "." + "data_type", data_type); + node->get_parameter(name_ + "." + source + "." + "inf_is_valid", inf_is_valid); + node->get_parameter(name_ + "." + source + "." + "init_scan_angle", init_scan_angle); + node->get_parameter(name_ + "." + source + "." + "scan_link_offset", scan_link_offset_); + node->get_parameter(name_ + "." + source + "." + "publish_cycle", publish_cycle); + + if (publish_cycle > 0) { + rclcpp::Duration::from_seconds(publish_cycle); + } else { + publish_cycle_ = rclcpp::Duration(0, 0); + } + + // create a callback for the topic + if (data_type == "LaserScan") { + auto filter = std::dynamic_pointer_cast>(observation_notifiers_.back()); + if (inf_is_valid) { + filter->registerCallback( + std::bind( + &GlobalVoxelLayer::laserScanValidInfCallback, this, std::placeholders::_1, + observation_buffers_.back())); + + } else { + filter->registerCallback( + std::bind( + &GlobalVoxelLayer::laserScanCallback, this, std::placeholders::_1, + observation_buffers_.back())); + } + } + + // [sungkyu.kang] initialize use_init_scan_angle_ parameter + use_init_scan_angle_ = init_scan_angle; + } + + // [sungkyu.kang] create for obstacle index + is_init_scan_angle_ = false; + + auto custom_qos2 = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(); + obstacle_grid_pub_ = node->create_publisher( + "obstacle_map", + custom_qos2); + + if (cost_translation_table_ == NULL) { + cost_translation_table_ = new char[256]; + + // special values: + cost_translation_table_[0] = FREE_SPACE; // NO obstacle + cost_translation_table_[253] = 99; // INSCRIBED obstacle + cost_translation_table_[254] = 100; // LETHAL obstacle + cost_translation_table_[255] = -1; // UNKNOWN + + // regular cost values scale the range 1 to 252 (inclusive) to fit + // into 1 to 98 (inclusive). + for (int i = 1; i < 253; i++) { + cost_translation_table_[i] = static_cast(1 + (97 * (i - 1)) / 251); + } + } +} + +void +GlobalVoxelLayer::initializeScanAngle(sensor_msgs::msg::LaserScan::ConstSharedPtr message) +{ + // [sungkyu.kang] initialize scan angles + RCLCPP_INFO(logger_, "GlobalVoxelLayer::initializeScanAngle"); + if (use_init_scan_angle_) { + float current_angle = message->angle_min; + float range = 0.0; + + for (size_t i = 0; i < message->ranges.size(); i++) { + range = message->ranges[i]; + if (range >= message->range_min && range <= message->range_max) { + scan_start_angle_ = current_angle + 0.05; + break; + } + current_angle = current_angle + message->angle_increment; + } + current_angle = message->angle_max; + for (size_t i = message->ranges.size(); i > 0; i--) { + range = message->ranges[i]; + if (range >= message->range_min && range <= message->range_max) { + scan_end_angle_ = current_angle - 0.05; + break; + } + current_angle = current_angle - message->angle_increment; + } + } else { + scan_start_angle_ = message->angle_min; + scan_end_angle_ = message->angle_max; + } + + RCLCPP_INFO(logger_, " scan_start_angle: %f, scan_end_angle_: %f", scan_start_angle_, scan_end_angle_); + is_init_scan_angle_ = true; +} + +void +GlobalVoxelLayer::laserScanCallback( + sensor_msgs::msg::LaserScan::ConstSharedPtr message, + const std::shared_ptr & buffer) +{ + if (!is_init_scan_angle_) { + initializeScanAngle(message); + } + ObstacleLayer::laserScanCallback(message, buffer); +} + +void +GlobalVoxelLayer::laserScanValidInfCallback( + sensor_msgs::msg::LaserScan::ConstSharedPtr raw_message, + const std::shared_ptr & buffer) +{ + if (!is_init_scan_angle_) { + initializeScanAngle(raw_message); + } + laserScanValidInfCallback(raw_message, buffer); +} + +void GlobalVoxelLayer::updateRobotYaw(const double robot_yaw) +{ + // [sungkyu.kang] set current_robot_yaw_ + current_robot_yaw_ = robot_yaw; + // RCLCPP_INFO(logger_, "updateBounds - robot_yaw: %f, current_robot_yaw_: %f", robot_yaw, current_robot_yaw_); +} + +void GlobalVoxelLayer::updateBounds( + double robot_x, double robot_y, double robot_yaw, + double * min_x, double * min_y, + double * max_x, double * max_y) +{ + { + std::lock_guard guard(*getMutex()); + updateRobotYaw(robot_yaw); + } + + VoxelLayer::updateBounds(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); +} + +void GlobalVoxelLayer::updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, int min_j, + int max_i, int max_j) +{ + ObstacleLayer::updateCosts(master_grid, min_i, min_j, max_i, max_j); + + // [sungkyu.kang] publish current master grid + if (publish_cycle_ > rclcpp::Duration(0, 0) && obstacle_grid_pub_->get_subscription_count() > 0) { + const auto current_time = clock_->now(); + if ((last_publish_ + publish_cycle_ < current_time) || // publish_cycle_ is due + (current_time < last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT + { + prepareGrid(master_grid); + obstacle_grid_pub_->publish(std::move(grid_)); + last_publish_ = current_time; + } + } +} + +void GlobalVoxelLayer::prepareGrid(nav2_costmap_2d::Costmap2D costmap) +{ + std::unique_lock lock(*(costmap.getMutex())); + float grid_resolution = costmap.getResolution(); + unsigned int grid_width = costmap.getSizeInCellsX(); + unsigned int grid_height = costmap.getSizeInCellsY(); + + grid_ = std::make_unique(); + + grid_->header.frame_id = global_frame_; + grid_->header.stamp = clock_->now(); + + grid_->info.resolution = grid_resolution; + + grid_->info.width = grid_width; + grid_->info.height = grid_height; + + double wx, wy; + costmap.mapToWorld(0, 0, wx, wy); + grid_->info.origin.position.x = wx - grid_resolution / 2; + grid_->info.origin.position.y = wy - grid_resolution / 2; + grid_->info.origin.position.z = 0.0; + grid_->info.origin.orientation.w = 1.0; + + grid_->data.resize(grid_->info.width * grid_->info.height); + + unsigned char * data = costmap.getCharMap(); + + double normalLeftX = -1 * sin(current_robot_yaw_ - scan_start_angle_); + double normalLeftY = cos(current_robot_yaw_ - scan_start_angle_); + double normalRightX = sin(current_robot_yaw_ - scan_end_angle_); + double normalRightY = -1 * cos(current_robot_yaw_ - scan_end_angle_); + + // RCLCPP_INFO( + // logger_, + // "\nGlobalVoxelLayer::prepareGrid %f, %f, %f.\n %f, %f, %f, %f", current_robot_yaw_, current_robot_yaw_ - scan_start_angle_, current_robot_yaw_ - scan_end_angle_, normalLeftX, normalLeftY, normalRightX, normalRightY); + int x, y; + unsigned int row_index, col_index; + int scan_tf_offset_x = round(scan_link_offset_ * cos(current_robot_yaw_)); + int scan_tf_offset_y = round(scan_link_offset_ * sin(current_robot_yaw_)); + // RCLCPP_INFO(logger_, "scan_tf_offset_x: %d, scan_tf_offset_y: %d", scan_tf_offset_x, scan_tf_offset_y); + for (unsigned int i = 0; i < grid_->data.size(); i++) { + // [sungkyu.kang] check robot orientation. fill -1 back of robot. + row_index = i / grid_width; + col_index = i % grid_width; + x = col_index - grid_height / 2 - scan_tf_offset_x; + y = row_index - grid_width / 2 - scan_tf_offset_y; + // RCLCPP_INFO( + // logger_, + // " [%4d] row_index: %2d, col_index: %2d, (%2d, %2d)", i, row_index, col_index, x, y); + if (x * normalLeftX + y * normalLeftY > 0 && x * normalRightX + y * normalRightY > 0) { + // RCLCPP_INFO( logger_," hit!!"); + grid_->data[i] = -1; + } else { + grid_->data[i] = cost_translation_table_[data[i]]; + } + } + // RCLCPP_INFO(logger_, "GlobalVoxelLayer::prepareGrid finish "); +} +} // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index c7acaff8d60..5d153e7d760 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -42,7 +42,6 @@ #include #include #include -#include #include "pluginlib/class_list_macros.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" @@ -61,10 +60,9 @@ using rcl_interfaces::msg::ParameterType; namespace nav2_costmap_2d { -char * ObstacleLayer::cost_translation_table_ = NULL; - ObstacleLayer::~ObstacleLayer() { + std::cout << "ObstacleLayer destructor called" << std::endl; dyn_params_handler_.reset(); for (auto & notifier : observation_notifiers_) { notifier.reset(); @@ -73,7 +71,8 @@ ObstacleLayer::~ObstacleLayer() void ObstacleLayer::onInitialize() { - RCLCPP_INFO(logger_, "[ObstacleLayer] onInitialize with Obstacle Index Publisher"); + std::cout << "ObstacleLayer onInitialize called" << std::endl; + bool track_unknown_space; double transform_tolerance; @@ -153,12 +152,6 @@ void ObstacleLayer::onInitialize() declareParameter(source + "." + "raytrace_max_range", rclcpp::ParameterValue(3.0)); declareParameter(source + "." + "raytrace_min_range", rclcpp::ParameterValue(0.0)); - bool init_scan_angle; - double publish_cycle; - declareParameter(source + "." + "init_scan_angle", rclcpp::ParameterValue(true)); - declareParameter(source + "." + "scan_link_offset", rclcpp::ParameterValue(2.0)); - declareParameter(source + "." + "publish_cycle", rclcpp::ParameterValue(1.0)); - node->get_parameter(name_ + "." + source + "." + "topic", topic); node->get_parameter(name_ + "." + source + "." + "sensor_frame", sensor_frame); node->get_parameter( @@ -192,15 +185,6 @@ void ObstacleLayer::onInitialize() node->get_parameter(name_ + "." + source + "." + "raytrace_min_range", raytrace_min_range); node->get_parameter(name_ + "." + source + "." + "raytrace_max_range", raytrace_max_range); - node->get_parameter(name_ + "." + source + "." + "init_scan_angle", init_scan_angle); - node->get_parameter(name_ + "." + source + "." + "scan_link_offset", scan_link_offset_); - node->get_parameter(name_ + "." + source + "." + "publish_cycle", publish_cycle); - - if (publish_cycle > 0) { - publish_cycle_ = rclcpp::Duration::from_seconds( publish_cycle ); - } else { - publish_cycle_ = rclcpp::Duration(0, 0); - } RCLCPP_DEBUG( logger_, @@ -302,34 +286,7 @@ void ObstacleLayer::onInitialize() target_frames.push_back(sensor_frame); observation_notifiers_.back()->setTargetFrames(target_frames); } - - // [sungkyu.kang] initialize use_init_scan_angle_ parameter - use_init_scan_angle_ = init_scan_angle; } - - // [sungkyu.kang] create for obstacle index - is_init_scan_angle_ = false; - - auto custom_qos2 = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(); - obstacle_grid_pub_ = node->create_publisher( - "obstacle_map", - custom_qos2); - - if (cost_translation_table_ == NULL) { - cost_translation_table_ = new char[256]; - - // special values: - cost_translation_table_[0] = 0; // NO obstacle - cost_translation_table_[253] = 99; // INSCRIBED obstacle - cost_translation_table_[254] = 100; // LETHAL obstacle - cost_translation_table_[255] = -1; // UNKNOWN - - // regular cost values scale the range 1 to 252 (inclusive) to fit - // into 1 to 98 (inclusive). - for (int i = 1; i < 253; i++) { - cost_translation_table_[i] = static_cast(1 + (97 * (i - 1)) / 251); - } - } } rcl_interfaces::msg::SetParametersResult @@ -366,49 +323,11 @@ ObstacleLayer::dynamicParametersCallback( return result; } -void -ObstacleLayer::initializeScanAngle(sensor_msgs::msg::LaserScan::ConstSharedPtr message) -{ - // [sungkyu.kang] initialize scan angles - RCLCPP_INFO(logger_, "ObstacleLayer::initializeScanAngle"); - if (use_init_scan_angle_) { - float current_angle = message->angle_min; - float range = 0.0; - - for (size_t i = 0; i < message->ranges.size(); i++) { - range = message->ranges[i]; - if (range >= message->range_min && range <= message->range_max) { - scan_start_angle_ = current_angle + 0.05; - break; - } - current_angle = current_angle + message->angle_increment; - } - current_angle = message->angle_max; - for (size_t i = message->ranges.size(); i > 0; i--) { - range = message->ranges[i]; - if (range >= message->range_min && range <= message->range_max) { - scan_end_angle_ = current_angle - 0.05; - break; - } - current_angle = current_angle - message->angle_increment; - } - } else { - scan_start_angle_ = message->angle_min; - scan_end_angle_ = message->angle_max; - } - - RCLCPP_INFO(logger_, " scan_start_angle: %f, scan_end_angle_: %f", scan_start_angle_, scan_end_angle_); - is_init_scan_angle_ = true; -} - void ObstacleLayer::laserScanCallback( sensor_msgs::msg::LaserScan::ConstSharedPtr message, const std::shared_ptr & buffer) { - if (!is_init_scan_angle_) { - initializeScanAngle(message); - } // project the laser into a point cloud sensor_msgs::msg::PointCloud2 cloud; cloud.header = message->header; @@ -443,9 +362,6 @@ ObstacleLayer::laserScanValidInfCallback( sensor_msgs::msg::LaserScan::ConstSharedPtr raw_message, const std::shared_ptr & buffer) { - if (!is_init_scan_angle_) { - initializeScanAngle(raw_message); - } // Filter positive infinities ("Inf"s) to max_range. float epsilon = 0.0001; // a tenth of a millimeter sensor_msgs::msg::LaserScan message = *raw_message; @@ -495,21 +411,13 @@ ObstacleLayer::pointCloud2Callback( buffer->unlock(); } -void -ObstacleLayer::updateRobotYaw(double robot_yaw) -{ - // [sungkyu.kang] set current_robot_yaw_ - current_robot_yaw_ = robot_yaw; - // RCLCPP_INFO(logger_, "updateBounds - robot_yaw: %f, current_robot_yaw_: %f", robot_yaw, current_robot_yaw_); -} - void ObstacleLayer::updateBounds( - double robot_x, double robot_y, double robot_yaw, double * min_x, - double * min_y, double * max_x, double * max_y) + double robot_x, double robot_y, double robot_yaw, + double * min_x, double * min_y, + double * max_x, double * max_y) { std::lock_guard guard(*getMutex()); - updateRobotYaw(robot_yaw); if (rolling_window_) { updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); } @@ -549,7 +457,6 @@ ObstacleLayer::updateBounds( sensor_msgs::PointCloud2ConstIterator iter_x(cloud, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); sensor_msgs::PointCloud2ConstIterator iter_z(cloud, "z"); - // RCLCPP_INFO(logger_, "[ObstacleLayer] observation --------------------------- "); for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { double px = *iter_x, py = *iter_y, pz = *iter_z; @@ -593,13 +500,11 @@ ObstacleLayer::updateBounds( unsigned int index = getIndex(mx, my); costmap_[index] = LETHAL_OBSTACLE; - touch(px, py, min_x, min_y, max_x, max_y); } } updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); - } void @@ -648,84 +553,8 @@ ObstacleLayer::updateCosts( default: // Nothing break; } - - // [sungkyu.kang] publish current master grid - - if (publish_cycle_ > rclcpp::Duration(0, 0) && obstacle_grid_pub_->get_subscription_count() > 0) { - - auto current_time = clock_->now(); - if ((last_publish_ + publish_cycle_ < current_time) || // publish_cycle_ is due - (current_time < last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT - { - prepareGrid(master_grid); - obstacle_grid_pub_->publish(std::move(grid_)); - last_publish_ = current_time; - } - } - } -void ObstacleLayer::prepareGrid(nav2_costmap_2d::Costmap2D costmap) -{ - std::unique_lock lock(*(costmap.getMutex())); - float grid_resolution = costmap.getResolution(); - unsigned int grid_width = costmap.getSizeInCellsX(); - unsigned int grid_height = costmap.getSizeInCellsY(); - - grid_ = std::make_unique(); - - grid_->header.frame_id = global_frame_; - grid_->header.stamp = clock_->now(); - - grid_->info.resolution = grid_resolution; - - grid_->info.width = grid_width; - grid_->info.height = grid_height; - - double wx, wy; - costmap.mapToWorld(0, 0, wx, wy); - grid_->info.origin.position.x = wx - grid_resolution / 2; - grid_->info.origin.position.y = wy - grid_resolution / 2; - grid_->info.origin.position.z = 0.0; - grid_->info.origin.orientation.w = 1.0; - - grid_->data.resize(grid_->info.width * grid_->info.height); - - unsigned char * data = costmap.getCharMap(); - - double normalLeftX = -1 * sin(current_robot_yaw_ - scan_start_angle_); - double normalLeftY = cos(current_robot_yaw_ - scan_start_angle_); - double normalRightX = sin(current_robot_yaw_ - scan_end_angle_); - double normalRightY = -1 * cos(current_robot_yaw_ - scan_end_angle_); - - // RCLCPP_INFO( - // logger_, - // "\nObstacleLayer::prepareGrid %f, %f, %f.\n %f, %f, %f, %f", current_robot_yaw_, current_robot_yaw_ - scan_start_angle_, current_robot_yaw_ - scan_end_angle_, normalLeftX, normalLeftY, normalRightX, normalRightY); - int x, y; - unsigned int row_index, col_index; - int scan_tf_offset_x = round(scan_link_offset_ * cos(current_robot_yaw_)); - int scan_tf_offset_y = round(scan_link_offset_ * sin(current_robot_yaw_)); - // RCLCPP_INFO(logger_, "scan_tf_offset_x: %d, scan_tf_offset_y: %d", scan_tf_offset_x, scan_tf_offset_y); - for (unsigned int i = 0; i < grid_->data.size(); i++) { - // [sungkyu.kang] check robot orientation. fill -1 back of robot. - row_index = i / grid_width; - col_index = i % grid_width; - x = col_index - grid_height / 2 - scan_tf_offset_x; - y = row_index - grid_width / 2 - scan_tf_offset_y; - // RCLCPP_INFO( - // logger_, - // " [%4d] row_index: %2d, col_index: %2d, (%2d, %2d)", i, row_index, col_index, x, y); - if (x * normalLeftX + y * normalLeftY > 0 && x * normalRightX + y * normalRightY > 0) { - // RCLCPP_INFO( logger_," hit!!"); - grid_->data[i] = -1; - } else { - grid_->data[i] = cost_translation_table_[data[i]]; - } - } - // RCLCPP_INFO(logger_, "ObstacleLayer::prepareGrid finish "); -} - - void ObstacleLayer::addStaticObservation( nav2_costmap_2d::Observation & obs, diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index f60a9f5c7b1..f8c62d06b9e 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -146,7 +146,7 @@ void VoxelLayer::updateBounds( double * min_y, double * max_x, double * max_y) { std::lock_guard guard(*getMutex()); - updateRobotYaw(robot_yaw); + if (rolling_window_) { updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); }