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 __init__(self):
self.node_name = 'virtual_learning_node'
# Setup the publishers and subscribers
self.sub_pose = rospy.Subscriber("~pose", Pose2DStamped,
self.pose2DCallback)
self.sub_wheels_cmd = rospy.Subscriber("~wheels_cmd", WheelsCmdStamped,
self.wheelsCmdCallback)
self.pub_theta_dot = rospy.Publisher("~theta_dot_sample",
ThetaDotSample,
queue_size=1)
self.pub_v = rospy.Publisher("~v_sample", Vsample, queue_size=1)
self.pose_prev = None
self.wheels_cmd = None
self.msg_theta_dot = ThetaDotSample()
self.msg_v = Vsample()
rospy.loginfo("[%s] has started", self.node_name)
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 __init__(self):
self.node_name = rospy.get_name()
## Parameters
self.stop_line_max_age = self.setupParameter(
"~stop_line_max_age", 1.0
) # [sec] don't use stop_line_reading msg pairs more than this far apart
self.lane_pose_max_age = self.setupParameter(
"~lane_pose_max_age", 1.0
) # [sec] don't use lane_pose msg pairs more than this far apart
self.wheels_cmd_max_age = self.setupParameter(
"~wheels_cmd_max_age",
2.0) # [sec] throw away wheels_cmd's older than this
## state vars
self.in_lane = False
self.current_wheels_cmd = WheelsCmdStamped()
self.old_stop_line_msg = StopLineReading()
self.old_lane_pose_msg = LanePose()
self.cmd_buffer = deque() # buffer of wheel commands
self.v_sample_msg = Vsample(
) # global variable because different parts of this are set in different callbacks
## publishers and subscribers
self.sub_stop_line_reading = rospy.Subscriber("~stop_line_reading",
StopLineReading,
self.stopLineCB)
self.sub_lane_pose = rospy.Subscriber("~lane_pose", LanePose,
self.lanePoseCB)
self.sub_wheels_cmd_executed = rospy.Subscriber(
"~wheels_cmd_executed", WheelsCmdStamped, self.wheelsCmdCB)
self.pub_v_sample = rospy.Publisher("~v_sample", Vsample, queue_size=1)
self.pub_theta_dot_sample = rospy.Publisher("~theta_dot_sample",
ThetaDotSample,
queue_size=1)
rospy.loginfo('[%s] Initialized' % self.node_name)
def setUp(self):
## Publishers
self.pub_stop_line_reading = rospy.Publisher(
"odometry_training_pairs_node/stop_line_reading",
StopLineReading,
queue_size=1)
self.pub_wheels_cmd_executed = rospy.Publisher(
"odometry_training_pairs_node/wheels_cmd_executed",
WheelsCmdStamped,
queue_size=1)
self.pub_lane_pose = rospy.Publisher(
"odometry_training_pairs_node/lane_pose", LanePose, queue_size=1)
## Subscribers
self.sub_theta_dot_sample = rospy.Subscriber(
"odometry_training_pairs_node/theta_dot_sample", ThetaDotSample,
self.theta_dot_sample_CB)
self.sub_v_sample = rospy.Subscriber(
"odometry_training_pairs_node/v_sample", Vsample, self.v_sample_CB)
## State variables
self.theta_dot_received = ThetaDotSample()
self.v_received = Vsample()
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 __init__(self, *args):
super(TestViconLearningNode, self).__init__(*args)
self.msg_v_received = False
self.msg_theta_dot_received = False
self.msg_theta_dot_sample = ThetaDotSample()
self.msg_v_sample = Vsample()