From 07221fdc645b76874ebba39ea5b438c8e1b5988f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 29 Mar 2018 16:24:04 -0400 Subject: [PATCH] Style fixes from ros2 (#11) * Style fixes (these can go to upstream kdl_parser) Signed-off-by: Chris Lalancette * Windows compatibility. Signed-off-by: Chris Lalancette --- kdl_parser/CMakeLists.txt | 4 + kdl_parser/include/kdl_parser/kdl_parser.hpp | 38 ++-- .../include/kdl_parser/visibility_control.hpp | 76 ++++++++ kdl_parser/src/check_kdl_parser.cpp | 57 +++--- kdl_parser/src/kdl_parser.cpp | 166 ++++++++++-------- kdl_parser/test/test_inertia_rpy.cpp | 62 +++++-- kdl_parser/test/test_kdl_parser.cpp | 31 ++-- 7 files changed, 275 insertions(+), 159 deletions(-) create mode 100644 kdl_parser/include/kdl_parser/visibility_control.hpp diff --git a/kdl_parser/CMakeLists.txt b/kdl_parser/CMakeLists.txt index 95d48d0..b9d649a 100644 --- a/kdl_parser/CMakeLists.txt +++ b/kdl_parser/CMakeLists.txt @@ -30,6 +30,10 @@ target_link_libraries(${PROJECT_NAME} ${TinyXML_LIBRARIES} ${orocos_kdl_LIBRARIES} ${catkin_LIBRARIES} ) +if(WIN32) + target_compile_definitions(${PROJECT_NAME} PRIVATE "KDL_PARSER_BUILDING_DLL") +endif() + add_executable(check_kdl_parser src/check_kdl_parser.cpp ) target_link_libraries(check_kdl_parser ${PROJECT_NAME}) diff --git a/kdl_parser/include/kdl_parser/kdl_parser.hpp b/kdl_parser/include/kdl_parser/kdl_parser.hpp index 68a6456..8d46a4e 100644 --- a/kdl_parser/include/kdl_parser/kdl_parser.hpp +++ b/kdl_parser/include/kdl_parser/kdl_parser.hpp @@ -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: -* +* * * 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 @@ -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 @@ -34,50 +34,58 @@ /* Author: Wim Meeussen */ -#ifndef KDL_PARSER_H -#define KDL_PARSER_H +#ifndef KDL_PARSER__KDL_PARSER_HPP_ +#define KDL_PARSER__KDL_PARSER_HPP_ #include #include #include -#include +#include // NOLINT -namespace kdl_parser{ +#include "kdl_parser/visibility_control.hpp" + +namespace kdl_parser +{ /** Constructs a KDL tree from a file, given the file name * \param file The filename from where to read the xml * \param tree The resulting KDL Tree * returns true on success, false on failure */ -bool treeFromFile(const std::string& file, KDL::Tree& tree); +KDL_PARSER_PUBLIC +bool treeFromFile(const std::string & file, KDL::Tree & tree); /** Constructs a KDL tree from the parameter server, given the parameter name * \param param the name of the parameter on the parameter server * \param tree The resulting KDL Tree * returns true on success, false on failure */ -bool treeFromParam(const std::string& param, KDL::Tree& tree); +KDL_PARSER_PUBLIC +bool treeFromParam(const std::string & param, KDL::Tree & tree); /** Constructs a KDL tree from a string containing xml * \param xml A string containting the xml description of the robot * \param tree The resulting KDL Tree * returns true on success, false on failure */ -bool treeFromString(const std::string& xml, KDL::Tree& tree); +KDL_PARSER_PUBLIC +bool treeFromString(const std::string & xml, KDL::Tree & tree); /** Constructs a KDL tree from a TiXmlDocument * \param xml_doc The TiXmlDocument containting the xml description of the robot * \param tree The resulting KDL Tree * returns true on success, false on failure */ -bool treeFromXml(TiXmlDocument *xml_doc, KDL::Tree& tree); +KDL_PARSER_PUBLIC +bool treeFromXml(TiXmlDocument * xml_doc, KDL::Tree & tree); /** Constructs a KDL tree from a URDF robot model * \param robot_model The URDF robot model * \param tree The resulting KDL Tree * returns true on success, false on failure */ -bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, KDL::Tree& tree); -} +KDL_PARSER_PUBLIC +bool treeFromUrdfModel(const urdf::ModelInterface & robot_model, KDL::Tree & tree); +} // namespace kdl_parser -#endif +#endif // KDL_PARSER__KDL_PARSER_HPP_ diff --git a/kdl_parser/include/kdl_parser/visibility_control.hpp b/kdl_parser/include/kdl_parser/visibility_control.hpp new file mode 100644 index 0000000..2e2a5f8 --- /dev/null +++ b/kdl_parser/include/kdl_parser/visibility_control.hpp @@ -0,0 +1,76 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2017, Open Source Robotics Foundation, 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 copyright holder 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. +*********************************************************************/ + +/* This header must be included by all kdl_parser headers which declare symbols + * which are defined in the kdl_parser library. When not building the kdl_parser + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the kdl_parser + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef KDL_PARSER__VISIBILITY_CONTROL_HPP_ +#define KDL_PARSER__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define KDL_PARSER_EXPORT __attribute__ ((dllexport)) + #define KDL_PARSER_IMPORT __attribute__ ((dllimport)) + #else + #define KDL_PARSER_EXPORT __declspec(dllexport) + #define KDL_PARSER_IMPORT __declspec(dllimport) + #endif + #ifdef KDL_PARSER_BUILDING_DLL + #define KDL_PARSER_PUBLIC KDL_PARSER_EXPORT + #else + #define KDL_PARSER_PUBLIC KDL_PARSER_IMPORT + #endif + #define KDL_PARSER_PUBLIC_TYPE KDL_PARSER_PUBLIC + #define KDL_PARSER_LOCAL +#else + #define KDL_PARSER_EXPORT __attribute__ ((visibility("default"))) + #define KDL_PARSER_IMPORT + #if __GNUC__ >= 4 + #define KDL_PARSER_PUBLIC __attribute__ ((visibility("default"))) + #define KDL_PARSER_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define KDL_PARSER_PUBLIC + #define KDL_PARSER_LOCAL + #endif + #define KDL_PARSER_PUBLIC_TYPE +#endif + +#endif // KDL_PARSER__VISIBILITY_CONTROL_HPP_ diff --git a/kdl_parser/src/check_kdl_parser.cpp b/kdl_parser/src/check_kdl_parser.cpp index 1dd0a6d..bb4cfb1 100644 --- a/kdl_parser/src/check_kdl_parser.cpp +++ b/kdl_parser/src/check_kdl_parser.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: -* +* * * 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 @@ -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 @@ -34,45 +34,46 @@ /* Author: Wim Meeussen */ +#include +#include + #include "kdl_parser/kdl_parser.hpp" #include #include #include -#include -using namespace KDL; -using namespace std; -using namespace urdf; - -void printLink(const SegmentMap::const_iterator& link, const std::string& prefix) +void printLink(const KDL::SegmentMap::const_iterator & link, const std::string & prefix) { - cout << prefix << "- Segment " << GetTreeElementSegment(link->second).getName() << " has " - << GetTreeElementChildren(link->second).size() << " children" << endl; - for (unsigned int i=0; i < GetTreeElementChildren(link->second).size(); i++) - printLink(GetTreeElementChildren(link->second)[i], prefix + " "); + std::cout << prefix << "- Segment " << GetTreeElementSegment(link->second).getName() << + " has " << GetTreeElementChildren(link->second).size() << " children" << std::endl; + for (unsigned int i = 0; i < GetTreeElementChildren(link->second).size(); i++) { + printLink(GetTreeElementChildren(link->second)[i], prefix + " "); + } } -int main(int argc, char** argv) +int main(int argc, char ** argv) { - if (argc < 2){ + if (argc < 2) { std::cerr << "Expect xml file to parse" << std::endl; return -1; } - Model robot_model; - if (!robot_model.initFile(argv[1])) - {cerr << "Could not generate robot model" << endl; return false;} + urdf::Model robot_model; + if (!robot_model.initFile(argv[1])) { + std::cerr << "Could not generate robot model" << std::endl; + return false; + } - Tree my_tree; - if (!kdl_parser::treeFromUrdfModel(robot_model, my_tree)) - {cerr << "Could not extract kdl tree" << endl; return false;} + KDL::Tree my_tree; + if (!kdl_parser::treeFromUrdfModel(robot_model, my_tree)) { + std::cerr << "Could not extract kdl tree" << std::endl; + return false; + } // walk through tree - cout << " ======================================" << endl; - cout << " Tree has " << my_tree.getNrOfSegments() << " link(s) and a root link" << endl; - cout << " ======================================" << endl; - SegmentMap::const_iterator root = my_tree.getRootSegment(); + std::cout << " ======================================" << std::endl; + std::cout << " Tree has " << my_tree.getNrOfSegments() << " link(s) and a root link" << std::endl; + std::cout << " ======================================" << std::endl; + KDL::SegmentMap::const_iterator root = my_tree.getRootSegment(); printLink(root, ""); } - - diff --git a/kdl_parser/src/kdl_parser.cpp b/kdl_parser/src/kdl_parser.cpp index 37ed58c..b7f040a 100644 --- a/kdl_parser/src/kdl_parser.cpp +++ b/kdl_parser/src/kdl_parser.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: -* +* * * 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 @@ -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 @@ -35,132 +35,137 @@ /* Author: Wim Meeussen */ #include "kdl_parser/kdl_parser.hpp" + +#include +#include + #include #include #include #include -using namespace std; -using namespace KDL; - -namespace kdl_parser{ - +namespace kdl_parser +{ // construct vector -Vector toKdl(urdf::Vector3 v) +KDL::Vector toKdl(urdf::Vector3 v) { - return Vector(v.x, v.y, v.z); + return KDL::Vector(v.x, v.y, v.z); } // construct rotation -Rotation toKdl(urdf::Rotation r) +KDL::Rotation toKdl(urdf::Rotation r) { - return Rotation::Quaternion(r.x, r.y, r.z, r.w); + return KDL::Rotation::Quaternion(r.x, r.y, r.z, r.w); } // construct pose -Frame toKdl(urdf::Pose p) +KDL::Frame toKdl(urdf::Pose p) { - return Frame(toKdl(p.rotation), toKdl(p.position)); + return KDL::Frame(toKdl(p.rotation), toKdl(p.position)); } // construct joint -Joint toKdl(urdf::JointSharedPtr jnt) +KDL::Joint toKdl(urdf::JointSharedPtr jnt) { - Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform); + KDL::Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform); - switch (jnt->type){ - case urdf::Joint::FIXED:{ - return Joint(jnt->name, Joint::None); + switch (jnt->type) { + case urdf::Joint::FIXED: { + return KDL::Joint(jnt->name, KDL::Joint::None); + } + case urdf::Joint::REVOLUTE: { + KDL::Vector axis = toKdl(jnt->axis); + return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::RotAxis); + } + case urdf::Joint::CONTINUOUS: { + KDL::Vector axis = toKdl(jnt->axis); + return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::RotAxis); + } + case urdf::Joint::PRISMATIC: { + KDL::Vector axis = toKdl(jnt->axis); + return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::TransAxis); + } + default: { + ROS_WARN("Converting unknown joint type of joint '%s' into a fixed joint", + jnt->name.c_str()); + return KDL::Joint(jnt->name, KDL::Joint::None); + } } - case urdf::Joint::REVOLUTE:{ - Vector axis = toKdl(jnt->axis); - return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis); - } - case urdf::Joint::CONTINUOUS:{ - Vector axis = toKdl(jnt->axis); - return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis); - } - case urdf::Joint::PRISMATIC:{ - Vector axis = toKdl(jnt->axis); - return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::TransAxis); - } - default:{ - ROS_WARN("Converting unknown joint type of joint '%s' into a fixed joint", jnt->name.c_str()); - return Joint(jnt->name, Joint::None); - } - } - return Joint(); + return KDL::Joint(); } // construct inertia -RigidBodyInertia toKdl(urdf::InertialSharedPtr i) +KDL::RigidBodyInertia toKdl(urdf::InertialSharedPtr i) { - Frame origin = toKdl(i->origin); - - // the mass is frame indipendent + KDL::Frame origin = toKdl(i->origin); + + // the mass is frame independent double kdl_mass = i->mass; - + // kdl and urdf both specify the com position in the reference frame of the link - Vector kdl_com = origin.p; - - // kdl specifies the inertia matrix in the reference frame of the link, + KDL::Vector kdl_com = origin.p; + + // kdl specifies the inertia matrix in the reference frame of the link, // while the urdf specifies the inertia matrix in the inertia reference frame - RotationalInertia urdf_inertia = - RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz); - + KDL::RotationalInertia urdf_inertia = + KDL::RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz); + // Rotation operators are not defined for rotational inertia, // so we use the RigidBodyInertia operators (with com = 0) as a workaround - RigidBodyInertia kdl_inertia_wrt_com_workaround = - origin.M *RigidBodyInertia(0, Vector::Zero(), urdf_inertia); - + KDL::RigidBodyInertia kdl_inertia_wrt_com_workaround = + origin.M * KDL::RigidBodyInertia(0, KDL::Vector::Zero(), urdf_inertia); + // Note that the RigidBodyInertia constructor takes the 3d inertia wrt the com // while the getRotationalInertia method returns the 3d inertia wrt the frame origin // (but having com = Vector::Zero() in kdl_inertia_wrt_com_workaround they match) - RotationalInertia kdl_inertia_wrt_com = + KDL::RotationalInertia kdl_inertia_wrt_com = kdl_inertia_wrt_com_workaround.getRotationalInertia(); - - return RigidBodyInertia(kdl_mass,kdl_com,kdl_inertia_wrt_com); + + return KDL::RigidBodyInertia(kdl_mass, kdl_com, kdl_inertia_wrt_com); } // recursive function to walk through tree -bool addChildrenToTree(urdf::LinkConstSharedPtr root, Tree& tree) +bool addChildrenToTree(urdf::LinkConstSharedPtr root, KDL::Tree & tree) { - std::vector children = root->child_links; - ROS_DEBUG("Link %s had %i children", root->name.c_str(), (int)children.size()); + std::vector children = root->child_links; + ROS_DEBUG("Link %s had %zu children", root->name.c_str(), children.size()); // constructs the optional inertia - RigidBodyInertia inert(0); - if (root->inertial) + KDL::RigidBodyInertia inert(0); + if (root->inertial) { inert = toKdl(root->inertial); + } // constructs the kdl joint - Joint jnt = toKdl(root->parent_joint); + KDL::Joint jnt = toKdl(root->parent_joint); // construct the kdl segment - Segment sgm(root->name, jnt, toKdl(root->parent_joint->parent_to_joint_origin_transform), inert); + KDL::Segment sgm(root->name, jnt, toKdl( + root->parent_joint->parent_to_joint_origin_transform), inert); // add segment to tree tree.addSegment(sgm, root->parent_joint->parent_link_name); // recurslively add all children - for (size_t i=0; iname); + tree = KDL::Tree(robot_model.getRoot()->name); // warn if root link has inertia. KDL does not support this - if (robot_model.getRoot()->inertial) - ROS_WARN("The root link %s has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.", robot_model.getRoot()->name.c_str()); + if (robot_model.getRoot()->inertial) { + ROS_WARN("The root link %s has an inertia specified in the URDF, but KDL does not " + "support a root link with an inertia. As a workaround, you can add an extra " + "dummy link to your URDF.", robot_model.getRoot()->name.c_str()); + } // add all children - for (size_t i=0; ichild_links.size(); i++) - if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree)) + for (size_t i = 0; i < robot_model.getRoot()->child_links.size(); i++) { + if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree)) { return false; + } + } return true; } -} - +} // namespace kdl_parser diff --git a/kdl_parser/test/test_inertia_rpy.cpp b/kdl_parser/test/test_inertia_rpy.cpp index 05b12d2..bc32126 100644 --- a/kdl_parser/test/test_inertia_rpy.cpp +++ b/kdl_parser/test/test_inertia_rpy.cpp @@ -1,5 +1,42 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2015 Open Source Robotics Foundation, 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 copyright holder 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: Jackie Kay */ + +#include +#include -#include #include #include #include @@ -8,22 +45,18 @@ #include #include "kdl_parser/kdl_parser.hpp" -using namespace kdl_parser; - int g_argc; -char** g_argv; +char ** g_argv; class TestInertiaRPY : public testing::Test { public: - protected: /// constructor TestInertiaRPY() { } - /// Destructor ~TestInertiaRPY() { @@ -31,10 +64,7 @@ protected: }; -TEST_F(TestInertiaRPY, test_torques) -{ - //ASSERT_EQ(g_argc, 3); - +TEST_F(TestInertiaRPY, test_torques) { // workaround for segfault issue with parsing 2 trees instantiated on the stack KDL::Tree * tree_1 = new KDL::Tree; KDL::Tree * tree_2 = new KDL::Tree; @@ -42,7 +72,7 @@ TEST_F(TestInertiaRPY, test_torques) KDL::JntArray torques_2; { - ASSERT_TRUE(treeFromFile(g_argv[1], *tree_1)); + ASSERT_TRUE(kdl_parser::treeFromFile(g_argv[1], *tree_1)); KDL::Vector gravity(0, 0, -9.81); KDL::Chain chain; std::cout << "number of joints: " << tree_1->getNrOfJoints() << std::endl; @@ -50,11 +80,9 @@ TEST_F(TestInertiaRPY, test_torques) ASSERT_TRUE(tree_1->getChain("base_link", "link2", chain)); KDL::ChainIdSolver_RNE solver(chain, gravity); - //JntArrays get initialized with all 0 values KDL::JntArray q(chain.getNrOfJoints()); KDL::JntArray qdot(chain.getNrOfJoints()); KDL::JntArray qdotdot(chain.getNrOfJoints()); - //KDL::JntArray torques(chain.getNrOfJoints()); std::vector wrenches(chain.getNrOfJoints()); solver.CartToJnt(q, qdot, qdotdot, wrenches, torques_1); @@ -63,17 +91,15 @@ TEST_F(TestInertiaRPY, test_torques) } { - ASSERT_TRUE(treeFromFile(g_argv[2], *tree_2)); + ASSERT_TRUE(kdl_parser::treeFromFile(g_argv[2], *tree_2)); KDL::Vector gravity(0, 0, -9.81); KDL::Chain chain; ASSERT_TRUE(tree_2->getChain("base_link", "link2", chain)); KDL::ChainIdSolver_RNE solver(chain, gravity); - //JntArrays get initialized with all 0 values KDL::JntArray q(chain.getNrOfJoints()); KDL::JntArray qdot(chain.getNrOfJoints()); KDL::JntArray qdotdot(chain.getNrOfJoints()); - //KDL::JntArray torques(chain.getNrOfJoints()); std::vector wrenches(chain.getNrOfJoints()); solver.CartToJnt(q, qdot, qdotdot, wrenches, torques_2); @@ -86,9 +112,7 @@ TEST_F(TestInertiaRPY, test_torques) SUCCEED(); } - - -int main(int argc, char** argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "test_kdl_parser"); diff --git a/kdl_parser/test/test_kdl_parser.cpp b/kdl_parser/test/test_kdl_parser.cpp index ad0bb98..bb90321 100644 --- a/kdl_parser/test/test_kdl_parser.cpp +++ b/kdl_parser/test/test_kdl_parser.cpp @@ -34,15 +34,12 @@ /* Author: Wim Meeussen */ -#include #include #include #include "kdl_parser/kdl_parser.hpp" -using namespace kdl_parser; - int g_argc; -char** g_argv; +char ** g_argv; class TestParser : public testing::Test { @@ -55,7 +52,6 @@ protected: { } - /// Destructor ~TestParser() { @@ -63,28 +59,25 @@ protected: }; -TEST_F(TestParser, test) -{ - for (int i=1; isecond.children.size(), (unsigned int)1); + ASSERT_EQ((unsigned int)1, my_tree.getRootSegment()->second.children.size()); ASSERT_TRUE(my_tree.getSegment("base_link")->second.parent == my_tree.getRootSegment()); - ASSERT_EQ(my_tree.getSegment("base_link")->second.segment.getInertia().getMass(), 10.0); - ASSERT_NEAR(my_tree.getSegment("base_link")->second.segment.getInertia().getRotationalInertia().data[0], 1.000, 0.001); + ASSERT_EQ(10.0, my_tree.getSegment("base_link")->second.segment.getInertia().getMass()); + ASSERT_NEAR(1.000, my_tree.getSegment( + "base_link")->second.segment.getInertia().getRotationalInertia().data[0], 0.001); SUCCEED(); } - - - -int main(int argc, char** argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "test_kdl_parser");