generated from wessel/boilerplate
feat(tests): Add gtests for each node and simulator class
This commit is contained in:
@@ -50,15 +50,30 @@ install(DIRECTORY launch config
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
|
||||
# Simulator unit tests
|
||||
ament_add_gtest(test_simulator test/test_simulator.cpp src/simulator/Simulator.cpp)
|
||||
target_include_directories(test_simulator PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src)
|
||||
ament_target_dependencies(test_simulator rclcpp)
|
||||
|
||||
# IMU simulator integration tests
|
||||
ament_add_gtest(test_imu_simulator test/test_imu_simulator.cpp
|
||||
src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.cpp
|
||||
src/simulator/Simulator.cpp)
|
||||
target_include_directories(test_imu_simulator PRIVATE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/src
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_imu_data_simulator_node)
|
||||
ament_target_dependencies(test_imu_simulator rclcpp sensor_msgs geometry_msgs)
|
||||
|
||||
# Wheel simulator integration tests
|
||||
ament_add_gtest(test_wheel_simulator test/test_wheel_simulator.cpp
|
||||
src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp
|
||||
src/simulator/Simulator.cpp)
|
||||
target_include_directories(test_wheel_simulator PRIVATE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/src
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_wheel_data_simulator_node)
|
||||
ament_target_dependencies(test_wheel_simulator rclcpp std_msgs)
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
|
||||
@@ -11,6 +11,7 @@
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
|
||||
171
src/g2_2025_odometry_pkg/test/test_imu_simulator.cpp
Normal file
171
src/g2_2025_odometry_pkg/test/test_imu_simulator.cpp
Normal file
@@ -0,0 +1,171 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/imu.hpp>
|
||||
#include "g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.hpp"
|
||||
#include <memory>
|
||||
|
||||
using namespace assignments::three::data_simulator_node;
|
||||
|
||||
class IMUSimulatorTest : public ::testing::Test {
|
||||
protected:
|
||||
static void SetUpTestSuite() {
|
||||
rclcpp::init(0, nullptr);
|
||||
}
|
||||
|
||||
static void TearDownTestSuite() {
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
|
||||
void setupBasicIMUParams(rclcpp::Node* node) {
|
||||
node->declare_parameter("max_intervals", 4);
|
||||
node->declare_parameter("publish_rate", 10.0);
|
||||
|
||||
// Setup linear_x
|
||||
node->declare_parameter("linear_x.num_intervals", 1);
|
||||
node->declare_parameter("linear_x.interval_0.type", "constant");
|
||||
node->declare_parameter("linear_x.interval_0.t_start", 0.0);
|
||||
node->declare_parameter("linear_x.interval_0.t_end", 5.0);
|
||||
node->declare_parameter("linear_x.interval_0.y_start", 1.0);
|
||||
node->declare_parameter("linear_x.interval_0.y_end", 0.0);
|
||||
node->declare_parameter("linear_x.interval_0.t_mid", 0.0);
|
||||
node->declare_parameter("linear_x.interval_0.y_mid", 0.0);
|
||||
|
||||
// Setup other axes with no intervals
|
||||
for (const auto& axis : { "linear_y", "linear_z", "angular_x", "angular_y", "angular_z" }) {
|
||||
node->declare_parameter(std::string(axis) + ".num_intervals", 0);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// Test node initialization
|
||||
TEST_F(IMUSimulatorTest, NodeInitialization) {
|
||||
auto node = std::make_shared<rclcpp::Node>("test_imu_init_node");
|
||||
setupBasicIMUParams(node.get());
|
||||
|
||||
// Should not throw
|
||||
EXPECT_NO_THROW({
|
||||
DataSimulator sim;
|
||||
});
|
||||
}
|
||||
|
||||
// Test IMU message publishing
|
||||
TEST_F(IMUSimulatorTest, MessagePublishing) {
|
||||
auto test_node = std::make_shared<rclcpp::Node>("test_imu_pub_node");
|
||||
|
||||
sensor_msgs::msg::Imu::SharedPtr received_msg;
|
||||
auto subscription = test_node->create_subscription<sensor_msgs::msg::Imu>(
|
||||
"simulated_imu_data",
|
||||
10,
|
||||
[&received_msg](sensor_msgs::msg::Imu::SharedPtr msg) {
|
||||
received_msg = msg;
|
||||
}
|
||||
);
|
||||
|
||||
auto sim_node = std::make_shared<DataSimulator>();
|
||||
|
||||
// Spin a few times to allow publishing
|
||||
for (int i = 0; i < 5; i++) {
|
||||
rclcpp::spin_some(sim_node);
|
||||
rclcpp::spin_some(test_node);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
}
|
||||
|
||||
// Check that we received a message
|
||||
ASSERT_NE(received_msg, nullptr);
|
||||
}
|
||||
|
||||
// Test IMU message frame_id
|
||||
TEST_F(IMUSimulatorTest, MessageFrameId) {
|
||||
auto test_node = std::make_shared<rclcpp::Node>("test_imu_frame_node");
|
||||
|
||||
sensor_msgs::msg::Imu::SharedPtr received_msg;
|
||||
auto subscription = test_node->create_subscription<sensor_msgs::msg::Imu>(
|
||||
"simulated_imu_data",
|
||||
10,
|
||||
[&received_msg](sensor_msgs::msg::Imu::SharedPtr msg) {
|
||||
received_msg = msg;
|
||||
}
|
||||
);
|
||||
|
||||
auto sim_node = std::make_shared<DataSimulator>();
|
||||
|
||||
// Spin a few times to allow publishing
|
||||
for (int i = 0; i < 5; i++) {
|
||||
rclcpp::spin_some(sim_node);
|
||||
rclcpp::spin_some(test_node);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
}
|
||||
|
||||
ASSERT_NE(received_msg, nullptr);
|
||||
EXPECT_EQ(received_msg->header.frame_id, "imu_link");
|
||||
}
|
||||
|
||||
// Test linear acceleration values
|
||||
TEST_F(IMUSimulatorTest, LinearAccelerationValues) {
|
||||
auto test_node = std::make_shared<rclcpp::Node>("test_imu_linear_node");
|
||||
|
||||
sensor_msgs::msg::Imu::SharedPtr received_msg;
|
||||
auto subscription = test_node->create_subscription<sensor_msgs::msg::Imu>(
|
||||
"simulated_imu_data",
|
||||
10,
|
||||
[&received_msg](sensor_msgs::msg::Imu::SharedPtr msg) {
|
||||
if (!received_msg) { // Only capture first message
|
||||
received_msg = msg;
|
||||
}
|
||||
}
|
||||
);
|
||||
|
||||
auto sim_node = std::make_shared<DataSimulator>();
|
||||
|
||||
// Spin until we receive a message
|
||||
for (int i = 0; i < 10 && !received_msg; i++) {
|
||||
rclcpp::spin_some(sim_node);
|
||||
rclcpp::spin_some(test_node);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
}
|
||||
|
||||
ASSERT_NE(received_msg, nullptr);
|
||||
|
||||
// Values should be set according to configuration
|
||||
// Since we don't control exact timing, just verify structure
|
||||
EXPECT_TRUE(std::isfinite(received_msg->linear_acceleration.x));
|
||||
EXPECT_TRUE(std::isfinite(received_msg->linear_acceleration.y));
|
||||
EXPECT_TRUE(std::isfinite(received_msg->linear_acceleration.z));
|
||||
}
|
||||
|
||||
// Test angular velocity values
|
||||
TEST_F(IMUSimulatorTest, AngularVelocityValues) {
|
||||
auto test_node = std::make_shared<rclcpp::Node>("test_imu_angular_node");
|
||||
|
||||
sensor_msgs::msg::Imu::SharedPtr received_msg;
|
||||
auto subscription = test_node->create_subscription<sensor_msgs::msg::Imu>(
|
||||
"simulated_imu_data",
|
||||
10,
|
||||
[&received_msg](sensor_msgs::msg::Imu::SharedPtr msg) {
|
||||
if (!received_msg) {
|
||||
received_msg = msg;
|
||||
}
|
||||
}
|
||||
);
|
||||
|
||||
auto sim_node = std::make_shared<DataSimulator>();
|
||||
|
||||
// Spin until we receive a message
|
||||
for (int i = 0; i < 10 && !received_msg; i++) {
|
||||
rclcpp::spin_some(sim_node);
|
||||
rclcpp::spin_some(test_node);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
}
|
||||
|
||||
ASSERT_NE(received_msg, nullptr);
|
||||
|
||||
// Values should be set according to configuration
|
||||
EXPECT_TRUE(std::isfinite(received_msg->angular_velocity.x));
|
||||
EXPECT_TRUE(std::isfinite(received_msg->angular_velocity.y));
|
||||
EXPECT_TRUE(std::isfinite(received_msg->angular_velocity.z));
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
257
src/g2_2025_odometry_pkg/test/test_simulator.cpp
Normal file
257
src/g2_2025_odometry_pkg/test/test_simulator.cpp
Normal file
@@ -0,0 +1,257 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "simulator/Simulator.hpp"
|
||||
#include <memory>
|
||||
|
||||
using namespace assignments::three;
|
||||
|
||||
class SimulatorTest : public ::testing::Test {
|
||||
protected:
|
||||
static void SetUpTestSuite() {
|
||||
rclcpp::init(0, nullptr);
|
||||
}
|
||||
|
||||
static void TearDownTestSuite() {
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
|
||||
// Helper to create a node with parameter overrides
|
||||
std::shared_ptr<rclcpp::Node> createNodeWithParams(
|
||||
const std::string& name,
|
||||
const std::vector<rclcpp::Parameter>& params)
|
||||
{
|
||||
rclcpp::NodeOptions options;
|
||||
options.parameter_overrides(params);
|
||||
return std::make_shared<rclcpp::Node>(name, options);
|
||||
}
|
||||
};
|
||||
|
||||
// Test constant interval
|
||||
TEST_F(SimulatorTest, ConstantInterval) {
|
||||
auto node = createNodeWithParams("test_constant_node", {
|
||||
rclcpp::Parameter("max_intervals", 4),
|
||||
rclcpp::Parameter("test_channel.num_intervals", 1),
|
||||
rclcpp::Parameter("test_channel.interval_0.type", "constant"),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_start", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_end", 10.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_start", 5.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_end", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0)
|
||||
});
|
||||
|
||||
std::vector<std::string> channels = { "test_channel" };
|
||||
Simulator sim(node.get(), channels); // Test values within interval
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 0.0), 5.0);
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 5.0);
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 10.0), 5.0);
|
||||
|
||||
// Test value after interval holds
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 15.0), 5.0);
|
||||
}
|
||||
|
||||
// Test linear interval
|
||||
TEST_F(SimulatorTest, LinearInterval) {
|
||||
auto node = createNodeWithParams("test_linear_node", {
|
||||
rclcpp::Parameter("max_intervals", 4),
|
||||
rclcpp::Parameter("test_channel.num_intervals", 1),
|
||||
rclcpp::Parameter("test_channel.interval_0.type", "linear"),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_start", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_end", 10.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_start", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_end", 10.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0)
|
||||
});
|
||||
|
||||
std::vector<std::string> channels = { "test_channel" };
|
||||
Simulator sim(node.get(), channels);
|
||||
|
||||
// Test linear interpolation
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 0.0), 0.0);
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 5.0);
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 10.0), 10.0);
|
||||
|
||||
// Test value after interval holds
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 15.0), 10.0);
|
||||
}
|
||||
|
||||
// Test quadratic interval
|
||||
TEST_F(SimulatorTest, QuadraticInterval) {
|
||||
auto node = createNodeWithParams("test_quadratic_node", {
|
||||
rclcpp::Parameter("max_intervals", 4),
|
||||
rclcpp::Parameter("test_channel.num_intervals", 1),
|
||||
rclcpp::Parameter("test_channel.interval_0.type", "quadratic"),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_start", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_end", 10.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_start", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_end", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_mid", 5.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_mid", 10.0)
|
||||
});
|
||||
|
||||
std::vector<std::string> channels = { "test_channel" };
|
||||
Simulator sim(node.get(), channels);
|
||||
|
||||
// Test quadratic interpolation
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 0.0), 0.0);
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 10.0);
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 10.0), 0.0);
|
||||
|
||||
// Test value after interval holds
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 15.0), 0.0);
|
||||
}
|
||||
|
||||
// Test multiple intervals
|
||||
TEST_F(SimulatorTest, MultipleIntervals) {
|
||||
auto node = createNodeWithParams("test_multiple_node", {
|
||||
rclcpp::Parameter("max_intervals", 4),
|
||||
rclcpp::Parameter("test_channel.num_intervals", 2),
|
||||
// First interval: constant 5.0 from t=0 to t=5
|
||||
rclcpp::Parameter("test_channel.interval_0.type", "constant"),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_start", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_end", 5.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_start", 5.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_end", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0),
|
||||
// Second interval: constant 10.0 from t=10 to t=15
|
||||
rclcpp::Parameter("test_channel.interval_1.type", "constant"),
|
||||
rclcpp::Parameter("test_channel.interval_1.t_start", 10.0),
|
||||
rclcpp::Parameter("test_channel.interval_1.t_end", 15.0),
|
||||
rclcpp::Parameter("test_channel.interval_1.y_start", 10.0),
|
||||
rclcpp::Parameter("test_channel.interval_1.y_end", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_1.t_mid", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_1.y_mid", 0.0)
|
||||
});
|
||||
|
||||
std::vector<std::string> channels = { "test_channel" };
|
||||
Simulator sim(node.get(), channels);
|
||||
|
||||
// Test first interval
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 2.5), 5.0);
|
||||
|
||||
// Test gap between intervals - should hold value from first interval
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 7.5), 5.0);
|
||||
|
||||
// Test second interval
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 12.5), 10.0);
|
||||
|
||||
// Test after all intervals - should hold value from last interval
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 20.0), 10.0);
|
||||
}
|
||||
|
||||
// Test value before any interval
|
||||
TEST_F(SimulatorTest, ValueBeforeIntervals) {
|
||||
auto node = createNodeWithParams("test_before_node", {
|
||||
rclcpp::Parameter("max_intervals", 4),
|
||||
rclcpp::Parameter("test_channel.num_intervals", 1),
|
||||
rclcpp::Parameter("test_channel.interval_0.type", "constant"),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_start", 10.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_end", 20.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_start", 5.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_end", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0)
|
||||
});
|
||||
|
||||
std::vector<std::string> channels = { "test_channel" };
|
||||
Simulator sim(node.get(), channels);
|
||||
|
||||
// Before any interval starts, should return 0.0
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 0.0);
|
||||
}
|
||||
|
||||
// Test non-existent channel
|
||||
TEST_F(SimulatorTest, NonExistentChannel) {
|
||||
auto node = createNodeWithParams("test_nonexistent_node", {
|
||||
rclcpp::Parameter("max_intervals", 4),
|
||||
rclcpp::Parameter("test_channel.num_intervals", 1),
|
||||
rclcpp::Parameter("test_channel.interval_0.type", "constant"),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_start", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_end", 10.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_start", 5.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_end", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0)
|
||||
});
|
||||
|
||||
std::vector<std::string> channels = { "test_channel" };
|
||||
Simulator sim(node.get(), channels);
|
||||
|
||||
// Query non-existent channel should return 0.0
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("nonexistent_channel", 5.0), 0.0);
|
||||
}
|
||||
|
||||
// Test overlapping intervals detection
|
||||
TEST_F(SimulatorTest, OverlappingIntervalsThrowsException) {
|
||||
auto node = createNodeWithParams("test_overlap_node", {
|
||||
rclcpp::Parameter("max_intervals", 4),
|
||||
rclcpp::Parameter("test_channel.num_intervals", 2),
|
||||
// First interval: t=0 to t=10
|
||||
rclcpp::Parameter("test_channel.interval_0.type", "constant"),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_start", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_end", 10.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_start", 5.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_end", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0),
|
||||
// Second interval: t=5 to t=15 (overlaps with first)
|
||||
rclcpp::Parameter("test_channel.interval_1.type", "constant"),
|
||||
rclcpp::Parameter("test_channel.interval_1.t_start", 5.0),
|
||||
rclcpp::Parameter("test_channel.interval_1.t_end", 15.0),
|
||||
rclcpp::Parameter("test_channel.interval_1.y_start", 10.0),
|
||||
rclcpp::Parameter("test_channel.interval_1.y_end", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_1.t_mid", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_1.y_mid", 0.0)
|
||||
});
|
||||
|
||||
std::vector<std::string> channels = { "test_channel" };
|
||||
|
||||
// Should throw exception due to overlapping intervals
|
||||
EXPECT_THROW({
|
||||
Simulator sim(node.get(), channels);
|
||||
}, std::runtime_error);
|
||||
}
|
||||
|
||||
// Test max_intervals limit
|
||||
TEST_F(SimulatorTest, MaxIntervalsLimit) {
|
||||
auto node = createNodeWithParams("test_max_intervals_node", {
|
||||
rclcpp::Parameter("max_intervals", 2),
|
||||
rclcpp::Parameter("test_channel.num_intervals", 3),
|
||||
rclcpp::Parameter("test_channel.interval_0.type", "constant"),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_start", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_end", 10.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_start", 1.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_end", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_1.type", "constant"),
|
||||
rclcpp::Parameter("test_channel.interval_1.t_start", 10.0),
|
||||
rclcpp::Parameter("test_channel.interval_1.t_end", 20.0),
|
||||
rclcpp::Parameter("test_channel.interval_1.y_start", 2.0),
|
||||
rclcpp::Parameter("test_channel.interval_1.y_end", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_1.t_mid", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_1.y_mid", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_2.type", "constant"),
|
||||
rclcpp::Parameter("test_channel.interval_2.t_start", 20.0),
|
||||
rclcpp::Parameter("test_channel.interval_2.t_end", 30.0),
|
||||
rclcpp::Parameter("test_channel.interval_2.y_start", 3.0),
|
||||
rclcpp::Parameter("test_channel.interval_2.y_end", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_2.t_mid", 0.0),
|
||||
rclcpp::Parameter("test_channel.interval_2.y_mid", 0.0)
|
||||
});
|
||||
|
||||
std::vector<std::string> channels = { "test_channel" };
|
||||
Simulator sim(node.get(), channels);
|
||||
|
||||
// Only first 2 intervals should be loaded
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 1.0); // First interval
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 15.0), 2.0); // Second interval
|
||||
EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 25.0), 2.0); // Third interval should not exist, holds second
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
168
src/g2_2025_odometry_pkg/test/test_wheel_simulator.cpp
Normal file
168
src/g2_2025_odometry_pkg/test/test_wheel_simulator.cpp
Normal file
@@ -0,0 +1,168 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <std_msgs/msg/float64_multi_array.hpp>
|
||||
#include "g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.hpp"
|
||||
#include <memory>
|
||||
|
||||
using namespace assignments::three::wheel_data_simulator_node;
|
||||
|
||||
class WheelSimulatorTest : public ::testing::Test {
|
||||
protected:
|
||||
static void SetUpTestSuite() {
|
||||
rclcpp::init(0, nullptr);
|
||||
}
|
||||
|
||||
static void TearDownTestSuite() {
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
|
||||
void setupBasicWheelParams(rclcpp::Node* node) {
|
||||
node->declare_parameter("max_intervals", 4);
|
||||
node->declare_parameter("publish_rate", 10.0);
|
||||
|
||||
// Setup wheel_fl
|
||||
node->declare_parameter("wheel_fl.num_intervals", 1);
|
||||
node->declare_parameter("wheel_fl.interval_0.type", "linear");
|
||||
node->declare_parameter("wheel_fl.interval_0.t_start", 0.0);
|
||||
node->declare_parameter("wheel_fl.interval_0.t_end", 10.0);
|
||||
node->declare_parameter("wheel_fl.interval_0.y_start", 0.0);
|
||||
node->declare_parameter("wheel_fl.interval_0.y_end", 5.0);
|
||||
node->declare_parameter("wheel_fl.interval_0.t_mid", 0.0);
|
||||
node->declare_parameter("wheel_fl.interval_0.y_mid", 0.0);
|
||||
|
||||
// Setup other wheels with no intervals
|
||||
for (const auto& wheel : {"wheel_fr", "wheel_bl", "wheel_br"}) {
|
||||
node->declare_parameter(std::string(wheel) + ".num_intervals", 0);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// Test node initialization
|
||||
TEST_F(WheelSimulatorTest, NodeInitialization) {
|
||||
auto node = std::make_shared<rclcpp::Node>("test_wheel_init_node");
|
||||
setupBasicWheelParams(node.get());
|
||||
|
||||
// Should not throw
|
||||
EXPECT_NO_THROW({
|
||||
DataSimulator sim;
|
||||
});
|
||||
}
|
||||
|
||||
// Test wheel message publishing
|
||||
TEST_F(WheelSimulatorTest, WheelDataPublishing) {
|
||||
auto test_node = std::make_shared<rclcpp::Node>("test_wheel_pub_node");
|
||||
|
||||
std_msgs::msg::Float64MultiArray::SharedPtr received_msg;
|
||||
auto subscription = test_node->create_subscription<std_msgs::msg::Float64MultiArray>(
|
||||
"simulated_wheel_data",
|
||||
10,
|
||||
[&received_msg](std_msgs::msg::Float64MultiArray::SharedPtr msg) {
|
||||
received_msg = msg;
|
||||
}
|
||||
);
|
||||
|
||||
auto sim_node = std::make_shared<DataSimulator>();
|
||||
|
||||
// Spin a few times to allow publishing
|
||||
for (int i = 0; i < 5; i++) {
|
||||
rclcpp::spin_some(sim_node);
|
||||
rclcpp::spin_some(test_node);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
}
|
||||
|
||||
// Check that we received a message
|
||||
ASSERT_NE(received_msg, nullptr);
|
||||
}
|
||||
|
||||
// Test wheel data array size
|
||||
TEST_F(WheelSimulatorTest, WheelDataArraySize) {
|
||||
auto test_node = std::make_shared<rclcpp::Node>("test_wheel_size_node");
|
||||
|
||||
std_msgs::msg::Float64MultiArray::SharedPtr received_msg;
|
||||
auto subscription = test_node->create_subscription<std_msgs::msg::Float64MultiArray>(
|
||||
"simulated_wheel_data",
|
||||
10,
|
||||
[&received_msg](std_msgs::msg::Float64MultiArray::SharedPtr msg) {
|
||||
if (!received_msg) {
|
||||
received_msg = msg;
|
||||
}
|
||||
}
|
||||
);
|
||||
|
||||
auto sim_node = std::make_shared<DataSimulator>();
|
||||
|
||||
// Spin until we receive a message
|
||||
for (int i = 0; i < 10 && !received_msg; i++) {
|
||||
rclcpp::spin_some(sim_node);
|
||||
rclcpp::spin_some(test_node);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
}
|
||||
|
||||
ASSERT_NE(received_msg, nullptr);
|
||||
// Should have 4 wheel values: FL, FR, RL, RR
|
||||
EXPECT_EQ(received_msg->data.size(), 4);
|
||||
}
|
||||
|
||||
// Test wheel velocity values are finite
|
||||
TEST_F(WheelSimulatorTest, ValidVelocityValues) {
|
||||
auto test_node = std::make_shared<rclcpp::Node>("test_wheel_values_node");
|
||||
|
||||
std_msgs::msg::Float64MultiArray::SharedPtr received_msg;
|
||||
auto subscription = test_node->create_subscription<std_msgs::msg::Float64MultiArray>(
|
||||
"simulated_wheel_data",
|
||||
10,
|
||||
[&received_msg](std_msgs::msg::Float64MultiArray::SharedPtr msg) {
|
||||
if (!received_msg) {
|
||||
received_msg = msg;
|
||||
}
|
||||
}
|
||||
);
|
||||
|
||||
auto sim_node = std::make_shared<DataSimulator>();
|
||||
|
||||
// Spin until we receive a message
|
||||
for (int i = 0; i < 10 && !received_msg; i++) {
|
||||
rclcpp::spin_some(sim_node);
|
||||
rclcpp::spin_some(test_node);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
}
|
||||
|
||||
ASSERT_NE(received_msg, nullptr);
|
||||
ASSERT_EQ(received_msg->data.size(), 4);
|
||||
|
||||
// All values should be finite
|
||||
for (const auto& val : received_msg->data) {
|
||||
EXPECT_TRUE(std::isfinite(val));
|
||||
}
|
||||
}
|
||||
|
||||
// Test multiple wheel messages received
|
||||
TEST_F(WheelSimulatorTest, MultipleMessagesReceived) {
|
||||
auto test_node = std::make_shared<rclcpp::Node>("test_multi_wheel_node");
|
||||
|
||||
int msg_count = 0;
|
||||
auto subscription = test_node->create_subscription<std_msgs::msg::Float64MultiArray>(
|
||||
"simulated_wheel_data",
|
||||
10,
|
||||
[&msg_count](std_msgs::msg::Float64MultiArray::SharedPtr) {
|
||||
msg_count++;
|
||||
}
|
||||
);
|
||||
|
||||
auto sim_node = std::make_shared<DataSimulator>();
|
||||
|
||||
// Spin for a bit to receive messages
|
||||
for (int i = 0; i < 10; i++) {
|
||||
rclcpp::spin_some(sim_node);
|
||||
rclcpp::spin_some(test_node);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
}
|
||||
|
||||
// Should have received at least one message
|
||||
EXPECT_GT(msg_count, 0);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
Reference in New Issue
Block a user