diff --git a/CMakeLists.txt b/CMakeLists.txt index 4e8c5229..14716275 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,7 +66,7 @@ string(REGEX REPLACE "[^/]+" ".." RELATIVE_PATH_LIBDIR_TO_PREFIX "${CMAKE_INSTAL add_subdirectory(urdf_parser) set(PKG_NAME ${PROJECT_NAME}) -set(PKG_LIBRARIES urdfdom_sensor urdfdom_model_state urdfdom_model urdfdom_world) +set(PKG_LIBRARIES urdfdom_sensor urdfdom_model urdfdom_world) set(PKG_DEPENDS urdfdom_headers) set(PKG_EXPORTS urdfdom) set(cmake_conf_file "cmake/urdfdom-config") @@ -91,7 +91,7 @@ install(FILES package.xml DESTINATION share/${PROJECT_NAME}) # Make the package config file set(PKG_DESC "Unified Robot Description Format") set(PKG_DEPENDS "urdfdom_headers") # make the list separated by spaces instead of ; -set(PKG_URDF_LIBS "-lurdfdom_sensor -lurdfdom_model_state -lurdfdom_model -lurdfdom_world") +set(PKG_URDF_LIBS "-lurdfdom_sensor -lurdfdom_model -lurdfdom_world") set(pkg_conf_file "cmake/pkgconfig/urdfdom.pc") configure_file("${CMAKE_CURRENT_SOURCE_DIR}/${pkg_conf_file}.in" "${CMAKE_BINARY_DIR}/${pkg_conf_file}" @ONLY) install(FILES ${CMAKE_BINARY_DIR}/${pkg_conf_file} diff --git a/urdf_parser/CMakeLists.txt b/urdf_parser/CMakeLists.txt index 5769916a..a0cef4d7 100644 --- a/urdf_parser/CMakeLists.txt +++ b/urdf_parser/CMakeLists.txt @@ -64,13 +64,6 @@ add_urdfdom_library( LINK urdfdom_model) -add_urdfdom_library( - LIBNAME - urdfdom_model_state - SOURCES - src/urdf_model_state.cpp - src/twist.cpp) - add_library(urdf_parser INTERFACE) target_include_directories(urdf_parser INTERFACE "$" @@ -86,10 +79,6 @@ add_executable(check_urdf src/check_urdf.cpp) target_include_directories(check_urdf PUBLIC include) target_link_libraries(check_urdf urdfdom_model urdfdom_world) -# Deprecated executable -add_executable(urdf_to_graphiz src/urdf_to_graphviz.cpp) -target_link_libraries(urdf_to_graphiz urdfdom_model) - add_executable(urdf_to_graphviz src/urdf_to_graphviz.cpp) target_include_directories(urdf_to_graphviz PUBLIC include) target_link_libraries(urdf_to_graphviz urdfdom_model) @@ -108,7 +97,6 @@ endif() INSTALL( TARGETS check_urdf - urdf_to_graphiz urdf_to_graphviz urdf_mem_test RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} @@ -120,7 +108,6 @@ INSTALL( urdfdom_model urdfdom_world urdfdom_sensor - urdfdom_model_state urdf_parser EXPORT urdfdom diff --git a/urdf_parser/include/urdf_parser/urdf_parser.h b/urdf_parser/include/urdf_parser/urdf_parser.h index 933739a5..8e32b0a6 100644 --- a/urdf_parser/include/urdf_parser/urdf_parser.h +++ b/urdf_parser/include/urdf_parser/urdf_parser.h @@ -45,7 +45,6 @@ #include #include #include -#include #include #include @@ -147,30 +146,8 @@ class URDFVersion final } namespace urdf{ - URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDF(const std::string &xml_string); URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDFFile(const std::string &path); - - [[deprecated("File an issue at https://github.com/ros/urdfdom if you rely on this")]] - URDFDOM_DLLAPI tinyxml2::XMLDocument* exportURDF(ModelInterfaceSharedPtr &model); - - [[deprecated("File an issue at https://github.com/ros/urdfdom if you rely on this")]] - URDFDOM_DLLAPI tinyxml2::XMLDocument* exportURDF(const ModelInterface &model); - - [[deprecated("File an issue at https://github.com/ros/urdfdom if you rely on this")]] - URDFDOM_DLLAPI bool parsePose(Pose&, tinyxml2::XMLElement*); - - [[deprecated("File an issue at https://github.com/ros/urdfdom if you rely on this")]] - URDFDOM_DLLAPI bool parseCamera(Camera&, tinyxml2::XMLElement*); - - [[deprecated("File an issue at https://github.com/ros/urdfdom if you rely on this")]] - URDFDOM_DLLAPI bool parseRay(Ray&, tinyxml2::XMLElement*); - - [[deprecated("File an issue at https://github.com/ros/urdfdom if you rely on this")]] - URDFDOM_DLLAPI bool parseSensor(Sensor&, tinyxml2::XMLElement*); - - [[deprecated("File an issue at https://github.com/ros/urdfdom if you rely on this")]] - URDFDOM_DLLAPI bool parseModelState(ModelState&, tinyxml2::XMLElement*); } #endif diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp index 5236668a..6d6407f6 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -303,16 +303,4 @@ tinyxml2::XMLDocument* exportURDFInternal(const ModelInterface &model) return doc; } - -tinyxml2::XMLDocument* exportURDF(const ModelInterface &model) -{ - return exportURDFInternal(model); -} - -tinyxml2::XMLDocument* exportURDF(ModelInterfaceSharedPtr &model) -{ - return exportURDFInternal(*model); -} - - } diff --git a/urdf_parser/src/pose.cpp b/urdf_parser/src/pose.cpp index ce6c92b2..a4a27c75 100644 --- a/urdf_parser/src/pose.cpp +++ b/urdf_parser/src/pose.cpp @@ -121,11 +121,6 @@ bool parsePoseInternal(Pose &pose, tinyxml2::XMLElement* xml) return true; } -bool parsePose(Pose &pose, tinyxml2::XMLElement* xml) -{ - return parsePoseInternal(pose, xml); -} - bool exportPose(Pose &pose, tinyxml2::XMLElement* xml) { tinyxml2::XMLElement* origin = xml->GetDocument()->NewElement("origin"); diff --git a/urdf_parser/src/urdf_model_state.cpp b/urdf_parser/src/urdf_model_state.cpp deleted file mode 100644 index 7f117ad1..00000000 --- a/urdf_parser/src/urdf_model_state.cpp +++ /dev/null @@ -1,152 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: John Hsu */ - - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -namespace urdf{ - -bool parseModelState(ModelState &ms, tinyxml2::XMLElement* config) -{ - ms.clear(); - - const char *name_char = config->Attribute("name"); - if (!name_char) - { - CONSOLE_BRIDGE_logError("No name given for the model_state."); - return false; - } - ms.name = std::string(name_char); - - const char *time_stamp_char = config->Attribute("time_stamp"); - if (time_stamp_char) - { - try { - ms.time_stamp.set(strToDouble(time_stamp_char)); - } catch(std::runtime_error &) { - CONSOLE_BRIDGE_logError("Parsing time stamp [%s] failed", time_stamp_char); - return false; - } - } - - tinyxml2::XMLElement *joint_state_elem = config->FirstChildElement("joint_state"); - if (joint_state_elem) - { - JointStateSharedPtr joint_state; - joint_state.reset(new JointState()); - - const char *joint_char = joint_state_elem->Attribute("joint"); - if (joint_char) - joint_state->joint = std::string(joint_char); - else - { - CONSOLE_BRIDGE_logError("No joint name given for the model_state."); - return false; - } - - // parse position - const char *position_char = joint_state_elem->Attribute("position"); - if (position_char) - { - - std::vector pieces; - urdf::split_string( pieces, position_char, " "); - for (unsigned int i = 0; i < pieces.size(); ++i){ - if (pieces[i] != ""){ - try { - joint_state->position.push_back(strToDouble(pieces[i].c_str())); - } catch(std::runtime_error &) { - throw ParseError("position element ("+ pieces[i] +") is not a valid float"); - } - } - } - } - - // parse velocity - const char *velocity_char = joint_state_elem->Attribute("velocity"); - if (velocity_char) - { - - std::vector pieces; - urdf::split_string( pieces, velocity_char, " "); - for (unsigned int i = 0; i < pieces.size(); ++i){ - if (pieces[i] != ""){ - try { - joint_state->velocity.push_back(strToDouble(pieces[i].c_str())); - } catch(std::runtime_error &) { - throw ParseError("velocity element ("+ pieces[i] +") is not a valid float"); - } - } - } - } - - // parse effort - const char *effort_char = joint_state_elem->Attribute("effort"); - if (effort_char) - { - - std::vector pieces; - urdf::split_string( pieces, effort_char, " "); - for (unsigned int i = 0; i < pieces.size(); ++i){ - if (pieces[i] != ""){ - try { - joint_state->effort.push_back(strToDouble(pieces[i].c_str())); - } catch(std::runtime_error &) { - throw ParseError("effort element ("+ pieces[i] +") is not a valid float"); - } - } - } - } - - // add to vector - ms.joint_states.push_back(joint_state); - } - return false; -} - - - -} diff --git a/urdf_parser/src/urdf_sensor.cpp b/urdf_parser/src/urdf_sensor.cpp index b9ac592b..7ce30d77 100644 --- a/urdf_parser/src/urdf_sensor.cpp +++ b/urdf_parser/src/urdf_sensor.cpp @@ -48,6 +48,7 @@ #include #include "./pose.hpp" +#include "./urdf_sensor.hpp" namespace urdf{ @@ -173,11 +174,6 @@ bool parseCameraInternal(Camera &camera, tinyxml2::XMLElement* config) return true; } -bool parseCamera(Camera &camera, tinyxml2::XMLElement* config) -{ - return parseCameraInternal(camera, config); -} - bool parseRayInternal(Ray &ray, tinyxml2::XMLElement* config) { ray.clear(); @@ -297,11 +293,6 @@ bool parseRayInternal(Ray &ray, tinyxml2::XMLElement* config) return false; } -bool parseRay(Ray &ray, tinyxml2::XMLElement* config) -{ - return parseRayInternal(ray, config); -} - VisualSensorSharedPtr parseVisualSensor(tinyxml2::XMLElement *g) { VisualSensorSharedPtr visual_sensor; @@ -331,40 +322,4 @@ VisualSensorSharedPtr parseVisualSensor(tinyxml2::XMLElement *g) return visual_sensor; } - -bool parseSensor(Sensor &sensor, tinyxml2::XMLElement* config) -{ - sensor.clear(); - - const char *name_char = config->Attribute("name"); - if (!name_char) - { - CONSOLE_BRIDGE_logError("No name given for the sensor."); - return false; - } - sensor.name = std::string(name_char); - - // parse parent_link_name - const char *parent_link_name_char = config->Attribute("parent_link_name"); - if (!parent_link_name_char) - { - CONSOLE_BRIDGE_logError("No parent_link_name given for the sensor."); - return false; - } - sensor.parent_link_name = std::string(parent_link_name_char); - - // parse origin - tinyxml2::XMLElement *o = config->FirstChildElement("origin"); - if (o) - { - if (!parsePoseInternal(sensor.origin, o)) - return false; - } - - // parse sensor - sensor.sensor = parseVisualSensor(config); - return true; -} - - } diff --git a/urdf_parser/src/twist.cpp b/urdf_parser/src/urdf_sensor.hpp similarity index 65% rename from urdf_parser/src/twist.cpp rename to urdf_parser/src/urdf_sensor.hpp index dd88794a..1a232c3b 100644 --- a/urdf_parser/src/twist.cpp +++ b/urdf_parser/src/urdf_sensor.hpp @@ -32,50 +32,12 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: John Hsu */ +/* Author: Wim Meeussen, John Hsu */ - -#include -#include -#include -#include #include -#include - -namespace urdf{ -bool parseTwist(Twist &twist, tinyxml2::XMLElement* xml) -{ - twist.clear(); - if (xml) - { - const char* linear_char = xml->Attribute("linear"); - if (linear_char != NULL) - { - try { - twist.linear.init(linear_char); - } - catch (ParseError &e) { - twist.linear.clear(); - CONSOLE_BRIDGE_logError("Malformed linear string [%s]: %s", linear_char, e.what()); - return false; - } - } +namespace urdf { - const char* angular_char = xml->Attribute("angular"); - if (angular_char != NULL) - { - try { - twist.angular.init(angular_char); - } - catch (ParseError &e) { - twist.angular.clear(); - CONSOLE_BRIDGE_logError("Malformed angular [%s]: %s", angular_char, e.what()); - return false; - } - } - } - return true; -} +URDFDOM_DLLAPI VisualSensorSharedPtr parseVisualSensor(tinyxml2::XMLElement* xml); } diff --git a/urdf_parser/src/urdf_to_graphviz.cpp b/urdf_parser/src/urdf_to_graphviz.cpp index 8e00db2b..e97c3df8 100644 --- a/urdf_parser/src/urdf_to_graphviz.cpp +++ b/urdf_parser/src/urdf_to_graphviz.cpp @@ -1,13 +1,13 @@ /********************************************************************* * Software License Agreement (BSD License) -* +* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redstributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -53,11 +53,11 @@ void addChildJointNames(LinkConstSharedPtr link, ofstream& os) double r, p, y; for (std::vector::const_iterator child = link->child_links.begin(); child != link->child_links.end(); ++child){ (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(r,p,y); - os << "\"" << link->name << "\" -> \"" << (*child)->parent_joint->name + os << "\"" << link->name << "\" -> \"" << (*child)->parent_joint->name << "\" [label=\"xyz: " - << (*child)->parent_joint->parent_to_joint_origin_transform.position.x << " " - << (*child)->parent_joint->parent_to_joint_origin_transform.position.y << " " - << (*child)->parent_joint->parent_to_joint_origin_transform.position.z << " " + << (*child)->parent_joint->parent_to_joint_origin_transform.position.x << " " + << (*child)->parent_joint->parent_to_joint_origin_transform.position.y << " " + << (*child)->parent_joint->parent_to_joint_origin_transform.position.z << " " << "\\nrpy: " << r << " " << p << " " << y << "\"]" << endl; os << "\"" << (*child)->parent_joint->name << "\" -> \"" << (*child)->name << "\"" << endl; addChildJointNames(*child, os); @@ -85,24 +85,12 @@ void printTree(LinkConstSharedPtr link, string file) int main(int argc, char** argv) { - std::string executable_name(argv[0]); - std::string deprecated_name("urdf_to_graphiz"); - if (deprecated_name == executable_name.substr(executable_name.size() - deprecated_name.size())) { - std::cerr << "WARNING: The executable named '" << deprecated_name - << "' is deprecated. Use 'urdf_to_graphviz' instead." << std::endl; - } if (argc < 2 || argc > 3) { std::cerr << "Usage: urdf_to_graphviz input.xml [OUTPUT]" << " Will create either $ROBOT_NAME.gv & $ROBOT_NAME.pdf in CWD" << " or OUTPUT.gv & OUTPUT.pdf." << std::endl; return -1; } - if (argc != 3) { - std::cerr << "WARNING: OUTPUT not given. This type of usage is deprecated!" - << "Usage: urdf_to_graphviz input.xml [OUTPUT]" - << " Will create either $ROBOT_NAME.gv & $ROBOT_NAME.pdf in CWD" - << " or OUTPUT.gv & OUTPUT.pdf." << std::endl; - } // get the entire file std::string xml_string; @@ -136,4 +124,3 @@ int main(int argc, char** argv) cout << "There was an error executing '" << command << "'" << endl; return 0; } -