Allow computing pose/twist from position/velocity

diff_drive_controller/cfg/DiffDriveController.cfg

@@ -6,14 +6,17 @@ from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, b
 
 gen = ParameterGenerator()
 
+gen.add("pose_from_joint_position", bool_t, 0, "Compute odometry pose from wheel joint position.", True)
+gen.add("twist_from_joint_position", bool_t, 0, "Compute odometry twist from wheel joint position.", False)
+
 gen.add("wheel_separation_multiplier",  double_t, 0, "Wheel separation multiplier.", 1.0, 0.5, 1.5)
 gen.add("left_wheel_radius_multiplier",  double_t, 0, "Left wheel radius multiplier.", 1.0, 0.5, 1.5)
 gen.add("right_wheel_radius_multiplier",  double_t, 0, "Right wheel radius multiplier.", 1.0, 0.5, 1.5)
 
 gen.add("k_l",  double_t, 0, "Covariance model k_l multiplier.", 1.0, 0.0, 10.0)
 gen.add("k_r",  double_t, 0, "Covariance model k_r multiplier.", 1.0, 0.0, 10.0)
 
-gen.add("publish_state", bool_t, 0, "Publish joint trajectory controller state.", False)
 gen.add("publish_cmd_vel_limited", bool_t, 0, "Publish limited velocity command.", False)
+gen.add("publish_state", bool_t, 0, "Publish joint trajectory controller state.", False)
 
 exit(gen.generate(PACKAGE, "diff_drive_controller", "DiffDriveController"))

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h

@@ -117,7 +117,12 @@ namespace diff_drive_controller
     ros::Duration publish_period_;
     ros::Time last_odom_publish_time_;
     bool open_loop_;
-    bool position_feedback_;
+
+    bool pose_from_joint_position_;
+    bool twist_from_joint_position_;
+
+    bool use_position_;
+    bool use_velocity_;
 
     /// Hardware handles:
     std::vector<hardware_interface::JointHandle> left_wheel_joints_;
@@ -181,6 +186,9 @@ namespace diff_drive_controller
 
     struct DynamicParams
     {
+      bool pose_from_joint_position;
+      bool twist_from_joint_position;
+
       double wheel_separation_multiplier;
       double left_wheel_radius_multiplier;
       double right_wheel_radius_multiplier;
@@ -192,7 +200,9 @@ namespace diff_drive_controller
       bool publish_cmd_vel_limited;
 
       DynamicParams()
-        : wheel_separation_multiplier(1.0)
+        : pose_from_joint_position(true)
+        , twist_from_joint_position(false)
+        , wheel_separation_multiplier(1.0)
         , left_wheel_radius_multiplier(1.0)
         , right_wheel_radius_multiplier(1.0)
         , k_l(1.0)

diff_drive_controller/include/diff_drive_controller/odometry.h

@@ -115,17 +115,10 @@ namespace diff_drive_controller
      * \param time      [in] Current time
      * \return true if the odometry is actually updated
      */
-    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);
+    bool updateCloseLoop(
+        const double left_position, const double right_position,
+        const double left_velocity, const double right_velocity,
+        const ros::Time &time);
 
     /**
      * \brief Updates the odometry class with latest velocity command, i.e. in
@@ -267,12 +260,15 @@ namespace diff_drive_controller
     /**
      * \brief Updates the odometry class with latest velocity command and wheel
      * velocities
-     * \param v_l  [in] Left  wheel velocity displacement [rad]
-     * \param v_r  [in] Right wheel velocity displacement [rad]
-     * \param time [in] Current time
+     * \param[in] dp_l  Left  wheel position increment [rad]
+     * \param[in] dp_r  Right wheel position increment [rad]
+     * \param[in] v_l   Left  wheel velocity [rad/s]
+     * \param[in] v_r   Right wheel velocity [rad/s]
+     * \param[in] time  Current time
      * \return true if the odometry is actually updated
      */
-    bool update(double v_l, double v_r, const ros::Time& time);
+    bool update(const double dp_l, const double dp_r,
+        const double v_l, const double v_r, const ros::Time& time);
 
     /**
      * \brief Update the odometry twist with the previous and current odometry
@@ -332,8 +328,8 @@ namespace diff_drive_controller
     double k_r_;
 
     /// Previous wheel position/state [rad]:
-    double left_wheel_old_pos_;
-    double right_wheel_old_pos_;
+    double left_position_previous_;
+    double right_position_previous_;
 
     /// Rolling mean accumulators for the linar and angular velocities:
     size_t velocity_rolling_window_size_;

diff_drive_controller/src/diff_drive_controller.cpp

@@ -123,7 +123,8 @@ namespace diff_drive_controller
 
   DiffDriveController::DiffDriveController()
     : open_loop_(false)
-    , position_feedback_(true)
+    , pose_from_joint_position_(true)
+    , twist_from_joint_position_(false)
     , command_struct_()
     , dynamic_params_struct_()
     , wheel_separation_(0.0)
@@ -184,10 +185,18 @@ namespace diff_drive_controller
     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("pose_from_joint_position", pose_from_joint_position_, pose_from_joint_position_);
+    ROS_INFO_STREAM_COND_NAMED(!open_loop_, name_,
+        "Odometry pose will be computed using the wheel joint " <<
+        (pose_from_joint_position_ ? "position" : "velocity") << " feedback.");
+
+    controller_nh.param("twist_from_joint_position", twist_from_joint_position_, twist_from_joint_position_);
+    ROS_INFO_STREAM_COND_NAMED(!open_loop_, name_,
+        "Odometry twist will be computed using the wheel joint " <<
+        (twist_from_joint_position_ ? "position" : "velocity") << " feedback.");
+
+    use_position_ =  pose_from_joint_position_ ||  twist_from_joint_position_;
+    use_velocity_ = !pose_from_joint_position_ || !twist_from_joint_position_;
 
     controller_nh.param("wheel_separation_multiplier",
         wheel_separation_multiplier_, wheel_separation_multiplier_);
@@ -303,6 +312,9 @@ namespace diff_drive_controller
                           "Measurement Covariance Model params : k_l " << k_l_
                           << ", k_r " << k_r_);
 
+    dynamic_params_struct_.pose_from_joint_position = pose_from_joint_position_;
+    dynamic_params_struct_.twist_from_joint_position = twist_from_joint_position_;
+
     dynamic_params_struct_.wheel_separation_multiplier = wheel_separation_multiplier_;
 
     dynamic_params_struct_.left_wheel_radius_multiplier  = left_wheel_radius_multiplier_;
@@ -449,6 +461,9 @@ namespace diff_drive_controller
     //
     //   ...
     // }
+    pose_from_joint_position_ = dynamic_params.pose_from_joint_position;
+    twist_from_joint_position_ = dynamic_params.twist_from_joint_position;
+
     wheel_separation_multiplier_ = dynamic_params.wheel_separation_multiplier;
     left_wheel_radius_multiplier_ = dynamic_params.left_wheel_radius_multiplier;
     right_wheel_radius_multiplier_ = dynamic_params.right_wheel_radius_multiplier;
@@ -469,51 +484,44 @@ namespace diff_drive_controller
     odometry_.setMeasCovarianceParams(k_l_, k_r_);
 
     // COMPUTE AND PUBLISH ODOMETRY
-    if (open_loop_)
+    // Read wheel joint positions and velocities:
+    for (size_t i = 0; i < wheel_joints_size_; ++i)
     {
-      odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, time);
+      left_positions_[i]  = left_wheel_joints_[i].getPosition();
+      right_positions_[i] = right_wheel_joints_[i].getPosition();
+
+      left_velocities_[i]  = left_wheel_joints_[i].getVelocity();
+      right_velocities_[i] = right_wheel_joints_[i].getVelocity();
     }
-    else
+
+    // Check for NaNs on wheel joint positions if we're going to use them:
+    use_position_ =  pose_from_joint_position_ ||  twist_from_joint_position_;
+    use_velocity_ = !pose_from_joint_position_ || !twist_from_joint_position_;
+
+    if (use_position_)
     {
-      if (position_feedback_)
+      for (size_t i = 0; i < wheel_joints_size_; ++i)
       {
-        double left_pos  = 0.0;
-        double right_pos = 0.0;
-        for (size_t i = 0; i < wheel_joints_size_; ++i)
+        if (std::isnan(left_positions_[i]) ||
+            std::isnan(right_positions_[i]))
         {
-          left_positions_[i] = left_wheel_joints_[i].getPosition();
-          right_positions_[i] = right_wheel_joints_[i].getPosition();
-          if (std::isnan(left_positions_[i]) || std::isnan(right_positions_[i]))
-            return;
-
-          left_pos  += left_positions_[i];
-          right_pos += right_positions_[i];
+          // @todo add a diagnostic message
+          return;
         }
-        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);
       }
-      else
+    }
+
+    // Check for NaNs on wheel joint velocities if we're going to use them:
+    if (use_velocity_)
+    {
+      for (size_t i = 0; i < wheel_joints_size_; ++i)
       {
-        double left_vel  = 0.0;
-        double right_vel = 0.0;
-        for (size_t i = 0; i < wheel_joints_size_; ++i)
+        if (std::isnan(left_velocities_[i]) ||
+            std::isnan(right_velocities_[i]))
         {
-          left_velocities_[i] = left_wheel_joints_[i].getVelocity();
-          right_velocities_[i] = right_wheel_joints_[i].getVelocity();
-          if (std::isnan(left_velocities_[i]) || std::isnan(right_velocities_[i]))
-            return;
-
-          left_vel  += left_velocities_[i];
-          right_vel += right_velocities_[i];
+          // @todo add a diagnostic message
+          return;
         }
-        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);
       }
     }
 
@@ -547,10 +555,30 @@ namespace diff_drive_controller
     const double left_velocity_estimated_average  = std::accumulate(left_velocities_estimated_.begin(), left_velocities_estimated_.end(), 0.0) / wheel_joints_size_;
     const double right_velocity_estimated_average = std::accumulate(right_velocities_estimated_.begin(), right_velocities_estimated_.end(), 0.0) / wheel_joints_size_;
 
+    // Set the wheel joint position that will be used to compute the pose:
+    const double left_position  = pose_from_joint_position_ ? left_position_average  : left_position_estimated_average;
+    const double right_position = pose_from_joint_position_ ? right_position_average : right_position_estimated_average;
+
+    // Set the wheel joint velocity that will be used to compute the twist:
+    const double left_velocity  = twist_from_joint_position_ ? left_velocity_estimated_average  : left_velocity_average;
+    const double right_velocity = twist_from_joint_position_ ? right_velocity_estimated_average : right_velocity_average;
+
+    // Update odometry:
+    if (open_loop_)
+    {
+      odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, time);
+    }
+    else
+    {
+      odometry_.updateCloseLoop(left_position, right_position, left_velocity, right_velocity, time);
+    }
+
     // Publish odometry message
     const ros::Duration half_period(0.5 * period.toSec());
     if (last_odom_publish_time_ + publish_period_ < time + half_period)
     {
+      // @todo should be after the trylock
+      // and duplicate the condition for the odom and odom_tf!
       last_odom_publish_time_ = time;
 
       // Compute and store orientation info
@@ -602,6 +630,7 @@ namespace diff_drive_controller
     // Limit velocities and accelerations:
     const double cmd_dt(period.toSec());
 
+    // @todo add an option to limit the velocity considering the actual velocity
     limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt);
     limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt);
 
@@ -620,7 +649,7 @@ namespace diff_drive_controller
     }
 
     // Publish limited velocity command:
-    if (dynamic_params.publish_cmd_vel_limited && cmd_vel_limited_pub_->trylock())
+    if (publish_cmd_vel_limited_ && cmd_vel_limited_pub_->trylock())
     {
       cmd_vel_limited_pub_->msg_.header.stamp = time;
 
@@ -854,6 +883,9 @@ namespace diff_drive_controller
   void DiffDriveController::reconfigureCallback(
       DiffDriveControllerConfig& config, uint32_t level)
   {
+    dynamic_params_struct_.pose_from_joint_position = config.pose_from_joint_position;
+    dynamic_params_struct_.twist_from_joint_position = config.twist_from_joint_position;
+
     dynamic_params_struct_.wheel_separation_multiplier = config.wheel_separation_multiplier;
 
     dynamic_params_struct_.left_wheel_radius_multiplier  = config.left_wheel_radius_multiplier;
@@ -869,6 +901,8 @@ namespace diff_drive_controller
 
     ROS_DEBUG_STREAM_NAMED(name_,
                           "Reconfigured Odometry params. "
+                          << "pose odometry computed from: " << (dynamic_params_struct_.pose_from_joint_position ? "position" : "velocity") << ", "
+                          << "twist odometry computed from: " << (dynamic_params_struct_.twist_from_joint_position ? "position" : "velocity") << ", "
                           << "wheel separation:   " << dynamic_params_struct_.wheel_separation_multiplier << ", "
                           << "left wheel radius:  " << dynamic_params_struct_.left_wheel_radius_multiplier << ", "
                           << "right wheel radius: " << dynamic_params_struct_.left_wheel_radius_multiplier);