From 94d20f7b95a17ebb0e52bb785f9ba0398c77f72c Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Thu, 23 Sep 2021 17:55:59 -0300 Subject: [PATCH] Change solver pointer to a smart pointer. --- .../computed_torque_controller/computed_torque_controller.hpp | 2 +- src/computed_torque_controller.cpp | 10 +++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/include/computed_torque_controller/computed_torque_controller.hpp b/include/computed_torque_controller/computed_torque_controller.hpp index afe8ef0..14ce4d1 100644 --- a/include/computed_torque_controller/computed_torque_controller.hpp +++ b/include/computed_torque_controller/computed_torque_controller.hpp @@ -47,7 +47,7 @@ namespace effort_controllers std::string robotDescription_; KDL::Chain chain_; - KDL::ChainIdSolver_RNE *idsolver_; + std::unique_ptr idsolver_; KDL::JntArray q_; KDL::JntArray dq_; diff --git a/src/computed_torque_controller.cpp b/src/computed_torque_controller.cpp index 3530442..f6ed0e2 100644 --- a/src/computed_torque_controller.cpp +++ b/src/computed_torque_controller.cpp @@ -54,7 +54,7 @@ namespace effort_controllers } catch(const std::exception &e) { - RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception thrown in on_init() with message: " << e.what() << "\n"); + RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception thrown in on_init() with message: " << e.what()); return CallbackReturn::ERROR; } @@ -121,9 +121,13 @@ namespace effort_controllers g[1]=node_->get_parameter("gravity.y").get_value(); g[2]=node_->get_parameter("gravity.x").get_value(); - if((idsolver_=new KDL::ChainIdSolver_RNE(chain_,g)) == NULL) + try + { + idsolver_=std::make_unique(chain_,g); + } + catch(const std::exception &e) { - RCLCPP_ERROR(node_->get_logger(),"Failed to create ChainIDSolver_RNE."); + RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception thrown while creating ChainIdSolver_RNE with message: " << e.what()); return CallbackReturn::ERROR; } -- 2.12.0