From 174878c7abe6856510e607317cc6683ea7b1b2ac Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sat, 8 Oct 2016 18:10:51 -0700 Subject: [PATCH] Added try-except around sensor params and added name argument for base controller --- ros_arduino_python/nodes/arduino_node.py | 19 ++++++++++++------- .../src/ros_arduino_python/base_controller.py | 3 ++- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 103179d..60b5620 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -30,11 +30,14 @@ import thread class ArduinoROS(): def __init__(self): - rospy.init_node('Arduino', log_level=rospy.DEBUG) - + rospy.init_node('arduino', log_level=rospy.INFO) + + # Get the actual node name in case it is set in the launch file + self.name = rospy.get_name() + # Cleanup when termniating the node rospy.on_shutdown(self.shutdown) - + self.port = rospy.get_param("~port", "/dev/ttyACM0") self.baud = int(rospy.get_param("~baud", 57600)) self.timeout = rospy.get_param("~timeout", 0.5) @@ -127,13 +130,15 @@ class ArduinoROS(): # if params['type'] == "MaxEZ1": # self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin'] # self.sensors[len(self.sensors)]['output_pin'] = params['output_pin'] - - self.mySensors.append(sensor) - rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name) + try: + self.mySensors.append(sensor) + rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name) + except: + rospy.logerr("Sensor type " + str(params['type']) + " not recognized.") # Initialize the base controller if used if self.use_base_controller: - self.myBaseController = BaseController(self.controller, self.base_frame) + self.myBaseController = BaseController(self.controller, self.base_frame, self.name + "_base_controller") # Start polling the sensors and base controller while not rospy.is_shutdown(): diff --git a/ros_arduino_python/src/ros_arduino_python/base_controller.py b/ros_arduino_python/src/ros_arduino_python/base_controller.py index 46d19ed..4c5092e 100755 --- a/ros_arduino_python/src/ros_arduino_python/base_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/base_controller.py @@ -31,8 +31,9 @@ from tf.broadcaster import TransformBroadcaster """ Class to receive Twist commands and publish Odometry data """ class BaseController: - def __init__(self, arduino, base_frame): + def __init__(self, arduino, base_frame, name="base_controllers"): self.arduino = arduino + self.name = name self.base_frame = base_frame self.rate = float(rospy.get_param("~base_controller_rate", 10)) self.timeout = rospy.get_param("~base_controller_timeout", 1.0)