From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Sun, 26 Sep 2021 07:28:17 +0000 (-0300)
Subject: Change error processing to use exceptions.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=76ad5bcca292a298ac5cf3fd152d683207f48ea0;p=pid_plus_gravity_controller.git

Change error processing to use exceptions.
---

diff --git a/src/pid_plus_gravity_controller.cpp b/src/pid_plus_gravity_controller.cpp
index 1834e8e..172af9c 100644
--- a/src/pid_plus_gravity_controller.cpp
+++ b/src/pid_plus_gravity_controller.cpp
@@ -101,52 +101,40 @@ 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
-		{
 			chainParam_=std::make_unique<KDL::ChainDynParam>(chain_,g);
+
+			q_.resize(chain_.getNrOfJoints());
+			qr_.resize(chain_.getNrOfJoints());
+			gravity_.resize(chain_.getNrOfJoints());
 		}
 		catch(const std::exception &e)
 		{
-			RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception thrown while creating ChainDynParam: " << e.what());
+			RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception in on_configure(): " << e.what());
 			return CallbackReturn::ERROR;
 		}
 
-		q_.resize(chain_.getNrOfJoints());
-		qr_.resize(chain_.getNrOfJoints());
-		gravity_.resize(chain_.getNrOfJoints());
-
 		if(!node_->get_parameter("priority",priority_))
 			RCLCPP_WARN(node_->get_logger(),"No 'priority' configured for controller. Using highest possible priority.");