diff --git a/overlay.nix b/overlay.nix index 9c85251..9e9957f 100644 --- a/overlay.nix +++ b/overlay.nix @@ -9,9 +9,6 @@ self: super: { cartesian-force-controller = super.callPackage ../cartesian_controllers/cartesian_force_controller/package.nix {}; cartesian-motion-controller = super.callPackage ../cartesian_controllers/cartesian_motion_controller/package.nix {}; cartesian-twist-controller = super.callPackage ../cartesian_controllers/cartesian_twist_controller/package.nix {}; - dynmsg = super.callPackage ../dynamic_message_introspection/dynmsg/package.nix {}; - dynmsg-demo = super.callPackage ../dynamic_message_introspection/dynmsg_demo/package.nix {}; - dynmsg-msgs = super.callPackage ../dynamic_message_introspection/dynmsg_msgs/package.nix {}; env-manager = super.callPackage ./env_manager/env_manager/package.nix {}; env-manager-interfaces = super.callPackage ./env_manager/env_manager_interfaces/package.nix {}; gym-gz-ros-python = super.callPackage ../gym-gz/package.nix {}; @@ -33,5 +30,4 @@ self: super: { robonomics = super.callPackage ./robonomics/package.nix {}; robot-builder = super.callPackage ../robot_builder/package.nix {}; scenario = super.callPackage ../scenario/package.nix {}; - test-dynmsg = super.callPackage ../dynamic_message_introspection/test_dynmsg/package.nix {}; } diff --git a/rbs_utils/rbs_utils/CMakeLists.txt b/rbs_utils/rbs_utils/CMakeLists.txt index 92f9442..42de0f7 100644 --- a/rbs_utils/rbs_utils/CMakeLists.txt +++ b/rbs_utils/rbs_utils/CMakeLists.txt @@ -16,7 +16,6 @@ find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(rbs_utils_interfaces REQUIRED) -find_package(dynmsg REQUIRED) find_package(rclpy REQUIRED) find_package(rosbag2_cpp REQUIRED) find_package(nlohmann_json REQUIRED) @@ -30,12 +29,9 @@ set(deps geometry_msgs sensor_msgs rbs_utils_interfaces - dynmsg rosbag2_cpp ) -add_library(${PROJECT_NAME} SHARED src/rbs_utils.cpp) - ament_python_install_package(${PROJECT_NAME}) install(PROGRAMS @@ -54,8 +50,6 @@ install( DESTINATION share/${PROJECT_NAME} ) -ament_target_dependencies(${PROJECT_NAME} PUBLIC ${deps}) - # target_include_directories(asm_folder_process # PUBLIC # "$" @@ -64,19 +58,6 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC ${deps}) add_executable(bagfile_recorder src/bagfile_recorder.cpp) ament_target_dependencies(bagfile_recorder ${deps}) -target_include_directories(${PROJECT_NAME} - PUBLIC - "$" - "$") - -install( - TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin -) - install( TARGETS bagfile_recorder DESTINATION lib/${PROJECT_NAME} @@ -96,8 +77,4 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies( - ${deps} -) ament_package() diff --git a/rbs_utils/rbs_utils/include/rbs_utils/rbs_utils.hpp b/rbs_utils/rbs_utils/include/rbs_utils/rbs_utils.hpp deleted file mode 100644 index a8e0fc2..0000000 --- a/rbs_utils/rbs_utils/include/rbs_utils/rbs_utils.hpp +++ /dev/null @@ -1,102 +0,0 @@ -#include "dynmsg/message_reading.hpp" -#include "dynmsg/msg_parser.hpp" -#include "dynmsg/typesupport.hpp" -#include "dynmsg/yaml_utils.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/pose_array.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.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 -#include -#include -#include -#include - -const std::string env_dir = std::getenv("RBS_ASSEMBLY_DIR"); - -namespace rbs_utils { - -struct EnvModel { - std::string model_name; - std::string mesh_path; - std::vector model_inertia; - std::vector model_pose; - double mass; -}; - -class AssemblyConfigLoader { -public: - template - explicit AssemblyConfigLoader(const std::string &t_assembly_dir, - const NodePtr &t_node) - : AssemblyConfigLoader(t_assembly_dir, - t_node->get_node_logging_interface(), - t_node->get_node_clock_interface()) {} - - explicit AssemblyConfigLoader( - const std::string &t_assembly_dir, - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr - &t_logger_node_interface, - const rclcpp::node_interfaces::NodeClockInterface::SharedPtr - &t_clock_node_interface); - - tf2_msgs::msg::TFMessage getAllPossibleTfData(); - tf2_msgs::msg::TFMessage getTfData(const std::string &model_name); - tf2_msgs::msg::TFMessage getGraspTfData(const std::string &model_name); - - std::vector getUniqueSceneModelNames(); - - void printWorkspace(); - geometry_msgs::msg::PoseArray getWorkspaceInspectorTrajectory(); - geometry_msgs::msg::Pose - transformTrajectory(const geometry_msgs::msg::Pose &pose); - void saveRbsConfig(); - tf2_msgs::msg::TFMessage getAdditionalPoses(); - -private: - std::vector m_env_files; - std::vector m_env_paths; - std::string m_assembly_dir; - std::unique_ptr m_tf_buffer; - rbs_utils_interfaces::msg::AssemblyConfig m_assembly_config; - rclcpp::Logger m_logger; - rclcpp::Clock::SharedPtr m_clock; - void parseRbsDb(const std::string &filepath); - - geometry_msgs::msg::PoseArray getWorkspace(); - geometry_msgs::msg::Transform - createTransform(const geometry_msgs::msg::Pose &pose); -}; - -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/rbs_utils/package.nix b/rbs_utils/rbs_utils/package.nix index 0da886f..6ce88e1 100644 --- a/rbs_utils/rbs_utils/package.nix +++ b/rbs_utils/rbs_utils/package.nix @@ -3,7 +3,7 @@ # Copyright 2025 None # Distributed under the terms of the Apache-2.0 license -{ lib, buildRosPackage, ament-cmake, ament-lint-auto, ament-lint-common, dynmsg, dynmsg-msgs, geometry-msgs, nlohmann_json, rbs-utils-interfaces, rclcpp, rclcpp-lifecycle, rclpy, rosbag2-cpp, rosidl-default-generators, rviz-visual-tools, sensor-msgs, std-msgs, tf2-eigen, tf2-ros }: +{ lib, buildRosPackage, ament-cmake, ament-lint-auto, ament-lint-common, geometry-msgs, nlohmann_json, rbs-utils-interfaces, rclcpp, rclcpp-lifecycle, rclpy, rosbag2-cpp, rosidl-default-generators, rviz-visual-tools, sensor-msgs, std-msgs, tf2-eigen, tf2-ros }: buildRosPackage rec { pname = "ros-jazzy-rbs-utils"; version = "0.0.0"; @@ -13,7 +13,7 @@ buildRosPackage rec { buildType = "ament_cmake"; buildInputs = [ ament-cmake ]; checkInputs = [ ament-lint-auto ament-lint-common ]; - propagatedBuildInputs = [ dynmsg dynmsg-msgs geometry-msgs nlohmann_json rbs-utils-interfaces rclcpp rclcpp-lifecycle rclpy rosbag2-cpp rosidl-default-generators rviz-visual-tools sensor-msgs std-msgs tf2-eigen tf2-ros ]; + propagatedBuildInputs = [ geometry-msgs nlohmann_json rbs-utils-interfaces rclcpp rclcpp-lifecycle rclpy rosbag2-cpp rosidl-default-generators rviz-visual-tools sensor-msgs std-msgs tf2-eigen tf2-ros ]; nativeBuildInputs = [ ament-cmake ]; meta = { diff --git a/rbs_utils/rbs_utils/package.xml b/rbs_utils/rbs_utils/package.xml index a01b979..7a1e0da 100644 --- a/rbs_utils/rbs_utils/package.xml +++ b/rbs_utils/rbs_utils/package.xml @@ -9,9 +9,6 @@ ament_cmake - dynmsg - dynmsg_msgs - rclpy rclcpp rclcpp_lifecycle diff --git a/rbs_utils/rbs_utils/src/rbs_utils.cpp b/rbs_utils/rbs_utils/src/rbs_utils.cpp deleted file mode 100644 index 5d84828..0000000 --- a/rbs_utils/rbs_utils/src/rbs_utils.cpp +++ /dev/null @@ -1,297 +0,0 @@ -#include "rbs_utils/rbs_utils.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rbs_utils { -AssemblyConfigLoader::AssemblyConfigLoader( - const std::string &t_assembly_dir, - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr - &t_logging_interface, - const rclcpp::node_interfaces::NodeClockInterface::SharedPtr - &t_clock_interface) - : m_assembly_dir(t_assembly_dir), - m_logger(t_logging_interface->get_logger()), - m_clock(t_clock_interface->get_clock()) { - if (!m_assembly_dir.empty()) { - std::vector filenames = {"rbs_db"}; - for (auto &filename : filenames) { - std::string filepath = - env_dir + "/" + m_assembly_dir + "/" + filename + ".yaml"; - - m_env_files.push_back(filepath); - parseRbsDb(filepath); - } - } else { - RCLCPP_ERROR(m_logger, "Assembly dir is not set"); - } -} - -void AssemblyConfigLoader::parseRbsDb(const std::string &filepath) { - try { - YAML::Node asm_config = YAML::LoadFile(filepath); - std::string asm_config_string = dynmsg::yaml_to_string(asm_config); - - RosMessage_Cpp rosmsg; - InterfaceTypeName interface{"rbs_utils_interfaces", "AssemblyConfig"}; - rosmsg.type_info = dynmsg::cpp::get_type_info(interface); - - void *ros_msg = reinterpret_cast(&m_assembly_config); - dynmsg::cpp::yaml_and_typeinfo_to_rosmsg(rosmsg.type_info, - asm_config_string, ros_msg); - - } catch (const std::exception &e) { - RCLCPP_ERROR(m_logger, "Exception reading file %s: %s", filepath.c_str(), - e.what()); - } -} - -[[deprecated("Not Implemented")]] void AssemblyConfigLoader::saveRbsConfig() {} - -std::vector AssemblyConfigLoader::getUniqueSceneModelNames() { - std::vector model_names; - if (m_assembly_config.relative_part.size() != 0) { - for (auto &t : m_assembly_config.relative_part) { - model_names.push_back(t.name); - } - } else { - return model_names; - } - // Sort and remove duplicates - std::sort(model_names.begin(), model_names.end()); - model_names.erase(std::unique(model_names.begin(), model_names.end()), - model_names.end()); - - return model_names; -} - -tf2_msgs::msg::TFMessage AssemblyConfigLoader::getAllPossibleTfData() { - tf2_msgs::msg::TFMessage tp; - // Get absolute parts - for (auto &abs_poses : m_assembly_config.absolute_part) { - geometry_msgs::msg::TransformStamped tmp; - tmp.transform = createTransform(abs_poses.pose); - tmp.child_frame_id = abs_poses.name; - tmp.header.frame_id = "world"; - tmp.header.stamp = m_clock->now(); - tp.transforms.push_back(tmp); - } - // Get relative parts - for (const auto &relative_part : m_assembly_config.relative_part) { - geometry_msgs::msg::TransformStamped tmp; - tmp.transform = createTransform(relative_part.pose); - tmp.child_frame_id = relative_part.name; - tmp.header.frame_id = relative_part.relative_at; - tmp.header.stamp = m_clock->now(); - tp.transforms.push_back(tmp); - RCLCPP_INFO(m_logger, "Model name [%s]", relative_part.name.c_str()); - } - // Get grasp poses - for (const auto &grasp_pose : m_assembly_config.grasp_poses) { - geometry_msgs::msg::TransformStamped tmp; - tmp.transform = createTransform(grasp_pose.pose); - tmp.child_frame_id = grasp_pose.name; - tmp.header.frame_id = grasp_pose.relative_at; - tmp.header.stamp = m_clock->now(); - tp.transforms.push_back(tmp); - } - - return tp; -} - -tf2_msgs::msg::TFMessage -AssemblyConfigLoader::getGraspTfData(const std::string &model_name) { - tf2_msgs::msg::TFMessage tp; - - bool found_grasp_pose = false; - if (!m_assembly_config.relative_part.empty()) { - for (auto &abs_poses : m_assembly_config.absolute_part) { - geometry_msgs::msg::TransformStamped tmp; - tmp.transform = createTransform(abs_poses.pose); - tmp.child_frame_id = abs_poses.name; - tmp.header.frame_id = "world"; - tmp.header.stamp = m_clock->now(); - tp.transforms.push_back(tmp); - } - } else { - RCLCPP_ERROR(m_logger, "Relative parts is empty size: %zu", - m_assembly_config.relative_part.size()); - } - - for (const auto &grasp_pose : m_assembly_config.grasp_poses) { - if (grasp_pose.relative_at == model_name) { - geometry_msgs::msg::TransformStamped tmp; - tmp.transform = createTransform(grasp_pose.pose); - tmp.child_frame_id = grasp_pose.name; - tmp.header.frame_id = grasp_pose.relative_at; - tmp.header.stamp = m_clock->now(); - tp.transforms.push_back(tmp); - found_grasp_pose = true; - } - } - - if (!found_grasp_pose) { - RCLCPP_ERROR(m_logger, "Grasp pose not found for model %s", - model_name.c_str()); - } - - return tp; - -} - -tf2_msgs::msg::TFMessage -AssemblyConfigLoader::getTfData(const std::string &model_name) { - tf2_msgs::msg::TFMessage tp; - - if (!m_assembly_config.absolute_part.empty()) { - for (auto &abs_part : m_assembly_config.absolute_part) { - geometry_msgs::msg::TransformStamped abs_transrorm_stamped; - abs_transrorm_stamped.transform = createTransform(abs_part.pose); - abs_transrorm_stamped.child_frame_id = abs_part.name; - abs_transrorm_stamped.header.frame_id = "world"; - abs_transrorm_stamped.header.stamp = m_clock->now(); - tp.transforms.push_back(abs_transrorm_stamped); - } - } else { - RCLCPP_ERROR(m_logger, "Absolute parts is empty size: %zu", - m_assembly_config.absolute_part.size()); - } - - bool found_model = false; - bool found_grasp_pose = false; - if (!m_assembly_config.relative_part.empty()) { - for (auto &abs_poses : m_assembly_config.absolute_part) { - geometry_msgs::msg::TransformStamped tmp; - tmp.transform = createTransform(abs_poses.pose); - tmp.child_frame_id = abs_poses.name; - tmp.header.frame_id = "world"; - tmp.header.stamp = m_clock->now(); - tp.transforms.push_back(tmp); - } - - for (const auto &relative_part : m_assembly_config.relative_part) { - // Find our model data - if (relative_part.name == model_name) { - geometry_msgs::msg::TransformStamped tmp; - tmp.transform = createTransform(relative_part.pose); - tmp.child_frame_id = relative_part.name; - tmp.header.frame_id = relative_part.relative_at; - tmp.header.stamp = m_clock->now(); - tp.transforms.push_back(tmp); - found_model = true; - } - RCLCPP_INFO(m_logger, "Model name [%s]", - relative_part.relative_at.c_str()); - } - } else { - RCLCPP_ERROR(m_logger, "Relative parts is empty size: %zu", - m_assembly_config.relative_part.size()); - } - - for (const auto &grasp_pose : m_assembly_config.grasp_poses) { - if (grasp_pose.relative_at == model_name) { - geometry_msgs::msg::TransformStamped tmp; - tmp.transform = createTransform(grasp_pose.pose); - tmp.child_frame_id = grasp_pose.name; - tmp.header.frame_id = grasp_pose.relative_at; - tmp.header.stamp = m_clock->now(); - tp.transforms.push_back(tmp); - found_grasp_pose = true; - } - } - - if (!found_model) { - RCLCPP_ERROR(m_logger, "Model %s not found in config", model_name.c_str()); - } - if (!found_grasp_pose) { - RCLCPP_ERROR(m_logger, "Grasp pose not found for model %s", - model_name.c_str()); - } - - return tp; -} - -geometry_msgs::msg::Transform -AssemblyConfigLoader::createTransform(const geometry_msgs::msg::Pose &pose) { - geometry_msgs::msg::Transform transform; - transform.translation.x = pose.position.x; - transform.translation.y = pose.position.y; - transform.translation.z = pose.position.z; - transform.rotation.x = pose.orientation.x; - transform.rotation.y = pose.orientation.y; - transform.rotation.z = pose.orientation.z; - transform.rotation.w = pose.orientation.w; - return transform; -} - -geometry_msgs::msg::PoseArray AssemblyConfigLoader::getWorkspace() { - geometry_msgs::msg::PoseArray pose_array; - pose_array.header.frame_id = "world"; - - if (m_assembly_config.workspace.empty()) { - RCLCPP_WARN(m_logger, "Workspace is empty, check your robossembler_db"); - return pose_array; - } - - pose_array.poses.reserve(m_assembly_config.workspace.size()); - - const double default_rotation_value = 0.0; - const double default_rotation_w = 1.0; - - for (const auto &point : m_assembly_config.workspace) { - geometry_msgs::msg::Pose pose; - pose.position.x = point.x; - pose.position.y = point.y; - pose.position.z = point.z; - pose.orientation.x = default_rotation_value; - pose.orientation.y = default_rotation_value; - pose.orientation.z = default_rotation_value; - pose.orientation.w = default_rotation_w; - pose_array.poses.push_back(pose); - } - - return pose_array; -} - -geometry_msgs::msg::PoseArray -AssemblyConfigLoader::getWorkspaceInspectorTrajectory() { - geometry_msgs::msg::PoseArray pose_array; - pose_array.header.frame_id = "world"; - - auto workspace_poses = getWorkspace(); - - for (const auto &pose : workspace_poses.poses) { - pose_array.poses.push_back(transformTrajectory(pose)); - } - - return pose_array; -} - -geometry_msgs::msg::Pose AssemblyConfigLoader::transformTrajectory( - const geometry_msgs::msg::Pose &pose) { - Eigen::Isometry3d pose_eigen; - tf2::fromMsg(pose, pose_eigen); - - Eigen::AngleAxisd rotation(M_PI, Eigen::Vector3d::UnitY()); - pose_eigen.rotate(rotation); - pose_eigen.translation().z() += 0.35; - return tf2::toMsg(pose_eigen); -} - -} // namespace rbs_utils diff --git a/repos/all-deps.repos b/repos/all-deps.repos index 6dde101..06aee04 100644 --- a/repos/all-deps.repos +++ b/repos/all-deps.repos @@ -7,18 +7,10 @@ repositories: type: git url: https://github.com/solid-sinusoid/robot-builder.git version: main - rbs_gripper: - type: git - url: https://github.com/solid-sinusoid/rbs-gripper.git - version: main behavior_tree: type: git url: https://github.com/BehaviorTree/BehaviorTree.ROS2.git version: humble - dynamic_message_introspection: - type: git - url: https://github.com/osrf/dynamic_message_introspection.git - version: main robot_builder: type: git url: https://github.com/solid-sinusoid/robot-builder.git @@ -27,11 +19,3 @@ repositories: type: git url: https://github.com/solid-sinusoid/cartesian_controllers.git version: gazebo-simulation - ros2_control: - type: git - url: https://github.com/solid-sinusoid/ros2_control.git - version: gz-ros2-cartesian-controllers - gz_ros2_control: - type: git - url: https://github.com/solid-sinusoid/gz_ros2_control.git - version: fts-sensor