From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Wed, 12 Dec 2018 14:13:33 +0000 (-0200)
Subject: Fix bug in context of tree and chain in wam_node_sim.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=b328b647ab94a32361a5ddc9635f5ca22b16b302;p=ufrgs_wam.git

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
--- 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
--- 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_;