[docs] readme, minor cleanup and parameter/topic name refactor (#8)

Modified a few of the parameter names and topics to make them less confusing. The rest is a copy and slight rewording of the tutorial from the old [ROS1 wiki](http://wiki.ros.org/yocs_velocity_smoother).

.gitignore

@@ -1,4 +1,5 @@
 bin
 build
 lib
-
+test/.cache
+test/__pycache__

CHANGELOG.rst

@@ -4,11 +4,11 @@ Velocity Smoother
 
 Forthcoming
 -----------
-* ...
+* [tests] translational smoothing test added, `#7 <https://github.com/kobuki-base/velocity_smoother/pull/7>`_
 
 0.13.0 (2020-01-20)
 -------------------
-* Refactored for ROS 2 and renamed to velocity_smoother, `#1 <https://github.com/kobuki-base/velocity_smoother/pull/1>`_
+* [infra] refactored for ROS 2 and renamed to velocity_smoother, `#1 <https://github.com/kobuki-base/velocity_smoother/pull/1>`_
 
 0.6.3 (2014-12-05)
 ------------------

README.md

@@ -0,0 +1,52 @@
+# Velocity Smoother
+
+[[About](#about)][[Parameters](#parameters)][[Topics](#topics)][[Usage](#usage)][[Feedback](#feedback)]
+
+## About
+
+ROS2 package for smoothing commanded velocities represented by a stream of
+`geometry_msg/Twist` messages. It applies limits to linear and angular
+components of both speed and acceleration. Feedback from either
+the odometry / actual commanded velocity can be used to provide
+a better result in some situations (read further below).
+
+![Profiles](test/translational_smoothing_profiles.png)
+
+## Parameters
+
+* **~accel_lim_v** (double, default: 0.8): linear acceleration limit
+* **~accel_lim_w** (double, default: 5.4): angular acceleration limit
+* **~speed_lim_v** (double, default: 0.3): linear velocity limit
+* **~speed_lim_w** (double, default: 3.5): angular velocity limit
+* **~decel_factor** (double, default: 1.0): multiplier for the acceleration limit when decelerating
+* **~frequency** (double, default: 20.0): computed and published rate, adhered to regardless of the incoming message rate (interpolates when necessary)
+* **~feedback** (int, default: 0): the type of feedback to use (0 - none, 1 - odometry, 2 - actual commanded velocities)
+
+## Topics
+
+Subscriptions
+
+* **~/input** (`geometry_msgs/Twist`): input velocity commands
+* **~/feedback/odometry** (`nav_msgs/Odometry`): topic for odometry feedback, only required if the requested feedback has been set for odometry
+* **~/feedback/cmd_vel** (`geometry_msgs/Twist`): topic for actual commanded velocity feedback, only required if the requested feedback has been set for actual commanded velocities
+
+Publications
+
+* **~smoothed** (`geometry_msgs/Twist`): smoothed output velocity commands respecting velocity and acceleration limits
+
+## Usage
+
+* All the parameters except frequency are dynamically reconfigurable.
+* Linear and angular velocities are smoothed proportionally to the more restricted, so we guaranty a constant rotation radius.
+* If the input topic gets inactive, and the last command is not a zero-velocity one, (maybe the controller crashed, or just forgot good manners...), we introduce a fake zero-velocity command after a sort timeout.
+
+Simply wire up the channels to their appropriate topics. The tests can be a useful starting point.
+
+## Feedback
+
+There are some reasons to use robot feedback. The two most frequently faced:
+
+* Multiple controllers compete for controlling the robot via a multiplexer. A controller that has been excluded by another with higher priority may suddenly issue commands significantly different to the
+last commanded velocity when the multiplexer has switched back to it. i.e. the commanded velocity
+profile experiences a large, discrete jump. In these cases, option 2 is very useful. 
+* The robot fails to generate the commanded velocity due to, for example, unmodelled inclines, carpets etc. In these cases, option 1 is very useful.

config/velocity_smoother_params.yaml

@@ -3,19 +3,21 @@
 # - acceleration limits are just low enough to avoid jerking
 velocity_smoother:
   ros__parameters:
-    # Mandatory parameters
+    # limits
     speed_lim_v: 0.8
     speed_lim_w: 5.4
 
     accel_lim_v: 0.3
     accel_lim_w: 3.5
 
-    # Optional parameters
-    frequency: 20.0
+    # multiply the acceleration limit by this to permit faster decellerations
     decel_factor: 1.0
 
-    # Robot velocity feedback type:
+    # recompute smoothed velocities at this rate
+    frequency: 20.0
+
+    # feedback type:
     #  0 - none
     #  1 - odometry
-    #  2 - robot commands (cmd_vel)
-    robot_feedback: 2
+    #  2 - actual commanded velocity (e.g. after it's been piped through a mux)
+    feedback: 2

include/velocity_smoother/velocity_smoother.hpp

@@ -53,7 +53,7 @@ class VelocitySmoother final : public rclcpp::Node
     NONE,
     ODOMETRY,
     COMMANDS
-  } robot_feedback_;  /**< What source to use as robot velocity feedback */
+  } feedback_;  /**< What source to use as feedback for smoothed velocity calculations */
 
   bool quiet_;        /**< Quieten some warnings that are unavoidable because of velocity multiplexing. **/
   double speed_lim_v_, accel_lim_v_, decel_lim_v_;

package.xml

@@ -29,6 +29,7 @@
   <test_depend>launch_testing</test_depend>
   <test_depend>launch_testing_ament_cmake</test_depend>
   <test_depend>launch_testing_ros</test_depend>
+  <test_depend>python3-matplotlib</test_depend>
   <test_depend>ros2test</test_depend>
 
   <export>

src/velocity_smoother.cpp

@@ -49,14 +49,14 @@ VelocitySmoother::VelocitySmoother(const rclcpp::NodeOptions & options) : rclcpp
   double frequency = this->declare_parameter("frequency", 20.0);
   quiet_ = this->declare_parameter("quiet", false);
   decel_factor_ = this->declare_parameter("decel_factor", 1.0);
-  int feedback = this->declare_parameter("robot_feedback", static_cast<int>(NONE));
+  int feedback = this->declare_parameter("feedback", static_cast<int>(NONE));
 
   if ((static_cast<int>(feedback) < NONE) || (static_cast<int>(feedback) > COMMANDS))
   {
     throw std::runtime_error("Invalid robot feedback type. Valid options are 0 (NONE, default), 1 (ODOMETRY) and 2 (COMMANDS)");
   }
 
-  robot_feedback_ = static_cast<RobotFeedbackType>(feedback);
+  feedback_ = static_cast<RobotFeedbackType>(feedback);
 
   // Mandatory parameters
   rclcpp::ParameterValue speed_v = this->declare_parameter(
@@ -100,10 +100,10 @@ VelocitySmoother::VelocitySmoother(const rclcpp::NodeOptions & options) : rclcpp
   decel_lim_w_ = decel_factor_*accel_lim_w_;
 
   // Publishers and subscribers
-  odometry_sub_ = this->create_subscription<nav_msgs::msg::Odometry>("odometry", rclcpp::QoS(1), std::bind(&VelocitySmoother::odometryCB, this, std::placeholders::_1));
-  current_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>("robot_cmd_vel", rclcpp::QoS(1), std::bind(&VelocitySmoother::robotVelCB, this, std::placeholders::_1));
-  raw_in_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>("raw_cmd_vel", rclcpp::QoS(1), std::bind(&VelocitySmoother::velocityCB, this, std::placeholders::_1));
-  smooth_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("smooth_cmd_vel", 1);
+  odometry_sub_ = this->create_subscription<nav_msgs::msg::Odometry>("~/feedback/odometry", rclcpp::QoS(1), std::bind(&VelocitySmoother::odometryCB, this, std::placeholders::_1));
+  current_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>("~/feedback/cmd_vel", rclcpp::QoS(1), std::bind(&VelocitySmoother::robotVelCB, this, std::placeholders::_1));
+  raw_in_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>("~/input", rclcpp::QoS(1), std::bind(&VelocitySmoother::velocityCB, this, std::placeholders::_1));
+  smooth_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("~/smoothed", 1);
 
   period_ = 1.0 / frequency;
   timer_ = this->create_wall_timer(std::chrono::milliseconds(static_cast<uint64_t>(period_ * 1000.0)), std::bind(&VelocitySmoother::timerCB, this));
@@ -156,7 +156,7 @@ void VelocitySmoother::velocityCB(const geometry_msgs::msg::Twist::SharedPtr msg
 
 void VelocitySmoother::odometryCB(const nav_msgs::msg::Odometry::SharedPtr msg)
 {
-  if (robot_feedback_ == ODOMETRY)
+  if (feedback_ == ODOMETRY)
   {
     current_vel_ = msg->twist.twist;
   }
@@ -166,7 +166,7 @@ void VelocitySmoother::odometryCB(const nav_msgs::msg::Odometry::SharedPtr msg)
 
 void VelocitySmoother::robotVelCB(const geometry_msgs::msg::Twist::SharedPtr msg)
 {
-  if (robot_feedback_ == COMMANDS)
+  if (feedback_ == COMMANDS)
   {
     current_vel_ = *msg;
   }
@@ -207,7 +207,7 @@ void VelocitySmoother::timerCB()
   bool v_different_from_feedback = current_vel_.linear.x < v_deviation_lower_bound || current_vel_.linear.x > v_deviation_upper_bound;
   bool w_different_from_feedback = current_vel_.angular.z < w_deviation_lower_bound || current_vel_.angular.z > w_deviation_upper_bound;
 
-  if ((robot_feedback_ != NONE) && (input_active_ == true) && (cb_avg_time_ > 0.0) &&
+  if ((feedback_ != NONE) && (input_active_ == true) && (cb_avg_time_ > 0.0) &&
       (((this->get_clock()->now() - last_velocity_cb_time_).seconds() > 5.0*cb_avg_time_)     || // 5 missing msgs
           v_different_from_feedback || w_different_from_feedback))
   {
@@ -218,7 +218,7 @@ void VelocitySmoother::timerCB()
       // this condition can be unavoidable due to preemption of current velocity control on
       // velocity multiplexer so be quiet if we're instructed to do so
       RCLCPP_WARN(get_logger(), "Velocity Smoother : using robot velocity feedback %s instead of last command: %f, %f, %f",
-                  std::string(robot_feedback_ == ODOMETRY ? "odometry" : "end commands").c_str(),
+                  std::string(feedback_ == ODOMETRY ? "odometry" : "end commands").c_str(),
                   (this->get_clock()->now() - last_velocity_cb_time_).seconds(),
                   current_vel_.linear.x  - last_cmd_vel_linear_x_,
                   current_vel_.angular.z - last_cmd_vel_angular_z_);
@@ -239,7 +239,7 @@ void VelocitySmoother::timerCB()
     double v_inc, w_inc, max_v_inc, max_w_inc;
 
     v_inc = target_vel_.linear.x - last_cmd_vel_linear_x_;
-    if ((robot_feedback_ == ODOMETRY) && (current_vel_.linear.x*target_vel_.linear.x < 0.0))
+    if ((feedback_ == ODOMETRY) && (current_vel_.linear.x*target_vel_.linear.x < 0.0))
     {
       // countermarch (on robots with significant inertia; requires odometry feedback to be detected)
       max_v_inc = decel_lim_v_*period_;
@@ -250,7 +250,7 @@ void VelocitySmoother::timerCB()
     }
 
     w_inc = target_vel_.angular.z - last_cmd_vel_angular_z_;
-    if ((robot_feedback_ == ODOMETRY) && (current_vel_.angular.z*target_vel_.angular.z < 0.0))
+    if ((feedback_ == ODOMETRY) && (current_vel_.angular.z*target_vel_.angular.z < 0.0))
     {
       // countermarch (on robots with significant inertia; requires odometry feedback to be detected)
       max_w_inc = decel_lim_w_*period_;
@@ -341,10 +341,10 @@ rcl_interfaces::msg::SetParametersResult VelocitySmoother::parameterUpdate(
 
       decel_factor_ = parameter.get_value<double>();
     }
-    else if (parameter.get_name() == "robot_feedback")
+    else if (parameter.get_name() == "feedback")
     {
       result.successful = false;
-      result.reason = "robot_feedback cannot be changed on-the-fly";
+      result.reason = "feedback cannot be changed on-the-fly";
       break;
     }
     else if (parameter.get_name() == "speed_lim_v")