From 192887ab8b828f4c2c3fd24f93e9183fa61ca6c0 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Sat, 30 Dec 2023 23:39:43 +0300 Subject: [PATCH] add workspace base functional Workspace functional includes basic read from workspace and trajectory calculation for inspection --- .../include/env_interface/env_interface.hpp | 2 +- .../include/gz_enviroment/gz_enviroment.hpp | 42 ++-- .../gz_enviroment/src/gz_enviroment.cpp | 24 +-- rbs_utils/CMakeLists.txt | 21 +- rbs_utils/include/rbs_utils/rbs_utils.hpp | 51 +++-- rbs_utils/package.xml | 2 +- rbs_utils/src/rbs_utils.cpp | 199 +++++++++++++----- 7 files changed, 240 insertions(+), 101 deletions(-) diff --git a/env_manager/env_interface/include/env_interface/env_interface.hpp b/env_manager/env_interface/include/env_interface/env_interface.hpp index 7b5268b..807bdf3 100644 --- a/env_manager/env_interface/include/env_interface/env_interface.hpp +++ b/env_manager/env_interface/include/env_interface/env_interface.hpp @@ -11,6 +11,6 @@ public: EnvInterface() = default; virtual ~EnvInterface() = default; protected: - std::shared_ptr> m_env_models; + std::shared_ptr> m_env_models; }; } // namespace env_interface diff --git a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp index 8013a79..853b2ec 100644 --- a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp +++ b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp @@ -1,37 +1,37 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" // #include "ros_gz_bridge/convert.hpp" +#include "env_interface/env_interface.hpp" +#include "gz/msgs/pose_v.pb.h" +#include +#include "geometry_msgs/msg/pose_array.hpp" +#include #include #include #include #include -#include #include -#include -#include -#include "gz/msgs/pose_v.pb.h" -#include "env_interface/env_interface.hpp" +#include -#include #include #include +#include -namespace gz_enviroment -{ -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +namespace gz_enviroment { +using CallbackReturn = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -class GzEnviroment : public env_interface::EnvInterface -{ +class GzEnviroment : public env_interface::EnvInterface { public: GzEnviroment(); CallbackReturn on_init() override; - CallbackReturn on_configure(const rclcpp_lifecycle::State&) override; - CallbackReturn on_activate(const rclcpp_lifecycle::State&) override; - CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override; - CallbackReturn on_cleanup(const rclcpp_lifecycle::State&) override; + CallbackReturn on_configure(const rclcpp_lifecycle::State &) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State &) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override; + CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override; protected: - void onGzPoseSub(const gz::msgs::Pose_V& pose_v); + void onGzPoseSub(const gz::msgs::Pose_V &pose_v); bool doGzSpawn(); @@ -47,9 +47,11 @@ private: tf2_msgs::msg::TFMessage m_transforms; tf2_msgs::msg::TFMessage m_target_places; - std::string getGzWorldName(); - std::string createGzModelString(const std::vector& pose, const std::vector& inertia, const double &mass, const std::string& mesh_filepath, - const std::string& name); + std::string createGzModelString(const std::vector &pose, + const std::vector &inertia, + const double &mass, + const std::string &mesh_filepath, + const std::string &name); }; -} // namespace gz_enviroment +} // namespace gz_enviroment diff --git a/env_manager/gz_enviroment/src/gz_enviroment.cpp b/env_manager/gz_enviroment/src/gz_enviroment.cpp index 65c58b8..89432c7 100644 --- a/env_manager/gz_enviroment/src/gz_enviroment.cpp +++ b/env_manager/gz_enviroment/src/gz_enviroment.cpp @@ -4,6 +4,7 @@ #include "ros_gz_bridge/convert.hpp" #include #include +#include #include #include @@ -25,7 +26,8 @@ CallbackReturn GzEnviroment::on_init() { for (const auto entity : modelEntities) { const auto modelName = ecm.Component(entity); if (modelName) { - RCLCPP_INFO_STREAM(getNode()->get_logger(), "Model Name: " << modelName->Data()); + RCLCPP_INFO_STREAM(getNode()->get_logger(), + "Model Name: " << modelName->Data()); } } return CallbackReturn::SUCCESS; @@ -38,11 +40,13 @@ CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) { m_world_name = getGzWorldName(); m_topic_name = std::string("/world/") + m_world_name + "/dynamic_pose/info"; m_service_spawn = std::string("/world/") + m_world_name + "/create"; - m_config_loader = - std::make_shared("asp-example2"); + m_config_loader = std::make_shared( + "asp-example2", getNode()); m_follow_frames = m_config_loader->getSceneModelNames(); // m_target_places = std::make_shared(); m_transforms = m_config_loader->getTfData("bishop"); + // TODO add workspace viewer in Rviz + // m_config_loader->printWorkspace(); // if (!doGzSpawn()) // { @@ -67,11 +71,7 @@ CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State &) { CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State &) { RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_cleanup"); - // Clear tf2 broadcasters - // m_tf2_broadcaster.reset(); - // m_tf2_broadcaster_target.reset(); - - // Clear other variables + // Clear variables on_cleanup m_gz_node.reset(); m_follow_frames.clear(); m_topic_name.clear(); @@ -86,10 +86,7 @@ CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State &) { CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_deactivate"); - // m_tf2_broadcaster.reset(); - // m_tf2_broadcaster_target.reset(); - // m_gz_node.reset(); - + // What we should use here? return CallbackReturn::SUCCESS; } @@ -133,7 +130,6 @@ void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V &pose_v) { } } - std::string GzEnviroment::getGzWorldName() { bool executed{false}; bool result{false}; @@ -212,4 +208,4 @@ std::string GzEnviroment::createGzModelString( #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(gz_enviroment::GzEnviroment, - env_interface::EnvInterface); \ No newline at end of file + env_interface::EnvInterface); diff --git a/rbs_utils/CMakeLists.txt b/rbs_utils/CMakeLists.txt index 06a0a2f..9a06110 100644 --- a/rbs_utils/CMakeLists.txt +++ b/rbs_utils/CMakeLists.txt @@ -8,12 +8,22 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_eigen REQUIRED) +find_package(rviz_visual_tools REQUIRED) +find_package(geometry_msgs REQUIRED) + +set(deps + rclcpp + rclcpp_lifecycle + tf2_ros + tf2_eigen + rviz_visual_tools + geometry_msgs +) + -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) add_library(${PROJECT_NAME} SHARED src/rbs_utils.cpp) @@ -22,7 +32,7 @@ install( DESTINATION include ) -ament_target_dependencies(${PROJECT_NAME} PUBLIC rclcpp tf2_ros tf2_eigen) +ament_target_dependencies(${PROJECT_NAME} PUBLIC ${deps}) # target_include_directories(asm_folder_process # PUBLIC @@ -57,4 +67,7 @@ if(BUILD_TESTING) endif() ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ${deps} +) ament_package() diff --git a/rbs_utils/include/rbs_utils/rbs_utils.hpp b/rbs_utils/include/rbs_utils/rbs_utils.hpp index d858a9c..e0f5d2e 100644 --- a/rbs_utils/include/rbs_utils/rbs_utils.hpp +++ b/rbs_utils/include/rbs_utils/rbs_utils.hpp @@ -1,24 +1,25 @@ +#include +#include "geometry_msgs/msg/pose.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rviz_visual_tools/rviz_visual_tools.hpp" #include "tf2_msgs/msg/tf_message.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/buffer.h" +#include #include "tf2_ros/static_transform_broadcaster.h" - #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 EnvModels { +struct EnvModel { std::string model_name; std::string mesh_path; std::vector model_inertia; @@ -26,10 +27,23 @@ struct EnvModels { double mass; }; -class AssemblyConfigLoader { +struct Workspace { + std::string name; + geometry_msgs::msg::PoseArray poses; +}; +class AssemblyConfigLoader { public: - AssemblyConfigLoader(const std::string &t_assembly_dir); + template + explicit AssemblyConfigLoader(const std::string &t_assembly_dir, + const NodePtr &t_node) + : AssemblyConfigLoader(t_assembly_dir, + t_node->get_node_logging_interface()) {} + + explicit AssemblyConfigLoader( + const std::string &t_assembly_dir, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr + &t_logger_node_interface); inline std::shared_ptr> getAssemblyFileData() { @@ -37,31 +51,40 @@ public: } tf2_msgs::msg::TFMessage getTfData(const std::string &model_name); - inline std::shared_ptr> getEnvModels() { + inline std::shared_ptr> getEnvModels() { return m_env_models; } std::vector getSceneModelNames(); + void printWorkspace(); + geometry_msgs::msg::PoseArray getWorkspaceInspectorTrajectory(); + geometry_msgs::msg::Pose + transformTrajectory(const geometry_msgs::msg::TransformStamped &pose); + private: std::shared_ptr> m_env_vars; std::vector m_env_files; std::vector m_env_paths; - std::shared_ptr> m_env_models{}; + std::shared_ptr> m_env_models{}; std::string m_assembly_dir; - std::unique_ptr m_tf_buffer; - + // rviz_visual_tools::RvizVisualToolsPtr m_visual_tools; + rclcpp::Logger m_logger; void readAssemblyFileData(const std::string &filename, const std::string &filepath); + tf2_msgs::msg::TFMessage readWorkspaceJson(const nlohmann::json &json); + tf2_msgs::msg::TFMessage parseJsonToTFMessage(const nlohmann::json &json, const std::string &model_name); + void publishWorkspace(const tf2_msgs::msg::TFMessage &tf_msg); + void setTfFromDb(const std::string &filename); double convertToDouble(const nlohmann::json &value); void readModelConfigs(); - EnvModels parseModelData(const std::string &jsonFilePath); + EnvModel parseModelData(const std::string &jsonFilePath); }; class StaticFramePublisher : public rclcpp::Node { diff --git a/rbs_utils/package.xml b/rbs_utils/package.xml index c0d5e9b..ad5eb00 100644 --- a/rbs_utils/package.xml +++ b/rbs_utils/package.xml @@ -8,7 +8,7 @@ Apache-2.0 ament_cmake - + rviz_visual_tools ament_lint_auto ament_lint_common diff --git a/rbs_utils/src/rbs_utils.cpp b/rbs_utils/src/rbs_utils.cpp index 6cf85c3..45db08f 100644 --- a/rbs_utils/src/rbs_utils.cpp +++ b/rbs_utils/src/rbs_utils.cpp @@ -1,26 +1,24 @@ #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 +#include namespace rbs_utils { - -AssemblyConfigLoader::AssemblyConfigLoader(const std::string &t_assembly_dir) +AssemblyConfigLoader::AssemblyConfigLoader( + const std::string &t_assembly_dir, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr + &t_logging_interface) : m_env_vars( std::make_shared>()), - m_env_models(std::make_shared>()), - m_assembly_dir(t_assembly_dir) { + m_env_models(std::make_shared>()), + m_assembly_dir(t_assembly_dir), + m_logger(t_logging_interface->get_logger()) { + // m_visual_tools.reset(new + // rviz_visual_tools::RvizVisualTools("world","/rviz_visual_markers", + // m_node)); if (!m_assembly_dir.empty()) { std::vector filenames = {"robossembler_db", "sequences"}; for (auto &filename : filenames) { @@ -31,9 +29,8 @@ AssemblyConfigLoader::AssemblyConfigLoader(const std::string &t_assembly_dir) readAssemblyFileData(filename, filepath); } readModelConfigs(); - } else { - RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), "Assembly dir is not set"); + RCLCPP_ERROR(m_logger, "Assembly dir is not set"); } } @@ -42,44 +39,39 @@ void AssemblyConfigLoader::readAssemblyFileData(const std::string &filename, try { std::ifstream i(filepath); if (!i.is_open()) { - RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), "Failed to open file: %s", - filepath.c_str()); + RCLCPP_ERROR(m_logger, "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()); + RCLCPP_ERROR(m_logger, "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()); + RCLCPP_ERROR(m_logger, "Exception reading file %s: %s", filepath.c_str(), + e.what()); } } -EnvModels +EnvModel AssemblyConfigLoader::parseModelData(const std::string &json_file_path) { nlohmann::json json_content{}; try { std::ifstream file(json_file_path); if (!file.is_open()) { - RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), "Failed to open file: %s", - json_file_path.c_str()); - return EnvModels{}; + RCLCPP_ERROR(m_logger, "Failed to open file: %s", json_file_path.c_str()); + return EnvModel{}; } json_content = nlohmann::json::parse(file); } catch (const nlohmann::json::parse_error &e) { - RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), - "Error parsing JSON in file %s: %s", json_file_path.c_str(), - e.what()); + RCLCPP_ERROR(m_logger, "Error parsing JSON in file %s: %s", + json_file_path.c_str(), e.what()); } catch (const std::exception &e) { - RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), - "Exception reading file %s: %s", json_file_path.c_str(), - e.what()); + RCLCPP_ERROR(m_logger, "Exception reading file %s: %s", + json_file_path.c_str(), e.what()); } - EnvModels envModel; + EnvModel envModel; if (!json_content.empty()) { envModel.model_name = json_content.at("name").get(); envModel.mesh_path = env_dir + "/" + m_assembly_dir + "/" + @@ -114,7 +106,7 @@ void AssemblyConfigLoader::readModelConfigs() { for (const auto &entry : std::filesystem::directory_iterator(assets_folder)) { if (entry.is_regular_file() && entry.path().extension() == ".json") { - EnvModels model = parseModelData(entry.path().string()); + EnvModel model = parseModelData(entry.path().string()); m_env_models->push_back(model); } } @@ -142,8 +134,7 @@ std::vector AssemblyConfigLoader::getSceneModelNames() { } } } else { - RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), - "Key 'robossembler_db' not found in m_env_vars."); + RCLCPP_ERROR(m_logger, "Key 'robossembler_db' not found in m_env_vars."); return model_names; } @@ -165,7 +156,7 @@ AssemblyConfigLoader::getTfData(const std::string &model_name) { // Output position information to console for (const auto &transform : tf_msg.transforms) { RCLCPP_INFO_STREAM( - rclcpp::get_logger("rbs_utils"), + m_logger, "Frame ID: " << transform.header.frame_id << ", Child Frame ID: " << transform.child_frame_id << ", Translation: [" << transform.transform.translation.x << ", " @@ -175,8 +166,7 @@ AssemblyConfigLoader::getTfData(const std::string &model_name) { // auto r = std::make_shared(tf_msg); } else { - RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), - "Key 'robossembler_db' not found in m_env_vars."); + RCLCPP_ERROR(m_logger, "Key 'robossembler_db' not found in m_env_vars."); } return tf_msg; } @@ -262,6 +252,122 @@ AssemblyConfigLoader::parseJsonToTFMessage(const nlohmann::json &json, return tf_msg; } +tf2_msgs::msg::TFMessage +AssemblyConfigLoader::readWorkspaceJson(const nlohmann::json &json) { + tf2_msgs::msg::TFMessage tf_msg; + int i{0}; + if (json.contains("workspace") && json.at("workspace").is_array()) { + for (auto &point : json.at("workspace")) { + if (point.contains("position") && point.at("position").is_object()) { + geometry_msgs::msg::TransformStamped workspace_point; + workspace_point.header.frame_id = "world"; + workspace_point.child_frame_id = "bbox_" + std::to_string(i); + i++; + + if (point["position"].contains("x") && + point["position"].contains("y") && + point["position"].contains("z")) { + workspace_point.transform.translation.x = + point["position"]["x"].get(); + workspace_point.transform.translation.y = + point["position"]["y"].get(); + workspace_point.transform.translation.z = + point["position"]["z"].get(); + tf_msg.transforms.push_back(workspace_point); + } else { + RCLCPP_ERROR(m_logger, + "Cannot find key 'x', 'y', 'z' in 'position' or 'name' " + "isn't found in key 'workspace'"); + } + } else { + RCLCPP_ERROR(m_logger, + "Cannot find key 'position', 'name', or 'position' isn't " + "an object in key 'workspace'"); + } + } + } else { + RCLCPP_ERROR(m_logger, "Cannot find key 'workspace' in robossembler_db"); + } + + return tf_msg; +} + +void AssemblyConfigLoader::publishWorkspace( + const tf2_msgs::msg::TFMessage &tf_msg) { + // if (tf_msg.transforms.size() >= 4) { + // auto pose1 = + // Eigen::Vector3d(tf_msg.transforms[0].transform.translation.x, + // tf_msg.transforms[0].transform.translation.y, + // tf_msg.transforms[0].transform.translation.z); + // + // auto pose2 = + // Eigen::Vector3d(tf_msg.transforms[1].transform.translation.x, + // tf_msg.transforms[1].transform.translation.y, + // tf_msg.transforms[1].transform.translation.z); + // + // auto pose3 = + // Eigen::Vector3d(tf_msg.transforms[2].transform.translation.x, + // tf_msg.transforms[2].transform.translation.y, + // tf_msg.transforms[2].transform.translation.z); + // + // auto pose4 = + // Eigen::Vector3d(tf_msg.transforms[3].transform.translation.x, + // tf_msg.transforms[3].transform.translation.y, + // tf_msg.transforms[3].transform.translation.z); + // + // // Connect workspace points as box + // m_visual_tools->publishLine(pose1, pose2, rviz_visual_tools::GREEN); + // m_visual_tools->publishLine(pose2, pose3, rviz_visual_tools::GREEN); + // m_visual_tools->publishLine(pose3, pose4, rviz_visual_tools::GREEN); + // m_visual_tools->publishLine(pose4, pose1, rviz_visual_tools::GREEN); + // } else { + // RCLCPP_ERROR(m_logger, + // "Not enough points to form a square"); + // } +} + +void AssemblyConfigLoader::printWorkspace() { + if (m_env_vars->find("robossembler_db") != m_env_vars->end()) { + const nlohmann::json json = (*m_env_vars)["robossembler_db"]; + if (json.contains("workspace")) { + auto workspace = readWorkspaceJson(json); + publishWorkspace(workspace); + } else { + RCLCPP_WARN(m_logger, "Workspace isn't set in robossembler_db"); + } + } else { + RCLCPP_ERROR(m_logger, "Cannot find robossembler_db"); + } +} + +geometry_msgs::msg::PoseArray +AssemblyConfigLoader::getWorkspaceInspectorTrajectory() { + geometry_msgs::msg::PoseArray pose_array; + pose_array.header.frame_id = "world"; + if (m_env_vars->find("robossembler_db") != m_env_vars->end()) { + const nlohmann::json json = (*m_env_vars)["robossembler_db"]; + if (json.contains("workspace")) { + auto workspace = readWorkspaceJson(json); + for (auto &point : workspace.transforms) { + geometry_msgs::msg::Pose pose; + pose = transformTrajectory(point); + pose_array.poses.push_back(pose); + } + } + } + return pose_array; +} + +geometry_msgs::msg::Pose AssemblyConfigLoader::transformTrajectory( + const geometry_msgs::msg::TransformStamped &pose) { + auto pose_eigen = tf2::transformToEigen(pose.transform); + Eigen::AngleAxisd rotation(M_PI, Eigen::Vector3d::UnitX()); + pose_eigen.rotate(rotation); + pose_eigen.translation().z() += 0.5; + auto pose_msg = tf2::toMsg(pose_eigen); + return pose_msg; +} + double AssemblyConfigLoader::convertToDouble(const nlohmann::json &value) { if (value.is_number()) { return value.get(); @@ -269,8 +375,7 @@ double AssemblyConfigLoader::convertToDouble(const nlohmann::json &value) { 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()); + RCLCPP_ERROR(m_logger, "Error converting string to double: %s", e.what()); throw std::runtime_error("Error converting string to double"); } } @@ -278,4 +383,4 @@ double AssemblyConfigLoader::convertToDouble(const nlohmann::json &value) { throw std::runtime_error("Invalid JSON type for conversion to double"); } -} // namespace rbs_utils \ No newline at end of file +} // namespace rbs_utils