Revert "Merge pull request #13 from sbgisen/feature/visualize_padding2"

moveit_core/robot_model/include/moveit/robot_model/robot_model.h

@@ -37,7 +37,6 @@
 
 #pragma once
 
-#include <geometric_shapes/shapes.h>
 #include <moveit/macros/class_forward.h>
 #include <moveit/exceptions/exceptions.h>
 #include <moveit/utils/lexical_casts.h>
@@ -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<std::string, double> scales,
-                                                       const std::map<std::string, double> 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

moveit_core/robot_model/src/robot_model.cpp

@@ -40,12 +40,10 @@
 #include <boost/math/constants/constants.hpp>
 #include <moveit/profiler/profiler.h>
 #include <algorithm>
-#include <fstream>
 #include <limits>
 #include <cmath>
 #include <memory>
 #include "order_robot_model_items.inc"
-#include <filesystem>
 
 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<const shapes::Sphere*>(shape.get());
-      urdf::SphereSharedPtr urdf_sphere = std::make_shared<urdf::Sphere>();
-      urdf_sphere->radius = sphere->radius;
-      return urdf_sphere;
-    }
-    case shapes::BOX:
-    {
-      const shapes::Box* box = static_cast<const shapes::Box*>(shape.get());
-      urdf::BoxSharedPtr urdf_box = std::make_shared<urdf::Box>();
-      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<const shapes::Cylinder*>(shape.get());
-      urdf::CylinderSharedPtr urdf_cylinder = std::make_shared<urdf::Cylinder>();
-      urdf_cylinder->radius = cylinder->radius;
-      urdf_cylinder->length = cylinder->length;
-      return urdf_cylinder;
-    }
-    case shapes::MESH:
-    {
-      urdf::MeshSharedPtr urdf_mesh = std::make_shared<urdf::Mesh>();
-      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<const urdf::Mesh*>(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<const urdf::Mesh*>(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<std::string, double> scales,
-                                                                 const std::map<std::string, double> paddings) const
-{
-  std::unique_ptr<urdf::Model> 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<const LinkModel*> models = getLinkModelsWithCollisionGeometry();
-  for (const auto link : models)
-  {
-    const std::string& link_name = link->getName();
-    std::vector<urdf::GeometrySharedPtr> 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<double>::epsilon() &&
-          std::abs(padd) <= std::numeric_limits<double>::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<char> buffer;
-          shapes::writeSTLBinary(static_cast<const shapes::Mesh*>(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>();
-          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

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_;
 };

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::Model> 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;

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<srdf::Model>();
-    model_ = std::make_shared<moveit::core::RobotModel>(rdf_loader_->getURDF(), srdf,
-                                                        rdf_loader_->getRobotDescriptionContent());
+    model_ = std::make_shared<moveit::core::RobotModel>(rdf_loader_->getURDF(), srdf);
   }
 
   if (model_ && !rdf_loader_->getRobotDescription().empty())

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);