m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add humble-navigation2

Browse Source
This commit is contained in:
X-lanni
2025-05-27 19:03:40 +08:00
parent 974abb5e1e
commit e74ec539c2
1280 changed files with 204114 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
navigation2/nav2_mppi_controller/test/CMakeLists.txt
+40
View File
@@ -0,0 +1,40 @@
set(TEST_NAMES
optimizer_smoke_test
controller_state_transition_test
models_test
noise_generator_test
parameter_handler_test
motion_model_tests
trajectory_visualizer_tests
utils_test
path_handler_test
critic_manager_test
optimizer_unit_tests
)
foreach(name IN LISTS TEST_NAMES)
ament_add_gtest(${name}
${name}.cpp
)
ament_target_dependencies(${name}
${dependencies_pkgs}
)
target_link_libraries(${name}
mppi_controller
)
if(${TEST_DEBUG_INFO})
target_compile_definitions(${name} PUBLIC -DTEST_DEBUG_INFO)
endif()
endforeach()
# This is a special case requiring linking against the critics library
ament_add_gtest(critics_tests critics_tests.cpp)
ament_target_dependencies(critics_tests ${dependencies_pkgs})
target_link_libraries(critics_tests mppi_controller mppi_critics)
if(${TEST_DEBUG_INFO})
target_compile_definitions(critics_tests PUBLIC -DTEST_DEBUG_INFO)
endif()
navigation2/nav2_mppi_controller/test/controller_state_transition_test.cpp
+75
View File
@@ -0,0 +1,75 @@
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// 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 "gtest/gtest.h"
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/path.hpp>
#include <nav2_costmap_2d/costmap_2d.hpp>
#include <nav2_costmap_2d/costmap_2d_ros.hpp>
#include "nav2_mppi_controller/controller.hpp"
#include "utils/utils.hpp"
class RosLockGuard
{
public:
RosLockGuard() {rclcpp::init(0, nullptr);}
~RosLockGuard() {rclcpp::shutdown();}
};
RosLockGuard g_rclcpp;
// Tests basic transition from configure->active->process->deactive->cleanup
TEST(ControllerStateTransitionTest, ControllerNotFail)
{
const bool visualize = true;
TestCostmapSettings costmap_settings{};
// Node Options
rclcpp::NodeOptions options;
std::vector<rclcpp::Parameter> params;
setUpControllerParams(visualize, params);
options.parameter_overrides(params);
auto node = getDummyNode(options);
auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node->get_clock());
auto costmap_ros = getDummyCostmapRos(costmap_settings);
costmap_ros->setRobotFootprint(getDummySquareFootprint(0.01));
auto controller = getDummyController(node, tf_buffer, costmap_ros);
TestPose start_pose = costmap_settings.getCenterPose();
const double path_step = costmap_settings.resolution;
TestPathSettings path_settings{start_pose, 8, path_step, path_step};
// evalControl args
auto pose = getDummyPointStamped(node, start_pose);
auto velocity = getDummyTwist();
auto path = getIncrementalDummyPath(node, path_settings);
path.header.frame_id = costmap_ros->getGlobalFrameID();
pose.header.frame_id = costmap_ros->getGlobalFrameID();
controller->setPlan(path);
EXPECT_NO_THROW(controller->computeVelocityCommands(pose, velocity, {}));
controller->setSpeedLimit(0.5, true);
controller->setSpeedLimit(0.5, false);
controller->setSpeedLimit(1.0, true);
controller->deactivate();
controller->cleanup();
}
navigation2/nav2_mppi_controller/test/critic_manager_test.cpp
+139
View File
@@ -0,0 +1,139 @@
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// 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 <chrono>
#include <thread>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_mppi_controller/critic_manager.hpp"
// Tests critic manager
class RosLockGuard
{
public:
RosLockGuard() {rclcpp::init(0, nullptr);}
~RosLockGuard() {rclcpp::shutdown();}
};
RosLockGuard g_rclcpp;
using namespace mppi; // NOLINT
using namespace mppi::critics; // NOLINT
class DummyCritic : public CriticFunction
{
public:
virtual void initialize() {initialized_ = true;}
virtual void score(CriticData & /*data*/) {scored_ = true;}
bool initialized_{false}, scored_{false};
};
class CriticManagerWrapper : public CriticManager
{
public:
CriticManagerWrapper()
: CriticManager() {}
virtual void loadCritics()
{
critics_.clear();
auto instance = std::unique_ptr<critics::CriticFunction>(new DummyCritic);
critics_.push_back(std::move(instance));
critics_.back()->on_configure(
parent_, name_, name_ + "." + "DummyCritic", costmap_ros_,
parameters_handler_);
}
std::string getFullNameWrapper(const std::string & name)
{
return getFullName(name);
}
bool getDummyCriticInitialized()
{
return dynamic_cast<DummyCritic *>(critics_[0].get())->initialized_;
}
bool getDummyCriticScored()
{
return dynamic_cast<DummyCritic *>(critics_[0].get())->scored_;
}
};
class CriticManagerWrapperEnum : public CriticManager
{
public:
CriticManagerWrapperEnum()
: CriticManager() {}
unsigned int getCriticNum()
{
return critics_.size();
}
};
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
// Configuration should get parameters and initialize critic functions
CriticManagerWrapper critic_manager;
critic_manager.on_configure(node, "critic_manager", costmap_ros, &param_handler);
EXPECT_TRUE(critic_manager.getDummyCriticInitialized());
// Evaluation of critics should score them, but only if failure flag is not set
models::State state;
models::ControlSequence control_sequence;
models::Trajectories generated_trajectories;
models::Path path;
geometry_msgs::msg::Pose goal;
xt::xtensor<float, 1> costs;
float model_dt = 0.1;
CriticData data =
{state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
std::nullopt, std::nullopt};
data.fail_flag = true;
EXPECT_FALSE(critic_manager.getDummyCriticScored());
data.fail_flag = false;
critic_manager.evalTrajectoriesScores(data);
EXPECT_TRUE(critic_manager.getDummyCriticScored());
// This should get the full namespaced name of the critics
EXPECT_EQ(critic_manager.getFullNameWrapper("name"), std::string("mppi::critics::name"));
}
TEST(CriticManagerTests, CriticLoadingTest)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
node->declare_parameter(
"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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State state;
costmap_ros->on_configure(state);
// This should grab the critics parameter and load the 2 requested plugins
CriticManagerWrapperEnum critic_manager;
critic_manager.on_configure(node, "critic_manager", costmap_ros, &param_handler);
EXPECT_EQ(critic_manager.getCriticNum(), 2u);
}
navigation2/nav2_mppi_controller/test/critics_tests.cpp
+798
View File
@@ -0,0 +1,798 @@
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// 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 <chrono>
#include <thread>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_mppi_controller/tools/utils.hpp"
#include "nav2_mppi_controller/motion_models.hpp"
#include "nav2_mppi_controller/critics/constraint_critic.hpp"
#include "nav2_mppi_controller/critics/goal_angle_critic.hpp"
#include "nav2_mppi_controller/critics/goal_critic.hpp"
#include "nav2_mppi_controller/critics/obstacles_critic.hpp"
#include "nav2_mppi_controller/critics/cost_critic.hpp"
#include "nav2_mppi_controller/critics/path_align_critic.hpp"
#include "nav2_mppi_controller/critics/path_align_legacy_critic.hpp"
#include "nav2_mppi_controller/critics/path_angle_critic.hpp"
#include "nav2_mppi_controller/critics/path_follow_critic.hpp"
#include "nav2_mppi_controller/critics/prefer_forward_critic.hpp"
#include "nav2_mppi_controller/critics/twirling_critic.hpp"
#include "nav2_mppi_controller/critics/velocity_deadband_critic.hpp"
#include "nav2_core/exceptions.hpp"
#include "utils_test.cpp" // NOLINT
// Tests the various critic plugin functions
// ROS lock used from utils_test.cpp
using namespace mppi; // NOLINT
using namespace mppi::critics; // NOLINT
using namespace mppi::utils; // NOLINT
using xt::evaluation_strategy::immediate;
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
models::State state;
models::ControlSequence control_sequence;
models::Trajectories generated_trajectories;
models::Path path;
geometry_msgs::msg::Pose goal;
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
float model_dt = 0.1;
CriticData data =
{state, generated_trajectories, path, goal, costs, model_dt,
false, nullptr, nullptr, std::nullopt, std::nullopt};
data.motion_model = std::make_shared<DiffDriveMotionModel>();
// Initialization testing
// Make sure initializes correctly and that defaults are reasonable
ConstraintCritic critic;
critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
EXPECT_EQ(critic.getName(), "critic");
EXPECT_TRUE(critic.getMaxVelConstraint() > 0.0);
EXPECT_TRUE(critic.getMinVelConstraint() < 0.0);
// Scoring testing
// provide velocities in constraints, should not have any costs
state.vx = 0.40 * xt::ones<float>({1000, 30});
state.vy = xt::zeros<float>({1000, 30});
state.wz = xt::ones<float>({1000, 30});
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6);
// provide out of maximum velocity constraint
auto last_batch_traj_in_full = xt::view(state.vx, -1, xt::all());
last_batch_traj_in_full = 0.60 * xt::ones<float>({30});
critic.score(data);
EXPECT_GT(xt::sum(costs, immediate)(), 0);
// 4.0 weight * 0.1 model_dt * 0.1 error introduced * 30 timesteps = 1.2
EXPECT_NEAR(costs(999), 1.2, 0.01);
costs = xt::zeros<float>({1000});
// provide out of minimum velocity constraint
auto first_batch_traj_in_full = xt::view(state.vx, 1, xt::all());
first_batch_traj_in_full = -0.45 * xt::ones<float>({30});
critic.score(data);
EXPECT_GT(xt::sum(costs, immediate)(), 0);
// 4.0 weight * 0.1 model_dt * 0.1 error introduced * 30 timesteps = 1.2
EXPECT_NEAR(costs(1), 1.2, 0.01);
costs = xt::zeros<float>({1000});
// Now with ackermann, all in constraint so no costs to score
state.vx = 0.40 * xt::ones<float>({1000, 30});
state.wz = 1.5 * xt::ones<float>({1000, 30});
data.motion_model = std::make_shared<AckermannMotionModel>(&param_handler, node->get_name());
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6);
// Now violating the ackermann constraints
state.wz = 2.5 * xt::ones<float>({1000, 30});
critic.score(data);
EXPECT_GT(xt::sum(costs, immediate)(), 0);
// 4.0 weight * 0.1 model_dt * (0.2 - 0.4/2.5) * 30 timesteps = 0.48
EXPECT_NEAR(costs(1), 0.48, 0.01);
}
TEST(CriticTests, ObstacleCriticMisalignedParams) {
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
auto getParam = param_handler.getParamGetter("critic");
bool consider_footprint;
getParam(consider_footprint, "consider_footprint", true);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
ObstaclesCritic critic;
// Expect throw when settings mismatched
EXPECT_THROW(
critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler),
nav2_core::PlannerException
);
}
TEST(CriticTests, ObstacleCriticAlignedParams) {
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap");
ParametersHandler param_handler(node);
auto getParam = param_handler.getParamGetter("critic");
bool consider_footprint;
getParam(consider_footprint, "consider_footprint", false);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
ObstaclesCritic critic;
critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
EXPECT_EQ(critic.getName(), "critic");
}
TEST(CriticTests, CostCriticMisAlignedParams) {
// 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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
auto getParam = param_handler.getParamGetter("critic");
bool consider_footprint;
getParam(consider_footprint, "consider_footprint", true);
costmap_ros->on_configure(lstate);
CostCritic critic;
// Expect throw when settings mismatched
EXPECT_THROW(
critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler),
nav2_core::PlannerException
);
}
TEST(CriticTests, CostCriticAlignedParams) {
// 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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
auto getParam = param_handler.getParamGetter("critic");
bool consider_footprint;
getParam(consider_footprint, "consider_footprint", false);
costmap_ros->on_configure(lstate);
CostCritic critic;
critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
EXPECT_EQ(critic.getName(), "critic");
}
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
models::State state;
models::ControlSequence control_sequence;
models::Trajectories generated_trajectories;
generated_trajectories.reset(1000, 30);
models::Path path;
geometry_msgs::msg::Pose goal;
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
float model_dt = 0.1;
CriticData data =
{state, generated_trajectories, path, goal, costs, model_dt,
false, nullptr, nullptr, std::nullopt, std::nullopt};
data.motion_model = std::make_shared<DiffDriveMotionModel>();
// Initialization testing
// Make sure initializes correctly
GoalAngleCritic critic;
critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
EXPECT_EQ(critic.getName(), "critic");
// Scoring testing
// provide state poses and path too far from `threshold_to_consider` to consider
state.pose.pose.position.x = 1.0;
path.reset(10);
path.x(9) = 10.0;
path.y(9) = 0.0;
path.yaws(9) = 3.14;
goal.position.x = 10.0;
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6);
// Lets move it even closer, just to be sure it still doesn't trigger
state.pose.pose.position.x = 9.2;
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6);
// provide state pose and path below `threshold_to_consider` to consider
state.pose.pose.position.x = 9.7;
critic.score(data);
EXPECT_GT(xt::sum(costs, immediate)(), 0);
EXPECT_NEAR(costs(0), 9.42, 0.02); // (3.14 - 0.0) * 3.0 weight
}
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
models::State state;
models::ControlSequence control_sequence;
models::Trajectories generated_trajectories;
generated_trajectories.reset(1000, 30);
models::Path path;
geometry_msgs::msg::Pose goal;
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
float model_dt = 0.1;
CriticData data =
{state, generated_trajectories, path, goal, costs, model_dt,
false, nullptr, nullptr, std::nullopt, std::nullopt};
data.motion_model = std::make_shared<DiffDriveMotionModel>();
// Initialization testing
// Make sure initializes correctly
GoalCritic critic;
critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
EXPECT_EQ(critic.getName(), "critic");
// Scoring testing with all trajectories set to 0
// provide state poses and path far, should not trigger
state.pose.pose.position.x = 1.0;
path.reset(10);
path.x(9) = 10.0;
path.y(9) = 0.0;
goal.position.x = 10.0;
critic.score(data);
EXPECT_NEAR(costs(2), 0.0, 1e-6); // (0 * 5.0 weight
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); // Should all be 0 * 1000
costs = xt::zeros<float>({1000});
// provide state pose and path close
path.x(9) = 0.5;
path.y(9) = 0.0;
goal.position.x = 0.5;
critic.score(data);
EXPECT_NEAR(costs(2), 2.5, 1e-6); // (sqrt(10.0 * 10.0) * 5.0 weight
EXPECT_NEAR(xt::sum(costs, immediate)(), 2500.0, 1e-6); // should be 2.5 * 1000
}
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
models::State state;
state.reset(1000, 30);
models::ControlSequence control_sequence;
models::Trajectories generated_trajectories;
generated_trajectories.reset(1000, 30);
models::Path path;
geometry_msgs::msg::Pose goal;
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
float model_dt = 0.1;
CriticData data =
{state, generated_trajectories, path, goal, costs, model_dt,
false, nullptr, nullptr, std::nullopt, std::nullopt};
data.motion_model = std::make_shared<DiffDriveMotionModel>();
TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally
// Initialization testing
// Make sure initializes correctly
PathAngleCritic critic;
critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
EXPECT_EQ(critic.getName(), "critic");
// Scoring testing
// provide state poses and path close, within pose tolerance so won't do anything
state.pose.pose.position.x = 0.0;
state.pose.pose.position.y = 0.0;
path.reset(10);
path.x(9) = 0.15;
goal.position.x = 0.15;
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
// provide state pose and path close but outside of tol. with less than PI/2 angular diff.
path.x(9) = 0.95;
goal.position.x = 0.95;
data.furthest_reached_path_point = 2; // So it grabs the 2 + offset_from_furthest_ = 6th point
path.x(6) = 1.0; // angle between path point and pose = 0 < max_angle_to_furthest_
path.y(6) = 0.0;
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
// provide state pose and path close but outside of tol. with more than PI/2 angular diff.
path.x(6) = -1.0; // angle between path point and pose > max_angle_to_furthest_
path.y(6) = 4.0;
critic.score(data);
EXPECT_GT(xt::sum(costs, immediate)(), 0.0);
EXPECT_NEAR(costs(0), 3.6315, 1e-2); // atan2(4,-1) [1.81] * 2.0 weight
}
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
models::State state;
state.reset(1000, 30);
models::ControlSequence control_sequence;
models::Trajectories generated_trajectories;
generated_trajectories.reset(1000, 30);
models::Path path;
geometry_msgs::msg::Pose goal;
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
float model_dt = 0.1;
CriticData data =
{state, generated_trajectories, path, goal, costs, model_dt,
false, nullptr, nullptr, std::nullopt, std::nullopt};
data.motion_model = std::make_shared<DiffDriveMotionModel>();
TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally
// Initialization testing
// Make sure initializes correctly
PreferForwardCritic critic;
critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
EXPECT_EQ(critic.getName(), "critic");
// Scoring testing
// provide state poses and path far away, not within positional tolerances
state.pose.pose.position.x = 1.0;
path.reset(10);
path.x(9) = 10.0;
goal.position.x = 10.0;
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0f, 1e-6f);
// provide state pose and path close to trigger behavior but with all forward motion
path.x(9) = 0.15;
goal.position.x = 0.15;
state.vx = xt::ones<float>({1000, 30});
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0f, 1e-6f);
// provide state pose and path close to trigger behavior but with all reverse motion
state.vx = -1.0 * xt::ones<float>({1000, 30});
critic.score(data);
EXPECT_GT(xt::sum(costs, immediate)(), 0.0f);
EXPECT_NEAR(costs(0), 15.0f, 1e-3f); // 1.0 * 0.1 model_dt * 5.0 weight * 30 length
}
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
models::State state;
state.reset(1000, 30);
models::ControlSequence control_sequence;
models::Trajectories generated_trajectories;
generated_trajectories.reset(1000, 30);
models::Path path;
geometry_msgs::msg::Pose goal;
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
float model_dt = 0.1;
CriticData data =
{state, generated_trajectories, path, goal, costs, model_dt,
false, nullptr, nullptr, std::nullopt, std::nullopt};
data.motion_model = std::make_shared<DiffDriveMotionModel>();
TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally
data.goal_checker = &goal_checker;
// Initialization testing
// Make sure initializes correctly
TwirlingCritic critic;
critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
EXPECT_EQ(critic.getName(), "critic");
// Scoring testing
// provide state poses and path far away, not within positional tolerances
state.pose.pose.position.x = 1.0;
path.reset(10);
path.x(9) = 10.0;
goal.position.x = 10.0;
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
// provide state pose and path close to trigger behavior but with no angular variation
path.x(9) = 0.15;
goal.position.x = 0.15;
state.wz = xt::zeros<float>({1000, 30});
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
// Provide nearby with some motion
auto traj_view = xt::view(state.wz, 0, xt::all());
traj_view = 10.0;
critic.score(data);
EXPECT_NEAR(costs(0), 100.0, 1e-6); // (mean(10.0) * 10.0 weight
costs = xt::zeros<float>({1000});
// Now try again with some wiggling noise
traj_view = xt::random::randn<float>({30}, 0.0, 0.5);
critic.score(data);
EXPECT_NEAR(costs(0), 3.3, 4e-1); // (mean of noise with mu=0, sigma=0.5 * 10.0 weight
}
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
models::State state;
state.reset(1000, 30);
models::ControlSequence control_sequence;
models::Trajectories generated_trajectories;
generated_trajectories.reset(1000, 30);
models::Path path;
geometry_msgs::msg::Pose goal;
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
float model_dt = 0.1;
CriticData data =
{state, generated_trajectories, path, goal, costs, model_dt,
false, nullptr, nullptr, std::nullopt, std::nullopt};
data.motion_model = std::make_shared<DiffDriveMotionModel>();
TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally
data.goal_checker = &goal_checker;
// Initialization testing
// Make sure initializes correctly
PathFollowCritic critic;
critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
EXPECT_EQ(critic.getName(), "critic");
// Scoring testing
// provide state poses and goal close within positional tolerances
state.pose.pose.position.x = 2.0;
path.reset(6);
path.x(5) = 1.8;
goal.position.x = 1.8;
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
// provide state pose and path far enough to enable
// pose differential is (0, 0) and (0.15, 0)
path.x(5) = 0.15;
goal.position.x = 0.15;
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 750.0, 1e-2); // 0.15 * 5 weight * 1000
}
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
models::State state;
state.reset(1000, 30);
models::ControlSequence control_sequence;
models::Trajectories generated_trajectories;
generated_trajectories.reset(1000, 30);
models::Path path;
geometry_msgs::msg::Pose goal;
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
float model_dt = 0.1;
CriticData data =
{state, generated_trajectories, path, goal, costs, model_dt,
false, nullptr, nullptr, std::nullopt, std::nullopt};
data.motion_model = std::make_shared<DiffDriveMotionModel>();
TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally
data.goal_checker = &goal_checker;
// Initialization testing
// Make sure initializes correctly
PathAlignCritic critic;
critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
EXPECT_EQ(critic.getName(), "critic");
// Scoring testing
// provide state poses and path close within positional tolerances
state.pose.pose.position.x = 1.0;
path.reset(10);
path.x(9) = 0.85;
goal.position.x = 0.85;
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
// provide state pose and path far enough to enable
// but data furthest point reached is 0 and offset default is 20, so returns
path.x(9) = 0.15;
goal.position.x = 0.15;
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
// provide state pose and path far enough to enable, with data to pass condition
// but with empty trajectories and paths, should still be zero
*data.furthest_reached_path_point = 21;
path.x(9) = 0.15;
goal.position.x = 0.15;
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
// provide state pose and path far enough to enable, with data to pass condition
// and with a valid path to pass invalid path condition
state.pose.pose.position.x = 0.0;
data.path_pts_valid.reset(); // Recompute on new path
path.reset(22);
path.x(0) = 0;
path.x(1) = 0.1;
path.x(2) = 0.2;
path.x(3) = 0.3;
path.x(4) = 0.4;
path.x(5) = 0.5;
path.x(6) = 0.6;
path.x(7) = 0.7;
path.x(8) = 0.8;
path.x(9) = 0.9;
path.x(10) = 0.9;
path.x(11) = 0.9;
path.x(12) = 0.9;
path.x(13) = 0.9;
path.x(14) = 0.9;
path.x(15) = 0.9;
path.x(16) = 0.9;
path.x(17) = 0.9;
path.x(18) = 0.9;
path.x(19) = 0.9;
path.x(20) = 0.9;
path.x(21) = 0.9;
goal.position.x = 0.9;
generated_trajectories.x = 0.66 * xt::ones<float>({1000, 30});
critic.score(data);
// 0.66 * 1000 * 10 weight * 6 num pts eval / 6 normalization term
EXPECT_NEAR(xt::sum(costs, immediate)(), 6600.0, 1e-2);
// provide state pose and path far enough to enable, with data to pass condition
// but path is blocked in collision
auto * costmap = costmap_ros->getCostmap();
// island in the middle of lethal cost to cross. Costmap defaults to size 5x5 @ 10cm resolution
for (unsigned int i = 11; i <= 30; ++i) { // 1.1m-3m
for (unsigned int j = 11; j <= 30; ++j) { // 1.1m-3m
costmap->setCost(i, j, 254);
}
}
data.path_pts_valid.reset(); // Recompute on new path
costs = xt::zeros<float>({1000});
path.x = 1.5 * xt::ones<float>({22});
path.y = 1.5 * xt::ones<float>({22});
goal.position.x = 1.5;
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
}
TEST(CriticTests, PathAlignLegacyCritic)
{
// 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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
models::State state;
state.reset(1000, 30);
models::ControlSequence control_sequence;
models::Trajectories generated_trajectories;
generated_trajectories.reset(1000, 30);
models::Path path;
geometry_msgs::msg::Pose goal;
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
float model_dt = 0.1;
CriticData data =
{state, generated_trajectories, path, goal, costs, model_dt,
false, nullptr, nullptr, std::nullopt, std::nullopt};
data.motion_model = std::make_shared<DiffDriveMotionModel>();
TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally
data.goal_checker = &goal_checker;
// Initialization testing
// Make sure initializes correctly
PathAlignLegacyCritic critic;
critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
EXPECT_EQ(critic.getName(), "critic");
// Scoring testing
// provide state poses and path close within positional tolerances
state.pose.pose.position.x = 1.0;
path.reset(10);
path.x(9) = 0.85;
goal.position.x = 0.85;
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
// provide state pose and path far enough to enable
// but data furthest point reached is 0 and offset default is 20, so returns
path.x(9) = 0.15;
goal.position.x = 0.15;
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
// provide state pose and path far enough to enable, with data to pass condition
// but with empty trajectories and paths, should still be zero
*data.furthest_reached_path_point = 21;
path.x(9) = 0.15;
goal.position.x = 0.15;
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
// provide state pose and path far enough to enable, with data to pass condition
// and with a valid path to pass invalid path condition
state.pose.pose.position.x = 0.0;
data.path_pts_valid.reset(); // Recompute on new path
path.reset(22);
path.x(0) = 0;
path.x(1) = 0.1;
path.x(2) = 0.2;
path.x(3) = 0.3;
path.x(4) = 0.4;
path.x(5) = 0.5;
path.x(6) = 0.6;
path.x(7) = 0.7;
path.x(8) = 0.8;
path.x(9) = 0.9;
path.x(10) = 0.9;
path.x(11) = 0.9;
path.x(12) = 0.9;
path.x(13) = 0.9;
path.x(14) = 0.9;
path.x(15) = 0.9;
path.x(16) = 0.9;
path.x(17) = 0.9;
path.x(18) = 0.9;
path.x(19) = 0.9;
path.x(20) = 0.9;
path.x(21) = 0.9;
goal.position.x = 0.9;
generated_trajectories.x = 0.66 * xt::ones<float>({1000, 30});
critic.score(data);
// 0.04 * 1000 * 10 weight * 6 num pts eval / 6 normalization term
EXPECT_NEAR(xt::sum(costs, immediate)(), 400.0, 1e-2);
// provide state pose and path far enough to enable, with data to pass condition
// but path is blocked in collision
auto * costmap = costmap_ros->getCostmap();
// island in the middle of lethal cost to cross. Costmap defaults to size 5x5 @ 10cm resolution
for (unsigned int i = 11; i <= 30; ++i) { // 1.1m-3m
for (unsigned int j = 11; j <= 30; ++j) { // 1.1m-3m
costmap->setCost(i, j, 254);
}
}
data.path_pts_valid.reset(); // Recompute on new path
costs = xt::zeros<float>({1000});
path.x = 1.5 * xt::ones<float>({22});
path.y = 1.5 * xt::ones<float>({22});
goal.position.x = 1.5;
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
}
TEST(CriticTests, VelocityDeadbandCritic)
{
// 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");
ParametersHandler param_handler(node);
auto getParam = param_handler.getParamGetter("critic");
std::vector<double> deadband_velocities_;
getParam(deadband_velocities_, "deadband_velocities", std::vector<double>{0.08, 0.08, 0.08});
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
models::State state;
models::ControlSequence control_sequence;
models::Trajectories generated_trajectories;
models::Path path;
geometry_msgs::msg::Pose goal;
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
float model_dt = 0.1;
CriticData data =
{state, generated_trajectories, path, goal, costs, model_dt,
false, nullptr, nullptr, std::nullopt, std::nullopt};
data.motion_model = std::make_shared<OmniMotionModel>();
// Initialization testing
// Make sure initializes correctly and that defaults are reasonable
VelocityDeadbandCritic critic;
critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
EXPECT_EQ(critic.getName(), "critic");
// Scoring testing
// provide velocities out of deadband bounds, should not have any costs
state.vx = 0.80 * xt::ones<float>({1000, 30});
state.vy = 0.60 * xt::ones<float>({1000, 30});
state.wz = 0.80 * xt::ones<float>({1000, 30});
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6);
// Test cost value
state.vx = 0.01 * xt::ones<float>({1000, 30});
state.vy = 0.02 * xt::ones<float>({1000, 30});
state.wz = 0.021 * xt::ones<float>({1000, 30});
critic.score(data);
// 35.0 weight * 0.1 model_dt * (0.07 + 0.06 + 0.059) * 30 timesteps = 56.7
EXPECT_NEAR(costs(1), 19.845, 0.01);
}
navigation2/nav2_mppi_controller/test/models_test.cpp
+149
View File
@@ -0,0 +1,149 @@
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// 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 "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_mppi_controller/models/control_sequence.hpp"
#include "nav2_mppi_controller/models/path.hpp"
#include "nav2_mppi_controller/models/state.hpp"
#include "nav2_mppi_controller/models/trajectories.hpp"
// Tests model classes with methods
class RosLockGuard
{
public:
RosLockGuard() {rclcpp::init(0, nullptr);}
~RosLockGuard() {rclcpp::shutdown();}
};
RosLockGuard g_rclcpp;
using namespace mppi::models; // NOLINT
TEST(ModelsTest, ControlSequenceTest)
{
// populate the object
ControlSequence sequence;
sequence.vx = xt::ones<float>({10});
sequence.vy = xt::ones<float>({10});
sequence.wz = xt::ones<float>({10});
// Show you can get contents
EXPECT_EQ(sequence.vx(4), 1);
EXPECT_EQ(sequence.vy(4), 1);
EXPECT_EQ(sequence.wz(4), 1);
sequence.reset(20);
// Show contents are gone and new size
EXPECT_EQ(sequence.vx(4), 0);
EXPECT_EQ(sequence.vy(4), 0);
EXPECT_EQ(sequence.wz(4), 0);
EXPECT_EQ(sequence.vx.shape(0), 20u);
EXPECT_EQ(sequence.vy.shape(0), 20u);
EXPECT_EQ(sequence.wz.shape(0), 20u);
}
TEST(ModelsTest, PathTest)
{
// populate the object
Path path;
path.x = xt::ones<float>({10});
path.y = xt::ones<float>({10});
path.yaws = xt::ones<float>({10});
// Show you can get contents
EXPECT_EQ(path.x(4), 1);
EXPECT_EQ(path.y(4), 1);
EXPECT_EQ(path.yaws(4), 1);
path.reset(20);
// Show contents are gone and new size
EXPECT_EQ(path.x(4), 0);
EXPECT_EQ(path.y(4), 0);
EXPECT_EQ(path.yaws(4), 0);
EXPECT_EQ(path.x.shape(0), 20u);
EXPECT_EQ(path.y.shape(0), 20u);
EXPECT_EQ(path.yaws.shape(0), 20u);
}
TEST(ModelsTest, StateTest)
{
// populate the object
State state;
state.vx = xt::ones<float>({10, 10});
state.vy = xt::ones<float>({10, 10});
state.wz = xt::ones<float>({10, 10});
state.cvx = xt::ones<float>({10, 10});
state.cvy = xt::ones<float>({10, 10});
state.cwz = xt::ones<float>({10, 10});
// Show you can get contents
EXPECT_EQ(state.cvx(4), 1);
EXPECT_EQ(state.cvy(4), 1);
EXPECT_EQ(state.cwz(4), 1);
EXPECT_EQ(state.vx(4), 1);
EXPECT_EQ(state.vy(4), 1);
EXPECT_EQ(state.wz(4), 1);
state.reset(20, 40);
// Show contents are gone and new size
EXPECT_EQ(state.cvx(4), 0);
EXPECT_EQ(state.cvy(4), 0);
EXPECT_EQ(state.cwz(4), 0);
EXPECT_EQ(state.vx(4), 0);
EXPECT_EQ(state.vy(4), 0);
EXPECT_EQ(state.wz(4), 0);
EXPECT_EQ(state.cvx.shape(0), 20u);
EXPECT_EQ(state.cvy.shape(0), 20u);
EXPECT_EQ(state.cwz.shape(0), 20u);
EXPECT_EQ(state.cvx.shape(1), 40u);
EXPECT_EQ(state.cvy.shape(1), 40u);
EXPECT_EQ(state.cwz.shape(1), 40u);
EXPECT_EQ(state.vx.shape(0), 20u);
EXPECT_EQ(state.vy.shape(0), 20u);
EXPECT_EQ(state.wz.shape(0), 20u);
EXPECT_EQ(state.vx.shape(1), 40u);
EXPECT_EQ(state.vy.shape(1), 40u);
EXPECT_EQ(state.wz.shape(1), 40u);
}
TEST(ModelsTest, TrajectoriesTest)
{
// populate the object
Trajectories trajectories;
trajectories.x = xt::ones<float>({10, 10});
trajectories.y = xt::ones<float>({10, 10});
trajectories.yaws = xt::ones<float>({10, 10});
// Show you can get contents
EXPECT_EQ(trajectories.x(4), 1);
EXPECT_EQ(trajectories.y(4), 1);
EXPECT_EQ(trajectories.yaws(4), 1);
trajectories.reset(20, 40);
// Show contents are gone and new size
EXPECT_EQ(trajectories.x(4), 0);
EXPECT_EQ(trajectories.y(4), 0);
EXPECT_EQ(trajectories.yaws(4), 0);
EXPECT_EQ(trajectories.x.shape(0), 20u);
EXPECT_EQ(trajectories.y.shape(0), 20u);
EXPECT_EQ(trajectories.yaws.shape(0), 20u);
EXPECT_EQ(trajectories.x.shape(1), 40u);
EXPECT_EQ(trajectories.y.shape(1), 40u);
EXPECT_EQ(trajectories.yaws.shape(1), 40u);
}
navigation2/nav2_mppi_controller/test/motion_model_tests.cpp
+257
View File
@@ -0,0 +1,257 @@
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// 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 <chrono>
#include <thread>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_mppi_controller/motion_models.hpp"
#include "nav2_mppi_controller/models/state.hpp"
#include "nav2_mppi_controller/models/control_sequence.hpp"
// Tests motion models
class RosLockGuard
{
public:
RosLockGuard() {rclcpp::init(0, nullptr);}
~RosLockGuard() {rclcpp::shutdown();}
};
RosLockGuard g_rclcpp;
using namespace mppi; // NOLINT
TEST(MotionModelTests, DiffDriveTest)
{
models::ControlSequence control_sequence;
models::State state;
int batches = 1000;
int timesteps = 50;
control_sequence.reset(timesteps); // populates with zeros
state.reset(batches, timesteps); // populates with zeros
std::unique_ptr<DiffDriveMotionModel> model =
std::make_unique<DiffDriveMotionModel>();
// Check that predict properly populates the trajectory velocities with the control velocities
state.cvx = 10 * xt::ones<float>({batches, timesteps});
state.cvy = 5 * xt::ones<float>({batches, timesteps});
state.cwz = 1 * xt::ones<float>({batches, timesteps});
// Manually set state index 0 from initial conditions which would be the speed of the robot
xt::view(state.vx, xt::all(), 0) = 10;
xt::view(state.wz, xt::all(), 0) = 1;
model->predict(state);
EXPECT_EQ(state.vx, state.cvx);
EXPECT_EQ(state.vy, xt::zeros<float>({batches, timesteps})); // non-holonomic
EXPECT_EQ(state.wz, state.cwz);
// Check that application of constraints are empty for Diff Drive
for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) {
control_sequence.vx(i) = i * i * i;
control_sequence.wz(i) = i * i * i;
}
models::ControlSequence initial_control_sequence = control_sequence;
model->applyConstraints(control_sequence);
EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx);
EXPECT_EQ(initial_control_sequence.vy, control_sequence.vy);
EXPECT_EQ(initial_control_sequence.wz, control_sequence.wz);
// Check that Diff Drive is properly non-holonomic
EXPECT_EQ(model->isHolonomic(), false);
// Check it cleanly destructs
model.reset();
}
TEST(MotionModelTests, OmniTest)
{
models::ControlSequence control_sequence;
models::State state;
int batches = 1000;
int timesteps = 50;
control_sequence.reset(timesteps); // populates with zeros
state.reset(batches, timesteps); // populates with zeros
std::unique_ptr<OmniMotionModel> model =
std::make_unique<OmniMotionModel>();
// Check that predict properly populates the trajectory velocities with the control velocities
state.cvx = 10 * xt::ones<float>({batches, timesteps});
state.cvy = 5 * xt::ones<float>({batches, timesteps});
state.cwz = 1 * xt::ones<float>({batches, timesteps});
// Manually set state index 0 from initial conditions which would be the speed of the robot
xt::view(state.vx, xt::all(), 0) = 10;
xt::view(state.vy, xt::all(), 0) = 5;
xt::view(state.wz, xt::all(), 0) = 1;
model->predict(state);
EXPECT_EQ(state.vx, state.cvx);
EXPECT_EQ(state.vy, state.cvy); // holonomic
EXPECT_EQ(state.wz, state.cwz);
// Check that application of constraints are empty for Omni Drive
for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) {
control_sequence.vx(i) = i * i * i;
control_sequence.vy(i) = i * i * i;
control_sequence.wz(i) = i * i * i;
}
models::ControlSequence initial_control_sequence = control_sequence;
model->applyConstraints(control_sequence);
EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx);
EXPECT_EQ(initial_control_sequence.vy, control_sequence.vy);
EXPECT_EQ(initial_control_sequence.wz, control_sequence.wz);
// Check that Omni Drive is properly holonomic
EXPECT_EQ(model->isHolonomic(), true);
// Check it cleanly destructs
model.reset();
}
TEST(MotionModelTests, AckermannTest)
{
models::ControlSequence control_sequence;
models::State state;
int batches = 1000;
int timesteps = 50;
control_sequence.reset(timesteps); // populates with zeros
state.reset(batches, timesteps); // populates with zeros
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
ParametersHandler param_handler(node);
std::unique_ptr<AckermannMotionModel> model =
std::make_unique<AckermannMotionModel>(&param_handler, node->get_name());
// Check that predict properly populates the trajectory velocities with the control velocities
state.cvx = 10 * xt::ones<float>({batches, timesteps});
state.cvy = 5 * xt::ones<float>({batches, timesteps});
state.cwz = 1 * xt::ones<float>({batches, timesteps});
// Manually set state index 0 from initial conditions which would be the speed of the robot
xt::view(state.vx, xt::all(), 0) = 10;
xt::view(state.wz, xt::all(), 0) = 1;
model->predict(state);
EXPECT_EQ(state.vx, state.cvx);
EXPECT_EQ(state.vy, xt::zeros<float>({batches, timesteps})); // non-holonomic
EXPECT_EQ(state.wz, state.cwz);
// Check that application of constraints are non-empty for Ackermann Drive
for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) {
control_sequence.vx(i) = i * i * i;
control_sequence.wz(i) = i * i * i * i;
}
models::ControlSequence initial_control_sequence = control_sequence;
model->applyConstraints(control_sequence);
// VX equal since this doesn't change, the WZ is reduced if breaking the constraint
EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx);
EXPECT_NE(initial_control_sequence.wz, control_sequence.wz);
for (unsigned int i = 1; i != control_sequence.wz.shape(0); i++) {
EXPECT_GT(control_sequence.wz(i), 0.0);
}
// Now, check the specifics of the minimum curvature constraint
EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6);
for (unsigned int i = 1; i != control_sequence.vx.shape(0); i++) {
EXPECT_TRUE(fabs(control_sequence.vx(i)) / fabs(control_sequence.wz(i)) >= 0.2);
}
// Check that Ackermann Drive is properly non-holonomic and parameterized
EXPECT_EQ(model->isHolonomic(), false);
// Check it cleanly destructs
model.reset();
}
TEST(MotionModelTests, AckermannReversingTest)
{
models::ControlSequence control_sequence;
models::ControlSequence control_sequence2;
models::State state;
int batches = 1000;
int timesteps = 50;
control_sequence.reset(timesteps); // populates with zeros
control_sequence2.reset(timesteps); // populates with zeros
state.reset(batches, timesteps); // populates with zeros
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
ParametersHandler param_handler(node);
std::unique_ptr<AckermannMotionModel> model =
std::make_unique<AckermannMotionModel>(&param_handler, node->get_name());
// Check that predict properly populates the trajectory velocities with the control velocities
state.cvx = 10 * xt::ones<float>({batches, timesteps});
state.cvy = 5 * xt::ones<float>({batches, timesteps});
state.cwz = 1 * xt::ones<float>({batches, timesteps});
// Manually set state index 0 from initial conditions which would be the speed of the robot
xt::view(state.vx, xt::all(), 0) = 10;
xt::view(state.wz, xt::all(), 0) = 1;
model->predict(state);
EXPECT_EQ(state.vx, state.cvx);
EXPECT_EQ(state.vy, xt::zeros<float>({batches, timesteps})); // non-holonomic
EXPECT_EQ(state.wz, state.cwz);
// Check that application of constraints are non-empty for Ackermann Drive
for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) {
float idx = static_cast<float>(i);
control_sequence.vx(i) = -idx * idx * idx; // now reversing
control_sequence.wz(i) = idx * idx * idx * idx;
}
models::ControlSequence initial_control_sequence = control_sequence;
model->applyConstraints(control_sequence);
// VX equal since this doesn't change, the WZ is reduced if breaking the constraint
EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx);
EXPECT_NE(initial_control_sequence.wz, control_sequence.wz);
for (unsigned int i = 1; i != control_sequence.wz.shape(0); i++) {
EXPECT_GT(control_sequence.wz(i), 0.0);
}
// Repeat with negative rotation direction
for (unsigned int i = 0; i != control_sequence2.vx.shape(0); i++) {
float idx = static_cast<float>(i);
control_sequence2.vx(i) = -idx * idx * idx; // now reversing
control_sequence2.wz(i) = -idx * idx * idx * idx;
}
models::ControlSequence initial_control_sequence2 = control_sequence2;
model->applyConstraints(control_sequence2);
// VX equal since this doesn't change, the WZ is reduced if breaking the constraint
EXPECT_EQ(initial_control_sequence2.vx, control_sequence2.vx);
EXPECT_NE(initial_control_sequence2.wz, control_sequence2.wz);
for (unsigned int i = 1; i != control_sequence2.wz.shape(0); i++) {
EXPECT_LT(control_sequence2.wz(i), 0.0);
}
// Now, check the specifics of the minimum curvature constraint
EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6);
for (unsigned int i = 1; i != control_sequence2.vx.shape(0); i++) {
EXPECT_TRUE(fabs(control_sequence2.vx(i)) / fabs(control_sequence2.wz(i)) >= 0.2);
}
// Check that Ackermann Drive is properly non-holonomic and parameterized
EXPECT_EQ(model->isHolonomic(), false);
// Check it cleanly destructs
model.reset();
}
navigation2/nav2_mppi_controller/test/noise_generator_test.cpp
+131
View File
@@ -0,0 +1,131 @@
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// 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 <chrono>
#include <thread>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "nav2_mppi_controller/tools/noise_generator.hpp"
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
#include "nav2_mppi_controller/models/optimizer_settings.hpp"
#include "nav2_mppi_controller/models/state.hpp"
#include "nav2_mppi_controller/models/control_sequence.hpp"
// Tests noise generator object
class RosLockGuard
{
public:
RosLockGuard() {rclcpp::init(0, nullptr);}
~RosLockGuard() {rclcpp::shutdown();}
};
RosLockGuard g_rclcpp;
using namespace mppi; // NOLINT
TEST(NoiseGeneratorTest, NoiseGeneratorLifecycle)
{
// Tests shuts down internal thread cleanly
NoiseGenerator generator;
mppi::models::OptimizerSettings settings;
settings.batch_size = 100;
settings.time_steps = 25;
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("node");
node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(false));
ParametersHandler handler(node);
generator.initialize(settings, false, "test_name", &handler);
generator.reset(settings, false);
generator.shutdown();
}
TEST(NoiseGeneratorTest, NoiseGeneratorMain)
{
// Tests shuts down internal thread cleanly
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("node");
node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(true));
ParametersHandler handler(node);
NoiseGenerator generator;
mppi::models::OptimizerSettings settings;
settings.batch_size = 100;
settings.time_steps = 25;
settings.sampling_std.vx = 0.1;
settings.sampling_std.vy = 0.1;
settings.sampling_std.wz = 0.1;
// Populate a potential control sequence
mppi::models::ControlSequence control_sequence;
control_sequence.reset(25);
for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) {
control_sequence.vx(i) = i;
control_sequence.vy(i) = i;
control_sequence.wz(i) = i;
}
mppi::models::State state;
state.reset(settings.batch_size, settings.time_steps);
// Request an update with no noise yet generated, should result in identical outputs
generator.initialize(settings, false, "test_name", &handler);
generator.reset(settings, false); // sets initial sizing and zeros out noises
generator.setNoisedControls(state, control_sequence);
EXPECT_EQ(state.cvx(0), 0);
EXPECT_EQ(state.cvy(0), 0);
EXPECT_EQ(state.cwz(0), 0);
EXPECT_EQ(state.cvx(9), 9);
EXPECT_EQ(state.cvy(9), 9);
EXPECT_EQ(state.cwz(9), 9);
// Request an update with noise requested
generator.generateNextNoises();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
generator.setNoisedControls(state, control_sequence);
EXPECT_NE(state.cvx(0), 0);
EXPECT_EQ(state.cvy(0), 0); // Not populated in non-holonomic
EXPECT_NE(state.cwz(0), 0);
EXPECT_NE(state.cvx(9), 9);
EXPECT_EQ(state.cvy(9), 9); // Not populated in non-holonomic
EXPECT_NE(state.cwz(9), 9);
EXPECT_NEAR(state.cvx(0), 0, 0.3);
EXPECT_NEAR(state.cvy(0), 0, 0.3);
EXPECT_NEAR(state.cwz(0), 0, 0.3);
EXPECT_NEAR(state.cvx(9), 9, 0.3);
EXPECT_NEAR(state.cvy(9), 9, 0.3);
EXPECT_NEAR(state.cwz(9), 9, 0.3);
// Test holonomic setting
generator.reset(settings, true); // Now holonomically
generator.generateNextNoises();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
generator.setNoisedControls(state, control_sequence);
EXPECT_NE(state.cvx(0), 0);
EXPECT_NE(state.cvy(0), 0); // Now populated in non-holonomic
EXPECT_NE(state.cwz(0), 0);
EXPECT_NE(state.cvx(9), 9);
EXPECT_NE(state.cvy(9), 9); // Now populated in non-holonomic
EXPECT_NE(state.cwz(9), 9);
EXPECT_NEAR(state.cvx(0), 0, 0.3);
EXPECT_NEAR(state.cvy(0), 0, 0.3);
EXPECT_NEAR(state.cwz(0), 0, 0.3);
EXPECT_NEAR(state.cvx(9), 9, 0.3);
EXPECT_NEAR(state.cvy(9), 9, 0.3);
EXPECT_NEAR(state.cwz(9), 9, 0.3);
generator.shutdown();
}
navigation2/nav2_mppi_controller/test/optimizer_smoke_test.cpp
+116
View File
@@ -0,0 +1,116 @@
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// 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 "gtest/gtest.h"
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/path.hpp>
#include <nav2_costmap_2d/cost_values.hpp>
#include <nav2_costmap_2d/costmap_2d.hpp>
#include <nav2_costmap_2d/costmap_2d_ros.hpp>
#include <nav2_core/goal_checker.hpp>
#include <xtensor/xarray.hpp>
#include <xtensor/xio.hpp>
#include <xtensor/xview.hpp>
#include "nav2_mppi_controller/optimizer.hpp"
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
#include "nav2_mppi_controller/motion_models.hpp"
#include "utils/utils.hpp"
class RosLockGuard
{
public:
RosLockGuard() {rclcpp::init(0, nullptr);}
~RosLockGuard() {rclcpp::shutdown();}
};
RosLockGuard g_rclcpp;
// Smoke tests the optimizer
class OptimizerSuite : public ::testing::TestWithParam<std::tuple<std::string,
std::vector<std::string>, bool>> {};
TEST_P(OptimizerSuite, OptimizerTest) {
auto [motion_model, critics, consider_footprint] = GetParam();
int batch_size = 400;
int time_steps = 15;
unsigned int path_points = 50u;
int iteration_count = 1;
double lookahead_distance = 10.0;
TestCostmapSettings costmap_settings{};
auto costmap_ros = getDummyCostmapRos(costmap_settings);
auto costmap = costmap_ros->getCostmap();
TestPose start_pose = costmap_settings.getCenterPose();
double path_step = costmap_settings.resolution;
TestPathSettings path_settings{start_pose, path_points, path_step, path_step};
TestOptimizerSettings optimizer_settings{batch_size, time_steps, iteration_count,
lookahead_distance, motion_model, consider_footprint};
unsigned int offset = 4;
unsigned int obstacle_size = offset * 2;
unsigned char obstacle_cost = 250;
auto [obst_x, obst_y] = costmap_settings.getCenterIJ();
obst_x = obst_x - offset;
obst_y = obst_y - offset;
addObstacle(costmap, {obst_x, obst_y, obstacle_size, obstacle_cost});
printInfo(optimizer_settings, path_settings, critics);
auto node = getDummyNode(optimizer_settings, critics);
auto parameters_handler = std::make_unique<mppi::ParametersHandler>(node);
auto optimizer = getDummyOptimizer(node, costmap_ros, parameters_handler.get());
// evalControl args
auto pose = getDummyPointStamped(node, start_pose);
auto velocity = getDummyTwist();
auto path = getIncrementalDummyPath(node, path_settings);
auto goal = path.poses.back().pose;
nav2_core::GoalChecker * dummy_goal_checker{nullptr};
EXPECT_NO_THROW(optimizer->evalControl(pose, velocity, path, goal, dummy_goal_checker));
}
INSTANTIATE_TEST_SUITE_P(
OptimizerTests,
OptimizerSuite,
::testing::Values(
std::make_tuple(
"Omni",
std::vector<std::string>(
{{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, {"PathAlignCritic"},
{"TwirlingCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}),
true),
std::make_tuple(
"DiffDrive",
std::vector<std::string>(
{{"GoalCritic"}, {"GoalAngleCritic"}, {"CostCritic"},
{"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}),
true),
std::make_tuple(
"Ackermann",
std::vector<std::string>(
{{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
{"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}),
true))
);
navigation2/nav2_mppi_controller/test/optimizer_unit_tests.cpp
+639
View File
@@ -0,0 +1,639 @@
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// 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 <chrono>
#include <thread>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_mppi_controller/optimizer.hpp"
// Tests main optimizer functions
class RosLockGuard
{
public:
RosLockGuard() {rclcpp::init(0, nullptr);}
~RosLockGuard() {rclcpp::shutdown();}
};
RosLockGuard g_rclcpp;
using namespace mppi; // NOLINT
using namespace mppi::critics; // NOLINT
using namespace mppi::utils; // NOLINT
using xt::evaluation_strategy::immediate;
class OptimizerTester : public Optimizer
{
public:
OptimizerTester()
: Optimizer() {}
void testSetDiffModel()
{
EXPECT_EQ(motion_model_.get(), nullptr);
EXPECT_NO_THROW(setMotionModel("DiffDrive"));
EXPECT_NE(motion_model_.get(), nullptr);
EXPECT_TRUE(dynamic_cast<DiffDriveMotionModel *>(motion_model_.get()));
EXPECT_FALSE(isHolonomic());
}
void testSetOmniModel()
{
EXPECT_EQ(motion_model_.get(), nullptr);
EXPECT_NO_THROW(setMotionModel("Omni"));
EXPECT_NE(motion_model_.get(), nullptr);
EXPECT_TRUE(dynamic_cast<OmniMotionModel *>(motion_model_.get()));
EXPECT_TRUE(isHolonomic());
}
void testSetAckModel()
{
EXPECT_EQ(motion_model_.get(), nullptr);
EXPECT_NO_THROW(setMotionModel("Ackermann"));
EXPECT_NE(motion_model_.get(), nullptr);
EXPECT_TRUE(dynamic_cast<AckermannMotionModel *>(motion_model_.get()));
EXPECT_FALSE(isHolonomic());
}
void testSetRandModel()
{
EXPECT_EQ(motion_model_.get(), nullptr);
try {
setMotionModel("Random");
FAIL();
} catch (...) {
SUCCEED();
}
EXPECT_EQ(motion_model_.get(), nullptr);
}
void resetMotionModel()
{
motion_model_.reset();
}
void setOffsetWrapper(const double freq)
{
return setOffset(freq);
}
bool getShiftControlSequence()
{
return settings_.shift_control_sequence;
}
void fillOptimizerWithGarbage()
{
state_.vx = 0.43432 * xt::ones<float>({1000, 10});
control_sequence_.vx = 342.0 * xt::ones<float>({30});
control_history_[0] = {43, 5646, 32432};
costs_ = 5.32 * xt::ones<float>({56453});
generated_trajectories_.x = 432.234 * xt::ones<float>({7865, 1});
}
void testReset()
{
reset();
EXPECT_EQ(state_.vx, xt::zeros<float>({1000, 50}));
EXPECT_EQ(control_sequence_.vx, xt::zeros<float>({50}));
EXPECT_EQ(control_history_[0].vx, 0.0);
EXPECT_EQ(control_history_[0].vy, 0.0);
EXPECT_NEAR(xt::sum(costs_, immediate)(), 0, 1e-6);
EXPECT_EQ(generated_trajectories_.x, xt::zeros<float>({1000, 50}));
}
bool fallbackWrapper(bool fail)
{
return fallback(fail);
}
void testPrepare(
const geometry_msgs::msg::PoseStamped & robot_pose,
const geometry_msgs::msg::Twist & robot_speed,
const nav_msgs::msg::Path & plan,
const geometry_msgs::msg::Pose & goal,
nav2_core::GoalChecker * goal_checker)
{
prepare(robot_pose, robot_speed, plan, goal, goal_checker);
EXPECT_EQ(critics_data_.goal_checker, nullptr);
EXPECT_NEAR(xt::sum(costs_, immediate)(), 0, 1e-6); // should be reset
EXPECT_FALSE(critics_data_.fail_flag); // should be reset
EXPECT_FALSE(critics_data_.motion_model->isHolonomic()); // object is valid + diff drive
EXPECT_FALSE(critics_data_.furthest_reached_path_point.has_value()); // val is not set
EXPECT_FALSE(critics_data_.path_pts_valid.has_value()); // val is not set
EXPECT_EQ(state_.pose.pose.position.x, 999);
EXPECT_EQ(state_.speed.linear.y, 4.0);
EXPECT_EQ(path_.x.shape(0), 17u);
}
void shiftControlSequenceWrapper()
{
return shiftControlSequence();
}
std::pair<double, double> getVelLimits()
{
auto & s = settings_;
return {s.constraints.vx_min, s.constraints.vx_max};
}
void applyControlSequenceConstraintsWrapper()
{
return applyControlSequenceConstraints();
}
models::ControlSequence & grabControlSequence()
{
return control_sequence_;
}
void testupdateStateVels()
{
// updateInitialStateVelocities
models::State state;
state.reset(1000, 50);
state.speed.linear.x = 5.0;
state.speed.linear.y = 1.0;
state.speed.angular.z = 6.0;
state.cvx = 0.75 * xt::ones<float>({1000, 50});
state.cvy = 0.5 * xt::ones<float>({1000, 50});
state.cwz = 0.1 * xt::ones<float>({1000, 50});
updateInitialStateVelocities(state);
EXPECT_NEAR(state.vx(0, 0), 5.0, 1e-6);
EXPECT_NEAR(state.vy(0, 0), 1.0, 1e-6);
EXPECT_NEAR(state.wz(0, 0), 6.0, 1e-6);
// propagateStateVelocitiesFromInitials
propagateStateVelocitiesFromInitials(state);
EXPECT_NEAR(state.vx(0, 0), 5.0, 1e-6);
EXPECT_NEAR(state.vy(0, 0), 1.0, 1e-6);
EXPECT_NEAR(state.wz(0, 0), 6.0, 1e-6);
EXPECT_NEAR(state.vx(0, 1), 0.75, 1e-6);
EXPECT_NEAR(state.vy(0, 1), 0.5, 1e-6);
EXPECT_NEAR(state.wz(0, 1), 0.1, 1e-6);
// Putting them together: updateStateVelocities
state.reset(1000, 50);
state.speed.linear.x = -5.0;
state.speed.linear.y = -1.0;
state.speed.angular.z = -6.0;
state.cvx = -0.75 * xt::ones<float>({1000, 50});
state.cvy = -0.5 * xt::ones<float>({1000, 50});
state.cwz = -0.1 * xt::ones<float>({1000, 50});
updateStateVelocities(state);
EXPECT_NEAR(state.vx(0, 0), -5.0, 1e-6);
EXPECT_NEAR(state.vy(0, 0), -1.0, 1e-6);
EXPECT_NEAR(state.wz(0, 0), -6.0, 1e-6);
EXPECT_NEAR(state.vx(0, 1), -0.75, 1e-6);
EXPECT_NEAR(state.vy(0, 1), -0.5, 1e-6);
EXPECT_NEAR(state.wz(0, 1), -0.1, 1e-6);
}
geometry_msgs::msg::TwistStamped getControlFromSequenceAsTwistWrapper()
{
builtin_interfaces::msg::Time stamp;
return getControlFromSequenceAsTwist(stamp);
}
void integrateStateVelocitiesWrapper(
models::Trajectories & traj,
const models::State & state)
{
return integrateStateVelocities(traj, state);
}
};
TEST(OptimizerTests, BasicInitializedFunctions)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
OptimizerTester optimizer_tester;
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
optimizer_tester.initialize(node, "mppic", costmap_ros, &param_handler);
// Should be empty of size batches x time steps
// and tests getting set params: time_steps, batch_size, controller_frequency
auto trajs = optimizer_tester.getGeneratedTrajectories();
EXPECT_EQ(trajs.x.shape(0), 1000u);
EXPECT_EQ(trajs.x.shape(1), 50u);
EXPECT_EQ(trajs.x, xt::zeros<float>({1000, 50}));
optimizer_tester.resetMotionModel();
optimizer_tester.testSetOmniModel();
auto traj = optimizer_tester.getOptimizedTrajectory();
EXPECT_EQ(traj(5, 0), 0.0); // x
EXPECT_EQ(traj(5, 1), 0.0); // y
EXPECT_EQ(traj(5, 2), 0.0); // yaw
EXPECT_EQ(traj.shape(0), 50u);
EXPECT_EQ(traj.shape(1), 3u);
optimizer_tester.reset();
optimizer_tester.shutdown();
}
TEST(OptimizerTests, TestOptimizerMotionModels)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
optimizer_tester.initialize(node, "mppic", costmap_ros, &param_handler);
// Diff Drive should be non-holonomic
optimizer_tester.resetMotionModel();
optimizer_tester.testSetDiffModel();
// Omni Drive should be holonomic
optimizer_tester.resetMotionModel();
optimizer_tester.testSetOmniModel();
// // Ackermann should be non-holonomic
optimizer_tester.resetMotionModel();
optimizer_tester.testSetAckModel();
// // Rand should fail
optimizer_tester.resetMotionModel();
optimizer_tester.testSetRandModel();
}
TEST(OptimizerTests, setOffsetTests)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
OptimizerTester optimizer_tester;
node->declare_parameter("mppic.model_dt", rclcpp::ParameterValue(0.1));
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
optimizer_tester.initialize(node, "mppic", costmap_ros, &param_handler);
// Test offsets are properly set based on relationship of model_dt and controller frequency
// Also tests getting set model_dt parameter.
EXPECT_THROW(optimizer_tester.setOffsetWrapper(1.0), std::runtime_error);
EXPECT_NO_THROW(optimizer_tester.setOffsetWrapper(30.0));
EXPECT_FALSE(optimizer_tester.getShiftControlSequence());
EXPECT_NO_THROW(optimizer_tester.setOffsetWrapper(10.0));
EXPECT_TRUE(optimizer_tester.getShiftControlSequence());
}
TEST(OptimizerTests, resetTests)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
OptimizerTester optimizer_tester;
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
optimizer_tester.initialize(node, "mppic", costmap_ros, &param_handler);
// Tests resetting the full state of all the functions after filling with garbage
optimizer_tester.fillOptimizerWithGarbage();
optimizer_tester.testReset();
}
TEST(OptimizerTests, FallbackTests)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
OptimizerTester optimizer_tester;
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
optimizer_tester.initialize(node, "mppic", costmap_ros, &param_handler);
// Test fallback logic, also tests getting set param retry_attempt_limit
// Because retry set to 2, it should attempt soft resets 2x before throwing exception
// for hard reset
EXPECT_FALSE(optimizer_tester.fallbackWrapper(false));
EXPECT_TRUE(optimizer_tester.fallbackWrapper(true));
EXPECT_TRUE(optimizer_tester.fallbackWrapper(true));
EXPECT_THROW(optimizer_tester.fallbackWrapper(true), std::runtime_error);
}
TEST(OptimizerTests, PrepareTests)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
OptimizerTester optimizer_tester;
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
optimizer_tester.initialize(node, "mppic", costmap_ros, &param_handler);
// Test Prepare function to set the state of the robot pose/speed on new cycle
// Populate the contents with things easily identifiable if correct
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = 999;
geometry_msgs::msg::Twist speed;
speed.linear.y = 4.0;
nav_msgs::msg::Path path;
geometry_msgs::msg::Pose goal;
path.poses.resize(17);
optimizer_tester.testPrepare(pose, speed, path, goal, nullptr);
}
TEST(OptimizerTests, shiftControlSequenceTests)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
OptimizerTester optimizer_tester;
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
optimizer_tester.initialize(node, "mppic", costmap_ros, &param_handler);
// Test shiftControlSequence by setting the 2nd value to something unique to neighbors
auto & sequence = optimizer_tester.grabControlSequence();
sequence.reset(100);
sequence.vx(0) = 9999;
sequence.vx(1) = 6;
sequence.vx(2) = 888;
sequence.vy(0) = 9999;
sequence.vy(1) = 6;
sequence.vy(2) = 888;
sequence.wz(0) = 9999;
sequence.wz(1) = 6;
sequence.wz(2) = 888;
optimizer_tester.resetMotionModel();
optimizer_tester.testSetOmniModel();
optimizer_tester.shiftControlSequenceWrapper();
EXPECT_EQ(sequence.vx(0), 6);
EXPECT_EQ(sequence.vy(0), 6);
EXPECT_EQ(sequence.wz(0), 6);
EXPECT_EQ(sequence.vx(1), 888);
EXPECT_EQ(sequence.vy(1), 888);
EXPECT_EQ(sequence.wz(1), 888);
EXPECT_EQ(sequence.vx(2), 0);
EXPECT_EQ(sequence.vy(2), 0);
EXPECT_EQ(sequence.wz(2), 0);
}
TEST(OptimizerTests, SpeedLimitTests)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
OptimizerTester optimizer_tester;
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
optimizer_tester.initialize(node, "mppic", costmap_ros, &param_handler);
// Test Speed limits API
auto [v_min, v_max] = optimizer_tester.getVelLimits();
EXPECT_EQ(v_max, 0.5f);
EXPECT_EQ(v_min, -0.35f);
optimizer_tester.setSpeedLimit(0, false);
auto [v_min2, v_max2] = optimizer_tester.getVelLimits();
EXPECT_EQ(v_max2, 0.5f);
EXPECT_EQ(v_min2, -0.35f);
optimizer_tester.setSpeedLimit(50.0, true);
auto [v_min3, v_max3] = optimizer_tester.getVelLimits();
EXPECT_NEAR(v_max3, 0.5 / 2.0, 1e-3);
EXPECT_NEAR(v_min3, -0.35 / 2.0, 1e-3);
optimizer_tester.setSpeedLimit(0, true);
auto [v_min4, v_max4] = optimizer_tester.getVelLimits();
EXPECT_EQ(v_max4, 0.5f);
EXPECT_EQ(v_min4, -0.35f);
optimizer_tester.setSpeedLimit(0.75, false);
auto [v_min5, v_max5] = optimizer_tester.getVelLimits();
EXPECT_NEAR(v_max5, 0.75, 1e-3);
EXPECT_NEAR(v_min5, -0.5249, 1e-2);
}
TEST(OptimizerTests, applyControlSequenceConstraintsTests)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
OptimizerTester optimizer_tester;
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
node->declare_parameter("mppic.vx_max", rclcpp::ParameterValue(1.0));
node->declare_parameter("mppic.vx_min", rclcpp::ParameterValue(-1.0));
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
optimizer_tester.initialize(node, "mppic", costmap_ros, &param_handler);
// Test constraints being applied to ensure feasibility of trajectories
// Also tests param get of set vx/vy/wz min/maxes
// Set model to omni to consider holonomic vy elements
// Ack is not tested here because `applyConstraints` is covered in detail
// in motion_models_test.cpp
optimizer_tester.resetMotionModel();
optimizer_tester.testSetOmniModel();
auto & sequence = optimizer_tester.grabControlSequence();
// Test boundary of limits
sequence.vx = xt::ones<float>({50});
sequence.vy = 0.75 * xt::ones<float>({50});
sequence.wz = 2.0 * xt::ones<float>({50});
optimizer_tester.applyControlSequenceConstraintsWrapper();
EXPECT_EQ(sequence.vx, xt::ones<float>({50}));
EXPECT_EQ(sequence.vy, 0.75 * xt::ones<float>({50}));
EXPECT_EQ(sequence.wz, 2.0 * xt::ones<float>({50}));
// Test breaking limits sets to maximum
sequence.vx = 5.0 * xt::ones<float>({50});
sequence.vy = 5.0 * xt::ones<float>({50});
sequence.wz = 5.0 * xt::ones<float>({50});
optimizer_tester.applyControlSequenceConstraintsWrapper();
EXPECT_EQ(sequence.vx, xt::ones<float>({50}));
EXPECT_EQ(sequence.vy, 0.75 * xt::ones<float>({50}));
EXPECT_EQ(sequence.wz, 2.0 * xt::ones<float>({50}));
// Test breaking limits sets to minimum
sequence.vx = -5.0 * xt::ones<float>({50});
sequence.vy = -5.0 * xt::ones<float>({50});
sequence.wz = -5.0 * xt::ones<float>({50});
optimizer_tester.applyControlSequenceConstraintsWrapper();
EXPECT_EQ(sequence.vx, -1.0 * xt::ones<float>({50}));
EXPECT_EQ(sequence.vy, -0.75 * xt::ones<float>({50}));
EXPECT_EQ(sequence.wz, -2.0 * xt::ones<float>({50}));
}
TEST(OptimizerTests, updateStateVelocitiesTests)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
OptimizerTester optimizer_tester;
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
node->declare_parameter("mppic.vx_max", rclcpp::ParameterValue(1.0));
node->declare_parameter("mppic.vx_min", rclcpp::ParameterValue(-1.0));
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
optimizer_tester.initialize(node, "mppic", costmap_ros, &param_handler);
// Test settings of the state to the initial robot speed to start rollout
// Set model to omni to consider holonomic vy elements
optimizer_tester.resetMotionModel();
optimizer_tester.testSetOmniModel();
optimizer_tester.testupdateStateVels();
}
TEST(OptimizerTests, getControlFromSequenceAsTwistTests)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
OptimizerTester optimizer_tester;
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
node->declare_parameter("mppic.vx_max", rclcpp::ParameterValue(1.0));
node->declare_parameter("mppic.vx_min", rclcpp::ParameterValue(-1.0));
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
optimizer_tester.initialize(node, "mppic", costmap_ros, &param_handler);
// Test conversion of control sequence into a Twist command to execute
auto & sequence = optimizer_tester.grabControlSequence();
sequence.vx = 0.25 * xt::ones<float>({10});
sequence.vy = 0.5 * xt::ones<float>({10});
sequence.wz = 0.1 * xt::ones<float>({10});
auto diff_t = optimizer_tester.getControlFromSequenceAsTwistWrapper();
EXPECT_NEAR(diff_t.twist.linear.x, 0.25, 1e-6);
EXPECT_NEAR(diff_t.twist.linear.y, 0.0, 1e-6); // Y should not be populated
EXPECT_NEAR(diff_t.twist.angular.z, 0.1, 1e-6);
// Set model to omni to consider holonomic vy elements
optimizer_tester.resetMotionModel();
optimizer_tester.testSetOmniModel();
auto omni_t = optimizer_tester.getControlFromSequenceAsTwistWrapper();
EXPECT_NEAR(omni_t.twist.linear.x, 0.25, 1e-6);
EXPECT_NEAR(omni_t.twist.linear.y, 0.5, 1e-6); // Now it should be
EXPECT_NEAR(omni_t.twist.angular.z, 0.1, 1e-6);
}
TEST(OptimizerTests, integrateStateVelocitiesTests)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
OptimizerTester optimizer_tester;
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
optimizer_tester.initialize(node, "mppic", costmap_ros, &param_handler);
optimizer_tester.resetMotionModel();
optimizer_tester.testSetOmniModel();
// Test integration of velocities for trajectory rollout poses
// Give it a couple of easy const traj and check rollout, start from 0
models::State state;
state.reset(1000, 50);
models::Trajectories traj;
state.vx = 0.1 * xt::ones<float>({1000, 50});
xt::view(state.vx, xt::all(), 0) = xt::zeros<float>({1000});
state.vy = xt::zeros<float>({1000, 50});
state.wz = xt::zeros<float>({1000, 50});
optimizer_tester.integrateStateVelocitiesWrapper(traj, state);
EXPECT_EQ(traj.y, xt::zeros<float>({1000, 50}));
EXPECT_EQ(traj.yaws, xt::zeros<float>({1000, 50}));
for (unsigned int i = 0; i != traj.x.shape(1); i++) {
EXPECT_NEAR(traj.x(1, i), i * 0.1 /*vel*/ * 0.1 /*dt*/, 1e-3);
}
// Give it a bit of a more complex trajectory to crunch
state.vy = 0.2 * xt::ones<float>({1000, 50});
xt::view(state.vy, xt::all(), 0) = xt::zeros<float>({1000});
optimizer_tester.integrateStateVelocitiesWrapper(traj, state);
EXPECT_EQ(traj.yaws, xt::zeros<float>({1000, 50}));
for (unsigned int i = 0; i != traj.x.shape(1); i++) {
EXPECT_NEAR(traj.x(1, i), i * 0.1 /*vel*/ * 0.1 /*dt*/, 1e-3);
EXPECT_NEAR(traj.y(1, i), i * 0.2 /*vel*/ * 0.1 /*dt*/, 1e-3);
}
// Lets add some angular motion to the mix
state.vy = xt::zeros<float>({1000, 50});
state.wz = 0.2 * xt::ones<float>({1000, 50});
xt::view(state.wz, xt::all(), 0) = xt::zeros<float>({1000});
optimizer_tester.integrateStateVelocitiesWrapper(traj, state);
float x = 0;
float y = 0;
for (unsigned int i = 1; i != traj.x.shape(1); i++) {
std::cout << i << std::endl;
x += (0.1 /*vx*/ * cos(0.2 /*wz*/ * 0.1 /*model_dt*/ * (i - 1))) * 0.1 /*model_dt*/;
y += (0.1 /*vx*/ * sin(0.2 /*wz*/ * 0.1 /*model_dt*/ * (i - 1))) * 0.1 /*model_dt*/;
EXPECT_NEAR(traj.x(1, i), x, 1e-6);
EXPECT_NEAR(traj.y(1, i), y, 1e-6);
}
}
navigation2/nav2_mppi_controller/test/parameter_handler_test.cpp
+181
View File
@@ -0,0 +1,181 @@
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// 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 <chrono>
#include <thread>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
// Tests parameter handler object
class RosLockGuard
{
public:
RosLockGuard() {rclcpp::init(0, nullptr);}
~RosLockGuard() {rclcpp::shutdown();}
};
RosLockGuard g_rclcpp;
using namespace mppi; // NOLINT
class ParametersHandlerWrapper : public ParametersHandler
{
public:
ParametersHandlerWrapper() = default;
explicit ParametersHandlerWrapper(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent)
: ParametersHandler(parent) {}
template<typename T>
auto asWrapped(rclcpp::Parameter parameter)
{
return ParametersHandler::as<T>(parameter);
}
};
using namespace mppi; // NOLINT
TEST(ParameterHandlerTest, asTypeConversionTest)
{
ParametersHandlerWrapper a;
rclcpp::Parameter int_p("int_parameter", rclcpp::ParameterValue(1));
rclcpp::Parameter double_p("double_parameter", rclcpp::ParameterValue(10.0));
rclcpp::Parameter bool_p("bool_parameter", rclcpp::ParameterValue(false));
rclcpp::Parameter string_p("string_parameter", rclcpp::ParameterValue(std::string("hello")));
rclcpp::Parameter intv_p("intv_parameter", rclcpp::ParameterValue(std::vector<int>{1}));
rclcpp::Parameter doublev_p(
"doublev_parameter", rclcpp::ParameterValue(std::vector<double>{10.0}));
rclcpp::Parameter boolv_p("boolv_parameter", rclcpp::ParameterValue(std::vector<bool>{false}));
rclcpp::Parameter stringv_p(
"stringv_parameter", rclcpp::ParameterValue(std::vector<std::string>{std::string("hello")}));
EXPECT_EQ(a.asWrapped<int>(int_p), 1);
EXPECT_EQ(a.asWrapped<double>(double_p), 10.0);
EXPECT_EQ(a.asWrapped<bool>(bool_p), false);
EXPECT_EQ(a.asWrapped<std::string>(string_p), std::string("hello"));
EXPECT_EQ(a.asWrapped<std::vector<int64_t>>(intv_p)[0], 1);
EXPECT_EQ(a.asWrapped<std::vector<double>>(doublev_p)[0], 10.0);
EXPECT_EQ(a.asWrapped<std::vector<bool>>(boolv_p)[0], false);
EXPECT_EQ(a.asWrapped<std::vector<std::string>>(stringv_p)[0], std::string("hello"));
}
TEST(ParameterHandlerTest, PrePostDynamicCallbackTest)
{
bool pre_triggered = false, post_triggered = false, dynamic_triggered = false;
auto preCb = [&]() {
if (post_triggered) {
throw std::runtime_error("Post-callback triggered before pre-callback!");
}
pre_triggered = true;
};
auto postCb = [&]() {
if (!pre_triggered) {
throw std::runtime_error("Pre-callback was not triggered before post-callback!");
}
post_triggered = true;
};
auto dynamicCb = [&](const rclcpp::Parameter & /*param*/) {
dynamic_triggered = true;
};
rclcpp::Parameter random_param("blah_blah", rclcpp::ParameterValue(true));
rclcpp::Parameter random_param2("use_sim_time", rclcpp::ParameterValue(true));
bool val = false;
ParametersHandlerWrapper a;
a.addPreCallback(preCb);
a.addPostCallback(postCb);
a.addDynamicParamCallback("use_sim_time", dynamicCb);
a.setDynamicParamCallback(val, "blah_blah");
// Dynamic callback should not trigger, wrong parameter, but val should be updated
a.dynamicParamsCallback(std::vector<rclcpp::Parameter>{random_param});
EXPECT_FALSE(dynamic_triggered);
EXPECT_TRUE(pre_triggered);
EXPECT_TRUE(post_triggered);
EXPECT_TRUE(val);
// Now dynamic parameter bool should be updated, right param called!
pre_triggered = false, post_triggered = false;
a.dynamicParamsCallback(std::vector<rclcpp::Parameter>{random_param2});
EXPECT_TRUE(dynamic_triggered);
EXPECT_TRUE(pre_triggered);
EXPECT_TRUE(post_triggered);
}
TEST(ParameterHandlerTest, GetSystemParamsTest)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
node->declare_parameter("param1", rclcpp::ParameterValue(true));
node->declare_parameter("ns.param2", rclcpp::ParameterValue(7));
// Get parameters in global namespace and in subnamespaces
ParametersHandler handler(node);
auto getParamer = handler.getParamGetter("");
bool p1 = false;
int p2 = 0;
getParamer(p1, "param1", false);
getParamer(p2, "ns.param2", 0);
EXPECT_EQ(p1, true);
EXPECT_EQ(p2, 7);
// Get parameters in subnamespaces using name semantics of getter
auto getParamer2 = handler.getParamGetter("ns");
p2 = 0;
getParamer2(p2, "param2", 0);
EXPECT_EQ(p2, 7);
}
TEST(ParameterHandlerTest, DynamicAndStaticParametersTest)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
node->declare_parameter("dynamic_int", rclcpp::ParameterValue(7));
node->declare_parameter("static_int", rclcpp::ParameterValue(7));
ParametersHandlerWrapper handler(node);
handler.start();
// Get parameters and check they have initial values
auto getParamer = handler.getParamGetter("");
int p1 = 0, p2 = 0;
getParamer(p1, "dynamic_int", 0, ParameterType::Dynamic);
getParamer(p2, "static_int", 0, ParameterType::Static);
EXPECT_EQ(p1, 7);
EXPECT_EQ(p2, 7);
// Now change them both via dynamic parameters
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
node->get_node_base_interface(), node->get_node_topics_interface(),
node->get_node_graph_interface(),
node->get_node_services_interface());
auto results = rec_param->set_parameters_atomically(
{rclcpp::Parameter("dynamic_int", 10),
rclcpp::Parameter("static_int", 10)});
rclcpp::spin_until_future_complete(
node->get_node_base_interface(),
results);
// Now, only param1 should change, param 2 should be the same
EXPECT_EQ(p1, 10);
EXPECT_EQ(p2, 7);
}
navigation2/nav2_mppi_controller/test/path_handler_test.cpp
+249
View File
@@ -0,0 +1,249 @@
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// 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 <chrono>
#include <thread>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_mppi_controller/tools/path_handler.hpp"
#include "tf2_ros/transform_broadcaster.h"
// Tests path handling
class RosLockGuard
{
public:
RosLockGuard() {rclcpp::init(0, nullptr);}
~RosLockGuard() {rclcpp::shutdown();}
};
RosLockGuard g_rclcpp;
using namespace mppi; // NOLINT
class PathHandlerWrapper : public PathHandler
{
public:
PathHandlerWrapper()
: PathHandler() {}
void pruneGlobalPlanWrapper(nav_msgs::msg::Path & path, const PathIterator end)
{
return prunePlan(path, end);
}
double getMaxCostmapDistWrapper()
{
return getMaxCostmapDist();
}
std::pair<nav_msgs::msg::Path, PathIterator>
getGlobalPlanConsideringBoundsInCostmapFrameWrapper(const geometry_msgs::msg::PoseStamped & pose)
{
return getGlobalPlanConsideringBoundsInCostmapFrame(pose);
}
bool transformPoseWrapper(
const std::string & frame, const geometry_msgs::msg::PoseStamped & in_pose,
geometry_msgs::msg::PoseStamped & out_pose) const
{
return transformPose(frame, in_pose, out_pose);
}
geometry_msgs::msg::PoseStamped transformToGlobalPlanFrameWrapper(
const geometry_msgs::msg::PoseStamped & pose)
{
return transformToGlobalPlanFrame(pose);
}
void setGlobalPlanUpToInversion(const nav_msgs::msg::Path & path)
{
global_plan_up_to_inversion_ = path;
}
bool isWithinInversionTolerancesWrapper(const geometry_msgs::msg::PoseStamped & robot_pose)
{
return isWithinInversionTolerances(robot_pose);
}
nav_msgs::msg::Path & getInvertedPath()
{
return global_plan_up_to_inversion_;
}
};
TEST(PathHandlerTests, GetAndPrunePath)
{
nav_msgs::msg::Path path;
PathHandlerWrapper handler;
path.header.frame_id = "fkframe";
path.poses.resize(11);
handler.setPath(path);
auto & rtn_path = handler.getPath();
EXPECT_EQ(path.header.frame_id, rtn_path.header.frame_id);
EXPECT_EQ(path.poses.size(), rtn_path.poses.size());
PathIterator it = rtn_path.poses.begin() + 5;
handler.pruneGlobalPlanWrapper(rtn_path, it);
auto rtn2_path = handler.getPath();
EXPECT_EQ(rtn2_path.poses.size(), 6u);
}
TEST(PathHandlerTests, TestBounds)
{
PathHandlerWrapper handler;
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");
auto results = costmap_ros->set_parameters_atomically(
{rclcpp::Parameter("global_frame", "odom"),
rclcpp::Parameter("robot_base_frame", "base_link")});
ParametersHandler param_handler(node);
rclcpp_lifecycle::State state;
costmap_ros->on_configure(state);
// Test initialization and getting costmap basic metadata
handler.initialize(node, "dummy", costmap_ros, costmap_ros->getTfBuffer(), &param_handler);
EXPECT_EQ(handler.getMaxCostmapDistWrapper(), 2.5);
// Set tf between map odom and base_link
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_ =
std::make_unique<tf2_ros::TransformBroadcaster>(node);
geometry_msgs::msg::TransformStamped t;
t.header.frame_id = "map";
t.child_frame_id = "base_link";
tf_broadcaster_->sendTransform(t);
t.header.frame_id = "map";
t.child_frame_id = "odom";
tf_broadcaster_->sendTransform(t);
// Test getting the global plans within a bounds window
nav_msgs::msg::Path path;
path.header.frame_id = "map";
path.poses.resize(100);
for (unsigned int i = 0; i != path.poses.size(); i++) {
path.poses[i].pose.position.x = i;
path.poses[i].header.frame_id = "map";
}
geometry_msgs::msg::PoseStamped robot_pose;
robot_pose.header.frame_id = "odom";
robot_pose.pose.position.x = 25.0;
handler.setPath(path);
auto [transformed_plan, closest] =
handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(robot_pose);
auto & path_inverted = handler.getInvertedPath();
EXPECT_EQ(closest - path_inverted.poses.begin(), 25);
handler.pruneGlobalPlanWrapper(path_inverted, closest);
auto & path_pruned = handler.getInvertedPath();
EXPECT_EQ(path_pruned.poses.size(), 75u);
}
TEST(PathHandlerTests, TestTransforms)
{
PathHandlerWrapper handler;
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");
ParametersHandler param_handler(node);
rclcpp_lifecycle::State state;
costmap_ros->on_configure(state);
// Test basic transformations and path handling
handler.initialize(node, "dummy", costmap_ros, costmap_ros->getTfBuffer(), &param_handler);
// Set tf between map odom and base_link
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_ =
std::make_unique<tf2_ros::TransformBroadcaster>(node);
geometry_msgs::msg::TransformStamped t;
t.header.frame_id = "map";
t.child_frame_id = "base_link";
tf_broadcaster_->sendTransform(t);
t.header.frame_id = "map";
t.child_frame_id = "odom";
tf_broadcaster_->sendTransform(t);
nav_msgs::msg::Path path;
path.header.frame_id = "map";
path.poses.resize(100);
for (unsigned int i = 0; i != path.poses.size(); i++) {
path.poses[i].pose.position.x = i;
path.poses[i].header.frame_id = "map";
}
geometry_msgs::msg::PoseStamped robot_pose, output_pose;
robot_pose.header.frame_id = "odom";
robot_pose.pose.position.x = 2.5;
EXPECT_TRUE(handler.transformPoseWrapper("map", robot_pose, output_pose));
EXPECT_EQ(output_pose.pose.position.x, 2.5);
EXPECT_THROW(handler.transformToGlobalPlanFrameWrapper(robot_pose), std::runtime_error);
handler.setPath(path);
EXPECT_NO_THROW(handler.transformToGlobalPlanFrameWrapper(robot_pose));
auto [path_out, closest] =
handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(robot_pose);
// Put it all together
auto final_path = handler.transformPath(robot_pose);
EXPECT_EQ(final_path.poses.size(), path_out.poses.size());
}
TEST(PathHandlerTests, TestInversionToleranceChecks)
{
nav_msgs::msg::Path path;
for (unsigned int i = 0; i != 10; i++) {
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = static_cast<double>(i);
path.poses.push_back(pose);
}
path.poses.back().pose.orientation.w = 1;
PathHandlerWrapper handler;
handler.setGlobalPlanUpToInversion(path);
// Not near (0,0)
geometry_msgs::msg::PoseStamped robot_pose;
EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose));
// Exactly on top of it
robot_pose.pose.position.x = 9;
robot_pose.pose.orientation.w = 1.0;
EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose));
// Laterally of it
robot_pose.pose.position.y = 9;
EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose));
// On top but off angled
robot_pose.pose.position.y = 0;
robot_pose.pose.orientation.z = 0.8509035;
robot_pose.pose.orientation.w = 0.525322;
EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose));
// On top but off angled within tolerances
robot_pose.pose.position.y = 0;
robot_pose.pose.orientation.w = 0.9961947;
robot_pose.pose.orientation.z = 0.0871558;
EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose));
// Offset spatially + off angled but both within tolerances
robot_pose.pose.position.x = 9.10;
EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose));
}
navigation2/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp
+155
View File
@@ -0,0 +1,155 @@
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// 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 <chrono>
#include <thread>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_mppi_controller/tools/trajectory_visualizer.hpp"
// Tests trajectory visualization
class RosLockGuard
{
public:
RosLockGuard() {rclcpp::init(0, nullptr);}
~RosLockGuard() {rclcpp::shutdown();}
};
RosLockGuard g_rclcpp;
using namespace mppi; // NOLINT
TEST(TrajectoryVisualizerTests, StateTransition)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
auto parameters_handler = std::make_unique<ParametersHandler>(node);
TrajectoryVisualizer vis;
vis.on_configure(node, "my_name", "map", parameters_handler.get());
vis.on_activate();
vis.on_deactivate();
vis.on_cleanup();
}
TEST(TrajectoryVisualizerTests, VisPathRepub)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
auto parameters_handler = std::make_unique<ParametersHandler>(node);
nav_msgs::msg::Path recieved_path;
nav_msgs::msg::Path pub_path;
pub_path.header.frame_id = "fake_frame";
pub_path.poses.resize(5);
auto my_sub = node->create_subscription<nav_msgs::msg::Path>(
"transformed_global_plan", 10,
[&](const nav_msgs::msg::Path msg) {recieved_path = msg;});
TrajectoryVisualizer vis;
vis.on_configure(node, "my_name", "map", parameters_handler.get());
vis.on_activate();
vis.visualize(pub_path);
rclcpp::spin_some(node->get_node_base_interface());
EXPECT_EQ(recieved_path.poses.size(), 5u);
EXPECT_EQ(recieved_path.header.frame_id, "fake_frame");
}
TEST(TrajectoryVisualizerTests, VisOptimalTrajectory)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
auto parameters_handler = std::make_unique<ParametersHandler>(node);
visualization_msgs::msg::MarkerArray recieved_msg;
auto my_sub = node->create_subscription<visualization_msgs::msg::MarkerArray>(
"/trajectories", 10,
[&](const visualization_msgs::msg::MarkerArray msg) {recieved_msg = msg;});
// optimal_trajectory empty, should fail to publish
xt::xtensor<float, 2> optimal_trajectory;
TrajectoryVisualizer vis;
vis.on_configure(node, "my_name", "fkmap", parameters_handler.get());
vis.on_activate();
vis.add(optimal_trajectory, "Optimal Trajectory");
nav_msgs::msg::Path bogus_path;
vis.visualize(bogus_path);
rclcpp::spin_some(node->get_node_base_interface());
EXPECT_EQ(recieved_msg.markers.size(), 0u);
// Now populated with content, should publish
optimal_trajectory = xt::ones<float>({20, 2});
vis.add(optimal_trajectory, "Optimal Trajectory");
vis.visualize(bogus_path);
rclcpp::spin_some(node->get_node_base_interface());
// Should have 20 trajectory points in the map frame
EXPECT_EQ(recieved_msg.markers.size(), 20u);
EXPECT_EQ(recieved_msg.markers[0].header.frame_id, "fkmap");
// Check IDs are properly populated
EXPECT_EQ(recieved_msg.markers[0].id, 0);
EXPECT_EQ(recieved_msg.markers[1].id, 1);
EXPECT_EQ(recieved_msg.markers[10].id, 10);
// Check poses are correct
EXPECT_EQ(recieved_msg.markers[0].pose.position.x, 1);
EXPECT_EQ(recieved_msg.markers[0].pose.position.y, 1);
EXPECT_EQ(recieved_msg.markers[0].pose.position.z, 0.06);
// Check that scales are rational
EXPECT_EQ(recieved_msg.markers[0].scale.x, 0.03);
EXPECT_EQ(recieved_msg.markers[0].scale.y, 0.03);
EXPECT_EQ(recieved_msg.markers[0].scale.z, 0.07);
EXPECT_EQ(recieved_msg.markers[19].scale.x, 0.07);
EXPECT_EQ(recieved_msg.markers[19].scale.y, 0.07);
EXPECT_EQ(recieved_msg.markers[19].scale.z, 0.09);
// Check that the colors are rational
for (unsigned int i = 0; i != recieved_msg.markers.size() - 1; i++) {
EXPECT_LT(recieved_msg.markers[i].color.g, recieved_msg.markers[i + 1].color.g);
EXPECT_LT(recieved_msg.markers[i].color.b, recieved_msg.markers[i + 1].color.b);
EXPECT_EQ(recieved_msg.markers[i].color.r, recieved_msg.markers[i + 1].color.r);
EXPECT_EQ(recieved_msg.markers[i].color.a, recieved_msg.markers[i + 1].color.a);
}
}
TEST(TrajectoryVisualizerTests, VisCandidateTrajectories)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
auto parameters_handler = std::make_unique<ParametersHandler>(node);
visualization_msgs::msg::MarkerArray recieved_msg;
auto my_sub = node->create_subscription<visualization_msgs::msg::MarkerArray>(
"/trajectories", 10,
[&](const visualization_msgs::msg::MarkerArray msg) {recieved_msg = msg;});
models::Trajectories candidate_trajectories;
candidate_trajectories.x = xt::ones<float>({200, 12});
candidate_trajectories.y = xt::ones<float>({200, 12});
candidate_trajectories.yaws = xt::ones<float>({200, 12});
TrajectoryVisualizer vis;
vis.on_configure(node, "my_name", "fkmap", parameters_handler.get());
vis.on_activate();
vis.add(candidate_trajectories, "Candidate Trajectories");
nav_msgs::msg::Path bogus_path;
vis.visualize(bogus_path);
rclcpp::spin_some(node->get_node_base_interface());
// 40 * 4, for 5 trajectory steps + 3 point steps
EXPECT_EQ(recieved_msg.markers.size(), 160u);
}
navigation2/nav2_mppi_controller/test/utils/factory.hpp
+247
View File
@@ -0,0 +1,247 @@
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// 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.
#pragma once
#include <memory>
#include <string>
#include <vector>
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/path.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include "nav2_mppi_controller/motion_models.hpp"
#include "nav2_mppi_controller/optimizer.hpp"
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
#include "nav2_mppi_controller/controller.hpp"
#include "models.hpp"
namespace detail
{
template<typename TMessage, typename TNode>
void setHeader(TMessage && msg, TNode node, std::string frame)
{
auto time = node->get_clock()->now();
msg.header.frame_id = frame;
msg.header.stamp = time;
}
} // namespace detail
/**
* Adds some parameters for the optimizer to a special container.
*
* @param params_ container for optimizer's parameters.
*/
void setUpOptimizerParams(
const TestOptimizerSettings & s,
const std::vector<std::string> & critics,
std::vector<rclcpp::Parameter> & params_, std::string node_name = std::string("dummy"))
{
constexpr double dummy_freq = 50.0;
params_.emplace_back(rclcpp::Parameter(node_name + ".iteration_count", s.iteration_count));
params_.emplace_back(rclcpp::Parameter(node_name + ".batch_size", s.batch_size));
params_.emplace_back(rclcpp::Parameter(node_name + ".time_steps", s.time_steps));
params_.emplace_back(rclcpp::Parameter(node_name + ".lookahead_dist", s.lookahead_distance));
params_.emplace_back(rclcpp::Parameter(node_name + ".motion_model", s.motion_model));
params_.emplace_back(rclcpp::Parameter(node_name + ".critics", critics));
params_.emplace_back(rclcpp::Parameter("controller_frequency", dummy_freq));
}
void setUpControllerParams(
bool visualize, std::vector<rclcpp::Parameter> & params_,
std::string node_name = std::string("dummy"))
{
double dummy_freq = 50.0;
params_.emplace_back(rclcpp::Parameter(node_name + ".visualize", visualize));
params_.emplace_back(rclcpp::Parameter("controller_frequency", dummy_freq));
}
rclcpp::NodeOptions getOptimizerOptions(
TestOptimizerSettings s,
const std::vector<std::string> & critics)
{
std::vector<rclcpp::Parameter> params;
rclcpp::NodeOptions options;
setUpOptimizerParams(s, critics, params);
options.parameter_overrides(params);
return options;
}
geometry_msgs::msg::Point getDummyPoint(double x, double y)
{
geometry_msgs::msg::Point point;
point.x = x;
point.y = y;
point.z = 0;
return point;
}
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> getDummyCostmapRos()
{
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("cost_map_node");
costmap_ros->on_configure(rclcpp_lifecycle::State{});
return costmap_ros;
}
std::shared_ptr<nav2_costmap_2d::Costmap2D> getDummyCostmap(TestCostmapSettings s)
{
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2D>(
s.cells_x, s.cells_y, s.resolution, s.origin_x, s.origin_y, s.cost_map_default_value);
return costmap;
}
std::vector<geometry_msgs::msg::Point> getDummySquareFootprint(double a)
{
return {getDummyPoint(a, a), getDummyPoint(-a, -a), getDummyPoint(a, -a), getDummyPoint(-a, a)};
}
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> getDummyCostmapRos(TestCostmapSettings s)
{
auto costmap_ros = getDummyCostmapRos();
auto costmap_ptr = costmap_ros->getCostmap();
auto costmap = getDummyCostmap(s);
*(costmap_ptr) = *costmap;
costmap_ros->setRobotFootprint(getDummySquareFootprint(s.footprint_size));
return costmap_ros;
}
std::shared_ptr<rclcpp_lifecycle::LifecycleNode>
getDummyNode(
TestOptimizerSettings s, std::vector<std::string> critics,
std::string node_name = std::string("dummy"))
{
auto node =
std::make_shared<rclcpp_lifecycle::LifecycleNode>(node_name, getOptimizerOptions(s, critics));
return node;
}
std::shared_ptr<rclcpp_lifecycle::LifecycleNode>
getDummyNode(rclcpp::NodeOptions options, std::string node_name = std::string("dummy"))
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(node_name, options);
return node;
}
template<typename TNode, typename TCostMap, typename TParamHandler>
std::shared_ptr<mppi::Optimizer> getDummyOptimizer(
TNode node, TCostMap costmap_ros,
TParamHandler * params_handler)
{
std::shared_ptr<mppi::Optimizer> optimizer = std::make_shared<mppi::Optimizer>();
std::weak_ptr<rclcpp_lifecycle::LifecycleNode> weak_ptr_node{node};
optimizer->initialize(weak_ptr_node, node->get_name(), costmap_ros, params_handler);
return optimizer;
}
template<typename TNode, typename TCostMap, typename TFBuffer, typename TParamHandler>
mppi::PathHandler getDummyPathHandler(
TNode node, TCostMap costmap_ros, TFBuffer tf_buffer,
TParamHandler * params_handler)
{
mppi::PathHandler path_handler;
std::weak_ptr<rclcpp_lifecycle::LifecycleNode> weak_ptr_node{node};
path_handler.initialize(weak_ptr_node, node->get_name(), costmap_ros, tf_buffer, params_handler);
return path_handler;
}
template<typename TNode, typename TCostMap, typename TFBuffer>
std::shared_ptr<nav2_mppi_controller::MPPIController> getDummyController(
TNode node, TFBuffer tf_buffer,
TCostMap costmap_ros)
{
auto controller = std::make_shared<nav2_mppi_controller::MPPIController>();
std::weak_ptr<rclcpp_lifecycle::LifecycleNode> weak_ptr_node{node};
controller->configure(weak_ptr_node, node->get_name(), tf_buffer, costmap_ros);
controller->activate();
return controller;
}
auto getDummyTwist()
{
geometry_msgs::msg::Twist twist;
return twist;
}
template<typename TNode>
geometry_msgs::msg::PoseStamped
getDummyPointStamped(TNode & node, std::string frame = std::string("odom"))
{
geometry_msgs::msg::PoseStamped point;
detail::setHeader(point, node, frame);
return point;
}
template<typename TNode>
geometry_msgs::msg::PoseStamped getDummyPointStamped(TNode & node, TestPose pose)
{
geometry_msgs::msg::PoseStamped point = getDummyPointStamped(node);
point.pose.position.x = pose.x;
point.pose.position.y = pose.y;
return point;
}
template<typename TNode>
nav_msgs::msg::Path getDummyPath(TNode node, std::string frame = std::string("odom"))
{
nav_msgs::msg::Path path;
detail::setHeader(path, node, frame);
return path;
}
template<typename TNode>
auto getDummyPath(size_t points_count, TNode node)
{
auto path = getDummyPath(node);
for (size_t i = 0; i < points_count; i++) {
path.poses.push_back(getDummyPointStamped(node));
}
return path;
}
template<typename TNode>
nav_msgs::msg::Path getIncrementalDummyPath(TNode node, TestPathSettings s)
{
auto path = getDummyPath(node);
for (size_t i = 0; i < s.poses_count; i++) {
double x = s.start_pose.x + static_cast<double>(i) * s.step_x;
double y = s.start_pose.y + static_cast<double>(i) * s.step_y;
path.poses.push_back(getDummyPointStamped(node, TestPose{x, y}));
}
return path;
}
navigation2/nav2_mppi_controller/test/utils/models.hpp
+75
View File
@@ -0,0 +1,75 @@
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// 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.
#pragma once
#include <vector>
#include <utility>
#include <string>
#include <rclcpp/rclcpp.hpp>
struct TestOptimizerSettings
{
int batch_size;
int time_steps;
int iteration_count;
double lookahead_distance;
std::string motion_model;
bool consider_footprint;
};
struct TestPose
{
double x;
double y;
};
struct TestCostmapSettings
{
const unsigned int cells_x = 40;
const unsigned int cells_y = 40;
const double origin_x = 0.0;
const double origin_y = 0.0;
const double resolution = 0.1;
const unsigned char cost_map_default_value = 0;
const double footprint_size = 0.15;
std::pair<unsigned int, unsigned int> getCenterIJ()
{
return {
cells_x / 2,
cells_y / 2};
}
TestPose getCenterPose()
{
return {
static_cast<double>(cells_x) * resolution / 2.0,
static_cast<double>(cells_y) * resolution / 2.0};
}
};
struct TestObstaclesSettings
{
unsigned int center_cells_x;
unsigned int center_cells_y;
unsigned int obstacle_size;
unsigned char obstacle_cost;
};
struct TestPathSettings
{
TestPose start_pose;
unsigned int poses_count;
double step_x;
double step_y;
};
navigation2/nav2_mppi_controller/test/utils/utils.hpp
+248
View File
@@ -0,0 +1,248 @@
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// 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.
#pragma once
#include <memory>
#include <string>
#include <vector>
#include <iostream>
#include <string_view>
#include <rclcpp/executors.hpp>
#include "tf2_ros/transform_broadcaster.h"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "models.hpp"
#include "factory.hpp"
using namespace std::chrono_literals; // NOLINT
template<typename TNode>
void waitSome(const std::chrono::nanoseconds & duration, TNode & node)
{
rclcpp::Time start_time = node->now();
while (rclcpp::ok() && node->now() - start_time <= rclcpp::Duration(duration)) {
rclcpp::spin_some(node->get_node_base_interface());
std::this_thread::sleep_for(3ms);
}
}
void sendTf(
std::string_view source, std::string_view dest,
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster,
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node, size_t n)
{
while (--n != 0u) {
auto t = geometry_msgs::msg::TransformStamped();
t.header.frame_id = source;
t.child_frame_id = dest;
t.header.stamp = node->now() + rclcpp::Duration(3ms);
t.transform.translation.x = 0.0;
t.transform.translation.y = 0.0;
t.transform.translation.z = 0.0;
t.transform.rotation.x = 0.0;
t.transform.rotation.y = 0.0;
t.transform.rotation.z = 0.0;
t.transform.rotation.w = 1.0;
tf_broadcaster->sendTransform(t);
// Allow tf_buffer_ to be filled by listener
waitSome(10ms, node);
}
}
/**
* Print costmap to stdout.
* @param costmap map to be printed.
*/
void printMap(const nav2_costmap_2d::Costmap2D & costmap)
{
for (unsigned int i = 0; i < costmap.getSizeInCellsY(); i++) {
for (unsigned int j = 0; j < costmap.getSizeInCellsX(); j++) {
printf("%4d", static_cast<int>(costmap.getCost(j, i)));
}
printf("\n\n");
}
}
/**
* Print costmap with trajectory and goal point to stdout.
* @param costmap map to be printed.
* @param trajectory trajectory container (xt::tensor) to be printed.
* @param goal_point goal point to be printed.
*/
template<typename TTrajectory>
void printMapWithTrajectoryAndGoal(
nav2_costmap_2d::Costmap2D & costmap, const TTrajectory & trajectory,
const geometry_msgs::msg::PoseStamped & goal)
{
const unsigned int trajectory_cost = 1;
const unsigned int goal_cost = 2;
std::cout << "Costmap: \n trajectory = " << trajectory_cost << "\n goal = " << goal_cost
<< "\n obsctacle = 255 \n";
// create new costmap
nav2_costmap_2d::Costmap2D costmap2d(
costmap.getSizeInCellsX(), costmap.getSizeInCellsY(), costmap.getResolution(),
costmap.getOriginX(), costmap.getOriginY(), costmap.getDefaultValue());
// copy obstacles from original costmap
costmap2d = costmap;
// add trajectory on map
unsigned int point_mx = 0;
unsigned int point_my = 0;
for (size_t i = 0; i < trajectory.shape()[0]; ++i) {
costmap2d.worldToMap(trajectory(i, 0), trajectory(i, 1), point_mx, point_my);
costmap2d.setCost(point_mx, point_my, trajectory_cost);
}
unsigned int goal_j{0};
unsigned int goal_i{0};
costmap2d.worldToMap(goal.pose.position.x, goal.pose.position.y, goal_j, goal_i);
std::cout << "Goal Position: " << goal_j << " " << goal_i << "\n";
costmap2d.setCost(goal_j, goal_i, goal_cost);
printMap(costmap2d);
}
/**
* Add a square obstacle to the costmap.
* @param costmap map to be modified.
* @param upper_left_corner_x obstacle upper left corner X coord (on the
* costmap).
* @param upper_left_corner_y obstacle upper left corner Y coord (on the
* costmap).
* @param size obstacle side size.
* @param cost obstacle value on costmap.
*/
void addObstacle(
nav2_costmap_2d::Costmap2D * costmap, unsigned int upper_left_corner_x,
unsigned int upper_left_corner_y, unsigned int size, unsigned char cost)
{
for (unsigned int i = upper_left_corner_x; i < upper_left_corner_x + size; i++) {
for (unsigned int j = upper_left_corner_y; j < upper_left_corner_y + size; j++) {
costmap->setCost(i, j, cost);
}
}
}
void printInfo(
TestOptimizerSettings os, TestPathSettings ps,
const std::vector<std::string> & critics)
{
std::stringstream ss;
for (auto str : critics) {
ss << str << " ";
}
std::cout << //
"\n\n--------------------OPTIMIZER OPTIONS-----------------------------\n" <<
"Critics: " << ss.str() << "\n" \
"Motion model: " << os.motion_model << "\n"
"Consider footprint: " << os.consider_footprint << "\n" <<
"Iterations: " << os.iteration_count << "\n" <<
"Batch size: " << os.batch_size << "\n" <<
"Time steps: " << os.time_steps << "\n" <<
"Path points: " << ps.poses_count << "\n" <<
"\n-------------------------------------------------------------------\n\n";
}
void addObstacle(nav2_costmap_2d::Costmap2D * costmap, TestObstaclesSettings s)
{
addObstacle(costmap, s.center_cells_x, s.center_cells_y, s.obstacle_size, s.obstacle_cost);
}
/**
* Check the trajectory for collisions with obstacles on the map.
* @param trajectory trajectory container (xt::tensor) to be checked.
* @param costmap costmap with obstacles
* @return true - if the trajectory crosses an obstacle on the map, false - if
* not
*/
template<typename TTrajectory>
bool inCollision(const TTrajectory & trajectory, const nav2_costmap_2d::Costmap2D & costmap)
{
unsigned int point_mx = 0;
unsigned int point_my = 0;
for (size_t i = 0; i < trajectory.shape(0); ++i) {
costmap.worldToMap(trajectory(i, 0), trajectory(i, 1), point_mx, point_my);
auto cost_ = costmap.getCost(point_mx, point_my);
if (cost_ > nav2_costmap_2d::FREE_SPACE || cost_ == nav2_costmap_2d::NO_INFORMATION) {
return true;
}
}
return false;
}
unsigned char getCost(const nav2_costmap_2d::Costmap2D & costmap, double x, double y)
{
unsigned int point_mx = 0;
unsigned int point_my = 0;
costmap.worldToMap(x, y, point_mx, point_my);
return costmap.getCost(point_mx, point_my);
}
template<typename TTrajectory>
bool isGoalReached(
const TTrajectory & trajectory, const nav2_costmap_2d::Costmap2D & costmap,
const geometry_msgs::msg::PoseStamped & goal)
{
unsigned int trajectory_j = 0;
unsigned int trajectory_i = 0;
unsigned int goal_j = 0;
unsigned int goal_i = 0;
costmap.worldToMap(goal.pose.position.x, goal.pose.position.y, goal_j, goal_i);
auto match = [](unsigned int i, unsigned int j, unsigned int i_dst, unsigned int j_dst) {
if (i == i_dst && j == j_dst) {
return true;
}
return false;
};
auto match_near = [&](unsigned int i, unsigned int j) {
if (match(i, j, goal_i, goal_j) ||
match(i, j, goal_i + 1, goal_j) ||
match(i, j, goal_i - 1, goal_j) ||
match(i, j, goal_i, goal_j + 1) ||
match(i, j, goal_i, goal_j - 1) ||
match(i, j, goal_i + 1, goal_j + 1) ||
match(i, j, goal_i + 1, goal_j - 1) ||
match(i, j, goal_i - 1, goal_j + 1) ||
match(i, j, goal_i - 1, goal_j - 1))
{
return true;
}
return false;
};
// clang-format on
for (size_t i = 0; i < trajectory.shape(0); ++i) {
costmap.worldToMap(trajectory(i, 0), trajectory(i, 1), trajectory_j, trajectory_i);
if (match_near(trajectory_i, trajectory_j)) {
return true;
}
}
return false;
}
navigation2/nav2_mppi_controller/test/utils_test.cpp
+445
View File
@@ -0,0 +1,445 @@
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// 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 <chrono>
#include <thread>
#include <xtensor/xrandom.hpp>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_mppi_controller/tools/utils.hpp"
#include "nav2_mppi_controller/models/path.hpp"
// Tests noise generator object
class RosLockGuard
{
public:
RosLockGuard() {rclcpp::init(0, nullptr);}
~RosLockGuard() {rclcpp::shutdown();}
};
RosLockGuard g_rclcpp;
using namespace mppi::utils; // NOLINT
using namespace mppi; // NOLINT
class TestGoalChecker : public nav2_core::GoalChecker
{
public:
TestGoalChecker() {}
virtual void initialize(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & /*parent*/,
const std::string & /*plugin_name*/,
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS>/*costmap_ros*/) {}
virtual void reset() {}
virtual bool isGoalReached(
const geometry_msgs::msg::Pose & /*query_pose*/,
const geometry_msgs::msg::Pose & /*goal_pose*/,
const geometry_msgs::msg::Twist & /*velocity*/) {return false;}
virtual bool getTolerances(
geometry_msgs::msg::Pose & pose_tolerance,
geometry_msgs::msg::Twist & /*vel_tolerance*/)
{
pose_tolerance.position.x = 0.25;
pose_tolerance.position.y = 0.25;
return true;
}
};
TEST(UtilsTests, MarkerPopulationUtils)
{
auto pose = createPose(1.0, 2.0, 3.0);
EXPECT_EQ(pose.position.x, 1.0);
EXPECT_EQ(pose.position.y, 2.0);
EXPECT_EQ(pose.position.z, 3.0);
EXPECT_EQ(pose.orientation.w, 1.0);
auto scale = createScale(1.0, 2.0, 3.0);
EXPECT_EQ(scale.x, 1.0);
EXPECT_EQ(scale.y, 2.0);
EXPECT_EQ(scale.z, 3.0);
auto color = createColor(1.0, 2.0, 3.0, 0.0);
EXPECT_EQ(color.r, 1.0);
EXPECT_EQ(color.g, 2.0);
EXPECT_EQ(color.b, 3.0);
EXPECT_EQ(color.a, 0.0);
auto marker = createMarker(999, pose, scale, color, "map", "ns");
EXPECT_EQ(marker.header.frame_id, "map");
EXPECT_EQ(marker.id, 999);
EXPECT_EQ(marker.pose, pose);
EXPECT_EQ(marker.scale, scale);
EXPECT_EQ(marker.color, color);
EXPECT_EQ(marker.ns, "ns");
}
TEST(UtilsTests, ConversionTests)
{
geometry_msgs::msg::TwistStamped output;
builtin_interfaces::msg::Time time;
// Check population is correct
output = toTwistStamped(0.5, 0.3, time, "map");
EXPECT_NEAR(output.twist.linear.x, 0.5, 1e-6);
EXPECT_NEAR(output.twist.linear.y, 0.0, 1e-6);
EXPECT_NEAR(output.twist.angular.z, 0.3, 1e-6);
EXPECT_EQ(output.header.frame_id, "map");
EXPECT_EQ(output.header.stamp, time);
output = toTwistStamped(0.5, 0.4, 0.3, time, "map");
EXPECT_NEAR(output.twist.linear.x, 0.5, 1e-6);
EXPECT_NEAR(output.twist.linear.y, 0.4, 1e-6);
EXPECT_NEAR(output.twist.angular.z, 0.3, 1e-6);
EXPECT_EQ(output.header.frame_id, "map");
EXPECT_EQ(output.header.stamp, time);
nav_msgs::msg::Path path;
path.poses.resize(5);
path.poses[2].pose.position.x = 5;
path.poses[2].pose.position.y = 50;
models::Path path_t = toTensor(path);
// Check population is correct
EXPECT_EQ(path_t.x.shape(0), 5u);
EXPECT_EQ(path_t.y.shape(0), 5u);
EXPECT_EQ(path_t.yaws.shape(0), 5u);
EXPECT_EQ(path_t.x(2), 5);
EXPECT_EQ(path_t.y(2), 50);
EXPECT_NEAR(path_t.yaws(2), 0.0, 1e-6);
}
TEST(UtilsTests, WithTolTests)
{
geometry_msgs::msg::Pose pose;
pose.position.x = 10.0;
pose.position.y = 1.0;
nav2_core::GoalChecker * goal_checker = new TestGoalChecker;
nav_msgs::msg::Path path;
path.poses.resize(2);
geometry_msgs::msg::Pose & goal = path.poses.back().pose;
// Create CriticData with state and goal initialized
models::State state;
state.pose.pose = pose;
models::Trajectories generated_trajectories;
models::Path path_critic;
xt::xtensor<float, 1> costs;
float model_dt;
CriticData data = {
state, generated_trajectories, path_critic, goal,
costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt};
// Test not in tolerance
goal.position.x = 0.0;
goal.position.y = 0.0;
EXPECT_FALSE(withinPositionGoalTolerance(goal_checker, pose, goal));
EXPECT_FALSE(withinPositionGoalTolerance(0.25, pose, goal));
// Test in tolerance
goal.position.x = 9.8;
goal.position.y = 0.95;
EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, goal));
EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, goal));
goal.position.x = 10.0;
goal.position.y = 0.76;
EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, goal));
EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, goal));
goal.position.x = 9.76;
goal.position.y = 1.0;
EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, goal));
EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, goal));
delete goal_checker;
goal_checker = nullptr;
EXPECT_FALSE(withinPositionGoalTolerance(goal_checker, pose, goal));
}
TEST(UtilsTests, AnglesTests)
{
// Test angle normalization by creating insane angles
xt::xtensor<float, 1> angles, zero_angles;
angles = xt::ones<float>({100});
for (unsigned int i = 0; i != angles.shape(0); i++) {
angles(i) = i * i;
if (i % 2 == 0) {
angles(i) *= -1;
}
}
auto norm_ang = normalize_angles(angles);
for (unsigned int i = 0; i != norm_ang.shape(0); i++) {
EXPECT_TRUE((norm_ang(i) >= -M_PI) && (norm_ang(i) <= M_PI));
}
// Test shortest angular distance
zero_angles = xt::zeros<float>({100});
auto ang_dist = shortest_angular_distance(angles, zero_angles);
for (unsigned int i = 0; i != ang_dist.shape(0); i++) {
EXPECT_TRUE((ang_dist(i) >= -M_PI) && (ang_dist(i) <= M_PI));
}
// Test point-pose angle
geometry_msgs::msg::Pose pose;
pose.position.x = 0.0;
pose.position.y = 0.0;
pose.orientation.w = 1.0;
double point_x = 1.0, point_y = 0.0;
bool forward_preference = true;
EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6);
forward_preference = false;
EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6);
point_x = -1.0;
EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6);
forward_preference = true;
EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), M_PI, 1e-6);
}
TEST(UtilsTests, FurthestAndClosestReachedPoint)
{
models::State state;
models::Trajectories generated_trajectories;
models::Path path;
geometry_msgs::msg::Pose goal;
xt::xtensor<float, 1> costs;
float model_dt = 0.1;
CriticData data =
{state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
std::nullopt, std::nullopt}; /// Caution, keep references
// Attempt to set furthest point if notionally set, should not change
data.furthest_reached_path_point = 99999;
setPathFurthestPointIfNotSet(data);
EXPECT_EQ(data.furthest_reached_path_point, 99999);
// Attempt to set if not set already with no other information, should fail
CriticData data2 =
{state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
std::nullopt, std::nullopt}; /// Caution, keep references
setPathFurthestPointIfNotSet(data2);
EXPECT_EQ(data2.furthest_reached_path_point, 0);
// Test the actual computation of the path point reached
generated_trajectories.x = xt::ones<float>({100, 2});
generated_trajectories.y = xt::zeros<float>({100, 2});
generated_trajectories.yaws = xt::zeros<float>({100, 2});
nav_msgs::msg::Path plan;
plan.poses.resize(10);
for (unsigned int i = 0; i != plan.poses.size(); i++) {
plan.poses[i].pose.position.x = 0.2 * i;
plan.poses[i].pose.position.y = 0.0;
}
path = toTensor(plan);
CriticData data3 =
{state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
std::nullopt, std::nullopt}; /// Caution, keep references
EXPECT_EQ(findPathFurthestReachedPoint(data3), 5u);
EXPECT_EQ(findPathTrajectoryInitialPoint(data3), 5u);
}
TEST(UtilsTests, findPathCosts)
{
models::State state;
models::Trajectories generated_trajectories;
models::Path path;
geometry_msgs::msg::Pose goal;
xt::xtensor<float, 1> costs;
float model_dt = 0.1;
CriticData data =
{state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
std::nullopt, std::nullopt}; /// Caution, keep references
// Test not set if already set, should not change
data.path_pts_valid = std::vector<bool>(10, false);
for (unsigned int i = 0; i != 10; i++) {
(*data.path_pts_valid)[i] = false;
}
EXPECT_TRUE(data.path_pts_valid);
setPathCostsIfNotSet(data, nullptr);
EXPECT_EQ(data.path_pts_valid->size(), 10u);
CriticData data3 =
{state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
std::nullopt, std::nullopt}; /// Caution, keep references
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"dummy_costmap", "", "dummy_costmap");
rclcpp_lifecycle::State lstate;
costmap_ros->on_configure(lstate);
auto * costmap = costmap_ros->getCostmap();
// island in the middle of lethal cost to cross. Costmap defaults to size 5x5 @ 10cm resolution
for (unsigned int i = 10; i <= 30; ++i) { // 1m-3m
for (unsigned int j = 10; j <= 30; ++j) { // 1m-3m
costmap->setCost(i, j, 254);
}
}
for (unsigned int i = 40; i <= 45; ++i) { // 4m-4.5m
for (unsigned int j = 45; j <= 45; ++j) { // 4m-4.5m
costmap->setCost(i, j, 253);
}
}
path.reset(50);
path.x(1) = 999999999; // OFF COSTMAP
path.y(1) = 999999999;
path.x(10) = 1.5; // IN LETHAL
path.y(10) = 1.5;
path.x(20) = 4.2; // IN INFLATED
path.y(20) = 4.2;
// This should be evaluated and have real outputs now
setPathCostsIfNotSet(data3, costmap_ros);
EXPECT_TRUE(data3.path_pts_valid.has_value());
for (unsigned int i = 0; i != path.x.shape(0) - 1; i++) {
if (i == 1 || i == 10) {
EXPECT_FALSE((*data3.path_pts_valid)[i]);
} else {
EXPECT_TRUE((*data3.path_pts_valid)[i]);
}
}
}
TEST(UtilsTests, SmootherTest)
{
models::ControlSequence noisey_sequence, sequence_init;
noisey_sequence.vx = 0.2 * xt::ones<float>({30});
noisey_sequence.vy = 0.0 * xt::ones<float>({30});
noisey_sequence.wz = 0.3 * xt::ones<float>({30});
// Make the sequence noisy
auto noises = xt::random::randn<float>({30}, 0.0, 0.2);
noisey_sequence.vx += noises;
noisey_sequence.vy += noises;
noisey_sequence.wz += noises;
sequence_init = noisey_sequence;
std::array<mppi::models::Control, 4> history, history_init;
history[3].vx = 0.1;
history[3].vy = 0.0;
history[3].wz = 0.3;
history[2].vx = 0.1;
history[2].vy = 0.0;
history[2].wz = 0.3;
history[1].vx = 0.1;
history[1].vy = 0.0;
history[1].wz = 0.3;
history[0].vx = 0.0;
history[0].vy = 0.0;
history[0].wz = 0.0;
history_init = history;
models::OptimizerSettings settings;
settings.shift_control_sequence = false; // so result stores 0th value in history
savitskyGolayFilter(noisey_sequence, history, settings);
// Check history is propogated backward
EXPECT_NEAR(history_init[3].vx, history[2].vx, 0.02);
EXPECT_NEAR(history_init[3].vy, history[2].vy, 0.02);
EXPECT_NEAR(history_init[3].wz, history[2].wz, 0.02);
// Check history element is updated for first command
EXPECT_NEAR(history[3].vx, 0.2, 0.05);
EXPECT_NEAR(history[3].vy, 0.0, 0.035);
EXPECT_NEAR(history[3].wz, 0.23, 0.02);
// Check that path is smoother
float smoothed_val{0}, original_val{0};
for (unsigned int i = 0; i != noisey_sequence.vx.shape(0); i++) {
smoothed_val += fabs(noisey_sequence.vx(i) - 0.2);
smoothed_val += fabs(noisey_sequence.vy(i) - 0.0);
smoothed_val += fabs(noisey_sequence.wz(i) - 0.3);
original_val += fabs(sequence_init.vx(i) - 0.2);
original_val += fabs(sequence_init.vy(i) - 0.0);
original_val += fabs(sequence_init.wz(i) - 0.3);
}
EXPECT_LT(smoothed_val, original_val);
}
TEST(UtilsTests, FindPathInversionTest)
{
// Straight path, no inversions to be found
nav_msgs::msg::Path path;
for (unsigned int i = 0; i != 10; i++) {
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = i;
path.poses.push_back(pose);
}
EXPECT_EQ(utils::findFirstPathInversion(path), 10u);
// To short to process
path.poses.erase(path.poses.begin(), path.poses.begin() + 7);
EXPECT_EQ(utils::findFirstPathInversion(path), 3u);
// Has inversion at index 10, so should return 11 for the first point afterwards
// 0 1 2 3 4 5 6 7 8 9 10 **9** 8 7 6 5 4 3 2 1
path.poses.clear();
for (unsigned int i = 0; i != 10; i++) {
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = i;
path.poses.push_back(pose);
}
for (unsigned int i = 0; i != 10; i++) {
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = 10 - i;
path.poses.push_back(pose);
}
EXPECT_EQ(utils::findFirstPathInversion(path), 11u);
}
TEST(UtilsTests, RemovePosesAfterPathInversionTest)
{
nav_msgs::msg::Path path;
// straight path
for (unsigned int i = 0; i != 10; i++) {
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = i;
path.poses.push_back(pose);
}
EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u);
// try empty path
path.poses.clear();
EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u);
// cusping path
for (unsigned int i = 0; i != 10; i++) {
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = i;
path.poses.push_back(pose);
}
for (unsigned int i = 0; i != 10; i++) {
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = 10 - i;
path.poses.push_back(pose);
}
EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 11u);
// Check to see if removed
EXPECT_EQ(path.poses.size(), 11u);
EXPECT_EQ(path.poses.back().pose.position.x, 10);
}