Port to Humble.

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 59fbad5..beb82c4 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -18,6 +18,7 @@ find_package(realtime_tools REQUIRED)
 find_package(nonsmooth_backstep_controller_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(Eigen3 REQUIRED)
 
@@ -40,6 +41,7 @@ ament_target_dependencies(
   "realtime_tools"
   "tf2"
   "tf2_msgs"
+  "tf2_geometry_msgs"
   "tf2_ros"
   "Eigen3"
 )

diff --git a/include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.hpp b/include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.hpp
index a7750bf..629f788 100644 (file)
--- a/include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.hpp
+++ b/include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.hpp
@@ -66,7 +66,7 @@ namespace effort_controllers
                CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
 
                NONSMOOTH_BACKSTEP_CONTROLLER_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 954fa18..0cee95e 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/nonsmooth_backstep_controller.cpp b/src/nonsmooth_backstep_controller.cpp
index d55f144..82dec52 100644 (file)
--- a/src/nonsmooth_backstep_controller.cpp
+++ b/src/nonsmooth_backstep_controller.cpp
@@ -70,7 +70,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;
                }
 
@@ -81,18 +81,18 @@ 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<nonsmooth_backstep_controller_msgs::msg::NonSmoothBackstepControllerStatus>("~/status",rclcpp::SystemDefaultsQoS());
+                       status_publisher_=get_node()->create_publisher<nonsmooth_backstep_controller_msgs::msg::NonSmoothBackstepControllerStatus>("~/status",rclcpp::SystemDefaultsQoS());
                        rt_status_publisher_=std::make_shared<realtime_tools::RealtimePublisher<nonsmooth_backstep_controller_msgs::msg::NonSmoothBackstepControllerStatus>>(status_publisher_);
                        rt_status_publisher_->msg_.header.frame_id=odom_frame_id;
 
-                       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,48 +134,48 @@ 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::Pose2D>("~/command",1000,
+                       sub_command_=get_node()->create_subscription<geometry_msgs::msg::Pose2D>("~/command",1000,
                                std::bind(&NonSmoothBackstepController::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(&NonSmoothBackstepController::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);
 
-                       if(!node_->get_parameter("gamma",gamma_))
+                       if(!get_node()->get_parameter("gamma",gamma_))
                                throw std::runtime_error("No 'gamma' parameter.");
 
-                       if(!node_->get_parameter("lambda",lambda_))
+                       if(!get_node()->get_parameter("lambda",lambda_))
                                throw std::runtime_error("No 'lambda' parameter.");
 
                        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;
        }
@@ -209,16 +209,16 @@ namespace effort_controllers
                odom_.setPose(x);
                xRef_.setZero();
                eta_.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;
        }
 
@@ -231,9 +231,9 @@ namespace effort_controllers
        }
 
 
-       controller_interface::return_type NonSmoothBackstepController::update(void)
+       controller_interface::return_type NonSmoothBackstepController::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;