def main():
rclpy.init()
node = Node("talker")
pub = node.create_publisher(Pose2D, 'robot0/target_velocity')
simTime = SimTimer(True, "veranda/timestamp", node)
# Factor to scale down speed by
speedScale = 10
# Tick time at 10 hz
dt = 0.1
def cb():
# Calculate wheel velocities for current time
x, y = speedScale * math.sin(
simTime.global_time()), speedScale * 2 * math.cos(
2 * simTime.global_time())
t = 0.
print(x, y)
# Publish velocities
publishTargetVelocity(pub, x, y, t)
simTime.create_timer(dt, cb)
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
def main():
rclpy.init()
node = Node("talker")
publeft = node.create_publisher(Float32, 'robot0/left_wheel')
pubright = node.create_publisher(Float32, 'robot0/right_wheel')
simTime = SimTimer(True, "veranda/timestamp", node)
# Factor to scale down speed by
speedScale = 2
# Tick time at 10 hz
dt = 0.1
def cb():
# Calculate wheel velocities for current time
phi1, phi2 = DD_IK(x_t, y_t, simTime.global_time() + 2*math.pi, speedScale)
print(phi1, phi2)
# Publish velocities
publishWheelVelocity(publeft, pubright, phi1, phi2)
simTime.create_timer(dt, cb)
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
def __init__(self):
"""Creates an instance of the Wiggler class."""
super().__init__('Wiggler')
# Create a simulation timer so we can speed up the simulation without
# breaking things.
self.simulation = SimTimer(True, 'veranda/timestamp', self)
self.timer = self.simulation.create_timer(1, self.wiggle_left)
# Create publishers for the left and right wheels.
self.left_pub = self.create_publisher(Float32, 'robot/left_wheel')
self.right_pub = self.create_publisher(Float32, 'robot/right_wheel')
class Fig8:
"""Drives a robot in a figure 8"""
def __init__(self, name):
"""Assumes robot's initial pose is (10, 10), facing 3pi/4."""
self.node = ros.create_node(name)
self.left_wheel = self.node.create_publisher(Float32,
f'{name}/left_wheel')
self.right_wheel = self.node.create_publisher(Float32,
f'{name}/right_wheel')
r = 0.5
L = 0.6
R = 5.0
self.left_speed = 3.0
self.right_speed = self.left_speed * (R + 2 * L) / R
circuit_time = (14 * R) / (r * (self.right_speed + self.left_speed))
self.simulation = SimTimer(True, 'veranda/timestamp', self.node)
self.timer = self.simulation.create_timer(circuit_time, self.callback)
def callback(self):
msg = Float32()
msg.data = self.left_speed
self.left_wheel.publish(msg)
msg.data = self.right_speed
self.right_wheel.publish(msg)
# Swap the wheel speeds to reverse path direction
self.left_speed, self.right_speed = self.right_speed, self.left_speed
def __init__(self, name):
"""Assumes robot's initial pose is (10, 10), facing 3pi/4."""
self.node = ros.create_node(name)
self.left_wheel = self.node.create_publisher(Float32,
f'{name}/left_wheel')
self.right_wheel = self.node.create_publisher(Float32,
f'{name}/right_wheel')
r = 0.5
L = 0.6
R = 5.0
self.left_speed = 3.0
self.right_speed = self.left_speed * (R + 2 * L) / R
circuit_time = (14 * R) / (r * (self.right_speed + self.left_speed))
self.simulation = SimTimer(True, 'veranda/timestamp', self.node)
self.timer = self.simulation.create_timer(circuit_time, self.callback)
def __init__(self, name, points, loop=False):
"""Creates a differential drive robot with the given name.
The name is used to determine which topics to listen and publish to.
The initial state of the robot is assumed to be (0, 0), facing heading 0.0 radians.
:param name: The name of the robot.
:param points: The points through which the robot should drive.
"""
self.node = ros.create_node(f'{name}_robot')
# We control our robot by publishing left and right wheel speeds.
self.left_wheel = self.node.create_publisher(Float32,
f'{name}/left_wheel')
self.right_wheel = self.node.create_publisher(Float32,
f'{name}/right_wheel')
# Use the simulation timer so we can play with the simulation speed.
self._simulation_timer = SimTimer(True, 'veranda/timestamp', self.node)
# NOTE: The fidelity of the veranda SimTimer appears to be 0.033 seconds.
self.timer = self._simulation_timer.create_timer(1, self.callback)
# Make sure each point is an np.array
points = [np.array(p) for p in points]
self.points = points
configs = self.config()
def insert_pauses(configs):
for config in configs:
yield config
yield 0.0, 0.0, 1.0
# Insert pauses between motion to make sure the robot comes to a complete stop.
configs = insert_pauses(configs)
self.configs = itertools.cycle(configs) if loop else configs
class Wiggler:
"""The wiggle code from the textbook, wrapped in a class to avoid
contracting cancer from using global variables.
"""
def __init__(self):
"""Creates an instance of the Wiggler class."""
self.node = ros.create_node('Wiggler')
# Create a simulation timer so we can speed up the simulation without
# breaking things.
self.simulation = SimTimer(True, 'veranda/timestamp', self.node)
self.timer = self.simulation.create_timer(1, self.wiggle_left)
# Create publishers for the left and right wheels.
self.left_pub = self.node.create_publisher(Float32, 'robot/left_wheel')
self.right_pub = self.node.create_publisher(Float32,
'robot/right_wheel')
def wiggle_left(self):
"""Wiggles the robot to the left"""
self.simulation.destroy_timer(self.timer)
msg = Float32()
msg.data = 5.0
self.right_pub.publish(msg)
msg.data = 0.0
self.left_pub.publish(msg)
self.timer = self.simulation.create_timer(1, self.wiggle_right)
def wiggle_right(self):
"""Wiggles the robot to the right"""
self.simulation.destroy_timer(self.timer)
msg = Float32()
msg.data = 0.0
self.right_pub.publish(msg)
msg.data = 5.0
self.left_pub.publish(msg)
self.timer = self.simulation.create_timer(1, self.wiggle_left)
class DiffDrive:
"""A differential drive robot that drives point-to-point.
The robot works by publishing left and right wheel speeds (radians-per-second) to the topics
`<robot name>/left_wheel` and `<robot name>/right_wheel`.
If r is the wheel radius, and L is the distance between the pivot point and the wheel, we have
theta(t) = (r / (2 * L)) * (w1 - w2) * t + theta_0
So, assuming fixed wheel speeds, we can solve for the time taken to turn as
t(theta) = ((2 * L) / r) * ((theta - theta_0) / (w1 - w2))
But note that theta - theta_0 is the angle between the two motion vectors when turning.
"""
def __init__(self, name, points, loop=False):
"""Creates a differential drive robot with the given name.
The name is used to determine which topics to listen and publish to.
The initial state of the robot is assumed to be (0, 0), facing heading 0.0 radians.
:param name: The name of the robot.
:param points: The points through which the robot should drive.
"""
self.node = ros.create_node(f'{name}_robot')
# We control our robot by publishing left and right wheel speeds.
self.left_wheel = self.node.create_publisher(Float32,
f'{name}/left_wheel')
self.right_wheel = self.node.create_publisher(Float32,
f'{name}/right_wheel')
# Use the simulation timer so we can play with the simulation speed.
self._simulation_timer = SimTimer(True, 'veranda/timestamp', self.node)
# NOTE: The fidelity of the veranda SimTimer appears to be 0.033 seconds.
self.timer = self._simulation_timer.create_timer(1, self.callback)
# Make sure each point is an np.array
points = [np.array(p) for p in points]
self.points = points
configs = self.config()
def insert_pauses(configs):
for config in configs:
yield config
yield 0.0, 0.0, 1.0
# Insert pauses between motion to make sure the robot comes to a complete stop.
configs = insert_pauses(configs)
self.configs = itertools.cycle(configs) if loop else configs
def config(self):
"""A finite iterable of robot configurations.
Makes the (big) assumption that the robot's initial state is at (0, 0),
and faces 0 radians.
Each configuration point is a (right speed, left speed, duration) tuple,
where the speeds are in rad/sec.
"""
# Fix the wheel speeds, because I'm almost out of rum.
speed = 10.0
left_turn_speed = -speed / 2
right_turn_speed = speed / 2
# Radius of wheel
r = 0.5
# Distance from center of robot to center of wheel
L = 0.6
# Convert the path points into vectors
vectors = [p2 - p1 for p1, p2 in pairwise(self.points)]
# Get the angles between each vector. Loop back around to close the loop.
angles = [
angle_between(v1, v2)
for v1, v2 in pairwise(vectors + [vectors[0]])
]
# Get the length of each vector
distances = [magnitude(v) for v in vectors]
for distance, angle in zip(distances, angles):
print(f'distance = {distance}')
# Calculate how long it will take to drive the straight distance.
drive_time = distance / (2 * np.pi * r * speed)
yield speed, speed, drive_time
print(f'angle = {angle}')
# Calculate how long it will take to turn the given angle with fixed wheel speeds.
# BUG: Either this calculation is incorrect, or the robot isn't driving the wheels
# at the given speeds for this time.
turn_time = (2 * L *
angle) / (r * (right_turn_speed - left_turn_speed))
yield right_turn_speed, left_turn_speed, turn_time
def callback(self):
"""Publishes the wheel speeds to control the robot.
Consumes the self.configs generator.
"""
# Destroy the timer
self._simulation_timer.destroy_timer(self.timer)
try:
# Get the next configuration point.
right, left, time = next(self.configs)
print(f'config = ({right}, {left}, {time})')
msg = Float32()
msg.data = right
self.right_wheel.publish(msg)
msg.data = left
self.left_wheel.publish(msg)
self.timer = self._simulation_timer.create_timer(
time, self.callback)
except StopIteration:
pass
def main():
args = sys.argv
joyname = ""
if len(args) > 1:
joyname = args[1]
else:
# Iterate over the joystick devices.
print("Available Joysticks:")
for fn in os.listdir('/dev/input'):
if fn.startswith('js'):
print('\t%s' % (fn))
sys.exit()
# We'll store the states here.
axis_states = {}
button_states = {}
axis_map = []
button_map = []
# Open the joystick device.
fn = '/dev/input/' + joyname
print('Opening %s...' % fn)
jsdev = open(fn, 'rb')
# Get the device name.
#buf = bytearray(63)
buf = array.array('b', [0] * 64)
ioctl(jsdev, 0x80006a13 + (0x10000 * len(buf)), buf) # JSIOCGNAME(len)
js_name = buf.tostring()
print('Device name: %s' % js_name)
# Get number of axes and buttons.
buf = array.array('B', [0])
ioctl(jsdev, 0x80016a11, buf) # JSIOCGAXES
num_axes = buf[0]
buf = array.array('B', [0])
ioctl(jsdev, 0x80016a12, buf) # JSIOCGBUTTONS
num_buttons = buf[0]
# Get the axis map.
buf = array.array('B', [0] * 0x40)
ioctl(jsdev, 0x80406a32, buf) # JSIOCGAXMAP
for axis in buf[:num_axes]:
axis_name = axis_names.get(axis, 'unknown(0x%02x)' % axis)
axis_map.append(axis_name)
axis_states[axis_name] = 0.0
# Get the button map.
buf = array.array('H', [0] * 200)
ioctl(jsdev, 0x80406a34, buf) # JSIOCGBTNMAP
for btn in buf[:num_buttons]:
btn_name = button_names.get(btn, 'unknown(0x%03x)' % btn)
button_map.append(btn_name)
button_states[btn_name] = 0
print('%d axes found: %s' % (num_axes, ', '.join(axis_map)))
print('%d buttons found: %s' % (num_buttons, ', '.join(button_map)))
# Spin child process to read the joystick
# with blocking reads
events = Queue()
t = Process(target=readJoystick, args=(jsdev, events))
t.start()
rclpy.init()
node = Node("joystick_" + joyname)
pubjoy = node.create_publisher(Joy, joyname + '/joystick')
simTime = SimTimer(False, "veranda/timestamp", node)
# Tick time at 10 hz
dt = 0.1
def cb():
processJoystick(events, button_states, axis_states, button_map,
axis_map)
msg = formatJoystickMessage(button_states, axis_states)
pubjoy.publish(msg)
simTime.create_timer(dt, cb)
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
t.terminate()
#!/usr/bin/env python3
import rclpy as ros
from rclpy.node import Node
from std_msgs.msg import Float32
from veranda.SimTimer import SimTimer
ros.init()
node = Node('Wiggle')
sim_timer = SimTimer(True, 'veranda/timestamp', node)
publeft = node.create_publisher(Float32, 'robot0/left_wheel')
pubright = node.create_publisher(Float32, 'robot0/right_wheel')
def wiggle_left():
global timer_handle
sim_timer.destroy_timer(timer_handle)
msg = Float32()
msg.data = 5.0
pubright.publish(msg)
msg.data = 0.0
publeft.publish(msg)
timer_handle = sim_timer.create_timer(1, wiggle_right)
def wiggle_right():
global timer_handle
sim_timer.destroy_timer(timer_handle)
left, right = go_straight(left, right)
time.sleep(8)
x, y, th = calc_values(x, y, th, 8, r, l, left, right)
print('x: {0:9f} y: {1:9f} th: {2:9f}'.format(x, y, th))
x = 0.0
y = 0.0
goalx = 0.0
goaly = 15.0
th = math.pi / 2.0
left = 0.0
right = 0.0
t = 0.01
r = 0.5
l = 0.6
turning = False
rclpy.init()
node = Node("Basic")
publeft = node.create_publisher(Float32, 'robot0/left_wheel')
pubright = node.create_publisher(Float32, 'robot0/right_wheel')
lidar = node.create_subscription(LaserScan, 'robot0/lidar', checkScans)
simTime = SimTimer(True, 'veranda/timestamp', node)
timer_handle = simTime.create_timer(0.01, run)
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()