Change error processing to use execptions.

diff --git a/src/computed_torque_controller.cpp b/src/computed_torque_controller.cpp
index 311b01e..f4a8ee5 100644 (file)
--- 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;