diff --git a/rbs_skill_servers/CMakeLists.txt b/rbs_skill_servers/CMakeLists.txt index bf14719..919b957 100644 --- a/rbs_skill_servers/CMakeLists.txt +++ b/rbs_skill_servers/CMakeLists.txt @@ -32,8 +32,9 @@ find_package(tf2_eigen REQUIRED) find_package(tf2_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) -find_package (Eigen3 3.3 REQUIRED) - +find_package(Eigen3 3.3 REQUIRED) +find_package(nlohmann_json 3.2.0 REQUIRED) +find_package(rbs_utils REQUIRED) # Default to Fortress set(SDF_VER 12) @@ -116,7 +117,8 @@ target_compile_definitions(pick_place_pose_loader PRIVATE "PICK_PLACE_POSE_LOADER_CPP_BUILDING_DLL") ament_target_dependencies(gripper_action_server ${deps}) -ament_target_dependencies(pick_place_pose_loader ${deps} Eigen3) + +ament_target_dependencies(pick_place_pose_loader ${deps} Eigen3 nlohmann_json rbs_utils) rclcpp_components_register_node(gripper_action_server PLUGIN "rbs_skill_actions::GripperControlActionServer" EXECUTABLE gripper_control_action_server) rclcpp_components_register_node(pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer" EXECUTABLE pick_place_pose_loader_service_server) diff --git a/rbs_skill_servers/include/rbs_skill_servers/pick_place_pose_loader.hpp b/rbs_skill_servers/include/rbs_skill_servers/pick_place_pose_loader.hpp index 11fce65..772884d 100644 --- a/rbs_skill_servers/include/rbs_skill_servers/pick_place_pose_loader.hpp +++ b/rbs_skill_servers/include/rbs_skill_servers/pick_place_pose_loader.hpp @@ -1,11 +1,9 @@ #include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp" +#include "rbs_utils/rbs_utils.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_components/register_node_macro.hpp" #include -#include -#include -#include -#include +#include #include #include #include @@ -17,16 +15,18 @@ public: private: static std::vector - collect_pose(const Eigen::Isometry3d &graspPose, - const geometry_msgs::msg::Vector3 &move_direction, - const Eigen::Vector3d &scale_vec); + collectPose(const Eigen::Isometry3d &graspPose, + const geometry_msgs::msg::Vector3 &move_direction, + const Eigen::Vector3d &scale_vec); rclcpp::Service::SharedPtr srv_; std::shared_ptr tf_listener_{nullptr}; std::unique_ptr tf_buffer_; geometry_msgs::msg::TransformStamped place_pose_tf; geometry_msgs::msg::TransformStamped grasp_pose_tf; - void handle_server( + std::vector + getPlacePoseJson(const nlohmann::json &json); + void handleServer( const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request, rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr diff --git a/rbs_skill_servers/package.xml b/rbs_skill_servers/package.xml index 683eb36..05b9c50 100644 --- a/rbs_skill_servers/package.xml +++ b/rbs_skill_servers/package.xml @@ -21,6 +21,7 @@ action_msgs rbs_skill_interfaces tf2_eigen + rbs_utils ament_lint_auto ament_lint_common diff --git a/rbs_skill_servers/src/pick_place_pose_loader.cpp b/rbs_skill_servers/src/pick_place_pose_loader.cpp index 028c2e3..60d2871 100644 --- a/rbs_skill_servers/src/pick_place_pose_loader.cpp +++ b/rbs_skill_servers/src/pick_place_pose_loader.cpp @@ -4,15 +4,14 @@ #include #include #include +#include +#include +#include +#include #include +#include #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include +#include using GetGraspPlacePoseServer = rbs_skill_actions::GetGraspPickPoseServer; using GetGraspPlacePoseService = rbs_skill_interfaces::srv::GetPickPlacePoses; @@ -28,11 +27,11 @@ GetGraspPlacePoseServer::GetGraspPickPoseServer(rclcpp::NodeOptions options) srv_ = create_service( "/get_pick_place_pose_service", - std::bind(&GetGraspPickPoseServer::handle_server, this, + std::bind(&GetGraspPickPoseServer::handleServer, this, std::placeholders::_1, std::placeholders::_2)); } -void GetGraspPlacePoseServer::handle_server( +void GetGraspPlacePoseServer::handleServer( const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request, rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr @@ -40,6 +39,11 @@ void GetGraspPlacePoseServer::handle_server( std::string object_name = request->object_name + "_place"; // TODO: replace with better name // Load place pose from TF2 + + auto d = std::make_shared("asp-example"); + d->setTfData(); + // rbs_utils::processAsmFolderName("asp-example"); + try { place_pose_tf = tf_buffer_->lookupTransform("world", object_name.c_str(), tf2::TimePointZero); @@ -80,13 +84,15 @@ void GetGraspPlacePoseServer::handle_server( Eigen::Vector3d scale_grasp(0, 0, 0.10); Eigen::Vector3d scale_place(0, 0, 0.15); + auto path = std::getenv("RBS_ASSEMBLY_PATH"); + RCLCPP_ERROR_STREAM(this->get_logger(), path); response->grasp = - collect_pose(grasp_pose, request->grasp_direction, scale_grasp); + collectPose(grasp_pose, request->grasp_direction, scale_grasp); response->place = - collect_pose(place_pose, request->place_direction, scale_place); + collectPose(place_pose, request->place_direction, scale_place); } -std::vector GetGraspPlacePoseServer::collect_pose( +std::vector GetGraspPlacePoseServer::collectPose( const Eigen::Isometry3d &graspPose, const geometry_msgs::msg::Vector3 &move_direction, const Eigen::Vector3d &scale_vec) { @@ -123,3 +129,12 @@ std::vector GetGraspPlacePoseServer::collect_pose( return poses; } + +// std::vector GetGraspPlacePoseServer::getPlacePoseJson(const nlohmann::json& json) +// { +// std::vector place_pose; +// auto env_path = std::getenv("PATH"); + +// RCLCPP_INFO_STREAM(this->get_logger(), env_path); +// return place_pose; +// } \ No newline at end of file diff --git a/rbs_utils/CMakeLists.txt b/rbs_utils/CMakeLists.txt new file mode 100644 index 0000000..9832398 --- /dev/null +++ b/rbs_utils/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.8) +project(rbs_utils) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_eigen REQUIRED) + +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_library(${PROJECT_NAME} SHARED src/rbs_utils.cpp) + +install( + DIRECTORY include/ + DESTINATION include +) + +ament_target_dependencies(${PROJECT_NAME} PUBLIC rclcpp tf2_ros tf2_eigen) + +# target_include_directories(asm_folder_process +# PUBLIC +# "$" +# "$") + +target_include_directories(${PROJECT_NAME} + PUBLIC + "$" + "$") + +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin +) + +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() +endif() + +# ament_export_dependencies() +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() diff --git a/rbs_utils/LICENSE b/rbs_utils/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/rbs_utils/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/rbs_utils/include/rbs_utils/rbs_utils.hpp b/rbs_utils/include/rbs_utils/rbs_utils.hpp new file mode 100644 index 0000000..a2fa54d --- /dev/null +++ b/rbs_utils/include/rbs_utils/rbs_utils.hpp @@ -0,0 +1,64 @@ +#include "rclcpp/rclcpp.hpp" +#include "tf2_msgs/msg/tf_message.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/static_transform_broadcaster.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +const std::string env_dir = std::getenv("RBS_ASSEMBLY_DIR"); + +namespace rbs_utils { + +class AssemblyConfigLoader { + +public: + AssemblyConfigLoader(const std::string &t_assembly_dir); + + inline std::shared_ptr> + getAssemblyFileData(); + + void setTfData(); + +private: + std::shared_ptr> m_env_vars; + std::vector m_env_files; + std::vector m_env_paths; + std::unique_ptr m_tf_buffer; + + void readAssemblyFileData(const std::string &filename, + const std::string &filepath); + + tf2_msgs::msg::TFMessage parseJsonToTFMessage(const nlohmann::json &json); + + void setTfFromDb(const std::string &filename); + double convertToDouble(const nlohmann::json &value); +}; + +class StaticFramePublisher : public rclcpp::Node { +public: + explicit StaticFramePublisher(const tf2_msgs::msg::TFMessage &tfs) + : Node("rbs_static_tf") { + m_tf_static_broadcaster = + std::make_shared(this); + + this->make_transforms(tfs); + } + +private: + void make_transforms(const tf2_msgs::msg::TFMessage &tfs) { + for (const auto &transform : tfs.transforms) { + m_tf_static_broadcaster->sendTransform(transform); + } + } + + std::shared_ptr m_tf_static_broadcaster; +}; +} // namespace rbs_utils diff --git a/rbs_utils/package.xml b/rbs_utils/package.xml new file mode 100644 index 0000000..c0d5e9b --- /dev/null +++ b/rbs_utils/package.xml @@ -0,0 +1,18 @@ + + + + rbs_utils + 0.0.0 + TODO: Package description + bill-finger + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/rbs_utils/src/rbs_utils.cpp b/rbs_utils/src/rbs_utils.cpp new file mode 100644 index 0000000..4bc80a8 --- /dev/null +++ b/rbs_utils/src/rbs_utils.cpp @@ -0,0 +1,165 @@ +#include "rbs_utils/rbs_utils.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rbs_utils { + +AssemblyConfigLoader::AssemblyConfigLoader(const std::string &t_assembly_dir) + : m_env_vars( + std::make_shared>()) { + if (!t_assembly_dir.empty()) { + std::vector filenames = {"robossembler_db", "sequences"}; + for (auto &filename : filenames) { + std::string filepath = + env_dir + "/" + t_assembly_dir + "/" + filename + ".json"; + + m_env_files.push_back(filepath); + readAssemblyFileData(filename, filepath); + } + } +} + +void AssemblyConfigLoader::readAssemblyFileData(const std::string &filename, + const std::string &filepath) { + try { + std::ifstream i(filepath); + if (!i.is_open()) { + RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), "Failed to open file: %s", + filepath.c_str()); + return; + } + + nlohmann::json json = nlohmann::json::parse(i); + m_env_vars->insert({filename, json}); + } catch (const nlohmann::json::parse_error &e) { + RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), + "Error parsing JSON in file %s: %s", filepath.c_str(), + e.what()); + } catch (const std::exception &e) { + RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), + "Exception reading file %s: %s", filepath.c_str(), e.what()); + } +} + +inline std::shared_ptr> +AssemblyConfigLoader::getAssemblyFileData() { + return m_env_vars; +} + +void AssemblyConfigLoader::setTfData() { + if (m_env_vars->find("robossembler_db") != m_env_vars->end()) { + const nlohmann::json &json = (*m_env_vars)["robossembler_db"]; + tf2_msgs::msg::TFMessage tf_msg = parseJsonToTFMessage(json); + + // Output position information to console + for (const auto &transform : tf_msg.transforms) { + RCLCPP_DEBUG_STREAM( + rclcpp::get_logger("rbs_utils"), + "Frame ID: " << transform.header.frame_id << ", Child Frame ID: " + << transform.child_frame_id << ", Translation: [" + << transform.transform.translation.x << ", " + << transform.transform.translation.y << ", " + << transform.transform.translation.z << "]"); + } + auto r = std::make_shared(tf_msg); + + } else { + RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), + "Key 'robossembler_db' not found in m_env_vars."); + } +} + +tf2_msgs::msg::TFMessage +AssemblyConfigLoader::parseJsonToTFMessage(const nlohmann::json &json) { + tf2_msgs::msg::TFMessage tf_msg; + + // Add absolute part to TFMessage + geometry_msgs::msg::TransformStamped absolute_transform; + absolute_transform.header.frame_id = "world"; + absolute_transform.child_frame_id = + json.at("absolutePart").get(); + absolute_transform.transform.translation.x = -1.0; + absolute_transform.transform.translation.y = 0.0; + absolute_transform.transform.translation.z = 0.0; + absolute_transform.transform.rotation.w = 1.0; + absolute_transform.transform.rotation.x = 0.0; + absolute_transform.transform.rotation.y = 0.0; + absolute_transform.transform.rotation.z = 0.0; + tf_msg.transforms.push_back(absolute_transform); + + // Add relative parts to TFMessage + for (const auto &relative_part : json.at("relativeParts")) { + geometry_msgs::msg::TransformStamped relative_transform; + relative_transform.header.frame_id = + json.at("absolutePart").get(); + relative_transform.child_frame_id = + relative_part.at("name").get(); + relative_transform.transform.translation.x = + convertToDouble(relative_part.at("position").at("x")); + relative_transform.transform.translation.y = + convertToDouble(relative_part.at("position").at("y")); + relative_transform.transform.translation.z = + convertToDouble(relative_part.at("position").at("z")); + relative_transform.transform.rotation.w = + convertToDouble(relative_part.at("quaternion").at("qw")); + relative_transform.transform.rotation.x = + convertToDouble(relative_part.at("quaternion").at("qx")); + relative_transform.transform.rotation.y = + convertToDouble(relative_part.at("quaternion").at("qy")); + relative_transform.transform.rotation.z = + convertToDouble(relative_part.at("quaternion").at("qz")); + tf_msg.transforms.push_back(relative_transform); + } + + // Add grasp pose models to TFMessage + for (const auto &grasp_pose : json.at("graspPoseModels")) { + geometry_msgs::msg::TransformStamped grasp_transform; + grasp_transform.header.frame_id = + json.at("absolutePart").get(); + grasp_transform.child_frame_id = grasp_pose.at("name").get(); + grasp_transform.transform.translation.x = + convertToDouble(grasp_pose.at("position").at("x")); + grasp_transform.transform.translation.y = + convertToDouble(grasp_pose.at("position").at("y")); + grasp_transform.transform.translation.z = + convertToDouble(grasp_pose.at("position").at("z")); + grasp_transform.transform.rotation.w = + convertToDouble(grasp_pose.at("quaternion").at("qw")); + grasp_transform.transform.rotation.x = + convertToDouble(grasp_pose.at("quaternion").at("qx")); + grasp_transform.transform.rotation.y = + convertToDouble(grasp_pose.at("quaternion").at("qy")); + grasp_transform.transform.rotation.z = + convertToDouble(grasp_pose.at("quaternion").at("qz")); + tf_msg.transforms.push_back(grasp_transform); + } + + return tf_msg; +} + +double AssemblyConfigLoader::convertToDouble(const nlohmann::json &value) { + if (value.is_number()) { + return value.get(); + } else if (value.is_string()) { + try { + return std::stod(value.get()); + } catch (const std::exception &e) { + RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), + "Error converting string to double: %s", e.what()); + throw std::runtime_error("Error converting string to double"); + } + } + + throw std::runtime_error("Invalid JSON type for conversion to double"); +} + +} // namespace rbs_utils \ No newline at end of file