d31896a545
* Implement frame ID checking and test it * Implement the visual odom function that does narrowing to floats * Normalize quaternions from ROS * Supply 0 error to EKF * Handle external odomo only if HAL_VISUALODOM_ENABLED is defined * Implement odom timestamping and improve docs * Add unit tests * Add a CONFIG file for DDS Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
39 lines
1.5 KiB
C++
39 lines
1.5 KiB
C++
#include <AP_gtest.h>
|
|
|
|
#include <AP_DDS/AP_DDS_External_Odom.h>
|
|
#include "geometry_msgs/msg/TransformStamped.h"
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
const AP_HAL::HAL &hal = AP_HAL::get_HAL();
|
|
|
|
TEST(AP_DDS_EXTERNAL_ODOM, test_is_odometry_success)
|
|
{
|
|
geometry_msgs_msg_TransformStamped msg {};
|
|
|
|
strncpy(msg.header.frame_id, "odom", strlen("odom") + 1);
|
|
strncpy(msg.child_frame_id, "base_link", strlen("base_link") + 1);
|
|
ASSERT_TRUE(AP_DDS_External_Odom::is_odometry_frame(msg));
|
|
|
|
strncpy(msg.header.frame_id, "invalid", strlen("invalid") + 1);
|
|
strncpy(msg.child_frame_id, "base_link", strlen("base_link") + 1);
|
|
ASSERT_FALSE(AP_DDS_External_Odom::is_odometry_frame(msg));
|
|
|
|
strncpy(msg.header.frame_id, "odom", strlen("odom") + 1);
|
|
strncpy(msg.child_frame_id, "invalid", strlen("invalid") + 1);
|
|
ASSERT_FALSE(AP_DDS_External_Odom::is_odometry_frame(msg));
|
|
|
|
strncpy(msg.header.frame_id, "odom_with_invalid_extra", strlen("odom_with_invalid_extra") + 1);
|
|
strncpy(msg.child_frame_id, "base_link", strlen("base_link") + 1);
|
|
ASSERT_FALSE(AP_DDS_External_Odom::is_odometry_frame(msg));
|
|
|
|
strncpy(msg.header.frame_id, "odom", strlen("odom") + 1);
|
|
strncpy(msg.child_frame_id, "base_link_with_invalid_extra", strlen("base_link_with_invalid_extra") + 1);
|
|
ASSERT_FALSE(AP_DDS_External_Odom::is_odometry_frame(msg));
|
|
|
|
strncpy(msg.header.frame_id, "x", strlen("x") + 1);
|
|
strncpy(msg.child_frame_id, "base_link", strlen("base_link") + 1);
|
|
ASSERT_FALSE(AP_DDS_External_Odom::is_odometry_frame(msg));
|
|
}
|
|
|
|
AP_GTEST_MAIN()
|