elevator.py
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 time
00019 import sys
00020 import random
00021 from math import *
00022 
00023 import rospy
00024 from gazebo_msgs.msg import *
00025 from std_msgs.msg import *
00026 
00027 door_closed = True
00028 
00029 def callback(ContactsState):
00030 
00031         if door_closed:
00032                 if (ContactsState.states != []):
00033                         rospy.loginfo("button pressed")
00034                         rand = (random.randint(0,1))
00035                         if rand == 0:
00036                                 move_door("left")
00037                         else:
00038                                 move_door("right")
00039                 else:
00040                         rospy.logdebug("button not pressed")
00041         else:
00042                 rospy.loginfo("Door Opened")
00043 
00044 def listener():
00045 
00046         rospy.init_node('listener', anonymous=True)
00047         rospy.Subscriber("/elevator_button1_bumper", ContactsState, callback, queue_size=1)
00048         rospy.spin()
00049 
00050 def move_door(side):
00051 
00052         door_closed = False
00053         topic_name = '/world/elevator_%s_joint_position_controller/command' %side
00054         pub = rospy.Publisher(topic_name,Float64,queue_size=10)
00055         rospy.sleep(1)
00056         pos = 0.87
00057         pub.publish(pos)
00058 
00059         rospy.loginfo("%s door is opening" %side)
00060 
00061         try:
00062                 rospy.sleep(10)
00063         except rospy.exceptions.ROSInterruptException as e:
00064                 return
00065 
00066         pos = 0
00067         rospy.loginfo("%s door is closing" %side)
00068         pub.publish(pos)
00069 
00070         try:
00071                 rospy.sleep(10)
00072         except rospy.exceptions.ROSInterruptException as e:
00073                 return
00074 
00075         door_closed = True
00076 
00077 if __name__ == '__main__':
00078         listener()
00079 
00080 
00081 


cob_gazebo_worlds
Author(s): Felix Messmer , Nadia Hammoudeh Garcia , Florian Weisshardt
autogenerated on Sat Jun 8 2019 18:52:29