From ee3799bfd0a02b1ecd72e613810b874080073495 Mon Sep 17 00:00:00 2001 From: khaledhazime Date: Wed, 23 Oct 2019 12:44:58 -0300 Subject: [PATCH] Fixed node interpreter --- butia_navigation_launchfiles/CMakeLists.txt | 4 +-- .../nodes/enable_motors_r.py | 25 +++++++++++++++++++ 2 files changed, 27 insertions(+), 2 deletions(-) create mode 100755 butia_navigation_launchfiles/nodes/enable_motors_r.py diff --git a/butia_navigation_launchfiles/CMakeLists.txt b/butia_navigation_launchfiles/CMakeLists.txt index ec4e3cd..017f11f 100644 --- a/butia_navigation_launchfiles/CMakeLists.txt +++ b/butia_navigation_launchfiles/CMakeLists.txt @@ -7,6 +7,6 @@ project(butia_navigation_launchfiles) # ) install(PROGRAMS - nodes/enable_motors.py + nodes/enable_motors_r.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) \ No newline at end of file +) diff --git a/butia_navigation_launchfiles/nodes/enable_motors_r.py b/butia_navigation_launchfiles/nodes/enable_motors_r.py new file mode 100755 index 0000000..73ed5dd --- /dev/null +++ b/butia_navigation_launchfiles/nodes/enable_motors_r.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python +import rospy +from std_msgs.msg import Bool + +flag = False + +def callBackEM(response): + global flag + if response.data: + flag = True + else: + flag = False + + +if __name__ == "__main__": + rospy.init_node("enable_motors") + motors_sub = rospy.Subscriber("/RosAria/motors_state", Bool, callBackEM, queue_size=1) + rospy.wait_for_service("/RosAria/enable_motors") + try: + enable_motors = rospy.ServiceProxy("/RosAria/enable_motors", Bool) + except rospy.ServiceException, e: + print "Service call failed %s"%e + while not rospy.is_shutdown(): + if not flag: + enable_motors(True) \ No newline at end of file