test_translational_input.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib
00003 roslib.load_manifest('yocs_velocity_smoother')
00004 import rospy
00005 
00006 import os
00007 import sys
00008 import time
00009 from geometry_msgs.msg import Twist
00010 from nav_msgs.msg import Odometry
00011 '''
00012   Varied translational input for a velocity smoother test.
00013 '''
00014 
00015 STATE_RAMP_UP = 0
00016 STATE_RAMP_LEVEL = 1
00017 STATE_RAMP_DOWN = 2
00018 STATE_ZERO = 3
00019 STATE_UP = 4
00020 STATE_DOWN = 5
00021 STATE_UP_AGAIN = 6
00022 STATE_NOTHING = 7
00023 
00024 def main():
00025     rospy.init_node("test_velocity_smoother_input")
00026     cmd_vel_publisher = rospy.Publisher("~cmd_vel", Twist)
00027     odom_publisher = rospy.Publisher("~odom", Odometry)
00028     param = {}
00029     param['velocity_maximum'] = rospy.get_param("~velocity_maximum", 0.50)
00030     param['ramp_increment'] = rospy.get_param("~ramp_increment", 0.02)
00031     rospy.loginfo("Test Input : ramp increment [%f]",param['ramp_increment'])
00032     param['ramp_decrement'] = rospy.get_param("~ramp_decrement", 0.02)
00033     rospy.loginfo("Test Input : ramp decrement [%f]",param['ramp_decrement'])
00034     cmd_vel = Twist()
00035     cmd_vel.linear.x = 0
00036     cmd_vel.linear.y = 0
00037     cmd_vel.linear.z = 0
00038     cmd_vel.angular.x = 0
00039     cmd_vel.angular.y = 0
00040     cmd_vel.angular.z = 0
00041     odom = Odometry()
00042     odom.header.frame_id = "base_link"
00043     odom.pose.pose.position.x = 0.0
00044     odom.pose.pose.position.y = 0.0
00045     odom.pose.pose.position.z = 0.0
00046     odom.pose.pose.orientation.x = 0.0
00047     odom.pose.pose.orientation.y = 0.0
00048     odom.pose.pose.orientation.z = 0.0
00049     odom.pose.pose.orientation.w = 1.0
00050 
00051     odom.pose.covariance[0]  = 0.1
00052     odom.pose.covariance[7]  = 0.1
00053     odom.pose.covariance[35] = 0.2
00054     odom.pose.covariance[14] = 10.0
00055     odom.pose.covariance[21] = 10.0
00056     odom.pose.covariance[28] = 10.0
00057 
00058     odom.twist.twist.linear.x = 0.0
00059     odom.twist.twist.linear.y = 0.0
00060     odom.twist.twist.linear.z = 0.0
00061     odom.twist.twist.angular.x = 0.0
00062     odom.twist.twist.angular.y = 0.0
00063     odom.twist.twist.angular.z = 0.0
00064     state = STATE_RAMP_UP
00065     count = 0
00066     count_max = 100
00067     publish = True
00068     #period = 0.01
00069     timer = rospy.Rate(100) # 10hz
00070     rospy.loginfo("Test Input : STATE_RAMP_UP")
00071     while not rospy.is_shutdown():
00072         if state == STATE_RAMP_UP:
00073             cmd_vel.linear.x = cmd_vel.linear.x + param['ramp_increment']
00074             if cmd_vel.linear.x >= param['velocity_maximum']:
00075                 state = STATE_RAMP_LEVEL
00076                 count = 0
00077                 rospy.loginfo("Test Input : STATE_RAMP_UP -> STATE_RAMP_LEVEL")
00078         elif state == STATE_RAMP_LEVEL:
00079             if count > count_max: # 0.5s
00080                 state = STATE_RAMP_DOWN
00081                 count = 0
00082                 rospy.loginfo("Test Input : STATE_RAMP_LEVEL -> STATE_RAMP_DOWN")
00083             else:
00084                 count = count + 1
00085         elif state == STATE_RAMP_DOWN:
00086             cmd_vel.linear.x = cmd_vel.linear.x - param['ramp_decrement']
00087             if cmd_vel.linear.x <= 0.0:
00088                 cmd_vel.linear.x = 0.0
00089                 state = STATE_ZERO
00090                 count = 0
00091                 rospy.loginfo("Test Input : STATE_RAMP_DOWN -> STATE_ZERO")
00092         elif state == STATE_ZERO:
00093             if count > count_max: # 0.5s
00094                 state = STATE_UP
00095                 cmd_vel.linear.x = param['velocity_maximum']
00096                 count = 0
00097                 rospy.loginfo("Test Input : STATE_ZERO -> STATE_UP")
00098             else:
00099                 count = count + 1
00100         elif state == STATE_UP:
00101             if count > count_max: # 0.5s
00102                 state = STATE_DOWN
00103                 cmd_vel.linear.x = 0.0
00104                 count = 0
00105                 rospy.loginfo("Test Input : STATE_UP -> STATE_DOWN")
00106             else:
00107                 count = count + 1
00108         elif state == STATE_DOWN:
00109             if count > count_max: # 0.5s
00110                 #state = STATE_UP_AGAIN
00111                 #cmd_vel.linear.x = param['velocity_maximum']
00112                 #rospy.loginfo("Test Input : STATE_DOWN -> STATE_UP_AGAIN")
00113                 state = STATE_RAMP_UP
00114                 cmd_vel.linear.x = 0.0
00115                 rospy.loginfo("Test Input : STATE_DOWN -> STATE_RAMP_UP")
00116                 count = 0
00117             else:
00118                 count = count + 1
00119         elif state == STATE_UP_AGAIN:
00120             if count > count_max: # 0.5s
00121                 state = STATE_NOTHING
00122                 count = 0
00123                 publish = False
00124                 rospy.loginfo("Test Input : STATE_UP_AGAIN -> STATE_NOTHING")
00125             else:
00126                 count = count + 1
00127         elif state == STATE_NOTHING:
00128             if count > count_max: # 0.5s
00129                 state = STATE_RAMP_UP
00130                 cmd_vel.linear.x = 0.0
00131                 count = 0
00132                 publish = True
00133                 rospy.loginfo("Test Input : STATE_NOTHING -> STATE_RAMP_UP")
00134             else:
00135                 count = count + 1
00136         if publish:
00137             odom.twist.twist.linear.x = cmd_vel.linear.x
00138             cmd_vel_publisher.publish(cmd_vel)
00139         else:
00140             # How to fake it when it's not publishing a cmd velocity? Up to the velocity controller there
00141             odom.twist.twist.linear.x = cmd_vel.linear.x
00142         odom.header.stamp = rospy.Time().now()
00143         odom_publisher.publish(odom)
00144         timer.sleep()
00145 
00146 if __name__ == "__main__":
00147   main()


yocs_velocity_smoother
Author(s): Jorge Santos Simon
autogenerated on Fri Aug 28 2015 13:45:00