diff --git a/nav2_amcl/include/nav2_amcl/motion_model/differential_motion_model.hpp b/nav2_amcl/include/nav2_amcl/motion_model/differential_motion_model.hpp index 120e3f8bf7..2d17360204 100644 --- a/nav2_amcl/include/nav2_amcl/motion_model/differential_motion_model.hpp +++ b/nav2_amcl/include/nav2_amcl/motion_model/differential_motion_model.hpp @@ -44,4 +44,4 @@ class DifferentialMotionModel : public nav2_amcl::MotionModel double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_; }; } // namespace nav2_amcl -#endif +#endif // NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_ diff --git a/nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp b/nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp index 58010aaffe..4cd1c6a879 100644 --- a/nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp +++ b/nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp @@ -18,9 +18,10 @@ #define NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_ #include +#include + #include "nav2_amcl/pf/pf.hpp" #include "nav2_amcl/pf/pf_pdf.hpp" -#include namespace nav2_amcl { @@ -55,7 +56,6 @@ class MotionModel * @param delta change in pose in odometry update */ virtual void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta) = 0; - }; } // namespace nav2_amcl diff --git a/nav2_amcl/include/nav2_amcl/motion_model/omni_motion_model.hpp b/nav2_amcl/include/nav2_amcl/motion_model/omni_motion_model.hpp index 3201028341..ae586d4d6c 100644 --- a/nav2_amcl/include/nav2_amcl/motion_model/omni_motion_model.hpp +++ b/nav2_amcl/include/nav2_amcl/motion_model/omni_motion_model.hpp @@ -19,8 +19,8 @@ * */ -#ifndef DIFFERENTIAL_MOTION_MODEL_HPP -#define DIFFERENTIAL_MOTION_MODEL_HPP +#ifndef NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_ +#define NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_ #include #include @@ -44,4 +44,4 @@ class OmniMotionModel : public nav2_amcl::MotionModel double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_; }; } // namespace nav2_amcl -#endif +#endif // NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_ diff --git a/nav2_amcl/include/nav2_amcl/pf/eig3.hpp b/nav2_amcl/include/nav2_amcl/pf/eig3.hpp index ae0d698b17..ea53e1e870 100644 --- a/nav2_amcl/include/nav2_amcl/pf/eig3.hpp +++ b/nav2_amcl/include/nav2_amcl/pf/eig3.hpp @@ -1,3 +1,23 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ /* Eigen-decomposition for symmetric 3x3 real matrices. Public domain, copied from the public domain Java library JAMA. */ diff --git a/nav2_amcl/include/nav2_amcl/portable_utils.hpp b/nav2_amcl/include/nav2_amcl/portable_utils.hpp new file mode 100644 index 0000000000..13d8a795c0 --- /dev/null +++ b/nav2_amcl/include/nav2_amcl/portable_utils.hpp @@ -0,0 +1,41 @@ +// Copyright (c) Samsung Research America +// +// 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_AMCL__PORTABLE_UTILS_HPP_ +#define NAV2_AMCL__PORTABLE_UTILS_HPP_ + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef HAVE_DRAND48 +// Some system (e.g., Windows) doesn't come with drand48(), srand48(). +// Use rand, and srand for such system. +static double drand48(void) +{ + return ((double)rand()) / RAND_MAX;// NOLINT +} + +static void srand48(long int seedval)// NOLINT +{ + srand(seedval); +} +#endif + +#ifdef __cplusplus +} +#endif + +#endif // NAV2_AMCL__PORTABLE_UTILS_HPP_ diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 8eef5b4f5f..b9770285bf 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -48,7 +48,7 @@ #include "tf2/utils.h" #pragma GCC diagnostic pop -#include "portable_utils.h" +#include "nav2_amcl/portable_utils.hpp" using namespace std::placeholders; using namespace std::chrono_literals; diff --git a/nav2_amcl/src/include/portable_utils.h b/nav2_amcl/src/include/portable_utils.h deleted file mode 100644 index 1577e6875b..0000000000 --- a/nav2_amcl/src/include/portable_utils.h +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef PORTABLE_UTILS_H -#define PORTABLE_UTILS_H - -#include - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef HAVE_DRAND48 -// Some system (e.g., Windows) doesn't come with drand48(), srand48(). -// Use rand, and srand for such system. -static double drand48(void) -{ - return ((double)rand()) / RAND_MAX; -} - -static void srand48(long int seedval) -{ - srand(seedval); -} -#endif - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/nav2_amcl/src/motion_model/omni_motion_model.cpp b/nav2_amcl/src/motion_model/omni_motion_model.cpp index 009efb862a..969fb7f0d6 100644 --- a/nav2_amcl/src/motion_model/omni_motion_model.cpp +++ b/nav2_amcl/src/motion_model/omni_motion_model.cpp @@ -88,7 +88,7 @@ OmniMotionModel::odometryUpdate( } } -} // namespace nav2_amcl +} // namespace nav2_amcl #include PLUGINLIB_EXPORT_CLASS(nav2_amcl::OmniMotionModel, nav2_amcl::MotionModel) diff --git a/nav2_amcl/src/pf/eig3.c b/nav2_amcl/src/pf/eig3.c index 0eec4ce1a1..a5a84ae5df 100644 --- a/nav2_amcl/src/pf/eig3.c +++ b/nav2_amcl/src/pf/eig3.c @@ -1,3 +1,23 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ /* Eigen decomposition code for symmetric 3x3 matrices, copied from the public domain Java Matrix library JAMA. */ diff --git a/nav2_amcl/src/pf/pf.c b/nav2_amcl/src/pf/pf.c index d2426906af..63d71b99bd 100644 --- a/nav2_amcl/src/pf/pf.c +++ b/nav2_amcl/src/pf/pf.c @@ -35,7 +35,7 @@ #include "nav2_amcl/pf/pf_pdf.hpp" #include "nav2_amcl/pf/pf_kdtree.hpp" -#include "portable_utils.h" +#include "nav2_amcl/portable_utils.hpp" // Compute the required number of samples, given that there are k bins diff --git a/nav2_amcl/src/pf/pf_pdf.c b/nav2_amcl/src/pf/pf_pdf.c index 858bea7df8..98a7748ffa 100644 --- a/nav2_amcl/src/pf/pf_pdf.c +++ b/nav2_amcl/src/pf/pf_pdf.c @@ -34,7 +34,7 @@ #include "nav2_amcl/pf/pf_pdf.hpp" -#include "portable_utils.h" +#include "nav2_amcl/portable_utils.hpp" // Random number generator seed value static unsigned int pf_pdf_seed; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index ba8c701262..04b9f4daa4 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -92,7 +92,7 @@ class Costmap2D * @brief Constructor for a costmap from an OccupancyGrid map * @param map The OccupancyGrid map to create costmap from */ - Costmap2D(const nav_msgs::msg::OccupancyGrid & map); + explicit Costmap2D(const nav_msgs::msg::OccupancyGrid & map); /** * @brief Overloaded assignment operator diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp index ecfb9d71f8..c71d3bfea1 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -35,8 +35,8 @@ * Author: Alexey Merzlyakov *********************************************************************/ -#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTER_HPP_ -#define NAV2_COSTMAP_2D__COSTMAP_FILTER_HPP_ +#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__COSTMAP_FILTER_HPP_ +#define NAV2_COSTMAP_2D__COSTMAP_FILTERS__COSTMAP_FILTER_HPP_ #include #include @@ -78,7 +78,7 @@ class CostmapFilter : public Layer /** * @brief Initialization process of layer on startup */ - virtual void onInitialize() final; + void onInitialize() final; /** * @brief Update the bounds of the master costmap by this layer's update dimensions @@ -90,7 +90,7 @@ class CostmapFilter : public Layer * @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( + void updateBounds( double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, double * max_x, double * max_y) final; @@ -102,27 +102,27 @@ class CostmapFilter : public Layer * @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( + void updateCosts( nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) final; /** * @brief Activate the layer */ - virtual void activate() final; + void activate() final; /** * @brief Deactivate the layer */ - virtual void deactivate() final; + void deactivate() final; /** * @brief Reset the layer */ - virtual void reset() final; + void reset() final; /** * @brief If clearing operations should be processed on this layer or not */ - virtual bool isClearable() {return false;} + bool isClearable() {return false;} /** CostmapFilter API **/ /** @@ -182,4 +182,4 @@ class CostmapFilter : public Layer } // namespace nav2_costmap_2d -#endif // NAV2_COSTMAP_2D__COSTMAP_FILTER_HPP_ +#endif // NAV2_COSTMAP_2D__COSTMAP_FILTERS__COSTMAP_FILTER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/filter_values.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/filter_values.hpp index 71d991c7e9..03a157a3e4 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/filter_values.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/filter_values.hpp @@ -35,8 +35,8 @@ * Author: Alexey Merzlyakov *********************************************************************/ -#ifndef NAV2_COSTMAP_2D__FILTER_VALUES_HPP_ -#define NAV2_COSTMAP_2D__FILTER_VALUES_HPP_ +#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__FILTER_VALUES_HPP_ +#define NAV2_COSTMAP_2D__COSTMAP_FILTERS__FILTER_VALUES_HPP_ /** Provides constants used in costmap filters */ @@ -59,4 +59,4 @@ static constexpr double NO_SPEED_LIMIT = 0.0; } // namespace nav2_costmap_2d -#endif // NAV2_COSTMAP_2D__FILTER_VALUES_HPP_ +#endif // NAV2_COSTMAP_2D__COSTMAP_FILTERS__FILTER_VALUES_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp index 1755d57eb7..e5434571d4 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp @@ -35,14 +35,14 @@ * Author: Alexey Merzlyakov *********************************************************************/ -#ifndef NAV2_COSTMAP_2D__KEEPOUT_FILTER_HPP_ -#define NAV2_COSTMAP_2D__KEEPOUT_FILTER_HPP_ - -#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp" +#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__KEEPOUT_FILTER_HPP_ +#define NAV2_COSTMAP_2D__COSTMAP_FILTERS__KEEPOUT_FILTER_HPP_ #include #include +#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp" + #include "rclcpp/rclcpp.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_msgs/msg/costmap_filter_info.hpp" @@ -108,4 +108,4 @@ class KeepoutFilter : public CostmapFilter } // namespace nav2_costmap_2d -#endif // NAV2_COSTMAP_2D__KEEPOUT_FILTER_HPP_ +#endif // NAV2_COSTMAP_2D__COSTMAP_FILTERS__KEEPOUT_FILTER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp index c6b86799aa..c4655607aa 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp @@ -35,8 +35,11 @@ * Author: Alexey Merzlyakov *********************************************************************/ -#ifndef NAV2_COSTMAP_2D__SPEED_FILTER_HPP_ -#define NAV2_COSTMAP_2D__SPEED_FILTER_HPP_ +#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__SPEED_FILTER_HPP_ +#define NAV2_COSTMAP_2D__COSTMAP_FILTERS__SPEED_FILTER_HPP_ + +#include +#include #include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp" @@ -44,8 +47,6 @@ #include "nav2_msgs/msg/costmap_filter_info.hpp" #include "nav2_msgs/msg/speed_limit.hpp" -#include - namespace nav2_costmap_2d { /** @@ -144,4 +145,4 @@ class SpeedFilter : public CostmapFilter } // namespace nav2_costmap_2d -#endif // NAV2_COSTMAP_2D__SPEED_FILTER_HPP_ +#endif // NAV2_COSTMAP_2D__COSTMAP_FILTERS__SPEED_FILTER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index d12924f019..def79e349f 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -40,6 +40,7 @@ #include #include +#include #include "map_msgs/msg/occupancy_grid_update.hpp" #include "message_filters/subscriber.h" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp index 37c2f5a357..14baefcbdd 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp @@ -38,6 +38,9 @@ #ifndef NAV2_COSTMAP_2D__VOXEL_LAYER_HPP_ #define NAV2_COSTMAP_2D__VOXEL_LAYER_HPP_ +#include +#include "message_filters/subscriber.h" + #include #include #include @@ -48,7 +51,6 @@ #include #include #include -#include #include #include diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 5fcb27783a..ac9245e8ad 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -441,7 +441,7 @@ InflationLayer::dynamicParametersCallback( inflation_radius_ = parameter.as_double(); need_reinflation_ = true; need_cache_recompute = true; - } else if (param_name == name_ + "." + "cost_scaling_factor" && + } else if (param_name == name_ + "." + "cost_scaling_factor" && // NOLINT cost_scaling_factor_ != parameter.as_double()) { cost_scaling_factor_ = parameter.as_double(); @@ -453,19 +453,18 @@ InflationLayer::dynamicParametersCallback( enabled_ = parameter.as_bool(); need_reinflation_ = true; current_ = false; - } else if (param_name == name_ + "." + "inflate_unknown" && + } else if (param_name == name_ + "." + "inflate_unknown" && // NOLINT inflate_unknown_ != parameter.as_bool()) { inflate_unknown_ = parameter.as_bool(); need_reinflation_ = true; - } else if (param_name == name_ + "." + "inflate_around_unknown" && + } else if (param_name == name_ + "." + "inflate_around_unknown" && // NOLINT inflate_around_unknown_ != parameter.as_bool()) { inflate_around_unknown_ = parameter.as_bool(); need_reinflation_ = true; } } - } if (need_cache_recompute) { diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 8475e0f141..073109c235 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -477,7 +477,6 @@ StaticLayer::dynamicParametersCallback( current_ = false; } } - } result.successful = true; return result; diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index b0a1982601..6bd1ae4b52 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -485,7 +485,6 @@ VoxelLayer::dynamicParametersCallback( const auto & param_name = parameter.get_name(); if (param_type == ParameterType::PARAMETER_DOUBLE) { - if (param_name == name_ + "." + "max_obstacle_height") { max_obstacle_height_ = parameter.as_double(); } else if (param_name == name_ + "." + "origin_z") { @@ -520,7 +519,6 @@ VoxelLayer::dynamicParametersCallback( combination_method_ = parameter.as_int(); } } - } if (resize_map_needed) { diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index 2c12cd104b..d56ac942e9 100644 --- a/nav2_costmap_2d/src/costmap_subscriber.cpp +++ b/nav2_costmap_2d/src/costmap_subscriber.cpp @@ -55,7 +55,6 @@ std::shared_ptr CostmapSubscriber::getCostmap() void CostmapSubscriber::toCostmap2D() { - auto current_costmap_msg = std::atomic_load(&costmap_msg_); if (costmap_ == nullptr) { diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp index f51ec286be..5898879e8c 100644 --- a/nav2_costmap_2d/src/footprint_collision_checker.cpp +++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp @@ -62,7 +62,6 @@ double FootprintCollisionChecker::footprintCost(const Footprint footpr // 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 second point if (!worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1)) { return static_cast(LETHAL_OBSTACLE); diff --git a/nav2_costmap_2d/test/testing_helper.hpp b/nav2_costmap_2d/test/testing_helper.hpp index 957b858452..84aa0e4d83 100644 --- a/nav2_costmap_2d/test/testing_helper.hpp +++ b/nav2_costmap_2d/test/testing_helper.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_COSTMAP_2D__TESTING_HELPER_HPP_ -#define NAV2_COSTMAP_2D__TESTING_HELPER_HPP_ +#ifndef TESTING_HELPER_HPP_ +#define TESTING_HELPER_HPP_ #include @@ -139,4 +139,4 @@ void addInflationLayer( } -#endif // NAV2_COSTMAP_2D__TESTING_HELPER_HPP_ +#endif // TESTING_HELPER_HPP_ diff --git a/nav2_costmap_2d/test/unit/copy_window_test.cpp b/nav2_costmap_2d/test/unit/copy_window_test.cpp index 0f4f9ebe0e..ce943e1cc3 100644 --- a/nav2_costmap_2d/test/unit/copy_window_test.cpp +++ b/nav2_costmap_2d/test/unit/copy_window_test.cpp @@ -35,8 +35,8 @@ TEST(CopyWindow, copyValidWindow) ASSERT_TRUE(dst.copyWindow(src, 2, 2, 6, 6, 0, 0)); // Check that both marked cells were copied to destination costmap - ASSERT_TRUE(dst.getCost(0, 0) == 100); - ASSERT_TRUE(dst.getCost(3, 3) == 200); + ASSERT_EQ(dst.getCost(0, 0), 100); + ASSERT_EQ(dst.getCost(3, 3), 200); } TEST(CopyWindow, copyInvalidWindow) diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp index d9db108653..2607108707 100644 --- a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp @@ -34,9 +34,9 @@ using namespace std::chrono_literals; -static const std::string FILTER_NAME = "keepout_filter"; -static const std::string INFO_TOPIC = "costmap_filter_info"; -static const std::string MASK_TOPIC = "mask"; +static const char FILTER_NAME[]{"keepout_filter"}; +static const char INFO_TOPIC[]{"costmap_filter_info"}; +static const char MASK_TOPIC[]{"mask"}; class InfoPublisher : public rclcpp::Node { @@ -69,7 +69,7 @@ class InfoPublisher : public rclcpp::Node class MaskPublisher : public rclcpp::Node { public: - MaskPublisher(const nav_msgs::msg::OccupancyGrid & mask) + explicit MaskPublisher(const nav_msgs::msg::OccupancyGrid & mask) : Node("mask_pub") { publisher_ = this->create_publisher( @@ -216,16 +216,18 @@ void TestNode::createKeepoutFilter(const std::string & global_frame) nav2_costmap_2d::LayeredCostmap layers(global_frame, false, false); node_->declare_parameter( - FILTER_NAME + ".transform_tolerance", rclcpp::ParameterValue(0.5)); + std::string(FILTER_NAME) + ".transform_tolerance", rclcpp::ParameterValue(0.5)); node_->set_parameter( - rclcpp::Parameter(FILTER_NAME + ".transform_tolerance", 0.5)); + rclcpp::Parameter(std::string(FILTER_NAME) + ".transform_tolerance", 0.5)); node_->declare_parameter( - FILTER_NAME + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC)); + std::string(FILTER_NAME) + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC)); node_->set_parameter( - rclcpp::Parameter(FILTER_NAME + ".filter_info_topic", INFO_TOPIC)); + rclcpp::Parameter(std::string(FILTER_NAME) + ".filter_info_topic", INFO_TOPIC)); keepout_filter_ = std::make_shared(); - keepout_filter_->initialize(&layers, FILTER_NAME, tf_buffer_.get(), node_, nullptr, nullptr); + keepout_filter_->initialize( + &layers, std::string(FILTER_NAME), + tf_buffer_.get(), node_, nullptr, nullptr); keepout_filter_->initializeFilter(INFO_TOPIC); // Wait until mask will be received by KeepoutFilter diff --git a/nav2_costmap_2d/test/unit/speed_filter_test.cpp b/nav2_costmap_2d/test/unit/speed_filter_test.cpp index a92bd5e156..a7dda6093d 100644 --- a/nav2_costmap_2d/test/unit/speed_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/speed_filter_test.cpp @@ -38,10 +38,10 @@ using namespace std::chrono_literals; -static const std::string FILTER_NAME = "speed_filter"; -static const std::string INFO_TOPIC = "costmap_filter_info"; -static const std::string MASK_TOPIC = "mask"; -static const std::string SPEED_LIMIT_TOPIC = "speed_limit"; +static const char FILTER_NAME[]{"speed_filter"}; +static const char INFO_TOPIC[]{"costmap_filter_info"}; +static const char MASK_TOPIC[]{"mask"}; +static const char SPEED_LIMIT_TOPIC[]{"speed_limit"}; static const double NO_TRANSLATION = 0.0; static const double TRANSLATION_X = 1.0; @@ -82,7 +82,7 @@ class InfoPublisher : public rclcpp::Node class MaskPublisher : public rclcpp::Node { public: - MaskPublisher(const nav_msgs::msg::OccupancyGrid & mask) + explicit MaskPublisher(const nav_msgs::msg::OccupancyGrid & mask) : Node("mask_pub") { publisher_ = this->create_publisher( @@ -104,7 +104,7 @@ class MaskPublisher : public rclcpp::Node class SpeedLimitSubscriber : public rclcpp::Node { public: - SpeedLimitSubscriber(const std::string & speed_limit_topic) + explicit SpeedLimitSubscriber(const std::string & speed_limit_topic) : Node("speed_limit_sub"), speed_limit_updated_(false) { subscriber_ = this->create_subscription( @@ -349,17 +349,17 @@ bool TestNode::createSpeedFilter(const std::string & global_frame) nav2_costmap_2d::LayeredCostmap layers(global_frame, false, false); node_->declare_parameter( - FILTER_NAME + ".transform_tolerance", rclcpp::ParameterValue(0.5)); + std::string(FILTER_NAME) + ".transform_tolerance", rclcpp::ParameterValue(0.5)); node_->set_parameter( - rclcpp::Parameter(FILTER_NAME + ".transform_tolerance", 0.5)); + rclcpp::Parameter(std::string(FILTER_NAME) + ".transform_tolerance", 0.5)); node_->declare_parameter( - FILTER_NAME + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC)); + std::string(FILTER_NAME) + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC)); node_->set_parameter( - rclcpp::Parameter(FILTER_NAME + ".filter_info_topic", INFO_TOPIC)); + rclcpp::Parameter(std::string(FILTER_NAME) + ".filter_info_topic", INFO_TOPIC)); node_->declare_parameter( - FILTER_NAME + ".speed_limit_topic", rclcpp::ParameterValue(SPEED_LIMIT_TOPIC)); + std::string(FILTER_NAME) + ".speed_limit_topic", rclcpp::ParameterValue(SPEED_LIMIT_TOPIC)); node_->set_parameter( - rclcpp::Parameter(FILTER_NAME + ".speed_limit_topic", SPEED_LIMIT_TOPIC)); + rclcpp::Parameter(std::string(FILTER_NAME) + ".speed_limit_topic", SPEED_LIMIT_TOPIC)); speed_filter_ = std::make_shared(); speed_filter_->initialize(&layers, FILTER_NAME, tf_buffer_.get(), node_, nullptr, nullptr);