Skip to content

Commit

Permalink
Revert "Merge pull request #13 from sbgisen/feature/visualize_padding2"
Browse files Browse the repository at this point in the history 
This reverts commit be3cd83.
  • Loading branch information
@Tacha-S @nyxrobotics
Tacha-S authored and nyxrobotics committed May 28, 2024
1 parent 4f6515e commit e3216a9
Show file tree
Hide file tree
Showing 6 changed files with 5 additions and 223 deletions.
18 changes: 0 additions & 18 deletions moveit_core/robot_model/include/moveit/robot_model/robot_model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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
{
Expand Down Expand Up @@ -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 */
Expand Down Expand Up @@ -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
Expand Down
179 changes: 0 additions & 179 deletions moveit_core/robot_model/src/robot_model.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand All @@ -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;
Expand Down Expand Up @@ -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
7 changes: 0 additions & 7 deletions moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -101,7 +95,6 @@ class RDFLoader

private:
std::string robot_description_;
std::string robot_description_content_;
srdf::ModelSharedPtr srdf_;
urdf::ModelInterfaceSharedPtr urdf_;
};
Expand Down
6 changes: 3 additions & 3 deletions moveit_ros/planning/rdf_loader/src/rdf_loader.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
3 changes: 1 addition & 2 deletions moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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())
Expand Down
15 changes: 1 addition & 14 deletions moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down

0 comments on commit e3216a9

Please sign in to comment.