def test_learning_from_generated_samples(self): self.setup_publishers_and_subscribers() # Generate random samples and publish msg_v_sample = Vsample() msg_theta_dot_sample = ThetaDotSample() theta_dot_weights = np.array([-1, 1]) v_weights = np.array([1,1]) rate = rospy.Rate(30) timeout = time.time()+10.0 while (not rospy.is_shutdown()) and (not (self.msg_theta_dot_received and self.msg_v_received)) and time.time()<timeout: sample = self.getRandomSamplePair(theta_dot_weights, v_weights) msg_v_sample.d_L = sample[0] msg_v_sample.d_R = sample[1] msg_v_sample.dt = sample[2] msg_v_sample.theta_angle_pose_delta = sample[3] msg_v_sample.x_axis_pose_delta = sample[4] msg_v_sample.y_axis_pose_delta = sample[5] msg_theta_dot_sample.d_L = sample[0] msg_theta_dot_sample.d_R = sample[1] msg_theta_dot_sample.dt = sample[2] msg_theta_dot_sample.theta_angle_pose_delta = sample[3] self.pub_v_sample.publish(msg_v_sample) self.pub_theta_dot_sample.publish(msg_theta_dot_sample) rate.sleep() # Notify if the timeout struck self.assertLess(time.time(),timeout,msg="WARNING: There was not enough time to receive the weights. Either increase the timeout or decrease the number of points needed by the learner.") # Check that the new weights are close to the generating ones self.assertAlmostEqual(self.msg_v_weights.weights[0],1, 0) self.assertAlmostEqual(self.msg_v_weights.weights[1],1, 0) self.assertAlmostEqual(self.msg_theta_dot_weights.weights[0],-1, 0) self.assertAlmostEqual(self.msg_theta_dot_weights.weights[1],1, 0)
def poseCallback(self, msg_pose): self.pose_list.append(msg_pose) n = 0 # make sure we have at least two wheels commands and two poses while len(self.wheels_cmd_list) > 1 and len(self.pose_list) > 1+n: # discard any pose messages received before the first wheel command plus timeOffset while len(self.pose_list) > 1+n and (self.pose_list[0].header.stamp.to_sec() + self.timeOffset < self.wheels_cmd_list[0].header.stamp.to_sec()): del self.pose_list[0] # need at least two poses to calculate delta if len(self.pose_list) > 1+n: # make sure that the second pose is not beyond the duration of the current wheel command if self.pose_list[1+n].header.stamp.to_sec() < (self.wheels_cmd_list[1].header.stamp.to_sec() + self.timeOffset): # Calculate the change in pose #print 'pose', self.pose_list[1] dx = self.pose_list[0].pose.position.x - self.pose_list[1+n].pose.position.x dy = self.pose_list[0].pose.position.y - self.pose_list[1+n].pose.position.y q = [self.pose_list[1+n].pose.orientation.x, self.pose_list[1+n].pose.orientation.y, self.pose_list[1+n].pose.orientation.z, self.pose_list[1+n].pose.orientation.w] q = q/linalg.norm(q) cur_phi = arctan2(2*(q[0]*q[1]+q[2]*q[3]), 1.0-2.0*(q[1]*q[1]+q[2]*q[2])) cur_theta = arcsin(2*(q[0]*q[2]-q[3]*q[1])) cur_psi = arctan2(2*(q[0]*q[3]+q[1]*q[2]), 1-2.0*(q[2]*q[2]+q[3]*q[3])) q = [self.pose_list[0].pose.orientation.x, self.pose_list[0].pose.orientation.y, self.pose_list[0].pose.orientation.z, self.pose_list[0].pose.orientation.w] q = q/linalg.norm(q) delta_phi = cur_phi - arctan2(2*(q[0]*q[1]+q[2]*q[3]), 1.0-2.0*(q[1]*q[1]+q[2]*q[2])) while delta_phi > pi: delta_phi = delta_phi - 2.0*pi while delta_phi <= -pi: delta_phi = delta_phi + 2.0*pi # delta_theta = cur_theta - arcsin(2*(q[0]*q[2]-q[3]*q[1])) # delta_psi = cur_psi - arctan2(2*(q[0]*q[3]+q[1]*q[2]), 1-2.0*(q[2]*q[2]+q[3]*q[3])) # print 'delta_phi', delta_phi, 'delta_theta', delta_theta, 'delta_psi', delta_psi #dw = euler_from_matrix(trans_diff)[2] #Only the z axis dw = -delta_phi dt = self.pose_list[1+n].header.stamp.to_sec() - self.pose_list[0].header.stamp.to_sec() #print 'dt', dt # Stuff the measurements into messages and publish msg_theta_dot = ThetaDotSample() msg_theta_dot.d_L = self.wheels_cmd_list[0].vel_left msg_theta_dot.d_R = self.wheels_cmd_list[0].vel_right msg_theta_dot.dt = dt msg_theta_dot.theta_angle_pose_delta = dw self.pub_theta_dot.publish(msg_theta_dot) msg_v = Vsample() msg_v.d_L = self.wheels_cmd_list[0].vel_left msg_v.d_R = self.wheels_cmd_list[0].vel_right msg_v.dt = dt msg_v.x_axis_pose_delta = -dy # The vicon frame is setup with -y facing forward on the duckiecar msg_v.y_axis_pose_delta = dx msg_v.theta_angle_pose_delta = dw self.pub_v.publish(msg_v) else: del self.wheels_cmd_list[0] del self.pose_list[0]
def test_learning_from_generated_samples(self): self.setup_publishers_and_subscribers() # Generate random samples and publish msg_v_sample = Vsample() msg_theta_dot_sample = ThetaDotSample() theta_dot_weights = np.array([-1, 1]) v_weights = np.array([1, 1]) rate = rospy.Rate(30) timeout = time.time() + 10.0 while ( (not rospy.is_shutdown()) and (not (self.msg_theta_dot_received and self.msg_v_received)) and time.time() < timeout ): sample = self.getRandomSamplePair(theta_dot_weights, v_weights) msg_v_sample.d_L = sample[0] msg_v_sample.d_R = sample[1] msg_v_sample.dt = sample[2] msg_v_sample.theta_angle_pose_delta = sample[3] msg_v_sample.x_axis_pose_delta = sample[4] msg_v_sample.y_axis_pose_delta = sample[5] msg_theta_dot_sample.d_L = sample[0] msg_theta_dot_sample.d_R = sample[1] msg_theta_dot_sample.dt = sample[2] msg_theta_dot_sample.theta_angle_pose_delta = sample[3] self.pub_v_sample.publish(msg_v_sample) self.pub_theta_dot_sample.publish(msg_theta_dot_sample) rate.sleep() # Notify if the timeout struck self.assertLess( time.time(), timeout, msg="WARNING: There was not enough time to receive the weights. Either increase the timeout or decrease the number of points needed by the learner.", ) # Check that the new weights are close to the generating ones self.assertAlmostEqual(self.msg_v_weights.weights[0], 1, 0) self.assertAlmostEqual(self.msg_v_weights.weights[1], 1, 0) self.assertAlmostEqual(self.msg_theta_dot_weights.weights[0], -1, 0) self.assertAlmostEqual(self.msg_theta_dot_weights.weights[1], 1, 0)
def poseCallback(self, msg_pose): self.pose_list.append(msg_pose) n = 0 # make sure we have at least two wheels commands and two poses while len(self.wheels_cmd_list) > 1 and len(self.pose_list) > 1 + n: # discard any pose messages received before the first wheel command plus timeOffset while len(self.pose_list) > 1 + n and ( self.pose_list[0].header.stamp.to_sec() + self.timeOffset < self.wheels_cmd_list[0].header.stamp.to_sec()): del self.pose_list[0] # need at least two poses to calculate delta if len(self.pose_list) > 1 + n: # make sure that the second pose is not beyond the duration of the current wheel command if self.pose_list[1 + n].header.stamp.to_sec() < ( self.wheels_cmd_list[1].header.stamp.to_sec() + self.timeOffset): # Calculate the change in pose #print 'pose', self.pose_list[1] dx = self.pose_list[0].pose.position.x - self.pose_list[ 1 + n].pose.position.x dy = self.pose_list[0].pose.position.y - self.pose_list[ 1 + n].pose.position.y q = [ self.pose_list[1 + n].pose.orientation.x, self.pose_list[1 + n].pose.orientation.y, self.pose_list[1 + n].pose.orientation.z, self.pose_list[1 + n].pose.orientation.w ] q = q / linalg.norm(q) cur_phi = arctan2(2 * (q[0] * q[1] + q[2] * q[3]), 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])) cur_theta = arcsin(2 * (q[0] * q[2] - q[3] * q[1])) cur_psi = arctan2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2.0 * (q[2] * q[2] + q[3] * q[3])) q = [ self.pose_list[0].pose.orientation.x, self.pose_list[0].pose.orientation.y, self.pose_list[0].pose.orientation.z, self.pose_list[0].pose.orientation.w ] q = q / linalg.norm(q) delta_phi = cur_phi - arctan2( 2 * (q[0] * q[1] + q[2] * q[3]), 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])) while delta_phi > pi: delta_phi = delta_phi - 2.0 * pi while delta_phi <= -pi: delta_phi = delta_phi + 2.0 * pi # delta_theta = cur_theta - arcsin(2*(q[0]*q[2]-q[3]*q[1])) # delta_psi = cur_psi - arctan2(2*(q[0]*q[3]+q[1]*q[2]), 1-2.0*(q[2]*q[2]+q[3]*q[3])) # print 'delta_phi', delta_phi, 'delta_theta', delta_theta, 'delta_psi', delta_psi #dw = euler_from_matrix(trans_diff)[2] #Only the z axis dw = -delta_phi dt = self.pose_list[1 + n].header.stamp.to_sec( ) - self.pose_list[0].header.stamp.to_sec() #print 'dt', dt # Stuff the measurements into messages and publish msg_theta_dot = ThetaDotSample() msg_theta_dot.d_L = self.wheels_cmd_list[0].vel_left msg_theta_dot.d_R = self.wheels_cmd_list[0].vel_right msg_theta_dot.dt = dt msg_theta_dot.theta_angle_pose_delta = dw self.pub_theta_dot.publish(msg_theta_dot) msg_v = Vsample() msg_v.d_L = self.wheels_cmd_list[0].vel_left msg_v.d_R = self.wheels_cmd_list[0].vel_right msg_v.dt = dt msg_v.x_axis_pose_delta = -dy # The vicon frame is setup with -y facing forward on the duckiecar msg_v.y_axis_pose_delta = dx msg_v.theta_angle_pose_delta = dw self.pub_v.publish(msg_v) else: del self.wheels_cmd_list[0] del self.pose_list[0]