From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Wed, 12 Dec 2018 18:05:22 +0000 (-0200)
Subject: Fix bug in scope of tree and chain.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=fd6b8c81adce6b33239aef9b761cfa6c92e98137;p=computed_torque_controller.git

Fix bug in scope of tree and chain.
---

diff --git a/include/computed_torque_controller/computed_torque_controller.h b/include/computed_torque_controller/computed_torque_controller.h
index d9ffae3..5a8d61a 100644
--- a/include/computed_torque_controller/computed_torque_controller.h
+++ b/include/computed_torque_controller/computed_torque_controller.h
@@ -49,7 +49,9 @@ namespace effort_controllers
 		int nJoints_;
 
 		ros::Subscriber sub_command_;
-			
+		
+		KDL::Tree tree_;
+		KDL::Chain chain_;	
 		KDL::ChainIdSolver_RNE *idsolver_;
 				
 		KDL::JntArray q_;
diff --git a/src/computed_torque_controller.cpp b/src/computed_torque_controller.cpp
index d4445c0..0dc4574 100644
--- a/src/computed_torque_controller.cpp
+++ b/src/computed_torque_controller.cpp
@@ -74,8 +74,7 @@ namespace effort_controllers
 			return false;
 		}
 		
-		KDL::Tree tree;
-		if (!kdl_parser::treeFromString(robot_desc_string,tree))
+		if (!kdl_parser::treeFromString(robot_desc_string,tree_))
 		{
 			ROS_ERROR("Failed to construct KDL tree.");
 			return false;
@@ -95,8 +94,7 @@ namespace effort_controllers
 			return false;
 		}
 		
-		KDL::Chain chain;
-		if (!tree.getChain(chainRoot,chainTip,chain)) 
+		if (!tree_.getChain(chainRoot,chainTip,chain_)) 
 		{
 			ROS_ERROR("Failed to get chain from KDL tree.");
 			return false;
@@ -107,7 +105,7 @@ namespace effort_controllers
 		node_.param("gravity/y",g[1],0.0);
 		node_.param("gravity/z",g[2],-9.8);
 
-		if((idsolver_=new KDL::ChainIdSolver_RNE(chain,g)) == NULL)
+		if((idsolver_=new KDL::ChainIdSolver_RNE(chain_,g)) == NULL)
 		{
 			ROS_ERROR("Failed to create ChainIDSolver_RNE.");
 			return false;
@@ -121,7 +119,7 @@ namespace effort_controllers
 		ddqr_.resize(nJoints_);
 		torque_.resize(nJoints_);
 
-		fext_.resize(chain.getNrOfSegments());
+		fext_.resize(chain_.getNrOfSegments());
 		
 		Kp_.resize(nJoints_,nJoints_);
 		Kd_.resize(nJoints_,nJoints_);