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 (file)
--- 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 (file)
--- 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_);