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 1a4b37dedd1481f3be7446bb97470686f479e414..39b236fa47f72e371fea212258447b578c22a76f 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp @@ -46,7 +46,7 @@ public: std::shared_ptr<FootprintSubscriber> footprint_sub, tf2_ros::Buffer & tf_buffer, std::string name = "collision_checker"); - + ~CollisionChecker(); double scorePose( @@ -70,7 +70,7 @@ protected: rclcpp::Node::SharedPtr node_; std::string name_; std::shared_ptr<CostmapSubscriber> costmap_sub_; - std::shared_ptr<FootprintSubscriber> footprint_sub_; + std::shared_ptr<FootprintSubscriber> footprint_sub_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp index cd14eb099433f5bbbf4ac4113da8d6794ed588d9..592561bfe1328ab8f994d2d5c99bc3a0e11ee7fb 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp @@ -133,7 +133,7 @@ private: rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::Costmap>::SharedPtr costmap_raw_pub_; nav_msgs::msg::OccupancyGrid grid_; - nav2_msgs::msg::Costmap costmap_raw_; + nav2_msgs::msg::Costmap costmap_raw_; // Translate from 0-255 values in costmap to -1 to 100 values in message. static char * cost_translation_table_; }; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp index a9686fb4f27b1007f8288dd1e64adbee60716f68..77203c5934fef9a03c1dca2cc2658aaeb5c213c1 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -32,7 +32,7 @@ public: std::string & topic_name); ~CostmapSubscriber() {} -Costmap2D * getCostmap(); + Costmap2D * getCostmap(); protected: void toCostmap2D(); diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp index 25dbdf24a06a6d6bd7230add115d7f983d428601..e4f6e2bc97dd32a45123d4c2792f3792902fcdad 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp @@ -31,7 +31,7 @@ public: std::string & topic_name); ~FootprintSubscriber() {} -bool getFootprint(std::vector<geometry_msgs::msg::Point> & footprint); + bool getFootprint(std::vector<geometry_msgs::msg::Point> & footprint); protected: void footprint_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg); diff --git a/nav2_costmap_2d/src/collision_checker.cpp b/nav2_costmap_2d/src/collision_checker.cpp index 0cef719932741cb8362bb531990920a006b6fb96..2566d3e45a790ca505b711d9ddd305a5fd1c639f 100644 --- a/nav2_costmap_2d/src/collision_checker.cpp +++ b/nav2_costmap_2d/src/collision_checker.cpp @@ -40,7 +40,7 @@ CollisionChecker::CollisionChecker( CollisionChecker::~CollisionChecker() {} bool CollisionChecker::isCollisionFree( -const geometry_msgs::msg::Pose2D & pose) + const geometry_msgs::msg::Pose2D & pose) { try { if (scorePose(pose) < 0) { @@ -57,7 +57,7 @@ const geometry_msgs::msg::Pose2D & pose) } double CollisionChecker::scorePose( -const geometry_msgs::msg::Pose2D & pose) + const geometry_msgs::msg::Pose2D & pose) { Costmap2D * costmap_; try { @@ -107,13 +107,13 @@ const geometry_msgs::msg::Pose2D & pose) // we also need to connect the first point in the footprint to the last point // get the cell coord of the last point if (!costmap_->worldToMap(footprint.back().x, footprint.back().y, x0, y0)) { - RCLCPP_DEBUG(node_->get_logger(), "Map Cell: [%d, %d]", x0, y0); + RCLCPP_DEBUG(node_->get_logger(), "Map Cell: [%d, %d]", x0, y0); throw IllegalPoseException(name_, "Footprint Goes Off Grid."); } // get the cell coord of the first point if (!costmap_->worldToMap(footprint.front().x, footprint.front().y, x1, y1)) { - RCLCPP_DEBUG(node_->get_logger(), "Map Cell: [%d, %d]", x1, y1); + RCLCPP_DEBUG(node_->get_logger(), "Map Cell: [%d, %d]", x1, y1); throw IllegalPoseException(name_, "Footprint Goes Off Grid."); } @@ -193,7 +193,7 @@ void CollisionChecker::unorientFootprint( std::vector<geometry_msgs::msg::Point> & reset_footprint) { geometry_msgs::msg::PoseStamped current_pose; - if (!getRobotPose(current_pose)) { + if (!getRobotPose(current_pose)) { throw CollisionCheckerException("Robot pose unavailable."); } @@ -207,4 +207,3 @@ void CollisionChecker::unorientFootprint( } } // namespace nav2_costmap_2d - diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index c4bed17c2ff59120cc09ea2c3f4f2aee0e1f6620..f03bf8434db1254ca122088dc42932b85ea16ba1 100644 --- a/nav2_costmap_2d/src/costmap_subscriber.cpp +++ b/nav2_costmap_2d/src/costmap_subscriber.cpp @@ -43,16 +43,16 @@ void CostmapSubscriber::toCostmap2D() { if (!costmap_received_) { costmap_ = new Costmap2D( - msg_->metadata.size_x, msg_->metadata.size_y, - msg_->metadata.resolution, msg_->metadata.origin.position.x, - msg_->metadata.origin.position.y); + msg_->metadata.size_x, msg_->metadata.size_y, + msg_->metadata.resolution, msg_->metadata.origin.position.x, + msg_->metadata.origin.position.y); } else if (costmap_->getSizeInCellsX() != msg_->metadata.size_x || costmap_->getSizeInCellsY() != msg_->metadata.size_y || costmap_->getResolution() != msg_->metadata.resolution || costmap_->getOriginX() != msg_->metadata.origin.position.x || costmap_->getOriginY() != msg_->metadata.origin.position.y) { - // Update the size of the costmap + // Update the size of the costmap costmap_->resizeMap(msg_->metadata.size_x, msg_->metadata.size_y, msg_->metadata.resolution, msg_->metadata.origin.position.x, @@ -61,7 +61,7 @@ void CostmapSubscriber::toCostmap2D() unsigned char * master_array = costmap_->getCharMap(); unsigned int index = 0; - for (unsigned int i = 0; i < msg_->metadata.size_x; ++i) { + for (unsigned int i = 0; i < msg_->metadata.size_x; ++i) { for (unsigned int j = 0; j < msg_->metadata.size_y; ++j) { master_array[index] = msg_->data[index]; ++index; @@ -79,4 +79,3 @@ void CostmapSubscriber::costmap_callback(const nav2_msgs::msg::Costmap::SharedPt } } // namespace nav2_costmap_2d - diff --git a/nav2_costmap_2d/src/footprint_subscriber.cpp b/nav2_costmap_2d/src/footprint_subscriber.cpp index aca500b881220979a33b6aac1e0c8962ce791148..3f75cec12b09640b88899a2efa8f56de8fa6ccc3 100644 --- a/nav2_costmap_2d/src/footprint_subscriber.cpp +++ b/nav2_costmap_2d/src/footprint_subscriber.cpp @@ -50,4 +50,3 @@ FootprintSubscriber::footprint_callback(const geometry_msgs::msg::PolygonStamped } } // namespace nav2_costmap_2d - diff --git a/nav2_costmap_2d/test/integration/test_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_collision_checker.cpp index 79eaa2db27dcdc4a8e4cd1f77b4f2864bfd03f9b..4689e691ee822692868b8f70806f406350a28a28 100644 --- a/nav2_costmap_2d/test/integration/test_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_collision_checker.cpp @@ -72,24 +72,23 @@ public: costmap_pub_ = std::make_unique<nav2_costmap_2d::Costmap2DPublisher>(node_, layers_.getCostmap(), global_frame_, "costmap", true); - + publish(); } ~TestCollisionChecker() {} - bool testPose(double x,double y, double theta) + bool testPose(double x, double y, double theta) { geometry_msgs::msg::Pose2D pose; pose.x = x; pose.y = y; pose.theta = theta; - setPose(x,y,theta); + setPose(x, y, theta); costmap_received_ = false; - while(!costmap_received_) - { + while (!costmap_received_) { rclcpp::spin_some(node_); } @@ -129,19 +128,19 @@ protected: void publish() { - auto timer_callback = [this]() -> void - { - try { - costmap_sub_->getCostmap(); - costmap_received_ = true; - } catch (const std::runtime_error & e) { - costmap_received_ = false; - } + auto timer_callback = [this]() -> void + { + try { + costmap_sub_->getCostmap(); + costmap_received_ = true; + } catch (const std::runtime_error & e) { + costmap_received_ = false; + } publishFootprint(); publishCostmap(); - }; + }; - timer_ = node_->create_wall_timer(0.1s, timer_callback); + timer_ = node_->create_wall_timer(0.1s, timer_callback); } void publishFootprint() @@ -180,7 +179,7 @@ public: node_ = rclcpp::Node::make_shared( "test_collision_checker", nav2_util::get_node_options_default()); - + tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock()); tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_); @@ -189,7 +188,8 @@ public: costmap_sub_ = std::make_shared<nav2_costmap_2d::CostmapSubscriber>(node_, costmap_topic); footprint_sub_ = std::make_shared<nav2_costmap_2d::FootprintSubscriber>(node_, footprint_topic); - collision_checker_ = std::make_unique<TestCollisionChecker>(node_, costmap_sub_, footprint_sub_, *tf_buffer_); + collision_checker_ = std::make_unique<TestCollisionChecker>(node_, costmap_sub_, footprint_sub_, + *tf_buffer_); } ~TestNode() {} diff --git a/nav2_dwb_controller/dwb_critics/CMakeLists.txt b/nav2_dwb_controller/dwb_critics/CMakeLists.txt index 353b9941c953cd134fcd3038f12dd24d89cd2481..c7187363581ba73970b4aa1e927de45a48286df7 100644 --- a/nav2_dwb_controller/dwb_critics/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_critics/CMakeLists.txt @@ -71,14 +71,10 @@ if(BUILD_TESTING) # the following line skips the linter which checks for copyrights set(ament_cmake_copyright_FOUND TRUE) ament_lint_auto_find_test_dependencies() - find_package(ament_cmake_pytest REQUIRED) - find_package(ament_cmake_gtest REQUIRED) endif() ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) - - ament_export_dependencies( ${dependencies} )