From fbb38e3b76451ecd6b3af3e6f1ca5374a7be2226 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Thu, 4 Jan 2024 16:37:54 +0100 Subject: [PATCH] CI (#3) * Workflows * Linting * README badges --- .github/workflows/iron.yaml | 38 ++++ .github/workflows/rolling.yaml | 38 ++++ CMakeLists.txt | 14 +- README.md | 4 + .../covariance_geometry_ros.hpp | 187 ++++++++++-------- include/covariance_geometry_ros/utils.hpp | 109 +++++----- src/covariance_geometry_ros.cpp | 186 +++++++++-------- test/composition_test.cpp | 4 +- test/conversion_test.cpp | 4 +- 9 files changed, 365 insertions(+), 219 deletions(-) create mode 100644 .github/workflows/iron.yaml create mode 100644 .github/workflows/rolling.yaml diff --git a/.github/workflows/iron.yaml b/.github/workflows/iron.yaml new file mode 100644 index 0000000..9e24c49 --- /dev/null +++ b/.github/workflows/iron.yaml @@ -0,0 +1,38 @@ +name: Ubuntu 22.04 Iron Build + +on: + pull_request: + push: + branches: + - iron + +jobs: + Build: + runs-on: ubuntu-latest + container: + image: osrf/ros:iron-desktop-full-jammy + + steps: + - name: Update + run: apt update + + - name: Install PIP + run: apt install -y python3-pip lcov + + - name: Install colcon tools + run: python3 -m pip install colcon-lcov-result colcon-coveragepy-result + + - name: Checkout + uses: actions/checkout@v2 + + - name: Run Tests + uses: ros-tooling/action-ros-ci@0.3.5 + with: + target-ros2-distro: iron + + - name: Upload Logs + uses: actions/upload-artifact@v1 + with: + name: colcon-logs + path: ros_ws/log + if: always() \ No newline at end of file diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml new file mode 100644 index 0000000..716582e --- /dev/null +++ b/.github/workflows/rolling.yaml @@ -0,0 +1,38 @@ +name: Ubuntu 22.04 Rolling Build + +on: + pull_request: + push: + branches: + - iron + +jobs: + Build: + runs-on: ubuntu-latest + container: + image: osrf/ros:rolling-desktop-full-jammy + + steps: + - name: Update + run: apt update + + - name: Install PIP + run: apt install -y python3-pip lcov + + - name: Install colcon tools + run: python3 -m pip install colcon-lcov-result colcon-coveragepy-result + + - name: Checkout + uses: actions/checkout@v2 + + - name: Run Tests + uses: ros-tooling/action-ros-ci@0.3.5 + with: + target-ros2-distro: rolling + + - name: Upload Logs + uses: actions/upload-artifact@v1 + with: + name: colcon-logs + path: ros_ws/log + if: always() \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index ac873b6..84ab2a7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,7 +19,7 @@ find_package(tf2_ros REQUIRED) include(ExternalProject) find_package(Git REQUIRED) -ExternalProject_Add( +externalproject_add( covariance_geometry_cpp PREFIX ${CMAKE_CURRENT_BINARY_DIR}/covariance_geometry_cpp GIT_REPOSITORY https://github.com/andreaostuni/covariance_geometry.git @@ -39,8 +39,8 @@ include_directories( add_library(${PROJECT_NAME} SHARED src/covariance_geometry_ros.cpp) target_include_directories( - ${PROJECT_NAME} - PUBLIC + ${PROJECT_NAME} + PUBLIC "$" "$" ) @@ -78,7 +78,7 @@ install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/covariance_geometry_cpp/src/lib/ D ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies( +ament_export_dependencies( Eigen3 covariance_geometry_cpp geometry_msgs @@ -89,12 +89,12 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - + set(TEST_NAMES conversion_test # composition_test - ) - + ) + foreach(TEST_NAME ${TEST_NAMES}) ament_add_gtest(${TEST_NAME} test/${TEST_NAME}.cpp) diff --git a/README.md b/README.md index 84385f1..247b7cb 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,6 @@ ## covariance_geometry_ros + +[![Ubuntu 22.04 Iron Build](https://github.com/giafranchini/covariance_geometry_ros/actions/workflows/iron.yaml/badge.svg)](https://github.com/giafranchini/covariance_geometry_ros/actions/workflows/iron.yaml) +[![Ubuntu 22.04 Rolling Build](https://github.com/giafranchini/covariance_geometry_ros/actions/workflows/rolling.yaml/badge.svg)](https://github.com/giafranchini/covariance_geometry_ros/actions/workflows/rolling.yaml) + A simple ROS2 wrapper for poses and covariances composition based on covariance_geometry library. diff --git a/include/covariance_geometry_ros/covariance_geometry_ros.hpp b/include/covariance_geometry_ros/covariance_geometry_ros.hpp index 0f1a817..30f7e2e 100644 --- a/include/covariance_geometry_ros/covariance_geometry_ros.hpp +++ b/include/covariance_geometry_ros/covariance_geometry_ros.hpp @@ -1,5 +1,18 @@ -#ifndef COVARIANCE_GEOMETRY_ROS_HPP_ -#define COVARIANCE_GEOMETRY_ROS_HPP_ +// Copyright 2023 Andrea Ostuni, Giacomo Franchini - PIC4SeR +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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. +#ifndef COVARIANCE_GEOMETRY_ROS__COVARIANCE_GEOMETRY_ROS_HPP_ +#define COVARIANCE_GEOMETRY_ROS__COVARIANCE_GEOMETRY_ROS_HPP_ #include #include @@ -10,83 +23,103 @@ using PoseWithCovariance = geometry_msgs::msg::PoseWithCovariance; namespace covariance_geometry { - /** @name Pose composition: out = a (+) b +/** @name Pose composition: out = a (+) b @{ */ - void compose(const Pose &a, const Pose &b, Pose &out); - void compose(const PoseWithCovariance &a, const PoseWithCovariance &b, - PoseWithCovariance &out); - void compose(const PoseWithCovariance &a, const Pose &b, - PoseWithCovariance &out); - void compose(const Pose &a, const PoseWithCovariance &b, - PoseWithCovariance &out); - - // Return-by-value versions: - static inline Pose compose(const Pose &a, const Pose &b) { - Pose out; - compose(a, b, out); - return out; - } - static inline PoseWithCovariance compose(const PoseWithCovariance &a, - const PoseWithCovariance &b) { - PoseWithCovariance out; - compose(a, b, out); - return out; - } - static inline PoseWithCovariance compose(const PoseWithCovariance &a, - const Pose &b) { - PoseWithCovariance out; - compose(a, b, out); - return out; - } - static inline PoseWithCovariance compose(const Pose &a, - const PoseWithCovariance &b) { - PoseWithCovariance out; - compose(a, b, out); - return out; - } +void compose(const Pose & a, const Pose & b, Pose & out); +void compose( + const PoseWithCovariance & a, const PoseWithCovariance & b, + PoseWithCovariance & out); +void compose( + const PoseWithCovariance & a, const Pose & b, + PoseWithCovariance & out); +void compose( + const Pose & a, const PoseWithCovariance & b, + PoseWithCovariance & out); - // Return-by-value versions using TF2 transforms: - // PoseWithCovariance compose(const PoseWithCovariance &a, - // const tf2::Transform &b); - /** @} */ - /** @name Pose inverse composition (a "as seen from" b): out = a (-) b +// Return-by-value versions: +static inline Pose compose(const Pose & a, const Pose & b) +{ + Pose out; + compose(a, b, out); + return out; +} +static inline PoseWithCovariance compose( + const PoseWithCovariance & a, + const PoseWithCovariance & b) +{ + PoseWithCovariance out; + compose(a, b, out); + return out; +} +static inline PoseWithCovariance compose( + const PoseWithCovariance & a, + const Pose & b) +{ + PoseWithCovariance out; + compose(a, b, out); + return out; +} +static inline PoseWithCovariance compose( + const Pose & a, + const PoseWithCovariance & b) +{ + PoseWithCovariance out; + compose(a, b, out); + return out; +} + +// Return-by-value versions using TF2 transforms: +// PoseWithCovariance compose(const PoseWithCovariance &a, +// const tf2::Transform &b); +/** @} */ +/** @name Pose inverse composition (a "as seen from" b): out = a (-) b @{ */ - void inverseCompose(const Pose &a, const Pose &b, Pose &out); - void inverseCompose(const PoseWithCovariance &a, const PoseWithCovariance &b, - PoseWithCovariance &out); - void inverseCompose(const PoseWithCovariance &a, const Pose &b, - PoseWithCovariance &out); - void inverseCompose(const Pose &a, const PoseWithCovariance &b, - PoseWithCovariance &out); - // Return-by-value versions: - static inline Pose inverseCompose(const Pose &a, const Pose &b) { - Pose out; - inverseCompose(a, b, out); - return out; - } - static inline PoseWithCovariance inverseCompose(const PoseWithCovariance &a, - const PoseWithCovariance &b) { - PoseWithCovariance out; - inverseCompose(a, b, out); - return out; - } - static inline PoseWithCovariance inverseCompose(const PoseWithCovariance &a, - const Pose &b) { - PoseWithCovariance out; - inverseCompose(a, b, out); - return out; - } - static inline PoseWithCovariance inverseCompose(const Pose &a, - const PoseWithCovariance &b) { - PoseWithCovariance out; - inverseCompose(a, b, out); - return out; - } - // Return-by-value versions using TF2 transforms: - // PoseWithCovariance inverseCompose(const PoseWithCovariance &a, - // const tf2::Transform &b); +void inverseCompose(const Pose & a, const Pose & b, Pose & out); +void inverseCompose( + const PoseWithCovariance & a, const PoseWithCovariance & b, + PoseWithCovariance & out); +void inverseCompose( + const PoseWithCovariance & a, const Pose & b, + PoseWithCovariance & out); +void inverseCompose( + const Pose & a, const PoseWithCovariance & b, + PoseWithCovariance & out); +// Return-by-value versions: +static inline Pose inverseCompose(const Pose & a, const Pose & b) +{ + Pose out; + inverseCompose(a, b, out); + return out; +} +static inline PoseWithCovariance inverseCompose( + const PoseWithCovariance & a, + const PoseWithCovariance & b) +{ + PoseWithCovariance out; + inverseCompose(a, b, out); + return out; +} +static inline PoseWithCovariance inverseCompose( + const PoseWithCovariance & a, + const Pose & b) +{ + PoseWithCovariance out; + inverseCompose(a, b, out); + return out; +} +static inline PoseWithCovariance inverseCompose( + const Pose & a, + const PoseWithCovariance & b) +{ + PoseWithCovariance out; + inverseCompose(a, b, out); + return out; +} +// Return-by-value versions using TF2 transforms: +// PoseWithCovariance inverseCompose(const PoseWithCovariance &a, +// const tf2::Transform &b); - /** @} */ -} // namespace covariance_geometry +/** @} */ +} // namespace covariance_geometry -#endif // COVARIANCE_GEOMETRY_ROS_HPP_ \ No newline at end of file +#endif // COVARIANCE_GEOMETRY_ROS__COVARIANCE_GEOMETRY_ROS_HPP_ diff --git a/include/covariance_geometry_ros/utils.hpp b/include/covariance_geometry_ros/utils.hpp index 9687b7f..c30488f 100644 --- a/include/covariance_geometry_ros/utils.hpp +++ b/include/covariance_geometry_ros/utils.hpp @@ -1,5 +1,18 @@ -#ifndef COVARIANCE_GEOMETRY_ROS_UTILS_HPP_ -#define COVARIANCE_GEOMETRY_ROS_UTILS_HPP_ +// Copyright 2023 Andrea Ostuni, Giacomo Franchini - PIC4SeR +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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. +#ifndef COVARIANCE_GEOMETRY_ROS__UTILS_HPP_ +#define COVARIANCE_GEOMETRY_ROS__UTILS_HPP_ #include @@ -12,57 +25,57 @@ using PoseWithCovariance = geometry_msgs::msg::PoseWithCovariance; namespace covariance_geometry { - /** @name Convert from ROS msgs to covariance geometry ROS type +/** @name Convert from ROS msgs to covariance geometry ROS type @{ */ - inline void fromROS(const Pose &in, PoseQuaternion &out) - { - out.first.x() = in.position.x; - out.first.y() = in.position.y; - out.first.z() = in.position.z; - out.second.x() = in.orientation.x; - out.second.y() = in.orientation.y; - out.second.z() = in.orientation.z; - out.second.w() = in.orientation.w; - } +inline void fromROS(const Pose & in, PoseQuaternion & out) +{ + out.first.x() = in.position.x; + out.first.y() = in.position.y; + out.first.z() = in.position.z; + out.second.x() = in.orientation.x; + out.second.y() = in.orientation.y; + out.second.z() = in.orientation.z; + out.second.w() = in.orientation.w; +} - inline void fromROS(const PoseWithCovariance &in, PoseQuaternionCovarianceRPY &out) - { - fromROS(in.pose, out.first); - Eigen::Map> cov(in.covariance.data()); - out.second = cov; - } +inline void fromROS(const PoseWithCovariance & in, PoseQuaternionCovarianceRPY & out) +{ + fromROS(in.pose, out.first); + Eigen::Map> cov(in.covariance.data()); + out.second = cov; +} - // inline void fromROS(const tf2::Transform &in, PoseQuaternion &out) - // { - // out.first(in.getOrigin()); - // out.second(in.getRotation()); - // } +// 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 +/** @name Convert from covariance geometry ROS type to ROS msgs @{ */ - inline void toROS(const PoseQuaternion &in, Pose &out) - { - out.position.x = in.first.x(); - out.position.y = in.first.y(); - out.position.z = in.first.z(); - out.orientation.x = in.second.x(); - out.orientation.y = in.second.y(); - out.orientation.z = in.second.z(); - out.orientation.w = in.second.w(); - } - - inline void toROS(const PoseQuaternionCovarianceRPY &in, PoseWithCovariance &out) - { - toROS(in.first, out.pose); - Eigen::Map> cov(out.covariance.data()); - cov = in.second; - } +inline void toROS(const PoseQuaternion & in, Pose & out) +{ + out.position.x = in.first.x(); + out.position.y = in.first.y(); + out.position.z = in.first.z(); + out.orientation.x = in.second.x(); + out.orientation.y = in.second.y(); + out.orientation.z = in.second.z(); + out.orientation.w = in.second.w(); +} - // inline void toROS(const PoseQuaternion &in, tf2::Transform &out) - // { - // out.setOrigin(in.first); - // out.setRotation(in.second); - // } +inline void toROS(const PoseQuaternionCovarianceRPY & in, PoseWithCovariance & out) +{ + toROS(in.first, out.pose); + Eigen::Map> cov(out.covariance.data()); + cov = in.second; } -#endif // COVARIANCE_GEOMETRY_ROS_UTILS_HPP_ \ No newline at end of file +// inline void toROS(const PoseQuaternion &in, tf2::Transform &out) +// { +// out.setOrigin(in.first); +// out.setRotation(in.second); +// } +} // namespace covariance_geometry + +#endif // COVARIANCE_GEOMETRY_ROS__UTILS_HPP_ diff --git a/src/covariance_geometry_ros.cpp b/src/covariance_geometry_ros.cpp index e73ccf7..ff6ad33 100644 --- a/src/covariance_geometry_ros.cpp +++ b/src/covariance_geometry_ros.cpp @@ -1,3 +1,16 @@ +// Copyright 2023 Andrea Ostuni, Giacomo Franchini - PIC4SeR +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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_composition.hpp" #include "covariance_geometry/pose_covariance_composition.hpp" #include "covariance_geometry/pose_covariance_representation.hpp" @@ -7,92 +20,99 @@ namespace covariance_geometry { - void compose(const Pose &a, const Pose &b, Pose &out) - { - PoseQuaternion pose_a, pose_b, pose_out; - fromROS(a, pose_a); - fromROS(b, pose_b); - ComposePose3DQuaternion(pose_a, pose_b, pose_out); - toROS(pose_out, out); - } +void compose(const Pose & a, const Pose & b, Pose & out) +{ + PoseQuaternion pose_a, pose_b, pose_out; + fromROS(a, pose_a); + fromROS(b, pose_b); + ComposePose3DQuaternion(pose_a, pose_b, pose_out); + toROS(pose_out, out); +} - void compose(const PoseWithCovariance &a, const PoseWithCovariance &b, - PoseWithCovariance &out) - { - PoseQuaternionCovarianceRPY pose_a, pose_b, pose_out; - fromROS(a, pose_a); - fromROS(b, pose_b); - ComposePoseQuaternionCovarianceRPY(pose_a, pose_b, pose_out); - toROS(pose_out, out); - } +void compose( + const PoseWithCovariance & a, const PoseWithCovariance & b, + PoseWithCovariance & out) +{ + PoseQuaternionCovarianceRPY pose_a, pose_b, pose_out; + fromROS(a, pose_a); + fromROS(b, pose_b); + ComposePoseQuaternionCovarianceRPY(pose_a, pose_b, pose_out); + toROS(pose_out, out); +} - void compose(const PoseWithCovariance &a, const Pose &b, - PoseWithCovariance &out) - { - PoseQuaternionCovarianceRPY pose_a, pose_out; - PoseQuaternion pose_b; - fromROS(a, pose_a); - fromROS(b, pose_b); - ComposePose3DQuaternion(pose_a.first, pose_b, pose_out.first); - pose_out.second = pose_a.second; - toROS(pose_out, out); - } +void compose( + const PoseWithCovariance & a, const Pose & b, + PoseWithCovariance & out) +{ + PoseQuaternionCovarianceRPY pose_a, pose_out; + PoseQuaternion pose_b; + fromROS(a, pose_a); + fromROS(b, pose_b); + ComposePose3DQuaternion(pose_a.first, pose_b, pose_out.first); + pose_out.second = pose_a.second; + toROS(pose_out, out); +} - void compose(const Pose &a, const PoseWithCovariance &b, - PoseWithCovariance &out) - { - PoseQuaternion pose_a; - PoseQuaternionCovarianceRPY pose_b, pose_out; - fromROS(a, pose_a); - fromROS(b, pose_b); - ComposePose3DQuaternion(pose_a, pose_b.first, pose_out.first); - pose_out.second = pose_b.second; - toROS(pose_out, out); - } +void compose( + const Pose & a, const PoseWithCovariance & b, + PoseWithCovariance & out) +{ + PoseQuaternion pose_a; + PoseQuaternionCovarianceRPY pose_b, pose_out; + fromROS(a, pose_a); + fromROS(b, pose_b); + ComposePose3DQuaternion(pose_a, pose_b.first, pose_out.first); + pose_out.second = pose_b.second; + toROS(pose_out, out); +} - // Compose a with b^-1 - void inverseCompose(const Pose &a, const Pose &b, Pose &out) - { - PoseQuaternion pose_a, pose_b, pose_out; - fromROS(a, pose_a); - fromROS(b, pose_b); - ComposePose3DQuaternion(pose_a, InversePose(pose_b), pose_out); - toROS(pose_out, out); - } +// Compose a with b^-1 +void inverseCompose(const Pose & a, const Pose & b, Pose & out) +{ + PoseQuaternion pose_a, pose_b, pose_out; + fromROS(a, pose_a); + fromROS(b, pose_b); + ComposePose3DQuaternion(pose_a, InversePose(pose_b), pose_out); + toROS(pose_out, out); +} - void inverseCompose(const PoseWithCovariance &a, const PoseWithCovariance &b, - PoseWithCovariance &out) - { - PoseQuaternionCovarianceRPY pose_a, pose_b, pose_out; - fromROS(a, pose_a); - fromROS(b, pose_b); - ComposePoseQuaternionCovarianceRPY( - pose_a, inversePose3DQuaternionCovarianceRPY(pose_b), pose_out); - toROS(pose_out, out); - } +void inverseCompose( + const PoseWithCovariance & a, const PoseWithCovariance & b, + PoseWithCovariance & out) +{ + PoseQuaternionCovarianceRPY pose_a, pose_b, pose_out; + fromROS(a, pose_a); + fromROS(b, pose_b); + ComposePoseQuaternionCovarianceRPY( + pose_a, inversePose3DQuaternionCovarianceRPY(pose_b), pose_out); + toROS(pose_out, out); +} - void inverseCompose(const PoseWithCovariance &a, const Pose &b, - PoseWithCovariance &out) - { - PoseQuaternionCovarianceRPY pose_a, pose_out; - PoseQuaternion pose_b; - fromROS(a, pose_a); - fromROS(b, pose_b); - ComposePose3DQuaternion(pose_a.first, InversePose(pose_b), pose_out.first); - pose_out.second = pose_a.second; - toROS(pose_out, out); - } - void inverseCompose(const Pose &a, const PoseWithCovariance &b, - PoseWithCovariance &out) - { - PoseQuaternion pose_a; - PoseQuaternionCovarianceRPY pose_b, pose_out; - fromROS(a, pose_a); - fromROS(b, pose_b); - pose_b = inversePose3DQuaternionCovarianceRPY(pose_b); - ComposePose3DQuaternion(pose_a, pose_b.first, - pose_out.first); - pose_out.second = pose_b.second; - toROS(pose_out, out); - } -} // namespace covariance_geometry \ No newline at end of file +void inverseCompose( + const PoseWithCovariance & a, const Pose & b, + PoseWithCovariance & out) +{ + PoseQuaternionCovarianceRPY pose_a, pose_out; + PoseQuaternion pose_b; + fromROS(a, pose_a); + fromROS(b, pose_b); + ComposePose3DQuaternion(pose_a.first, InversePose(pose_b), pose_out.first); + pose_out.second = pose_a.second; + toROS(pose_out, out); +} +void inverseCompose( + const Pose & a, const PoseWithCovariance & b, + PoseWithCovariance & out) +{ + PoseQuaternion pose_a; + PoseQuaternionCovarianceRPY pose_b, pose_out; + fromROS(a, pose_a); + fromROS(b, pose_b); + pose_b = inversePose3DQuaternionCovarianceRPY(pose_b); + ComposePose3DQuaternion( + pose_a, pose_b.first, + pose_out.first); + pose_out.second = pose_b.second; + toROS(pose_out, out); +} +} // namespace covariance_geometry diff --git a/test/composition_test.cpp b/test/composition_test.cpp index 98b7d36..8618b42 100644 --- a/test/composition_test.cpp +++ b/test/composition_test.cpp @@ -23,10 +23,10 @@ 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}; // roll, pitch, yaw +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}; // roll, pitch, yaw +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 TEST(Composition, ROSPoses) diff --git a/test/conversion_test.cpp b/test/conversion_test.cpp index 5813441..b11fcff 100644 --- a/test/conversion_test.cpp +++ b/test/conversion_test.cpp @@ -23,10 +23,10 @@ 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}; // roll, pitch, yaw +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}; // roll, pitch, yaw +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 TEST(Conversion, PoseFromROS)