Port to Humble.

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8460f0c..7c2f964 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -19,6 +19,7 @@ find_package(realtime_tools REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 find_package(Eigen3 REQUIRED)
 
 add_library(linearizing_controllers SHARED
@@ -39,6 +40,7 @@ ament_target_dependencies(
   "realtime_tools"
   "tf2"
   "tf2_msgs"
+  "tf2_geometry_msgs"
   "tf2_ros"
   "Eigen3"
 )

diff --git a/include/linearizing_controllers/dynamics_linearizing_controller.hpp b/include/linearizing_controllers/dynamics_linearizing_controller.hpp
index 4b64829..2696221 100644 (file)
--- a/include/linearizing_controllers/dynamics_linearizing_controller.hpp
+++ b/include/linearizing_controllers/dynamics_linearizing_controller.hpp
@@ -65,7 +65,7 @@ namespace effort_controllers
                CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
 
                LINEARIZING_CONTROLLERS_PUBLIC
-               controller_interface::return_type update(void) override;
+               controller_interface::return_type update(const rclcpp::Time &time,const rclcpp::Duration &period) override;
 
                private:
                std::vector<std::string> joints_;

diff --git a/include/linearizing_controllers/twist_mrac_linearizing_controller.hpp b/include/linearizing_controllers/twist_mrac_linearizing_controller.hpp
index 7f59f82..f0a7af7 100644 (file)
--- a/include/linearizing_controllers/twist_mrac_linearizing_controller.hpp
+++ b/include/linearizing_controllers/twist_mrac_linearizing_controller.hpp
@@ -65,7 +65,7 @@ namespace effort_controllers
                CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
 
                LINEARIZING_CONTROLLERS_PUBLIC
-               controller_interface::return_type update(void) override;
+               controller_interface::return_type update(const rclcpp::Time &time,const rclcpp::Duration &period) override;
 
                private:
                std::vector<std::string> joints_;

diff --git a/package.xml b/package.xml
index 43345ac..6a504d5 100644 (file)
--- a/package.xml
+++ b/package.xml
@@ -19,6 +19,7 @@
   <depend>realtime_tools</depend>
   <depend>tf2</depend>
   <depend>tf2_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>Eigen3</depend>
 

diff --git a/src/dynamics_linearizing_controller.cpp b/src/dynamics_linearizing_controller.cpp
index 1dcd514..1ed47fa 100644 (file)
--- a/src/dynamics_linearizing_controller.cpp
+++ b/src/dynamics_linearizing_controller.cpp
@@ -56,7 +56,7 @@ namespace effort_controllers
                }
                catch(const std::exception &e)
                {
-                       RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception thrown in on_init() with message: " << e.what());
+                       RCLCPP_ERROR_STREAM(get_node()->get_logger(),"Exception thrown in on_init() with message: " << e.what());
                        return CallbackReturn::ERROR;
                }
 
@@ -67,14 +67,14 @@ namespace effort_controllers
        {
                try
                {
-                       joints_=node_->get_parameter("joints").as_string_array();
+                       joints_=get_node()->get_parameter("joints").as_string_array();
                        if(joints_.empty())
                                throw std::runtime_error("'joints' parameter was empty,");
 
-                       auto odom_frame_id=node_->get_parameter("odom_frame_id").as_string();
-                       auto base_frame_id=node_->get_parameter("base_frame_id").as_string();
+                       auto odom_frame_id=get_node()->get_parameter("odom_frame_id").as_string();
+                       auto base_frame_id=get_node()->get_parameter("base_frame_id").as_string();
 
-                       status_publisher_=node_->create_publisher<linearizing_controllers_msgs::msg::DynamicsLinearizingControllerStatus>("~/status",rclcpp::SystemDefaultsQoS());
+                       status_publisher_=get_node()->create_publisher<linearizing_controllers_msgs::msg::DynamicsLinearizingControllerStatus>("~/status",rclcpp::SystemDefaultsQoS());
                        rt_status_publisher_=std::make_shared<realtime_tools::RealtimePublisher<linearizing_controllers_msgs::msg::DynamicsLinearizingControllerStatus>>(status_publisher_);
                        rt_status_publisher_->msg_.header.frame_id=base_frame_id;
                        rt_status_publisher_->msg_.set_point.linear.y=0.0;
@@ -86,7 +86,7 @@ namespace effort_controllers
                        rt_status_publisher_->msg_.process_value.angular.x=0.0;
                        rt_status_publisher_->msg_.process_value.angular.y=0.0;
 
-                       odom_publisher_=node_->create_publisher<nav_msgs::msg::Odometry>("~/odom",rclcpp::SystemDefaultsQoS());
+                       odom_publisher_=get_node()->create_publisher<nav_msgs::msg::Odometry>("~/odom",rclcpp::SystemDefaultsQoS());
                        rt_odom_publisher_=std::make_shared<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>(odom_publisher_);
                        rt_odom_publisher_->msg_.header.frame_id=odom_frame_id;
                        rt_odom_publisher_->msg_.child_frame_id=base_frame_id;
@@ -118,7 +118,7 @@ namespace effort_controllers
                        0, 0, 0, 0, twist_cov[4], 0,
                        0, 0, 0, 0, 0, twist_cov[5]};
 
-                       tf_odom_publisher_=node_->create_publisher<tf2_msgs::msg::TFMessage>("/tf",rclcpp::SystemDefaultsQoS());
+                       tf_odom_publisher_=get_node()->create_publisher<tf2_msgs::msg::TFMessage>("/tf",rclcpp::SystemDefaultsQoS());
                        rt_tf_odom_publisher_=std::make_shared<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>(tf_odom_publisher_);
                        rt_tf_odom_publisher_->msg_.transforms.resize(1);
                        rt_tf_odom_publisher_->msg_.transforms[0].transform.translation.z=0.0;
@@ -128,42 +128,42 @@ namespace effort_controllers
                        rt_tf_odom_publisher_->msg_.transforms[0].header.frame_id=odom_frame_id;
 
                        using std::placeholders::_1;
-                       sub_command_=node_->create_subscription<geometry_msgs::msg::Accel>("~/command",1000,
+                       sub_command_=get_node()->create_subscription<geometry_msgs::msg::Accel>("~/command",1000,
                                std::bind(&DynamicsLinearizingController::commandCB,this,_1));
 
-                       sub_parameters_=node_->create_subscription<std_msgs::msg::Float64MultiArray>("~/dynamics_parameters",1000,
+                       sub_parameters_=get_node()->create_subscription<std_msgs::msg::Float64MultiArray>("~/dynamics_parameters",1000,
                                std::bind(&DynamicsLinearizingController::parametersCB,this,_1));
 
                        double wheelBase;
-                       if(!node_->get_parameter("wheel_separation",wheelBase))
+                       if(!get_node()->get_parameter("wheel_separation",wheelBase))
                                throw std::runtime_error("No 'wheel_separation' parameter.");
 
                        std::vector<double> wheelRadius(2);
-                       if(!node_->get_parameter("wheel_radius",wheelRadius))
+                       if(!get_node()->get_parameter("wheel_radius",wheelRadius))
                                throw std::runtime_error("No 'wheel_radius' parameter.");
 
                        odom_.setParams(wheelBase,wheelRadius);
 
                        std::vector<double> FVec;
-                       if(!node_->get_parameter("F",FVec))
+                       if(!get_node()->get_parameter("F",FVec))
                                throw std::runtime_error("No 'F' parameter.");
                        F_=Eigen::Map<Eigen::Matrix2d>(FVec.data(),2,2).transpose();
 
                        std::vector<double> GVec;
-                       if(!node_->get_parameter("G",GVec))
+                       if(!get_node()->get_parameter("G",GVec))
                                throw std::runtime_error("No 'G' parameter.");
                        Ginv_=Eigen::Map<Eigen::Matrix2d>(GVec.data(),2,2).transpose().inverse();
 
-                       time_step_=node_->get_parameter("time_step").as_double();
+                       time_step_=get_node()->get_parameter("time_step").as_double();
                }
                catch(const std::exception &e)
                {
-                       RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception thrown in on_confiture(): " << e.what());
+                       RCLCPP_ERROR_STREAM(get_node()->get_logger(),"Exception thrown in on_confiture(): " << e.what());
                        return CallbackReturn::ERROR;
                }
 
-               if(!node_->get_parameter("priority",priority_))
-                       RCLCPP_WARN(node_->get_logger(),"No 'priority' configured for controller. Using highest possible priority.");
+               if(!get_node()->get_parameter("priority",priority_))
+                       RCLCPP_WARN(get_node()->get_logger(),"No 'priority' configured for controller. Using highest possible priority.");
 
                return CallbackReturn::SUCCESS;
        }
@@ -196,16 +196,16 @@ namespace effort_controllers
                x.setZero();
                odom_.setPose(x);
                v_.setZero();
-               lastSamplingTime_=node_->get_clock()->now();
+               lastSamplingTime_=get_node()->get_clock()->now();
                for(unsigned int i=0;i < joints_.size();i++)
                        phi_[i]=state_interfaces_[i].get_value();
 
                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.");
+                       RCLCPP_WARN(get_node()->get_logger(),"Failed to set real-time scheduler.");
                if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1)
-                       RCLCPP_WARN(node_->get_logger(),"Failed to lock memory.");
+                       RCLCPP_WARN(get_node()->get_logger(),"Failed to lock memory.");
 
                return CallbackReturn::SUCCESS;
        }
@@ -218,9 +218,9 @@ namespace effort_controllers
                return CallbackReturn::SUCCESS;
        }
 
-       controller_interface::return_type DynamicsLinearizingController::update(void)
+       controller_interface::return_type DynamicsLinearizingController::update(const rclcpp::Time & /*time*/,const rclcpp::Duration & /*period*/)
        {
-               auto time=node_->get_clock()->now();
+               auto time=get_node()->get_clock()->now();
                auto dt=time-lastSamplingTime_;
 
                if(dt.seconds() < time_step_) return controller_interface::return_type::OK;

diff --git a/src/twist_mrac_linearizing_controller.cpp b/src/twist_mrac_linearizing_controller.cpp
index 0187ca7..a0d1a2c 100644 (file)
--- a/src/twist_mrac_linearizing_controller.cpp
+++ b/src/twist_mrac_linearizing_controller.cpp
@@ -62,7 +62,7 @@ namespace effort_controllers
                }
                catch(const std::exception &e)
                {
-                       RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception thrown in on_init() with message: " << e.what());
+                       RCLCPP_ERROR_STREAM(get_node()->get_logger(),"Exception thrown in on_init() with message: " << e.what());
                        return CallbackReturn::ERROR;
                }
 
@@ -73,14 +73,14 @@ namespace effort_controllers
        {
                try
                {
-                       joints_=node_->get_parameter("joints").as_string_array();
+                       joints_=get_node()->get_parameter("joints").as_string_array();
                        if(joints_.empty())
                                throw std::runtime_error("'joints' parameter was empty,");
 
-                       auto odom_frame_id=node_->get_parameter("odom_frame_id").as_string();
-                       auto base_frame_id=node_->get_parameter("base_frame_id").as_string();
+                       auto odom_frame_id=get_node()->get_parameter("odom_frame_id").as_string();
+                       auto base_frame_id=get_node()->get_parameter("base_frame_id").as_string();
 
-                       status_publisher_=node_->create_publisher<linearizing_controllers_msgs::msg::TwistMracLinearizingControllerStatus>("~/status",rclcpp::SystemDefaultsQoS());
+                       status_publisher_=get_node()->create_publisher<linearizing_controllers_msgs::msg::TwistMracLinearizingControllerStatus>("~/status",rclcpp::SystemDefaultsQoS());
                        rt_status_publisher_=std::make_shared<realtime_tools::RealtimePublisher<linearizing_controllers_msgs::msg::TwistMracLinearizingControllerStatus>>(status_publisher_);
                        rt_status_publisher_->msg_.header.frame_id=base_frame_id;
                        rt_status_publisher_->msg_.set_point.linear.y=0.0;
@@ -92,7 +92,7 @@ namespace effort_controllers
                        rt_status_publisher_->msg_.process_value.angular.x=0.0;
                        rt_status_publisher_->msg_.process_value.angular.y=0.0;
 
-                       odom_publisher_=node_->create_publisher<nav_msgs::msg::Odometry>("~/odom",rclcpp::SystemDefaultsQoS());
+                       odom_publisher_=get_node()->create_publisher<nav_msgs::msg::Odometry>("~/odom",rclcpp::SystemDefaultsQoS());
                        rt_odom_publisher_=std::make_shared<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>(odom_publisher_);
                        rt_odom_publisher_->msg_.header.frame_id=odom_frame_id;
                        rt_odom_publisher_->msg_.child_frame_id=base_frame_id;
@@ -124,7 +124,7 @@ namespace effort_controllers
                        0, 0, 0, 0, twist_cov[4], 0,
                        0, 0, 0, 0, 0, twist_cov[5]};
 
-                       tf_odom_publisher_=node_->create_publisher<tf2_msgs::msg::TFMessage>("/tf",rclcpp::SystemDefaultsQoS());
+                       tf_odom_publisher_=get_node()->create_publisher<tf2_msgs::msg::TFMessage>("/tf",rclcpp::SystemDefaultsQoS());
                        rt_tf_odom_publisher_=std::make_shared<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>(tf_odom_publisher_);
                        rt_tf_odom_publisher_->msg_.transforms.resize(1);
                        rt_tf_odom_publisher_->msg_.transforms[0].transform.translation.z=0.0;
@@ -134,57 +134,57 @@ namespace effort_controllers
                        rt_tf_odom_publisher_->msg_.transforms[0].header.frame_id=odom_frame_id;
 
                        using std::placeholders::_1;
-                       sub_command_=node_->create_subscription<geometry_msgs::msg::Twist>("~/command",1000,
+                       sub_command_=get_node()->create_subscription<geometry_msgs::msg::Twist>("~/command",1000,
                                std::bind(&TwistMracLinearizingController::commandCB,this,_1));
 
-                       sub_parameters_=node_->create_subscription<std_msgs::msg::Float64MultiArray>("~/dynamics_parameters",1000,
+                       sub_parameters_=get_node()->create_subscription<std_msgs::msg::Float64MultiArray>("~/dynamics_parameters",1000,
                                std::bind(&TwistMracLinearizingController::parametersCB,this,_1));
 
                        double wheelBase;
-                       if(!node_->get_parameter("wheel_separation",wheelBase))
+                       if(!get_node()->get_parameter("wheel_separation",wheelBase))
                                throw std::runtime_error("No 'wheel_separation' parameter.");
 
                        std::vector<double> wheelRadius(2);
-                       if(!node_->get_parameter("wheel_radius",wheelRadius))
+                       if(!get_node()->get_parameter("wheel_radius",wheelRadius))
                                throw std::runtime_error("No 'wheel_radius' parameter.");
 
                        odom_.setParams(wheelBase,wheelRadius);
 
                        std::vector<double> FVec;
-                       if(!node_->get_parameter("F",FVec))
+                       if(!get_node()->get_parameter("F",FVec))
                                throw std::runtime_error("No 'F' parameter.");
                        F_=Eigen::Map<Eigen::Matrix2d>(FVec.data(),2,2).transpose();
 
                        std::vector<double> GVec;
-                       if(!node_->get_parameter("G",GVec))
+                       if(!get_node()->get_parameter("G",GVec))
                                throw std::runtime_error("No 'G' parameter.");
                        Ginv_=Eigen::Map<Eigen::Matrix2d>(GVec.data(),2,2).transpose().inverse();
 
                        std::vector<double> AlphaVec;
-                       if(!node_->get_parameter("Alpha",AlphaVec))
+                       if(!get_node()->get_parameter("Alpha",AlphaVec))
                                throw std::runtime_error("No 'Alpha' parameter.");
                        Alpha_.diagonal()=Eigen::Map<Eigen::Vector2d>(AlphaVec.data(),2);
                        
-                       adaptive_=node_->get_parameter("adaptive").as_bool();
+                       adaptive_=get_node()->get_parameter("adaptive").as_bool();
                        
                        if(adaptive_)
                        {
                                std::vector<double> EpsilonVec;
-                               if(!node_->get_parameter("Epsilon",EpsilonVec))
+                               if(!get_node()->get_parameter("Epsilon",EpsilonVec))
                                        throw std::runtime_error("No 'Epsilon' parameter.");
                                Epsilon_.diagonal()=Eigen::Map<Eigen::Vector2d>(EpsilonVec.data(),2);
                        }
 
-                       time_step_=node_->get_parameter("time_step").as_double();
+                       time_step_=get_node()->get_parameter("time_step").as_double();
                }
                catch(const std::exception &e)
                {
-                       RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception thrown in on_confiture(): " << e.what());
+                       RCLCPP_ERROR_STREAM(get_node()->get_logger(),"Exception thrown in on_confiture(): " << e.what());
                        return CallbackReturn::ERROR;
                }
 
-               if(!node_->get_parameter("priority",priority_))
-                       RCLCPP_WARN(node_->get_logger(),"No 'priority' configured for controller. Using highest possible priority.");
+               if(!get_node()->get_parameter("priority",priority_))
+                       RCLCPP_WARN(get_node()->get_logger(),"No 'priority' configured for controller. Using highest possible priority.");
 
                return CallbackReturn::SUCCESS;
        }
@@ -218,7 +218,7 @@ namespace effort_controllers
                odom_.setPose(x);
                uRef_.setZero();
                uModel_.setZero();
-               lastSamplingTime_=node_->get_clock()->now();
+               lastSamplingTime_=get_node()->get_clock()->now();
                for(unsigned int i=0;i < joints_.size();i++)
                        phi_[i]=state_interfaces_[i].get_value();
 
@@ -245,9 +245,9 @@ 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.");
+                        RCLCPP_WARN(get_node()->get_logger(),"Failed to set real-time scheduler.");
                 if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1)
-                        RCLCPP_WARN(node_->get_logger(),"Failed to lock memory.");
+                        RCLCPP_WARN(get_node()->get_logger(),"Failed to lock memory.");
 
                return CallbackReturn::SUCCESS;
        }
@@ -260,9 +260,9 @@ namespace effort_controllers
                return CallbackReturn::SUCCESS;
        }
 
-       controller_interface::return_type TwistMracLinearizingController::update(void)
+       controller_interface::return_type TwistMracLinearizingController::update(const rclcpp::Time & /*time*/,const rclcpp::Duration & /*period*/)
        {
-               auto time=node_->get_clock()->now();
+               auto time=get_node()->get_clock()->now();
                auto dt=time-lastSamplingTime_;
 
                if(dt.seconds() < time_step_) return controller_interface::return_type::OK;
@@ -321,11 +321,11 @@ namespace effort_controllers
                        // Ginv=[0.5/g11 0.5/g21
                        //      0.5/g11 -0.5/g21]
                        F_(0,1)=thetaEst_[0];
-                       F_(1.0)=thetaEst_[1];
+                       F_(1,0)=thetaEst_[1];
                        Ginv_(0,0)=0.5/thetaEst_[2];
                        Ginv_(0,1)=0.5/thetaEst_[3];
-                       Ginv_(1.0)=0.5/thetaEst_[2];
-                       Ginv_(1.1)=-0.5/thetaEst_[3];
+                       Ginv_(1,0)=0.5/thetaEst_[2];
+                       Ginv_(1,1)=-0.5/thetaEst_[3];
                }
 
                // Compute reference model