From: Walter Fetter Lages Date: Thu, 6 Jul 2023 22:41:07 +0000 (-0300) Subject: Port to Humble. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=121e3eeb0e1018be6ec81aac8fb1dafb5623e939;p=linearizing_controllers.git Port to Humble. --- diff --git a/CMakeLists.txt b/CMakeLists.txt index 8460f0c..7c2f964 100644 --- 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 --- 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 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 --- 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 joints_; diff --git a/package.xml b/package.xml index 43345ac..6a504d5 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/dynamics_linearizing_controller.cpp b/src/dynamics_linearizing_controller.cpp index 1dcd514..1ed47fa 100644 --- 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("~/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=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("~/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; @@ -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("/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; @@ -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("~/command",1000, + sub_command_=get_node()->create_subscription("~/command",1000, std::bind(&DynamicsLinearizingController::commandCB,this,_1)); - sub_parameters_=node_->create_subscription("~/dynamics_parameters",1000, + sub_parameters_=get_node()->create_subscription("~/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 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 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; } @@ -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,¶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; } @@ -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 --- 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("~/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=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("~/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,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("~/command",1000, + sub_command_=get_node()->create_subscription("~/command",1000, std::bind(&TwistMracLinearizingController::commandCB,this,_1)); - sub_parameters_=node_->create_subscription("~/dynamics_parameters",1000, + sub_parameters_=get_node()->create_subscription("~/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 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 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(); std::vector 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(AlphaVec.data(),2); - adaptive_=node_->get_parameter("adaptive").as_bool(); + adaptive_=get_node()->get_parameter("adaptive").as_bool(); if(adaptive_) { std::vector 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(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,¶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; } @@ -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