Fix bug in context of tree and chain in wam_node_sim.

diff --git a/wam_node_sim/include/wam_node_sim/detail/wam-impl.h b/wam_node_sim/include/wam_node_sim/detail/wam-impl.h
index f955a0f..5b231e1 100644 (file)
--- a/wam_node_sim/include/wam_node_sim/detail/wam-impl.h
+++ b/wam_node_sim/include/wam_node_sim/detail/wam-impl.h
@@ -71,17 +71,16 @@ Wam<DOF>::Wam(Hand *hand,const std::string& sysName)
         if(!node_.getParam("/robot_description",robotDescription))
                 ROS_ERROR("Could not find 'robot_description'.");
                 
-        KDL::Tree tree;
-        if (!kdl_parser::treeFromString(robotDescription,tree))
+        if (!kdl_parser::treeFromString(robotDescription,tree_))
                 ROS_ERROR("Failed to construct KDL tree.");
                 
                 
-       KDL::SegmentMap::const_iterator s=tree.getRootSegment();
-        KDL::Chain chain;
-        if (!tree.getChain(GetTreeElementSegment(s->second).getName(),"wam_tool_plate",chain)) 
+       KDL::SegmentMap::const_iterator s=tree_.getRootSegment();
+
+        if (!tree_.getChain(GetTreeElementSegment(s->second).getName(),"wam_tool_plate",chain_)) 
                 ROS_ERROR("Failed to get chain from KDL tree.");
 
-        fwdKinSolverPos_=new KDL::ChainFkSolverPos_recursive(chain);
+        fwdKinSolverPos_=new KDL::ChainFkSolverPos_recursive(chain_);
 }
 
 template<size_t DOF>

diff --git a/wam_node_sim/include/wam_node_sim/wam.h b/wam_node_sim/include/wam_node_sim/wam.h
index c9b5259..3a40716 100644 (file)
--- a/wam_node_sim/include/wam_node_sim/wam.h
+++ b/wam_node_sim/include/wam_node_sim/wam.h
@@ -53,6 +53,7 @@
 #include "trajectory_msgs/JointTrajectoryPoint.h"
 #include <sensor_msgs/JointState.h>
 
+#include <kdl/tree.hpp>
 #include <kdl/chainfksolverpos_recursive.hpp>
 #include <wam_node_sim/hand.h>
 
@@ -185,6 +186,8 @@ private:
        jp_type jointPosition_;
        jv_type jointVelocity_;
        jt_type jointEffort_;
+        KDL::Tree tree_;
+        KDL::Chain chain_;
        KDL::ChainFkSolverPos_recursive *fwdKinSolverPos_;
        Hand *hand_;