Added option to obtain velocity directly from joint_state.

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h

@@ -124,6 +124,7 @@ namespace diff_drive_controller{
     boost::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > odom_pub_;
     boost::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > tf_odom_pub_;
     Odometry odometry_;
+    bool estimate_velocity_from_position_;
 
     /// Wheel separation, wrt the midpoint of the wheel width:
     double wheel_separation_;

diff_drive_controller/include/diff_drive_controller/odometry.h

@@ -86,6 +86,18 @@ namespace diff_drive_controller
      */
     bool update(double left_pos, double right_pos, const ros::Time &time);
 
+    /**
+     * \brief Updates the odometry class with latest wheels position and velocity
+     * \param left_pos  Left  wheel position [rad]
+     * \param right_pos Right wheel position [rad]
+     * \param left_vel  Left  wheel velocity [rad/s]
+     * \param right_vel Right wheel velocity [rad/s]
+     * \param time      Current time
+     * \return true if the odometry is actually updated
+     */
+    bool update(double left_pos, double right_pos, double left_vel,
+                double right_vel, const ros::Time &time);
+
     /**
      * \brief Updates the odometry class with latest velocity command
      * \param linear  Linear velocity [m/s]

diff_drive_controller/src/diff_drive_controller.cpp

@@ -109,6 +109,7 @@ namespace diff_drive_controller{
 
   DiffDriveController::DiffDriveController()
     : open_loop_(false)
+    , estimate_velocity_from_position_(true)
     , command_struct_()
     , wheel_separation_(0.0)
     , wheel_radius_(0.0)
@@ -162,6 +163,9 @@ namespace diff_drive_controller{
     publish_period_ = ros::Duration(1.0 / publish_rate);
 
     controller_nh.param("open_loop", open_loop_, open_loop_);
+    controller_nh.param("estimate_velocity_from_position", estimate_velocity_from_position_, estimate_velocity_from_position_);
+    ROS_INFO_STREAM_COND_NAMED(estimate_velocity_from_position_, name_, "Velocity will be estimated from position.");
+    ROS_INFO_STREAM_COND_NAMED(!estimate_velocity_from_position_, name_, "Velocity will not be estimated from position.");
 
     controller_nh.param("wheel_separation_multiplier", wheel_separation_multiplier_, wheel_separation_multiplier_);
     ROS_INFO_STREAM_NAMED(name_, "Wheel separation will be multiplied by "
@@ -265,8 +269,11 @@ namespace diff_drive_controller{
     }
     else
     {
+      bool trust_velocity = !estimate_velocity_from_position_;
       double left_pos  = 0.0;
       double right_pos = 0.0;
+      double left_vel  = 0.0;
+      double right_vel = 0.0;
       for (size_t i = 0; i < wheel_joints_size_; ++i)
       {
         const double lp = left_wheel_joints_[i].getPosition();
@@ -276,12 +283,34 @@ namespace diff_drive_controller{
 
         left_pos  += lp;
         right_pos += rp;
+
+        if (!trust_velocity)
+          continue;
+        const double lv = left_wheel_joints_[i].getVelocity();
+        const double rv = right_wheel_joints_[i].getVelocity();
+        if (std::isnan(lv) || std::isnan(rv))
+        {
+          trust_velocity = false;
+          continue;
+        }
+
+        left_vel  += lv;
+        right_vel += rv;
       }
       left_pos  /= wheel_joints_size_;
       right_pos /= wheel_joints_size_;
 
       // Estimate linear and angular velocity using joint information
-      odometry_.update(left_pos, right_pos, time);
+      if (!trust_velocity)
+      {
+        odometry_.update(left_pos, right_pos, time);
+      }
+      else
+      {
+        left_vel  /= wheel_joints_size_;
+        right_vel /= wheel_joints_size_;
+        odometry_.update(left_pos, right_pos, left_vel, right_vel, time);
+      }
     }
 
     // Publish odometry message

diff_drive_controller/src/odometry.cpp

@@ -110,6 +110,46 @@ namespace diff_drive_controller
     return true;
   }
 
+  bool Odometry::update(double left_pos, double right_pos,
+                        double left_vel, double right_vel,
+                        const ros::Time &time)
+  {
+    /// Get current wheel joint positions:
+    const double left_wheel_cur_pos  = left_pos  * wheel_radius_;
+    const double right_wheel_cur_pos = right_pos * wheel_radius_;
+
+    /// Get current wheel joint velocities:
+    const double dt = (time - timestamp_).toSec();
+    const double left_wheel_est_vel  = left_vel * wheel_radius_ * dt;
+    const double right_wheel_est_vel = right_vel * wheel_radius_ * dt;
+
+    /// Update old position with current:
+    left_wheel_old_pos_  = left_wheel_cur_pos;
+    right_wheel_old_pos_ = right_wheel_cur_pos;
+
+    /// Compute linear and angular diff:
+    const double linear  = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ;
+    const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_;
+
+    /// Integrate odometry:
+    integrate_fun_(linear, angular);
+
+    /// We cannot estimate the speed with very small time intervals:
+    if (dt < 0.0001)
+      return false; // Interval too small to integrate with
+
+    timestamp_ = time;
+
+    /// Estimate speeds using a rolling mean to filter them out:
+    linear_acc_(linear/dt);
+    angular_acc_(angular/dt);
+
+    linear_ = bacc::rolling_mean(linear_acc_);
+    angular_ = bacc::rolling_mean(angular_acc_);
+
+    return true;
+  }
+
   void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time)
   {
     /// Save last linear and angular velocity:

diff_drive_controller/test/diff_drive_raw_velocity.test

@@ -0,0 +1,18 @@
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <rosparam>
+    diffbot_controller:
+      estimate_velocity_from_position: false
+  </rosparam>
+
+  <!-- Controller test -->
+  <test test-name="diff_raw_velocity_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

diff_drive_controller/test/diff_drive_raw_velocity_invalid.test

@@ -0,0 +1,18 @@
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <rosparam>
+    diffbot_controller:
+      estimate_velocity_from_position: something_invalid
+  </rosparam>
+
+  <!-- Controller test -->
+  <test test-name="diff_raw_velocity_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>