def mv_wall(self, secs=3):
vel = max_vel
vec_zero = Vector3(0, 0, 0)
vec_fwd = Vector3(vel, 0, 0)
vec_bwd = Vector3(-vel, 0, 0)
move_fwd = Twist(vec_fwd, vec_zero)
move_bwd = Twist(vec_bwd, vec_zero)
steps = int(self.hertz * secs)
steps2 = int(self.hertz * secs * 0.7)
samples = {'accel': [], 'gyro': [], 'compass': [], 'image': []}
recording = False
for i in range(steps):
if recording:
get_vals(samples)
elif i >= steps * 0.5:
im.get_vals() #empty queue
recording = True
self.pub.publish(move_fwd)
self.rate.sleep()
#*0.5 since it shouldn't go back too far
for i in range(steps2):
self.pub.publish(move_bwd)
self.rate.sleep()
return samples, vel, 0, 0
def mv_straight(self, secs=4):
vel = max_vel
vec_zero = Vector3(0, 0, 0)
vec_fwd = Vector3(vel, 0, 0)
vec_bwd = Vector3(-vel, 0, 0)
move_fwd = Twist(vec_fwd, vec_zero)
move_bwd = Twist(vec_bwd, vec_zero)
steps = int(self.hertz * secs)
samples = {'accel': [], 'gyro': [], 'compass': [], 'image': []}
recording = False
for i in range(steps):
if recording:
get_vals(samples)
elif i >= steps * 0.2:
im.get_vals() #empty queue
recording = True
self.pub.publish(move_fwd)
self.rate.sleep()
for i in range(steps):
self.pub.publish(move_bwd)
self.rate.sleep()
return samples, vel, 0, 0
def mv_curve(self, velocity, radius, angle, left=True, short=False):
if angle == None:
angle = velocity / radius
#seconds for the curve (calculated for a 90 degree turn)
secs = math.pi / 2 / angle
if short:
secs = secs / 2
#seconds to go straight
secs_straight = 2
if not left:
angle = -angle
#movement data
vec_zero = Vector3(0, 0, 0)
vec_fwd = Vector3(velocity, 0, 0)
vec_curve = Vector3(0, 0, angle)
move_fwd = Twist(vec_fwd, vec_zero)
move_curve = Twist(vec_fwd, vec_curve)
#movement: first half straight, curve, second half straight
total = int(self.hertz * secs_straight)
first_half = int(total / 2)
sec_half = total - first_half
steps = int(self.hertz * secs)
samples = {'accel': [], 'gyro': [], 'compass': [], 'image': []}
recording = False
#one second straight
for i in range(first_half):
if recording:
get_vals(samples)
elif i >= first_half * 0.5:
recording = True
im.get_vals() #empty_queue
self.pub.publish(move_fwd)
self.rate.sleep()
#curve
for i in range(steps):
get_vals(samples)
self.pub.publish(move_curve)
self.rate.sleep()
#one second straight
for i in range(sec_half):
if i <= sec_half * 0.6:
get_vals(samples)
self.pub.publish(move_fwd)
self.rate.sleep()
return samples, velocity, angle, radius
def imu_publisher(hertz=80, secs=5):
pub = rospy.Publisher('/odom', Twist, queue_size=10)
rospy.init_node('imu_publisher', anonymous=True)
rate = rospy.Rate(hertz)
vals = []
while not rospy.is_shutdown():
#for i in range(secs*hertz):
vals = im.get_vals()
for v in vals:
v1 = Vector3(v['accel'][0], v['accel'][1], v['accel'][2])
v2 = Vector3(v['gyro'][0], v['gyro'][1], v['gyro'][2])
#v1 = Vector3(random.randint(-10,10),random.randint(-10,10),random.randint(-10,10))
#v2 = Vector3(random.randint(-10,10),random.randint(-10,10),random.randint(-10,10))
t = Twist(v1, v2)
pub.publish(t)
rate.sleep()
im.stop()
def mv_angle(self, degree, left=True):
#half the window length -> window size is steps_to_save*2
steps_to_save = 25
vel = max_vel
secs = 2
angle = math.pi * degree / 180
velY = math.sin(angle) * vel
velX = math.cos(angle) * vel
#make sure there is enough time to get back (worst case 180 degree -> 2*secs)
#if secs_back is chosen too low, max speed might get exceeded
#use law of cosines to calculate best value
#ex. for angle=180 -> secs_back = 2*secs
#ex. for angle=90 -> secs_back = sqrt(2)*secs
#note: angle needs to be converted (by "mirroring"), since we need the angle inside of the triangle (currently angle = <outside angle>-180)
secs_back = math.sqrt(secs * secs * 2 *
(1 - math.cos(math.pi - angle)))
velX_back = (-vel - velX) * secs / secs_back
velY_back = (-velX) * secs / secs_back
#if degree == 0:
# secs_back = 2*secs
if not left:
velY = -velY
vec_zero = Vector3(0, 0, 0)
vec_fwd = Vector3(vel, 0, 0)
vec_side = Vector3(velX, velY, 0)
vec_back = Vector3(velX_back, velY_back, 0)
move_fwd = Twist(vec_fwd, vec_zero)
move_side = Twist(vec_side, vec_zero)
move_back = Twist(vec_back, vec_zero)
steps = int(self.hertz * secs)
steps_back = int(self.hertz * secs_back + 0.5)
samples = {'accel': [], 'gyro': [], 'compass': [], 'image': []}
recording = False
for i in range(steps):
if recording:
get_vals(samples)
elif i >= steps * 0.2:
im.get_vals() #empty queue
recording = True
self.pub.publish(move_fwd)
self.rate.sleep()
#middle of the sample
pivot = len(samples['accel']) + 10
for i in range(steps):
if i <= steps * 0.8:
get_vals(samples)
self.pub.publish(move_side)
self.rate.sleep()
#go back to start
for i in range(steps_back):
self.pub.publish(move_back)
self.rate.sleep()
#cut sample to fixed window size
keys = ['accel', 'gyro', 'compass'] #(keys except image)
for key in keys:
data = samples[key]
samples[key] = data[(pivot - steps_to_save):(pivot +
steps_to_save)]
assert len(
samples[key]
) == steps_to_save * 2 #in case steps_to_save is too big
#return the degree for the radius, not really needed
return samples, vel, angle, degree
def get_vals(samples):
imu_vals = im.get_vals()
for imu_val in imu_vals:
samples['accel'].append(imu_val['accel'])
samples['gyro'].append(imu_val['gyro'])
samples['compass'].append(imu_val['compass'])