From 8ec8dc88163e5dd2d84eb199fcb94c8a48a35c21 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Thu, 27 Feb 2014 07:16:09 -0800 Subject: [PATCH] Added base_frame parameter (default base_link) that can be set to base_footprint for robots that use base_footprint instead of base_link --- ros_arduino_python/config/arduino_params.yaml | 3 +++ ros_arduino_python/nodes/arduino_node.py | 19 ++++++++++--------- .../src/ros_arduino_python/arduino_sensors.py | 2 +- .../src/ros_arduino_python/base_controller.py | 11 ++++++----- 4 files changed, 20 insertions(+), 15 deletions(-) diff --git a/ros_arduino_python/config/arduino_params.yaml b/ros_arduino_python/config/arduino_params.yaml index 84f9b77..9acbe3d 100644 --- a/ros_arduino_python/config/arduino_params.yaml +++ b/ros_arduino_python/config/arduino_params.yaml @@ -13,6 +13,9 @@ sensorstate_rate: 10 use_base_controller: False base_controller_rate: 10 +# For a robot that uses base_footprint, change base_frame to base_footprint +base_frame: base_link + # === Robot drivetrain parameters #wheel_diameter: 0.146 #wheel_track: 0.2969 diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index d844f3e..4e2f34b 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -38,6 +38,7 @@ class ArduinoROS(): self.port = rospy.get_param("~port", "/dev/ttyACM0") self.baud = int(rospy.get_param("~baud", 57600)) self.timeout = rospy.get_param("~timeout", 0.5) + self.base_frame = rospy.get_param("~base_frame", 'base_link') # Overall loop rate: should be faster than fastest sensor rate self.rate = int(rospy.get_param("~rate", 50)) @@ -100,19 +101,19 @@ class ArduinoROS(): params['direction'] = 'input' if params['type'] == "Ping": - sensor = Ping(self.controller, name, params['pin'], params['rate']) + sensor = Ping(self.controller, name, params['pin'], params['rate'], self.base_frame) elif params['type'] == "GP2D12": - sensor = GP2D12(self.controller, name, params['pin'], params['rate']) + sensor = GP2D12(self.controller, name, params['pin'], params['rate'], self.base_frame) elif params['type'] == 'Digital': - sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], direction=params['direction']) + sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction']) elif params['type'] == 'Analog': - sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], direction=params['direction']) + sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction']) elif params['type'] == 'PololuMotorCurrent': - sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate']) + sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame) elif params['type'] == 'PhidgetsVoltage': - sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate']) + sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate'], self.base_frame) elif params['type'] == 'PhidgetsCurrent': - sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate']) + sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame) # if params['type'] == "MaxEZ1": # self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin'] @@ -123,7 +124,7 @@ class ArduinoROS(): # Initialize the base controller if used if self.use_base_controller: - self.myBaseController = BaseController(self.controller) + self.myBaseController = BaseController(self.controller, self.base_frame) # Start polling the sensors and base controller while not rospy.is_shutdown(): @@ -142,7 +143,7 @@ class ArduinoROS(): if now > self.t_next_sensors: msg = SensorState() - msg.header.frame_id = 'base_link' + msg.header.frame_id = self.base_frame msg.header.stamp = now for i in range(len(self.mySensors)): msg.name.append(self.mySensors[i].name) diff --git a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py index c5601ce..2f9b11c 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py @@ -39,7 +39,7 @@ class MessageType: BOOL = 5 class Sensor(object): - def __init__(self, controller, name, pin, rate, frame_id="/base_link", direction="input", **kwargs): + def __init__(self, controller, name, pin, rate, frame_id, direction="input", **kwargs): self.controller = controller self.name = name self.pin = pin 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 0c7656e..246be0f 100755 --- a/ros_arduino_python/src/ros_arduino_python/base_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/base_controller.py @@ -31,10 +31,11 @@ from tf.broadcaster import TransformBroadcaster """ Class to receive Twist commands and publish Odometry data """ class BaseController: - def __init__(self, arduino): + def __init__(self, arduino, base_frame): self.arduino = arduino + 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) + self.timeout = rospy.get_param("~base_controller_timeout", 1.0) self.stopped = False pid_params = dict() @@ -90,7 +91,7 @@ class BaseController: self.odomBroadcaster = TransformBroadcaster() rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev") - rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz") + rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame") def setup_pid(self, pid_params): # Check to see if any PID parameters are missing @@ -166,13 +167,13 @@ class BaseController: (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), rospy.Time.now(), - "base_link", + self.base_frame, "odom" ) odom = Odometry() odom.header.frame_id = "odom" - odom.child_frame_id = "base_link" + odom.child_frame_id = self.base_frame odom.header.stamp = now odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y