HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

107
navigations/dwb_plugins/test/goal_checker.cpp Executable file
View File

@@ -0,0 +1,107 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <gtest/gtest.h>
#include <dwb_plugins/simple_goal_checker.h>
#include <dwb_plugins/stopped_goal_checker.h>
using dwb_plugins::SimpleGoalChecker;
using dwb_plugins::StoppedGoalChecker;
void checkMacro(dwb_local_planner::GoalChecker& gc,
double x0, double y0, double theta0,
double x1, double y1, double theta1,
double xv, double yv, double thetav,
bool expected_result)
{
gc.reset();
geometry_msgs::Pose2D pose0, pose1;
pose0.x = x0;
pose0.y = y0;
pose0.theta = theta0;
pose1.x = x1;
pose1.y = y1;
pose1.theta = theta1;
nav_2d_msgs::Twist2D v;
v.x = xv;
v.y = yv;
v.theta = thetav;
if (expected_result)
EXPECT_TRUE(gc.isGoalReached(pose0, pose1, v));
else
EXPECT_FALSE(gc.isGoalReached(pose0, pose1, v));
}
void sameResult(dwb_local_planner::GoalChecker& gc0, dwb_local_planner::GoalChecker& gc1,
double x0, double y0, double theta0,
double x1, double y1, double theta1,
double xv, double yv, double thetav,
bool expected_result)
{
checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result);
checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result);
}
void trueFalse(dwb_local_planner::GoalChecker& gc0, dwb_local_planner::GoalChecker& gc1,
double x0, double y0, double theta0,
double x1, double y1, double theta1,
double xv, double yv, double thetav)
{
checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, true);
checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, false);
}
TEST(VelocityIterator, two_checks)
{
ros::NodeHandle x;
SimpleGoalChecker gc;
StoppedGoalChecker sgc;
gc.initialize(x);
sgc.initialize(x);
sameResult(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, true);
sameResult(gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, false);
sameResult(gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, false);
sameResult(gc, sgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, false);
sameResult(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, true);
trueFalse(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0);
trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 1, 0, 0);
trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 1, 0);
trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 1);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "goal_checker");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

3
navigations/dwb_plugins/test/goal_checker.launch Executable file
View File

@@ -0,0 +1,3 @@
<launch>
<test test-name="goal_checker" pkg="dwb_plugins" type="goal_checker" />
</launch>

422
navigations/dwb_plugins/test/twist_gen.cpp Executable file
View File

@@ -0,0 +1,422 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <gtest/gtest.h>
#include <dwb_plugins/standard_traj_generator.h>
#include <dwb_plugins/limited_accel_generator.h>
#include <nav_core2/exceptions.h>
#include <vector>
#include <algorithm>
using dwb_plugins::StandardTrajectoryGenerator;
geometry_msgs::Pose2D origin;
nav_2d_msgs::Twist2D zero;
nav_2d_msgs::Twist2D forward;
void checkLimits(const std::vector<nav_2d_msgs::Twist2D>& twists,
double exp_min_x, double exp_max_x, double exp_min_y, double exp_max_y,
double exp_min_theta, double exp_max_theta,
double exp_max_xy = -1.0,
double exp_min_xy = -1.0, double exp_min_speed_theta = -1.0)
{
ASSERT_GT(twists.size(), 0U);
nav_2d_msgs::Twist2D first = twists[0];
double min_x = first.x, max_x = first.x, min_y = first.y, max_y = first.y;
double min_theta = first.theta, max_theta = first.theta;
double max_xy = hypot(first.x, first.y);
for (nav_2d_msgs::Twist2D twist : twists)
{
min_x = std::min(min_x, twist.x);
min_y = std::min(min_y, twist.y);
min_theta = std::min(min_theta, twist.theta);
max_x = std::max(max_x, twist.x);
max_y = std::max(max_y, twist.y);
max_theta = std::max(max_theta, twist.theta);
double hyp = hypot(twist.x, twist.y);
max_xy = std::max(max_xy, hyp);
if (exp_min_xy >= 0 && exp_min_speed_theta >= 0)
{
EXPECT_TRUE(fabs(twist.theta) >= exp_min_speed_theta || hyp >= exp_min_xy);
}
}
EXPECT_DOUBLE_EQ(min_x, exp_min_x);
EXPECT_DOUBLE_EQ(max_x, exp_max_x);
EXPECT_DOUBLE_EQ(min_y, exp_min_y);
EXPECT_DOUBLE_EQ(max_y, exp_max_y);
EXPECT_DOUBLE_EQ(min_theta, exp_min_theta);
EXPECT_DOUBLE_EQ(max_theta, exp_max_theta);
if (exp_max_xy >= 0)
{
EXPECT_DOUBLE_EQ(max_xy, exp_max_xy);
}
}
TEST(VelocityIterator, standard_gen)
{
ros::NodeHandle nh("st_gen");
StandardTrajectoryGenerator gen;
gen.initialize(nh);
std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
EXPECT_EQ(twists.size(), 1926U);
checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, 0.55, 0.1, 0.4);
}
TEST(VelocityIterator, max_xy)
{
ros::NodeHandle nh("max_xy");
nh.setParam("max_speed_xy", 1.0);
StandardTrajectoryGenerator gen;
gen.initialize(nh);
std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
// Expect more twists since max_speed_xy is now beyond feasible limits
EXPECT_EQ(twists.size(), 2010U);
checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1));
}
TEST(VelocityIterator, min_xy)
{
ros::NodeHandle nh("min_xy");
nh.setParam("min_speed_xy", -1);
StandardTrajectoryGenerator gen;
gen.initialize(nh);
std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
// Expect even more since theres no min_speed_xy
EXPECT_EQ(twists.size(), 2015U);
checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0);
}
TEST(VelocityIterator, min_theta)
{
ros::NodeHandle nh("min_theta");
nh.setParam("min_speed_theta", -1);
StandardTrajectoryGenerator gen;
gen.initialize(nh);
std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
// Expect even more since theres no min_speed_xy
EXPECT_EQ(twists.size(), 2015U);
checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0);
}
TEST(VelocityIterator, no_limits)
{
ros::NodeHandle nh("no_limits");
nh.setParam("max_speed_xy", -1.0);
nh.setParam("min_speed_xy", -1);
nh.setParam("min_speed_theta", -1);
StandardTrajectoryGenerator gen;
gen.initialize(nh);
std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
// vx_samples * vtheta_samples * vy_samples + added zero theta samples - (0,0,0)
EXPECT_EQ(twists.size(), static_cast<unsigned int>(20 * 20 * 5 + 100 - 1));
checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1), 0.0, 0.0);
}
TEST(VelocityIterator, no_limits_samples)
{
ros::NodeHandle nh("no_limits_samples");
nh.setParam("max_speed_xy", -1.0);
nh.setParam("min_speed_xy", -1);
nh.setParam("min_speed_theta", -1);
int x_samples = 10, y_samples = 3, theta_samples = 5;
nh.setParam("vx_samples", x_samples);
nh.setParam("vy_samples", y_samples);
nh.setParam("vtheta_samples", theta_samples);
StandardTrajectoryGenerator gen;
gen.initialize(nh);
std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
EXPECT_EQ(twists.size(), static_cast<unsigned int>(x_samples * y_samples * theta_samples - 1));
checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1), 0.0, 0.0);
}
TEST(VelocityIterator, dwa_gen_exception)
{
ros::NodeHandle nh("dwa_gen_exception");
nh.setParam("use_dwa", true);
StandardTrajectoryGenerator gen;
EXPECT_THROW(gen.initialize(nh), nav_core2::PlannerException);
}
TEST(VelocityIterator, no_dwa_gen_exception)
{
ros::NodeHandle nh("no_dwa_gen_exception");
nh.setParam("use_dwa", false);
dwb_plugins::LimitedAccelGenerator gen;
EXPECT_THROW(gen.initialize(nh), nav_core2::PlannerException);
}
TEST(VelocityIterator, dwa_gen)
{
ros::NodeHandle nh("dwa_gen");
nh.setParam("use_dwa", true);
nh.setParam("min_speed_theta", -1);
dwb_plugins::LimitedAccelGenerator gen;
gen.initialize(nh);
std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
// Same as no-limits since everything is within our velocity limits
EXPECT_EQ(twists.size(), static_cast<unsigned int>(20 * 20 * 5 + 100 - 1));
checkLimits(twists, 0.0, 0.125, -0.1, 0.1, -0.16, 0.16, hypot(0.125, 0.1), 0.0, 0.1);
}
TEST(VelocityIterator, dwa_gen_no_param)
{
ros::NodeHandle nh("dwa_gen_no_param");
nh.setParam("min_speed_theta", -1);
dwb_plugins::LimitedAccelGenerator gen;
gen.initialize(nh);
std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
EXPECT_EQ(twists.size(), static_cast<unsigned int>(20 * 20 * 5 + 100 - 1));
checkLimits(twists, 0.0, 0.125, -0.1, 0.1, -0.16, 0.16, hypot(0.125, 0.1), 0.0, 0.1);
}
TEST(VelocityIterator, nonzero)
{
ros::NodeHandle nh("nonzero");
nh.setParam("use_dwa", true);
nh.setParam("min_speed_theta", -1);
dwb_plugins::LimitedAccelGenerator gen;
gen.initialize(nh);
nav_2d_msgs::Twist2D initial;
initial.x = 0.1;
initial.y = -0.08;
initial.theta = 0.05;
std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(initial);
EXPECT_EQ(twists.size(), 2519U);
checkLimits(twists, 0.0, 0.225, -0.1, 0.045, -0.11000000000000003, 0.21,
0.24622144504490268, 0.0, 0.1);
}
void matchPose(const geometry_msgs::Pose2D& a, const geometry_msgs::Pose2D& b)
{
EXPECT_DOUBLE_EQ(a.x, b.x);
EXPECT_DOUBLE_EQ(a.y, b.y);
EXPECT_DOUBLE_EQ(a.theta, b.theta);
}
void matchPose(const geometry_msgs::Pose2D& a, const double x, const double y, const double theta)
{
EXPECT_DOUBLE_EQ(a.x, x);
EXPECT_DOUBLE_EQ(a.y, y);
EXPECT_DOUBLE_EQ(a.theta, theta);
}
void matchTwist(const nav_2d_msgs::Twist2D& a, const nav_2d_msgs::Twist2D& b)
{
EXPECT_DOUBLE_EQ(a.x, b.x);
EXPECT_DOUBLE_EQ(a.y, b.y);
EXPECT_DOUBLE_EQ(a.theta, b.theta);
}
void matchTwist(const nav_2d_msgs::Twist2D& a, const double x, const double y, const double theta)
{
EXPECT_DOUBLE_EQ(a.x, x);
EXPECT_DOUBLE_EQ(a.y, y);
EXPECT_DOUBLE_EQ(a.theta, theta);
}
const double DEFAULT_SIM_TIME = 1.7;
TEST(TrajectoryGenerator, basic)
{
ros::NodeHandle nh("basic");
StandardTrajectoryGenerator gen;
nh.setParam("linear_granularity", 0.5);
gen.initialize(nh);
dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
matchTwist(res.velocity, forward);
EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME);
int n = res.poses.size();
EXPECT_EQ(n, 3);
ASSERT_GT(n, 0);
matchPose(res.poses[0], origin);
matchPose(res.poses[n - 1], DEFAULT_SIM_TIME * forward.x, 0, 0);
}
TEST(TrajectoryGenerator, basic_no_last_point)
{
ros::NodeHandle nh("basic_no_last_point");
StandardTrajectoryGenerator gen;
nh.setParam("include_last_point", false);
nh.setParam("linear_granularity", 0.5);
gen.initialize(nh);
dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
matchTwist(res.velocity, forward);
EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME / 2);
int n = res.poses.size();
EXPECT_EQ(n, 2);
ASSERT_GT(n, 0);
matchPose(res.poses[0], origin);
matchPose(res.poses[n - 1], 0.255, 0, 0);
}
TEST(TrajectoryGenerator, too_slow)
{
ros::NodeHandle nh("too_slow");
StandardTrajectoryGenerator gen;
nh.setParam("linear_granularity", 0.5);
gen.initialize(nh);
nav_2d_msgs::Twist2D cmd;
cmd.x = 0.2;
dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, cmd, cmd);
matchTwist(res.velocity, cmd);
EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME);
int n = res.poses.size();
EXPECT_EQ(n, 2);
ASSERT_GT(n, 0);
matchPose(res.poses[0], origin);
}
TEST(TrajectoryGenerator, holonomic)
{
ros::NodeHandle nh("holonomic");
StandardTrajectoryGenerator gen;
nh.setParam("linear_granularity", 0.5);
gen.initialize(nh);
nav_2d_msgs::Twist2D cmd;
cmd.x = 0.3;
cmd.y = 0.2;
dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, cmd, cmd);
matchTwist(res.velocity, cmd);
EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME);
int n = res.poses.size();
EXPECT_EQ(n, 3);
ASSERT_GT(n, 0);
matchPose(res.poses[0], origin);
matchPose(res.poses[n - 1], cmd.x*DEFAULT_SIM_TIME, cmd.y*DEFAULT_SIM_TIME, 0);
}
TEST(TrajectoryGenerator, twisty)
{
ros::NodeHandle nh("twisty");
StandardTrajectoryGenerator gen;
nh.setParam("linear_granularity", 0.5);
nh.setParam("angular_granularity", 0.025);
gen.initialize(nh);
nav_2d_msgs::Twist2D cmd;
cmd.x = 0.3;
cmd.y = -0.2;
cmd.theta = 0.111;
dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, cmd, cmd);
matchTwist(res.velocity, cmd);
EXPECT_NEAR(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME, 1.0E-5);
// Given the number of poses, the resulting time is slightly less precise
int n = res.poses.size();
EXPECT_EQ(n, 9);
ASSERT_GT(n, 0);
matchPose(res.poses[0], origin);
matchPose(res.poses[n - 1], 0.5355173615993063, -0.29635287789821596, cmd.theta * DEFAULT_SIM_TIME);
}
TEST(TrajectoryGenerator, sim_time)
{
ros::NodeHandle nh("sim_time");
const double sim_time = 2.5;
nh.setParam("sim_time", sim_time);
nh.setParam("linear_granularity", 0.5);
StandardTrajectoryGenerator gen;
gen.initialize(nh);
dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
matchTwist(res.velocity, forward);
EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), sim_time);
int n = res.poses.size();
EXPECT_EQ(n, 3);
ASSERT_GT(n, 0);
matchPose(res.poses[0], origin);
matchPose(res.poses[n - 1], sim_time * forward.x, 0, 0);
}
TEST(TrajectoryGenerator, accel)
{
ros::NodeHandle nh("accel");
nh.setParam("sim_time", 5.0);
nh.setParam("discretize_by_time", true);
nh.setParam("sim_granularity", 1.0);
nh.setParam("acc_lim_x", 0.1);
nh.setParam("min_speed_xy", -1.0);
StandardTrajectoryGenerator gen;
gen.initialize(nh);
dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, zero, forward);
matchTwist(res.velocity, forward);
EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), 5.0);
ASSERT_EQ(res.poses.size(), 6U);
matchPose(res.poses[0], origin);
matchPose(res.poses[1], 0.1, 0, 0);
matchPose(res.poses[2], 0.3, 0, 0);
matchPose(res.poses[3], 0.6, 0, 0);
matchPose(res.poses[4], 0.9, 0, 0);
matchPose(res.poses[5], 1.2, 0, 0);
}
TEST(TrajectoryGenerator, dwa)
{
ros::NodeHandle nh("dwa");
nh.setParam("use_dwa", true);
nh.setParam("sim_period", 1.0);
nh.setParam("sim_time", 5.0);
nh.setParam("discretize_by_time", true);
nh.setParam("sim_granularity", 1.0);
nh.setParam("acc_lim_x", 0.1);
nh.setParam("min_speed_xy", -1.0);
dwb_plugins::LimitedAccelGenerator gen;
gen.initialize(nh);
dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, zero, forward);
matchTwist(res.velocity, forward);
EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), 5.0);
ASSERT_EQ(res.poses.size(), 6U);
matchPose(res.poses[0], origin);
matchPose(res.poses[1], 0.3, 0, 0);
matchPose(res.poses[2], 0.6, 0, 0);
matchPose(res.poses[3], 0.9, 0, 0);
matchPose(res.poses[4], 1.2, 0, 0);
matchPose(res.poses[5], 1.5, 0, 0);
}
int main(int argc, char **argv)
{
forward.x = 0.3;
ros::init(argc, argv, "twist_gen");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

3
navigations/dwb_plugins/test/twist_gen.launch Executable file
View File

@@ -0,0 +1,3 @@
<launch>
<test test-name="twist_gen_test" pkg="dwb_plugins" type="twist_gen_test" />
</launch>

155
navigations/dwb_plugins/test/velocity_iterator_test.cpp Executable file
View File

@@ -0,0 +1,155 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <gtest/gtest.h>
#include <dwb_plugins/one_d_velocity_iterator.h>
using dwb_plugins::OneDVelocityIterator;
using dwb_plugins::projectVelocity;
const double EPSILON = 1e-3;
TEST(VelocityIterator, basics)
{
OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 1.0, 2);
EXPECT_FALSE(it.isFinished());
EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
EXPECT_FALSE(it.isFinished());
++it;
EXPECT_FALSE(it.isFinished());
EXPECT_NEAR(it.getVelocity(), 3.0, EPSILON);
EXPECT_FALSE(it.isFinished());
++it;
EXPECT_TRUE(it.isFinished());
it.reset();
EXPECT_FALSE(it.isFinished());
EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
}
TEST(VelocityIterator, limits)
{
OneDVelocityIterator it(2.0, 1.5, 2.5, 1.0, -1.0, 1.0, 2);
EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
}
TEST(VelocityIterator, acceleration)
{
OneDVelocityIterator it(2.0, 0.0, 5.0, 0.5, -0.5, 1.0, 2);
EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
}
TEST(VelocityIterator, time)
{
OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 0.5, 2);
EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
}
TEST(VelocityIterator, samples)
{
OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 1.0, 3);
EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 2.0, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 3.0, EPSILON);
++it;
EXPECT_TRUE(it.isFinished());
}
TEST(VelocityIterator, samples2)
{
OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 1.0, 5);
EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 2.0, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 3.0, EPSILON);
++it;
EXPECT_TRUE(it.isFinished());
}
TEST(VelocityIterator, around_zero)
{
OneDVelocityIterator it(0.0, -5.0, 5.0, 1.0, -1.0, 1.0, 2);
EXPECT_NEAR(it.getVelocity(), -1.0, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 0.0, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
++it;
}
TEST(VelocityIterator, around_zero2)
{
OneDVelocityIterator it(0.0, -5.0, 5.0, 1.0, -1.0, 1.0, 4);
EXPECT_NEAR(it.getVelocity(), -1.0, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), -0.3333, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 0.0, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 0.3333, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
++it;
}
TEST(VelocityIterator, acceleration_magnitude)
{
double accel_rate = 2.0;
double decel_rate = -3.0;
EXPECT_NEAR(projectVelocity(+0.0, accel_rate, decel_rate, 0.1, +1.5), +0.2, EPSILON);
EXPECT_NEAR(projectVelocity(+0.0, accel_rate, decel_rate, 0.1, -1.5), -0.2, EPSILON);
EXPECT_NEAR(projectVelocity(+1.0, accel_rate, decel_rate, 0.1, +1.5), +1.2, EPSILON);
EXPECT_NEAR(projectVelocity(+1.0, accel_rate, decel_rate, 0.1, +0.5), +0.7, EPSILON);
EXPECT_NEAR(projectVelocity(-1.0, accel_rate, decel_rate, 0.1, -1.5), -1.2, EPSILON);
EXPECT_NEAR(projectVelocity(-1.0, accel_rate, decel_rate, 0.1, -0.5), -0.7, EPSILON);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}