Fix iTaSC build error when building with libc++.
authorBrecht Van Lommel <brechtvanlommel@gmail.com>
Thu, 28 Nov 2013 20:26:55 +0000 (21:26 +0100)
committerBrecht Van Lommel <brechtvanlommel@gmail.com>
Thu, 28 Nov 2013 20:35:28 +0000 (21:35 +0100)
This was using TreeElement before it was fully defined, which gives undefined
behavior that happened to work with other libraries but not libc++.

Based on patch by Marcus von Appen, modifications for brevity and to ensure we
don't dereference invalid memory. Ref T37477.

intern/itasc/Armature.cpp
intern/itasc/kdl/tree.cpp
intern/itasc/kdl/tree.hpp
intern/itasc/kdl/treefksolverpos_recursive.cpp
intern/itasc/kdl/treefksolverpos_recursive.hpp
intern/itasc/kdl/treejnttojacsolver.cpp

index e6f5fda406620a02f078ef44a6250996a0a1fa44..0c736566b2523181e88865c43a58fec8d5ba6dab 100644 (file)
@@ -402,7 +402,7 @@ bool Armature::finalize()
        m_armlength = 0.0;
        for (i=0; i<m_neffector; i++) {
                length = 0.0;
-               KDL::SegmentMap::const_iterator sit = m_tree.getSegment(m_effectors[i].name);
+               KDL::SegmentMap::value_type const *sit = m_tree.getSegmentPtr(m_effectors[i].name);
                while (sit->first != "root") {
                        Frame tip = sit->second.segment.pose(m_qKdl(sit->second.q_nr));
                        length += tip.p.Norm();
index 8776e43f5b454a4082d98066acecbc24fc743fb6..a31ac79bdf5b7a800e4b484e338fe186e75da038 100644 (file)
@@ -64,7 +64,7 @@ bool Tree::addSegment(const Segment& segment, const std::string& segment_name,
         return false;
     pair<SegmentMap::iterator, bool> retval;
     //insert new element
-       TreeElement elem(segment, parent, nrOfJoints);
+       TreeElement elem(segment, *parent, nrOfJoints);
        std::pair<std::string, TreeElement> val(segment_name, elem);
 
     retval = segments.insert(val);
index 82794f96b94122a997955cd5b90951ce1233148c..8f97120096966f334bd38a1074ac4d3afdeb55c5 100644 (file)
@@ -41,32 +41,28 @@ namespace KDL
 {
     //Forward declaration
     class TreeElement;
-#if defined(__APPLE__)
-#  if MAC_OS_X_VERSION_MIN_REQUIRED <= 1050
+#if defined(__APPLE__) && MAC_OS_X_VERSION_MIN_REQUIRED <= 1050
     typedef std::map<std::string,TreeElement> SegmentMap;
-#  else
-    // Eigen allocator is needed for alignment of Eigen data types
-    typedef std::map<std::string,TreeElement, std::less<std::string>, Eigen::aligned_allocator<std::pair<std::string, TreeElement> > > SegmentMap;
-#  endif /* MAC_OS_X_VERSION_MIN_REQUIRED */
 #else
     // Eigen allocator is needed for alignment of Eigen data types
     typedef std::map<std::string,TreeElement, std::less<std::string>, Eigen::aligned_allocator<std::pair<std::string, TreeElement> > > SegmentMap;
 #endif
+
     class TreeElement
     {
     public:
-        TreeElement():q_nr(0)
+        TreeElement():q_nr(0),parent(0)
         {};
     public:
         Segment segment;
         unsigned int q_nr;
-        SegmentMap::const_iterator  parent;
+        SegmentMap::value_type const *parent;
         std::vector<SegmentMap::const_iterator > children;
-        TreeElement(const Segment& segment_in,const SegmentMap::const_iterator& parent_in,unsigned int q_nr_in)
+        TreeElement(const Segment& segment_in,const SegmentMap::value_type& parent_in,unsigned int q_nr_in)
         {
                        q_nr=q_nr_in;
             segment=segment_in;
-            parent=parent_in;
+            parent=&parent_in;
         };
         static TreeElement Root()
         {
@@ -167,7 +163,15 @@ namespace KDL
             return segments.find(segment_name);
         };
 
+        SegmentMap::value_type const* getSegmentPtr(const std::string& segment_name)const
+        {
+            SegmentMap::const_iterator it = segments.find(segment_name);
+            
+            if (it == segments.end())
+                return 0;
 
+            return &*it;
+        };
 
         const SegmentMap& getSegments()const
         {
index 9103a2165bbe3a1f5e84ed4bd002eaca46890755..fd78c46f837b746f67f222e62519fd915be3093c 100644 (file)
@@ -35,22 +35,22 @@ namespace KDL {
 
     int TreeFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName)
     {      
-               SegmentMap::const_iterator it = tree.getSegment(segmentName); 
-               SegmentMap::const_iterator baseit = tree.getSegment(baseName); 
+               SegmentMap::value_type const* it = tree.getSegmentPtr(segmentName); 
+               SegmentMap::value_type const* baseit = tree.getSegmentPtr(baseName); 
         
         if(q_in.rows() != tree.getNrOfJoints())
                return -1;
-        else if(it == tree.getSegments().end()) //if the segment name is not found
+        else if(!it) //if the segment name is not found
                return -2;
-        else if(baseit == tree.getSegments().end()) //if the base segment name is not found
+        else if(!baseit) //if the base segment name is not found
                return -3;
         else{
-                       p_out = recursiveFk(q_in, it, baseit);  
-               return 0;               
+                       p_out = recursiveFk(q_in, it, baseit);
+               return 0;
         }
     }
 
-       Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it, const SegmentMap::const_iterator& baseit)
+       Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, SegmentMap::value_type const* it, SegmentMap::value_type const* baseit)
        {
                //gets the frame for the current element (segment)
                const TreeElement& currentElement = it->second;
@@ -60,8 +60,7 @@ namespace KDL {
                }
                else{
                        Frame currentFrame = currentElement.segment.pose(((JntArray&)q_in)(currentElement.q_nr));
-                       SegmentMap::const_iterator parentIt = currentElement.parent;
-                       return recursiveFk(q_in, parentIt, baseit) * currentFrame;
+                       return recursiveFk(q_in, currentElement.parent, baseit) * currentFrame;
                }
        }
 
index c22fe4af75b7fbbc7bf9a48a90b4f800c29cd645..2081f23a9ffd591e88d4eb4ada4c70ac0088d45d 100644 (file)
@@ -45,7 +45,7 @@ namespace KDL {
     private:
         const Tree tree;
         
-        Frame recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it, const SegmentMap::const_iterator& baseit);
+        Frame recursiveFk(const JntArray& q_in, SegmentMap::value_type const* it, SegmentMap::value_type const* baseit);
     };
 
 }
index 624bbef79909de95cb62c1e9d52d081de8d9f71c..e8b4d385ab2558891a9ef24e39a7717e7a44cbef 100644 (file)
@@ -28,16 +28,16 @@ int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac,
         return -1;
 
     //Lets search the tree-element
-    SegmentMap::const_iterator it = tree.getSegments().find(segmentname);
+    SegmentMap::value_type const* it = tree.getSegmentPtr(segmentname);
 
     //If segmentname is not inside the tree, back out:
-    if (it == tree.getSegments().end())
+    if (!it)
         return -2;
 
     //Let's make the jacobian zero:
     SetToZero(jac);
 
-    SegmentMap::const_iterator root = tree.getSegments().find("root");
+    SegmentMap::value_type const* root = tree.getSegmentPtr("root");
 
     Frame T_total = Frame::Identity();
        Frame T_local, T_joint;