def execute(self, userdata):
global isToTheLeft, CURRENT_STATE
CURRENT_STATE = "SeanTurn"
turn_direction = 1
start_pose = [0, 0, 0, 0]
if self.angle == 0: # target is goal + 0
goal = start_pose[1]
elif self.angle == 90: # target is goal + turn_direction * 90
goal = start_pose[1] + np.pi / 2 * turn_direction
elif self.angle == 180: # target is goal + turn_direction * 180
goal = start_pose[1] + np.pi * turn_direction
elif self.angle == -90: # target is goal + turn_direction * 270
goal = start_pose[1] - np.pi / 2 * turn_direction
elif self.angle == -100: # target is goal + turn_direction * 270
goal = start_pose[1] - 5 * np.pi / 9 * turn_direction
elif self.angle == 120:
goal = start_pose[1] + 2 * np.pi / 3 * turn_direction
elif self.angle == 135:
goal = start_pose[1] + 150 * np.pi / 180 * turn_direction
elif self.angle == 999:
if isToTheLeft:
goal = start_pose[1] - 85 * np.pi / 180 * turn_direction
else:
goal = start_pose[1] + 85 * np.pi / 180 * turn_direction
goal = angles_lib.normalize_angle(goal)
cur = np.abs(angles_lib.normalize_angle(self.tb_rot[2]) - goal)
speed = 0.55
rate = rospy.Rate(30)
direction = turn_direction
if 2 * np.pi - angles_lib.normalize_angle_positive(
goal) < angles_lib.normalize_angle_positive(
goal) or self.angle == 0:
direction = turn_direction * -1
while not rospy.is_shutdown():
cur = np.abs(angles_lib.normalize_angle(self.tb_rot[2]) - goal)
# slow down turning as we get closer to the target heading
if cur < 0.1:
speed = 0.1
if cur < 0.0174533:
break
msg = Twist()
msg.linear.x = 0.0
msg.angular.z = direction * speed
self.cmd_pub.publish(msg)
rate.sleep()
msg = Twist()
msg.linear.x = 0.0
msg.angular.z = 0.0
self.cmd_pub.publish(msg)
return 'done'
def execute(self, userdata):
global turn_direction
global START
global POSE
if not START:
return 'exit'
start_pose = POSE
if self.angle == 0: # target is goal + 0
goal = start_pose[1]
elif self.angle == 90: # target is goal + turn_direction * 90
goal = start_pose[1] + np.pi/2 * turn_direction
elif self.angle == 180: # target is goal + turn_direction * 180
goal = start_pose[1] + np.pi * turn_direction
elif self.angle == -90: # target is goal + turn_direction * 270
goal = start_pose[1] - np.pi/2 * turn_direction
elif self.angle == -100: # target is goal + turn_direction * 270
goal = start_pose[1] - 5*np.pi/9 * turn_direction
goal = angles_lib.normalize_angle(goal)
cur = np.abs(angles_lib.normalize_angle(self.tb_rot[2]) - goal)
speed = 0.55
rate = rospy.Rate(30)
direction = turn_direction
if 2 * np.pi - angles_lib.normalize_angle_positive(goal) < angles_lib.normalize_angle_positive(goal) or self.angle == 0:
direction = turn_direction * -1
while not rospy.is_shutdown():
cur = np.abs(angles_lib.normalize_angle(self.tb_rot[2]) - goal)
# slow down turning as we get closer to the target heading
if cur < 0.1:
speed = 0.15
if cur < 0.0571:
break
msg = Twist()
msg.linear.x = 0.0
msg.angular.z = direction * speed
self.cmd_pub.publish(msg)
rate.sleep()
msg = Twist()
msg.linear.x = 0.0
msg.angular.z = 0.0
self.cmd_pub.publish(msg)
return 'success'
def sensor_cb(self, us1_sub, us2_sub, us3_sub, us4_sub, us5_sub, us6_sub, odom_sub):
orientation_in_quaternions = (
odom_sub.pose.pose.orientation.x,
odom_sub.pose.pose.orientation.y,
odom_sub.pose.pose.orientation.z,
odom_sub.pose.pose.orientation.w)
orientation_in_euler = euler_from_quaternion(orientation_in_quaternions)
yaw = orientation_in_euler[2]
yaw_radians = angles.normalize_angle_positive(yaw)
ground_truth_x = odom_sub.pose.pose.position.x
ground_truth_y = odom_sub.pose.pose.position.y
self.predict_data[0][0] = yaw_radians
self.predict_data[0][1] = us1_sub.range
self.predict_data[0][2] = us2_sub.range
self.predict_data[0][3] = us3_sub.range
self.predict_data[0][4] = us4_sub.range
self.predict_data[0][5] = us5_sub.range
self.predict_data[0][6] = us6_sub.range
test_predictions = self.model.predict(self.predict_data)
print("predictions " + str(test_predictions[0][0]) + " " + str(test_predictions[0][1]))
print("ground truth " + str(ground_truth_x) + " " + str(ground_truth_y))
msg = Pose()
msg.position.x = test_predictions[0][0]
msg.position.y = test_predictions[0][1]
msg.position.z = 0
msg.orientation.x = 0
msg.orientation.y = 0
msg.orientation.z = 0
msg.orientation.w = 0
self.predicted_pose_publisher.publish(msg)
def sensor_cb(self, lidar1_sub, odom_sub):
self.reset_counter += 1
self.write_to_csv_counter += 1
lidar1_ranges = lidar1_sub.ranges
print(lidar1_ranges)
# Kerätään asematieto Odom-viestin Quanterion-tiedosta...
orientation_in_quaterions = (odom_sub.pose.pose.orientation.x,
odom_sub.pose.pose.orientation.y,
odom_sub.pose.pose.orientation.z,
odom_sub.pose.pose.orientation.w)
orientation_in_euler = euler_from_quaternion(orientation_in_quaterions)
roll = orientation_in_euler[0]
pitch = orientation_in_euler[1]
yaw = orientation_in_euler[2]
yaw_radians = angles.normalize_angle_positive(yaw)
#ground_truth_x = odom_sub.pose.pose.position.x
ground_truth_x = odom_sub.pose.pose.orientation.x
#ground_truth_y = odom_sub.pose.pose.position.y
ground_truth_y = odom_sub.pose.pose.orientation.y
if self.write_to_csv_counter > 19:
self.positions_writer.writerow(
[yaw_radians, lidar1_ranges, ground_truth_x, ground_truth_y])
self.write_to_csv_counter = 0
if self.reset_counter > 10000:
self.reset_simulation_call()
self.reset_counter = 0
def sensor_cb(self, us1_sub, us2_sub, us3_sub, us4_sub, us5_sub, us6_sub,
odom_sub):
self.reset_counter += 1
self.write_to_csv_counter += 1
orientation_in_quaternions = (odom_sub.pose.pose.orientation.x,
odom_sub.pose.pose.orientation.y,
odom_sub.pose.pose.orientation.z,
odom_sub.pose.pose.orientation.w)
orientation_in_euler = euler_from_quaternion(
orientation_in_quaternions)
roll = orientation_in_euler[0]
pitch = orientation_in_euler[1]
yaw = orientation_in_euler[2]
yaw_radians = angles.normalize_angle_positive(yaw)
ground_truth_x = odom_sub.pose.pose.position.x
ground_truth_y = odom_sub.pose.pose.position.y
if self.write_to_csv_counter > 19:
self.positions_writer.writerow([
yaw_radians, us1_sub.range, us2_sub.range, us3_sub.range,
us4_sub.range, us5_sub.range, us6_sub.range, ground_truth_x,
ground_truth_y
])
self.write_to_csv_counter = 0
if self.reset_counter > 10000:
self.reset_simulation_call()
self.reset_simulation_call = 0
def test_normalize_angle_positive(self, a):
a = a * np.pi
ref_r = normalize_angle_positive(a)
sw_r = w.compile_and_execute(w.normalize_angle_positive, [a])
self.assertAlmostEqual(shortest_angular_distance(ref_r, sw_r),
0.0,
places=5)
def sensor_cb(self, us1_sub, us2_sub, us3_sub, us4_sub, us5_sub, us6_sub,
odom_sub):
self.reset_counter += 1
self.write_to_csv_counter += 1
orientation_in_quarternions = (odom_sub.pose.pose.orientation.x,
odom_sub.pose.pose.orientation.y,
odom_sub.pose.pose.orientation.z,
odom_sub.pose.pose.orientation.w)
orientation_in_euler = euler_from_quaternion(
orientation_in_quarternions) # quater.. to euler
roll = orientation_in_euler[0]
pitch = orientation_in_euler[1]
yaw = orientation_in_euler[2]
yaw_radians = angles.normalize_angle_positive(yaw)
ground_truth_x = odom_sub.pose.pose.position.x
ground_truth_y = odom_sub.pose.pose.position.y
if self.write_to_csv_counter > 19: # joka 20:a
self.positions_writer.writerow([
yaw_radians, us1_sub.range, us2_sub.range, us3_sub.range,
us4_sub.range, us5_sub.range, us6_sub.range, ground_truth_x,
ground_truth_y
])
#self.positions_writer.writerow([
#yaw_radians, us1_sub.range, us2_sub.range,
#us3_sub.range,us4_sub.range, us5_sub.range, # otetaann mittaukset ja oikeat arvot x ja y
#us6_sub.range, ground_truth_x, ground_truth_y])
self.write_to_csv_counter = 0
if self.reset_counter > 10000:
self.reset_simulation_call() # käynnnistää uudelleen
self.reset_counter = 0
def sensor_cb(self, us1_sub, us2_sub, us3_sub, us4_sub, us5_sub, us6_sub,
imu_sub):
self.reset_counter += 1
self.write_to_csv_counter += 1
# Kerätään asematieto Imu-viestin Quanterion-tiedosta...
orientation_in_quaterions = (
#odom_sub.pose.pose.orientation.x,
#odom_sub.pose.pose.orientation.y,
#odom_sub.pose.pose.orientation.z,
#odom_sub.pose.pose.orientation.w
imu_sub.orientation.x,
imu_sub.orientation.y,
imu_sub.orientation.z,
imu_sub.orientation.w)
orientation_in_euler = euler_from_quaternion(orientation_in_quaterions)
roll = orientation_in_euler[0]
pitch = orientation_in_euler[1]
yaw = orientation_in_euler[2]
yaw_radians = angles.normalize_angle_positive(yaw)
#ground_truth_x = odom_sub.pose.pose.position.x
ground_truth_x = imu_sub.orientation.x
#ground_truth_y = odom_sub.pose.pose.position.y
ground_truth_y = imu_sub.orientation.y
if self.write_to_csv_counter > 19:
self.positions_writer.writerow([
yaw_radians, us1_sub.range, us2_sub.range, us3_sub.range,
us4_sub.range, us5_sub.range, us6_sub.range, ground_truth_x,
ground_truth_y
])
self.write_to_csv_counter = 0
if self.reset_counter > 10000:
self.reset_simulation_call()
self.reset_counter = 0
def test_normalize_angle_positive(self, a):
expected = normalize_angle_positive(a)
actual = w.compile_and_execute(w.normalize_angle_positive, [a])
self.assertAlmostEqual(shortest_angular_distance(expected, actual), 0.0, places=5)
def test_normalize_angle_positive(self):
self.assertAlmostEqual(0, normalize_angle_positive(0))
self.assertAlmostEqual(pi, normalize_angle_positive(pi))
self.assertAlmostEqual(0, normalize_angle_positive(2*pi))
self.assertAlmostEqual(pi, normalize_angle_positive(3*pi))
self.assertAlmostEqual(0, normalize_angle_positive(4*pi))
self.assertAlmostEqual(0, normalize_angle_positive(-0))
self.assertAlmostEqual(pi, normalize_angle_positive(-pi))
self.assertAlmostEqual(0, normalize_angle_positive(-2*pi))
self.assertAlmostEqual(pi, normalize_angle_positive(-3*pi))
self.assertAlmostEqual(0, normalize_angle_positive(-4*pi))
self.assertAlmostEqual(0, normalize_angle_positive(-0))
self.assertAlmostEqual(3*pi/2, normalize_angle_positive(-pi/2))
self.assertAlmostEqual(pi, normalize_angle_positive(-pi))
self.assertAlmostEqual(pi/2, normalize_angle_positive(-3*pi/2))
self.assertAlmostEqual(0, normalize_angle_positive(-4*pi/2))
self.assertAlmostEqual(0, normalize_angle_positive(0))
self.assertAlmostEqual(pi/2, normalize_angle_positive(pi/2))
self.assertAlmostEqual(pi/2, normalize_angle_positive(5*pi/2))
self.assertAlmostEqual(pi/2, normalize_angle_positive(9*pi/2))
self.assertAlmostEqual(pi/2, normalize_angle_positive(-3*pi/2))
def test_normalize_angle_positive(self):
self.assertAlmostEqual(0, normalize_angle_positive(0))
self.assertAlmostEqual(pi, normalize_angle_positive(pi))
self.assertAlmostEqual(0, normalize_angle_positive(2 * pi))
self.assertAlmostEqual(pi, normalize_angle_positive(3 * pi))
self.assertAlmostEqual(0, normalize_angle_positive(4 * pi))
self.assertAlmostEqual(0, normalize_angle_positive(-0))
self.assertAlmostEqual(pi, normalize_angle_positive(-pi))
self.assertAlmostEqual(0, normalize_angle_positive(-2 * pi))
self.assertAlmostEqual(pi, normalize_angle_positive(-3 * pi))
self.assertAlmostEqual(0, normalize_angle_positive(-4 * pi))
self.assertAlmostEqual(0, normalize_angle_positive(-0))
self.assertAlmostEqual(3 * pi / 2, normalize_angle_positive(-pi / 2))
self.assertAlmostEqual(pi, normalize_angle_positive(-pi))
self.assertAlmostEqual(pi / 2, normalize_angle_positive(-3 * pi / 2))
self.assertAlmostEqual(0, normalize_angle_positive(-4 * pi / 2))
self.assertAlmostEqual(0, normalize_angle_positive(0))
self.assertAlmostEqual(pi / 2, normalize_angle_positive(pi / 2))
self.assertAlmostEqual(pi / 2, normalize_angle_positive(5 * pi / 2))
self.assertAlmostEqual(pi / 2, normalize_angle_positive(9 * pi / 2))
self.assertAlmostEqual(pi / 2, normalize_angle_positive(-3 * pi / 2))
