From fb69b25be69b17bc48e556916be80224e19a9afc Mon Sep 17 00:00:00 2001 From: Shivang Patel Date: Thu, 7 May 2020 20:24:06 -0400 Subject: [PATCH 1/9] Add collision checking for footprint without using subscibers. --- nav2_costmap_2d/CMakeLists.txt | 16 +++- .../nav2_costmap_2d/collision_base.hpp | 50 ++++++++++ .../nav2_costmap_2d/collision_checker.hpp | 14 +-- .../nav2_costmap_2d/collision_footprint.hpp | 52 ++++++++++ nav2_costmap_2d/src/collision_base.cpp | 78 +++++++++++++++ nav2_costmap_2d/src/collision_checker.cpp | 55 +---------- nav2_costmap_2d/src/collision_footprint.cpp | 63 +++++++++++++ nav2_costmap_2d/test/unit/CMakeLists.txt | 5 + .../test/unit/collision_footprint_test.cpp | 94 +++++++++++++++++++ 9 files changed, 368 insertions(+), 59 deletions(-) create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/collision_base.hpp create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/collision_footprint.hpp create mode 100644 nav2_costmap_2d/src/collision_base.cpp create mode 100644 nav2_costmap_2d/src/collision_footprint.cpp create mode 100644 nav2_costmap_2d/test/unit/collision_footprint_test.cpp diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 8f3262b8a69..808b2db2c26 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -95,6 +95,7 @@ add_library(nav2_costmap_2d_client SHARED src/footprint_subscriber.cpp src/costmap_subscriber.cpp src/collision_checker.cpp + src/collision_base.cpp ) ament_target_dependencies(nav2_costmap_2d_client @@ -105,6 +106,19 @@ target_link_libraries(nav2_costmap_2d_client nav2_costmap_2d_core ) +add_library(nav2_costmap_2d_standalone SHARED + src/collision_footprint.cpp + src/collision_base.cpp +) + +ament_target_dependencies(nav2_costmap_2d_standalone + ${dependencies} +) + +target_link_libraries(nav2_costmap_2d_standalone + nav2_costmap_2d_core +) + add_executable(nav2_costmap_2d_markers src/costmap_2d_markers.cpp) target_link_libraries(nav2_costmap_2d_markers nav2_costmap_2d_core @@ -165,7 +179,7 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(layers nav2_costmap_2d_core nav2_costmap_2d_client) +ament_export_libraries(layers nav2_costmap_2d_core nav2_costmap_2d_client nav2_costmap_2d_standalone) ament_export_dependencies(${dependencies}) pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml) ament_package() diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/collision_base.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/collision_base.hpp new file mode 100644 index 00000000000..b6d8fadc047 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/collision_base.hpp @@ -0,0 +1,50 @@ +// Copyright (c) 2020 Shivang Patel +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_COSTMAP_2D__COLLISION_BASE_HPP_ +#define NAV2_COSTMAP_2D__COLLISION_BASE_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_costmap_2d/footprint_subscriber.hpp" +#include "nav2_util/robot_utils.hpp" +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wpedantic" +#include "tf2/utils.h" +#pragma GCC diagnostic pop + +namespace nav2_costmap_2d +{ +typedef std::vector Footprint; + +class CollisionBase +{ +public: + double footprintCost(const Footprint footprint); + double lineCost(int x0, int x1, int y0, int y1) const; + virtual void worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) = 0; + virtual double pointCost(int x, int y) const = 0; +}; + +} // namespace nav2_costmap_2d + +#endif // NAV2_COSTMAP_2D__COLLISION_BASE_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp index 13139e6cd7d..93e2d7084e7 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 Intel Corporation +// Copyright (c) 2020 Shivang Patel // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -24,6 +25,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose2_d.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/collision_base.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_costmap_2d/footprint_subscriber.hpp" #include "nav2_util/robot_utils.hpp" @@ -34,9 +36,8 @@ namespace nav2_costmap_2d { -typedef std::vector Footprint; -class CollisionChecker +class CollisionChecker : public CollisionBase { public: CollisionChecker( @@ -46,19 +47,17 @@ class CollisionChecker std::string name = "collision_checker", std::string global_frame = "map"); - ~CollisionChecker(); + ~CollisionChecker() = default; // Returns the obstacle footprint score for a particular pose double scorePose(const geometry_msgs::msg::Pose2D & pose); bool isCollisionFree(const geometry_msgs::msg::Pose2D & pose); protected: - double lineCost(int x0, int x1, int y0, int y1) const; - double pointCost(int x, int y) const; + double pointCost(int x, int y) const override; void unorientFootprint(const Footprint & oriented_footprint, Footprint & reset_footprint); - void worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my); + void worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) override; Footprint getFootprint(const geometry_msgs::msg::Pose2D & pose); - double footprintCost(const Footprint footprint); std::shared_ptr costmap_; @@ -69,6 +68,7 @@ class CollisionChecker CostmapSubscriber & costmap_sub_; FootprintSubscriber & footprint_sub_; }; + } // namespace nav2_costmap_2d #endif // NAV2_COSTMAP_2D__COLLISION_CHECKER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/collision_footprint.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/collision_footprint.hpp new file mode 100644 index 00000000000..5a1ec4015d3 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/collision_footprint.hpp @@ -0,0 +1,52 @@ +// Copyright (c) 2020 Shivang Patel +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_COSTMAP_2D__COLLISION_FOOTPRINT_HPP_ +#define NAV2_COSTMAP_2D__COLLISION_FOOTPRINT_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/collision_base.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_costmap_2d/footprint_subscriber.hpp" +#include "nav2_util/robot_utils.hpp" +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wpedantic" +#include "tf2/utils.h" +#pragma GCC diagnostic pop + +namespace nav2_costmap_2d +{ +class CollisionFootprint : public CollisionBase +{ +public: + CollisionFootprint(std::shared_ptr costmap); + double footprintCostWithPose(double x, double y, double theta, const Footprint footprint); + void worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) override; + double pointCost(int x, int y) const override; + +private: + std::shared_ptr costmap_; +}; + +} // namespace nav2_costmap_2d + +#endif // NAV2_COSTMAP_2D__COLLISION_FOOTPRINT_HPP_ diff --git a/nav2_costmap_2d/src/collision_base.cpp b/nav2_costmap_2d/src/collision_base.cpp new file mode 100644 index 00000000000..dd4bc59974d --- /dev/null +++ b/nav2_costmap_2d/src/collision_base.cpp @@ -0,0 +1,78 @@ +// Copyright (c) 2020 Shivang Patel +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "nav2_costmap_2d/collision_base.hpp" + +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_costmap_2d/exceptions.hpp" +#include "nav2_costmap_2d/footprint.hpp" +#include "nav2_util/line_iterator.hpp" + +using namespace std::chrono_literals; + +namespace nav2_costmap_2d +{ + +double CollisionBase::footprintCost(const Footprint footprint) +{ + // now we really have to lay down the footprint in the costmap_ grid + unsigned int x0, x1, y0, y1; + double footprint_cost = 0.0; + + // we need to rasterize each line in the footprint + for (unsigned int i = 0; i < footprint.size() - 1; ++i) { + // get the cell coord of the first point + worldToMap(footprint[i].x, footprint[i].y, x0, y0); + + // get the cell coord of the second point + worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1); + + footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost); + } + + // we also need to connect the first point in the footprint to the last point + // get the cell coord of the last point + worldToMap(footprint.back().x, footprint.back().y, x0, y0); + + // get the cell coord of the first point + worldToMap(footprint.front().x, footprint.front().y, x1, y1); + + footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost); + + // if all line costs are legal... then we can return that the footprint is legal + return footprint_cost; +} + +double CollisionBase::lineCost(int x0, int x1, int y0, int y1) const +{ + double line_cost = 0.0; + double point_cost = -1.0; + + for (nav2_util::LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) { + point_cost = pointCost(line.getX(), line.getY()); // Score the current point + + if (line_cost < point_cost) { + line_cost = point_cost; + } + } + + return line_cost; +} + +} // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/collision_checker.cpp b/nav2_costmap_2d/src/collision_checker.cpp index 4e8d92c1278..5931440ee3e 100644 --- a/nav2_costmap_2d/src/collision_checker.cpp +++ b/nav2_costmap_2d/src/collision_checker.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 Intel Corporation +// Copyright (c) 2020 Shivang Patel // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -35,7 +36,8 @@ CollisionChecker::CollisionChecker( tf2_ros::Buffer & tf, std::string name, std::string global_frame) -: name_(name), +: CollisionBase(), + name_(name), global_frame_(global_frame), tf_(tf), costmap_sub_(costmap_sub), @@ -43,10 +45,6 @@ CollisionChecker::CollisionChecker( { } -CollisionChecker::~CollisionChecker() -{ -} - bool CollisionChecker::isCollisionFree( const geometry_msgs::msg::Pose2D & pose) { @@ -107,52 +105,6 @@ Footprint CollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose return footprint; } -double CollisionChecker::footprintCost(const Footprint footprint) -{ - // now we really have to lay down the footprint in the costmap_ grid - unsigned int x0, x1, y0, y1; - double footprint_cost = 0.0; - - // we need to rasterize each line in the footprint - for (unsigned int i = 0; i < footprint.size() - 1; ++i) { - // get the cell coord of the first point - worldToMap(footprint[i].x, footprint[i].y, x0, y0); - - // get the cell coord of the second point - worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1); - - footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost); - } - - // we also need to connect the first point in the footprint to the last point - // get the cell coord of the last point - worldToMap(footprint.back().x, footprint.back().y, x0, y0); - - // get the cell coord of the first point - worldToMap(footprint.front().x, footprint.front().y, x1, y1); - - footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost); - - // if all line costs are legal... then we can return that the footprint is legal - return footprint_cost; -} - -double CollisionChecker::lineCost(int x0, int x1, int y0, int y1) const -{ - double line_cost = 0.0; - double point_cost = -1.0; - - for (nav2_util::LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) { - point_cost = pointCost(line.getX(), line.getY()); // Score the current point - - if (line_cost < point_cost) { - line_cost = point_cost; - } - } - - return line_cost; -} - double CollisionChecker::pointCost(int x, int y) const { unsigned char cost = costmap_->getCost(x, y); @@ -186,4 +138,5 @@ void CollisionChecker::unorientFootprint( transformFootprint(0, 0, -theta, temp, reset_footprint); } + } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/collision_footprint.cpp b/nav2_costmap_2d/src/collision_footprint.cpp new file mode 100644 index 00000000000..42559eaaa81 --- /dev/null +++ b/nav2_costmap_2d/src/collision_footprint.cpp @@ -0,0 +1,63 @@ +// Copyright (c) 2020 Shivang Patel +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "nav2_costmap_2d/collision_footprint.hpp" + +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_costmap_2d/exceptions.hpp" +#include "nav2_costmap_2d/footprint.hpp" +#include "nav2_util/line_iterator.hpp" + +using namespace std::chrono_literals; + +namespace nav2_costmap_2d +{ + +CollisionFootprint::CollisionFootprint(std::shared_ptr costmap) +: costmap_(costmap) +{} + +double CollisionFootprint::pointCost(int x, int y) const +{ + return costmap_->getCost(x, y); +} + +void CollisionFootprint::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) +{ + costmap_->worldToMap(wx, wy, mx, my); +} + +double CollisionFootprint::footprintCostWithPose( + double x, double y, double theta, + const Footprint footprint) +{ + double cos_th = cos(theta); + double sin_th = sin(theta); + Footprint oriented_footprint; + for (unsigned int i = 0; i < footprint.size(); ++i) { + geometry_msgs::msg::Point new_pt; + new_pt.x = x + (footprint[i].x * cos_th - footprint[i].y * sin_th); + new_pt.y = y + (footprint[i].x * sin_th + footprint[i].y * cos_th); + oriented_footprint.push_back(new_pt); + } + + return footprintCost(oriented_footprint); +} + +} // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index 5a952eaf280..ab6a0934137 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -2,3 +2,8 @@ ament_add_gtest(array_parser_test array_parser_test.cpp) target_link_libraries(array_parser_test nav2_costmap_2d_core ) + +ament_add_gtest(collision_footprint_test collision_footprint_test.cpp) +target_link_libraries(collision_footprint_test + nav2_costmap_2d_standalone +) diff --git a/nav2_costmap_2d/test/unit/collision_footprint_test.cpp b/nav2_costmap_2d/test/unit/collision_footprint_test.cpp new file mode 100644 index 00000000000..a29d6c0cc0c --- /dev/null +++ b/nav2_costmap_2d/test/unit/collision_footprint_test.cpp @@ -0,0 +1,94 @@ +#include +#include +#include + +#include "gtest/gtest.h" +#include "nav2_costmap_2d/collision_footprint.hpp" + +TEST(collision_footprint, footprintCostWithPose) +{ + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + geometry_msgs::msg::Point p1; + p1.x = -0.5; + p1.y = 0.0; + geometry_msgs::msg::Point p2; + p2.x = 0.0; + p2.y = 0.5; + geometry_msgs::msg::Point p3; + p3.x = 0.5; + p3.y = 0.0; + geometry_msgs::msg::Point p4; + p4.x = 0.0; + p4.y = -0.5; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + nav2_costmap_2d::CollisionFootprint collision_footprint(costmap_); + + auto value = collision_footprint.footprintCostWithPose(5.0, 5.0, 0.0, footprint); + + EXPECT_NEAR(value, 0.0, 0.001); +} + +TEST(collision_footprint, pointCost) +{ + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + nav2_costmap_2d::CollisionFootprint collision_footprint(costmap_); + + auto value = collision_footprint.pointCost(50, 50); + + EXPECT_NEAR(value, 0.0, 0.001); +} + +TEST(collision_footprint, worldToMap) +{ + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + nav2_costmap_2d::CollisionFootprint collision_footprint(costmap_); + + unsigned int x, y; + + collision_footprint.worldToMap(5.0, 5.0, x, y); + + auto value = collision_footprint.pointCost(x, y); + + EXPECT_NEAR(value, 0.0, 0.001); +} + +TEST(collision_footprint, footprintCostWithPoseObstacle) +{ + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + for (unsigned int i = 45; i <= 65; ++i) { + for (unsigned int j = 45; j <= 65; ++j) { + costmap_->setCost(i, j, 254); + } + } + + geometry_msgs::msg::Point p1; + p1.x = -0.5; + p1.y = 0.0; + geometry_msgs::msg::Point p2; + p2.x = 0.0; + p2.y = 0.5; + geometry_msgs::msg::Point p3; + p3.x = 0.5; + p3.y = 0.0; + geometry_msgs::msg::Point p4; + p4.x = 0.0; + p4.y = -0.5; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + nav2_costmap_2d::CollisionFootprint collision_footprint(costmap_); + + auto value = collision_footprint.footprintCostWithPose(5.0, 5.0, 0.0, footprint); + + EXPECT_NEAR(value, 254.0, 0.001); +} From e8c04c818213ff41b7b567846cf915854c996c08 Mon Sep 17 00:00:00 2001 From: Shivang Patel Date: Sat, 9 May 2020 00:35:04 -0400 Subject: [PATCH 2/9] Address reviewer's comments - Changed the design if the footprint collision checkers - And propogate the changes to dependencies such as nav2_recoveries and nav2_core --- nav2_core/include/nav2_core/recovery.hpp | 4 +- nav2_costmap_2d/CMakeLists.txt | 19 +-- .../nav2_costmap_2d/collision_footprint.hpp | 52 ------- ...pp => costmap_topic_collision_checker.hpp} | 24 ++- ...se.hpp => footprint_collision_checker.hpp} | 25 +-- nav2_costmap_2d/src/collision_base.cpp | 78 ---------- nav2_costmap_2d/src/collision_footprint.cpp | 63 -------- ...pp => costmap_topic_collision_checker.cpp} | 51 ++---- .../src/footprint_collision_checker.cpp | 133 ++++++++++++++++ .../test/integration/CMakeLists.txt | 2 +- ... test_costmap_topic_collision_checker.cpp} | 6 +- nav2_costmap_2d/test/unit/CMakeLists.txt | 4 +- .../test/unit/collision_footprint_test.cpp | 94 ----------- .../unit/footprint_collision_checker_test.cpp | 146 ++++++++++++++++++ .../include/nav2_recoveries/recovery.hpp | 4 +- .../nav2_recoveries/recovery_server.hpp | 2 +- nav2_recoveries/src/recovery_server.cpp | 2 +- nav2_recoveries/test/test_recoveries.cpp | 4 +- 18 files changed, 338 insertions(+), 375 deletions(-) delete mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/collision_footprint.hpp rename nav2_costmap_2d/include/nav2_costmap_2d/{collision_checker.hpp => costmap_topic_collision_checker.hpp} (77%) rename nav2_costmap_2d/include/nav2_costmap_2d/{collision_base.hpp => footprint_collision_checker.hpp} (60%) delete mode 100644 nav2_costmap_2d/src/collision_base.cpp delete mode 100644 nav2_costmap_2d/src/collision_footprint.cpp rename nav2_costmap_2d/src/{collision_checker.cpp => costmap_topic_collision_checker.cpp} (68%) create mode 100644 nav2_costmap_2d/src/footprint_collision_checker.cpp rename nav2_costmap_2d/test/integration/{test_collision_checker.cpp => test_costmap_topic_collision_checker.cpp} (97%) delete mode 100644 nav2_costmap_2d/test/unit/collision_footprint_test.cpp create mode 100644 nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp diff --git a/nav2_core/include/nav2_core/recovery.hpp b/nav2_core/include/nav2_core/recovery.hpp index 1041d74d8e9..5d3fa6de52e 100644 --- a/nav2_core/include/nav2_core/recovery.hpp +++ b/nav2_core/include/nav2_core/recovery.hpp @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_util/lifecycle_node.hpp" #include "tf2_ros/buffer.h" -#include "nav2_costmap_2d/collision_checker.hpp" +#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp" namespace nav2_core { @@ -49,7 +49,7 @@ class Recovery virtual void configure( const rclcpp_lifecycle::LifecycleNode::SharedPtr parent, const std::string & name, std::shared_ptr tf, - std::shared_ptr collision_checker) = 0; + std::shared_ptr collision_checker) = 0; /** * @brief Method to cleanup resources used on shutdown. diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 808b2db2c26..d7508415edf 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -47,6 +47,7 @@ add_library(nav2_costmap_2d_core SHARED src/costmap_layer.cpp src/observation_buffer.cpp src/clear_costmap_service.cpp + src/footprint_collision_checker.cpp ) # prevent pluginlib from using boost @@ -94,8 +95,7 @@ target_link_libraries(layers add_library(nav2_costmap_2d_client SHARED src/footprint_subscriber.cpp src/costmap_subscriber.cpp - src/collision_checker.cpp - src/collision_base.cpp + src/costmap_topic_collision_checker.cpp ) ament_target_dependencies(nav2_costmap_2d_client @@ -106,19 +106,6 @@ target_link_libraries(nav2_costmap_2d_client nav2_costmap_2d_core ) -add_library(nav2_costmap_2d_standalone SHARED - src/collision_footprint.cpp - src/collision_base.cpp -) - -ament_target_dependencies(nav2_costmap_2d_standalone - ${dependencies} -) - -target_link_libraries(nav2_costmap_2d_standalone - nav2_costmap_2d_core -) - add_executable(nav2_costmap_2d_markers src/costmap_2d_markers.cpp) target_link_libraries(nav2_costmap_2d_markers nav2_costmap_2d_core @@ -179,7 +166,7 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(layers nav2_costmap_2d_core nav2_costmap_2d_client nav2_costmap_2d_standalone) +ament_export_libraries(layers nav2_costmap_2d_core nav2_costmap_2d_client) ament_export_dependencies(${dependencies}) pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml) ament_package() diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/collision_footprint.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/collision_footprint.hpp deleted file mode 100644 index 5a1ec4015d3..00000000000 --- a/nav2_costmap_2d/include/nav2_costmap_2d/collision_footprint.hpp +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright (c) 2020 Shivang Patel -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NAV2_COSTMAP_2D__COLLISION_FOOTPRINT_HPP_ -#define NAV2_COSTMAP_2D__COLLISION_FOOTPRINT_HPP_ - -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/pose2_d.hpp" -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_costmap_2d/collision_base.hpp" -#include "nav2_costmap_2d/costmap_subscriber.hpp" -#include "nav2_costmap_2d/footprint_subscriber.hpp" -#include "nav2_util/robot_utils.hpp" -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" -#include "tf2/utils.h" -#pragma GCC diagnostic pop - -namespace nav2_costmap_2d -{ -class CollisionFootprint : public CollisionBase -{ -public: - CollisionFootprint(std::shared_ptr costmap); - double footprintCostWithPose(double x, double y, double theta, const Footprint footprint); - void worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) override; - double pointCost(int x, int y) const override; - -private: - std::shared_ptr costmap_; -}; - -} // namespace nav2_costmap_2d - -#endif // NAV2_COSTMAP_2D__COLLISION_FOOTPRINT_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp similarity index 77% rename from nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp rename to nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp index 93e2d7084e7..7a4c3f43f23 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp @@ -1,5 +1,4 @@ -// Copyright (c) 2019 Intel Corporation -// Copyright (c) 2020 Shivang Patel +// Copyright (c) 2019 Intel Corporationl // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,9 +11,11 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. +// +// Modified by: Shivang Patel (shivaan14@gmail.com) -#ifndef NAV2_COSTMAP_2D__COLLISION_CHECKER_HPP_ -#define NAV2_COSTMAP_2D__COLLISION_CHECKER_HPP_ +#ifndef NAV2_COSTMAP_2D__COSTMAP_TOPIC_COLLISION_CHECKER_HPP_ +#define NAV2_COSTMAP_2D__COSTMAP_TOPIC_COLLISION_CHECKER_HPP_ #include #include @@ -25,7 +26,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose2_d.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_costmap_2d/collision_base.hpp" +#include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_costmap_2d/footprint_subscriber.hpp" #include "nav2_util/robot_utils.hpp" @@ -37,38 +38,35 @@ namespace nav2_costmap_2d { -class CollisionChecker : public CollisionBase +class CostmapTopicCollisionChecker { public: - CollisionChecker( + CostmapTopicCollisionChecker( CostmapSubscriber & costmap_sub, FootprintSubscriber & footprint_sub, tf2_ros::Buffer & tf, std::string name = "collision_checker", std::string global_frame = "map"); - ~CollisionChecker() = default; + ~CostmapTopicCollisionChecker() = default; // Returns the obstacle footprint score for a particular pose double scorePose(const geometry_msgs::msg::Pose2D & pose); bool isCollisionFree(const geometry_msgs::msg::Pose2D & pose); protected: - double pointCost(int x, int y) const override; void unorientFootprint(const Footprint & oriented_footprint, Footprint & reset_footprint); - void worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) override; Footprint getFootprint(const geometry_msgs::msg::Pose2D & pose); - std::shared_ptr costmap_; - // Name used for logging std::string name_; std::string global_frame_; tf2_ros::Buffer & tf_; CostmapSubscriber & costmap_sub_; FootprintSubscriber & footprint_sub_; + FootprintCollisionChecker collision_checker_; }; } // namespace nav2_costmap_2d -#endif // NAV2_COSTMAP_2D__COLLISION_CHECKER_HPP_ +#endif // NAV2_COSTMAP_2D__COSTMAP_TOPIC_COLLISION_CHECKER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/collision_base.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp similarity index 60% rename from nav2_costmap_2d/include/nav2_costmap_2d/collision_base.hpp rename to nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp index b6d8fadc047..ffedb95a576 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/collision_base.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2020 Shivang Patel +// Copyright (c) 2019 Intel Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -11,9 +11,11 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. +// +// Modified by: Shivang Patel (shivaang14@gmail.com) -#ifndef NAV2_COSTMAP_2D__COLLISION_BASE_HPP_ -#define NAV2_COSTMAP_2D__COLLISION_BASE_HPP_ +#ifndef NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_ +#define NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_ #include #include @@ -24,8 +26,6 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose2_d.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_costmap_2d/costmap_subscriber.hpp" -#include "nav2_costmap_2d/footprint_subscriber.hpp" #include "nav2_util/robot_utils.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" @@ -36,15 +36,22 @@ namespace nav2_costmap_2d { typedef std::vector Footprint; -class CollisionBase +class FootprintCollisionChecker { public: + FootprintCollisionChecker(); + explicit FootprintCollisionChecker(std::shared_ptr costmap); double footprintCost(const Footprint footprint); + double footprintCostAtPose(double x, double y, double theta, const Footprint footprint); double lineCost(int x0, int x1, int y0, int y1) const; - virtual void worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) = 0; - virtual double pointCost(int x, int y) const = 0; + bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my); + double pointCost(int x, int y) const; + void setCostmap(std::shared_ptr costmap); + +private: + std::shared_ptr costmap_; }; } // namespace nav2_costmap_2d -#endif // NAV2_COSTMAP_2D__COLLISION_BASE_HPP_ +#endif // NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_ diff --git a/nav2_costmap_2d/src/collision_base.cpp b/nav2_costmap_2d/src/collision_base.cpp deleted file mode 100644 index dd4bc59974d..00000000000 --- a/nav2_costmap_2d/src/collision_base.cpp +++ /dev/null @@ -1,78 +0,0 @@ -// Copyright (c) 2020 Shivang Patel -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include - -#include "nav2_costmap_2d/collision_base.hpp" - -#include "nav2_costmap_2d/cost_values.hpp" -#include "nav2_costmap_2d/exceptions.hpp" -#include "nav2_costmap_2d/footprint.hpp" -#include "nav2_util/line_iterator.hpp" - -using namespace std::chrono_literals; - -namespace nav2_costmap_2d -{ - -double CollisionBase::footprintCost(const Footprint footprint) -{ - // now we really have to lay down the footprint in the costmap_ grid - unsigned int x0, x1, y0, y1; - double footprint_cost = 0.0; - - // we need to rasterize each line in the footprint - for (unsigned int i = 0; i < footprint.size() - 1; ++i) { - // get the cell coord of the first point - worldToMap(footprint[i].x, footprint[i].y, x0, y0); - - // get the cell coord of the second point - worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1); - - footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost); - } - - // we also need to connect the first point in the footprint to the last point - // get the cell coord of the last point - worldToMap(footprint.back().x, footprint.back().y, x0, y0); - - // get the cell coord of the first point - worldToMap(footprint.front().x, footprint.front().y, x1, y1); - - footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost); - - // if all line costs are legal... then we can return that the footprint is legal - return footprint_cost; -} - -double CollisionBase::lineCost(int x0, int x1, int y0, int y1) const -{ - double line_cost = 0.0; - double point_cost = -1.0; - - for (nav2_util::LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) { - point_cost = pointCost(line.getX(), line.getY()); // Score the current point - - if (line_cost < point_cost) { - line_cost = point_cost; - } - } - - return line_cost; -} - -} // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/collision_footprint.cpp b/nav2_costmap_2d/src/collision_footprint.cpp deleted file mode 100644 index 42559eaaa81..00000000000 --- a/nav2_costmap_2d/src/collision_footprint.cpp +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright (c) 2020 Shivang Patel -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include - -#include "nav2_costmap_2d/collision_footprint.hpp" - -#include "nav2_costmap_2d/cost_values.hpp" -#include "nav2_costmap_2d/exceptions.hpp" -#include "nav2_costmap_2d/footprint.hpp" -#include "nav2_util/line_iterator.hpp" - -using namespace std::chrono_literals; - -namespace nav2_costmap_2d -{ - -CollisionFootprint::CollisionFootprint(std::shared_ptr costmap) -: costmap_(costmap) -{} - -double CollisionFootprint::pointCost(int x, int y) const -{ - return costmap_->getCost(x, y); -} - -void CollisionFootprint::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) -{ - costmap_->worldToMap(wx, wy, mx, my); -} - -double CollisionFootprint::footprintCostWithPose( - double x, double y, double theta, - const Footprint footprint) -{ - double cos_th = cos(theta); - double sin_th = sin(theta); - Footprint oriented_footprint; - for (unsigned int i = 0; i < footprint.size(); ++i) { - geometry_msgs::msg::Point new_pt; - new_pt.x = x + (footprint[i].x * cos_th - footprint[i].y * sin_th); - new_pt.y = y + (footprint[i].x * sin_th + footprint[i].y * cos_th); - oriented_footprint.push_back(new_pt); - } - - return footprintCost(oriented_footprint); -} - -} // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/collision_checker.cpp b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp similarity index 68% rename from nav2_costmap_2d/src/collision_checker.cpp rename to nav2_costmap_2d/src/costmap_topic_collision_checker.cpp index 5931440ee3e..610528cb4bf 100644 --- a/nav2_costmap_2d/src/collision_checker.cpp +++ b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp @@ -1,5 +1,4 @@ // Copyright (c) 2019 Intel Corporation -// Copyright (c) 2020 Shivang Patel // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,13 +11,16 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. +// +// Modified by: Shivang Patel (shivaan14@gmail.com) #include #include #include #include +#include -#include "nav2_costmap_2d/collision_checker.hpp" +#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp" #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_costmap_2d/exceptions.hpp" @@ -30,22 +32,22 @@ using namespace std::chrono_literals; namespace nav2_costmap_2d { -CollisionChecker::CollisionChecker( +CostmapTopicCollisionChecker::CostmapTopicCollisionChecker( CostmapSubscriber & costmap_sub, FootprintSubscriber & footprint_sub, tf2_ros::Buffer & tf, std::string name, std::string global_frame) -: CollisionBase(), - name_(name), +: name_(name), global_frame_(global_frame), tf_(tf), costmap_sub_(costmap_sub), - footprint_sub_(footprint_sub) + footprint_sub_(footprint_sub), + collision_checker_(nullptr) { } -bool CollisionChecker::isCollisionFree( +bool CostmapTopicCollisionChecker::isCollisionFree( const geometry_msgs::msg::Pose2D & pose) { try { @@ -65,33 +67,25 @@ bool CollisionChecker::isCollisionFree( } } -double CollisionChecker::scorePose( +double CostmapTopicCollisionChecker::scorePose( const geometry_msgs::msg::Pose2D & pose) { try { - costmap_ = costmap_sub_.getCostmap(); + collision_checker_.setCostmap(costmap_sub_.getCostmap()); } catch (const std::runtime_error & e) { throw CollisionCheckerException(e.what()); } unsigned int cell_x, cell_y; - if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) { + if (!collision_checker_.worldToMap(pose.x, pose.y, cell_x, cell_y)) { RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", cell_x, cell_y); throw IllegalPoseException(name_, "Pose Goes Off Grid."); } - return footprintCost(getFootprint(pose)); -} - -void CollisionChecker::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) -{ - if (!costmap_->worldToMap(wx, wy, mx, my)) { - RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", mx, my); - throw IllegalPoseException(name_, "Footprint Goes Off Grid."); - } + return collision_checker_.footprintCost(getFootprint(pose)); } -Footprint CollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose) +Footprint CostmapTopicCollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose) { Footprint footprint; if (!footprint_sub_.getFootprint(footprint)) { @@ -105,22 +99,7 @@ Footprint CollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose return footprint; } -double CollisionChecker::pointCost(int x, int y) const -{ - unsigned char cost = costmap_->getCost(x, y); - // if the cell is in an obstacle the path is invalid or unknown - if (cost == LETHAL_OBSTACLE) { - RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", x, y); - throw IllegalPoseException(name_, "Footprint Hits Obstacle."); - } else if (cost == NO_INFORMATION) { - RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", x, y); - throw IllegalPoseException(name_, "Footprint Hits Unknown Region."); - } - - return cost; -} - -void CollisionChecker::unorientFootprint( +void CostmapTopicCollisionChecker::unorientFootprint( const std::vector & oriented_footprint, std::vector & reset_footprint) { diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp new file mode 100644 index 00000000000..720ed108a4d --- /dev/null +++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp @@ -0,0 +1,133 @@ +// Copyright (c) 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Modified by: Shivang Patel (shivaang14@gmail.com) + +#include +#include +#include +#include + +#include "nav2_costmap_2d/footprint_collision_checker.hpp" + +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_costmap_2d/exceptions.hpp" +#include "nav2_costmap_2d/footprint.hpp" +#include "nav2_util/line_iterator.hpp" + +using namespace std::chrono_literals; + +namespace nav2_costmap_2d +{ + +FootprintCollisionChecker::FootprintCollisionChecker() +: costmap_(nullptr) +{ +} + +FootprintCollisionChecker::FootprintCollisionChecker( + std::shared_ptr costmap) +: costmap_(costmap) +{ +} + +double FootprintCollisionChecker::footprintCost(const Footprint footprint) +{ + // now we really have to lay down the footprint in the costmap_ grid + unsigned int x0, x1, y0, y1; + double footprint_cost = 0.0; + + // we need to rasterize each line in the footprint + for (unsigned int i = 0; i < footprint.size() - 1; ++i) { + // get the cell coord of the first point + if (!worldToMap(footprint[i].x, footprint[i].y, x0, y0)) { + return static_cast(LETHAL_OBSTACLE); + } + + // get the cell coord of the second point + if (!worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1)) { + return static_cast(LETHAL_OBSTACLE); + } + + footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost); + } + + // we also need to connect the first point in the footprint to the last point + // get the cell coord of the last point + if (!worldToMap(footprint.back().x, footprint.back().y, x0, y0)) { + return static_cast(LETHAL_OBSTACLE); + } + + // get the cell coord of the first point + if (!worldToMap(footprint.front().x, footprint.front().y, x1, y1)) { + return static_cast(LETHAL_OBSTACLE); + } + + footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost); + + // if all line costs are legal... then we can return that the footprint is legal + return footprint_cost; +} + +double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int y1) const +{ + double line_cost = 0.0; + double point_cost = -1.0; + + for (nav2_util::LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) { + point_cost = pointCost(line.getX(), line.getY()); // Score the current point + + if (line_cost < point_cost) { + line_cost = point_cost; + } + } + + return line_cost; +} + +bool FootprintCollisionChecker::worldToMap( + double wx, double wy, unsigned int & mx, unsigned int & my) +{ + return costmap_->worldToMap(wx, wy, mx, my); +} + +double FootprintCollisionChecker::pointCost(int x, int y) const +{ + auto value = costmap_->getCost(x, y); + RCLCPP_INFO(rclcpp::get_logger("TEST"), "CALLED: [%d, %d] %f", x, y, value); + return value; +} + +void FootprintCollisionChecker::setCostmap(std::shared_ptr costmap) +{ + costmap_ = costmap; +} + +double FootprintCollisionChecker::footprintCostAtPose( + double x, double y, double theta, const Footprint footprint) +{ + double cos_th = cos(theta); + double sin_th = sin(theta); + Footprint oriented_footprint; + for (unsigned int i = 0; i < footprint.size(); ++i) { + geometry_msgs::msg::Point new_pt; + new_pt.x = x + (footprint[i].x * cos_th - footprint[i].y * sin_th); + new_pt.y = y + (footprint[i].x * sin_th + footprint[i].y * cos_th); + oriented_footprint.push_back(new_pt); + } + + return footprintCost(oriented_footprint); +} + +} // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 57271a9b515..4170ad3875b 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -10,7 +10,7 @@ target_link_libraries(footprint_tests_exec ) ament_add_gtest_executable(test_collision_checker_exec - test_collision_checker.cpp + test_costmap_topic_collision_checker.cpp ) ament_target_dependencies(test_collision_checker_exec ${dependencies} diff --git a/nav2_costmap_2d/test/integration/test_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp similarity index 97% rename from nav2_costmap_2d/test/integration/test_collision_checker.cpp rename to nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index ae73b914f64..56baa03a4b4 100644 --- a/nav2_costmap_2d/test/integration/test_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -18,7 +18,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/collision_checker.hpp" +#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav2_costmap_2d/static_layer.hpp" @@ -123,7 +123,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode shared_from_this(), footprint_topic); - collision_checker_ = std::make_unique( + collision_checker_ = std::make_unique( *costmap_sub_, *footprint_sub_, *tf_buffer_, get_name(), "map"); layers_ = new nav2_costmap_2d::LayeredCostmap("map", false, false); @@ -274,7 +274,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode std::shared_ptr costmap_sub_; std::shared_ptr footprint_sub_; - std::unique_ptr collision_checker_; + std::unique_ptr collision_checker_; nav2_costmap_2d::LayeredCostmap * layers_{nullptr}; std::string global_frame_; diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index ab6a0934137..78085141b38 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -3,7 +3,7 @@ target_link_libraries(array_parser_test nav2_costmap_2d_core ) -ament_add_gtest(collision_footprint_test collision_footprint_test.cpp) +ament_add_gtest(collision_footprint_test footprint_collision_checker_test.cpp) target_link_libraries(collision_footprint_test - nav2_costmap_2d_standalone + nav2_costmap_2d_core ) diff --git a/nav2_costmap_2d/test/unit/collision_footprint_test.cpp b/nav2_costmap_2d/test/unit/collision_footprint_test.cpp deleted file mode 100644 index a29d6c0cc0c..00000000000 --- a/nav2_costmap_2d/test/unit/collision_footprint_test.cpp +++ /dev/null @@ -1,94 +0,0 @@ -#include -#include -#include - -#include "gtest/gtest.h" -#include "nav2_costmap_2d/collision_footprint.hpp" - -TEST(collision_footprint, footprintCostWithPose) -{ - std::shared_ptr costmap_ = - std::make_shared(100, 100, 0.1, 0, 0, 0); - - geometry_msgs::msg::Point p1; - p1.x = -0.5; - p1.y = 0.0; - geometry_msgs::msg::Point p2; - p2.x = 0.0; - p2.y = 0.5; - geometry_msgs::msg::Point p3; - p3.x = 0.5; - p3.y = 0.0; - geometry_msgs::msg::Point p4; - p4.x = 0.0; - p4.y = -0.5; - - nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - - nav2_costmap_2d::CollisionFootprint collision_footprint(costmap_); - - auto value = collision_footprint.footprintCostWithPose(5.0, 5.0, 0.0, footprint); - - EXPECT_NEAR(value, 0.0, 0.001); -} - -TEST(collision_footprint, pointCost) -{ - std::shared_ptr costmap_ = - std::make_shared(100, 100, 0.1, 0, 0, 0); - - nav2_costmap_2d::CollisionFootprint collision_footprint(costmap_); - - auto value = collision_footprint.pointCost(50, 50); - - EXPECT_NEAR(value, 0.0, 0.001); -} - -TEST(collision_footprint, worldToMap) -{ - std::shared_ptr costmap_ = - std::make_shared(100, 100, 0.1, 0, 0, 0); - - nav2_costmap_2d::CollisionFootprint collision_footprint(costmap_); - - unsigned int x, y; - - collision_footprint.worldToMap(5.0, 5.0, x, y); - - auto value = collision_footprint.pointCost(x, y); - - EXPECT_NEAR(value, 0.0, 0.001); -} - -TEST(collision_footprint, footprintCostWithPoseObstacle) -{ - std::shared_ptr costmap_ = - std::make_shared(100, 100, 0.1, 0, 0, 0); - - for (unsigned int i = 45; i <= 65; ++i) { - for (unsigned int j = 45; j <= 65; ++j) { - costmap_->setCost(i, j, 254); - } - } - - geometry_msgs::msg::Point p1; - p1.x = -0.5; - p1.y = 0.0; - geometry_msgs::msg::Point p2; - p2.x = 0.0; - p2.y = 0.5; - geometry_msgs::msg::Point p3; - p3.x = 0.5; - p3.y = 0.0; - geometry_msgs::msg::Point p4; - p4.x = 0.0; - p4.y = -0.5; - - nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - - nav2_costmap_2d::CollisionFootprint collision_footprint(costmap_); - - auto value = collision_footprint.footprintCostWithPose(5.0, 5.0, 0.0, footprint); - - EXPECT_NEAR(value, 254.0, 0.001); -} diff --git a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp new file mode 100644 index 00000000000..47f81ef8596 --- /dev/null +++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp @@ -0,0 +1,146 @@ +// Copyright (c) 2020 Shivang Patel +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "gtest/gtest.h" +#include "nav2_costmap_2d/footprint_collision_checker.hpp" + +TEST(collision_footprint, footprintCostWithPose) +{ + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + geometry_msgs::msg::Point p1; + p1.x = -0.5; + p1.y = 0.0; + geometry_msgs::msg::Point p2; + p2.x = 0.0; + p2.y = 0.5; + geometry_msgs::msg::Point p3; + p3.x = 0.5; + p3.y = 0.0; + geometry_msgs::msg::Point p4; + p4.x = 0.0; + p4.y = -0.5; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + + auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); + + EXPECT_NEAR(value, 0.0, 0.001); +} + +TEST(collision_footprint, pointCost) +{ + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + + auto value = collision_checker.pointCost(50, 50); + + EXPECT_NEAR(value, 0.0, 0.001); +} + +TEST(collision_footprint, worldToMap) +{ + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + + unsigned int x, y; + + collision_checker.worldToMap(5.0, 5.0, x, y); + + auto value = collision_checker.pointCost(x, y); + + EXPECT_NEAR(value, 0.0, 0.001); +} + +TEST(collision_footprint, footprintCostWithPoseObstacle) +{ + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + for (unsigned int i = 45; i <= 65; ++i) { + for (unsigned int j = 45; j <= 65; ++j) { + costmap_->setCost(i, j, 254); + } + } + + geometry_msgs::msg::Point p1; + p1.x = -0.5; + p1.y = 0.0; + geometry_msgs::msg::Point p2; + p2.x = 0.0; + p2.y = 0.5; + geometry_msgs::msg::Point p3; + p3.x = 0.5; + p3.y = 0.0; + geometry_msgs::msg::Point p4; + p4.x = 0.0; + p4.y = -0.5; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + + auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); + + EXPECT_NEAR(value, 254.0, 0.001); +} + +TEST(collision_footprint, footprintCostWithPoseWithMovement) +{ + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 254); + + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmap_->setCost(i, j, 0); + } + } + + geometry_msgs::msg::Point p1; + p1.x = -1.0; + p1.y = 1.0; + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = 1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; + p3.y = -1.0; + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = -1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + + auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); + EXPECT_NEAR(value, 0.0, 0.001); + + auto up_value = collision_checker.footprintCostAtPose(5.0, 4.9, 0.0, footprint); + EXPECT_NEAR(up_value, 254.0, 0.001); + + auto down_value = collision_checker.footprintCostAtPose(5.0, 5.2, 0.0, footprint); + EXPECT_NEAR(down_value, 254.0, 0.001); +} diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp index 80606e90ff1..002ebb28bfd 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery.hpp @@ -88,7 +88,7 @@ class Recovery : public nav2_core::Recovery void configure( const rclcpp_lifecycle::LifecycleNode::SharedPtr parent, const std::string & name, std::shared_ptr tf, - std::shared_ptr collision_checker) override + std::shared_ptr collision_checker) override { RCLCPP_INFO(parent->get_logger(), "Configuring %s", name.c_str()); @@ -135,7 +135,7 @@ class Recovery : public nav2_core::Recovery std::string recovery_name_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr vel_pub_; std::shared_ptr action_server_; - std::shared_ptr collision_checker_; + std::shared_ptr collision_checker_; std::shared_ptr tf_; double cycle_frequency_; diff --git a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp index 026c3ecab4f..0eff75fb77f 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp @@ -59,7 +59,7 @@ class RecoveryServer : public nav2_util::LifecycleNode // Utilities std::unique_ptr costmap_sub_; std::unique_ptr footprint_sub_; - std::shared_ptr collision_checker_; + std::shared_ptr collision_checker_; }; } // namespace recovery_server diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp index ecd23d4481f..58b2b240dec 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/nav2_recoveries/src/recovery_server.cpp @@ -71,7 +71,7 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/) shared_from_this(), costmap_topic); footprint_sub_ = std::make_unique( shared_from_this(), footprint_topic, 1.0); - collision_checker_ = std::make_shared( + collision_checker_ = std::make_shared( *costmap_sub_, *footprint_sub_, *tf_, this->get_name(), "odom"); this->get_parameter("plugin_names", plugin_names_); diff --git a/nav2_recoveries/test/test_recoveries.cpp b/nav2_recoveries/test/test_recoveries.cpp index f1600094a9b..137e3500e41 100644 --- a/nav2_recoveries/test/test_recoveries.cpp +++ b/nav2_recoveries/test/test_recoveries.cpp @@ -126,8 +126,8 @@ class RecoveryTest : public ::testing::Test std::shared_ptr footprint_sub_ = std::make_shared( node_lifecycle_, footprint_topic, 1.0); - std::shared_ptr collision_checker_ = - std::make_shared( + std::shared_ptr collision_checker_ = + std::make_shared( *costmap_sub_, *footprint_sub_, *tf_buffer_, node_lifecycle_->get_name(), "odom"); From 0e2ff34c818e2c15ca2971f0d411d5d661f086c2 Mon Sep 17 00:00:00 2001 From: Shivang Patel Date: Sat, 9 May 2020 00:40:52 -0400 Subject: [PATCH 3/9] Remove some extra headers --- .../include/nav2_costmap_2d/footprint_collision_checker.hpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp index ffedb95a576..24f35aa1359 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp @@ -27,10 +27,6 @@ #include "geometry_msgs/msg/pose2_d.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_util/robot_utils.hpp" -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" -#include "tf2/utils.h" -#pragma GCC diagnostic pop namespace nav2_costmap_2d { From f5bdeb2011f4333d01553c0d377f8df88ca340ba Mon Sep 17 00:00:00 2001 From: Shivang Patel Date: Sat, 9 May 2020 10:04:03 -0400 Subject: [PATCH 4/9] Remove debuging code --- nav2_costmap_2d/src/footprint_collision_checker.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp index 720ed108a4d..15adcd8665b 100644 --- a/nav2_costmap_2d/src/footprint_collision_checker.cpp +++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp @@ -104,9 +104,7 @@ bool FootprintCollisionChecker::worldToMap( double FootprintCollisionChecker::pointCost(int x, int y) const { - auto value = costmap_->getCost(x, y); - RCLCPP_INFO(rclcpp::get_logger("TEST"), "CALLED: [%d, %d] %f", x, y, value); - return value; + return costmap_->getCost(x, y); } void FootprintCollisionChecker::setCostmap(std::shared_ptr costmap) From 9274ab18b62ebf61b9148f11d729f96a4cdea4bb Mon Sep 17 00:00:00 2001 From: Shivang Patel Date: Tue, 12 May 2020 22:45:15 -0400 Subject: [PATCH 5/9] Add requested test --- .../unit/footprint_collision_checker_test.cpp | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp index 47f81ef8596..3edb130e691 100644 --- a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp +++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp @@ -144,3 +144,38 @@ TEST(collision_footprint, footprintCostWithPoseWithMovement) auto down_value = collision_checker.footprintCostAtPose(5.0, 5.2, 0.0, footprint); EXPECT_NEAR(down_value, 254.0, 0.001); } + +TEST(collision_footprint, footprintCostWithPoseCheckingPointAndLine) +{ + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.10000, 0, 0.0, 0.0); + + costmap_->setCost(62, 50, 254); + costmap_->setCost(39, 60, 254); + + geometry_msgs::msg::Point p1; + p1.x = -1.0; + p1.y = 1.0; + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = 1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; + p3.y = -1.0; + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = -1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + + auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); + EXPECT_NEAR(value, 0.0, 0.001); + + auto left_value = collision_checker.footprintCostAtPose(4.9, 5.0, 0.0, footprint); + EXPECT_NEAR(left_value, 254.0, 0.001); + + auto right_value = collision_checker.footprintCostAtPose(double(5.20000), 5.0, 0.0, footprint); + EXPECT_NEAR(right_value, 254.0, 0.001); +} From 6067d3a9614bd50f664dec9605915c39b9387595 Mon Sep 17 00:00:00 2001 From: Shivang Patel Date: Tue, 12 May 2020 22:50:47 -0400 Subject: [PATCH 6/9] Change weird test names. --- .../unit/footprint_collision_checker_test.cpp | 43 +++---------------- 1 file changed, 5 insertions(+), 38 deletions(-) diff --git a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp index 3edb130e691..b5474e6c44a 100644 --- a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp +++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp @@ -19,7 +19,7 @@ #include "gtest/gtest.h" #include "nav2_costmap_2d/footprint_collision_checker.hpp" -TEST(collision_footprint, footprintCostWithPose) +TEST(collision_footprint, test_basic) { std::shared_ptr costmap_ = std::make_shared(100, 100, 0.1, 0, 0, 0); @@ -46,7 +46,7 @@ TEST(collision_footprint, footprintCostWithPose) EXPECT_NEAR(value, 0.0, 0.001); } -TEST(collision_footprint, pointCost) +TEST(collision_footprint, test_point_cost) { std::shared_ptr costmap_ = std::make_shared(100, 100, 0.1, 0, 0, 0); @@ -58,7 +58,7 @@ TEST(collision_footprint, pointCost) EXPECT_NEAR(value, 0.0, 0.001); } -TEST(collision_footprint, worldToMap) +TEST(collision_footprint, test_world_to_map) { std::shared_ptr costmap_ = std::make_shared(100, 100, 0.1, 0, 0, 0); @@ -74,40 +74,7 @@ TEST(collision_footprint, worldToMap) EXPECT_NEAR(value, 0.0, 0.001); } -TEST(collision_footprint, footprintCostWithPoseObstacle) -{ - std::shared_ptr costmap_ = - std::make_shared(100, 100, 0.1, 0, 0, 0); - - for (unsigned int i = 45; i <= 65; ++i) { - for (unsigned int j = 45; j <= 65; ++j) { - costmap_->setCost(i, j, 254); - } - } - - geometry_msgs::msg::Point p1; - p1.x = -0.5; - p1.y = 0.0; - geometry_msgs::msg::Point p2; - p2.x = 0.0; - p2.y = 0.5; - geometry_msgs::msg::Point p3; - p3.x = 0.5; - p3.y = 0.0; - geometry_msgs::msg::Point p4; - p4.x = 0.0; - p4.y = -0.5; - - nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - - nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); - - auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); - - EXPECT_NEAR(value, 254.0, 0.001); -} - -TEST(collision_footprint, footprintCostWithPoseWithMovement) +TEST(collision_footprint, test_footprint_at_pose_with_movement) { std::shared_ptr costmap_ = std::make_shared(100, 100, 0.1, 0, 0, 254); @@ -145,7 +112,7 @@ TEST(collision_footprint, footprintCostWithPoseWithMovement) EXPECT_NEAR(down_value, 254.0, 0.001); } -TEST(collision_footprint, footprintCostWithPoseCheckingPointAndLine) +TEST(collision_footprint, test_point_and_line_cost) { std::shared_ptr costmap_ = std::make_shared(100, 100, 0.10000, 0, 0.0, 0.0); From 826761eff00cee389c807e1602c358943f2d4687 Mon Sep 17 00:00:00 2001 From: Shivang Patel Date: Tue, 12 May 2020 23:02:59 -0400 Subject: [PATCH 7/9] Remove unorientFootprint function dependency --- nav2_costmap_2d/src/costmap_topic_collision_checker.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp index 610528cb4bf..5c1ab50234b 100644 --- a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp @@ -85,16 +85,16 @@ double CostmapTopicCollisionChecker::scorePose( return collision_checker_.footprintCost(getFootprint(pose)); } -Footprint CostmapTopicCollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose) +Footprint CostmapTopicCollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & /*pose*/) { Footprint footprint; if (!footprint_sub_.getFootprint(footprint)) { throw CollisionCheckerException("Current footprint not available."); } - Footprint footprint_spec; - unorientFootprint(footprint, footprint_spec); - transformFootprint(pose.x, pose.y, pose.theta, footprint_spec, footprint); + // Footprint footprint_spec; + // unorientFootprint(footprint, footprint_spec); + // transformFootprint(pose.x, pose.y, pose.theta, footprint_spec, footprint); return footprint; } From 82e793164e80cb0dab0f9552b96ff0cc3b02a9f1 Mon Sep 17 00:00:00 2001 From: Shivang Patel Date: Thu, 14 May 2020 10:09:10 -0400 Subject: [PATCH 8/9] Imporve tests --- .../nav2_costmap_2d/costmap_topic_collision_checker.hpp | 2 +- nav2_costmap_2d/src/costmap_topic_collision_checker.cpp | 6 +++--- .../test/unit/footprint_collision_checker_test.cpp | 9 +++++++-- 3 files changed, 11 insertions(+), 6 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp index 7a4c3f43f23..4a511092a66 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Intel Corporationl +// Copyright (c) 2019 Intel Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp index 5c1ab50234b..7473cf1ed79 100644 --- a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp @@ -92,9 +92,9 @@ Footprint CostmapTopicCollisionChecker::getFootprint(const geometry_msgs::msg::P throw CollisionCheckerException("Current footprint not available."); } - // Footprint footprint_spec; - // unorientFootprint(footprint, footprint_spec); - // transformFootprint(pose.x, pose.y, pose.theta, footprint_spec, footprint); + Footprint footprint_spec; + unorientFootprint(footprint, footprint_spec); + transformFootprint(pose.x, pose.y, pose.theta, footprint_spec, footprint); return footprint; } diff --git a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp index b5474e6c44a..f0e9c306233 100644 --- a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp +++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp @@ -67,11 +67,16 @@ TEST(collision_footprint, test_world_to_map) unsigned int x, y; - collision_checker.worldToMap(5.0, 5.0, x, y); + collision_checker.worldToMap(1.0, 1.0, x, y); auto value = collision_checker.pointCost(x, y); EXPECT_NEAR(value, 0.0, 0.001); + + costmap_->setCost(50, 50, 200); + collision_checker.worldToMap(5.0, 5.0, x, y); + + EXPECT_NEAR(collision_checker.pointCost(x, y), 200.0, 0.001); } TEST(collision_footprint, test_footprint_at_pose_with_movement) @@ -143,6 +148,6 @@ TEST(collision_footprint, test_point_and_line_cost) auto left_value = collision_checker.footprintCostAtPose(4.9, 5.0, 0.0, footprint); EXPECT_NEAR(left_value, 254.0, 0.001); - auto right_value = collision_checker.footprintCostAtPose(double(5.20000), 5.0, 0.0, footprint); + auto right_value = collision_checker.footprintCostAtPose(5.2, 5.0, 0.0, footprint); EXPECT_NEAR(right_value, 254.0, 0.001); } From 87dfc8d062a68b57e3aec7d84867e572e176f672 Mon Sep 17 00:00:00 2001 From: Shivang Patel Date: Thu, 14 May 2020 10:21:37 -0400 Subject: [PATCH 9/9] Fix commented Varible --- nav2_costmap_2d/src/costmap_topic_collision_checker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp index 7473cf1ed79..610528cb4bf 100644 --- a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp @@ -85,7 +85,7 @@ double CostmapTopicCollisionChecker::scorePose( return collision_checker_.footprintCost(getFootprint(pose)); } -Footprint CostmapTopicCollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & /*pose*/) +Footprint CostmapTopicCollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose) { Footprint footprint; if (!footprint_sub_.getFootprint(footprint)) {