Merge pull request #10 from clearpathrobotics/odometry_from_wheel_joi…

…nt_velocity

Add position_feedback param

diff_drive_controller/CMakeLists.txt

@@ -91,6 +91,7 @@ if (CATKIN_ENABLE_TESTING)
     test/diff_drive_odom_tf_test.cpp)
   target_link_libraries(diff_drive_odom_tf_test ${catkin_LIBRARIES})
   add_rostest(test/diff_drive_open_loop.test)
+  add_rostest(test/diff_drive_velocity_feedback.test)
   add_rostest(test/skid_steer_controller.test)
   add_rostest(test/skid_steer_no_wheels.test)
   add_rostest(test/diff_drive_radius_param.test)

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h

@@ -115,6 +115,7 @@ namespace diff_drive_controller
     ros::Duration publish_period_;
     ros::Time last_state_publish_time_;
     bool open_loop_;
+    bool position_feedback_;
 
     /// Hardware handles:
     std::vector<hardware_interface::JointHandle> left_wheel_joints_;

diff_drive_controller/include/diff_drive_controller/odometry.h

@@ -100,6 +100,16 @@ namespace diff_drive_controller
      */
     bool updateCloseLoop(double left_pos, double right_pos, const ros::Time &time);
 
+    /**
+     * \brief Updates the odometry class with latest wheels velocity, i.e. in
+     * close loop
+     * \param left_vel  [in] Left  wheel velocity [rad/s]
+     * \param right_vel [in] Right wheel velocity [rad/s]
+     * \param time      [in] Current time
+     * \return true if the odometry is actually updated
+     */
+    bool updateCloseLoopFromVelocity(double left_pos, double right_pos, const ros::Time &time);
+
     /**
      * \brief Updates the odometry class with latest velocity command, i.e. in
      * open loop

diff_drive_controller/src/diff_drive_controller.cpp

@@ -119,6 +119,7 @@ namespace diff_drive_controller
 
   DiffDriveController::DiffDriveController()
     : open_loop_(false)
+    , position_feedback_(true)
     , command_struct_()
     , dynamic_params_struct_()
     , wheel_separation_(0.0)
@@ -174,6 +175,13 @@ namespace diff_drive_controller
     publish_period_ = ros::Duration(1.0 / publish_rate);
 
     controller_nh.param("open_loop", open_loop_, open_loop_);
+    ROS_INFO_STREAM_NAMED(name_, "Odometry will be computed in "
+                          << (open_loop_ ? "open" : "close") << " loop.");
+
+    controller_nh.param("position_feedback", position_feedback_, position_feedback_);
+    ROS_DEBUG_STREAM_COND_NAMED(!open_loop_, name_,
+        "Odometry will be computed using the wheel " <<
+        (position_feedback_ ? "postion" : "velocity") << " feedback.");
 
     controller_nh.param("wheel_separation_multiplier",
         wheel_separation_multiplier_, wheel_separation_multiplier_);
@@ -339,23 +347,46 @@ namespace diff_drive_controller
     }
     else
     {
-      double left_pos  = 0.0;
-      double right_pos = 0.0;
-      for (size_t i = 0; i < wheel_joints_size_; ++i)
+      if (position_feedback_)
       {
-        const double lp = left_wheel_joints_[i].getPosition();
-        const double rp = right_wheel_joints_[i].getPosition();
-        if (std::isnan(lp) || std::isnan(rp))
-          return;
+        double left_pos  = 0.0;
+        double right_pos = 0.0;
+        for (size_t i = 0; i < wheel_joints_size_; ++i)
+        {
+          const double lp = left_wheel_joints_[i].getPosition();
+          const double rp = right_wheel_joints_[i].getPosition();
+          if (std::isnan(lp) || std::isnan(rp))
+            return;
 
-        left_pos  += lp;
-        right_pos += rp;
+          left_pos  += lp;
+          right_pos += rp;
+        }
+        left_pos  /= wheel_joints_size_;
+        right_pos /= wheel_joints_size_;
+
+        // Estimate linear and angular velocity using joint position information
+        odometry_.updateCloseLoop(left_pos, right_pos, time);
       }
-      left_pos  /= wheel_joints_size_;
-      right_pos /= wheel_joints_size_;
+      else
+      {
+        double left_vel  = 0.0;
+        double right_vel = 0.0;
+        for (size_t i = 0; i < wheel_joints_size_; ++i)
+        {
+          const double lv = left_wheel_joints_[i].getVelocity();
+          const double rv = right_wheel_joints_[i].getVelocity();
+          if (std::isnan(lv) || std::isnan(rv))
+            return;
 
-      // Estimate linear and angular velocity using joint information
-      odometry_.updateCloseLoop(left_pos, right_pos, time);
+          left_vel  += lv;
+          right_vel += rv;
+        }
+        left_vel  /= wheel_joints_size_;
+        right_vel /= wheel_joints_size_;
+
+        // Estimate linear and angular velocity using joint velocity information
+        odometry_.updateCloseLoopFromVelocity(left_vel, right_vel, time);
+      }
     }
 
     // Publish odometry message

diff_drive_controller/src/odometry.cpp

@@ -119,6 +119,19 @@ namespace diff_drive_controller
     return update(left_wheel_est_vel, right_wheel_est_vel, time);
   }
 
+  bool Odometry::updateCloseLoopFromVelocity(double left_vel, double right_vel, const ros::Time& time)
+  {
+    /// Compute time step:
+    const double dt = (time - timestamp_).toSec();
+
+    /// Convert velocities into displacements/movements:
+    left_vel  *= dt;
+    right_vel *= dt;
+
+    /// Update pose and twist:
+    return update(left_vel, right_vel, time);
+  }
+
   bool Odometry::updateOpenLoop(double linear, double angular, const ros::Time& time)
   {
     /// Compute time step:

diff_drive_controller/test/diff_drive_velocity_feedback.test

@@ -0,0 +1,16 @@
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Load diff drive parameter velocity feedback (position feedback false) -->
+  <rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_velocity_feedback.yaml" />
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_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/diffbot_velocity_feedback.yaml

@@ -0,0 +1,2 @@
+diffbot_controller:
+  position_feedback: false