Change nJoints_ type to unsigned int.

diff --git a/include/computed_torque_controller/computed_torque_controller.hpp b/include/computed_torque_controller/computed_torque_controller.hpp
index f63d7c5..afe8ef0 100644 (file)
--- a/include/computed_torque_controller/computed_torque_controller.hpp
+++ b/include/computed_torque_controller/computed_torque_controller.hpp
@@ -41,7 +41,7 @@ namespace effort_controllers
        class ComputedTorqueController: public controller_interface::ControllerInterface
        {
                std::vector<std::string> jointNames_;
-               int nJoints_;
+               unsigned int nJoints_;
 
                rclcpp::Subscription<trajectory_msgs::msg::JointTrajectoryPoint>::SharedPtr sub_command_;
 

diff --git a/src/computed_torque_controller.cpp b/src/computed_torque_controller.cpp
index 0b00f63..3530442 100644 (file)
--- a/src/computed_torque_controller.cpp
+++ b/src/computed_torque_controller.cpp
@@ -143,7 +143,7 @@ namespace effort_controllers
                std::vector<double> KpVec=node_->get_parameter("Kp").as_double_array();
                if(KpVec.empty())
                {
-                       RCLCPP_ERROR(node_->get_logger(),"No 'Kp' in controller %s.",node_->get_namespace());
+                       RCLCPP_ERROR(node_->get_logger(),"No 'Kp' in controller.");
                        return CallbackReturn::ERROR;
                }
                Kp_=Eigen::Map<Eigen::MatrixXd>(KpVec.data(),nJoints_,nJoints_).transpose();
@@ -151,13 +151,13 @@ namespace effort_controllers
                std::vector<double> KdVec=node_->get_parameter("Kd").as_double_array();
                if(KdVec.empty())
                {
-                       RCLCPP_ERROR(node_->get_logger(),"No 'Kd' in controller %s.",node_->get_namespace());
+                       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 %s. Using highest possible priority.",node_->get_namespace());
+                       RCLCPP_WARN(node_->get_logger(),"No 'priority' configured for controller. Using highest possible priority.");
 
                return CallbackReturn::SUCCESS;
        }
@@ -189,7 +189,7 @@ namespace effort_controllers
 
        CallbackReturn ComputedTorqueController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
        {
-               for(int i=0;i < nJoints_;i++)
+               for(unsigned int i=0;i < nJoints_;i++)
                {
                        q_(i)=state_interfaces_[2*i].get_value();
                        dq_(i)=state_interfaces_[2*i+1].get_value();
@@ -213,17 +213,17 @@ namespace effort_controllers
 
        CallbackReturn ComputedTorqueController::on_deactivate(const rclcpp_lifecycle::State &/*previous_state*/)
        {
-               for(int i=0;i < nJoints_;i++)
+               for(unsigned int i=0;i < nJoints_;i++)
                        q_(i)=state_interfaces_[2*i].get_value();
                SetToZero(dqr_);
                SetToZero(ddqr_);
 
-               for(unsigned int i=0;i < fext_.size();i++) fext_[i].Zero();
+               for(auto &f : fext_) f.Zero();
 
                if(idsolver_->CartToJnt(q_,dqr_,ddqr_,fext_,torque_) < 0)
                        RCLCPP_ERROR(node_->get_logger(),"KDL inverse dynamics solver failed.");
 
-               for(int i=0;i < nJoints_;i++)
+               for(unsigned int i=0;i < nJoints_;i++)
                        command_interfaces_[i].set_value(torque_(i));
 
                return CallbackReturn::SUCCESS;
@@ -234,29 +234,29 @@ namespace effort_controllers
                auto referencePoint=referencePoint_.readFromRT();
                if(referencePoint && *referencePoint)
                {
-                       if((*referencePoint)->positions.size() >= nJoints_)
+                       if((*referencePoint)->positions.size() == nJoints_)
                                qr_.data=Eigen::VectorXd::Map((*referencePoint)->positions.data(),nJoints_);
                                
-                       if((*referencePoint)->velocities.size() >= nJoints_)
+                       if((*referencePoint)->velocities.size() == nJoints_)
                                dqr_.data=Eigen::VectorXd::Map((*referencePoint)->velocities.data(),nJoints_);
                                
-                       if((*referencePoint)->accelerations.size() >= nJoints_)
+                       if((*referencePoint)->accelerations.size() == nJoints_)
                                ddqr_.data=Eigen::VectorXd::Map((*referencePoint)->accelerations.data(),nJoints_);
                }
 
-               for(int i=0;i < nJoints_;i++)
+               for(unsigned int i=0;i < nJoints_;i++)
                {
                        q_(i)=state_interfaces_[2*i].get_value();
                        dq_(i)=state_interfaces_[2*i+1].get_value();
                }
-               for(unsigned int i=0;i < fext_.size();i++) fext_[i].Zero();
+               for(auto &f : fext_) f.Zero();
 
                v_.data=ddqr_.data+Kp_*(qr_.data-q_.data)+Kd_*(dqr_.data-dq_.data);
                
                if(idsolver_->CartToJnt(q_,dq_,v_,fext_,torque_) < 0)
                        RCLCPP_ERROR(node_->get_logger(),"KDL inverse dynamics solver failed.");
 
-               for(int i=0;i < nJoints_;i++)
+               for(unsigned int i=0;i < nJoints_;i++)
                        command_interfaces_[i].set_value(torque_(i));
 
                return controller_interface::return_type::OK;