Style fixes from ros2 (#11)

* Style fixes (these can go to upstream kdl_parser)

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>

* Windows compatibility.

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>

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})
 

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
+#ifndef KDL_PARSER__KDL_PARSER_HPP_
-#define KDL_PARSER_H
+#define KDL_PARSER__KDL_PARSER_HPP_
 
 #include <kdl/tree.hpp>
 #include <string>
 #include <urdf_model/model.h>
-#include <tinyxml.h>
+#include <tinyxml.h>  // 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_

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_

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 <iostream>
+#include <string>
+
 #include "kdl_parser/kdl_parser.hpp"
 #include <kdl/chainfksolverpos_recursive.hpp>
 #include <kdl/frames_io.hpp>
 #include <urdf/model.h>
-#include <iostream>
 
-using namespace KDL;
+void printLink(const KDL::SegmentMap::const_iterator & link, const std::string & prefix)
-using namespace std;
-using namespace urdf;
-
-void printLink(const SegmentMap::const_iterator& link, const std::string& prefix)
 {
-  cout << prefix << "- Segment " << GetTreeElementSegment(link->second).getName() << " has "
+  std::cout << prefix << "- Segment " << GetTreeElementSegment(link->second).getName() <<
-       << GetTreeElementChildren(link->second).size() << " children" << endl;
+    " has " << GetTreeElementChildren(link->second).size() << " children" << std::endl;
-  for (unsigned int i=0; i < GetTreeElementChildren(link->second).size(); i++)
+  for (unsigned int i = 0; i < GetTreeElementChildren(link->second).size(); i++) {
-      printLink(GetTreeElementChildren(link->second)[i], prefix + "  ");
+    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;
+  urdf::Model robot_model;
-  if (!robot_model.initFile(argv[1]))
+  if (!robot_model.initFile(argv[1])) {
-  {cerr << "Could not generate robot model" << endl; return false;}
+    std::cerr << "Could not generate robot model" << std::endl;
+    return false;
+  }
 
-  Tree my_tree;
+  KDL::Tree my_tree;
-  if (!kdl_parser::treeFromUrdfModel(robot_model, my_tree)) 
+  if (!kdl_parser::treeFromUrdfModel(robot_model, my_tree)) {
-  {cerr << "Could not extract kdl tree" << endl; return false;}
+    std::cerr << "Could not extract kdl tree" << std::endl;
+    return false;
+  }
 
   // walk through tree
-  cout << " ======================================" << endl;
+  std::cout << " ======================================" << std::endl;
-  cout << " Tree has " << my_tree.getNrOfSegments() << " link(s) and a root link" << endl;
+  std::cout << " Tree has " << my_tree.getNrOfSegments() << " link(s) and a root link" << std::endl;
-  cout << " ======================================" << endl;
+  std::cout << " ======================================" << std::endl;
-  SegmentMap::const_iterator root = my_tree.getRootSegment();
+  KDL::SegmentMap::const_iterator root = my_tree.getRootSegment();
   printLink(root, "");
 }
-
-

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 <string>
+#include <vector>
+
 #include <urdf/model.h>
 #include <urdf/urdfdom_compatibility.h>
 #include <kdl/frames_io.hpp>
 #include <ros/console.h>
 
-using namespace std;
+namespace kdl_parser
-using namespace KDL;
+{
-
-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){
+  switch (jnt->type) {
-  case urdf::Joint::FIXED:{
+    case urdf::Joint::FIXED: {
-    return Joint(jnt->name, Joint::None);
+        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:{
+  return KDL::Joint();
-    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();
 }
 
 // construct inertia
-RigidBodyInertia toKdl(urdf::InertialSharedPtr i)
+KDL::RigidBodyInertia toKdl(urdf::InertialSharedPtr i)
 {
-  Frame origin = toKdl(i->origin);
+  KDL::Frame origin = toKdl(i->origin);
-  
+
-  // the mass is frame indipendent 
+  // 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::Vector kdl_com = origin.p;
-  
+
-  // kdl specifies the inertia matrix in the reference frame of the link, 
+  // 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 = 
+  KDL::RotationalInertia urdf_inertia =
-    RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz);
+    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 =
+  KDL::RigidBodyInertia kdl_inertia_wrt_com_workaround =
-    origin.M *RigidBodyInertia(0, Vector::Zero(), urdf_inertia);
+    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<urdf::LinkSharedPtr > children = root->child_links;
+  std::vector<urdf::LinkSharedPtr> children = root->child_links;
-  ROS_DEBUG("Link %s had %i children", root->name.c_str(), (int)children.size());
+  ROS_DEBUG("Link %s had %zu children", root->name.c_str(), children.size());
 
   // constructs the optional inertia
-  RigidBodyInertia inert(0);
+  KDL::RigidBodyInertia inert(0);
-  if (root->inertial) 
+  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; i<children.size(); i++){
+  for (size_t i = 0; i < children.size(); i++) {
-    if (!addChildrenToTree(children[i], tree))
+    if (!addChildrenToTree(children[i], tree)) {
       return false;
+    }
   }
   return true;
 }
 
 
-bool treeFromFile(const string& file, Tree& tree)
+bool treeFromFile(const std::string & file, KDL::Tree & tree)
 {
   TiXmlDocument urdf_xml;
   urdf_xml.LoadFile(file);
   return treeFromXml(&urdf_xml, tree);
 }
 
-bool treeFromParam(const string& param, Tree& tree)
+bool treeFromParam(const std::string & param, KDL::Tree & tree)
 {
   urdf::Model robot_model;
   if (!robot_model.initParam(param)){
@@ -170,17 +175,17 @@ bool treeFromParam(const string& param, Tree& tree)
   return treeFromUrdfModel(robot_model, tree);
 }
 
-bool treeFromString(const string& xml, Tree& tree)
+bool treeFromString(const std::string & xml, KDL::Tree & tree)
 {
   TiXmlDocument urdf_xml;
   urdf_xml.Parse(xml.c_str());
   return treeFromXml(&urdf_xml, tree);
 }
 
-bool treeFromXml(TiXmlDocument *xml_doc, Tree& tree)
+bool treeFromXml(TiXmlDocument * xml_doc, KDL::Tree & tree)
 {
   urdf::Model robot_model;
-  if (!robot_model.initXml(xml_doc)){
+  if (!robot_model.initXml(xml_doc)) {
     ROS_ERROR("Could not generate robot model");
     return false;
   }
@@ -188,24 +193,29 @@ bool treeFromXml(TiXmlDocument *xml_doc, Tree& tree)
 }
 
 
-bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree)
+bool treeFromUrdfModel(const urdf::ModelInterface & robot_model, KDL::Tree & tree)
 {
-  if (!robot_model.getRoot())
+  if (!robot_model.getRoot()) {
     return false;
+  }
 
-  tree = Tree(robot_model.getRoot()->name);
+  tree = KDL::Tree(robot_model.getRoot()->name);
 
   // warn if root link has inertia. KDL does not support this
-  if (robot_model.getRoot()->inertial)
+  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());
+    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; i<robot_model.getRoot()->child_links.size(); i++)
+  for (size_t i = 0; i < robot_model.getRoot()->child_links.size(); i++) {
-    if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree))
+    if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree)) {
       return false;
+    }
+  }
 
   return true;
 }
 
-}
+}  // namespace kdl_parser
-

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 <iostream>
+#include <vector>
 
-#include <string>
 #include <gtest/gtest.h>
 #include <kdl/jntarray.hpp>
 #include <kdl/chainidsolver_recursive_newton_euler.hpp>
@@ -8,22 +45,18 @@
 #include <ros/console.h>
 #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)
+TEST_F(TestInertiaRPY, test_torques) {
-{
-  //ASSERT_EQ(g_argc, 3);
-
   // 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<KDL::Wrench> 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<KDL::Wrench> 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");

kdl_parser/test/test_kdl_parser.cpp

@@ -34,15 +34,12 @@
 
 /* Author: Wim Meeussen */
 
-#include <string>
 #include <gtest/gtest.h>
 #include <ros/ros.h>
 #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)
+TEST_F(TestParser, test) {
-{
+  for (int i = 1; i < g_argc - 2; i++) {
-  for (int i=1; i<g_argc-2; i++){
     ROS_ERROR("Testing file %s", g_argv[i]);
-    ASSERT_FALSE(treeFromFile(g_argv[i], my_tree));
+    ASSERT_FALSE(kdl_parser::treeFromFile(g_argv[i], my_tree));
   }
 
-  ASSERT_TRUE(treeFromFile(g_argv[g_argc-1], my_tree));
+  ASSERT_TRUE(kdl_parser::treeFromFile(g_argv[g_argc - 1], my_tree));
-  ASSERT_EQ(my_tree.getNrOfJoints(), 8);
+  ASSERT_EQ(8, my_tree.getNrOfJoints());
-  ASSERT_EQ(my_tree.getNrOfSegments(), 16);
+  ASSERT_EQ(16, my_tree.getNrOfSegments());
   ASSERT_TRUE(my_tree.getSegment("dummy_link") == my_tree.getRootSegment());
-  ASSERT_EQ(my_tree.getRootSegment()->second.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_EQ(10.0, my_tree.getSegment("base_link")->second.segment.getInertia().getMass());
-  ASSERT_NEAR(my_tree.getSegment("base_link")->second.segment.getInertia().getRotationalInertia().data[0], 1.000, 0.001);
+  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");