Added base_frame parameter (default base_link) that can be set to base_footprint for robots that use base_footprint instead of base_link

This commit is contained in:
Patrick Goebel 2014-02-27 07:16:09 -08:00
parent 6faf351cc7
commit 8ec8dc8816
4 changed files with 20 additions and 15 deletions

View File

@ -13,6 +13,9 @@ sensorstate_rate: 10
use_base_controller: False use_base_controller: False
base_controller_rate: 10 base_controller_rate: 10
# For a robot that uses base_footprint, change base_frame to base_footprint
base_frame: base_link
# === Robot drivetrain parameters # === Robot drivetrain parameters
#wheel_diameter: 0.146 #wheel_diameter: 0.146
#wheel_track: 0.2969 #wheel_track: 0.2969

View File

@ -38,6 +38,7 @@ class ArduinoROS():
self.port = rospy.get_param("~port", "/dev/ttyACM0") self.port = rospy.get_param("~port", "/dev/ttyACM0")
self.baud = int(rospy.get_param("~baud", 57600)) self.baud = int(rospy.get_param("~baud", 57600))
self.timeout = rospy.get_param("~timeout", 0.5) 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 # Overall loop rate: should be faster than fastest sensor rate
self.rate = int(rospy.get_param("~rate", 50)) self.rate = int(rospy.get_param("~rate", 50))
@ -100,19 +101,19 @@ class ArduinoROS():
params['direction'] = 'input' params['direction'] = 'input'
if params['type'] == "Ping": 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": 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': 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': 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': 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': 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': 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": # if params['type'] == "MaxEZ1":
# self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin'] # self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
@ -123,7 +124,7 @@ class ArduinoROS():
# Initialize the base controller if used # Initialize the base controller if used
if self.use_base_controller: 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 # Start polling the sensors and base controller
while not rospy.is_shutdown(): while not rospy.is_shutdown():
@ -142,7 +143,7 @@ class ArduinoROS():
if now > self.t_next_sensors: if now > self.t_next_sensors:
msg = SensorState() msg = SensorState()
msg.header.frame_id = 'base_link' msg.header.frame_id = self.base_frame
msg.header.stamp = now msg.header.stamp = now
for i in range(len(self.mySensors)): for i in range(len(self.mySensors)):
msg.name.append(self.mySensors[i].name) msg.name.append(self.mySensors[i].name)

View File

@ -39,7 +39,7 @@ class MessageType:
BOOL = 5 BOOL = 5
class Sensor(object): 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.controller = controller
self.name = name self.name = name
self.pin = pin self.pin = pin

View File

@ -31,10 +31,11 @@ from tf.broadcaster import TransformBroadcaster
""" Class to receive Twist commands and publish Odometry data """ """ Class to receive Twist commands and publish Odometry data """
class BaseController: class BaseController:
def __init__(self, arduino): def __init__(self, arduino, base_frame):
self.arduino = arduino self.arduino = arduino
self.base_frame = base_frame
self.rate = float(rospy.get_param("~base_controller_rate", 10)) 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 self.stopped = False
pid_params = dict() pid_params = dict()
@ -90,7 +91,7 @@ class BaseController:
self.odomBroadcaster = TransformBroadcaster() 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("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): def setup_pid(self, pid_params):
# Check to see if any PID parameters are missing # Check to see if any PID parameters are missing
@ -166,13 +167,13 @@ class BaseController:
(self.x, self.y, 0), (self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w), (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), rospy.Time.now(),
"base_link", self.base_frame,
"odom" "odom"
) )
odom = Odometry() odom = Odometry()
odom.header.frame_id = "odom" odom.header.frame_id = "odom"
odom.child_frame_id = "base_link" odom.child_frame_id = self.base_frame
odom.header.stamp = now odom.header.stamp = now
odom.pose.pose.position.x = self.x odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y odom.pose.pose.position.y = self.y