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()的參數含義分別是：參數名，參數類型，級別，描述，缺省值，最小值，最大值。
參考：