diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h index a05e37daf..e0cb4d408 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h +++ b/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 --git a/diff_drive_controller/include/diff_drive_controller/odometry.h b/diff_drive_controller/include/diff_drive_controller/odometry.h index 47615d121..d92766a97 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.h +++ b/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,8 +189,8 @@ namespace diff_drive_controller } /** - * \brief pose covariance getter - * \return pose covariance + * \brief Pose covariance getter + * \return Pose covariance */ const PoseCovariance& getPoseCovariance() const { @@ -194,8 +198,8 @@ namespace diff_drive_controller } /** - * \brief twist covariance getter - * \return twist covariance + * \brief Twist covariance getter + * \return Twist covariance */ const TwistCovariance& getTwistCovariance() const { @@ -203,8 +207,8 @@ namespace diff_drive_controller } /** - * \brief minimum twist covariance getter - * \return minimum twist covariance + * \brief Minimum twist covariance getter + * \return Minimum twist covariance */ const TwistCovariance& getMinimumTwistCovariance() const { @@ -212,8 +216,8 @@ namespace diff_drive_controller } /** - * \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) { @@ -221,8 +225,8 @@ namespace diff_drive_controller } /** - * \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,11 +278,11 @@ 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, @@ -285,10 +290,10 @@ namespace diff_drive_controller /** * \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 --git a/diff_drive_controller/include/diff_drive_controller/speed_limiter.h b/diff_drive_controller/include/diff_drive_controller/speed_limiter.h index 9be008007..67624bbea 100644 --- a/diff_drive_controller/include/diff_drive_controller/speed_limiter.h +++ b/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 --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 64d3a9be3..d3710fda4 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/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 --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index e86a28398..213b3f918 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/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; diff --git a/diff_drive_controller/src/speed_limiter.cpp b/diff_drive_controller/src/speed_limiter.cpp index a7e84c621..794d1e519 100644 --- a/diff_drive_controller/src/speed_limiter.cpp +++ b/diff_drive_controller/src/speed_limiter.cpp @@ -58,7 +58,9 @@ namespace diff_drive_controller double min_acceleration, double max_acceleration, double min_jerk, - double max_jerk + double max_jerk, + double min_deceleration, + double max_deceleration ) : has_velocity_limits(has_velocity_limits) , has_acceleration_limits(has_acceleration_limits) @@ -69,6 +71,8 @@ namespace diff_drive_controller , max_acceleration(max_acceleration) , min_jerk(min_jerk) , max_jerk(max_jerk) + , min_deceleration(min_deceleration) + , max_deceleration(max_deceleration) { } @@ -101,11 +105,45 @@ namespace diff_drive_controller if (has_acceleration_limits) { - const double dv_min = min_acceleration * dt; - const double dv_max = max_acceleration * dt; - - const double dv = clamp(v - v0, dv_min, dv_max); - + double dv_min, dv_max, dv; + + if (v0 >= 0.0 && v >= 0.0) + { + // Moving in positive direction + dv_min = min_deceleration * dt; + dv_max = max_acceleration * dt; + } + else if (v0 <= 0.0 && v <= 0.0) + { + // Moving in the negative direction + dv_min = min_acceleration * dt; + dv_max = max_deceleration * dt; + } + else + { + // Transitioning from positive to negative velocity + if (v0 > 0) + { + dv_max = v0; + dv_min = min_deceleration * dt; + if (v0 + dv < 0) + { + dv_min = min_acceleration * (dt - v0 / min_deceleration); + } + } + // Transitioning from negative to positive velocity + else if (v0 < 0) + { + dv_min = v0; + dv_max = max_deceleration * dt; + if (v0 + dv > 0) + { + dv_max = max_acceleration * (dt - v0 / max_deceleration); + } + } + } + + dv = clamp(v - v0, dv_min, dv_max); v = v0 + dv; }