Skip to content

Commit

Permalink
Adapt tests to humble Costmap2DROS constructor
Browse files Browse the repository at this point in the history
  • Loading branch information
Tony Najjar committed Mar 3, 2023
1 parent 61fcd76 commit 8c54aba
Show file tree
Hide file tree
Showing 5 changed files with 25 additions and 25 deletions.
4 changes: 2 additions & 2 deletions nav2_mppi_controller/test/critic_manager_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,7 @@ TEST(CriticManagerTests, BasicCriticOperations)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand Down Expand Up @@ -126,7 +126,7 @@ TEST(CriticManagerTests, CriticLoadingTest)
"critic_manager.critics",
rclcpp::ParameterValue(std::vector<std::string>{"ConstraintCritic", "PreferForwardCritic"}));
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State state;
costmap_ros->on_configure(state);
Expand Down
16 changes: 8 additions & 8 deletions nav2_mppi_controller/test/critics_tests.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ TEST(CriticTests, ConstraintsCritic)
// Standard preamble
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand Down Expand Up @@ -116,7 +116,7 @@ TEST(CriticTests, GoalAngleCritic)
// Standard preamble
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand Down Expand Up @@ -168,7 +168,7 @@ TEST(CriticTests, GoalCritic)
// Standard preamble
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand Down Expand Up @@ -217,7 +217,7 @@ TEST(CriticTests, PathAngleCritic)
// Standard preamble
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand Down Expand Up @@ -274,7 +274,7 @@ TEST(CriticTests, PreferForwardCritic)
// Standard preamble
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand Down Expand Up @@ -327,7 +327,7 @@ TEST(CriticTests, TwirlingCritic)
// Standard preamble
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand Down Expand Up @@ -387,7 +387,7 @@ TEST(CriticTests, PathFollowCritic)
// Standard preamble
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand Down Expand Up @@ -435,7 +435,7 @@ TEST(CriticTests, PathAlignCritic)
// Standard preamble
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand Down
24 changes: 12 additions & 12 deletions nav2_mppi_controller/test/optimizer_unit_tests.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -223,7 +223,7 @@ TEST(OptimizerTests, BasicInitializedFunctions)
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand Down Expand Up @@ -255,7 +255,7 @@ TEST(OptimizerTests, TestOptimizerMotionModels)
OptimizerTester optimizer_tester;
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand Down Expand Up @@ -287,7 +287,7 @@ TEST(OptimizerTests, setOffsetTests)
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand All @@ -310,7 +310,7 @@ TEST(OptimizerTests, resetTests)
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand All @@ -330,7 +330,7 @@ TEST(OptimizerTests, FallbackTests)
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2));
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand All @@ -354,7 +354,7 @@ TEST(OptimizerTests, PrepareTests)
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2));
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand All @@ -381,7 +381,7 @@ TEST(OptimizerTests, shiftControlSequenceTests)
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2));
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand Down Expand Up @@ -424,7 +424,7 @@ TEST(OptimizerTests, SpeedLimitTests)
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2));
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand Down Expand Up @@ -464,7 +464,7 @@ TEST(OptimizerTests, applyControlSequenceConstraintsTests)
node->declare_parameter("mppic.vy_max", rclcpp::ParameterValue(0.75));
node->declare_parameter("mppic.wz_max", rclcpp::ParameterValue(2.0));
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand Down Expand Up @@ -520,7 +520,7 @@ TEST(OptimizerTests, updateStateVelocitiesTests)
node->declare_parameter("mppic.vy_max", rclcpp::ParameterValue(0.60));
node->declare_parameter("mppic.wz_max", rclcpp::ParameterValue(2.0));
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand All @@ -545,7 +545,7 @@ TEST(OptimizerTests, getControlFromSequenceAsTwistTests)
node->declare_parameter("mppic.vy_max", rclcpp::ParameterValue(0.60));
node->declare_parameter("mppic.wz_max", rclcpp::ParameterValue(2.0));
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand Down Expand Up @@ -580,7 +580,7 @@ TEST(OptimizerTests, integrateStateVelocitiesTests)
node->declare_parameter("mppic.model_dt", rclcpp::ParameterValue(0.1));
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
Expand Down
4 changes: 2 additions & 2 deletions nav2_mppi_controller/test/path_handler_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -93,7 +93,7 @@ TEST(PathHandlerTests, TestBounds)
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9));
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
auto results = costmap_ros->set_parameters_atomically(
{rclcpp::Parameter("global_frame", "odom"),
rclcpp::Parameter("robot_base_frame", "base_link")});
Expand Down Expand Up @@ -144,7 +144,7 @@ TEST(PathHandlerTests, TestTransforms)
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9));
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State state;
costmap_ros->on_configure(state);
Expand Down
2 changes: 1 addition & 1 deletion nav2_mppi_controller/test/utils_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -267,7 +267,7 @@ TEST(UtilsTests, findPathCosts)
std::nullopt, std::nullopt}; /// Caution, keep references

auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap", true);
"dummy_costmap", "", "dummy_costmap");
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
auto * costmap = costmap_ros->getCostmap();
Expand Down

0 comments on commit 8c54aba

Please sign in to comment.