From f8b0c66bbaed1580c5820af8f50363b8e2d3e47a Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Wed, 23 Mar 2022 16:05:22 -0300 Subject: [PATCH] Fix typo in dynamics_parameters topic name. --- src/dynamics_linearizing_controller.cpp | 46 +++++++++++++++---------------- src/twist_mrac_linearizing_controller.cpp | 2 +- 2 files changed, 24 insertions(+), 24 deletions(-) diff --git a/src/dynamics_linearizing_controller.cpp b/src/dynamics_linearizing_controller.cpp index 1e5a18a..1dcd514 100644 --- a/src/dynamics_linearizing_controller.cpp +++ b/src/dynamics_linearizing_controller.cpp @@ -131,10 +131,10 @@ namespace effort_controllers sub_command_=node_->create_subscription("~/command",1000, std::bind(&DynamicsLinearizingController::commandCB,this,_1)); - sub_parameters_=node_->create_subscription("~/dynamics_paremeters",1000, + sub_parameters_=node_->create_subscription("~/dynamics_parameters",1000, std::bind(&DynamicsLinearizingController::parametersCB,this,_1)); - double wheelBase; + double wheelBase; if(!node_->get_parameter("wheel_separation",wheelBase)) throw std::runtime_error("No 'wheel_separation' parameter."); @@ -149,10 +149,10 @@ namespace effort_controllers throw std::runtime_error("No 'F' parameter."); F_=Eigen::Map(FVec.data(),2,2).transpose(); - std::vector GVec; + std::vector GVec; if(!node_->get_parameter("G",GVec)) throw std::runtime_error("No 'G' parameter."); - Ginv_=Eigen::Map(GVec.data(),2,2).transpose().inverse(); + Ginv_=Eigen::Map(GVec.data(),2,2).transpose().inverse(); time_step_=node_->get_parameter("time_step").as_double(); } @@ -200,12 +200,12 @@ namespace effort_controllers 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."); - if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) - RCLCPP_WARN(node_->get_logger(),"Failed to lock memory."); + 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."); + if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) + RCLCPP_WARN(node_->get_logger(),"Failed to lock memory."); return CallbackReturn::SUCCESS; } @@ -270,16 +270,16 @@ namespace effort_controllers } } - // Linearization - Eigen::Vector2d uf(u[0]*u[1],sqr(u[1])); - auto torque=Ginv_*(v_-F_*uf); + // Linearization + Eigen::Vector2d uf(u[0]*u[1],sqr(u[1])); + auto torque=Ginv_*(v_-F_*uf); // Apply torques for(unsigned int i=0;i < joints_.size();i++) command_interfaces_[i].set_value(torque[i]); - if(rt_status_publisher_ && rt_status_publisher_->trylock()) - { + if(rt_status_publisher_ && rt_status_publisher_->trylock()) + { rt_status_publisher_->msg_.header.stamp=time; rt_status_publisher_->msg_.set_point.linear.x=v_[0]; @@ -294,13 +294,13 @@ namespace effort_controllers rt_status_publisher_->msg_.command[i]=torque[i]; rt_status_publisher_->unlockAndPublish(); - } + } - Eigen::Vector3d x; + Eigen::Vector3d x; odom_.getPose(x); - if(rt_odom_publisher_ && rt_odom_publisher_->trylock()) - { + if(rt_odom_publisher_ && rt_odom_publisher_->trylock()) + { rt_odom_publisher_->msg_.header.stamp=time; rt_odom_publisher_->msg_.pose.pose.position.x=x[0]; @@ -313,10 +313,10 @@ namespace effort_controllers rt_odom_publisher_->msg_.twist.twist.angular.z=u[1]; rt_odom_publisher_->unlockAndPublish(); - } + } - if(rt_tf_odom_publisher_ && rt_tf_odom_publisher_->trylock()) - { + if(rt_tf_odom_publisher_ && rt_tf_odom_publisher_->trylock()) + { geometry_msgs::msg::TransformStamped &odom_frame= rt_tf_odom_publisher_->msg_.transforms[0]; odom_frame.header.stamp=time; @@ -326,7 +326,7 @@ namespace effort_controllers odom_frame.transform.rotation.w=cos(x[2]/2); rt_tf_odom_publisher_->unlockAndPublish(); - } + } return controller_interface::return_type::OK; } diff --git a/src/twist_mrac_linearizing_controller.cpp b/src/twist_mrac_linearizing_controller.cpp index bf9d15f..93ce659 100644 --- a/src/twist_mrac_linearizing_controller.cpp +++ b/src/twist_mrac_linearizing_controller.cpp @@ -133,7 +133,7 @@ namespace effort_controllers sub_command_=node_->create_subscription("~/command",1000, std::bind(&TwistMracLinearizingController::commandCB,this,_1)); - sub_parameters_=node_->create_subscription("~/dynamics_paremeters",1000, + sub_parameters_=node_->create_subscription("~/dynamics_parameters",1000, std::bind(&TwistMracLinearizingController::parametersCB,this,_1)); double wheelBase; -- 2.12.0