From 429108aeb4a53fcae33b64016efb7394c2548552 Mon Sep 17 00:00:00 2001
From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Sun, 26 Sep 2021 05:02:32 -0300
Subject: [PATCH] Change error processing to use execptions.

---
 src/computed_torque_controller.cpp | 103 +++++++++++++++----------------------
 1 file changed, 41 insertions(+), 62 deletions(-)

diff --git a/src/computed_torque_controller.cpp b/src/computed_torque_controller.cpp
index 311b01e..f4a8ee5 100644
--- a/src/computed_torque_controller.cpp
+++ b/src/computed_torque_controller.cpp
@@ -89,76 +89,58 @@ namespace effort_controllers
 		while(robotDescription_.empty())
 			RCLCPP_WARN_SKIPFIRST_THROTTLE(node_->get_logger(),*node_->get_clock(),1000,"Waiting for robot model on /robot_description.");
 
-		KDL::Tree tree;		
-		if (!kdl_parser::treeFromString(robotDescription_,tree))
+		try
 		{
-			RCLCPP_ERROR(node_->get_logger(),"Failed to construct KDL tree.");
-			return CallbackReturn::ERROR;
-		}
+			KDL::Tree tree;		
+			if(!kdl_parser::treeFromString(robotDescription_,tree))
+				throw std::runtime_error("Failed to construct KDL tree.");
 
-		std::string chainRoot=node_->get_parameter("chain.root").as_string();
-		if(chainRoot.empty())
-		{
-			RCLCPP_ERROR(node_->get_logger(),"Could not find 'chain.root' parameter.");
-			return CallbackReturn::ERROR;
-		}
+			std::string chainRoot=node_->get_parameter("chain.root").as_string();
+			if(chainRoot.empty())
+				throw std::runtime_error("Could not find 'chain.root' parameter.");
 
-		std::string chainTip=node_->get_parameter("chain.tip").as_string();
-		if(chainTip.empty())
-		{
-			RCLCPP_ERROR(node_->get_logger(),"Could not find 'chain,tip' parameter.");
-			return CallbackReturn::ERROR;
-		}
+			std::string chainTip=node_->get_parameter("chain.tip").as_string();
+			if(chainTip.empty())
+				throw std::runtime_error("Could not find 'chain,tip' parameter.");
 
-		if (!tree.getChain(chainRoot,chainTip,chain_)) 
-		{
-			RCLCPP_ERROR(node_->get_logger(),"Failed to get chain from KDL tree.");
-			return CallbackReturn::ERROR;
-		}
+			if(!tree.getChain(chainRoot,chainTip,chain_))
+				throw std::runtime_error("Failed to get chain from KDL tree.");
 
-		KDL::Vector g;
-		g[0]=node_->get_parameter("gravity.x").get_value<double>();
-		g[1]=node_->get_parameter("gravity.y").get_value<double>();
-		g[2]=node_->get_parameter("gravity.x").get_value<double>();
+			KDL::Vector g;
+			g[0]=node_->get_parameter("gravity.x").get_value<double>();
+			g[1]=node_->get_parameter("gravity.y").get_value<double>();
+			g[2]=node_->get_parameter("gravity.x").get_value<double>();
 
-		try
-		{
 			idsolver_=std::make_unique<KDL::ChainIdSolver_RNE>(chain_,g);
+		
+			q_.resize(nJoints_);
+			dq_.resize(nJoints_);
+			v_.resize(nJoints_);
+			qr_.resize(nJoints_);
+			dqr_.resize(nJoints_);
+			ddqr_.resize(nJoints_);
+			torque_.resize(nJoints_);
+
+			fext_.resize(chain_.getNrOfSegments());
+
+			Kp_.resize(nJoints_,nJoints_);
+			Kd_.resize(nJoints_,nJoints_);
+
+			std::vector<double> KpVec=node_->get_parameter("Kp").as_double_array();
+			if(KpVec.empty())
+				throw std::runtime_error("No 'Kp' in controller.");
+			Kp_=Eigen::Map<Eigen::MatrixXd>(KpVec.data(),nJoints_,nJoints_).transpose();
+
+			std::vector<double> KdVec=node_->get_parameter("Kd").as_double_array();
+			if(KdVec.empty())
+				throw std::runtime_error("No 'Kd' in controller.");
+			Kd_=Eigen::Map<Eigen::MatrixXd>(KdVec.data(),nJoints_,nJoints_).transpose();
 		}
 		catch(const std::exception &e)
 		{
-			RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception thrown while creating ChainIdSolver_RNE: " << e.what());
-			return CallbackReturn::ERROR;
-		}
-
-		q_.resize(nJoints_);
-		dq_.resize(nJoints_);
-		v_.resize(nJoints_);
-		qr_.resize(nJoints_);
-		dqr_.resize(nJoints_);
-		ddqr_.resize(nJoints_);
-		torque_.resize(nJoints_);
-
-		fext_.resize(chain_.getNrOfSegments());
-
-		Kp_.resize(nJoints_,nJoints_);
-		Kd_.resize(nJoints_,nJoints_);
-
-		std::vector<double> KpVec=node_->get_parameter("Kp").as_double_array();
-		if(KpVec.empty())
-		{
-			RCLCPP_ERROR(node_->get_logger(),"No 'Kp' in controller.");
+			RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception thrown in on_confiture(): " << e.what());
 			return CallbackReturn::ERROR;
 		}
-		Kp_=Eigen::Map<Eigen::MatrixXd>(KpVec.data(),nJoints_,nJoints_).transpose();
-
-		std::vector<double> KdVec=node_->get_parameter("Kd").as_double_array();
-		if(KdVec.empty())
-		{
-			RCLCPP_ERROR(node_->get_logger(),"No 'Kd' in controller.");
-			return CallbackReturn::ERROR;
-		}
-		Kd_=Eigen::Map<Eigen::MatrixXd>(KdVec.data(),nJoints_,nJoints_).transpose();
 
 		if(!node_->get_parameter("priority",priority_))
 			RCLCPP_WARN(node_->get_logger(),"No 'priority' configured for controller. Using highest possible priority.");
@@ -205,11 +187,8 @@ namespace effort_controllers
 		struct sched_param param;
 		param.sched_priority=priority_;
 		if(sched_setscheduler(0,SCHED_FIFO,&param) == -1)
-		{
 			RCLCPP_WARN(node_->get_logger(),"Failed to set real-time scheduler.");
-			return CallbackReturn::SUCCESS;
-		}
-		if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1)
+		else if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1)
 			RCLCPP_WARN(node_->get_logger(),"Failed to lock memory.");
 
 		return CallbackReturn::SUCCESS;
-- 
2.12.0