Skip to content

Commit

Permalink
Add pose and tf composition + composition tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@giafranchini
giafranchini committed Nov 29, 2024
1 parent 182993b commit 7f81d1e
Show file tree
Hide file tree
Showing 5 changed files with 131 additions and 104 deletions.
35 changes: 14 additions & 21 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,8 @@ add_compile_options(-Wall -Wextra -Wpedantic -Wconversion -Wdouble-promotion -Wf
find_package(ament_cmake REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)

include(ExternalProject)
find_package(Git REQUIRED)
Expand Down Expand Up @@ -57,7 +58,8 @@ ament_target_dependencies(
${PROJECT_NAME}
Eigen3
geometry_msgs
tf2_ros
tf2
tf2_geometry_msgs
)

install(
Expand All @@ -80,39 +82,30 @@ ament_export_dependencies(
Eigen3
covariance_geometry_cpp
geometry_msgs
tf2_ros
tf2
tf2_geometry_msgs
)

if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
find_package(ament_cmake_gtest REQUIRED)
find_package(ament_cmake_gmock REQUIRED)

ament_lint_auto_find_test_dependencies()
ament_find_gtest()
ament_find_gmock()

set(TEST_NAMES
conversion_test
# composition_test
composition_test
)

foreach(TEST_NAME ${TEST_NAMES})
ament_add_gtest(${TEST_NAME} test/${TEST_NAME}.cpp)

add_dependencies(
${TEST_NAME}
covariance_geometry_cpp
)

target_link_libraries(
${TEST_NAME}
${CMAKE_CURRENT_BINARY_DIR}/covariance_geometry_cpp/src/lib/libcovariance_geometry_cpp.so
)

ament_target_dependencies(
${TEST_NAME}
Eigen3
geometry_msgs
tf2_ros
${PROJECT_NAME}
)

endforeach()
endif()

Expand Down
14 changes: 11 additions & 3 deletions include/covariance_geometry_ros/covariance_geometry_ros.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,9 +14,11 @@
#ifndef COVARIANCE_GEOMETRY_ROS__COVARIANCE_GEOMETRY_ROS_HPP_
#define COVARIANCE_GEOMETRY_ROS__COVARIANCE_GEOMETRY_ROS_HPP_

#include <tf2/LinearMath/Transform.h>

#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_with_covariance.hpp>
// #include <tf2/LinearMath/Transform.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

using Pose = geometry_msgs::msg::Pose;
using PoseWithCovariance = geometry_msgs::msg::PoseWithCovariance;
Expand Down Expand Up @@ -69,8 +71,14 @@ static inline PoseWithCovariance compose(
}

// Return-by-value versions using TF2 transforms:
// PoseWithCovariance compose(const PoseWithCovariance &a,
// const tf2::Transform &b);
Pose compose(
const Pose & a,
const tf2::Transform & b);

PoseWithCovariance compose(
const PoseWithCovariance & a,
const tf2::Transform & b);

/** @} */
/** @name Pose inverse composition (a "as seen from" b): out = a (-) b
@{ */
Expand Down
7 changes: 0 additions & 7 deletions include/covariance_geometry_ros/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,6 @@

#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_with_covariance.hpp>
// #include <tf2/LinearMath/Transform.h>

using Pose = geometry_msgs::msg::Pose;
using PoseWithCovariance = geometry_msgs::msg::PoseWithCovariance;
Expand All @@ -45,12 +44,6 @@ inline void fromROS(const PoseWithCovariance & in, PoseQuaternionCovarianceRPY &
out.second = cov;
}

// inline void fromROS(const tf2::Transform &in, PoseQuaternion &out)
// {
// out.first(in.getOrigin());
// out.second(in.getRotation());
// }

/** @name Convert from covariance geometry ROS type to ROS msgs
@{ */
inline void toROS(const PoseQuaternion & in, Pose & out)
Expand Down
20 changes: 20 additions & 0 deletions src/covariance_geometry_ros.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,26 @@ void compose(
toROS(pose_out, out);
}

Pose compose(
const Pose & a,
const tf2::Transform & b)
{
Pose b_msg, out;
tf2::toMsg(b, b_msg);
compose(a, b_msg, out);
return out;
}

PoseWithCovariance compose(
const PoseWithCovariance & a,
const tf2::Transform & b)
{
PoseWithCovariance b_msg, out;
tf2::toMsg(b, b_msg.pose);
compose(a, b_msg, out);
return out;
}

// Compose a with b^-1
void inverseCompose(const Pose & a, const Pose & b, Pose & out)
{
Expand Down
159 changes: 86 additions & 73 deletions test/composition_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,96 +11,109 @@
// 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 "covariance_geometry/pose_covariance_composition.hpp"
#include "covariance_geometry_ros/utils.hpp"

#include "gtest/gtest.h"
#include <tf2/LinearMath/Transform.h>

#include <geometry_msgs/msg/pose_with_covariance.hpp>
#include "covariance_geometry/pose_representation.hpp"
#include "covariance_geometry/pose_covariance_representation.hpp"
#include "covariance_geometry_ros/covariance_geometry_ros.hpp"

using Pose = geometry_msgs::msg::Pose;
using PoseWithCovariance = geometry_msgs::msg::PoseWithCovariance;
#include "gtest/gtest.h"

namespace covariance_geometry
{

const Eigen::Vector3d coord = {0.111, -1.24, 0.35}; // x, y, z
const Eigen::Vector3d rpy = {0.9067432, 0.4055079, 0.1055943}; // r, p, y
const Eigen::Quaterniond quat = {0.8746791, 0.4379822, 0.1581314, 0.1345454}; // w, x, y, z

const Eigen::Vector3d rpy_gl = {0.12, M_PI_2, 0.34}; // r, p, y
const Eigen::Quaterniond quat_gl = {0.6884861, 0.1612045, 0.6884861, 0.1612045}; // w, x, y, z
const Eigen::Vector3d coord1 = {-0.333330, 0, 0.100000}; // x, y, z
const Eigen::Quaterniond quat1 = {0.707107, 0.0, 0.707107, -0.000005}; // w, x, y, z
const Eigen::Vector3d coord2 = {1.435644, 0, 0.0}; // x, y, z
const Eigen::Quaterniond quat2 = {1.0, 0.0, 0.0, 0.0}; // w, x, y, z

TEST(Composition, ROSPoses)
{
Pose pose_in;
PoseQuaternion pose_out;
pose_in.position.x = coord.x();
pose_in.position.y = coord.y();
pose_in.position.z = coord.z();
pose_in.orientation.x = quat.x();
pose_in.orientation.y = quat.y();
pose_in.orientation.z = quat.z();
pose_in.orientation.w = quat.w();
fromROS(pose_in, pose_out);
EXPECT_TRUE(pose_out.first.isApprox(coord));
EXPECT_TRUE(pose_out.second.isApprox(quat));
geometry_msgs::msg::Pose pose1, pose2, pose_out;

pose1.position.x = coord1.x();
pose1.position.y = coord1.y();
pose1.position.z = coord1.z();
pose1.orientation.x = quat1.x();
pose1.orientation.y = quat1.y();
pose1.orientation.z = quat1.z();
pose1.orientation.w = quat1.w();

pose2.position.x = coord2.x();
pose2.position.y = coord2.y();
pose2.position.z = coord2.z();
pose2.orientation.x = quat2.x();
pose2.orientation.y = quat2.y();
pose2.orientation.z = quat2.z();
pose2.orientation.w = quat2.w();

pose_out = compose(pose1, pose2);
EXPECT_NEAR(pose_out.position.x, coord1.x(), 1e-6);
}

TEST(Conversion, ConvertPoseWithCovarianceFromROS)
TEST(Composition, ROSPoseAndTf)
{
PoseWithCovariance pose_in;
PoseQuaternionCovarianceRPY pose_out;
pose_in.pose.position.x = coord.x();
pose_in.pose.position.y = coord.y();
pose_in.pose.position.z = coord.z();
pose_in.pose.orientation.x = quat.x();
pose_in.pose.orientation.y = quat.y();
pose_in.pose.orientation.z = quat.z();
pose_in.pose.orientation.w = quat.w();
Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor>> cov(pose_in.covariance.data());
cov = Eigen::Matrix<double, 6, 6, Eigen::RowMajor>::Random();
cov.selfadjointView<Eigen::Upper>();
Eigen::Matrix<double, 6, 6, Eigen::RowMajor> cov_out;
cov_out = cov;
fromROS(pose_in, pose_out);
EXPECT_TRUE(pose_out.first.first.isApprox(coord));
EXPECT_TRUE(pose_out.first.second.isApprox(quat));
EXPECT_TRUE(pose_out.second.isApprox(cov_out));
geometry_msgs::msg::Pose pose, pose_out;
tf2::Transform tf;

pose.position.x = coord1.x();
pose.position.y = coord1.y();
pose.position.z = coord1.z();
pose.orientation.x = quat1.x();
pose.orientation.y = quat1.y();
pose.orientation.z = quat1.z();
pose.orientation.w = quat1.w();

tf.setOrigin(tf2::Vector3(coord2.x(), coord2.y(), coord2.z()));
tf.setRotation(tf2::Quaternion(quat2.x(), quat2.y(), quat2.z(), quat2.w()));

pose_out = compose(pose, tf);
EXPECT_NEAR(pose_out.position.x, coord1.x(), 1e-6);
}

TEST(Conversion, ConvertPoseToROS)
TEST(Composition, ROSPoseAndTfWithCovariance)
{
PoseQuaternion pose_in;
Pose pose_out;
pose_in.first = coord;
pose_in.second = quat;
toROS(pose_in, pose_out);
EXPECT_DOUBLE_EQ(pose_out.position.x, coord.x());
EXPECT_DOUBLE_EQ(pose_out.position.y, coord.y());
EXPECT_DOUBLE_EQ(pose_out.position.z, coord.z());
EXPECT_DOUBLE_EQ(pose_out.orientation.x, quat.x());
EXPECT_DOUBLE_EQ(pose_out.orientation.y, quat.y());
EXPECT_DOUBLE_EQ(pose_out.orientation.z, quat.z());
EXPECT_DOUBLE_EQ(pose_out.orientation.w, quat.w());
geometry_msgs::msg::PoseWithCovariance pose, pose_out;
tf2::Transform tf;

pose.pose.position.x = coord1.x();
pose.pose.position.y = coord1.y();
pose.pose.position.z = coord1.z();
pose.pose.orientation.x = quat1.x();
pose.pose.orientation.y = quat1.y();
pose.pose.orientation.z = quat1.z();
pose.pose.orientation.w = quat1.w();

tf.setOrigin(tf2::Vector3(coord2.x(), coord2.y(), coord2.z()));
tf.setRotation(tf2::Quaternion(quat2.x(), quat2.y(), quat2.z(), quat2.w()));

pose_out = compose(pose, tf);
EXPECT_NEAR(pose_out.pose.position.x, coord1.x(), 1e-6);
}

TEST(Conversion, ConvertPoseWithCovarianceToROS)
TEST(Composition, ROSPosesWithCovariance)
{
PoseQuaternionCovarianceRPY pose_in;
PoseWithCovariance pose_out;
pose_in.first.first = coord;
pose_in.first.second = quat;
pose_in.second = Eigen::Matrix<double, 6, 6>::Random();
pose_in.second.selfadjointView<Eigen::Upper>();

toROS(pose_in, pose_out);
EXPECT_DOUBLE_EQ(pose_out.pose.position.x, coord.x());
EXPECT_DOUBLE_EQ(pose_out.pose.position.y, coord.y());
EXPECT_DOUBLE_EQ(pose_out.pose.position.z, coord.z());
EXPECT_DOUBLE_EQ(pose_out.pose.orientation.x, quat.x());
EXPECT_DOUBLE_EQ(pose_out.pose.orientation.y, quat.y());
EXPECT_DOUBLE_EQ(pose_out.pose.orientation.z, quat.z());
EXPECT_DOUBLE_EQ(pose_out.pose.orientation.w, quat.w());
Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor>> cov(pose_out.covariance.data());
EXPECT_TRUE(cov.isApprox(pose_in.second));
geometry_msgs::msg::PoseWithCovariance pose1, pose2, pose_out;

pose1.pose.position.x = coord1.x();
pose1.pose.position.y = coord1.y();
pose1.pose.position.z = coord1.z();
pose1.pose.orientation.x = quat1.x();
pose1.pose.orientation.y = quat1.y();
pose1.pose.orientation.z = quat1.z();
pose1.pose.orientation.w = quat1.w();

pose2.pose.position.x = coord2.x();
pose2.pose.position.y = coord2.y();
pose2.pose.position.z = coord2.z();
pose2.pose.orientation.x = quat2.x();
pose2.pose.orientation.y = quat2.y();
pose2.pose.orientation.z = quat2.z();
pose2.pose.orientation.w = quat2.w();

pose_out = compose(pose1, pose2);
EXPECT_NEAR(pose_out.pose.position.x, coord1.x(), 1e-6);
}
} // namespace covariance_geometry

0 comments on commit 7f81d1e

Please sign in to comment.