Separate odom tf publish time

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h

@@ -116,6 +116,7 @@ namespace diff_drive_controller
     /// Odometry related:
     ros::Duration publish_period_;
     ros::Time last_odom_publish_time_;
+    ros::Time last_odom_tf_publish_time_;
     bool open_loop_;
 
     bool pose_from_joint_position_;

diff_drive_controller/include/diff_drive_controller/odometry.h

@@ -97,22 +97,25 @@ namespace diff_drive_controller
      * \brief Constructor
      * Timestamp will get the current time value
      * Value will be set to zero
-     * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean
+     * \param[in] velocity_rolling_window_size Rolling window size used to
+     *                                         compute the velocity mean
      */
-    explicit Odometry(size_t velocity_rolling_window_size = 10);
+    explicit Odometry(const size_t velocity_rolling_window_size = 10);
 
     /**
      * \brief Initialize the odometry
-     * \param time [in] Current time
+     * \param[in] time Current time
      */
     void init(const ros::Time &time);
 
     /**
      * \brief Updates the odometry class with latest wheels position, i.e. in
      * close loop
-     * \param left_pos  [in] Left  wheel position [rad]
-     * \param right_pos [in] Right wheel position [rad]
-     * \param time      [in] Current time
+     * \param[in] left_position  Left  wheel position [rad]
+     * \param[in] right_position Right wheel position [rad]
+     * \param[in] left_velocity  Left  wheel velocity [rad/s]
+     * \param[in] right_velocity Right wheel velocity [rad/s]
+     * \param[in] time           Current time
      * \return true if the odometry is actually updated
      */
     bool updateCloseLoop(
@@ -123,16 +126,17 @@ namespace diff_drive_controller
     /**
      * \brief Updates the odometry class with latest velocity command, i.e. in
      * open loop
-     * \param linear  [in] Linear  velocity [m/s]
-     * \param angular [in] Angular velocity [rad/s]
-     * \param time    [in] Current time
+     * \param[in] linear  Linear  velocity [m/s]
+     * \param[in] angular Angular velocity [rad/s]
+     * \param[in] time    Current time
      * \return true if the odometry is actually updated
      */
-    bool updateOpenLoop(double linear, double angular, const ros::Time &time);
+    bool updateOpenLoop(const double linear, const double angular,
+        const ros::Time &time);
 
     /**
-     * \brief heading getter
-     * \return heading [rad]
+     * \brief Heading getter
+     * \return Heading [rad]
      */
     double getHeading() const
     {
@@ -185,44 +189,44 @@ namespace diff_drive_controller
     }
 
     /**
-     * \brief pose covariance getter
-     * \return pose covariance
+     * \brief Pose covariance getter
+     * \return Pose covariance
      */
     const PoseCovariance& getPoseCovariance() const
     {
       return pose_covariance_;
     }
 
     /**
-     * \brief twist covariance getter
-     * \return twist covariance
+     * \brief Twist covariance getter
+     * \return Twist covariance
      */
     const TwistCovariance& getTwistCovariance() const
     {
       return twist_covariance_;
     }
 
     /**
-     * \brief minimum twist covariance getter
-     * \return minimum twist covariance
+     * \brief Minimum twist covariance getter
+     * \return Minimum twist covariance
      */
     const TwistCovariance& getMinimumTwistCovariance() const
     {
       return minimum_twist_covariance_;
     }
 
     /**
-     * \brief pose covariance setter
-     * \param pose_covariance [in] pose covariance
+     * \brief Pose covariance setter
+     * \param[in] pose_covariance Pose covariance
      */
     void setPoseCovariance(const PoseCovariance& pose_covariance)
     {
       pose_covariance_ = pose_covariance;
     }
 
     /**
-     * \brief minimum twist covariance setter
-     * \param twist_covariance [in] twist covariance
+     * \brief Minimum twist covariance setter
+     * \param[in] twist_covariance Twist covariance
      */
     void setMinimumTwistCovariance(const TwistCovariance& twist_covariance)
     {
@@ -231,26 +235,27 @@ namespace diff_drive_controller
 
     /**
      * \brief Sets the wheel parameters: radius and separation
-     * \param wheel_separation   [in] Seperation between
-     *                                left and right wheels [m]
-     * \param left_wheel_radius  [in] Left  wheel radius [m]
-     * \param right_wheel_radius [in] Right wheel radius [m]
+     * \param[in] wheel_separation   Seperation between
+     *                               left and right wheels [m]
+     * \param[in] left_wheel_radius  Left  wheel radius [m]
+     * \param[in] right_wheel_radius Right wheel radius [m]
      */
-    void setWheelParams(double wheel_separation,
-        double left_wheel_radius, double right_wheel_radius);
+    void setWheelParams(const double wheel_separation,
+        const double left_wheel_radius, const double right_wheel_radius);
 
     /**
      * \brief Sets the Measurement Covariance Model parameters: k_l and k_r
-     * \param k_l [in] Left  wheel velocity multiplier
-     * \param k_r [in] Right wheel velocity multiplier
+     * \param[in] k_l Left  wheel velocity multiplier
+     * \param[in] k_r Right wheel velocity multiplier
      */
-    void setMeasCovarianceParams(double k_l, double k_r);
+    void setMeasCovarianceParams(const double k_l, const double k_r);
 
     /**
      * \brief Velocity rolling window size setter
      * \param[in] velocity_rolling_window_size Velocity rolling window size
      */
-    void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);
+    void setVelocityRollingWindowSize(
+        const size_t velocity_rolling_window_size);
 
   private:
     /// Rolling mean accumulator and window:
@@ -273,22 +278,22 @@ namespace diff_drive_controller
     /**
      * \brief Update the odometry twist with the previous and current odometry
      * pose
-     * \param p0   [in] Previous odometry pose
-     * \param p1   [in] Current  odometry pose
-     * \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] p0   Previous odometry pose
+     * \param[in] p1   Current  odometry pose
+     * \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 twist is actually updated
      */
     bool updateTwist(const SE2& p0, const SE2& p1,
         const double v_l, const double v_r, const ros::Time& time);
 
     /**
      * \brief Update the measurement covariance
-     * \param v_l [in] Left  wheel velocity displacement [rad]
-     * \param v_r [in] Right wheel velocity displacement [rad]
+     * \param[in] dp_l Left  wheel position increment [rad]
+     * \param[in] dp_r Right wheel position increment [rad]
      */
-    void updateMeasCovariance(double v_l, double v_r);
+    void updateMeasCovariance(const double dp_l, const double dp_r);
 
     /**
      * \brief Reset linear and angular accumulators

diff_drive_controller/include/diff_drive_controller/speed_limiter.h

@@ -56,6 +56,8 @@ namespace diff_drive_controller
      * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0
      * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0
      * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0
+     * \param [in] min_deceleration Minimum deceleration [m/s^2], usually <= 0
+     * \param [in] max_deceleration Maximum deceleration [m/s^2], usually >= 0
      */
     SpeedLimiter(
       bool has_velocity_limits = false,
@@ -66,7 +68,9 @@ namespace diff_drive_controller
       double min_acceleration = 0.0,
       double max_acceleration = 0.0,
       double min_jerk = 0.0,
-      double max_jerk = 0.0);
+      double max_jerk = 0.0,
+      double min_deceleration = 0.0,
+      double max_deceleration = 0.0);
 
     /**
      * \brief Limit the velocity and acceleration
@@ -122,6 +126,10 @@ namespace diff_drive_controller
     // Jerk limits:
     double min_jerk;
     double max_jerk;
+
+    // Deceleration limits
+    double min_deceleration;  // Minimum acceleration used to slow down vehicle to zero velocity.
+    double max_deceleration;  // Maximum acceleration used to slow down vehicle to zero velocity.
   };
 
 }  // namespace diff_drive_controller

diff_drive_controller/src/diff_drive_controller.cpp

@@ -270,6 +270,8 @@ namespace diff_drive_controller
     controller_nh.param("linear/x/min_velocity"           , limiter_lin_.min_velocity           , -limiter_lin_.max_velocity          );
     controller_nh.param("linear/x/max_acceleration"       , limiter_lin_.max_acceleration       ,  limiter_lin_.max_acceleration      );
     controller_nh.param("linear/x/min_acceleration"       , limiter_lin_.min_acceleration       , -limiter_lin_.max_acceleration      );
+    controller_nh.param("linear/x/max_deceleration"       , limiter_lin_.max_deceleration       ,  limiter_lin_.max_acceleration      );
+    controller_nh.param("linear/x/min_deceleration"       , limiter_lin_.min_deceleration       ,  limiter_lin_.min_acceleration      );
     controller_nh.param("linear/x/max_jerk"               , limiter_lin_.max_jerk               ,  limiter_lin_.max_jerk              );
     controller_nh.param("linear/x/min_jerk"               , limiter_lin_.min_jerk               , -limiter_lin_.max_jerk              );
 
@@ -280,6 +282,8 @@ namespace diff_drive_controller
     controller_nh.param("angular/z/min_velocity"           , limiter_ang_.min_velocity           , -limiter_ang_.max_velocity          );
     controller_nh.param("angular/z/max_acceleration"       , limiter_ang_.max_acceleration       ,  limiter_ang_.max_acceleration      );
     controller_nh.param("angular/z/min_acceleration"       , limiter_ang_.min_acceleration       , -limiter_ang_.max_acceleration      );
+    controller_nh.param("angular/z/max_deceleration"       , limiter_ang_.max_deceleration       ,  limiter_ang_.max_acceleration      );
+    controller_nh.param("angular/z/min_deceleration"       , limiter_ang_.min_deceleration       ,  limiter_ang_.min_acceleration      );
     controller_nh.param("angular/z/max_jerk"               , limiter_ang_.max_jerk               ,  limiter_ang_.max_jerk              );
     controller_nh.param("angular/z/min_jerk"               , limiter_ang_.min_jerk               , -limiter_ang_.max_jerk              );
 
@@ -573,46 +577,49 @@ namespace diff_drive_controller
       odometry_.updateCloseLoop(left_position, right_position, left_velocity, right_velocity, time);
     }
 
-    // Publish odometry message
+    // Publish odometry message:
     const ros::Duration half_period(0.5 * period.toSec());
-    if (last_odom_publish_time_ + publish_period_ < time + half_period)
+    if (last_odom_publish_time_ + publish_period_ < time + half_period &&
+        odom_pub_->trylock())
     {
-      // @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
+      // Populate odom message and publish:
       const geometry_msgs::Quaternion orientation(
             tf::createQuaternionMsgFromYaw(odometry_.getHeading()));
 
-      // Populate odom message and publish
-      if (odom_pub_->trylock())
-      {
-        odom_pub_->msg_.header.stamp = time;
-        odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
-        odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
-        odom_pub_->msg_.pose.pose.orientation = orientation;
+      odom_pub_->msg_.header.stamp = time;
+      odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
+      odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
+      odom_pub_->msg_.pose.pose.orientation = orientation;
 
-        odom_pub_->msg_.twist.twist.linear.x  = odometry_.getVx();
-        odom_pub_->msg_.twist.twist.linear.y  = odometry_.getVy();
-        odom_pub_->msg_.twist.twist.angular.z = odometry_.getVyaw();
+      odom_pub_->msg_.twist.twist.linear.x  = odometry_.getVx();
+      odom_pub_->msg_.twist.twist.linear.y  = odometry_.getVy();
+      odom_pub_->msg_.twist.twist.angular.z = odometry_.getVyaw();
 
-        covarianceToMsg(odometry_.getPoseCovariance() , odom_pub_->msg_.pose.covariance);
-        covarianceToMsg(odometry_.getTwistCovariance(), odom_pub_->msg_.twist.covariance);
+      covarianceToMsg(odometry_.getPoseCovariance() , odom_pub_->msg_.pose.covariance);
+      covarianceToMsg(odometry_.getTwistCovariance(), odom_pub_->msg_.twist.covariance);
 
-        odom_pub_->unlockAndPublish();
-      }
+      odom_pub_->unlockAndPublish();
+    }
 
-      // Publish tf /odom frame
-      if (enable_odom_tf_ && tf_odom_pub_->trylock())
-      {
-        geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
-        odom_frame.header.stamp = time;
-        odom_frame.transform.translation.x = odometry_.getX();
-        odom_frame.transform.translation.y = odometry_.getY();
-        odom_frame.transform.rotation = orientation;
-        tf_odom_pub_->unlockAndPublish();
-      }
+    // Publish tf odometry frame:
+    if (enable_odom_tf_ &&
+        last_odom_tf_publish_time_ + publish_period_ < time + half_period &&
+        tf_odom_pub_->trylock())
+    {
+      last_odom_tf_publish_time_ = time;
+
+      // Populate tf odometry frame message and publish:
+      const geometry_msgs::Quaternion orientation(
+            tf::createQuaternionMsgFromYaw(odometry_.getHeading()));
+
+      geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
+      odom_frame.header.stamp = time;
+      odom_frame.transform.translation.x = odometry_.getX();
+      odom_frame.transform.translation.y = odometry_.getY();
+      odom_frame.transform.rotation = orientation;
+      tf_odom_pub_->unlockAndPublish();
     }
 
     // MOVE ROBOT
@@ -840,7 +847,7 @@ namespace diff_drive_controller
     brake();
 
     // Register starting time used to keep fixed rate
-    last_odom_publish_time_ = time;
+    last_odom_publish_time_ = last_odom_tf_publish_time_ = time;
 
     odometry_.init(time);
   }

diff_drive_controller/src/odometry.cpp

@@ -242,8 +242,8 @@ namespace diff_drive_controller
                                    k_r_ * std::abs(v_r);
   }
 
-  void Odometry::setWheelParams(double wheel_separation,
-      double left_wheel_radius, double right_wheel_radius)
+  void Odometry::setWheelParams(const double wheel_separation,
+      const double left_wheel_radius, const double right_wheel_radius)
   {
     wheel_separation_   = wheel_separation;
     left_wheel_radius_  = left_wheel_radius;
@@ -253,14 +253,15 @@ namespace diff_drive_controller
         left_wheel_radius, right_wheel_radius);
   }
 
-  void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
+  void Odometry::setVelocityRollingWindowSize(
+      const size_t velocity_rolling_window_size)
   {
     velocity_rolling_window_size_ = velocity_rolling_window_size;
 
     resetAccumulators();
   }
 
-  void Odometry::setMeasCovarianceParams(double k_l, double k_r)
+  void Odometry::setMeasCovarianceParams(const double k_l, const double k_r)
   {
     k_l_ = k_l;
     k_r_ = k_r;