Orange
8991e5208e
modified: src/navigation/obstacle_nav2/include/obstacle_nav2/trajectory_guard.hpp modified: src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py modified: src/navigation/obstacle_nav2/src/trajectory_guard.cpp modified: src/navigation/obstacle_nav2/src/trajectory_guard_node.cpp modified: src/navigation/obstacle_nav2/test/test_trajectory_guard.cpp modified: src/origincar_base/CMakeLists.txt modified: src/origincar_base/config/ekf.yaml new file: src/origincar_base/include/origincar_base/log.hpp modified: src/origincar_base/include/origincar_base/origincar_base.h modified: src/origincar_base/launch/base_serial.launch.py modified: src/origincar_base/launch/base_serial.launch.py.bak new file: src/origincar_base/src/log.cpp modified: src/origincar_base/src/origincar_base.cpp modified: src/origincar_base/src/origincar_base.cpp.bak new file: src/origincar_base/test/scan_odom_timing_logger_test.cpp modified: src/planner/REAL_ROBOT_RUNBOOK.md modified: src/planner/launch/odom_hybrid_astar.launch.py modified: src/planner/launch/real_hybrid_astar.launch.py modified: "src/planner/\346\223\215\344\275\234\346\211\213\345\206\214.md" modified: src/racing_control/CMakeLists.txt modified: src/racing_control/config/racing_control.yaml modified: src/racing_control/include/racing_control/racing_control.hpp modified: src/racing_control/launch/racing_control.launch.py modified: src/racing_control/package.xml new file: src/racing_control/src/racing_control copy.cpp modified: src/racing_control/src/racing_control.cpp modified: src/racing_control/test/test_racing_control_helpers.cpp modified: "src/racing_control/\347\202\271\344\275\215\346\240\274\345\274\217\350\275\254\346\215\242.md"
106 lines
3.3 KiB
C++
106 lines
3.3 KiB
C++
#include <cmath>
|
|
#include <stdexcept>
|
|
|
|
#include "gtest/gtest.h"
|
|
#include "racing_control/racing_control.hpp"
|
|
|
|
namespace
|
|
{
|
|
|
|
constexpr double kTolerance = 1e-6;
|
|
|
|
} // namespace
|
|
|
|
TEST(RacingControlHelpers, ConvertsFlatTriplesToStampedPoses)
|
|
{
|
|
const auto poses = racing_control::posesFromFlatDoubles(
|
|
{1.0, 2.0, M_PI_2, -0.5, 0.25, -M_PI},
|
|
"odom");
|
|
|
|
ASSERT_EQ(poses.size(), 2U);
|
|
EXPECT_EQ(poses[0].header.frame_id, "odom");
|
|
EXPECT_DOUBLE_EQ(poses[0].pose.position.x, 1.0);
|
|
EXPECT_DOUBLE_EQ(poses[0].pose.position.y, 2.0);
|
|
EXPECT_NEAR(poses[0].pose.orientation.z, std::sin(M_PI_4), kTolerance);
|
|
EXPECT_NEAR(poses[0].pose.orientation.w, std::cos(M_PI_4), kTolerance);
|
|
EXPECT_DOUBLE_EQ(poses[1].pose.position.x, -0.5);
|
|
EXPECT_DOUBLE_EQ(poses[1].pose.position.y, 0.25);
|
|
}
|
|
|
|
TEST(RacingControlHelpers, RejectsIncompletePoseTriples)
|
|
{
|
|
EXPECT_THROW(
|
|
racing_control::posesFromFlatDoubles({1.0, 2.0, 0.0, 3.0}, "odom"),
|
|
std::invalid_argument);
|
|
}
|
|
|
|
TEST(RacingControlHelpers, ParsesQrDirectionFromText)
|
|
{
|
|
EXPECT_EQ(
|
|
racing_control::directionFromQrResult("7 顺时针"),
|
|
racing_control::RouteDirection::Clockwise);
|
|
EXPECT_EQ(
|
|
racing_control::directionFromQrResult("8 逆时针"),
|
|
racing_control::RouteDirection::Counterclockwise);
|
|
EXPECT_EQ(
|
|
racing_control::directionFromQrResult("5"),
|
|
racing_control::RouteDirection::Clockwise);
|
|
EXPECT_EQ(
|
|
racing_control::directionFromQrResult("6"),
|
|
racing_control::RouteDirection::Counterclockwise);
|
|
EXPECT_EQ(
|
|
racing_control::directionFromQrResult("未识别"),
|
|
racing_control::RouteDirection::Unknown);
|
|
}
|
|
|
|
TEST(RacingControlHelpers, LatchesFirstKnownQrDirection)
|
|
{
|
|
EXPECT_TRUE(
|
|
racing_control::shouldAcceptQrDirection(
|
|
racing_control::RouteDirection::Unknown,
|
|
racing_control::RouteDirection::Clockwise));
|
|
EXPECT_FALSE(
|
|
racing_control::shouldAcceptQrDirection(
|
|
racing_control::RouteDirection::Unknown,
|
|
racing_control::RouteDirection::Unknown));
|
|
EXPECT_FALSE(
|
|
racing_control::shouldAcceptQrDirection(
|
|
racing_control::RouteDirection::Clockwise,
|
|
racing_control::RouteDirection::Clockwise));
|
|
EXPECT_FALSE(
|
|
racing_control::shouldAcceptQrDirection(
|
|
racing_control::RouteDirection::Clockwise,
|
|
racing_control::RouteDirection::Counterclockwise));
|
|
}
|
|
|
|
TEST(RacingControlHelpers, SelectsPostQrTransitTargetWhenEnabled)
|
|
{
|
|
EXPECT_EQ(
|
|
racing_control::qrTransitTargetAfterRecognition(true),
|
|
racing_control::QrTransitTarget::PostQr);
|
|
EXPECT_EQ(
|
|
racing_control::qrTransitTargetAfterRecognition(false),
|
|
racing_control::QrTransitTarget::Entry);
|
|
}
|
|
|
|
TEST(RacingControlHelpers, PublishesOneVlmImageFrameOnlyWhenEnabledAndAvailable)
|
|
{
|
|
EXPECT_TRUE(racing_control::shouldPublishVlmImageFrame(true, true));
|
|
EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(false, true));
|
|
EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(true, false));
|
|
EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(false, false));
|
|
}
|
|
|
|
TEST(RacingControlHelpers, ParsesVlmCaptureMode)
|
|
{
|
|
EXPECT_EQ(
|
|
racing_control::vlmCaptureModeFromString("stop"),
|
|
racing_control::VlmCaptureMode::Stop);
|
|
EXPECT_EQ(
|
|
racing_control::vlmCaptureModeFromString("pass_through"),
|
|
racing_control::VlmCaptureMode::PassThrough);
|
|
EXPECT_EQ(
|
|
racing_control::vlmCaptureModeFromString("unknown"),
|
|
racing_control::VlmCaptureMode::Unknown);
|
|
}
|