def lanePoseCB(self, lane_pose_msg):
        self.lane_pose = lane_pose_msg
        self.in_lane = lane_pose_msg.in_lane
        # only execute if in_lane and if the old message exists and is not too old
        if (
            self.in_lane
            and hasattr(self.old_lane_pose_msg, "header")
            and (lane_pose_msg.header.stamp - self.old_lane_pose_msg.header.stamp).to_sec() <= self.lane_pose_max_age
        ):
            theta_dot_sample_msg = ThetaDotSample()
            (d_L, d_R) = self.findMatchingDuties(lane_pose_msg.header.stamp)
            theta_dot_sample_msg.d_L = d_L
            theta_dot_sample_msg.d_R = d_R
            # theta_dot_sample_msg.dt = (lane_pose_msg.header.stamp.secs + lane_pose_msg.header.stamp.nsecs/1e9) - (self.old_lane_pose_msg.header.stamp.secs + self.old_lane_pose_msg.header.stamp.nsecs/1e9)
            theta_dot_sample_msg.dt = (lane_pose_msg.header.stamp - self.old_lane_pose_msg.header.stamp).to_sec()
            # rospy.loginfo("theta_dot_sample_msg.dt = %s"%theta_dot_sample_msg.dt)
            # new - old since measure robot angle
            theta_dot_sample_msg.theta_angle_pose_delta = lane_pose_msg.phi - self.old_lane_pose_msg.phi
            # is actually (delta d)*cos(phi), but is probably pretty noisy since we get error from both d and phi
            # theta_dot_sample_msg.x_axis_pose_delta = (lane_pose_msg.d - self.old_lane_pose_msg.d)*cos(lane_pose_msg.phi)
            # theta_dot_sample_msg.y_axis_pose_delta = 0
            self.pub_theta_dot_sample.publish(theta_dot_sample_msg)

            self.v_sample_msg.theta_angle_pose_delta = theta_dot_sample_msg.theta_angle_pose_delta
            # may need to swap signs on this one
            self.v_sample_msg.y_axis_pose_delta = lane_pose_msg.d - self.old_lane_pose_msg.d

        self.old_lane_pose_msg = lane_pose_msg
Пример #2
0
    def lanePoseCB(self, lane_pose_msg):
        self.lane_pose = lane_pose_msg
        self.in_lane = lane_pose_msg.in_lane
        # only execute if in_lane and if the old message exists and is not too old
        if self.in_lane and hasattr(
                self.old_lane_pose_msg,
                'header') and (lane_pose_msg.header.stamp -
                               self.old_lane_pose_msg.header.stamp
                               ).to_sec() <= self.lane_pose_max_age:
            theta_dot_sample_msg = ThetaDotSample()
            (d_L, d_R) = self.findMatchingDuties(lane_pose_msg.header.stamp)
            theta_dot_sample_msg.d_L = d_L
            theta_dot_sample_msg.d_R = d_R
            # theta_dot_sample_msg.dt = (lane_pose_msg.header.stamp.secs + lane_pose_msg.header.stamp.nsecs/1e9) - (self.old_lane_pose_msg.header.stamp.secs + self.old_lane_pose_msg.header.stamp.nsecs/1e9)
            theta_dot_sample_msg.dt = (
                lane_pose_msg.header.stamp -
                self.old_lane_pose_msg.header.stamp).to_sec()
            # rospy.loginfo("theta_dot_sample_msg.dt = %s"%theta_dot_sample_msg.dt)
            #new - old since measure robot angle
            theta_dot_sample_msg.theta_angle_pose_delta = lane_pose_msg.phi - self.old_lane_pose_msg.phi
            #is actually (delta d)*cos(phi), but is probably pretty noisy since we get error from both d and phi
            #theta_dot_sample_msg.x_axis_pose_delta = (lane_pose_msg.d - self.old_lane_pose_msg.d)*cos(lane_pose_msg.phi)
            #theta_dot_sample_msg.y_axis_pose_delta = 0
            self.pub_theta_dot_sample.publish(theta_dot_sample_msg)

            self.v_sample_msg.theta_angle_pose_delta = theta_dot_sample_msg.theta_angle_pose_delta
            #may need to swap signs on this one
            self.v_sample_msg.y_axis_pose_delta = lane_pose_msg.d - self.old_lane_pose_msg.d

        self.old_lane_pose_msg = lane_pose_msg
    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)
Пример #6
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]