diff --git a/CMakeLists.txt b/CMakeLists.txt index d43de59..991e80c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,23 +1,19 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.16) project(nimbro_primitive_fitter) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake) # For finding custom .cmake files - -find_package(catkin REQUIRED COMPONENTS - cmake_modules - roslib - roscpp - rospack - rosparam -) +SET(CMAKE_VERBOSE_MAKEFILE TRUE) +SET(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake) # For finding custom .cmake files +set(DISABLE_PAL_FLAGS true) find_package(Boost REQUIRED COMPONENTS system regex) find_package(Eigen3 REQUIRED) find_package(PugiXML REQUIRED) find_package(PkgConfig REQUIRED) - -pkg_check_modules(roboptim-capsule REQUIRED roboptim-capsule) +find_package(roboptim-capsule REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) include_directories( include @@ -30,7 +26,7 @@ include_directories( contrib/approx_mvbb/external/GeometryPredicates/include/ApproxMVBB/GeometryPredicates ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} - ${PUGIXML_INCLUDE_DIR} + ${PUGIXML_INCLUDE_DIR} ${roboptim-capsule_INCLUDE_DIRS} ) @@ -38,12 +34,12 @@ link_directories( ${roboptim-capsule_LIBRARY_DIRS} ) -catkin_package( +#catkin_package( # INCLUDE_DIRS include # LIBRARIES nimbro_primitive_fitter # CATKIN_DEPENDS other_catkin_pkg # DEPENDS system_lib -) +#) add_library(approx_mvbb contrib/approx_mvbb/src/ApproxMVBB/AABB.cpp @@ -60,7 +56,7 @@ add_library(approx_mvbb contrib/approx_mvbb/external/Diameter/src/EstimateDiameter.cpp ) -add_executable(nimbro_primitive_fitter +add_executable(nimbro_primitive_fitter src/primitive_fitter.cpp src/inertia.cpp src/util.cpp @@ -68,9 +64,28 @@ add_executable(nimbro_primitive_fitter ) target_link_libraries(nimbro_primitive_fitter - ${catkin_LIBRARIES} + rclcpp::rclcpp ${PUGIXML_LIBRARIES} - ${roboptim-capsule_LIBRARIES} + roboptim-capsule::roboptim-capsule approx_mvbb + ) + + +string(REPLACE ";" " " WARNING_CXX_FLAGS "${WARNING_CXX_FLAGS_LIST}") + +set(CMAKE_CXX_FLAGS "${WARNING_CXX_FLAGS} ${CMAKE_CXX_FLAGS}") +install(TARGETS + nimbro_primitive_fitter + DESTINATION lib/${PROJECT_NAME} +) + +install(TARGETS + approx_mvbb + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY launch models + DESTINATION share/${PROJECT_NAME} ) +ament_package() diff --git a/include/nimbro_primitive_fitter/inertia.h b/include/nimbro_primitive_fitter/inertia.h index 57f4933..7a6f584 100644 --- a/include/nimbro_primitive_fitter/inertia.h +++ b/include/nimbro_primitive_fitter/inertia.h @@ -17,12 +17,5 @@ class MyEdge : public vcg::Edge {}; class MyMesh : public vcg::tri::TriMesh< std::vector, std::vector , std::vector > {}; -std::string getFileExtension(std::string meshfile); +//std::string getFileExtension(std::string meshfile, const rclcpp::Node &nh); -/* - * Reads URDF and updates inertia values and center of mass for each link. - * 'urdf_filename': Path of the URDF - * 'visual': If true, visual meshes are used for calculation. - * If false, collision meshes are used. - */ -void update_inertia(std::string in_filename, std::string out_filename, bool visual, const ros::NodeHandle nh); diff --git a/include/nimbro_primitive_fitter/primitive_fitter.h b/include/nimbro_primitive_fitter/primitive_fitter.h index 5a0d7d1..1f5d98b 100644 --- a/include/nimbro_primitive_fitter/primitive_fitter.h +++ b/include/nimbro_primitive_fitter/primitive_fitter.h @@ -16,38 +16,69 @@ #include "../contrib/approx_mvbb/include/ApproxMVBB/ComputeApproxMVBB.hpp" -#include +#include #include #include +#include +#include + +#include + +#include +#include + + +#define NODE_NAME "nimbro_primitive_fitter" +#include + + using namespace roboptim; using namespace roboptim::capsule; class Urdf; +class PrimitiveFitterNode : public rclcpp::Node { + public: + std::shared_ptr shr_ptr_to_this_; + PrimitiveFitterNode(); + ~PrimitiveFitterNode(); + + void setSharedPointer(std::shared_ptr a_shr_ptr); + std::string getFileExtension(std::string meshfile); + void update_inertia(std::string in_filename, std::string out_filename, + bool visual); + void main(void); + +}; /** - Abstract ShapeFit class for fitting different shapes + Abstract ShapeFit class for fitting different shapes */ class ShapeFit { public: - /** - Writes the fitted model into URDF file. + ShapeFit(std::shared_ptr nh) + : nh_(nh) + {}; + + std::shared_ptr nh_; + /** + Writes the fitted model into URDF file. - @param lastLinkNode The XML node where collision-block shall be appended. - */ - virtual void writeUrdf(boost::shared_ptr lastLinkNode) = 0; + @param lastLinkNode The XML node where collision-block shall be appended. + */ + virtual void writeUrdf(std::shared_ptr lastLinkNode) = 0; - /** - Writes the fitted model into URDF file. Uses Xacro Macros. + /** + Writes the fitted model into URDF file. Uses Xacro Macros. - @param lastLinkNode The XML node where collision-block shall be appended. - */ - virtual void writeUrdfXacro(boost::shared_ptr lastLinkNode) = 0; + @param lastLinkNode The XML node where collision-block shall be appended. + */ + virtual void writeUrdfXacro(std::shared_ptr lastLinkNode) = 0; }; @@ -57,184 +88,187 @@ class ShapeFit * */ class CapsuleFit: public ShapeFit { -public: + public: - Eigen::Vector3d endpoint1() const { return m_endpoint1; }; + Eigen::Vector3d endpoint1() const { return m_endpoint1; }; - Eigen::Vector3d endpoint2() const { return m_endpoint2; }; + Eigen::Vector3d endpoint2() const { return m_endpoint2; }; - double radius() const { return m_radius; }; + double radius() const { return m_radius; }; - /* - * Capsule length. (Length of the cylinder between the two half-sphere endpoints. This does not include the sphere radii. - */ - double getLength(); + /* + * Capsule length. (Length of the cylinder between the two half-sphere endpoints. This does not include the sphere radii. + */ + double getLength(); - /* - * The capsule's mid-point. - */ - Eigen::Vector3d getMidpoint(); + /* + * The capsule's mid-point. + */ + Eigen::Vector3d getMidpoint(); - /* - * The capsule's orientation in space. - */ - Eigen::Vector3d getRotation(); + /* + * The capsule's orientation in space. + */ + Eigen::Vector3d getRotation(); - /* - * Compute the best fit capsule from the mesh vertices, mesh orientation and position as defined in the urdf file and also in the mesh's collada file / STL file - */ - CapsuleFit(std::vector points, - Eigen::Vector3d translation, - Eigen::Vector3d rotation, - Eigen::Vector3d scale, - Eigen::Matrix4d transformation); + /* + * Compute the best fit capsule from the mesh vertices, mesh orientation and position as defined in the urdf file and also in the mesh's collada file / STL file + */ + CapsuleFit(std::vector points, Eigen::Vector3d translation, + Eigen::Vector3d rotation, Eigen::Vector3d scale, + Eigen::Matrix4d transformation, + std::shared_ptr nh); - /** - Writes the fitted model into URDF file. + /** + Writes the fitted model into URDF file. - @param lastLinkNode The XML node where collision-block shall be appended. - Should be the last link in the XML. - */ - void writeUrdf(boost::shared_ptr lastLinkNode); + @param lastLinkNode The XML node where collision-block shall be appended. + Should be the last link in the XML. + */ + void writeUrdf(std::shared_ptr lastLinkNode); - /** - Writes the fitted model into URDF file. Uses Xacro Macros. + /** + Writes the fitted model into URDF file. Uses Xacro Macros. - @param lastLinkNode The XML node where collision-block shall be appended. - Should be the last link in the XML. - */ - void writeUrdfXacro(boost::shared_ptr< pugi::xml_node > lastLinkNode); + @param lastLinkNode The XML node where collision-block shall be appended. + Should be the last link in the XML. + */ + void writeUrdfXacro(std::shared_ptr< pugi::xml_node > lastLinkNode); - /** - Writes Xacro Macro for the used structure. Empty since Box natively supported + /** + Writes Xacro Macro for the used structure. Empty since Box natively supported - @param doc The XML document to which the macro shall be added. - */ - static void writeXacroMacro(const pugi::xml_document* doc); + @param doc The XML document to which the macro shall be added. + */ + static void writeXacroMacro(const pugi::xml_document* doc); -private: - void transformParameters(const Eigen::Matrix4d &transform, const Eigen::Matrix4d &localTransform); + private: + void transformParameters(const Eigen::Matrix4d &transform, const Eigen::Matrix4d &localTransform); - /* - * Capsule endpoint1 - */ - Eigen::Vector3d m_endpoint1; + /* + * Capsule endpoint1 + */ + Eigen::Vector3d m_endpoint1; - /* - * Capsule endpoint2 - */ - Eigen::Vector3d m_endpoint2; + /* + * Capsule endpoint2 + */ + Eigen::Vector3d m_endpoint2; - /* - * Capsule radius - */ - double m_radius; + /* + * Capsule radius + */ + double m_radius; - std::vector m_points; + std::vector m_points; - double m_length; + double m_length; - Eigen::Vector3d m_midpoint; - Eigen::Vector3d m_rot; + Eigen::Vector3d m_midpoint; + Eigen::Vector3d m_rot; }; //****************************************************************************************************************** /* -* Class for finding an Optimal Oriented Bounding Box (OOBB) -* for a given point set. -*/ + * Class for finding an Optimal Oriented Bounding Box (OOBB) + * for a given point set. + */ class BoxFit: public ShapeFit { -public: - BoxFit(std::vector points, - Eigen::Vector3d translation, - Eigen::Vector3d rotation, - Eigen::Vector3d scale, - Eigen::Matrix4d transformation, - const ros::NodeHandle nh - ); - - /** - Writes the fitted model into URDF file. - - @param lastLinkNode The XML node where collision-block shall be appended. - Should be the last link in the XML. - */ - void writeUrdf(boost::shared_ptr lastLinkNode); - - /** - Writes the fitted model into URDF file. Same as writeUrdf, since boxes natively supported. - - @param lastLinkNode The XML node where collision-block shall be appended. - Should be the last link in the XML. - */ - void writeUrdfXacro(boost::shared_ptr lastLinkNode); - - /** - Writes Xacro Macro for the used structure. Empty since Box natively supported - - @param doc The XML document to which the macro shall be added. - */ - static void writeXacroMacro(const pugi::xml_document* doc) { }; // Empty since natively supported - -private: - /** - Read the points and process them in a way suitable for the fitter. - - @param points The flat(!) vector containing all points in form (x1, y1, z1, x2, y2, z2,...) - */ - void readPoints(const std::vector points); - /** - Checks if point-vector is OK and can be read. - - @param points The flat(!) vector containing all points in form (x1, y1, z1, x2, y2, z2,...) - */ - bool checkPoints(const std::vector points); - - // Output of the Optimal Oriented Bounding Box fitting - ApproxMVBB::OOBB m_oobb; - - // Point set used by the fitter - read in via readPoints(std::vector) - Eigen::Matrix m_points; - - Eigen::Vector3d m_trans; - Eigen::Vector3d m_rot; - Eigen::Vector3d m_box_center; - Eigen::Vector3d m_box_size; + public: + BoxFit(std::vector points, + Eigen::Vector3d translation, + Eigen::Vector3d rotation, + Eigen::Vector3d scale, + Eigen::Matrix4d transformation, + std::shared_ptr nh + ); + + /** + Writes the fitted model into URDF file. + + @param lastLinkNode The XML node where collision-block shall be appended. + Should be the last link in the XML. + */ + void writeUrdf(std::shared_ptr lastLinkNode); + + /** + Writes the fitted model into URDF file. Same as writeUrdf, since boxes natively supported. + + @param lastLinkNode The XML node where collision-block shall be appended. + Should be the last link in the XML. + */ + void writeUrdfXacro(std::shared_ptr lastLinkNode); + + /** + Writes Xacro Macro for the used structure. Empty since Box natively supported + + @param doc The XML document to which the macro shall be added. + */ + static void writeXacroMacro(const pugi::xml_document* doc) { }; // Empty since natively supported + + private: + /** + Read the points and process them in a way suitable for the fitter. + + @param points The flat(!) vector containing all points in form (x1, y1, z1, x2, y2, z2,...) + */ + void readPoints(const std::vector points); + /** + Checks if point-vector is OK and can be read. + + @param points The flat(!) vector containing all points in form (x1, y1, z1, x2, y2, z2,...) + */ + bool checkPoints(const std::vector points); + + // Output of the Optimal Oriented Bounding Box fitting + ApproxMVBB::OOBB m_oobb; + + // Point set used by the fitter - read in via readPoints(std::vector) + Eigen::Matrix m_points; + + Eigen::Vector3d m_trans; + Eigen::Vector3d m_rot; + Eigen::Vector3d m_box_center; + Eigen::Vector3d m_box_size; - double m_roll; - double m_pitch; - double m_yaw; + double m_roll; + double m_pitch; + double m_yaw; + + std::shared_ptr nh_; }; //****************************************************************************************************************** /* -* Organizer of the fitting processes. -* Can fit different kind of shapes. -*/ + * Organizer of the fitting processes. + * Can fit different kind of shapes. + */ class VersatileFitter { -public: - VersatileFitter(); - - /** - * All currently supported shapes. - */ - enum Shape {Capsule, Box}; - - /** - Reads URDF and successively fits all links to the defined shape. - - @param urdf_filename The path of the URDF that shall be fitted. - @param shape Shape that shall be fitted to the meshes. - @see Shape - */ - void fit(std::string urdf_filename, std::string &output_filename, Shape shape, std::string shape_name, const ros::NodeHandle nh); -private: - std::string getFileExtension(std::string meshfile); - void loadMesh(std::string meshfile); + public: + VersatileFitter(std::shared_ptr nh); + + /** + * All currently supported shapes. + */ + enum Shape {Capsule, Box}; + + /** + Reads URDF and successively fits all links to the defined shape. + + @param urdf_filename The path of the URDF that shall be fitted. + @param shape Shape that shall be fitted to the meshes. + @see Shape + */ + void fit(std::string urdf_filename, std::string &output_filename, Shape shape, + std::string shape_name); + private: + std::string getFileExtension(std::string meshfile); + void loadMesh(std::string meshfile); + std::shared_ptr nh_; }; @@ -246,115 +280,117 @@ class VersatileFitter */ class Urdf { -public: - /* - * Get next mesh file defining collision properties. - */ - std::string getNextMesh(); - - /* - * Get next mesh file defining visual properties. - */ - std::string getNextVisualMesh(); - - /* - * True: File has more mesh files defining collision properties. - */ - bool hasMoreMesh(); - - /* - * True: File has more mesh files defining visual properties. - */ - bool hasMoreVisualMesh(); - - /* - * Constructor: Accept the urdf file path (mandatory). Add collision tags as xacro (optional) - */ - Urdf(const char* filepath, bool xacro); - - /* - * Replace mesh based collision file capsule based plain urdf collision tags. Adds three tags that approximate a capsule: , , . - */ - void replaceMesh(boost::shared_ptr shape); - - /* - * Replace mesh based collision with capsule based xacro collision tags. - */ - void replaceMeshWithXacro(boost::shared_ptr shape); - - /* - * Get the original mesh rotation as a 3D Eulerian rotation vector. - */ - Eigen::Vector3d getMeshRotation() const; - - /* - * Get the original mesh translation as a 3D vector. - */ - Eigen::Vector3d getMeshTranslation() const; - - /* - * Get the original mesh scale as a 3D vector. - */ - Eigen::Vector3d getMeshScale() const; - - /* - * Save the updated urdf file at the supplied location. - */ - - bool save(const char* path); - - /* - * Returns mass of current mesh. - * If visual = true, visual mesh will be used, otherwise collision mesh is used. - * Mesh must be previously extracted using getNextMesh() or getNextVisualMesh(). - */ - float getMass(bool visual); - - /* - * Replaces Inertia for current mesh. - * If visual = true, visual mesh will be used, otherwise collision mesh is used. - * Mesh must be previously extracted using getNextMesh() or getNextVisualMesh(), accordingly. - */ - void setInertia(vcg::Matrix33f Inertia, bool visual); - - /* - * Replaces center of mass for current mesh. - * If visual = true, visual mesh will be used, otherwise collision mesh is used. - * Mesh must be previously extracted using getNextMesh() or getNextVisualMesh(), accordingly. - */ - void setCenterOfMass(vcg::Point3f CenterOfMass, bool visual); - - /* - * Whether to add new collision tags as xacros. - */ - bool isXacro(); - - bool loaded() { return m_URDFLoaded; }; - -protected: - void addXacroDef(); - - const char* m_filepath; - const std::string m_collision_path = "/robot/link[*]/collision/geometry/mesh"; - const std::string m_visual_path = "/robot/link[*]/visual/geometry/mesh"; - - const pugi::xpath_node* m_it; - const pugi::xpath_node* m_visual_it; - pugi::xpath_node_set m_nodes; - pugi::xpath_node_set m_visual_nodes; - - pugi::xml_node m_lastLinkNode; - pugi::xml_node m_lastCollisionNode; - pugi::xml_node m_lastMeshNode; - - pugi::xml_node m_lastVisualLinkNode; - pugi::xml_node m_lastVisualNode; - pugi::xml_node m_lastVisualMeshNode; - - pugi::xml_document m_doc; - - bool m_xacro; - bool m_URDFLoaded = false; + public: + /* + * Get next mesh file defining collision properties. + */ + std::string getNextMesh(); + + /* + * Get next mesh file defining visual properties. + */ + std::string getNextVisualMesh(); + + /* + * True: File has more mesh files defining collision properties. + */ + bool hasMoreMesh(); + + /* + * True: File has more mesh files defining visual properties. + */ + bool hasMoreVisualMesh(); + + /* + * Constructor: Accept the urdf file path (mandatory). Add collision tags as xacro (optional) + */ + Urdf(const char* filepath, bool xacro, std::shared_ptr nh); + + /* + * Replace mesh based collision file capsule based plain urdf collision tags. Adds three tags that approximate a capsule: , , . + */ + void replaceMesh(std::shared_ptr shape); + + /* + * Replace mesh based collision with capsule based xacro collision tags. + */ + void replaceMeshWithXacro(std::shared_ptr shape); + + /* + * Get the original mesh rotation as a 3D Eulerian rotation vector. + */ + Eigen::Vector3d getMeshRotation() const; + + /* + * Get the original mesh translation as a 3D vector. + */ + Eigen::Vector3d getMeshTranslation() const; + + /* + * Get the original mesh scale as a 3D vector. + */ + Eigen::Vector3d getMeshScale() const; + + /* + * Save the updated urdf file at the supplied location. + */ + + bool save(const char* path); + + /* + * Returns mass of current mesh. + * If visual = true, visual mesh will be used, otherwise collision mesh is used. + * Mesh must be previously extracted using getNextMesh() or getNextVisualMesh(). + */ + float getMass(bool visual); + + /* + * Replaces Inertia for current mesh. + * If visual = true, visual mesh will be used, otherwise collision mesh is used. + * Mesh must be previously extracted using getNextMesh() or getNextVisualMesh(), accordingly. + */ + void setInertia(vcg::Matrix33f Inertia, bool visual); + + /* + * Replaces center of mass for current mesh. + * If visual = true, visual mesh will be used, otherwise collision mesh is used. + * Mesh must be previously extracted using getNextMesh() or getNextVisualMesh(), accordingly. + */ + void setCenterOfMass(vcg::Point3f CenterOfMass, bool visual); + + /* + * Whether to add new collision tags as xacros. + */ + bool isXacro(); + + bool loaded() { return m_URDFLoaded; }; + + protected: + void addXacroDef(); + + const char* m_filepath; + const std::string m_collision_path = "/robot/link[*]/collision/geometry/mesh"; + const std::string m_visual_path = "/robot/link[*]/visual/geometry/mesh"; + + const pugi::xpath_node* m_it; + const pugi::xpath_node* m_visual_it; + pugi::xpath_node_set m_nodes; + pugi::xpath_node_set m_visual_nodes; + + pugi::xml_node m_lastLinkNode; + pugi::xml_node m_lastCollisionNode; + pugi::xml_node m_lastMeshNode; + + pugi::xml_node m_lastVisualLinkNode; + pugi::xml_node m_lastVisualNode; + pugi::xml_node m_lastVisualMeshNode; + + pugi::xml_document m_doc; + + bool m_xacro; + bool m_URDFLoaded = false; + + std::shared_ptr nh_; }; @@ -365,19 +401,24 @@ class Urdf class MeshFile { public: - /** - Read the points from the mesh file and return them in a flat vector of type double. - - @returns points The flat(!) vector containing all points in form (x1, y1, z1, x2, y2, z2,...) - */ - virtual std::vector getPoints() = 0; - - /** - Return the transformation of the mesh like it is neccessary for e.g. collada files. - - @returns matrix A transformation matrix in homogenous coordinates - */ - virtual Eigen::Matrix4d getTransformationMatrix() = 0; + + MeshFile(std::shared_ptr nh) : nh_(nh){}; + + std::shared_ptr nh_; + + /** + Read the points from the mesh file and return them in a flat vector of type double. + + @returns points The flat(!) vector containing all points in form (x1, y1, z1, x2, y2, z2,...) + */ + virtual std::vector getPoints() = 0; + + /** + Return the transformation of the mesh like it is neccessary for e.g. collada files. + + @returns matrix A transformation matrix in homogenous coordinates + */ + virtual Eigen::Matrix4d getTransformationMatrix() = 0; }; @@ -387,55 +428,57 @@ class MeshFile */ class Dae: public MeshFile { -public: - /* - * Open file handle to supplied dae file. - */ - Dae(std::string filename); - - /* - * Get mesh vertices as a list. - */ - std::vector getPoints(); - - /* - * Get the mesh model's orientation in space. - */ - Eigen::Matrix4d getTransformationMatrix(); - -private: - std::string m_filename; - pugi::xml_document m_doc; + public: + /* + * Open file handle to supplied dae file. + */ + Dae(std::string filename, + std::shared_ptr nh); + + /* + * Get mesh vertices as a list. + */ + std::vector getPoints(); + + /* + * Get the mesh model's orientation in space. + */ + Eigen::Matrix4d getTransformationMatrix(); + + private: + std::string m_filename; + pugi::xml_document m_doc; }; //****************************************************************************************************************** /* -*.stl file operations. Read vertices from binary STL files -*/ + *.stl file operations. Read vertices from binary STL files + */ class Stl: public MeshFile { -public: - /* - * Open file handle to supplied STL file. - */ - Stl(std::string filename); - - /* - * Get mesh vertices as a list. - */ - std::vector getPoints(); - - /* - * Get the mesh model's orientation in space. (here just identity) - */ - Eigen::Matrix4d getTransformationMatrix() { return Eigen::Matrix4d::Identity(); }; - -private: - float parseFloat(); - void dumpPoint(); + public: + /* + * Open file handle to supplied STL file. + */ + Stl(std::string filename, + std::shared_ptr nh); + + /* + * Get mesh vertices as a list. + */ + std::vector getPoints(); + + /* + * Get the mesh model's orientation in space. (here just identity) + */ + Eigen::Matrix4d getTransformationMatrix() { return Eigen::Matrix4d::Identity(); }; + + private: + float parseFloat(); + void dumpPoint(); - std::ifstream m_filestream; + std::ifstream m_filestream; - unsigned int m_num_faces; + unsigned int m_num_faces; }; diff --git a/include/nimbro_primitive_fitter/util.h b/include/nimbro_primitive_fitter/util.h index e6ef685..513e788 100644 --- a/include/nimbro_primitive_fitter/util.h +++ b/include/nimbro_primitive_fitter/util.h @@ -4,6 +4,7 @@ #pragma once +#include #include diff --git a/launch/capsule_ur10.launch.py b/launch/capsule_ur10.launch.py new file mode 100644 index 0000000..b75bcd1 --- /dev/null +++ b/launch/capsule_ur10.launch.py @@ -0,0 +1,63 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, TextSubstitution +import os +from ament_index_python.packages import get_package_share_directory + + + +def generate_launch_description(): + + # Build parameters + package_name ='nimbro_primitive_fitter' + # may raise PackageNotFoundError + package_share_directory = get_package_share_directory(package_name) + print('package_share_directory: ' + package_share_directory) + urdf_filename = package_share_directory + \ + '/models/ur10_stl.urdf' + print('urdf_filename : ' + urdf_filename) + output_filename = package_share_directory + \ + '/models/ur10_stl_fitcapsule.urdf.xacro' + print('output_filename : ' + output_filename) + + urdf_filename_launch_arg = DeclareLaunchArgument( + 'urdf_filename', default_value=TextSubstitution(text=urdf_filename) + ) + output_filename_launch_arg = DeclareLaunchArgument( + 'output_filename', default_value=TextSubstitution(text=output_filename) + ) + fit_shape_launch_arg = DeclareLaunchArgument( + 'fit_shape', default_value=TextSubstitution(text='capsule') + ) + use_fitter_launch_arg = DeclareLaunchArgument( + 'use_fitter', default_value=TextSubstitution(text='true') + ) + use_inertia_launch_arg = DeclareLaunchArgument( + 'use_inertia', default_value=TextSubstitution(text='true') + ) + mesh_type_launch_arg = DeclareLaunchArgument( + 'mesh_type', default_value=TextSubstitution(text='collision') + ) + + return LaunchDescription([ + urdf_filename_launch_arg, + output_filename_launch_arg, + fit_shape_launch_arg, + use_fitter_launch_arg, + use_inertia_launch_arg, + mesh_type_launch_arg, + Node( + package='nimbro_primitive_fitter', + executable='nimbro_primitive_fitter', + name='nimbro_primitive_fitter', + parameters=[{ + 'urdf_filename': LaunchConfiguration('urdf_filename'), + 'output_filename': LaunchConfiguration('output_filename'), + 'fit_shape': LaunchConfiguration('fit_shape'), + 'use_fitter': LaunchConfiguration('use_fitter'), + 'use_inertia': LaunchConfiguration('use_inertia'), + 'mesh_type': LaunchConfiguration('mesh_type'), + }] + ), + ]) diff --git a/package.xml b/package.xml index 71ca446..aeea4a9 100644 --- a/package.xml +++ b/package.xml @@ -1,5 +1,5 @@ - + nimbro_primitive_fitter 0.1.0 This package fits primitive shapes to complex meshes to reduce computational complexity in simulation and planning tasks. @@ -8,15 +8,19 @@ Diego Rodriguez BSD + + ament_cmake + + libpugixml-dev + rclcpp + roboptim-capsule - cmake_modules - roslib - roslib - rosparam - rosparam - rospack - rospack - roscpp - roscpp - catkin + libpugixml-dev + rclcpp + roboptim-capsule + + + ament_cmake + + diff --git a/src/inertia.cpp b/src/inertia.cpp index 76265a5..9153256 100644 --- a/src/inertia.cpp +++ b/src/inertia.cpp @@ -4,128 +4,12 @@ #include #include #include -#include -#include -#include + #include #include -std::string getFileExtension(std::string meshfile) -{ - std::string file_extension(""); - - // Get file extension - std::regex file_regex("([^\\\\.]+)(\\.)([^\\\\.]+)"); - std::smatch r_match; - - if (std::regex_search(meshfile, r_match, file_regex)) - { - file_extension = r_match[3]; - ROS_DEBUG_STREAM( "Detected file extension ." << r_match[3] ); - } - else - { - ROS_INFO( "NO FILE EXTENSION FOUND!" ); - } - - return file_extension; -} - - -void update_inertia(std::string in_filename, std::string out_filename, bool visual, const ros::NodeHandle nh) -{ - Urdf urdf(in_filename.c_str(), false); - - // Return if the urdf was not loaded - if ( !urdf.loaded() ) - { - ROS_ERROR( "URDF could not be loaded." ); - return; - } - - bool hasMoreMesh; - - if (visual) - { - hasMoreMesh = urdf.hasMoreVisualMesh(); - } - else - { - hasMoreMesh = urdf.hasMoreMesh(); - } - - while (hasMoreMesh) - { - std::string meshfile; - - if (visual) - { - meshfile = urdf.getNextVisualMesh(); - hasMoreMesh = urdf.hasMoreVisualMesh(); - } - else - { - meshfile = urdf.getNextMesh(); - hasMoreMesh = urdf.hasMoreMesh(); - } - - std::string file_extension = getFileExtension(meshfile); - - if (file_extension == "stl" || file_extension == "STL") - { - ROS_DEBUG( "Handling STL file ..." ); - ROS_DEBUG_STREAM( "path: " << meshfile ); - - // get global meshfile path - url::Url filepath(meshfile); - std::string pkgName(filepath.getPackageName()); - std::string pkgPath(ros::package::getPath(pkgName)); - std::string path(filepath.getRelativePath()); - std::string global_meshfile((pkgPath + path).c_str()); - ROS_INFO_STREAM( "global path: " << global_meshfile ); - - // open STL file - MyMesh mesh; - int mask = 0; - - if(vcg::tri::io::ImporterSTL::Open(mesh, global_meshfile.c_str(), mask)) - { - ROS_INFO_STREAM( "Error reading file " << global_meshfile ); - return; - } - - // read mass from file - float mass = urdf.getMass(visual); - - // compute inertia and center of mass - vcg::Matrix33f Inertia; - vcg::tri::Inertia I(mesh); - I.InertiaTensor(Inertia); - vcg::Point3f CenterOfMass = I.CenterOfMass(); - - // scale inertia according to mass - float volume = I.Mass(); - Inertia /= volume; - Inertia *= mass; - - // set new values - urdf.setCenterOfMass(CenterOfMass, visual); - urdf.setInertia(Inertia, visual); - } - else - { - ROS_ERROR( "UNKNOWN FILE EXTENSION! (only .stl supported)" ); - return; - } - } - if (urdf.save(out_filename.c_str())) - { - ROS_INFO( "Inertia updated successfully!" ); - } - return; -} diff --git a/src/main.cpp b/src/main.cpp index b794251..b83e8df 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,98 +1,20 @@ #include - -#include -#include +#include #include #include -#define NODE_NAME "nimbro_primitive_fitter" + int main(int argc, char **argv) { - std::string nodeName = NODE_NAME; - ros::init(argc, argv, nodeName.c_str()); - - ros::NodeHandle pvtNh("~" + nodeName); // private node handle - - ROS_INFO(" ********** NimbRo Primitive Fitter started ********** "); - - std::string in_filename; - pvtNh.getParam("/" + nodeName + "/urdf_filename", in_filename); - - std::string out_filename(""); - pvtNh.getParam("/" + nodeName + "/output_filename", out_filename); - - if(out_filename.empty()) - { - size_t lastindex = in_filename.find_last_of("."); - out_filename = in_filename.substr(0, lastindex) + "_optimized.urdf.xacro"; - } - - bool fitter = true; - pvtNh.getParam("/" + nodeName + "/use_fitter", fitter); - - bool inertia = false; - pvtNh.getParam("/" + nodeName + "/use_inertia", inertia); - - std::string capsule_filename(""); - - if (inertia) - { - ROS_INFO("Inertia calculation started."); - - std::string mesh_type = "visual"; - bool visual = true; - pvtNh.getParam("/" + nodeName + "/mesh_type", mesh_type); - - if (mesh_type == "visual") - { - visual = true; - ROS_INFO("Using visual meshes."); - } - else if (mesh_type == "collision") - { - visual = false; - ROS_INFO("Using collision meshes."); - } - else - { - ROS_WARN("PLEASE SET ROSPARAM /nimbro_primitive_fitter/mesh_type TO EITHER visual OR collsion -> Using 'visual'."); - visual = true; - } - - update_inertia(in_filename, out_filename, visual, pvtNh); - in_filename = out_filename; - } - - if (fitter) - { - std::string shape = "box"; - pvtNh.getParam("/" + nodeName + "/fit_shape", shape); - VersatileFitter::Shape vf_shape; - - if (shape == "box" || shape == "Box") - { - ROS_INFO("Will try to fit URDF to Box."); - vf_shape = VersatileFitter::Shape::Box; - } - else if (shape == "capsule" || shape == "Capsule") - { - ROS_INFO("Will try to fit URDF to Capsule."); - vf_shape = VersatileFitter::Shape::Capsule; - } - else - { - ROS_ERROR("PLEASE SET ROSPARAM /nimbro_primitive_fitter/fit_shape TO EITHER box OR capsule"); - ROS_INFO("Will try to fit URDF to Box."); - vf_shape = VersatileFitter::Shape::Box; - } - VersatileFitter vf; - vf.fit(in_filename, out_filename, vf_shape, shape, pvtNh); - } - - ros::shutdown(); - return 0; + rclcpp::init(argc, argv); + std::shared_ptr aPrimitiveFitterNode_shr_ptr = + std::make_shared(); + aPrimitiveFitterNode_shr_ptr->setSharedPointer(aPrimitiveFitterNode_shr_ptr); + aPrimitiveFitterNode_shr_ptr->main(); + rclcpp::spin(aPrimitiveFitterNode_shr_ptr); + rclcpp::shutdown(); } diff --git a/src/primitive_fitter.cpp b/src/primitive_fitter.cpp index e1be60e..23e8af2 100644 --- a/src/primitive_fitter.cpp +++ b/src/primitive_fitter.cpp @@ -26,13 +26,14 @@ #include #include -#include -#include +#include +#include #include #include "../contrib/approx_mvbb/include/ApproxMVBB/ComputeApproxMVBB.hpp" +#include "nimbro_primitive_fitter/inertia.h" #include #define NODE_NAME "nimbro_primitive_fitter" @@ -42,163 +43,176 @@ // Shape fitter for boxes -BoxFit::BoxFit(std::vector points, - Eigen::Vector3d translation, - Eigen::Vector3d rotation, - Eigen::Vector3d scale, +BoxFit::BoxFit(std::vector points, Eigen::Vector3d translation, + Eigen::Vector3d rotation, Eigen::Vector3d scale, Eigen::Matrix4d localTransformation, - const ros::NodeHandle nh - ) -{ - // Transform points to 3xLEN matrix - checkPoints(points); - readPoints(points); - - // Call MVBB routine - ROS_INFO("Computing MVBB... "); - - bool pointSamples_is_relative; - double epsilon, pointSamples, gridSize, mvbbDiamOptLoops, mvbbGridSearchOptLoops; - - std::string nodeName = NODE_NAME; - nh.getParam("/" + nodeName + "/boxfit/pointSamples_is_relative", pointSamples_is_relative); - nh.getParam("/" + nodeName + "/boxfit/epsilon", epsilon); - nh.getParam("/" + nodeName + "/boxfit/pointSamples", pointSamples); - nh.getParam("/" + nodeName + "/boxfit/gridSize", gridSize); - nh.getParam("/" + nodeName + "/boxfit/mvbbDiamOptLoops", mvbbDiamOptLoops); - nh.getParam("/" + nodeName + "/boxfit/mvbbGridSearchOptLoops", mvbbGridSearchOptLoops); - - int num_samples; - - if(pointSamples_is_relative) - { - num_samples = std::floor(pointSamples * m_points.cols()); - } - else - { - num_samples = std::floor(pointSamples); - } - - ROS_INFO_STREAM("using " << num_samples << " samples(out of " << m_points.cols() << ")..."); - - m_oobb = ApproxMVBB::approximateMVBB(m_points, epsilon, num_samples, gridSize, mvbbDiamOptLoops, mvbbGridSearchOptLoops); - - ApproxMVBB::Matrix33 A_KI = m_oobb.m_q_KI.matrix().transpose(); - auto size = m_points.cols(); - - for( unsigned int i = 0; i < size; ++i ) - { - m_oobb.unite(A_KI * m_points.col(i)); - } - - // Extract results - m_box_center = (m_oobb.m_q_KI * m_oobb.center()).eval(); - m_box_size = m_oobb.extent(); - - // Check if an error occurred in the process - if ( std::isinf(m_box_size[0]*m_box_size[1]*m_box_size[2]) ) - { - ROS_ERROR("Faulty box-size vector! (Infinity detected)"); - ROS_ERROR("Most likely this is caused by a bad mesh. Try reducing the detail of the mesh."); - - m_box_size[0] = 0; - m_box_size[1] = 0; - m_box_size[2] = 0; - } - - // Calculate RPY from the rotation matrix - Eigen::Vector3d rpy; - geometry::matrix2rpy(m_oobb.m_q_KI.matrix(), rpy); - m_roll = rpy[2]; - m_pitch = rpy[1]; - m_yaw = rpy[0]; - - ROS_INFO_STREAM("Box size: " << m_box_size ); - ROS_INFO_STREAM("Box center: " << m_box_center ); - ROS_INFO_STREAM("r: p: y:" << m_roll << " " << m_pitch << " " << m_yaw ) ; + std::shared_ptr nh + ) : ShapeFit(nh) +{ + // Transform points to 3xLEN matrix + checkPoints(points); + readPoints(points); + + // Call MVBB routine + RCLCPP_INFO(nh->get_logger(), "Computing MVBB... "); + + bool pointSamples_is_relative; + double epsilon, pointSamples, gridSize, mvbbDiamOptLoops, mvbbGridSearchOptLoops; + + std::string nodeName = NODE_NAME; + + pointSamples = nh_->get_parameter("/" + nodeName + + "/boxfit/pointSamples_is_relative").as_double(); + epsilon = nh_->get_parameter("/" + nodeName + + "/boxfit/epsilon").as_double(); + pointSamples = nh_->get_parameter("/" + nodeName + + "/boxfit/pointSamples").as_double(); + gridSize = nh_->get_parameter("/" + nodeName + + "/boxfit/gridSize").as_double(); + mvbbDiamOptLoops = nh_->get_parameter("/" + nodeName + + "/boxfit/mvbbDiamOptLoops").as_double(); + mvbbGridSearchOptLoops = nh_->get_parameter("/" + nodeName + + "/boxfit/mvbbGridSearchOptLoops").as_double(); + + int num_samples; + + if(pointSamples_is_relative) + { + num_samples = std::floor(pointSamples * m_points.cols()); + } + else + { + num_samples = std::floor(pointSamples); + } + + RCLCPP_INFO_STREAM(nh_->get_logger(), + "using " << num_samples + << " samples(out of " + << m_points.cols() << ")..."); + + m_oobb = ApproxMVBB::approximateMVBB(m_points, epsilon, num_samples, gridSize, + mvbbDiamOptLoops, + mvbbGridSearchOptLoops); + + ApproxMVBB::Matrix33 A_KI = m_oobb.m_q_KI.matrix().transpose(); + auto size = m_points.cols(); + + for( unsigned int i = 0; i < size; ++i ) + { + m_oobb.unite(A_KI * m_points.col(i)); + } + + // Extract results + m_box_center = (m_oobb.m_q_KI * m_oobb.center()).eval(); + m_box_size = m_oobb.extent(); + + // Check if an error occurred in the process + if ( std::isinf(m_box_size[0]*m_box_size[1]*m_box_size[2]) ) + { + RCLCPP_ERROR(nh_->get_logger(),"Faulty box-size vector! (Infinity detected)"); + RCLCPP_ERROR(nh_->get_logger(), + "Most likely this is caused by a bad mesh. Try reducing the detail of the mesh."); + + m_box_size[0] = 0; + m_box_size[1] = 0; + m_box_size[2] = 0; + } + + // Calculate RPY from the rotation matrix + Eigen::Vector3d rpy; + geometry::matrix2rpy(m_oobb.m_q_KI.matrix(), rpy); + m_roll = rpy[2]; + m_pitch = rpy[1]; + m_yaw = rpy[0]; + + RCLCPP_INFO_STREAM(nh_->get_logger(),"Box size: " << m_box_size ); + RCLCPP_INFO_STREAM(nh_->get_logger(),"Box center: " << m_box_center ); + RCLCPP_INFO_STREAM(nh_->get_logger(), "r: p: y:" << m_roll << " " + << m_pitch + << " " << m_yaw ) ; } // Check if the points are OK to be read in bool BoxFit::checkPoints(const std::vector points) { - // Test if number of coordinates is multiple of 3 - if (points.size() % 3 != 0) - { - throw std::invalid_argument( - "Error: points should be an array of 3D points, e.g. x0 y0 z0 x1 y1 z1 etc."); - } - - return true; + // Test if number of coordinates is multiple of 3 + if (points.size() % 3 != 0) + { + throw std::invalid_argument( + "Error: points should be an array of 3D points, e.g. x0 y0 z0 x1 y1 z1 etc."); + } + + return true; } // Read the points from the flat vector void BoxFit::readPoints(const std::vector points) { - int num_coords = points.size(); + int num_coords = points.size(); + + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"File contains: " << num_coords / 3 << " points"); - ROS_DEBUG_STREAM("File contains: " << num_coords / 3 << " points"); - - // Parse points - boost::unordered_set> u_pointset; - u_pointset.clear(); + // Parse points + boost::unordered_set> u_pointset; + u_pointset.clear(); - for (int i = 0; i < num_coords; i += 3) - { - std::vector f; - f.push_back(points.at(i)); - f.push_back(points.at(i + 1)); - f.push_back(points.at(i + 2)); - u_pointset.insert(f); - } + for (int i = 0; i < num_coords; i += 3) + { + std::vector f; + f.push_back(points.at(i)); + f.push_back(points.at(i + 1)); + f.push_back(points.at(i + 2)); + u_pointset.insert(f); + } - ROS_DEBUG_STREAM("(" << u_pointset.size() << " unique) "); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "(" << u_pointset.size() << " unique) "); - boost::unordered_set>::iterator u_iter; + boost::unordered_set>::iterator u_iter; - for (u_iter = u_pointset.begin(); u_iter != u_pointset.end(); u_iter++) - { - m_points.conservativeResize(m_points.rows(), m_points.cols() + 1); - Eigen::Vector3d vec((*u_iter)[0], (*u_iter)[1], (*u_iter)[2]); - m_points.col(m_points.cols() - 1) = vec; - } + for (u_iter = u_pointset.begin(); u_iter != u_pointset.end(); u_iter++) + { + m_points.conservativeResize(m_points.rows(), m_points.cols() + 1); + Eigen::Vector3d vec((*u_iter)[0], (*u_iter)[1], (*u_iter)[2]); + m_points.col(m_points.cols() - 1) = vec; + } } // Writes the collision model to the URDF -void BoxFit::writeUrdf(boost::shared_ptr lastLinkNode) +void BoxFit::writeUrdf(std::shared_ptr lastLinkNode) { - // Box - pugi::xml_node collision = lastLinkNode->append_child("collision"); - pugi::xml_node origin = collision.append_child("origin"); - - // Actual collision model - pugi::xml_node geom = collision.append_child("geometry"); - pugi::xml_node box = geom.append_child("box"); - pugi::xml_attribute dimensions = box.append_attribute("size"); - - // Box dimension - std::stringstream dimSS; - dimSS << m_box_size[0] << " " << m_box_size[1] << " " << m_box_size[2]; - dimensions.set_value(dimSS.str().c_str()); - - // Translation ... - std::stringstream xyzSS; - xyzSS << (m_box_center[0]) << " " << (m_box_center[1]) << " " << (m_box_center[2]); - origin.append_attribute("xyz").set_value(xyzSS.str().c_str()); - - // ... and rotation of box - std::stringstream rpySS; - rpySS << m_roll << " " << m_pitch << " " << m_yaw; - origin.append_attribute("rpy").set_value(rpySS.str().c_str()); + // Box + pugi::xml_node collision = lastLinkNode->append_child("collision"); + pugi::xml_node origin = collision.append_child("origin"); + + // Actual collision model + pugi::xml_node geom = collision.append_child("geometry"); + pugi::xml_node box = geom.append_child("box"); + pugi::xml_attribute dimensions = box.append_attribute("size"); + + // Box dimension + std::stringstream dimSS; + dimSS << m_box_size[0] << " " << m_box_size[1] << " " << m_box_size[2]; + dimensions.set_value(dimSS.str().c_str()); + + // Translation ... + std::stringstream xyzSS; + xyzSS << (m_box_center[0]) << " " << (m_box_center[1]) << " " << (m_box_center[2]); + origin.append_attribute("xyz").set_value(xyzSS.str().c_str()); + + // ... and rotation of box + std::stringstream rpySS; + rpySS << m_roll << " " << m_pitch << " " << m_yaw; + origin.append_attribute("rpy").set_value(rpySS.str().c_str()); } -void BoxFit::writeUrdfXacro(boost::shared_ptr lastLinkNode) +void BoxFit::writeUrdfXacro(std::shared_ptr lastLinkNode) { - // Same deal, since natively supported structure - writeUrdf(lastLinkNode); + // Same deal, since natively supported structure + writeUrdf(lastLinkNode); } @@ -206,792 +220,1048 @@ void BoxFit::writeUrdfXacro(boost::shared_ptr lastLinkNode) double CapsuleFit::getLength() { - return m_length; + return m_length; } Eigen::Vector3d CapsuleFit::getMidpoint() { - return m_midpoint; + return m_midpoint; } Eigen::Vector3d CapsuleFit::getRotation() { - return m_rot; + return m_rot; } // Shape fitter for capsules -CapsuleFit::CapsuleFit(std::vector points, - Eigen::Vector3d translation, - Eigen::Vector3d rotation, - Eigen::Vector3d scale, - Eigen::Matrix4d localTransformation) +CapsuleFit::CapsuleFit(std::vector points, Eigen::Vector3d translation, + Eigen::Vector3d rotation, Eigen::Vector3d scale, + Eigen::Matrix4d localTransformation, + std::shared_ptr nh) : ShapeFit(nh) { - m_points = points; + m_points = points; - /* Get the capsule endpoints and radius using roboptim capsule*/ - assert(m_points.size() > 0 && "Cannot compute capsule for point set."); + /* Get the capsule endpoints and radius using roboptim capsule*/ + assert(m_points.size() > 0 && "Cannot compute capsule for point set."); - std::string solver = "ipopt"; + std::string solver = "ipopt"; - if (m_points.size() % 3 != 0) - { - throw std::invalid_argument( - "Error: points should be an array of 3D points, e.g. x0 y0 z0 x1 y1 z1 etc."); - } + if (m_points.size() % 3 != 0) + { + throw std::invalid_argument( + "Error: points should be an array of 3D points, e.g. x0 y0 z0 x1 y1 z1 etc."); + } - // Load polyhedron - polyhedron_t polyhedron; + // Load polyhedron + polyhedron_t polyhedron; - for (size_t i = 0; i < m_points.size(); i += 3) - { - point_t p(m_points[i], m_points[i + 1], m_points[i + 2]); - polyhedron.push_back(p); - } + for (size_t i = 0; i < m_points.size(); i += 3) + { + point_t p(m_points[i], m_points[i + 1], m_points[i + 2]); + polyhedron.push_back(p); + } - // Fitter expects a vector of polyhedrons - polyhedrons_t polyhedrons; - polyhedrons.push_back(polyhedron); + // Fitter expects a vector of polyhedrons + polyhedrons_t polyhedrons; + polyhedrons.push_back(polyhedron); - // Create fitter - Fitter fitter(polyhedrons, solver); + // Create fitter + Fitter fitter(polyhedrons, solver); - fitter.logDirectory() = "log"; + fitter.logDirectory() = "log"; - // Compute initial guess - point_t P0; - point_t P1; - value_type r = 0.; - argument_t initParam(7); + // Compute initial guess + point_t P0; + point_t P1; + value_type r = 0.; + argument_t initParam(7); - polyhedrons_t convexPolyhedrons; - computeConvexPolyhedron(polyhedrons, convexPolyhedrons); - computeBoundingCapsulePolyhedron(convexPolyhedrons, P0, P1, r); - convertCapsuleToSolverParam(initParam, P0, P1, r); + polyhedrons_t convexPolyhedrons; + computeConvexPolyhedron(polyhedrons, convexPolyhedrons); + computeBoundingCapsulePolyhedron(convexPolyhedrons, P0, P1, r); + convertCapsuleToSolverParam(initParam, P0, P1, r); - // Compute optimal capsule - fitter.computeBestFitCapsule(initParam); - ROS_INFO_STREAM("best capsule computed" ); + // Compute optimal capsule + fitter.computeBestFitCapsule(initParam); + RCLCPP_INFO_STREAM(nh_->get_logger(), "best capsule computed" ); - // assign values to e1,e2 and radius - m_endpoint1 = Eigen::Vector3d(fitter.solutionParam()(0, 0), - fitter.solutionParam()(1, 0), fitter.solutionParam()(2, 0)); - m_endpoint2 = Eigen::Vector3d(fitter.solutionParam()(3, 0), - fitter.solutionParam()(4, 0), fitter.solutionParam()(5, 0)); - m_radius = fitter.solutionParam()(6, 0); + // assign values to e1,e2 and radius + m_endpoint1 = Eigen::Vector3d(fitter.solutionParam()(0, 0), + fitter.solutionParam()(1, 0), fitter.solutionParam()(2, 0)); + m_endpoint2 = Eigen::Vector3d(fitter.solutionParam()(3, 0), + fitter.solutionParam()(4, 0), fitter.solutionParam()(5, 0)); + m_radius = fitter.solutionParam()(6, 0); - Eigen::Matrix4d transform = geometry::create_transformation_matrix(rotation, translation, scale); + Eigen::Matrix4d transform = geometry::create_transformation_matrix(rotation, translation, scale); - transformParameters(transform, localTransformation); + transformParameters(transform, localTransformation); } /* Transform capsule parameters based on urdf and collada files' original orientation for the mesh*/ void CapsuleFit::transformParameters(const Eigen::Matrix4d &transform, const Eigen::Matrix4d &localTransform) { - Eigen::Vector4d homoe1 = m_endpoint1.homogeneous(); - Eigen::Vector4d homoe2 = m_endpoint2.homogeneous(); + Eigen::Vector4d homoe1 = m_endpoint1.homogeneous(); + Eigen::Vector4d homoe2 = m_endpoint2.homogeneous(); - homoe1 = transform * localTransform * homoe1; - homoe2 = transform * localTransform * homoe2; - m_radius = m_radius * localTransform(0, 0); + homoe1 = transform * localTransform * homoe1; + homoe2 = transform * localTransform * homoe2; + m_radius = m_radius * localTransform(0, 0); - m_endpoint1 = homoe1.hnormalized(); - m_endpoint2 = homoe2.hnormalized(); + m_endpoint1 = homoe1.hnormalized(); + m_endpoint2 = homoe2.hnormalized(); - Eigen::Vector3d capAxis = m_endpoint2 - m_endpoint1; + Eigen::Vector3d capAxis = m_endpoint2 - m_endpoint1; - m_length = (m_endpoint1 - m_endpoint2).norm(); - m_midpoint = (m_endpoint1 + m_endpoint2) * 0.5; + m_length = (m_endpoint1 - m_endpoint2).norm(); + m_midpoint = (m_endpoint1 + m_endpoint2) * 0.5; - m_rot = geometry::rpyFromaxisAngles(capAxis); + m_rot = geometry::rpyFromaxisAngles(capAxis); } -void CapsuleFit::writeUrdf(boost::shared_ptr lastLinkNode) +void CapsuleFit::writeUrdf(std::shared_ptr lastLinkNode) { - // The first sphere - pugi::xml_node s1Collision = lastLinkNode->append_child("collision"); - pugi::xml_node s1Origin = s1Collision.append_child("origin"); - - std::stringstream s1rpySS; - s1rpySS << m_rot(0) << " " << m_rot(1) << " " << m_rot(2); - s1Origin.append_attribute("rpy").set_value(s1rpySS.str().c_str()); - - std::stringstream s1xyzSS; - s1xyzSS << (m_endpoint1(0)) << " " << (m_endpoint1(1)) << " " << (m_endpoint1(2)); - s1Origin.append_attribute("xyz").set_value(s1xyzSS.str().c_str()); - - pugi::xml_node s1Geom = s1Collision.append_child("geometry"); - pugi::xml_node s1 = s1Geom.append_child("sphere"); - pugi::xml_attribute rs1 = s1.append_attribute("radius"); - rs1.set_value(m_radius); - - // The middle cylinder - pugi::xml_node crCollision = lastLinkNode->append_child("collision"); - pugi::xml_node crOrigin = crCollision.append_child("origin"); - - std::stringstream crrpySS; - crrpySS << m_rot(0) << " " << m_rot(1) << " " << m_rot(2); - crOrigin.append_attribute("rpy").set_value(crrpySS.str().c_str()); - - std::stringstream crxyzSS; - crxyzSS << (m_midpoint(0)) << " " << (m_midpoint(1)) << " " << (m_midpoint(2)); - crOrigin.append_attribute("xyz").set_value(crxyzSS.str().c_str()); - - pugi::xml_node crGeom = crCollision.append_child("geometry"); - pugi::xml_node cyl = crGeom.append_child("cylinder"); - pugi::xml_attribute crl = cyl.append_attribute("length"); - crl.set_value(m_length); - pugi::xml_attribute crr = cyl.append_attribute("radius"); - crr.set_value(m_radius); - - // The second sphere - pugi::xml_node s2Collision = lastLinkNode->append_child("collision"); - pugi::xml_node s2Origin = s2Collision.append_child("origin"); - - std::stringstream s2rpySS; - s2rpySS << m_rot(0) << " " << m_rot(1) << " " << m_rot(2); - s2Origin.append_attribute("rpy").set_value(s1rpySS.str().c_str()); - - std::stringstream s2xyzSS; - s2xyzSS << (m_endpoint2(0)) << " " << (m_endpoint2(1)) << " " << (m_endpoint2(2)); - s2Origin.append_attribute("xyz").set_value(s2xyzSS.str().c_str()); - - pugi::xml_node s2Geom = s2Collision.append_child("geometry"); - pugi::xml_node s2 = s2Geom.append_child("sphere"); - pugi::xml_attribute s2r = s2.append_attribute("radius"); - s2r.set_value(m_radius); + // The first sphere + pugi::xml_node s1Collision = lastLinkNode->append_child("collision"); + pugi::xml_node s1Origin = s1Collision.append_child("origin"); + + std::stringstream s1rpySS; + s1rpySS << m_rot(0) << " " << m_rot(1) << " " << m_rot(2); + s1Origin.append_attribute("rpy").set_value(s1rpySS.str().c_str()); + + std::stringstream s1xyzSS; + s1xyzSS << (m_endpoint1(0)) << " " << (m_endpoint1(1)) << " " << (m_endpoint1(2)); + s1Origin.append_attribute("xyz").set_value(s1xyzSS.str().c_str()); + + pugi::xml_node s1Geom = s1Collision.append_child("geometry"); + pugi::xml_node s1 = s1Geom.append_child("sphere"); + pugi::xml_attribute rs1 = s1.append_attribute("radius"); + rs1.set_value(m_radius); + + // The middle cylinder + pugi::xml_node crCollision = lastLinkNode->append_child("collision"); + pugi::xml_node crOrigin = crCollision.append_child("origin"); + + std::stringstream crrpySS; + crrpySS << m_rot(0) << " " << m_rot(1) << " " << m_rot(2); + crOrigin.append_attribute("rpy").set_value(crrpySS.str().c_str()); + + std::stringstream crxyzSS; + crxyzSS << (m_midpoint(0)) << " " << (m_midpoint(1)) << " " << (m_midpoint(2)); + crOrigin.append_attribute("xyz").set_value(crxyzSS.str().c_str()); + + pugi::xml_node crGeom = crCollision.append_child("geometry"); + pugi::xml_node cyl = crGeom.append_child("cylinder"); + pugi::xml_attribute crl = cyl.append_attribute("length"); + crl.set_value(m_length); + pugi::xml_attribute crr = cyl.append_attribute("radius"); + crr.set_value(m_radius); + + // The second sphere + pugi::xml_node s2Collision = lastLinkNode->append_child("collision"); + pugi::xml_node s2Origin = s2Collision.append_child("origin"); + + std::stringstream s2rpySS; + s2rpySS << m_rot(0) << " " << m_rot(1) << " " << m_rot(2); + s2Origin.append_attribute("rpy").set_value(s1rpySS.str().c_str()); + + std::stringstream s2xyzSS; + s2xyzSS << (m_endpoint2(0)) << " " << (m_endpoint2(1)) << " " << (m_endpoint2(2)); + s2Origin.append_attribute("xyz").set_value(s2xyzSS.str().c_str()); + + pugi::xml_node s2Geom = s2Collision.append_child("geometry"); + pugi::xml_node s2 = s2Geom.append_child("sphere"); + pugi::xml_attribute s2r = s2.append_attribute("radius"); + s2r.set_value(m_radius); } -void CapsuleFit::writeUrdfXacro(boost::shared_ptr lastLinkNode) +void CapsuleFit::writeUrdfXacro(std::shared_ptr lastLinkNode) { - pugi::xml_node collision = lastLinkNode->append_child( - "xacro:capsule-collision"); + pugi::xml_node collision = lastLinkNode->append_child( + "xacro:capsule-collision"); - std::stringstream e1SS; - e1SS << (m_endpoint1(0)) << " " << (m_endpoint1(1)) << " " << (m_endpoint1(2)); - collision.append_attribute("e1").set_value(e1SS.str().c_str()); + std::stringstream e1SS; + e1SS << (m_endpoint1(0)) << " " << (m_endpoint1(1)) << " " << (m_endpoint1(2)); + collision.append_attribute("e1").set_value(e1SS.str().c_str()); - std::stringstream e2SS; - e2SS << (m_endpoint2(0)) << " " << (m_endpoint2(1)) << " " << (m_endpoint2(2)); - collision.append_attribute("e2").set_value(e2SS.str().c_str()); + std::stringstream e2SS; + e2SS << (m_endpoint2(0)) << " " << (m_endpoint2(1)) << " " << (m_endpoint2(2)); + collision.append_attribute("e2").set_value(e2SS.str().c_str()); - collision.append_attribute("rad").set_value(m_radius); + collision.append_attribute("rad").set_value(m_radius); - collision.append_attribute("len").set_value(m_length); + collision.append_attribute("len").set_value(m_length); - std::stringstream rpySS; - rpySS << (m_rot(0)) << " " << (m_rot(1)) << " " << (m_rot(2)); - collision.append_attribute("rpy").set_value(rpySS.str().c_str()); + std::stringstream rpySS; + rpySS << (m_rot(0)) << " " << (m_rot(1)) << " " << (m_rot(2)); + collision.append_attribute("rpy").set_value(rpySS.str().c_str()); - std::stringstream midSS; - midSS << (m_midpoint(0)) << " " << (m_midpoint(1)) << " " << (m_midpoint(2)); - collision.append_attribute("mid").set_value(midSS.str().c_str()); + std::stringstream midSS; + midSS << (m_midpoint(0)) << " " << (m_midpoint(1)) << " " << (m_midpoint(2)); + collision.append_attribute("mid").set_value(midSS.str().c_str()); } // static! void CapsuleFit::writeXacroMacro(const pugi::xml_document* doc) { - pugi::xml_node robotNode = doc->child("robot"); - - pugi::xml_node xacroNode = robotNode.prepend_child("xacro:macro"); - xacroNode.append_attribute("name").set_value("capsule-collision"); - xacroNode.append_attribute("params").set_value("e1 e2 rad len rpy mid"); - - // The first sphere - pugi::xml_node s1Collision = xacroNode.append_child("collision"); - pugi::xml_node s1Origin = s1Collision.append_child("origin"); - s1Origin.append_attribute("rpy").set_value("${rpy}"); - s1Origin.append_attribute("xyz").set_value("${e1}"); - - pugi::xml_node s1Geom = s1Collision.append_child("geometry"); - pugi::xml_node s1 = s1Geom.append_child("sphere"); - pugi::xml_attribute rs1 = s1.append_attribute("radius"); - rs1.set_value("${rad}"); - - // The middle cylinder - pugi::xml_node crCollision = xacroNode.append_child("collision"); - pugi::xml_node crOrigin = crCollision.append_child("origin"); - crOrigin.append_attribute("rpy").set_value("${rpy}"); - crOrigin.append_attribute("xyz").set_value("${mid}"); - - pugi::xml_node crGeom = crCollision.append_child("geometry"); - pugi::xml_node cyl = crGeom.append_child("cylinder"); - pugi::xml_attribute crl = cyl.append_attribute("length"); - crl.set_value("${len}"); - pugi::xml_attribute crr = cyl.append_attribute("radius"); - crr.set_value("${rad}"); - - // The second sphere - pugi::xml_node s2Collision = xacroNode.append_child("collision"); - pugi::xml_node s2Origin = s2Collision.append_child("origin"); - s2Origin.append_attribute("rpy").set_value("${rpy}"); - s2Origin.append_attribute("xyz").set_value("${e2}"); - - pugi::xml_node s2Geom = s2Collision.append_child("geometry"); - pugi::xml_node s2 = s2Geom.append_child("sphere"); - pugi::xml_attribute s2r = s2.append_attribute("radius"); - s2r.set_value("${rad}"); + pugi::xml_node robotNode = doc->child("robot"); + + pugi::xml_node xacroNode = robotNode.prepend_child("xacro:macro"); + xacroNode.append_attribute("name").set_value("capsule-collision"); + xacroNode.append_attribute("params").set_value("e1 e2 rad len rpy mid"); + + // The first sphere + pugi::xml_node s1Collision = xacroNode.append_child("collision"); + pugi::xml_node s1Origin = s1Collision.append_child("origin"); + s1Origin.append_attribute("rpy").set_value("${rpy}"); + s1Origin.append_attribute("xyz").set_value("${e1}"); + + pugi::xml_node s1Geom = s1Collision.append_child("geometry"); + pugi::xml_node s1 = s1Geom.append_child("sphere"); + pugi::xml_attribute rs1 = s1.append_attribute("radius"); + rs1.set_value("${rad}"); + + // The middle cylinder + pugi::xml_node crCollision = xacroNode.append_child("collision"); + pugi::xml_node crOrigin = crCollision.append_child("origin"); + crOrigin.append_attribute("rpy").set_value("${rpy}"); + crOrigin.append_attribute("xyz").set_value("${mid}"); + + pugi::xml_node crGeom = crCollision.append_child("geometry"); + pugi::xml_node cyl = crGeom.append_child("cylinder"); + pugi::xml_attribute crl = cyl.append_attribute("length"); + crl.set_value("${len}"); + pugi::xml_attribute crr = cyl.append_attribute("radius"); + crr.set_value("${rad}"); + + // The second sphere + pugi::xml_node s2Collision = xacroNode.append_child("collision"); + pugi::xml_node s2Origin = s2Collision.append_child("origin"); + s2Origin.append_attribute("rpy").set_value("${rpy}"); + s2Origin.append_attribute("xyz").set_value("${e2}"); + + pugi::xml_node s2Geom = s2Collision.append_child("geometry"); + pugi::xml_node s2 = s2Geom.append_child("sphere"); + pugi::xml_attribute s2r = s2.append_attribute("radius"); + s2r.set_value("${rad}"); } //****************************************************************************************************************** // URDF class -Urdf::Urdf(const char* filepath, bool xacro_support) +Urdf::Urdf(const char *filepath, bool xacro_support, + std::shared_ptr nh) : nh_(nh) { - m_filepath = filepath; - m_xacro = xacro_support; // boolean - - // Load URDF XML - pugi::xml_parse_result result = m_doc.load_file(filepath); - ROS_DEBUG("URDF File: %s", filepath); - ROS_DEBUG("URDF File parse result: %s", result.description()); - - // check whether XML could be read - m_URDFLoaded = (result.status == pugi::status_ok); - - if (!m_URDFLoaded) - { - ROS_ERROR("The URDF could NOT be loaded"); - ros::shutdown(); - } - - m_nodes = m_doc.select_nodes(m_collision_path.c_str()); - m_visual_nodes = m_doc.select_nodes(m_visual_path.c_str()); - - m_it = m_nodes.begin(); - m_visual_it = m_visual_nodes.begin(); - - // write the xacro macros for all shapes - if (xacro_support) - { - addXacroDef(); - } + m_filepath = filepath; + m_xacro = xacro_support; // boolean + + // Load URDF XML + pugi::xml_parse_result result = m_doc.load_file(filepath); + RCLCPP_DEBUG(nh_->get_logger(), + "URDF File: %s", filepath); + RCLCPP_DEBUG(nh_->get_logger(), + "URDF File parse result: %s", result.description()); + + // check whether XML could be read + m_URDFLoaded = (result.status == pugi::status_ok); + + if (!m_URDFLoaded) + { + RCLCPP_ERROR(nh_->get_logger(), + "The URDF could NOT be loaded"); + rclcpp::shutdown(); + } + + m_nodes = m_doc.select_nodes(m_collision_path.c_str()); + m_visual_nodes = m_doc.select_nodes(m_visual_path.c_str()); + + m_it = m_nodes.begin(); + m_visual_it = m_visual_nodes.begin(); + + // write the xacro macros for all shapes + if (xacro_support) + { + addXacroDef(); + } } bool Urdf::save(const char* path) { - ROS_INFO("************************************************"); - ROS_INFO("Saved file with path: %s", path); - ROS_INFO("************************************************"); - return m_doc.save_file(path); + RCLCPP_INFO(nh_->get_logger(), + "************************************************"); + RCLCPP_INFO(nh_->get_logger(), + "Saved file with path: %s", path); + RCLCPP_INFO(nh_->get_logger(), + "************************************************"); + return m_doc.save_file(path); } void Urdf::addXacroDef() { - ROS_DEBUG("Adding xacro definitions..."); - - // NOTE: write macros for each shape! - CapsuleFit::writeXacroMacro(&m_doc); - BoxFit::writeXacroMacro(&m_doc); + RCLCPP_DEBUG(nh_->get_logger(), + "Adding xacro definitions..."); + + // NOTE: write macros for each shape! + CapsuleFit::writeXacroMacro(&m_doc); + BoxFit::writeXacroMacro(&m_doc); } bool Urdf::isXacro() { - return m_xacro; + return m_xacro; } std::string Urdf::getNextMesh() { - m_lastMeshNode = m_it->node(); - m_lastCollisionNode = m_lastMeshNode.parent().parent(); - m_lastLinkNode = m_lastCollisionNode.parent(); + m_lastMeshNode = m_it->node(); + m_lastCollisionNode = m_lastMeshNode.parent().parent(); + m_lastLinkNode = m_lastCollisionNode.parent(); - std::string filename( m_lastMeshNode.attribute("filename").value() ); - m_it++; + std::string filename( m_lastMeshNode.attribute("filename").value() ); + m_it++; - return filename; + return filename; } std::string Urdf::getNextVisualMesh() { - m_lastVisualMeshNode = m_visual_it->node(); - m_lastVisualNode = m_lastVisualMeshNode.parent().parent(); - m_lastVisualLinkNode = m_lastVisualNode.parent(); + m_lastVisualMeshNode = m_visual_it->node(); + m_lastVisualNode = m_lastVisualMeshNode.parent().parent(); + m_lastVisualLinkNode = m_lastVisualNode.parent(); - std::string filename( m_lastVisualMeshNode.attribute("filename").value() ); - m_visual_it++; + std::string filename( m_lastVisualMeshNode.attribute("filename").value() ); + m_visual_it++; - return filename; + return filename; } bool Urdf::hasMoreMesh() { - return m_it != m_nodes.end(); + return m_it != m_nodes.end(); } bool Urdf::hasMoreVisualMesh() { - return m_visual_it != m_visual_nodes.end(); + return m_visual_it != m_visual_nodes.end(); } -void Urdf::replaceMesh(boost::shared_ptr shape) +void Urdf::replaceMesh(std::shared_ptr shape) { - //remove old collision element - if (!m_lastLinkNode.remove_child("collision")) - { - ROS_ERROR("Can't remove collision element. Skipping..." ); - } - - shape->writeUrdf(boost::make_shared(m_lastLinkNode)); + //remove old collision element + if (!m_lastLinkNode.remove_child("collision")) + { + RCLCPP_ERROR(nh_->get_logger(), + "Can't remove collision element. Skipping..." ); + } + + shape->writeUrdf(std::make_shared(m_lastLinkNode)); } -void Urdf::replaceMeshWithXacro(boost::shared_ptr shape) +void Urdf::replaceMeshWithXacro(std::shared_ptr shape) { - //remove old collision element - if (!m_lastLinkNode.remove_child("collision")) // BUG Says skipping, but is not? - { - ROS_INFO_STREAM("Can't remove collision element. Skipping..." ); - } - - shape->writeUrdfXacro(boost::make_shared(m_lastLinkNode)); + //remove old collision element + if (!m_lastLinkNode.remove_child("collision")) // BUG Says skipping, but is not? + { + RCLCPP_INFO_STREAM(nh_->get_logger(), + "Can't remove collision element. Skipping..." ); + } + + shape->writeUrdfXacro(std::make_shared(m_lastLinkNode)); } Eigen::Vector3d Urdf::getMeshRotation() const { - pugi::xml_node originNode = m_lastCollisionNode.child("origin"); + pugi::xml_node originNode = m_lastCollisionNode.child("origin"); - std::vector rpy; + std::vector rpy; - if (!originNode.attribute("rpy")) - { - return Eigen::Vector3d(0, 0, 0); - } + if (!originNode.attribute("rpy")) + { + return Eigen::Vector3d(0, 0, 0); + } - const char* rpyStr = originNode.attribute("rpy").value(); - geometry::getAsVector(rpyStr, &rpy); + const char* rpyStr = originNode.attribute("rpy").value(); + geometry::getAsVector(rpyStr, &rpy); - return Eigen::Vector3d(rpy.data()); + return Eigen::Vector3d(rpy.data()); } Eigen::Vector3d Urdf::getMeshTranslation() const { - pugi::xml_node originNode = m_lastCollisionNode.child("origin"); + pugi::xml_node originNode = m_lastCollisionNode.child("origin"); - std::vector xyz; + std::vector xyz; - if (!originNode.attribute("xyz")) - { - return Eigen::Vector3d(0, 0, 0); - } + if (!originNode.attribute("xyz")) + { + return Eigen::Vector3d(0, 0, 0); + } - const char* xyzStr = originNode.attribute("xyz").value(); - geometry::getAsVector(xyzStr, &xyz); + const char* xyzStr = originNode.attribute("xyz").value(); + geometry::getAsVector(xyzStr, &xyz); - return Eigen::Vector3d(xyz[0], xyz[1], xyz[2]); + return Eigen::Vector3d(xyz[0], xyz[1], xyz[2]); } Eigen::Vector3d Urdf::getMeshScale() const { - std::vector scaleVec; + std::vector scaleVec; - const char* scaleStr = ""; + const char* scaleStr = ""; - if (!m_lastMeshNode.attribute("scale")) - { - return Eigen::Vector3d(1, 1, 1); - } + if (!m_lastMeshNode.attribute("scale")) + { + return Eigen::Vector3d(1, 1, 1); + } - scaleStr = m_lastMeshNode.attribute("scale").value(); - ROS_INFO_STREAM("scaleStr: " << scaleStr ); - geometry::getAsVector(scaleStr, &scaleVec); + scaleStr = m_lastMeshNode.attribute("scale").value(); + RCLCPP_INFO_STREAM(nh_->get_logger(),"scaleStr: " << scaleStr ); + geometry::getAsVector(scaleStr, &scaleVec); - return Eigen::Vector3d(scaleVec[0], scaleVec[1], scaleVec[2]); + return Eigen::Vector3d(scaleVec[0], scaleVec[1], scaleVec[2]); } float Urdf::getMass(bool visual) { - pugi::xml_node mass_node; - - if (visual) - { - mass_node = m_lastVisualLinkNode.child("inertial").child("mass"); - } - else - { - mass_node = m_lastLinkNode.child("inertial").child("mass"); - } - - if (mass_node) - { - ROS_INFO_STREAM("Mass: " << mass_node.attribute("value").as_float() ); - return mass_node.attribute("value").as_float(); - } - else - { - ROS_INFO_STREAM("Did not find node 'inertial/mass'. Skipping mass." ); - return 1.; - } + pugi::xml_node mass_node; + + if (visual) + { + mass_node = m_lastVisualLinkNode.child("inertial").child("mass"); + } + else + { + mass_node = m_lastLinkNode.child("inertial").child("mass"); + } + + if (mass_node) + { + RCLCPP_INFO_STREAM(nh_->get_logger(),"Mass: " << mass_node.attribute("value").as_float() ); + return mass_node.attribute("value").as_float(); + } + else + { + RCLCPP_INFO_STREAM(nh_->get_logger(),"Did not find node 'inertial/mass'. Skipping mass." ); + return 1.; + } } void Urdf::setInertia(vcg::Matrix33f Inertia, bool visual) { - pugi::xml_node inertia_node; - - if (visual) - { - inertia_node = m_lastVisualLinkNode.child("inertial").child("inertia"); - } - else - { - inertia_node = m_lastLinkNode.child("inertial").child("inertia"); - } - - if (inertia_node) - { - pugi::xml_attribute ixx = inertia_node.attribute("ixx"); - pugi::xml_attribute ixy = inertia_node.attribute("ixy"); - pugi::xml_attribute ixz = inertia_node.attribute("ixz"); - pugi::xml_attribute iyy = inertia_node.attribute("iyy"); - pugi::xml_attribute iyz = inertia_node.attribute("iyz"); - pugi::xml_attribute izz = inertia_node.attribute("izz"); - - ROS_INFO_STREAM("Old Inertia:" ); - ROS_INFO_STREAM("Ixx: " << ixx.as_float() << ", Ixy: " << ixy.as_float() << ", Ixz: " << ixz.as_float() << ", Iyy: " << iyy.as_float() << ", Iyz: " << iyz.as_float() << ", Izz: " << izz.as_float() ); - - ixx.set_value(Inertia[0][0]); - ixy.set_value(Inertia[0][1]); - ixz.set_value(Inertia[0][2]); - iyy.set_value(Inertia[1][1]); - iyz.set_value(Inertia[1][2]); - izz.set_value(Inertia[2][2]); - - ROS_INFO_STREAM("New Inertia:" ); - ROS_INFO_STREAM("Ixx: " << ixx.as_float() << ", Ixy: " << ixy.as_float() << ", Ixz: " << ixz.as_float() << ", Iyy: " << iyy.as_float() << ", Iyz: " << iyz.as_float() << ", Izz: " << izz.as_float() ); - } - else - { - ROS_INFO_STREAM("Did not find node 'inertial/inertia'. Skipping inertia." ); - } + pugi::xml_node inertia_node; + + if (visual) + { + inertia_node = m_lastVisualLinkNode.child("inertial").child("inertia"); + } + else + { + inertia_node = m_lastLinkNode.child("inertial").child("inertia"); + } + + if (inertia_node) + { + pugi::xml_attribute ixx = inertia_node.attribute("ixx"); + pugi::xml_attribute ixy = inertia_node.attribute("ixy"); + pugi::xml_attribute ixz = inertia_node.attribute("ixz"); + pugi::xml_attribute iyy = inertia_node.attribute("iyy"); + pugi::xml_attribute iyz = inertia_node.attribute("iyz"); + pugi::xml_attribute izz = inertia_node.attribute("izz"); + + RCLCPP_INFO_STREAM(nh_->get_logger(),"Old Inertia:" ); + RCLCPP_INFO_STREAM(nh_->get_logger(),"Ixx: " << ixx.as_float() << ", Ixy: " << ixy.as_float() << ", Ixz: " << ixz.as_float() << ", Iyy: " << iyy.as_float() << ", Iyz: " << iyz.as_float() << ", Izz: " << izz.as_float() ); + + ixx.set_value(Inertia[0][0]); + ixy.set_value(Inertia[0][1]); + ixz.set_value(Inertia[0][2]); + iyy.set_value(Inertia[1][1]); + iyz.set_value(Inertia[1][2]); + izz.set_value(Inertia[2][2]); + + RCLCPP_INFO_STREAM(nh_->get_logger(),"New Inertia:" ); + RCLCPP_INFO_STREAM(nh_->get_logger(),"Ixx: " << ixx.as_float() << ", Ixy: " << ixy.as_float() << ", Ixz: " << ixz.as_float() << ", Iyy: " << iyy.as_float() << ", Iyz: " << iyz.as_float() << ", Izz: " << izz.as_float() ); + } + else + { + RCLCPP_INFO_STREAM(nh_->get_logger(),"Did not find node 'inertial/inertia'. Skipping inertia." ); + } } void Urdf::setCenterOfMass(vcg::Point3f CenterOfMass, bool visual) { - pugi::xml_node com_node; - - if (visual) - { - com_node = m_lastVisualLinkNode.child("inertial").child("origin"); - } - else - { - com_node = m_lastLinkNode.child("inertial").child("origin"); - } - - if (com_node) - { - pugi::xml_attribute xyz = com_node.attribute("xyz"); - - ROS_INFO_STREAM("Old center of mass: " << xyz.value() ); - - xyz.set_value((std::to_string(CenterOfMass[0]) + " " + std::to_string(CenterOfMass[1]) + " " + std::to_string(CenterOfMass[2])).c_str()); - - ROS_INFO_STREAM("New center of mass: " << xyz.value() ); - } - else - { - ROS_INFO_STREAM("Did not find node 'inertial/origin'. Skipping center of mass." ); - } + pugi::xml_node com_node; + + if (visual) + { + com_node = m_lastVisualLinkNode.child("inertial").child("origin"); + } + else + { + com_node = m_lastLinkNode.child("inertial").child("origin"); + } + + if (com_node) + { + pugi::xml_attribute xyz = com_node.attribute("xyz"); + + RCLCPP_INFO_STREAM(nh_->get_logger(),"Old center of mass: " << xyz.value() ); + + xyz.set_value((std::to_string(CenterOfMass[0]) + " " + std::to_string(CenterOfMass[1]) + " " + std::to_string(CenterOfMass[2])).c_str()); + + RCLCPP_INFO_STREAM(nh_->get_logger(),"New center of mass: " << xyz.value() ); + } + else + { + RCLCPP_INFO_STREAM(nh_->get_logger(),"Did not find node 'inertial/origin'. Skipping center of mass." ); + } } /*******************************************************************************************************************************************************/ -Stl::Stl(std::string url) -{ - url::Url filepath(url); - - std::string pkgPath = ros::package::getPath(filepath.getPackageName()); - std::string filename = pkgPath + filepath.getRelativePath(); - ROS_INFO_STREAM("STL file path: " << filename ); - - m_filestream.open(filename.c_str(), std::ios::in | std::ios::binary); - - // Check if filestream is corrupted - if (!m_filestream) - { - ROS_ERROR("ERROR OPENING STL FILESTREAM!"); - assert(false); - } - else - { - // Check if the filestream is OK - if (m_filestream.fail()) - { - ROS_ERROR("! ! ! ERROR AFTER STL FILESTREAM OPENED ! ! !"); - - if (m_filestream.eof()) - { - ROS_ERROR("Unexpectedly reached EOF!"); - } - } - else - { - ROS_DEBUG("STL filestream successfully opened"); - } - } - - // Read header - char header_info[80] = ""; - m_filestream.read(header_info, 80); - - // Read number of faces/triangles - char n_triangles[4]; - m_filestream.read(n_triangles, 4); - unsigned int* r = (unsigned int*) n_triangles; - m_num_faces = *r; +Stl::Stl(std::string url, + std::shared_ptr nh): MeshFile(nh) +{ + url::Url filepath(url); + + std::string pkgPath = ament_index_cpp::get_package_share_directory(filepath.getPackageName()); + std::string filename = pkgPath + filepath.getRelativePath(); + RCLCPP_INFO_STREAM(nh_->get_logger(),"STL file path: " << filename ); + + m_filestream.open(filename.c_str(), std::ios::in | std::ios::binary); + + // Check if filestream is corrupted + if (!m_filestream) + { + RCLCPP_ERROR(nh_->get_logger(), + "ERROR OPENING STL FILESTREAM!"); + assert(false); + } + else + { + // Check if the filestream is OK + if (m_filestream.fail()) + { + RCLCPP_ERROR(nh_->get_logger(), + "! ! ! ERROR AFTER STL FILESTREAM OPENED ! ! !"); + + if (m_filestream.eof()) + { + RCLCPP_ERROR(nh_->get_logger(), + "Unexpectedly reached EOF!"); + } + } + else + { + RCLCPP_DEBUG(nh_->get_logger(), + "STL filestream successfully opened"); + } + } + + // Read header + char header_info[80] = ""; + m_filestream.read(header_info, 80); + + // Read number of faces/triangles + char n_triangles[4]; + m_filestream.read(n_triangles, 4); + unsigned int* r = (unsigned int*) n_triangles; + m_num_faces = *r; } float Stl::parseFloat() { - char f_buf[sizeof(float)]; - m_filestream.read(f_buf, 4); - float* fptr = (float*) f_buf; - return *fptr; + char f_buf[sizeof(float)]; + m_filestream.read(f_buf, 4); + float* fptr = (float*) f_buf; + return *fptr; } void Stl::dumpPoint() { - char dummy[12]; - m_filestream.read(dummy, 12); + char dummy[12]; + m_filestream.read(dummy, 12); } std::vector Stl::getPoints() { - std::vector points = {}; - - ROS_DEBUG_STREAM("File contains " << m_num_faces << " faces"); - - for (unsigned int i = 0; i < m_num_faces; i++) - { - dumpPoint(); // dump the normal vector - - // Read the actual points (3 points with 3 dimensions) - for (uint j = 0; j < 3; j++) - { - for (uint k = 0; k < 3; k++) - { - points.push_back(parseFloat()); - } - } - - // Check if the filestream is OK - if (m_filestream.fail()) - { - ROS_ERROR("! ! ! ERROR IN STL FILESTREAM ! ! !"); - - if (m_filestream.eof()) - { - ROS_ERROR("Unexpectedly reached EOF!"); - } - - ROS_ERROR_STREAM("Error No.:" << strerror(errno)); - break; - } - - // another dummy - char dummy[2]; - m_filestream.read(dummy, 2); - } - - return points; + std::vector points = {}; + + RCLCPP_DEBUG_STREAM(nh_->get_logger(), + "File contains " << m_num_faces << " faces"); + + for (unsigned int i = 0; i < m_num_faces; i++) + { + dumpPoint(); // dump the normal vector + + // Read the actual points (3 points with 3 dimensions) + for (uint j = 0; j < 3; j++) + { + for (uint k = 0; k < 3; k++) + { + points.push_back(parseFloat()); + } + } + + // Check if the filestream is OK + if (m_filestream.fail()) + { + RCLCPP_ERROR(nh_->get_logger(),"! ! ! ERROR IN STL FILESTREAM ! ! !"); + + if (m_filestream.eof()) + { + RCLCPP_ERROR(nh_->get_logger(),"Unexpectedly reached EOF!"); + } + + RCLCPP_ERROR_STREAM(nh_->get_logger(),"Error No.:" << strerror(errno)); + break; + } + + // another dummy + char dummy[2]; + m_filestream.read(dummy, 2); + } + + return points; } //****************************************************************************************************************** -Dae::Dae(std::string filename) +Dae::Dae(std::string filename, + std::shared_ptr nh): MeshFile(nh) { - url::Url filepath(filename); - std::string pkgName = filepath.getPackageName(); - ROS_INFO_STREAM("pkgName: " << pkgName ); + url::Url filepath(filename); + std::string pkgName = filepath.getPackageName(); + RCLCPP_INFO_STREAM(nh_->get_logger(),"pkgName: " << pkgName ); - std::string pkgPath = ros::package::getPath(pkgName); - ROS_INFO_STREAM("pkgPath: " << pkgPath ); + std::string pkgPath = ament_index_cpp::get_package_share_directory(pkgName); + RCLCPP_INFO_STREAM(nh_->get_logger(),"pkgPath: " << pkgPath ); - std::string path = filepath.getRelativePath(); - ROS_INFO_STREAM("path: " << path ); + std::string path = filepath.getRelativePath(); + RCLCPP_INFO_STREAM(nh_->get_logger(),"path: " << path ); - m_filename = (pkgPath + path).c_str(); - ROS_INFO_STREAM("DAE file path: " << m_filename ); + m_filename = (pkgPath + path).c_str(); + RCLCPP_INFO_STREAM(nh_->get_logger(),"DAE file path: " << m_filename ); - pugi::xml_parse_result result = m_doc.load_file(m_filename.c_str()); - ROS_INFO_STREAM("DAE file load result: " << result.description() ); + pugi::xml_parse_result result = m_doc.load_file(m_filename.c_str()); + RCLCPP_INFO_STREAM(nh_->get_logger(),"DAE file load result: " << result.description() ); } /* Extract all polygon vertices from DAE file*/ std::vector Dae::getPoints() { - std::vector points; - const std::string path = - "/COLLADA/library_geometries/geometry/mesh/source[*]/float_array[contains(@id,'positions')]"; + std::vector points; + const std::string path = + "/COLLADA/library_geometries/geometry/mesh/source[*]/float_array[contains(@id,'positions')]"; - const pugi::xpath_node_set nodes = m_doc.select_nodes(path.c_str()); + const pugi::xpath_node_set nodes = m_doc.select_nodes(path.c_str()); - for (const pugi::xpath_node* it = nodes.begin(); it != nodes.end(); ++it) - { - const char* vertexString = it->node().text().get(); - geometry::getAsVector(vertexString, &points); - } + for (const pugi::xpath_node* it = nodes.begin(); it != nodes.end(); ++it) + { + const char* vertexString = it->node().text().get(); + geometry::getAsVector(vertexString, &points); + } - return points; + return points; } /* Get transformation matrix from DAE file*/ Eigen::Matrix4d Dae::getTransformationMatrix() { - Eigen::Matrix4d t; - std::vector els; + Eigen::Matrix4d t; + std::vector els; - const std::string path = - "/COLLADA/library_visual_scenes/visual_scene[@id=\"Scene\"]/node/matrix[@sid=\"transform\"]"; - const pugi::xpath_node_set nodes = m_doc.select_nodes(path.c_str()); - ROS_INFO_STREAM("getting trans mat... " ); + const std::string path = + "/COLLADA/library_visual_scenes/visual_scene[@id=\"Scene\"]/node/matrix[@sid=\"transform\"]"; + const pugi::xpath_node_set nodes = m_doc.select_nodes(path.c_str()); + RCLCPP_INFO_STREAM(nh_->get_logger(),"getting trans mat... " ); - for (const pugi::xpath_node* it = nodes.begin(); it != nodes.end(); ++it) - { - const char* elString = it->node().text().get(); - ROS_INFO_STREAM("elString: " << elString ); - geometry::getAsVector(elString, &els); - } + for (const pugi::xpath_node* it = nodes.begin(); it != nodes.end(); ++it) + { + const char* elString = it->node().text().get(); + RCLCPP_INFO_STREAM(nh_->get_logger(),"elString: " << elString ); + geometry::getAsVector(elString, &els); + } - t = Eigen::Matrix4d(els.data()); - ROS_INFO_STREAM("trans mat: " << t ); + t = Eigen::Matrix4d(els.data()); + RCLCPP_INFO_STREAM(nh_->get_logger(),"trans mat: " << t ); - return t; + return t; } //******************************************************************************************************************* -VersatileFitter::VersatileFitter() +VersatileFitter::VersatileFitter(std::shared_ptr nh) : nh_(nh) { } std::string VersatileFitter::getFileExtension(std::string meshfile) { - std::string file_extension(""); - - // Get file extension - std::regex file_regex("([^\\\\.]+)(\\.)([^\\\\.]+)"); - std::smatch r_match; - - if (std::regex_search(meshfile, r_match, file_regex)) - { - file_extension = r_match[3]; - ROS_DEBUG_STREAM("Detected file extension ." << r_match[3] ); - } - else - { - ROS_INFO_STREAM("NO FILE EXTENSION FOUND!" ); - } - - return file_extension; -} - - -void VersatileFitter::fit(std::string urdf_filename, std::string &output_filename, VersatileFitter::Shape shape, std::string shape_name, const ros::NodeHandle nh) -{ - try - { - Urdf urdf(urdf_filename.c_str(), true); - - // Return if the urdf was not loaded - if ( !urdf.loaded() ) - { - ROS_ERROR("URDF could not be loaded." ); - return; - } - - while (urdf.hasMoreMesh()) - { - std::string meshfile = urdf.getNextMesh(); - - std::string file_extension = getFileExtension(meshfile); - - // Abstract meshfile ptr - boost::shared_ptr mf; - - // Initialize meshfile ptr to concrete filetype - if (file_extension == "stl" || file_extension == "STL") - { - ROS_DEBUG( "Handling STL file..." ); - mf = boost::make_shared(meshfile); - } - else if (file_extension == "dae" || file_extension == "DAE") - { - ROS_DEBUG( "Handling DAE file..." ); - mf = boost::make_shared(meshfile); - } - else - { - ROS_ERROR("UNKNOWN FILE EXTENSION! (.stl or .dae supported)" ); - mf = nullptr; - } - - std::vector points = mf->getPoints(); - - Eigen::Matrix4d M = mf->getTransformationMatrix(); - Eigen::Vector3d t = urdf.getMeshTranslation(); - Eigen::Vector3d r = urdf.getMeshRotation(); - Eigen::Vector3d s = urdf.getMeshScale(); - - ROS_DEBUG_STREAM("Mesh translation: " << t ); - ROS_DEBUG_STREAM("Mesh rotation: " << r ); - ROS_DEBUG_STREAM("Mesh scale: " << s ); - - boost::shared_ptr shapefitter; - - if (shape == Shape::Box) - { - // Fit the box - shapefitter = boost::make_shared(points, t, r, s, M, nh); - } - else if (shape == Shape::Capsule) - // Fit the capsule - { - shapefitter = boost::make_shared(points, t, r, s, M); - } - - // Save the results - if (urdf.isXacro()) - { - urdf.replaceMeshWithXacro(shapefitter); - } - else - { - urdf.replaceMesh(shapefitter); - } - } - - if (urdf.save(output_filename.c_str())) - { - ROS_INFO_STREAM("Primitives generated successfully!" ); - output_filename = output_filename; - } - } - catch (std::exception& e) - { - ROS_ERROR_STREAM("Unhandled Exception: " << e.what() ); - } + std::string file_extension(""); + + // Get file extension + std::regex file_regex("([^\\\\.]+)(\\.)([^\\\\.]+)"); + std::smatch r_match; + + if (std::regex_search(meshfile, r_match, file_regex)) + { + file_extension = r_match[3]; + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Detected file extension ." << r_match[3] ); + } + else + { + RCLCPP_INFO_STREAM(nh_->get_logger(),"NO FILE EXTENSION FOUND!" ); + } + + return file_extension; } + + +void VersatileFitter::fit(std::string urdf_filename, + std::string &output_filename, + VersatileFitter::Shape shape, std::string shape_name) +{ + try + { + Urdf urdf(urdf_filename.c_str(), true,nh_); + + + // Return if the urdf was not loaded + if ( !urdf.loaded() ) + { + RCLCPP_ERROR(nh_->get_logger(),"URDF could not be loaded." ); + return; + } + + while (urdf.hasMoreMesh()) + { + std::string meshfile = urdf.getNextMesh(); + + std::string file_extension = getFileExtension(meshfile); + + // Abstract meshfile ptr + std::shared_ptr mf; + + // Initialize meshfile ptr to concrete filetype + if (file_extension == "stl" || file_extension == "STL") + { + RCLCPP_DEBUG(nh_->get_logger(),"Handling STL file..." ); + mf = std::make_shared(meshfile,nh_); + } + else if (file_extension == "dae" || file_extension == "DAE") + { + RCLCPP_DEBUG(nh_->get_logger(), "Handling DAE file..." ); + mf = std::make_shared(meshfile,nh_); + } + else + { + RCLCPP_ERROR(nh_->get_logger(),"UNKNOWN FILE EXTENSION! (.stl or .dae supported)" ); + mf = nullptr; + } + + std::vector points = mf->getPoints(); + + Eigen::Matrix4d M = mf->getTransformationMatrix(); + Eigen::Vector3d t = urdf.getMeshTranslation(); + Eigen::Vector3d r = urdf.getMeshRotation(); + Eigen::Vector3d s = urdf.getMeshScale(); + + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Mesh translation: " << t ); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Mesh rotation: " << r ); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Mesh scale: " << s ); + + std::shared_ptr shapefitter; + + if (shape == Shape::Box) + { + // Fit the box + shapefitter = std::make_shared(points, t, r, s, M, nh_); + } + else if (shape == Shape::Capsule) + // Fit the capsule + { + shapefitter = std::make_shared(points, t, r, s, M, nh_); + } + + // Save the results + if (urdf.isXacro()) + { + urdf.replaceMeshWithXacro(shapefitter); + } + else + { + urdf.replaceMesh(shapefitter); + } + } + + if (urdf.save(output_filename.c_str())) + { + RCLCPP_INFO_STREAM(nh_->get_logger(),"Primitives generated successfully!" ); + output_filename = output_filename; + } + } + catch (std::exception& e) + { + RCLCPP_ERROR_STREAM(nh_->get_logger(),"Unhandled Exception: " << e.what() ); + } +} + +PrimitiveFitterNode::PrimitiveFitterNode() + : Node(NODE_NAME) { + RCLCPP_INFO(this->get_logger(), + " ********** NimbRo Primitive Fitter started ********** "); + + declare_parameter("urdf_filename", rclcpp::PARAMETER_STRING); + declare_parameter("output_filename", rclcpp::PARAMETER_STRING); + declare_parameter("fit_shape", rclcpp::PARAMETER_STRING); + declare_parameter("use_fitter", rclcpp::PARAMETER_BOOL); + declare_parameter("use_inertia", rclcpp::PARAMETER_BOOL); + declare_parameter("mesh_type", rclcpp::PARAMETER_STRING); +}; + +PrimitiveFitterNode::~PrimitiveFitterNode() +{}; + +void PrimitiveFitterNode::setSharedPointer(std::shared_ptr a_shr_ptr) { + shr_ptr_to_this_ = a_shr_ptr; +} + +std::string PrimitiveFitterNode::getFileExtension(std::string meshfile) +{ + std::string file_extension(""); + + // Get file extension + std::regex file_regex("([^\\\\.]+)(\\.)([^\\\\.]+)"); + std::smatch r_match; + + if (std::regex_search(meshfile, r_match, file_regex)) + { + file_extension = r_match[3]; + RCLCPP_DEBUG_STREAM(get_logger(), "Detected file extension ." << r_match[3] ); + } + else + { + RCLCPP_INFO(get_logger(), "NO FILE EXTENSION FOUND!" ); + } + + return file_extension; +} + +void PrimitiveFitterNode::update_inertia(std::string in_filename, + std::string out_filename, + bool visual) +{ + Urdf urdf(in_filename.c_str(), false, shr_ptr_to_this_); + + // Return if the urdf was not loaded + if ( !urdf.loaded() ) + { + RCLCPP_ERROR(get_logger(), "URDF could not be loaded." ); + return; + } + + bool hasMoreMesh; + + if (visual) + { + hasMoreMesh = urdf.hasMoreVisualMesh(); + } + else + { + hasMoreMesh = urdf.hasMoreMesh(); + } + + while (hasMoreMesh) + { + std::string meshfile; + + if (visual) + { + meshfile = urdf.getNextVisualMesh(); + hasMoreMesh = urdf.hasMoreVisualMesh(); + } + else + { + meshfile = urdf.getNextMesh(); + hasMoreMesh = urdf.hasMoreMesh(); + } + + std::string file_extension = getFileExtension(meshfile); + + if (file_extension == "stl" || file_extension == "STL") + { + RCLCPP_DEBUG(get_logger(), "Handling STL file ..." ); + RCLCPP_DEBUG_STREAM(get_logger(), "path: " << meshfile ); + + // get global meshfile path + url::Url filepath(meshfile); + std::string pkgName(filepath.getPackageName()); + // may throw ament_index_cpp::PackageNotFoundError exception + std::string pkgPath = + ament_index_cpp::get_package_share_directory(pkgName); + + std::string path(filepath.getRelativePath()); + std::string global_meshfile((pkgPath + path).c_str()); + RCLCPP_INFO_STREAM(get_logger(), "global path: " << global_meshfile ); + + // open STL file + MyMesh mesh; + int mask = 0; + + if(vcg::tri::io::ImporterSTL::Open(mesh, global_meshfile.c_str(), mask)) + { + RCLCPP_INFO_STREAM(get_logger(), "Error reading file " << global_meshfile ); + return; + } + + // read mass from file + float mass = urdf.getMass(visual); + + // compute inertia and center of mass + vcg::Matrix33f Inertia; + vcg::tri::Inertia I(mesh); + I.InertiaTensor(Inertia); + vcg::Point3f CenterOfMass = I.CenterOfMass(); + + // scale inertia according to mass + float volume = I.Mass(); + Inertia /= volume; + Inertia *= mass; + + // set new values + urdf.setCenterOfMass(CenterOfMass, visual); + urdf.setInertia(Inertia, visual); + } + else + { + RCLCPP_ERROR(this->get_logger(), "UNKNOWN FILE EXTENSION! (only .stl supported)" ); + return; + } + } + + if (urdf.save(out_filename.c_str())) + { + RCLCPP_INFO(get_logger(), "Inertia updated successfully!" ); + } + + return; +} + +void PrimitiveFitterNode::main(void) { + RCLCPP_INFO(this->get_logger(), "step 1 " ); + std::string in_filename; + try { + in_filename = + this->get_parameter("urdf_filename") + .as_string(); + } catch (...) { + std::cerr << "Unable to find parameter urdf_filename" << std::endl; + return; + } + + RCLCPP_INFO(this->get_logger(), "step 2 "); + std::string out_filename(""); + out_filename = this->get_parameter("output_filename").as_string(); + + RCLCPP_INFO(this->get_logger(), "step 3 " ); + if(out_filename.empty()) + { + size_t lastindex = in_filename.find_last_of("."); + out_filename = in_filename.substr(0, lastindex) + "_optimized.urdf.xacro"; + } + + RCLCPP_INFO(this->get_logger(), "step 4 " ); + bool fitter = true; + fitter = this->get_parameter("use_fitter").as_bool(); + + bool inertia = false; + inertia = this->get_parameter("use_inertia").as_bool(); + + RCLCPP_INFO(this->get_logger(), "step 5 " ); + std::string capsule_filename(""); + + if (inertia) + { + RCLCPP_INFO(this->get_logger(), + "Inertia calculation started."); + + std::string mesh_type = "visual"; + bool visual = true; + mesh_type = this->get_parameter("mesh_type").as_string(); + + if (mesh_type == "visual") + { + visual = true; + RCLCPP_INFO(this->get_logger(),"Using visual meshes."); + } + else if (mesh_type == "collision") + { + visual = false; + RCLCPP_INFO(this->get_logger(), + "Using collision meshes."); + } + else + { + RCLCPP_WARN(this->get_logger(), + "PLEASE SET ROSPARAM /nimbro_primitive_fitter/mesh_type" + " TO EITHER visual OR collsion -> Using 'visual'."); + visual = true; + } + + update_inertia(in_filename, out_filename, visual); + in_filename = out_filename; + } + + if (fitter) + { + std::string shape = "box"; + shape = this->get_parameter("fit_shape").as_string(); + VersatileFitter::Shape vf_shape; + + if (shape == "box" || shape == "Box") + { + RCLCPP_INFO(this->get_logger(), + "Will try to fit URDF to Box."); + vf_shape = VersatileFitter::Shape::Box; + } + else if (shape == "capsule" || shape == "Capsule") + { + RCLCPP_INFO(this->get_logger(), + "Will try to fit URDF to Capsule."); + vf_shape = VersatileFitter::Shape::Capsule; + } + else + { + RCLCPP_ERROR(this->get_logger(), + "PLEASE SET ROSPARAM /nimbro_primitive_fitter/fit_shape TO EITHER box OR capsule"); + RCLCPP_INFO(this->get_logger(),"Will try to fit URDF to Box."); + vf_shape = VersatileFitter::Shape::Box; + } + + VersatileFitter vf(shr_ptr_to_this_); + vf.fit(in_filename, out_filename, vf_shape, shape); + } +};