使用yocs_velocity_smoother对机器人速度进行限制

　　yocs_velocity_smoother是一个速度、加速度限制器，用来防止robot navigation的速度/转速过快，加速度/快减速过大。Bound incoming velocity messages according to robot velocity and acceleration limits. The velocity smoother nodelet runs together with the kobuki_node to apply robot's velocity and acceleration limits to the incoming commands before resending them to the robot. It will however also work for any other generic ros mobile base driver.

　　输入下面命令安装yocs_velocity_smoother或者从GitHub上下载源代码进行编译。

sudo apt-get install ros-kinetic-yocs-velocity-smoother

订阅的话题

• raw_cmd_vel：Input velocity commands. 原始速度指令
• odometry：We compare the output velocity commands to measured velocity to ensure we don't create very big jumps in the velocity profile.
• robot_cmd_vel：Alternatively, we can also compare the output velocity commands to end robot velocity commands to ensure we don't create very big jumps in the velocity profile. See robot_feedback parameter description below for more details.

发布的话题

• smooth_cmd_vel：Smoothed output velocity commands respecting velocity and acceleration limits. 限制幅度后的速度

launch文件

　　standalone.launch文件先运行一个名为nodelet_manager的nodelet manager节点，然后加载velocity_smoother.launch文件：

<!--
  Example standalone launcher for the velocity smoother
 -->
<launch>
  <arg name="node_name"             value="velocity_smoother"/>
  <arg name="nodelet_manager_name"  value="nodelet_manager"/>
  <arg name="config_file"           value="$(find yocs_velocity_smoother)/param/standalone.yaml"/>
  <arg name="raw_cmd_vel_topic"     value="raw_cmd_vel"/>
  <arg name="smooth_cmd_vel_topic"  value="smooth_cmd_vel"/>
  <arg name="robot_cmd_vel_topic"   value="robot_cmd_vel"/>
  <arg name="odom_topic"            value="odom"/>

  <!-- nodelet manager -->
  <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager_name)" args="manager"/>

  <!-- velocity smoother -->
  <include file="$(find yocs_velocity_smoother)/launch/velocity_smoother.launch">
    <arg name="node_name"             value="$(arg node_name)"/>
    <arg name="nodelet_manager_name"  value="$(arg nodelet_manager_name)"/>
    <arg name="config_file"           value="$(arg config_file)"/>
    <arg name="raw_cmd_vel_topic"     value="$(arg raw_cmd_vel_topic)"/>
    <arg name="smooth_cmd_vel_topic"  value="$(arg smooth_cmd_vel_topic)"/>
    <arg name="robot_cmd_vel_topic"   value="$(arg robot_cmd_vel_topic)"/>
    <arg name="odom_topic"            value="$(arg odom_topic)"/>
  </include>
</launch>

　　在standalone.launch中使用nodelet_manager加载yocs_velocity_smoother/VelocitySmootherNodelet.

<!--
  YOCS velocity smoother launcher
  -->

<launch>
  <arg name="node_name"             default="velocity_smoother"/>
  <arg name="nodelet_manager_name"  default="nodelet_manager"/>
  <arg name="config_file"           default="$(find yocs_velocity_smoother)/param/standalone.yaml"/>
  <arg name="raw_cmd_vel_topic"     default="raw_cmd_vel"/>
  <arg name="smooth_cmd_vel_topic"  default="smooth_cmd_vel"/>
  <arg name="robot_cmd_vel_topic"   default="robot_cmd_vel"/>
  <arg name="odom_topic"            default="odom"/>

  <node pkg="nodelet" type="nodelet" name="$(arg node_name)"
        args="load yocs_velocity_smoother/VelocitySmootherNodelet $(arg nodelet_manager_name)">

    <!-- parameters -->
    <rosparam file="$(arg config_file)" command="load"/>

    <!-- velocity commands I/O -->
    <remap from="$(arg node_name)/raw_cmd_vel"    to="$(arg raw_cmd_vel_topic)"/>
    <remap from="$(arg node_name)/smooth_cmd_vel" to="$(arg smooth_cmd_vel_topic)"/>

    <!-- Robot velocity feedbacks -->
    <remap from="$(arg node_name)/robot_cmd_vel"  to="$(arg robot_cmd_vel_topic)"/>
    <remap from="$(arg node_name)/odometry"       to="$(arg odom_topic)"/>
  </node>
</launch>

　　standalone.yaml文件用于配置速度平滑参数：

# Example configuration:
# - velocity limits are around a 10% above the physical limits
# - acceleration limits are just low enough to avoid jerking

# Mandatory parameters
speed_lim_v: 0.8  # Linear velocity limit
speed_lim_w: 5.4  # Angular velocity limit

accel_lim_v: 0.3  # Linear acceleration limit
accel_lim_w: 3.5  # Angular acceleration limit

# Optional parameters
frequency: 20.0   # Output messages rate. The velocity smoother keeps it regardless incoming messages rate, interpolating whenever necessary
decel_factor: 1.0 # Deceleration/acceleration ratio. Useful to make deceleration more aggressive

# Robot velocity feedback type:
#  0 - none
#  1 - odometry
#  2 - end robot commands
robot_feedback: 2

速度限制测试

　　yocs_velocity_smoothe包的源文件中有一个测试程序，可以测试速度限制的效果。测试launch文件test_translational_smoothing.launch如下：

<!--Tests the velocity smoother with varied translational inputs.-->
<launch>

    <!-- Launch a nodelet manager node -->
      <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>

    <!--  Launch a nodelet of type pkg/Type on manager manager -->
    <node pkg="nodelet" type="nodelet" name="velocity_smoother" args="load yocs_velocity_smoother/VelocitySmootherNodelet nodelet_manager" output="screen">
        <rosparam file="$(find yocs_velocity_smoother)/param/standalone.yaml" command="load"/>
        <!-- Subscribed Topics -->
        <remap from="velocity_smoother/odometry" to="odom"/>
        <remap from="velocity_smoother/robot_cmd_vel" to="cmd_vel/output"/>
        <remap from="velocity_smoother/raw_cmd_vel" to="cmd_vel/input"/>
        <!-- Published Topics -->
        <remap from="velocity_smoother/smooth_cmd_vel" to="cmd_vel/output"/>
    </node>

    <!-- Launch test node -->
    <node pkg="yocs_velocity_smoother" type="test_translational_input.py" name="test_translational_input" output="screen">
        <remap from="test_translational_input/cmd_vel" to="cmd_vel/input"/>
        <remap from="test_translational_input/odom" to="odom"/>
        <!--<param name="velocity_maximum" value="0.50"/> -->
        <param name="ramp_increment" value="0.01"/>
        <param name="ramp_decrement" value="0.01"/>
    </node>
</launch>

　　test_translational_input.py程序中根据加速度参数（ramp_increment、ramp_decrement）周期性的调整X方向的移动速度，并将cmd_vel和odom信息发布出去。注意在launch文件中又分别将test_translational_input/cmd_vel和test_translational_input/odom重映射为cmd_vel/input与odom.

#!/usr/bin/env python
import roslib
roslib.load_manifest('yocs_velocity_smoother')
import rospy

import os
import sys
import time
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
'''
  Varied translational input for a velocity smoother test.
'''

STATE_RAMP_UP = 0
STATE_RAMP_LEVEL = 1
STATE_RAMP_DOWN = 2
STATE_ZERO = 3
STATE_UP = 4
STATE_DOWN = 5
STATE_UP_AGAIN = 6
STATE_NOTHING = 7

def main():
    rospy.init_node("test_velocity_smoother_input")
    cmd_vel_publisher = rospy.Publisher("~cmd_vel", Twist)
    odom_publisher = rospy.Publisher("~odom", Odometry)
    param = {}
    param['velocity_maximum'] = rospy.get_param("~velocity_maximum", 0.50)
    param['ramp_increment'] = rospy.get_param("~ramp_increment", 0.02)
    rospy.loginfo("Test Input : ramp increment [%f]",param['ramp_increment'])
    param['ramp_decrement'] = rospy.get_param("~ramp_decrement", 0.02)
    rospy.loginfo("Test Input : ramp decrement [%f]",param['ramp_decrement'])
    cmd_vel = Twist()
    cmd_vel.linear.x = 0
    cmd_vel.linear.y = 0
    cmd_vel.linear.z = 0
    cmd_vel.angular.x = 0
    cmd_vel.angular.y = 0
    cmd_vel.angular.z = 0
    odom = Odometry()
    odom.header.frame_id = "base_link"
    odom.pose.pose.position.x = 0.0
    odom.pose.pose.position.y = 0.0
    odom.pose.pose.position.z = 0.0
    odom.pose.pose.orientation.x = 0.0
    odom.pose.pose.orientation.y = 0.0
    odom.pose.pose.orientation.z = 0.0
    odom.pose.pose.orientation.w = 1.0

    odom.pose.covariance[0]  = 0.1
    odom.pose.covariance[7]  = 0.1
    odom.pose.covariance[35] = 0.2
    odom.pose.covariance[14] = 10.0
    odom.pose.covariance[21] = 10.0
    odom.pose.covariance[28] = 10.0

    odom.twist.twist.linear.x = 0.0
    odom.twist.twist.linear.y = 0.0
    odom.twist.twist.linear.z = 0.0
    odom.twist.twist.angular.x = 0.0
    odom.twist.twist.angular.y = 0.0
    odom.twist.twist.angular.z = 0.0
    state = STATE_RAMP_UP
    count = 0
    count_max = 100
    publish = True
    #period = 0.01
    timer = rospy.Rate(100) # 10hz
    rospy.loginfo("Test Input : STATE_RAMP_UP")
    while not rospy.is_shutdown():
        if state == STATE_RAMP_UP:
            cmd_vel.linear.x = cmd_vel.linear.x + param['ramp_increment']
            if cmd_vel.linear.x >= param['velocity_maximum']:
                state = STATE_RAMP_LEVEL
                count = 0
                rospy.loginfo("Test Input : STATE_RAMP_UP -> STATE_RAMP_LEVEL")
        elif state == STATE_RAMP_LEVEL:
            if count > count_max: # 0.5s
                state = STATE_RAMP_DOWN
                count = 0
                rospy.loginfo("Test Input : STATE_RAMP_LEVEL -> STATE_RAMP_DOWN")
            else:
                count = count + 1
        elif state == STATE_RAMP_DOWN:
            cmd_vel.linear.x = cmd_vel.linear.x - param['ramp_decrement']
            if cmd_vel.linear.x <= 0.0:
                cmd_vel.linear.x = 0.0
                state = STATE_ZERO
                count = 0
                rospy.loginfo("Test Input : STATE_RAMP_DOWN -> STATE_ZERO")
        elif state == STATE_ZERO:
            if count > count_max: # 0.5s
                state = STATE_UP
                cmd_vel.linear.x = param['velocity_maximum']
                count = 0
                rospy.loginfo("Test Input : STATE_ZERO -> STATE_UP")
            else:
                count = count + 1
        elif state == STATE_UP:
            if count > count_max: # 0.5s
                state = STATE_DOWN
                cmd_vel.linear.x = 0.0
                count = 0
                rospy.loginfo("Test Input : STATE_UP -> STATE_DOWN")
            else:
                count = count + 1
        elif state == STATE_DOWN:
            if count > count_max: # 0.5s
                #state = STATE_UP_AGAIN
                #cmd_vel.linear.x = param['velocity_maximum']
                #rospy.loginfo("Test Input : STATE_DOWN -> STATE_UP_AGAIN")
                state = STATE_RAMP_UP
                cmd_vel.linear.x = 0.0
                rospy.loginfo("Test Input : STATE_DOWN -> STATE_RAMP_UP")
                count = 0
            else:
                count = count + 1
        elif state == STATE_UP_AGAIN:
            if count > count_max: # 0.5s
                state = STATE_NOTHING
                count = 0
                publish = False
                rospy.loginfo("Test Input : STATE_UP_AGAIN -> STATE_NOTHING")
            else:
                count = count + 1
        elif state == STATE_NOTHING:
            if count > count_max: # 0.5s
                state = STATE_RAMP_UP
                cmd_vel.linear.x = 0.0
                count = 0
                publish = True
                rospy.loginfo("Test Input : STATE_NOTHING -> STATE_RAMP_UP")
            else:
                count = count + 1
        if publish:
            odom.twist.twist.linear.x = cmd_vel.linear.x
            cmd_vel_publisher.publish(cmd_vel)
        else:
            # How to fake it when it's not publishing a cmd velocity? Up to the velocity controller there
            odom.twist.twist.linear.x = cmd_vel.linear.x
        odom.header.stamp = rospy.Time().now()
        odom_publisher.publish(odom)
        timer.sleep()

if __name__ == "__main__":
  main()

　　输入下面的命令运行测试程序：

roslaunch yocs_velocity_smoother test_translational_smoothing.launch

　　可以使用rqt_plot工具绘制速度变化曲线，在终端中输入下面的命令进行绘制：

rqt_plot /cmd_vel/output/linear/x

　　可以将默认配置文件中的速度上限(speed_lim_v)从0.8改为0.4，再次运行测试程序。从速度曲线图中可以看出，最大速度已经被限制在0.4m/s

　　velocity_smoother还可以防止速度指令的跳变引起机器人的抖动。比如使用cmd_vel_mux进行速度切换时，前一个控制速度为2m/s，某一时刻切换到另一个速度假设为15m/s，如果不做处理瞬时加速度会很大。如下图所示，使用velocity_smoother对加速度进行限制，在速度指令跳变时能够使其不超出限制：

　　另外要注意yocs_velocity_smoother包中使用了动态参数配置的功能（参考ROS的参数应用以及动态调整），在yocs_velocity_smoother/cfg文件夹下的params.cfg文件如下。注意standalone.yaml配置文件中的速度和加速度参数不要超过cfg文件中动态参数的最大值。

#!/usr/bin/env python

PACKAGE = "yocs_velocity_smoother"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("speed_lim_v", double_t, 0, "Maximum linear velocity", 1.0, 0.0, 100.0)
gen.add("speed_lim_w", double_t, 0, "Maximum angular velocity", 5.0, 0.0, 100.0)

gen.add("accel_lim_v", double_t, 0, "Maximum linear acceleration", 0.5, 0.0, 100.0)
gen.add("accel_lim_w", double_t, 0, "Maximum angular acceleration", 2.5, 0.0, 100.0)

gen.add("decel_factor", double_t, 0, "Deceleration to acceleration ratio", 1.0, 0.0, 10.0)

# Second arg is node name it will run in (doc purposes only), third is generated filename prefix
exit(gen.generate(PACKAGE, "velocity_smoother_configure", "params"))

　　params.cfg其实是一个基于python的模块，首先创建一个ParameterGenerator对象，然后调用其add()函数将参数添加到参数列表中。add()的参数含义分别是：参数名，参数类型，级别，描述，缺省值，最小值，最大值。

参考：

rqt_plot

yocs_velocity_smoother

ROS的参数应用以及动态调整

Kobuki入门教程-Kobuki控制系统

ros中的velocity smoother详细分析

使用yocs_cmd_vel_mux进行机器人速度控制切换