From e3216a9911508e71dae901c374d91af57982b1ba Mon Sep 17 00:00:00 2001 From: Tatsuro Sakaguchi Date: Thu, 21 Sep 2023 13:15:52 +0900 Subject: [PATCH] Revert "Merge pull request #13 from sbgisen/feature/visualize_padding2" This reverts commit be3cd8335f1190a6845c662b8fcd8d14c6076d78. --- .../include/moveit/robot_model/robot_model.h | 18 -- moveit_core/robot_model/src/robot_model.cpp | 179 ------------------ .../include/moveit/rdf_loader/rdf_loader.h | 7 - .../planning/rdf_loader/src/rdf_loader.cpp | 6 +- .../src/robot_model_loader.cpp | 3 +- .../src/planning_scene_display.cpp | 15 +- 6 files changed, 5 insertions(+), 223 deletions(-) diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index 3b54c776aa1..503e7027ec2 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h @@ -37,7 +37,6 @@ #pragma once -#include #include #include #include @@ -78,10 +77,6 @@ static inline void checkInterpolationParamBounds(const char LOGNAME[], double t) class RobotModel { public: - /** \brief Construct a kinematic model from a parsed description and a list of planning groups */ - RobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model, - std::string content); - /** \brief Construct a kinematic model from a parsed description and a list of planning groups */ RobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model); @@ -109,13 +104,6 @@ class RobotModel return root_link_ == nullptr; } - /** \brief construct padded and scaled URDF model*/ - urdf::ModelInterfaceSharedPtr createPaddedScaledURDF(const std::map scales, - const std::map paddings) const; - - /** \brief remove STL file, use after loading padded/scaled URDF model*/ - void deleteSTLFromURDF(const urdf::ModelInterfaceSharedPtr& urdf_model) const; - /** \brief Get the parsed URDF model */ const urdf::ModelInterfaceSharedPtr& getURDF() const { @@ -509,9 +497,6 @@ class RobotModel urdf::ModelInterfaceSharedPtr urdf_; - /** \brief Content of robot description from parameter server */ - std::string robot_description_content_; - // LINKS /** \brief The first physical link for the robot */ @@ -669,9 +654,6 @@ class RobotModel /** \brief Given a geometry spec from the URDF and a filename (for a mesh), construct the corresponding shape object*/ shapes::ShapePtr constructShape(const urdf::Geometry* geom); - - /** \brief Given a shape object, construct a geometry spec for the URDF*/ - urdf::GeometrySharedPtr constructGeometry(const shapes::ShapePtr& shape) const; }; } // namespace core } // namespace moveit diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 81a3884b152..5f6eed84c93 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -40,12 +40,10 @@ #include #include #include -#include #include #include #include #include "order_robot_model_items.inc" -#include namespace moveit { @@ -56,16 +54,6 @@ namespace constexpr char LOGNAME[] = "robot_model"; } // namespace -RobotModel::RobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model, - const std::string content) -{ - root_joint_ = nullptr; - urdf_ = urdf_model; - srdf_ = srdf_model; - buildModel(*urdf_model, *srdf_model); - robot_description_content_ = content; -} - RobotModel::RobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model) { root_joint_ = nullptr; @@ -1560,172 +1548,5 @@ void RobotModel::computeFixedTransforms(const LinkModel* link, const Eigen::Isom transform * link->getJointOriginTransform(), associated_transforms); } -urdf::GeometrySharedPtr RobotModel::constructGeometry(const shapes::ShapePtr& shape) const -{ - if (!shape) - { - return nullptr; - } - switch (shape->type) - { - case shapes::SPHERE: - { - const shapes::Sphere* sphere = static_cast(shape.get()); - urdf::SphereSharedPtr urdf_sphere = std::make_shared(); - urdf_sphere->radius = sphere->radius; - return urdf_sphere; - } - case shapes::BOX: - { - const shapes::Box* box = static_cast(shape.get()); - urdf::BoxSharedPtr urdf_box = std::make_shared(); - urdf_box->dim.x = box->size[0]; - urdf_box->dim.y = box->size[1]; - urdf_box->dim.z = box->size[2]; - return urdf_box; - } - case shapes::CYLINDER: - { - const shapes::Cylinder* cylinder = static_cast(shape.get()); - urdf::CylinderSharedPtr urdf_cylinder = std::make_shared(); - urdf_cylinder->radius = cylinder->radius; - urdf_cylinder->length = cylinder->length; - return urdf_cylinder; - } - case shapes::MESH: - { - urdf::MeshSharedPtr urdf_mesh = std::make_shared(); - return urdf_mesh; - } - default: - ROS_ERROR_NAMED(LOGNAME, "Unknown geometry type: %d", (int)shape->type); - return nullptr; - } -} - -void RobotModel::deleteSTLFromURDF(const urdf::ModelInterfaceSharedPtr& urdf_model) const -{ - for (auto& link : urdf_model->links_) - { - if (!link.second->collision_array.empty()) - { - for (const auto& collision : link.second->collision_array) - { - if (collision->geometry->type == urdf::Geometry::MESH) - { - auto mesh = static_cast(collision->geometry.get()); - std::string filename = mesh->filename; - if (filename.substr(0, 7) == "file://") - { - filename = filename.substr(7); - std::filesystem::remove(filename.c_str()); - } - } - } - } - else if (link.second->collision) - { - if (link.second->collision->geometry->type == urdf::Geometry::MESH) - { - auto mesh = static_cast(link.second->collision->geometry.get()); - std::string filename = mesh->filename; - if (filename.substr(0, 7) == "file://") - { - filename = filename.substr(7); - std::filesystem::remove(filename.c_str()); - } - } - } - } - ROS_INFO_STREAM_NAMED(LOGNAME, "Removed all STL files"); -} - -urdf::ModelInterfaceSharedPtr RobotModel::createPaddedScaledURDF(const std::map scales, - const std::map paddings) const -{ - std::unique_ptr urdf(new urdf::Model()); - if (!urdf->initString(robot_description_content_)) - { - ROS_WARN_STREAM_NAMED(LOGNAME, "Could not set padding and scaling parameters because the URDF could not be parsed"); - return nullptr; - } - - urdf::ModelInterfaceSharedPtr padded_scaled_urdf = std::move(urdf); - - std::vector models = getLinkModelsWithCollisionGeometry(); - for (const auto link : models) - { - const std::string& link_name = link->getName(); - std::vector geometries; - for (const auto& shape : link->getShapes()) - { - shapes::ShapePtr scaled_padded_shape(shape->clone()); - double scale = scales.at(link_name); - double padd = paddings.at(link_name); - if (std::abs(scale - 1.0) <= std::numeric_limits::epsilon() && - std::abs(padd) <= std::numeric_limits::epsilon()) - { - continue; - } - scaled_padded_shape->scaleAndPadd(scale, padd); - if (scaled_padded_shape) - { - if (scaled_padded_shape->type == shapes::MESH) - { - std::string ros_home_dir; - ros::get_environment_variable(ros_home_dir, "ROS_HOME"); - if (ros_home_dir.empty()) - { - ros_home_dir = getenv("HOME") + std::string("/.ros"); - } - std::string filename = ros_home_dir + "/" + link->getName() + "_padded_scaled.stl"; - std::vector buffer; - shapes::writeSTLBinary(static_cast(scaled_padded_shape.get()), buffer); - std::ofstream out(filename, std::ios::binary | std::ios::trunc); - if (!out) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Failed to create/open " << filename << " due to " << strerror(errno)); - continue; - } - out.write(&buffer[0], buffer.size()); - out.close(); - if (!out) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Failed to write mesh to STL file: " << filename); - continue; - } - urdf::MeshSharedPtr urdf_mesh = std::make_shared(); - urdf_mesh->filename = "file://" + filename; - geometries.push_back(urdf_mesh); - } - else - { - geometries.push_back(constructGeometry(scaled_padded_shape)); - } - } - } - if (!padded_scaled_urdf->links_[link_name]->collision_array.empty()) - { - if (padded_scaled_urdf->links_[link_name]->collision_array.size() != geometries.size()) - { - continue; - } - for (std::size_t i = 0; i < padded_scaled_urdf->links_[link_name]->collision_array.size(); ++i) - { - padded_scaled_urdf->links_[link_name]->collision_array[i]->geometry = geometries[i]; - } - } - else if (padded_scaled_urdf->links_[link_name]->collision) - { - if (geometries.size() != 1) - { - continue; - } - padded_scaled_urdf->links_[link_name]->collision->geometry = geometries[0]; - } - } - return padded_scaled_urdf; -} - } // end of namespace core } // end of namespace moveit diff --git a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h index d264f107a4e..ae8b8cded84 100644 --- a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h +++ b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h @@ -62,12 +62,6 @@ class RDFLoader return robot_description_; } - /** @brief Get the robot_description content from the resolved parameter name*/ - const std::string& getRobotDescriptionContent() const - { - return robot_description_content_; - } - /** @brief Get the parsed URDF model*/ const urdf::ModelInterfaceSharedPtr& getURDF() const { @@ -101,7 +95,6 @@ class RDFLoader private: std::string robot_description_; - std::string robot_description_content_; srdf::ModelSharedPtr srdf_; urdf::ModelInterfaceSharedPtr urdf_; }; diff --git a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp index 56d0f9cb408..d2a43cc9dde 100644 --- a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp +++ b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp @@ -62,16 +62,16 @@ rdf_loader::RDFLoader::RDFLoader(const std::string& robot_description) ros::WallTime start = ros::WallTime::now(); ros::NodeHandle nh("~"); + std::string content; - if (!nh.searchParam(robot_description, robot_description_) || - !nh.getParam(robot_description_, robot_description_content_)) + if (!nh.searchParam(robot_description, robot_description_) || !nh.getParam(robot_description_, content)) { ROS_ERROR_NAMED("rdf_loader", "Robot model parameter not found! Did you remap '%s'?", robot_description.c_str()); return; } std::unique_ptr urdf(new urdf::Model()); - if (!urdf->initString(robot_description_content_)) + if (!urdf->initString(content)) { ROS_ERROR_NAMED("rdf_loader", "Unable to parse URDF from parameter '%s'", robot_description_.c_str()); return; diff --git a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp index 79d345d8bab..2ce6c2f6527 100644 --- a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp +++ b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp @@ -99,8 +99,7 @@ void RobotModelLoader::configure(const Options& opt) { const srdf::ModelSharedPtr& srdf = rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : std::make_shared(); - model_ = std::make_shared(rdf_loader_->getURDF(), srdf, - rdf_loader_->getRobotDescriptionContent()); + model_ = std::make_shared(rdf_loader_->getURDF(), srdf); } if (model_ && !rdf_loader_->getRobotDescription().empty()) diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index 62b68a9fbf4..e174dfb88f2 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -556,23 +556,10 @@ void PlanningSceneDisplay::onRobotModelLoaded() const planning_scene_monitor::LockedPlanningSceneRO& ps = getPlanningSceneRO(); if (planning_scene_robot_) { - auto scale = ps->getCollisionEnv()->getLinkScale(); - auto padd = ps->getCollisionEnv()->getLinkPadding(); - auto urdf = getRobotModel()->createPaddedScaledURDF(scale, padd); - bool use_padd_scale = true; - if (!urdf) - { - use_padd_scale = false; - urdf = getRobotModel()->getURDF(); - } - planning_scene_robot_->load(*urdf); + planning_scene_robot_->load(*getRobotModel()->getURDF()); moveit::core::RobotState* rs = new moveit::core::RobotState(ps->getCurrentState()); rs->update(); planning_scene_robot_->update(moveit::core::RobotStateConstPtr(rs)); - if (use_padd_scale) - { - getRobotModel()->deleteSTLFromURDF(urdf); - } } bool old_state = scene_name_property_->blockSignals(true);