robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

Remove deprecated files and dependencies from CMakeLists.txt and rbs_utils

Browse source 
- Removed dynamic_message_introspection, ros2_contro, and gz_ros2_controllers from all-deps.repos file
- Removed AssemblyConfigLoader due to it being deprecated (migrated to Python)
This commit is contained in:
Ilya Uraev 2025-02-06 17:54:22 +03:00
parent c48edd0d82
commit 50a97b0cbd
7 changed files with 2 additions and 447 deletions
Download patch file Download diff file Expand all files Collapse all files

4
overlay.nix
View file

@ -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 {};
}

23
rbs_utils/rbs_utils/CMakeLists.txt
View file

@ -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
# "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
@ -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
"$<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
)
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()

102
rbs_utils/rbs_utils/include/rbs_utils/rbs_utils.hpp
View file

@ -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 <Eigen/Core>
#include <fstream>
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <rbs_utils_interfaces/msg/detail/assembly_config__struct.hpp>
#include <rclcpp/clock.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/node_interfaces/node_clock_interface.hpp>
#include <tf2/convert.h>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <unordered_map>
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<double> model_inertia;
std::vector<double> model_pose;
double mass;
};
class AssemblyConfigLoader {
public:
template <typename NodePtr>
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<std::string> 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<std::string> m_env_files;
std::vector<std::ifstream> m_env_paths;
std::string m_assembly_dir;
std::unique_ptr<tf2_ros::Buffer> 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<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

4
rbs_utils/rbs_utils/package.nix
View file

@ -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 = {

3
rbs_utils/rbs_utils/package.xml
View file

@ -9,9 +9,6 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>dynmsg</build_depend>
<build_depend>dynmsg_msgs</build_depend>
<depend>rclpy</depend>
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>

297
rbs_utils/rbs_utils/src/rbs_utils.cpp
View file

@ -1,297 +0,0 @@
#include "rbs_utils/rbs_utils.hpp"
#include <Eigen/src/Geometry/Transform.h>
#include <dynmsg/typesupport.hpp>
#include <fstream>
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <geometry_msgs/msg/detail/transform_stamped__struct.hpp>
#include <rbs_utils_interfaces/msg/detail/assembly_config__struct.hpp>
#include <rbs_utils_interfaces/msg/detail/assembly_config__traits.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/node_interfaces/node_clock_interface.hpp>
#include <string>
#include <strstream>
#include <tf2/LinearMath/Transform.h>
#include <tf2/convert.h>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <tf2_ros/buffer_interface.h>
#include <yaml-cpp/emitter.h>
#include <yaml-cpp/node/parse.h>
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<std::string> 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<void *>(&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<std::string> AssemblyConfigLoader::getUniqueSceneModelNames() {
std::vector<std::string> 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

16
repos/all-deps.repos
View file

@ -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