Use arc_odometry.

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 195a64e..d32bc03 100644 (file)
--- 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 (file)
--- a/include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.h
+++ b/include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.h
@@ -40,6 +40,7 @@
 #include <controller_interface/controller.h>
 #include <realtime_tools/realtime_publisher.h>
 
+#include <arc_odometry/diff_odometry.h>
 #include <nonsmooth_backstep_controller/NonSmoothBackstepControllerStatus.h>
 
 namespace effort_controllers
@@ -76,10 +77,8 @@ namespace effort_controllers
                Eigen::Matrix2d Ginv_;
                Eigen::Matrix2d F_;
                
-               std::vector<double> 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 (file)
--- a/package.xml
+++ b/package.xml
@@ -50,6 +50,7 @@
   <build_depend>nav_msgs</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>tf</build_depend>
+  <build_depend>arc_odometry</build_depend>
   <run_depend>Eigen</run_depend>
   <run_depend>controller_interface</run_depend>
   <run_depend>effort_controllers</run_depend>
@@ -59,6 +60,7 @@
   <run_depend>nav_msgs</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>tf</run_depend>
+  <run_depend>arc_odometry</run_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

diff --git a/src/nonsmooth_backstep_controller.cpp b/src/nonsmooth_backstep_controller.cpp
index 47ac6d8..149e173 100644 (file)
--- 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<double>(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<double> 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();
                 }