test_translational_input.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import roslib
3 roslib.load_manifest('yocs_velocity_smoother')
4 import rospy
5 
6 import os
7 import sys
8 import time
9 from geometry_msgs.msg import Twist
10 from nav_msgs.msg import Odometry
11 '''
12  Varied translational input for a velocity smoother test.
13 '''
14 
15 STATE_RAMP_UP = 0
16 STATE_RAMP_LEVEL = 1
17 STATE_RAMP_DOWN = 2
18 STATE_ZERO = 3
19 STATE_UP = 4
20 STATE_DOWN = 5
21 STATE_UP_AGAIN = 6
22 STATE_NOTHING = 7
23 
24 def main():
25  rospy.init_node("test_velocity_smoother_input")
26  cmd_vel_publisher = rospy.Publisher("~cmd_vel", Twist)
27  odom_publisher = rospy.Publisher("~odom", Odometry)
28  param = {}
29  param['velocity_maximum'] = rospy.get_param("~velocity_maximum", 0.50)
30  param['ramp_increment'] = rospy.get_param("~ramp_increment", 0.02)
31  rospy.loginfo("Test Input : ramp increment [%f]",param['ramp_increment'])
32  param['ramp_decrement'] = rospy.get_param("~ramp_decrement", 0.02)
33  rospy.loginfo("Test Input : ramp decrement [%f]",param['ramp_decrement'])
34  cmd_vel = Twist()
35  cmd_vel.linear.x = 0
36  cmd_vel.linear.y = 0
37  cmd_vel.linear.z = 0
38  cmd_vel.angular.x = 0
39  cmd_vel.angular.y = 0
40  cmd_vel.angular.z = 0
41  odom = Odometry()
42  odom.header.frame_id = "base_link"
43  odom.pose.pose.position.x = 0.0
44  odom.pose.pose.position.y = 0.0
45  odom.pose.pose.position.z = 0.0
46  odom.pose.pose.orientation.x = 0.0
47  odom.pose.pose.orientation.y = 0.0
48  odom.pose.pose.orientation.z = 0.0
49  odom.pose.pose.orientation.w = 1.0
50 
51  odom.pose.covariance[0] = 0.1
52  odom.pose.covariance[7] = 0.1
53  odom.pose.covariance[35] = 0.2
54  odom.pose.covariance[14] = 10.0
55  odom.pose.covariance[21] = 10.0
56  odom.pose.covariance[28] = 10.0
57 
58  odom.twist.twist.linear.x = 0.0
59  odom.twist.twist.linear.y = 0.0
60  odom.twist.twist.linear.z = 0.0
61  odom.twist.twist.angular.x = 0.0
62  odom.twist.twist.angular.y = 0.0
63  odom.twist.twist.angular.z = 0.0
64  state = STATE_RAMP_UP
65  count = 0
66  count_max = 100
67  publish = True
68  #period = 0.01
69  timer = rospy.Rate(100) # 10hz
70  rospy.loginfo("Test Input : STATE_RAMP_UP")
71  while not rospy.is_shutdown():
72  if state == STATE_RAMP_UP:
73  cmd_vel.linear.x = cmd_vel.linear.x + param['ramp_increment']
74  if cmd_vel.linear.x >= param['velocity_maximum']:
75  state = STATE_RAMP_LEVEL
76  count = 0
77  rospy.loginfo("Test Input : STATE_RAMP_UP -> STATE_RAMP_LEVEL")
78  elif state == STATE_RAMP_LEVEL:
79  if count > count_max: # 0.5s
80  state = STATE_RAMP_DOWN
81  count = 0
82  rospy.loginfo("Test Input : STATE_RAMP_LEVEL -> STATE_RAMP_DOWN")
83  else:
84  count = count + 1
85  elif state == STATE_RAMP_DOWN:
86  cmd_vel.linear.x = cmd_vel.linear.x - param['ramp_decrement']
87  if cmd_vel.linear.x <= 0.0:
88  cmd_vel.linear.x = 0.0
89  state = STATE_ZERO
90  count = 0
91  rospy.loginfo("Test Input : STATE_RAMP_DOWN -> STATE_ZERO")
92  elif state == STATE_ZERO:
93  if count > count_max: # 0.5s
94  state = STATE_UP
95  cmd_vel.linear.x = param['velocity_maximum']
96  count = 0
97  rospy.loginfo("Test Input : STATE_ZERO -> STATE_UP")
98  else:
99  count = count + 1
100  elif state == STATE_UP:
101  if count > count_max: # 0.5s
102  state = STATE_DOWN
103  cmd_vel.linear.x = 0.0
104  count = 0
105  rospy.loginfo("Test Input : STATE_UP -> STATE_DOWN")
106  else:
107  count = count + 1
108  elif state == STATE_DOWN:
109  if count > count_max: # 0.5s
110  #state = STATE_UP_AGAIN
111  #cmd_vel.linear.x = param['velocity_maximum']
112  #rospy.loginfo("Test Input : STATE_DOWN -> STATE_UP_AGAIN")
113  state = STATE_RAMP_UP
114  cmd_vel.linear.x = 0.0
115  rospy.loginfo("Test Input : STATE_DOWN -> STATE_RAMP_UP")
116  count = 0
117  else:
118  count = count + 1
119  elif state == STATE_UP_AGAIN:
120  if count > count_max: # 0.5s
121  state = STATE_NOTHING
122  count = 0
123  publish = False
124  rospy.loginfo("Test Input : STATE_UP_AGAIN -> STATE_NOTHING")
125  else:
126  count = count + 1
127  elif state == STATE_NOTHING:
128  if count > count_max: # 0.5s
129  state = STATE_RAMP_UP
130  cmd_vel.linear.x = 0.0
131  count = 0
132  publish = True
133  rospy.loginfo("Test Input : STATE_NOTHING -> STATE_RAMP_UP")
134  else:
135  count = count + 1
136  if publish:
137  odom.twist.twist.linear.x = cmd_vel.linear.x
138  cmd_vel_publisher.publish(cmd_vel)
139  else:
140  # How to fake it when it's not publishing a cmd velocity? Up to the velocity controller there
141  odom.twist.twist.linear.x = cmd_vel.linear.x
142  odom.header.stamp = rospy.Time().now()
143  odom_publisher.publish(odom)
144  timer.sleep()
145 
146 if __name__ == "__main__":
147  main()
test_translational_input.main
def main()
Definition: test_translational_input.py:24

yocs_velocity_smoother
Author(s): Jorge Santos Simon
autogenerated on Mon Jun 10 2019 15:54:03