test_publisher_twist_sine.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 #
3 # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
4 #
5 # Licensed under the Apache License, Version 2.0 (the "License");
6 # you may not use this file except in compliance with the License.
7 # You may obtain a copy of the License at
8 #
9 # http://www.apache.org/licenses/LICENSE-2.0
10 #
11 # Unless required by applicable law or agreed to in writing, software
12 # distributed under the License is distributed on an "AS IS" BASIS,
13 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 # See the License for the specific language governing permissions and
15 # limitations under the License.
16 
17 
18 import rospy
19 import math
20 from geometry_msgs.msg import Twist
21 
22 
23 def velPub():
24  rospy.init_node("test_publisher_twist", anonymous=True)
25 
26  pub_vel = rospy.Publisher("command", Twist, queue_size=1)
27  rospy.sleep(1.0)
28 
29  freq = 10.0
30  r = rospy.Rate(freq)
31 
32  a = 0.5#-1.0
33  b = 0.1 * (2.0*math.pi/freq)
34  c = -math.pi/2.0
35  d = a
36  i = 0.0
37 
38  vel_msg = Twist()
39  time = rospy.Time.now()
40 
41  while not rospy.is_shutdown():
42  vel = a*math.sin(b*i+c) + d
43  #print("vel: ", vel)
44  vel_msg.angular.z = vel
45  pub_vel.publish(vel_msg)
46  i += 1.0
47  r.sleep()
48 
49 if __name__ == '__main__':
50  try:
51  velPub()
52  except rospy.ROSInterruptException: pass


cob_base_controller_utils
Author(s): Felix Messmer , Mathias L├╝dtke
autogenerated on Fri Nov 8 2019 03:46:51