-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #65 from Caltech-AMBER/std-obk-msgs
New Obelisk Message Types
- Loading branch information
Showing
22 changed files
with
2,438 additions
and
1,291 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,34 @@ | ||
| message(STATUS "Configuring Obelisk Utils") | ||
|
|
||
|
|
||
| if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") | ||
| add_compile_options(-Wall -Wextra -Wpedantic) | ||
| endif() | ||
|
|
||
| # ------- ROS 2 Packages ------- # | ||
| find_package(ament_cmake REQUIRED) | ||
| find_package(rclcpp REQUIRED) | ||
| find_package(rclcpp_lifecycle REQUIRED) | ||
| find_package(rcl REQUIRED) | ||
|
|
||
| # ------- Obelisk Messages ------- # | ||
| find_package(obelisk_std_msgs REQUIRED) | ||
|
|
||
| # ------- Eigen ------- # | ||
| find_package(Eigen3 REQUIRED) | ||
|
|
||
| # ------- Source files ------- # | ||
| set(UTILS_INC "${CMAKE_CURRENT_SOURCE_DIR}/include") | ||
|
|
||
| # ------- Making the library ------- # | ||
| add_library(ObkUtils INTERFACE) | ||
| add_library(Obelisk::Utils ALIAS ObkUtils) # Namespaced alias | ||
| target_include_directories(ObkUtils INTERFACE ${UTILS_INC} ${mujoco_SOURCE_DIR}/include) | ||
|
|
||
| target_link_libraries(ObkUtils INTERFACE Eigen3::Eigen) | ||
|
|
||
| ament_target_dependencies(ObkUtils INTERFACE | ||
| rclcpp | ||
| rclcpp_lifecycle | ||
| obelisk_std_msgs | ||
| std_msgs) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,148 @@ | ||
| #include <unsupported/Eigen/CXX11/Tensor> | ||
|
|
||
| #include "rclcpp/rclcpp.hpp" | ||
|
|
||
| #include "obelisk_std_msgs/msg/float_multi_array.hpp" | ||
| #include "obelisk_std_msgs/msg/u_int8_multi_array.hpp" | ||
|
|
||
| namespace obelisk::utils::msgs { | ||
| namespace internal { | ||
| /** | ||
| * @brief Internal helper function to create tensors of arbitrary dimension. | ||
| * | ||
| * @param data the data to assign to the tensor | ||
| * @param dims the dimensions of the tensor | ||
| * @param Indices the index sequence to use for the parameter pack | ||
| * | ||
| * @return the Eigen Tensor | ||
| */ | ||
| template <typename ScalarT, std::size_t N, std::size_t... Indices> | ||
| Eigen::Tensor<ScalarT, N> CreateTensor(std::vector<ScalarT>& data, const std::array<int, N>& dims, | ||
| std::index_sequence<Indices...>) { | ||
| Eigen::TensorMap<Eigen::Tensor<ScalarT, N>> tensor(data.data(), dims[Indices]...); | ||
| return tensor; | ||
| } | ||
| } // namespace internal | ||
|
|
||
| /** | ||
| * @brief Convert MultiArray message to Eigen Tensor array. | ||
| * | ||
| * @param msg the message to convert | ||
| * @return the Eigen Tensor | ||
| */ | ||
| template <int Size> | ||
| Eigen::Tensor<double, Size> MultiArrayToTensor(const obelisk_std_msgs::msg::FloatMultiArray& msg) { | ||
|
|
||
| // Get the flat part of the data | ||
| std::vector<double> data(msg.data.begin() + msg.layout.data_offset, msg.data.end()); | ||
|
|
||
| if (msg.layout.dim.size() != Size) { | ||
| RCLCPP_ERROR_STREAM(rclcpp::get_logger("msg_conversion"), | ||
| "Templated size does not match the size provided by the message!"); | ||
| } | ||
|
|
||
| std::array<int, Size> sizes; | ||
| for (int i = 0; i < Size; i++) { | ||
| sizes.at(i) = msg.layout.dim.at(i).size; | ||
| } | ||
|
|
||
| auto tensor = internal::CreateTensor<double, Size>(data, sizes, std::make_index_sequence<Size>{}); | ||
|
|
||
| return tensor; | ||
| } | ||
|
|
||
| /** | ||
| * @brief Convert MultiArray message to Eigen Tensor array. | ||
| * | ||
| * @param msg the message to convert | ||
| * @return the Eigen Tensor | ||
| */ | ||
| template <int Size> | ||
| Eigen::Tensor<uint8_t, Size> MultiArrayToTensor(const obelisk_std_msgs::msg::UInt8MultiArray& msg) { | ||
|
|
||
| // Get the flat part of the data | ||
| std::vector<uint8_t> data(msg.data.begin() + msg.layout.data_offset, msg.data.end()); | ||
|
|
||
| if (msg.layout.dim.size() != Size) { | ||
| RCLCPP_ERROR_STREAM(rclcpp::get_logger("msg_conversion"), | ||
| "Templated size does not match the size provided by the message!"); | ||
| } | ||
|
|
||
| std::array<int, Size> sizes; | ||
| for (int i = 0; i < Size; i++) { | ||
| sizes.at(i) = msg.layout.dim.at(i).size; | ||
| } | ||
|
|
||
| auto tensor = internal::CreateTensor<uint8_t, Size>(data, sizes, std::make_index_sequence<Size>{}); | ||
|
|
||
| return tensor; | ||
| } | ||
|
|
||
| /** | ||
| * @brief Convert an Eigen Tensor into a multiarray message. | ||
| * | ||
| * @param tensor the tensor to convert | ||
| * @return the multiarray message | ||
| */ | ||
| template <int Size> | ||
| obelisk_std_msgs::msg::FloatMultiArray TensorToMultiArray(const Eigen::Tensor<double, Size>& tensor) { | ||
| obelisk_std_msgs::msg::FloatMultiArray msg; | ||
| msg.layout.data_offset = 0; | ||
|
|
||
| // Get data into flat vector | ||
| msg.data.resize(tensor.size()); | ||
| std::copy(tensor.data(), tensor.data() + tensor.size(), msg.data.begin()); | ||
|
|
||
| // Compute stride lengths | ||
| std_msgs::msg::MultiArrayDimension dim; | ||
| dim.label = "dim_" + std::to_string(Size); | ||
| dim.size = tensor.dimension(Size - 1); | ||
| dim.stride = 1; | ||
|
|
||
| msg.layout.dim.emplace_back(dim); // The stride for the last dimension | ||
| for (int i = tensor.dimensions().size() - 2; i >= 0; --i) { | ||
| dim.label = "dim_" + std::to_string(i); | ||
| dim.size = tensor.dimension(i); | ||
| dim.stride = msg.layout.dim.back().stride * tensor.dimension(i + 1); | ||
|
|
||
| msg.layout.dim.emplace_back(dim); | ||
| } | ||
| std::reverse(msg.layout.dim.begin(), msg.layout.dim.end()); // Reverse to match dimension order | ||
|
|
||
| return msg; | ||
| } | ||
|
|
||
| /** | ||
| * @brief Convert an Eigen Tensor into a multiarray message. | ||
| * | ||
| * @param tensor the tensor to convert | ||
| * @return the multiarray message | ||
| */ | ||
| template <int Size> | ||
| obelisk_std_msgs::msg::UInt8MultiArray TensorToMultiArray(const Eigen::Tensor<uint8_t, Size>& tensor) { | ||
| obelisk_std_msgs::msg::UInt8MultiArray msg; | ||
| msg.layout.data_offset = 0; | ||
|
|
||
| // Get data into flat vector | ||
| msg.data.resize(tensor.size()); | ||
| std::copy(tensor.data(), tensor.data() + tensor.size(), msg.data.begin()); | ||
|
|
||
| // Compute stride lengths | ||
| std_msgs::msg::MultiArrayDimension dim; | ||
| dim.label = "dim_" + std::to_string(Size); | ||
| dim.size = tensor.dimension(Size - 1); | ||
| dim.stride = 1; | ||
|
|
||
| msg.layout.dim.emplace_back(dim); // The stride for the last dimension | ||
| for (int i = tensor.dimensions().size() - 2; i >= 0; --i) { | ||
| dim.label = "dim_" + std::to_string(i); | ||
| dim.size = tensor.dimension(i); | ||
| dim.stride = msg.layout.dim.back().stride * tensor.dimension(i + 1); | ||
|
|
||
| msg.layout.dim.emplace_back(dim); | ||
| } | ||
| std::reverse(msg.layout.dim.begin(), msg.layout.dim.end()); // Reverse to match dimension order | ||
|
|
||
| return msg; | ||
| } | ||
| } // namespace obelisk::utils::msgs |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.