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