mirror of
https://github.com/YikeStone/ros_arduino_bridge.git
synced 2025-08-03 19:24:09 +05:30
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:
parent
6faf351cc7
commit
8ec8dc8816
@ -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
|
||||||
|
@ -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)
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
Loading…
x
Reference in New Issue
Block a user