From f6f7352f03ba3a1785a031c67439227bbbecc397 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Wed, 14 Mar 2018 15:06:24 -0300 Subject: [PATCH] Use arc_odometry. --- CMakeLists.txt | 3 +- .../nonsmooth_backstep_controller.h | 5 +- package.xml | 2 + src/nonsmooth_backstep_controller.cpp | 86 +++++++++++----------- 4 files changed, 47 insertions(+), 49 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 195a64e..d32bc03 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS nav_msgs roscpp tf + arc_odometry ) find_package(cmake_modules REQUIRED) @@ -116,7 +117,7 @@ generate_messages( catkin_package( INCLUDE_DIRS include LIBRARIES nonsmooth_backstep_controller - CATKIN_DEPENDS controller_interface effort_controllers geometry_msgs message_generation message_runtime nav_msgs roscpp tf + CATKIN_DEPENDS controller_interface effort_controllers geometry_msgs message_generation message_runtime nav_msgs roscpp tf arc_odometry DEPENDS Eigen ) diff --git a/include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.h b/include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.h index 04a8c5a..b4f23da 100644 --- a/include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.h +++ b/include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.h @@ -40,6 +40,7 @@ #include #include +#include #include namespace effort_controllers @@ -76,10 +77,8 @@ namespace effort_controllers Eigen::Matrix2d Ginv_; Eigen::Matrix2d F_; - std::vector wheelRadius_; - double wheelBase_; + arc_odometry::DiffOdometry odom_; - Eigen::Vector3d x_; Eigen::Vector3d xRef_; Eigen::Vector2d eta_; diff --git a/package.xml b/package.xml index 3fc77b8..c683d36 100644 --- a/package.xml +++ b/package.xml @@ -50,6 +50,7 @@ nav_msgs roscpp tf + arc_odometry Eigen controller_interface effort_controllers @@ -59,6 +60,7 @@ nav_msgs roscpp tf + arc_odometry diff --git a/src/nonsmooth_backstep_controller.cpp b/src/nonsmooth_backstep_controller.cpp index 47ac6d8..149e173 100644 --- a/src/nonsmooth_backstep_controller.cpp +++ b/src/nonsmooth_backstep_controller.cpp @@ -33,7 +33,7 @@ namespace effort_controllers { NonSmoothBackstepController::NonSmoothBackstepController(void): - wheelRadius_(2),lambda_(5),gamma_(4) + odom_(0.0,std::vector(2)),lambda_(5),gamma_(4) { } @@ -126,19 +126,22 @@ namespace effort_controllers sub_parameters_=node_.subscribe("dynamic_parameters",1000,&NonSmoothBackstepController::parametersCB,this); - if(!node_.getParam("wheel_separation",wheelBase_)) + double wheelBase; + if(!node_.getParam("wheel_separation",wheelBase)) { ROS_ERROR("No 'wheel_separation' in controller %s.",node_.getNamespace().c_str()); return false; } - wheelRadius_.resize(joints_.size()); - if(!node_.getParam("wheel_radius",wheelRadius_)) + std::vector wheelRadius(2); + if(!node_.getParam("wheel_radius",wheelRadius)) { ROS_ERROR("No 'wheel_radius' in controller %s.",node_.getNamespace().c_str()); return false; } + odom_.setParams(wheelBase,wheelRadius); + gamma_[0]=10.0; gamma_[1]=1.0; gamma_[2]=10.0; @@ -175,7 +178,9 @@ namespace effort_controllers void NonSmoothBackstepController::starting(const ros::Time& time) { - x_.setZero(); + Eigen::Vector3d x; + x.setZero(); + odom_.setPose(x); xRef_.setZero(); eta_.setZero(); lastSamplingTime_=time; @@ -202,9 +207,9 @@ namespace effort_controllers void NonSmoothBackstepController::update(const ros::Time& time, const ros::Duration& duration) { - double dt=time.toSec()-lastSamplingTime_.toSec(); + ros::Duration dt=time-lastSamplingTime_; - if(fabs(dt-time_step_) > time_step_/20) return; + if(fabs(dt.toSec()-time_step_) > time_step_/20) return; lastSamplingTime_=time; // Incremental encoders sense angular displacement and @@ -218,29 +223,20 @@ namespace effort_controllers } deltaPhi+=phi_; - // Estimate pose by odometry - Eigen::Vector2d deltaU; - deltaU << (deltaPhi[0]*wheelRadius_[0]+deltaPhi[1]*wheelRadius_[1])/2.0, - (deltaPhi[1]*wheelRadius_[1]-deltaPhi[0]*wheelRadius_[0])/wheelBase_; - - double deltaS=(fabs(deltaU[1]) > DBL_EPSILON)? - deltaU[0]*sin(deltaU[1]/2)/(deltaU[1]/2):deltaU[0]; - - Eigen::Vector3d deltaX; - deltaX << deltaS*cos(x_[2]+deltaU[1]/2.0), - deltaS*sin(x_[2]+deltaU[1]/2.0), - deltaU[1]; - - x_+=deltaX; + odom_.update(deltaPhi,dt); + + Eigen::Vector3d x; + odom_.getPose(x); - Eigen::Vector2d u=deltaU/dt; + Eigen::Vector2d u; + odom_.getVelocity(u); // Change of coordinates Eigen::Matrix3d R; R << cos(xRef_[2]), sin(xRef_[2]), 0.0, -sin(xRef_[2]), cos(xRef_[2]), 0.0, 0.0, 0.0, 1.0; - Eigen::Vector3d xBar=R*(x_-xRef_); + Eigen::Vector3d xBar=R*(x-xRef_); // Discontinuous transformation double e=sqrt(sqr(xBar[0])+sqr(xBar[1])); @@ -258,7 +254,7 @@ namespace effort_controllers lambda_[1]/alpha*cos(alpha); else eta_[1]=gamma_[0]*lambda_[2]*psi/lambda_[1]; deta+=eta_; - deta/=dt; + deta/=dt.toSec(); Eigen::Vector2d eb=u-eta_; @@ -290,19 +286,19 @@ namespace effort_controllers status_publisher_->msg_.set_point.y=xRef_[1]; status_publisher_->msg_.set_point.theta=xRef_[2]; - status_publisher_->msg_.process_value.x=x_[0]; - status_publisher_->msg_.process_value.y=x_[1]; - status_publisher_->msg_.process_value.theta=x_[2]; + status_publisher_->msg_.process_value.x=x[0]; + status_publisher_->msg_.process_value.y=x[1]; + status_publisher_->msg_.process_value.theta=x[2]; - status_publisher_->msg_.process_value_dot.x=deltaX[0]/dt; - status_publisher_->msg_.process_value_dot.y=deltaX[1]/dt; - status_publisher_->msg_.process_value_dot.theta=deltaX[2]/dt; + status_publisher_->msg_.process_value_dot.x=u[0]*cos(x[2]); + status_publisher_->msg_.process_value_dot.y=u[0]*sin(x[2]); + status_publisher_->msg_.process_value_dot.theta=u[1]; - status_publisher_->msg_.error.x=xRef_[0]-x_[0]; - status_publisher_->msg_.error.y=xRef_[1]-x_[1]; - status_publisher_->msg_.error.theta=xRef_[2]-x_[2]; + status_publisher_->msg_.error.x=xRef_[0]-x[0]; + status_publisher_->msg_.error.y=xRef_[1]-x[1]; + status_publisher_->msg_.error.theta=xRef_[2]-x[2]; - status_publisher_->msg_.time_step=dt; + status_publisher_->msg_.time_step=dt.toSec(); for(int i=0;i < torque.size();i++) status_publisher_->msg_.command[i]=torque[i]; @@ -342,14 +338,14 @@ namespace effort_controllers { odom_publisher_->msg_.header.stamp=time; - odom_publisher_->msg_.pose.pose.position.x=x_[0]; - odom_publisher_->msg_.pose.pose.position.y=x_[1]; - odom_publisher_->msg_.pose.pose.orientation.z=sin(x_[2]/2); - odom_publisher_->msg_.pose.pose.orientation.w=cos(x_[2]/2); + odom_publisher_->msg_.pose.pose.position.x=x[0]; + odom_publisher_->msg_.pose.pose.position.y=x[1]; + odom_publisher_->msg_.pose.pose.orientation.z=sin(x[2]/2); + odom_publisher_->msg_.pose.pose.orientation.w=cos(x[2]/2); - odom_publisher_->msg_.twist.twist.linear.x=deltaX[0]/dt; - odom_publisher_->msg_.twist.twist.linear.y=deltaX[1]/dt; - odom_publisher_->msg_.twist.twist.angular.z=deltaX[2]/dt; + odom_publisher_->msg_.twist.twist.linear.x=u[0]*cos(x[2]); + odom_publisher_->msg_.twist.twist.linear.y=u[0]*sin(x[2]); + odom_publisher_->msg_.twist.twist.angular.z=u[1]; odom_publisher_->unlockAndPublish(); } @@ -359,10 +355,10 @@ namespace effort_controllers geometry_msgs::TransformStamped &odom_frame= tf_odom_publisher_->msg_.transforms[0]; odom_frame.header.stamp=time; - odom_frame.transform.translation.x=x_[0]; - odom_frame.transform.translation.y=x_[1]; - odom_frame.transform.rotation.z=sin(x_[2]/2); - odom_frame.transform.rotation.w=cos(x_[2]/2); + odom_frame.transform.translation.x=x[0]; + odom_frame.transform.translation.y=x[1]; + odom_frame.transform.rotation.z=sin(x[2]/2); + odom_frame.transform.rotation.w=cos(x[2]/2); tf_odom_publisher_->unlockAndPublish(); } -- 2.12.0