add rbs_utils
This commit is contained in:
parent
f1a6f4d817
commit
d84e8ead97
9 changed files with 551 additions and 23 deletions
|
@ -32,8 +32,9 @@ find_package(tf2_eigen REQUIRED)
|
||||||
find_package(tf2_msgs REQUIRED)
|
find_package(tf2_msgs REQUIRED)
|
||||||
find_package(tinyxml2_vendor REQUIRED)
|
find_package(tinyxml2_vendor REQUIRED)
|
||||||
find_package(TinyXML2 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
|
# Default to Fortress
|
||||||
set(SDF_VER 12)
|
set(SDF_VER 12)
|
||||||
|
@ -116,7 +117,8 @@ target_compile_definitions(pick_place_pose_loader
|
||||||
PRIVATE "PICK_PLACE_POSE_LOADER_CPP_BUILDING_DLL")
|
PRIVATE "PICK_PLACE_POSE_LOADER_CPP_BUILDING_DLL")
|
||||||
|
|
||||||
ament_target_dependencies(gripper_action_server ${deps})
|
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(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)
|
rclcpp_components_register_node(pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer" EXECUTABLE pick_place_pose_loader_service_server)
|
||||||
|
|
|
@ -1,11 +1,9 @@
|
||||||
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
|
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
|
||||||
|
#include "rbs_utils/rbs_utils.hpp"
|
||||||
#include "rclcpp/rclcpp.hpp"
|
#include "rclcpp/rclcpp.hpp"
|
||||||
#include "rclcpp_components/register_node_macro.hpp"
|
#include "rclcpp_components/register_node_macro.hpp"
|
||||||
#include <Eigen/Core>
|
#include <Eigen/Core>
|
||||||
#include <Eigen/Geometry>
|
#include <nlohmann/json.hpp>
|
||||||
#include <algorithm>
|
|
||||||
#include <geometry_msgs/msg/detail/transform_stamped__struct.hpp>
|
|
||||||
#include <memory>
|
|
||||||
#include <rclcpp/rclcpp.hpp>
|
#include <rclcpp/rclcpp.hpp>
|
||||||
#include <tf2_eigen/tf2_eigen.hpp>
|
#include <tf2_eigen/tf2_eigen.hpp>
|
||||||
#include <tf2_ros/transform_listener.h>
|
#include <tf2_ros/transform_listener.h>
|
||||||
|
@ -17,7 +15,7 @@ public:
|
||||||
|
|
||||||
private:
|
private:
|
||||||
static std::vector<geometry_msgs::msg::Pose>
|
static std::vector<geometry_msgs::msg::Pose>
|
||||||
collect_pose(const Eigen::Isometry3d &graspPose,
|
collectPose(const Eigen::Isometry3d &graspPose,
|
||||||
const geometry_msgs::msg::Vector3 &move_direction,
|
const geometry_msgs::msg::Vector3 &move_direction,
|
||||||
const Eigen::Vector3d &scale_vec);
|
const Eigen::Vector3d &scale_vec);
|
||||||
rclcpp::Service<rbs_skill_interfaces::srv::GetPickPlacePoses>::SharedPtr srv_;
|
rclcpp::Service<rbs_skill_interfaces::srv::GetPickPlacePoses>::SharedPtr srv_;
|
||||||
|
@ -26,7 +24,9 @@ private:
|
||||||
geometry_msgs::msg::TransformStamped place_pose_tf;
|
geometry_msgs::msg::TransformStamped place_pose_tf;
|
||||||
geometry_msgs::msg::TransformStamped grasp_pose_tf;
|
geometry_msgs::msg::TransformStamped grasp_pose_tf;
|
||||||
|
|
||||||
void handle_server(
|
std::vector<geometry_msgs::msg::Pose>
|
||||||
|
getPlacePoseJson(const nlohmann::json &json);
|
||||||
|
void handleServer(
|
||||||
const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr
|
const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr
|
||||||
request,
|
request,
|
||||||
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr
|
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr
|
||||||
|
|
|
@ -21,6 +21,7 @@
|
||||||
<depend>action_msgs</depend>
|
<depend>action_msgs</depend>
|
||||||
<depend>rbs_skill_interfaces</depend>
|
<depend>rbs_skill_interfaces</depend>
|
||||||
<depend>tf2_eigen</depend>
|
<depend>tf2_eigen</depend>
|
||||||
|
<depend>rbs_utils</depend>
|
||||||
|
|
||||||
<test_depend>ament_lint_auto</test_depend>
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
<test_depend>ament_lint_common</test_depend>
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
|
@ -4,15 +4,14 @@
|
||||||
#include <Eigen/src/Geometry/Quaternion.h>
|
#include <Eigen/src/Geometry/Quaternion.h>
|
||||||
#include <Eigen/src/Geometry/Transform.h>
|
#include <Eigen/src/Geometry/Transform.h>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
#include <fstream>
|
||||||
|
#include <geometry_msgs/msg/detail/pose__struct.hpp>
|
||||||
|
#include <memory>
|
||||||
|
#include <nlohmann/json_fwd.hpp>
|
||||||
#include <rclcpp/logging.hpp>
|
#include <rclcpp/logging.hpp>
|
||||||
|
#include <sdf/Geometry.hh>
|
||||||
#include <tf2_eigen/tf2_eigen.hpp>
|
#include <tf2_eigen/tf2_eigen.hpp>
|
||||||
// #include <Eigen/src/Core/Matrix.h>
|
#include <vector>
|
||||||
// #include <Eigen/src/Geometry/Transform.h>
|
|
||||||
// #include <rclcpp/logging.hpp>
|
|
||||||
// #include <tf2/LinearMath/Quaternion.h>
|
|
||||||
// #include <tf2/exceptions.h>
|
|
||||||
// #include <tf2/time.h>
|
|
||||||
// #include <tf2_eigen/tf2_eigen.hpp>
|
|
||||||
|
|
||||||
using GetGraspPlacePoseServer = rbs_skill_actions::GetGraspPickPoseServer;
|
using GetGraspPlacePoseServer = rbs_skill_actions::GetGraspPickPoseServer;
|
||||||
using GetGraspPlacePoseService = rbs_skill_interfaces::srv::GetPickPlacePoses;
|
using GetGraspPlacePoseService = rbs_skill_interfaces::srv::GetPickPlacePoses;
|
||||||
|
@ -28,11 +27,11 @@ GetGraspPlacePoseServer::GetGraspPickPoseServer(rclcpp::NodeOptions options)
|
||||||
|
|
||||||
srv_ = create_service<GetGraspPlacePoseService>(
|
srv_ = create_service<GetGraspPlacePoseService>(
|
||||||
"/get_pick_place_pose_service",
|
"/get_pick_place_pose_service",
|
||||||
std::bind(&GetGraspPickPoseServer::handle_server, this,
|
std::bind(&GetGraspPickPoseServer::handleServer, this,
|
||||||
std::placeholders::_1, std::placeholders::_2));
|
std::placeholders::_1, std::placeholders::_2));
|
||||||
}
|
}
|
||||||
|
|
||||||
void GetGraspPlacePoseServer::handle_server(
|
void GetGraspPlacePoseServer::handleServer(
|
||||||
const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr
|
const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr
|
||||||
request,
|
request,
|
||||||
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr
|
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr
|
||||||
|
@ -40,6 +39,11 @@ void GetGraspPlacePoseServer::handle_server(
|
||||||
std::string object_name =
|
std::string object_name =
|
||||||
request->object_name + "_place"; // TODO: replace with better name
|
request->object_name + "_place"; // TODO: replace with better name
|
||||||
// Load place pose from TF2
|
// Load place pose from TF2
|
||||||
|
|
||||||
|
auto d = std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example");
|
||||||
|
d->setTfData();
|
||||||
|
// rbs_utils::processAsmFolderName("asp-example");
|
||||||
|
|
||||||
try {
|
try {
|
||||||
place_pose_tf = tf_buffer_->lookupTransform("world", object_name.c_str(),
|
place_pose_tf = tf_buffer_->lookupTransform("world", object_name.c_str(),
|
||||||
tf2::TimePointZero);
|
tf2::TimePointZero);
|
||||||
|
@ -80,13 +84,15 @@ void GetGraspPlacePoseServer::handle_server(
|
||||||
|
|
||||||
Eigen::Vector3d scale_grasp(0, 0, 0.10);
|
Eigen::Vector3d scale_grasp(0, 0, 0.10);
|
||||||
Eigen::Vector3d scale_place(0, 0, 0.15);
|
Eigen::Vector3d scale_place(0, 0, 0.15);
|
||||||
|
auto path = std::getenv("RBS_ASSEMBLY_PATH");
|
||||||
|
RCLCPP_ERROR_STREAM(this->get_logger(), path);
|
||||||
response->grasp =
|
response->grasp =
|
||||||
collect_pose(grasp_pose, request->grasp_direction, scale_grasp);
|
collectPose(grasp_pose, request->grasp_direction, scale_grasp);
|
||||||
response->place =
|
response->place =
|
||||||
collect_pose(place_pose, request->place_direction, scale_place);
|
collectPose(place_pose, request->place_direction, scale_place);
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::collect_pose(
|
std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::collectPose(
|
||||||
const Eigen::Isometry3d &graspPose,
|
const Eigen::Isometry3d &graspPose,
|
||||||
const geometry_msgs::msg::Vector3 &move_direction,
|
const geometry_msgs::msg::Vector3 &move_direction,
|
||||||
const Eigen::Vector3d &scale_vec) {
|
const Eigen::Vector3d &scale_vec) {
|
||||||
|
@ -123,3 +129,12 @@ std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::collect_pose(
|
||||||
|
|
||||||
return poses;
|
return poses;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::getPlacePoseJson(const nlohmann::json& json)
|
||||||
|
// {
|
||||||
|
// std::vector<geometry_msgs::msg::Pose> place_pose;
|
||||||
|
// auto env_path = std::getenv("PATH");
|
||||||
|
|
||||||
|
// RCLCPP_INFO_STREAM(this->get_logger(), env_path);
|
||||||
|
// return place_pose;
|
||||||
|
// }
|
61
rbs_utils/CMakeLists.txt
Normal file
61
rbs_utils/CMakeLists.txt
Normal file
|
@ -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(<dependency> 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
|
||||||
|
# "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
|
||||||
|
# "$<INSTALL_INTERFACE:include/>")
|
||||||
|
|
||||||
|
target_include_directories(${PROJECT_NAME}
|
||||||
|
PUBLIC
|
||||||
|
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
|
||||||
|
"$<INSTALL_INTERFACE:include/>")
|
||||||
|
|
||||||
|
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()
|
202
rbs_utils/LICENSE
Normal file
202
rbs_utils/LICENSE
Normal file
|
@ -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.
|
64
rbs_utils/include/rbs_utils/rbs_utils.hpp
Normal file
64
rbs_utils/include/rbs_utils/rbs_utils.hpp
Normal file
|
@ -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 <fstream>
|
||||||
|
#include <memory>
|
||||||
|
#include <nlohmann/json.hpp>
|
||||||
|
#include <rclcpp/node.hpp>
|
||||||
|
#include <string>
|
||||||
|
#include <tf2_eigen/tf2_eigen.hpp>
|
||||||
|
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
|
||||||
|
#include <unordered_map>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
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<std::unordered_map<std::string, nlohmann::json>>
|
||||||
|
getAssemblyFileData();
|
||||||
|
|
||||||
|
void setTfData();
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::shared_ptr<std::unordered_map<std::string, nlohmann::json>> m_env_vars;
|
||||||
|
std::vector<std::string> m_env_files;
|
||||||
|
std::vector<std::ifstream> m_env_paths;
|
||||||
|
std::unique_ptr<tf2_ros::Buffer> 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<tf2_ros::StaticTransformBroadcaster>(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<tf2_ros::StaticTransformBroadcaster> m_tf_static_broadcaster;
|
||||||
|
};
|
||||||
|
} // namespace rbs_utils
|
18
rbs_utils/package.xml
Normal file
18
rbs_utils/package.xml
Normal file
|
@ -0,0 +1,18 @@
|
||||||
|
<?xml version="1.0"?>
|
||||||
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>rbs_utils</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>TODO: Package description</description>
|
||||||
|
<maintainer email="ur.narmak@gmail.com">bill-finger</maintainer>
|
||||||
|
<license>Apache-2.0</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
165
rbs_utils/src/rbs_utils.cpp
Normal file
165
rbs_utils/src/rbs_utils.cpp
Normal file
|
@ -0,0 +1,165 @@
|
||||||
|
#include "rbs_utils/rbs_utils.hpp"
|
||||||
|
#include <fstream>
|
||||||
|
#include <iostream>
|
||||||
|
#include <memory>
|
||||||
|
#include <nlohmann/json_fwd.hpp>
|
||||||
|
#include <rclcpp/clock.hpp>
|
||||||
|
#include <rclcpp/logger.hpp>
|
||||||
|
#include <rclcpp/logging.hpp>
|
||||||
|
#include <string>
|
||||||
|
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
|
||||||
|
#include <tf2_ros/transform_broadcaster.h>
|
||||||
|
#include <unordered_map>
|
||||||
|
|
||||||
|
namespace rbs_utils {
|
||||||
|
|
||||||
|
AssemblyConfigLoader::AssemblyConfigLoader(const std::string &t_assembly_dir)
|
||||||
|
: m_env_vars(
|
||||||
|
std::make_shared<std::unordered_map<std::string, nlohmann::json>>()) {
|
||||||
|
if (!t_assembly_dir.empty()) {
|
||||||
|
std::vector<std::string> 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<std::unordered_map<std::string, nlohmann::json>>
|
||||||
|
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<StaticFramePublisher>(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<std::string>();
|
||||||
|
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<std::string>();
|
||||||
|
relative_transform.child_frame_id =
|
||||||
|
relative_part.at("name").get<std::string>();
|
||||||
|
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<std::string>();
|
||||||
|
grasp_transform.child_frame_id = grasp_pose.at("name").get<std::string>();
|
||||||
|
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<double>();
|
||||||
|
} else if (value.is_string()) {
|
||||||
|
try {
|
||||||
|
return std::stod(value.get<std::string>());
|
||||||
|
} 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
|
Loading…
Add table
Add a link
Reference in a new issue