cob_twist_controller: test_publisher_twist

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00004 #
00005 # Licensed under the Apache License, Version 2.0 (the "License");
00006 # you may not use this file except in compliance with the License.
00007 # You may obtain a copy of the License at
00008 #
00009 #   http://www.apache.org/licenses/LICENSE-2.0
00010 #
00011 # Unless required by applicable law or agreed to in writing, software
00012 # distributed under the License is distributed on an "AS IS" BASIS,
00013 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014 # See the License for the specific language governing permissions and
00015 # limitations under the License.
00016 
00017 
00018 import rospy
00019 from geometry_msgs.msg import TwistStamped
00020 
00021 def twistPub():
00022   rospy.init_node("test_publisher_twist_series", anonymous=True)
00023 
00024   pub = rospy.Publisher("twist_controller/command_twist_stamped", TwistStamped, queue_size=1)
00025 
00026   rospy.sleep(1.0)
00027 
00028   twist_msg =  TwistStamped()
00029   twist_msg.header.stamp = rospy.Time.now()
00030   twist_msg.header.frame_id = "arm_left_base_link"
00031   twist_msg.twist.linear.x = 0
00032   twist_msg.twist.linear.y = 0
00033   twist_msg.twist.linear.z = 0
00034   twist_msg.twist.angular.x = 0
00035   twist_msg.twist.angular.y = 0
00036   twist_msg.twist.angular.z = 0
00037 
00038   rate = 50
00039   r = rospy.Rate(rate)
00040 
00041   for i in range(0, 5*rate, 1):
00042     twist_msg.header.stamp = rospy.Time.now()
00043     twist_msg.header.frame_id = "arm_left_base_link"
00044     twist_msg.twist.linear.x = 0
00045     twist_msg.twist.linear.y = 0
00046     twist_msg.twist.linear.z = -0.02
00047     twist_msg.twist.angular.x = 0
00048     twist_msg.twist.angular.y = 0
00049     twist_msg.twist.angular.z = 0
00050     pub.publish(twist_msg)
00051     r.sleep()
00052 
00053   #pause
00054   for i in range(0, 1*rate, 1):
00055     twist_msg.header.stamp = rospy.Time.now()
00056     twist_msg.header.frame_id = "arm_left_base_link"
00057     twist_msg.twist.linear.x = 0
00058     twist_msg.twist.linear.y = 0
00059     twist_msg.twist.linear.z = 0
00060     twist_msg.twist.angular.x = 0
00061     twist_msg.twist.angular.y = 0
00062     twist_msg.twist.angular.z = 0
00063     pub.publish(twist_msg)
00064     r.sleep()
00065 
00066   for i in range(0, 5*rate, 1):
00067     twist_msg.header.stamp = rospy.Time.now()
00068     twist_msg.header.frame_id = "arm_left_base_link"
00069     twist_msg.twist.linear.x = 0
00070     twist_msg.twist.linear.y = 0
00071     twist_msg.twist.linear.z = 0.02
00072     twist_msg.twist.angular.x = 0
00073     twist_msg.twist.angular.y = 0
00074     twist_msg.twist.angular.z = 0
00075     pub.publish(twist_msg)
00076     r.sleep()
00077 
00078   #pause
00079   for i in range(0, 1*rate, 1):
00080     twist_msg.header.stamp = rospy.Time.now()
00081     twist_msg.header.frame_id = "arm_left_base_link"
00082     twist_msg.twist.linear.x = 0
00083     twist_msg.twist.linear.y = 0
00084     twist_msg.twist.linear.z = 0
00085     twist_msg.twist.angular.x = 0
00086     twist_msg.twist.angular.y = 0
00087     twist_msg.twist.angular.z = 0
00088     pub.publish(twist_msg)
00089     r.sleep()
00090 
00091   print "done"
00092 
00093 if __name__ == '__main__':
00094   try:
00095       twistPub()
00096   except rospy.ROSInterruptException: pass
00097