def start_test(self, challenge):
m = self.RX.match(challenge)
if m is None:
raise CourseraException(
"Unknown challenge format. Please contact developers for assistance."
)
try:
v = float(m.group('v'))
w = float(m.group('w'))
except ValueError:
raise CourseraException(
"Unknown challenge format. Please contact developers for assistance."
)
from supervisors.week2 import QuickBotSupervisor
from robots.quickbot import QuickBot
from pose import Pose
info = QuickBot(Pose()).get_info()
info.color = 0
s = QuickBotSupervisor(Pose(), info)
vl, vr = s.uni2diff((v, w))
self.testsuite.respond("{:0.3f},{:0.3f}".format(
vr, vl)) # Note the inverse order
def updatePoseList(lastPose, u_in, nextTimestamp_s, dT, path=None):
"""
Update vehicle pose with given longtitude acceleration and timestamp
Args:
lastPose: the lastest pose
u_in: current acceleration as input
nextTimestamp_s: next timestamp
Return:
l_pose(timestamp: pose): discreted pose list to given timestamp
l_u(timestamp: u): input list to given timestamp
"""
# assert nextTimestamp_s > lastPose.timestamp_s
l_pose = {}
dyaw = 0
step = int((nextTimestamp_s - lastPose.timestamp_s) / dT)
vx = lastPose.vdy.vx_ms
for k in range(1, step + 1, 1):
dTime = k * dT
nextT = round(lastPose.timestamp_s + dTime, 2)
next_vdy = VehicleDynamic(vx + u_in * dTime, dyaw)
dP = vx * dTime + 0.5 * u_in * (dTime**2)
dP = max(dP, 0)
dX = dP * math.cos(lastPose.yaw_rad)
dY = dP * math.sin(lastPose.yaw_rad)
# update covariance matrix from the nearest pose
next_covLatLong = 0
if not l_pose:
next_covLatLong = updateCovLatlong(lastPose.covLatLong, dT, dP, 0)
else:
nearestPose = l_pose[max(l_pose)]
dPNearest = np.linalg.norm([
lastPose.x_m + dX - nearestPose.x_m,
lastPose.y_m + dY - nearestPose.y_m
])
next_covLatLong = updateCovLatlong(nearestPose.covLatLong, dT,
dPNearest, 0)
if param._TEST and path is not None:
ix, iy, iyaw = path.getDs(dP)
nextPose = Pose(x_m=ix,
y_m=iy,
yaw_rad=iyaw,
vdy=next_vdy,
covLatLong=next_covLatLong,
timestamp_s=nextT)
else:
nextPose = Pose(x_m=lastPose.x_m + dX,
y_m=lastPose.y_m + dY,
yaw_rad=lastPose.yaw_rad,
vdy=next_vdy,
covLatLong=next_covLatLong,
timestamp_s=nextT)
l_pose[nextT] = nextPose
return l_pose
def start_test(self, challenge):
m = self.RX.match(challenge)
if m is None:
raise CourseraException(
"Unknown challenge format. Please contact developers for assistance."
)
try:
d1 = float(m.group('d1'))
d2 = float(m.group('d2'))
except ValueError:
raise CourseraException(
"Unknown challenge format. Please contact developers for assistance."
)
from supervisors.week2 import QuickBotSupervisor
from robots.quickbot import QuickBot, QuickBot_IRSensor
from pose import Pose
bot = QuickBot(Pose())
sensor = QuickBot_IRSensor(Pose(), bot)
id1 = sensor.distance_to_value(d1)
id2 = sensor.distance_to_value(d2)
info = bot.get_info()
info.color = 0
s = QuickBotSupervisor(Pose(), info)
# Just in case a student iterates in a weird way
s.robot.ir_sensors.readings = [id1, id2, id1, id2, id1]
ird = s.get_ir_distances()
self.testsuite.respond("{:0.3f},{:0.3f}".format(
abs((d1 - ird[0]) / d1), abs((d2 - ird[1]) / d2)))
def start_test(self, challenge):
vals = self.parseChallenge(challenge)
if 'd1' not in vals or 'd2' not in vals:
raise CourseraException(
"Unknown challenge format. Please contact developers for assistance."
)
d1 = vals['d1']
d2 = vals['d2']
from supervisors.week2 import QuickBotSupervisor
from robots.quickbot import QuickBot, QuickBot_IRSensor
from pose import Pose
bot = QuickBot(Pose())
sensor = QuickBot_IRSensor(Pose(), bot)
id1 = sensor.distance_to_value(d1)
id2 = sensor.distance_to_value(d2)
info = bot.get_info()
info.color = 0
s = QuickBotSupervisor(Pose(), info)
# Just in case a student iterates in a weird way
s.robot.ir_sensors.readings = [id1, id2, id1, id2, id1]
ird = s.get_ir_distances()
self.testsuite.respond("{:0.3f},{:0.3f}".format(
abs((d1 - ird[0]) / d1), abs((d2 - ird[1]) / d2)))
def start_test(self, challenge):
vals = self.parseChallenge(challenge)
if 'v' not in vals or 'w' not in vals:
raise CourseraException(
"Unknown challenge format. Please contact developers for assistance."
)
vd = vals['v']
wd = vals['w']
QuickBotSupervisor = helpers.load_by_name('week3.QBGTGSupervisor',
'supervisors')
QuickBot = helpers.load_by_name('QuickBot', 'robots')
from pose import Pose
bot = QuickBot(Pose())
info = bot.get_info()
info.color = 0
s = QuickBotSupervisor(Pose(), info)
vld, vrd = s.uni2diff((vd, wd))
vl, vr = s.ensure_w((vld, vrd))
# Clamp to robot maxima
vl = max(-info.wheels.max_velocity, min(info.wheels.max_velocity, vl))
vr = max(-info.wheels.max_velocity, min(info.wheels.max_velocity, vr))
v, w = bot.diff2uni((vl, vr))
self.testsuite.respond("{:0.3f}".format(abs(w - wd) / wd))
def calculate_pose(old_pose, motion, timedelta, map):
"""Calculates a possible new position, with added gaussian noise."""
distance_left = motion.left * timedelta * FACTOR
distance_right = motion.right * timedelta * FACTOR
if distance_left == distance_right:
dx = distance_left * math.cos(old_pose.theta)
dy = distance_left * math.sin(old_pose.theta)
dtheta = 0
elif distance_left == -distance_right:
dx, dy = 0, 0
#dtheta = #timedelta * (math.pi / 4) / MS_PER_45_DEGREES
dtheta = RADIANS_DURING_1_turn
if distance_left > 0:
dtheta *= -1
# dtheta=distance_left/(WHEEL_DISTANCE_IN_CM/2) #left turn results in positive dtheta, uses formula length arc
if dx == 0.0 and dtheta == 0.0:
return Pose(old_pose.x, old_pose.y, old_pose.theta)
if GAUSSIAN_NOISE:
if dx == 0.0 and dy == 0.0:
return sample_rotation(old_pose, dtheta)
elif dtheta == 0.0:
return sample_forward_move(old_pose, dx, dy, map)
else:
print('dx, dy and dtheta are not zero ??')
exit()
else:
return Pose(old_pose.x + dx, old_pose.y + dy, old_pose.theta + dtheta)
def test_transform(self):
random_e = np.random.rand(3) * np.pi * 2
random_t = np.random.rand(3)
pose = Pose(random_t, random_e)
matrix = ttf.compose_matrix(angles=random_e, translate=random_t)
random_e = np.random.rand(3) * np.pi * 2
random_t = np.random.rand(3)
pose_2 = Pose(random_t, random_e)
matrix_2 = ttf.compose_matrix(angles=random_e, translate=random_t)
matrix_t = matrix_2.dot(matrix)
pose_t = pose_2.transform(pose)
self.assert_pose_matrix_eq(pose_t, matrix_t)
def start_test(self, challenge):
m = self.RX.match(challenge)
if m is None:
raise CourseraException(
"Unknown challenge format. Please contact developers for assistance."
)
try:
v = float(m.group('v'))
theta = float(m.group('theta'))
except ValueError:
raise CourseraException(
"Unknown challenge format. Please contact developers for assistance."
)
from supervisors.week2 import QuickBotSupervisor
from robots.quickbot import QuickBot
from pose import Pose
from helpers import Struct
from math import pi
bot = QuickBot(Pose())
info = bot.get_info()
info.color = 0
s = QuickBotSupervisor(Pose(), info)
params = Struct()
params.goal = theta * 180 / pi
params.velocity = v
params.pgain = 1
s.set_parameters(params)
tc = 0.033 # 0.033 sec' is the SimIAm time step
for step in range(25): # 25 steps
bot.move(tc)
bot.set_inputs(s.execute(bot.get_info(), tc))
xe, ye, te = s.pose_est
xr, yr, tr = bot.get_pose()
if xr == 0:
xr = 0.0000001
if yr == 0:
yr = 0.0000001
if tr == 0:
tr = 0.0000001
self.testsuite.respond("{:0.3f},{:0.3f},{:0.3f}".format(
abs((xr - xe) / xr), abs((yr - ye) / yr),
abs(abs(tr - te) % (2 * pi) / tr)))
def test_inverse(self):
random_e = np.random.rand(3) * np.pi * 2
random_t = np.random.rand(3)
pose = Pose(random_t, random_e).inverse()
matrix = np.linalg.inv(
ttf.compose_matrix(angles=random_e, translate=random_t))
self.assert_pose_matrix_eq(pose, matrix)
def draw(self, renderer):
"""Draw a circular goal, path and ir_sensors"""
# Draw goal
renderer.set_pose(Pose(self.pose_est.x,self.pose_est.y,pi*self.parameters.goal/180))
renderer.set_pen(0)
# renderer.draw_line(0.03,0,100,0)
renderer.draw_arrow(0.05,0,0.5,0,close=True)
# Draw robot path
self.tracker.draw(renderer)
renderer.set_pose(self.pose_est)
renderer.set_brush(0)
renderer.draw_ellipse(0,0,0.01,0.01)
# Draw IR distances
crosses = array([ dot(p.get_transformation(), [d,0,1])
for d, p in zip(self.get_ir_distances(), self.robot.ir_sensors.poses) ] )
renderer.set_pen(0)
for c in crosses:
x,y,z = c
renderer.push_state()
renderer.translate(x,y)
renderer.rotate(atan2(y,x))
renderer.draw_line(0.01,0.01,-0.01,-0.01)
renderer.draw_line(0.01,-0.01,-0.01,0.01)
renderer.pop_state()
def estimate_pose(self):
"""Update self.pose_est using odometry"""
# Get tick updates
dtl = self.robot.wheels.left_ticks - self.left_ticks
dtr = self.robot.wheels.right_ticks - self.right_ticks
# Save the wheel encoder ticks for the next estimate
self.left_ticks += dtl
self.right_ticks += dtr
x, y, theta = self.pose_est
R = self.robot.wheels.radius
L = self.robot.wheels.base_length
m_per_tick = (2*pi*R)/self.robot.wheels.ticks_per_rev
# distance travelled by left wheel
dl = dtl*m_per_tick
# distance travelled by right wheel
dr = dtr*m_per_tick
theta_dt = (dr-dl)/L
theta_mid = theta + theta_dt/2
dst = (dr+dl)/2
x_dt = dst*cos(theta_mid)
y_dt = dst*sin(theta_mid)
theta_new = theta + theta_dt
x_new = x + x_dt
y_new = y + y_dt
return Pose(x_new, y_new, (theta_new + pi)%(2*pi)-pi)
def draw(self, renderer):
"""Draw a circular goal"""
renderer.set_pose(Pose(self.parameters.goal.x, self.parameters.goal.y))
renderer.set_pen(0)
renderer.set_brush(self.robot_color)
r = self.robot_size/2
renderer.draw_ellipse(0,0,r,r)
def draw_background(self, renderer):
"""Draw a circular goal"""
renderer.set_pose(Pose(self.parameters.goal.x, self.parameters.goal.y))
renderer.set_pen(0)
renderer.set_brush(self.robot_color)
for r in numpy.arange(self.robot_size/2,0,-0.01):
renderer.draw_ellipse(0,0,r,r)
def set_animation():
pose = Pose()
animation = Animation()
animation.add_pose(pose)
return animation
def __init__(self):
self.lwheel_prev = int(0)
self.rwheel_prev = int(0)
self.lwheel_cur = int(0)
self.rwheel_cur = int(0)
self.leftTicks = 0
self.rightTicks = 0
self.x_prev = 0
self.y_prev = 0
self.x_cur = 0
self.y_cur = 0
self.theta_prev = 0 # in rad
self.theta_cur = 0 # in rad
self.lin_x_vel = 0
self.lin_y_vel = 0
self.yaw_vel = 0
self.wheelRad = 0.0325 ##in mts
self.gearRatio = 120 / 1
self.ticksPerOneMotorRot = 8
self.ticksPerOneWheelRot = self.gearRatio * self.ticksPerOneMotorRot #960
self.oneMeterToWheelRot = 1 / (2 * pi * self.wheelRad)
self.oneMeterToEncoderTicks = self.oneMeterToWheelRot * self.ticksPerOneWheelRot
self.oneTickInMeters = 1 / self.oneMeterToEncoderTicks
self.wheelSeparation = 14.5 * 0.01 # metres
self.pose = Pose()
self.baseFrameID = 'base_link'
self.odomFrameID = 'odom'
def get_pose(self, pose_id):
if (pose_id >= len(self.data["poses"])):
return None
tmp = self.data["poses"][pose_id]
time = tmp[0]
pose = Pose(tmp[1][0], tmp[1][1], tmp[1][2])
return (time, pose)
def set_pose(self, rpose):
RealBot.set_pose(self, rpose)
# We have to update walls here. WHY?
for pose, dst in zip(
self.info.ir_sensors.poses,
np.polyval(self.ir_coeff, self.info.ir_sensors.readings)):
if dst / self.info.ir_sensors.rmax <= 0.99:
self.walls.add_point(Pose(dst) >> pose >> self.get_pose())
def draw_annotations(coords_folder, video_folder, output_folder, cam_ids):
for cam_id in cam_ids:
cam_coords_path = os.path.join(coords_folder, "cam_{}".format(cam_id),
"coords_cam_{}.csv".format(cam_id))
cam_video_path = os.path.join(video_folder, "cam_{}".format(cam_id),
"cam_{}.mp4".format(cam_id))
print("Loading {}".format(cam_video_path))
cam_video_output_folder = os.path.join(output_folder,
"cam_{}".format(cam_id))
os.makedirs(cam_video_output_folder, exist_ok=True)
output_cam_video_path = os.path.join(
cam_video_output_folder, "annotations_cam_{}.mp4".format(cam_id))
print("Writing {}".format(output_cam_video_path))
cam_coords = pd.read_csv(cam_coords_path)
fourcc = cv2.VideoWriter_fourcc(*'mp4v')
out = cv2.VideoWriter(output_cam_video_path, fourcc, 41.0,
(1920, 1080))
#Get frames until the video ends
cam_video_capture = cv2.VideoCapture(cam_video_path)
total_frames = int(cam_video_capture.get(cv2.CAP_PROP_FRAME_COUNT))
tqdm_progress = tqdm(total=total_frames)
current_frame_no = 0
while True:
ret, frame = cam_video_capture.read()
tqdm_progress.update()
coords_frame = cam_coords[cam_coords["frame_no_cam"] ==
current_frame_no]
for person_id in list(coords_frame["person_id"]):
coords_one_person_frame = coords_frame[
coords_frame["person_id"] == person_id]
person_joints = get_joints_from_person_coords(
coords_one_person_frame)
pose = Pose(person_joints)
pose.drawBoundingBox(image=frame, color=(255, 0, 0))
pose.drawPersonId(image=frame)
pose.drawPose(image=frame, color=[0, 255, 0])
if not ret:
out.release()
cam_video_capture.release()
break
cv2.imshow("", mat=frame)
cv2.waitKey(1)
out.write(image=frame)
current_frame_no += 1
def __init__(self, n_particles, copy=False):
self.map = OccupancyGridMap(copy)
if not copy:
x_co = random.gauss(0, 0.2)
y_co = random.gauss(0, 0.2)
theta = random.gauss(0, 0.05)
self.pose = Pose(x_co, y_co, theta)
self.weight = 1.0 / n_particles
def __init__(self,
idx,
length,
width,
from_x_m,
from_y_m,
to_x_m,
to_y_m,
covLong,
covLat,
vx_ms,
startTime,
isStop=False,
appearRate=1,
interactRate=1,
dT=param._dT):
self._idx = idx
self._length = length
self._width = width
self._lw_std = np.array([self._length / 2,
self._width / 2]) # std offset
self._isDetected = False
self._detectedTime = None # position where first detected
self._Pcoll = 0 # maximum collision probability with ego
startTime = round(round(startTime / dT, 2) * dT, 2)
self._startTime = startTime
self.theta = math.atan2(to_y_m - from_y_m, to_x_m - from_x_m)
startPose = Pose(x_m=from_x_m,
y_m=from_y_m,
yaw_rad=self.theta,
covLatLong=np.diag([covLong, covLat]),
vdy=VehicleDynamic(vx_ms, 0),
timestamp_s=startTime)
if isStop:
# deaccleration if stop
self._u = pfnc.computeAccToStop(from_x_m=from_x_m,
from_y_m=from_y_m,
to_x_m=to_x_m,
to_y_m=to_y_m,
vx_ms=vx_ms)
else:
self._u = 0
# for hypothetical object
self._appearRate = appearRate
self._interactRate = interactRate
# pose
self._p_pose = {}
self._currentPose = startPose
self._l_pose = {startPose.timestamp_s: startPose}
# first states prediction
self.predict(dT=param._PREDICT_STEP, pT=param._PREDICT_TIME)
def insert_scan_only_endpoint(self, scan, robot_pose):
rays = scan.get_rays(robot_pose)
for ray in rays:
pose, dist = ray
if dist < self.min_dist or dist > self.max_dist:
continue
end_pose = pose.clone().add_relative_pose(Pose(dist, 0, 0))
self.add_density(end_pose, self.positive_scan_weight)
def get_pose(frame_data, person_id):
# type: (np.ndarray, int) -> Pose
"""
:param frame_data: data of the current frame
:param person_id: person identifier
:return: list of joints in the current frame with the required person ID
"""
pose = [Joint(j) for j in frame_data[frame_data[:, 1] == person_id]]
pose.sort(key=(lambda j: j.type))
return Pose(pose, "JTA")
def stripPose( rect, highResolution=True, scale=None ):
"return relative pose of image rectangle"
(x,y),(w,h),a = rect
assert w > 3*h, (w,h) # 30cm long, 5cm wide ... i.e. should be 6 times
a = -a-90
if a > 90:
a -= 180
if a < -90:
a += 180
if scale == None:
if w >= 5*h: # it should be 6x, but width is more precise
scale = 0.3/float(w)
else:
scale = 0.05/float(h)
if highResolution:
return Pose( scale*(720/2-y), scale*(1280/2-x), math.radians( a ) )
else:
# return Pose( scale*(360/2-y), scale*(640/2-x), math.radians( a ) )
return Pose( scale*(360/2-y), scale*(270-x), math.radians( a ) )
def copy_particle(self, old_p):
p = Particle(self.n_particles, True)
p.pose = Pose(old_p.pose.x, old_p.pose.y, old_p.pose.theta)
# Reset the weight of the particle after selection ??
p.weight = old_p.weight
p.map.minrange = old_p.map.minrange
p.map.maxrange = old_p.map.maxrange
p.map.grid = old_p.map.grid.copy()
p.map.path = old_p.map.path.copy()
return p
def draw_foreground(self, renderer):
"""Draw controller info"""
QuickBotSupervisor.draw_foreground(self, renderer)
renderer.set_pose(Pose(self.pose_est.x, self.pose_est.y))
arrow_length = self.robot_size * 5
# Draw arrow to goal
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0, 0, arrow_length * cos(self.gtg.heading_angle),
arrow_length * sin(self.gtg.heading_angle))
def __init__(self):
self.lefthandle = Pose("Left")
self.righthandle = Pose("Right")
self.HMD = Pose("HMD")
self.eyehandle = PoseEye("Eye")
self.currentDevice = 0
self.systemcmds = ["", "Ls", "Rs"]
self.triggercmds = ["", "Lt", "Rt"]
self.devicecmds = ["H ", "L ", "R ", "E "]
self.devices = [
self.HMD, self.lefthandle, self.righthandle, self.eyehandle
] ## order corresponds to currendDevie
self.ZMQhmd = zmqsocket.ZMQsocket(5577)
self.ZMQeye = zmqsocket.ZMQsocket(5555)
self.testcaseinprogress = False
# System button S/L grip G/H app:F/J trig:D/K
self.allowedKeysLeft = "SDFG"
self.allowedKeysRight = "LKJH"
self.lastcmd = ""
self.statusstring = "No Test Case Loaded"
def get_heading(self, state):
"""Get the direction along the wall as a vector."""
# Calculate vectors for the sensors
if state.direction == 'left': # 0-2
d, i = min(zip(state.sensor_distances[:3], [0, 1, 2]))
if i == 0 or (i == 1 and state.sensor_distances[0] <=
state.sensor_distances[2]):
i, j, k = 1, 0, 2
else:
i, j, k = 2, 1, 0
else: # 2-4
d, i = min(zip(state.sensor_distances[2:], [2, 3, 4]))
if i == 4 or (i == 3 and state.sensor_distances[4] <=
state.sensor_distances[2]):
i, j, k = 3, 4, 2
else:
i, j, k = 2, 3, 4
p_front = Pose(state.sensor_distances[i]) >> self.sensor_poses[i]
p_back = Pose(state.sensor_distances[j]) >> self.sensor_poses[j]
self.vectors = [(p_front.x, p_front.y, 1), (p_back.x, p_back.y, 1)]
# Calculate the two vectors:
ds = ((p_front.x - p_back.x)**2 + (p_front.y - p_back.y)**2)
ms = (p_front.x * p_back.y - p_front.y * p_back.x)
self.to_wall_vector = numpy.array([(p_back.y - p_front.y) * ms / ds,
(p_front.x - p_back.x) * ms / ds,
1])
self.along_wall_vector = numpy.array(
[p_front.x - p_back.x, p_front.y - p_back.y, 1])
# Calculate and return the heading vector:
offset = abs(ms / math.sqrt(ds)) - state.distance
if offset > 0:
return 0.3 * self.along_wall_vector + 2 * offset * self.to_wall_vector
else:
return 0.3 * self.along_wall_vector + 3 * offset * self.to_wall_vector
def get_heading(self, state):
"""Get the direction away from the obstacles as a vector."""
# Calculate heading:
x, y = 0, 0
for d, p, w in zip(state.sensor_distances, self.sensor_poses,
self.weights):
pose = Pose(d) >> p
x += pose.x * w
y += pose.y * w
return numpy.array([x, y, 1])
def set_view_rect(self, x, y, width, height):
"""Zoom on the rectangle to fit it into the view
"""
self.__view_rect = (x, y, width, height)
zoom = min(self.size[0] / float(width), self.size[1] / float(height))
xtra_width = self.size[0] / zoom - float(width)
xtra_height = self.size[1] / zoom - float(height)
self._defpose = Pose(x - xtra_width / 2, y - xtra_height / 2, 0)
self._zoom = zoom
self._zoom_c = False
self._adjust_grid(zoom)
self._update_default_state()
def __init__(self):
"""
"""
## params
# tf frames
self._odom_frame = rospy.get_param('~odom_frame', 'odom')
self._base_frame = rospy.get_param('~base_frame', 'base_footprint')
# 2 wheel dolly's wheel separation is ~30.5 cm
self._wheel_separation = float(
rospy.get_param('~wheel_separation', 0.305))
# 2 wheel dolly's wheel diameter is ~12 cm
self._wheel_diameter = float(rospy.get_param('~wheel_diameter', 0.12))
# wheel directions
self._left_wheel_forward = rospy.get_param('~left_wheel_forward', True)
self._right_wheel_forward = rospy.get_param('~right_wheel_forward',
False)
# keigan motors BLE mac addresses
self._left_wheel_mac = rospy.get_param('~left_wheel_mac',
'D8:BA:37:4A:88:A1')
self._right_wheel_mac = rospy.get_param('~right_wheel_mac',
'DD:7A:96:3C:E1:51')
# update rate, default 30 Hz
self._update_rate = rospy.get_param('~update_rate', 30.0)
# lock for synchronizing between read and write to BLE
self._lock = threading.Lock()
## vars
self._pose = Pose()
self._left_last_pos = 0.0
self._right_last_pos = 0.0
self._last_time = rospy.Time.now()
# max rotation is 250 rpm, or ~26 rad/s
self._max_rot = 26.0 #rad/s
# wheels BLE device handle
self._left_wheel_dev = KMControllers.BLEController(
self._left_wheel_mac)
self._right_wheel_dev = KMControllers.BLEController(
self._right_wheel_mac)
## tf
self._tfb = tf.TransformBroadcaster()
## pubs
self._odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
## subs
self._twist_sub = rospy.Subscriber('cmd_vel',
Twist,
self._twist_callback,
queue_size=1)
