From b09f4617acff6af50ef68a5462f0bd72b1b1872d Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Thu, 6 Jul 2023 05:51:46 -0300 Subject: [PATCH] Port to Humble. --- CMakeLists.txt | 2 + .../nonsmooth_backstep_controller.hpp | 2 +- package.xml | 1 + src/nonsmooth_backstep_controller.cpp | 48 +++++++++++----------- 4 files changed, 28 insertions(+), 25 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 59fbad5..beb82c4 100644 --- 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 --- 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 joints_; diff --git a/package.xml b/package.xml index 954fa18..0cee95e 100644 --- a/package.xml +++ b/package.xml @@ -19,6 +19,7 @@ realtime_tools tf2 tf2_msgs + tf2_geometry_msgs tf2_ros Eigen3 diff --git a/src/nonsmooth_backstep_controller.cpp b/src/nonsmooth_backstep_controller.cpp index d55f144..82dec52 100644 --- 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("~/status",rclcpp::SystemDefaultsQoS()); + status_publisher_=get_node()->create_publisher("~/status",rclcpp::SystemDefaultsQoS()); rt_status_publisher_=std::make_shared>(status_publisher_); rt_status_publisher_->msg_.header.frame_id=odom_frame_id; - odom_publisher_=node_->create_publisher("~/odom",rclcpp::SystemDefaultsQoS()); + odom_publisher_=get_node()->create_publisher("~/odom",rclcpp::SystemDefaultsQoS()); rt_odom_publisher_=std::make_shared>(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("/tf",rclcpp::SystemDefaultsQoS()); + tf_odom_publisher_=get_node()->create_publisher("/tf",rclcpp::SystemDefaultsQoS()); rt_tf_odom_publisher_=std::make_shared>(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("~/command",1000, + sub_command_=get_node()->create_subscription("~/command",1000, std::bind(&NonSmoothBackstepController::commandCB,this,_1)); - sub_parameters_=node_->create_subscription("~/dynamics_parameters",1000, + sub_parameters_=get_node()->create_subscription("~/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 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 FVec; - if(!node_->get_parameter("F",FVec)) + if(!get_node()->get_parameter("F",FVec)) throw std::runtime_error("No 'F' parameter."); F_=Eigen::Map(FVec.data(),2,2).transpose(); std::vector GVec; - if(!node_->get_parameter("G",GVec)) + if(!get_node()->get_parameter("G",GVec)) throw std::runtime_error("No 'G' parameter."); Ginv_=Eigen::Map(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,¶m) == -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; -- 2.12.0