diff --git a/urdf/CMakeLists.txt b/urdf/CMakeLists.txt index 1576919..76c8c7f 100644 --- a/urdf/CMakeLists.txt +++ b/urdf/CMakeLists.txt @@ -30,6 +30,10 @@ rosbuild_add_boost_directories() rosbuild_add_executable(urdf_check src/urdf_check.cpp) target_link_libraries(urdf_check ${PROJECT_NAME}) +rosbuild_add_executable(mem_test test/memtest.cpp) +target_link_libraries(mem_test ${PROJECT_NAME}) + + rosbuild_add_executable(test_parser EXCLUDE_FROM_ALL test/test_robot_model_parser.cpp) rosbuild_add_gtest_build_flags(test_parser) target_link_libraries(test_parser ${PROJECT_NAME}) diff --git a/urdf/include/urdf/link.h b/urdf/include/urdf/link.h index b5722fa..6dc90e2 100644 --- a/urdf/include/urdf/link.h +++ b/urdf/include/urdf/link.h @@ -206,12 +206,16 @@ public: /// every link can have one parent boost::shared_ptr parent_joint; /// Get Parent Link throught the Parent Joint - boost::shared_ptr parent_link; + boost::shared_ptr parent_link __attribute__((deprecated)); // use getParent() instead std::vector > child_joints; std::vector > child_links; bool initXml(TiXmlElement* config); + + boost::shared_ptr getParent() const + {return parent_link_.lock();}; + void setParent(boost::shared_ptr parent); void clear() @@ -228,6 +232,10 @@ public: void setParentJoint(boost::shared_ptr child); void addChild(boost::shared_ptr child); void addChildJoint(boost::shared_ptr child); + +private: + boost::weak_ptr parent_link_; + }; diff --git a/urdf/src/link.cpp b/urdf/src/link.cpp index a979a67..1d56b8e 100644 --- a/urdf/src/link.cpp +++ b/urdf/src/link.cpp @@ -407,6 +407,7 @@ bool Link::initXml(TiXmlElement* config) void Link::setParent(boost::shared_ptr parent) { this->parent_link = parent; + this->parent_link_ = parent; ROS_DEBUG("set parent Link '%s' for Link '%s'", parent->name.c_str(), this->name.c_str()); } diff --git a/urdf/test/memtest.cpp b/urdf/test/memtest.cpp new file mode 100644 index 0000000..e699d54 --- /dev/null +++ b/urdf/test/memtest.cpp @@ -0,0 +1,10 @@ +#include +#include + +int main(int argc, char** argv){ + ros::init(argc, argv, "memtest"); + while (ros::ok()){ + urdf::Model urdf; + urdf.initFile(std::string(argv[1])); + } +}