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 1a4b37dedd..39b236fa47 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 @@ class CollisionChecker std::shared_ptr footprint_sub, tf2_ros::Buffer & tf_buffer, std::string name = "collision_checker"); - + ~CollisionChecker(); double scorePose( @@ -70,7 +70,7 @@ class CollisionChecker rclcpp::Node::SharedPtr node_; std::string name_; std::shared_ptr costmap_sub_; - std::shared_ptr footprint_sub_; + std::shared_ptr 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 4b94ebb9b3..ec734a91ba 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 @@ -115,7 +115,7 @@ class Costmap2DPublisher rclcpp::Publisher::SharedPtr costmap_update_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 a9686fb4f2..77203c5934 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 @@ class CostmapSubscriber 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 25dbdf24a0..e4f6e2bc97 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 @@ class FootprintSubscriber std::string & topic_name); ~FootprintSubscriber() {} -bool getFootprint(std::vector & footprint); + bool getFootprint(std::vector & 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 0cef719932..2566d3e45a 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 & 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_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index d4d8098d89..d2c85a275e 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -61,9 +61,9 @@ Costmap2DPublisher::Costmap2DPublisher( // TODO(bpwilcox): port onNewSubscription functionality for publisher costmap_pub_ = ros_node->create_publisher(topic_name, - custom_qos_profile); + custom_qos_profile); costmap_raw_pub_ = ros_node->create_publisher(topic_name + "_raw", - custom_qos_profile); + custom_qos_profile); costmap_update_pub_ = ros_node->create_publisher( topic_name + "_updates", custom_qos_profile); @@ -161,12 +161,12 @@ void Costmap2DPublisher::prepareCostmap() void Costmap2DPublisher::publishCostmap() { if (node_->count_subscribers(topic_name_) == 0 && - node_->count_subscribers(topic_name_ + "_raw") == 0) + node_->count_subscribers(topic_name_ + "_raw") == 0) { // No subscribers, so why do any work? return; } - // Always publish raw costmap + // Always publish raw costmap prepareCostmap(); costmap_raw_pub_->publish(costmap_raw_); diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index c4bed17c2f..f03bf8434d 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 aca500b881..3f75cec12b 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 79eaa2db27..4689e691ee 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 @@ class TestCollisionChecker : public nav2_costmap_2d::CollisionChecker costmap_pub_ = std::make_unique(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 @@ class TestCollisionChecker : public nav2_costmap_2d::CollisionChecker 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 @@ class TestNode : public ::testing::Test node_ = rclcpp::Node::make_shared( "test_collision_checker", nav2_util::get_node_options_default()); - + tf_buffer_ = std::make_shared(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -189,7 +188,8 @@ class TestNode : public ::testing::Test costmap_sub_ = std::make_shared(node_, costmap_topic); footprint_sub_ = std::make_shared(node_, footprint_topic); - collision_checker_ = std::make_unique(node_, costmap_sub_, footprint_sub_, *tf_buffer_); + collision_checker_ = std::make_unique(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 353b9941c9..c718736358 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} )