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))