Ejemplo n.º 1
0
 def toLanePose(self,vicon_pose_msg):
     quat = vicon_pose_msg.pose.orientation
     euler_angles = tf.transformations.euler_from_quaternion([quat.x, quat.y,quat.z,quat.w])
     lane_pose = LanePose()
     lane_pose.d = vicon_pose_msg.pose.position.x - self.x_goal
     lane_pose.phi = euler_angles[2] - self.phi_goal
     lane_pose.header.stamp = vicon_pose_msg.header.stamp
     return lane_pose
Ejemplo n.º 2
0
 def toLanePose(self, vicon_pose_msg):
     quat = vicon_pose_msg.pose.orientation
     euler_angles = tf.transformations.euler_from_quaternion(
         [quat.x, quat.y, quat.z, quat.w])
     lane_pose = LanePose()
     lane_pose.d = vicon_pose_msg.pose.position.x - self.x_goal
     lane_pose.phi = euler_angles[2] - self.phi_goal
     lane_pose.header.stamp = vicon_pose_msg.header.stamp
     return lane_pose
Ejemplo n.º 3
0
    def test_v_dot_simple(self):
        # publish wheel_cmd_executed
        wheels_cmd = WheelsCmdStamped()
        rate = rospy.Rate(10)
        for i in range(10):
            wheels_cmd.header.stamp = rospy.Time.now()
            wheels_cmd.vel_left = 0.5
            wheels_cmd.vel_right = 0.5
            self.pub_wheels_cmd_executed.publish(wheels_cmd)
            rate.sleep()

        # publish LanePose
        lane_pose = LanePose()
        lane_pose.d = 0
        lane_pose.sigma_d = 0
        lane_pose.phi = 0
        lane_pose.sigma_phi = 0
        lane_pose.status = 0
        lane_pose.in_lane = True
        for i in [1, 2]:
            lane_pose.header.stamp = rospy.Time.now()
            self.pub_lane_pose.publish(lane_pose)
            rate.sleep()

        # publish StopLineReading
        stop_line_reading = StopLineReading()
        stop_line_reading.stop_line_detected = True
        stop_line_reading.at_stop_line = False
        stop_line_reading.stop_line_point = Point()
        for x in [0.51, 0.5]:
            stop_line_reading.header.stamp = rospy.Time.now()
            stop_line_reading.stop_line_point.x = x
            self.pub_stop_line_reading.publish(stop_line_reading)
            rate.sleep()

        # Wait for the odometry_training_pairs_node to finish starting up
        timeout = time.time() + 2.0
        while not self.sub_v_sample.get_num_connections() and \
                not rospy.is_shutdown() and not time.time() > timeout:
            rospy.sleep(0.1)

        msg = self.v_received
        # self.assertEqual(hasattr(msg,'d_L'),True)
        self.assertEqual(msg.d_L, 0.5, 'd_L = %s' % msg.d_L)
        self.assertEqual(msg.d_R, 0.5, 'd_R = %s' % msg.d_R)
        self.assertAlmostEqual(msg.dt, 0.1, 2, 'dt = %s' % msg.dt)
        self.assertEqual(
            msg.theta_angle_pose_delta, 0,
            'theta_angle_pose_delta = %s' % msg.theta_angle_pose_delta)
        self.assertAlmostEqual(msg.x_axis_pose_delta, 0.01, 5,
                               'x = %s' % msg.x_axis_pose_delta)
        self.assertEqual(msg.y_axis_pose_delta, 0,
                         'y = %s' % msg.y_axis_pose_delta)
    def test_v_dot_simple(self):
        # publish wheel_cmd_executed
        wheels_cmd = WheelsCmdStamped()
        rate = rospy.Rate(10)
        for i in range(10):
            wheels_cmd.header.stamp = rospy.Time.now()
            wheels_cmd.vel_left = 0.5
            wheels_cmd.vel_right = 0.5
            self.pub_wheels_cmd_executed.publish(wheels_cmd)
            rate.sleep()

        # publish LanePose
        lane_pose = LanePose()
        lane_pose.d = 0
        lane_pose.sigma_d = 0
        lane_pose.phi = 0
        lane_pose.sigma_phi = 0
        lane_pose.status = 0
        lane_pose.in_lane = True
        for i in [1, 2]:
            lane_pose.header.stamp = rospy.Time.now()
            self.pub_lane_pose.publish(lane_pose)
            rate.sleep()

        # publish StopLineReading
        stop_line_reading = StopLineReading()
        stop_line_reading.stop_line_detected = True
        stop_line_reading.at_stop_line = False
        stop_line_reading.stop_line_point = Point()
        for x in [0.51, 0.5]:
            stop_line_reading.header.stamp = rospy.Time.now()
            stop_line_reading.stop_line_point.x = x
            self.pub_stop_line_reading.publish(stop_line_reading)
            rate.sleep()

        # Wait for the odometry_training_pairs_node to finish starting up
        timeout = time.time()+2.0
        while not self.sub_v_sample.get_num_connections() and \
                not rospy.is_shutdown() and not time.time() > timeout:
            rospy.sleep(0.1)

        msg = self.v_received
        # self.assertEqual(hasattr(msg,'d_L'),True)
        self.assertEqual(msg.d_L, 0.5, 'd_L = %s' % msg.d_L)
        self.assertEqual(msg.d_R, 0.5, 'd_R = %s' % msg.d_R)
        self.assertAlmostEqual(msg.dt, 0.1, 2,'dt = %s' % msg.dt)
        self.assertEqual(msg.theta_angle_pose_delta, 0, 'theta_angle_pose_delta = %s' % msg.theta_angle_pose_delta)
        self.assertAlmostEqual(msg.x_axis_pose_delta, 0.01, 5, 'x = %s' % msg.x_axis_pose_delta)
        self.assertEqual(msg.y_axis_pose_delta, 0, 'y = %s' % msg.y_axis_pose_delta)
Ejemplo n.º 5
0
    def processSegments(self, segment_list_msg):
        if not self.active:
            return

        current_time = time.time()
        self.filter.predict(dt=current_time - self.t_last_update,
                            v=self.velocity.v,
                            w=self.velocity.omega)
        self.t_last_update = current_time

        ml = self.filter.update(segment_list_msg.segments)
        if ml is not None:
            ml_img = self.getDistributionImage(ml,
                                               segment_list_msg.header.stamp)
            self.pub_ml_img.publish(ml_img)

        [d_max, phi_max] = self.filter.getEstimate()
        max_val = self.filter.getMax()
        in_lane = bool(max_val > self.filter.min_max)

        lanePose = LanePose()
        lanePose.header.stamp = segment_list_msg.header.stamp
        lanePose.d = d_max
        lanePose.phi = phi_max
        lanePose.in_lane = in_lane
        lanePose.status = lanePose.NORMAL

        # publish the belief image
        belief_img = self.getDistributionImage(self.filter.belief,
                                               segment_list_msg.header.stamp)

        current_time_print = time.time()
        message_time = segment_list_msg.header.stamp.sec + segment_list_msg.header.stamp.nanosec * 1e-9
        delay = current_time_print - message_time
        #print("wheels_cmd timestamp: " + str(segment_list_msg.header.stamp))
        #print("current time: " + str(current_time_print))
        #print("delay: " + str(delay))

        self.pub_lane_pose.publish(lanePose)
        self.pub_belief_img.publish(belief_img)

        # also publishing a separate Bool for the FSM
        in_lane_msg = BoolStamped()
        in_lane_msg.header.stamp = segment_list_msg.header.stamp
        in_lane_msg.data = in_lane
        self.pub_in_lane.publish(in_lane_msg)
Ejemplo n.º 6
0
    def cbProcessSegments(self, segment_list_msg):
        """Callback to process the segments

        Args:
            segment_list_msg (:obj:`SegmentList`): message containing list of processed segments

        """
        # Get actual timestamp for latency measurement
        timestamp_before_processing = rospy.Time.now()

        # Step 1: predict
        current_time = rospy.get_time()
        if self.currentVelocity:
            dt = current_time - self.t_last_update
            self.filter.predict(dt=dt,
                                v=self.currentVelocity.v,
                                w=self.currentVelocity.omega)

        self.t_last_update = current_time

        # Step 2: update
        self.filter.update(segment_list_msg.segments)

        # Step 3: build messages and publish things
        [d_max, phi_max] = self.filter.getEstimate()
        # print "d_max = ", d_max
        # print "phi_max = ", phi_max

        # Getting the highest belief value from the belief matrix
        max_val = self.filter.getMax()
        # Comparing it to a minimum belief threshold to make sure we are certain enough of our estimate
        in_lane = max_val > self.filter.min_max

        # build lane pose message to send
        lanePose = LanePose()
        lanePose.header.stamp = segment_list_msg.header.stamp
        lanePose.d = d_max
        lanePose.phi = phi_max
        lanePose.in_lane = in_lane
        # XXX: is it always NORMAL?
        lanePose.status = lanePose.NORMAL

        self.pub_lane_pose.publish(lanePose)
        self.debugOutput(segment_list_msg, d_max, phi_max,
                         timestamp_before_processing)
Ejemplo n.º 7
0
    def publishEstimate(self, segment_list_msg=None):

        belief = self.filter.getEstimate()

        # build lane pose message to send
        lanePose = LanePose()
        lanePose.header.stamp = self.last_update_stamp
        lanePose.d = belief['mean'][0]
        lanePose.phi = belief['mean'][1]
        lanePose.d_phi_covariance = [
            belief['covariance'][0][0], belief['covariance'][0][1],
            belief['covariance'][1][0], belief['covariance'][1][1]
        ]
        lanePose.in_lane = True
        lanePose.status = lanePose.NORMAL

        self.pub_lane_pose.publish(lanePose)
        if segment_list_msg is not None:
            self.debugOutput(segment_list_msg, lanePose.d, lanePose.phi)
Ejemplo n.º 8
0
    def processSegments(self, segment_list_msg):
        if not self.active:
            return

        # Step 1: predict
        current_time = rospy.get_time()
        self.filter.predict(dt=current_time - self.t_last_update,
                            v=self.velocity.v,
                            w=self.velocity.omega)
        self.t_last_update = current_time

        # Step 2: update
        ml = self.filter.update(segment_list_msg.segments)
        if ml is not None:
            ml_img = self.getDistributionImage(ml,
                                               segment_list_msg.header.stamp)
            self.pub_ml_img.publish(ml_img)

        # Step 3: build messages and publish things
        [d_max, phi_max] = self.filter.getEstimate()
        max_val = self.filter.getMax()
        in_lane = max_val > self.filter.min_max

        # build lane pose message to send
        lanePose = LanePose()
        lanePose.header.stamp = segment_list_msg.header.stamp
        lanePose.d = d_max
        lanePose.phi = phi_max
        lanePose.in_lane = in_lane
        lanePose.status = lanePose.NORMAL

        # publish the belief image
        belief_img = self.getDistributionImage(self.filter.belief,
                                               segment_list_msg.header.stamp)
        self.pub_lane_pose.publish(lanePose)
        self.pub_belief_img.publish(belief_img)

        # also publishing a separate Bool for the FSM
        in_lane_msg = BoolStamped()
        in_lane_msg.header.stamp = segment_list_msg.header.stamp
        in_lane_msg.data = in_lane
        self.pub_in_lane.publish(in_lane_msg)
Ejemplo n.º 9
0
    def cb_intpose(self, int_pose):
        '''
        cb_intpose Function, that subscribes to the incoming PoseStamped (position and orientation of duckiebot) message and
        publishes the LanePose message with the entries d and phi, that can be used for the controller
        :param int_pose: PoseStamped ros message
        :return: None
        '''
        # TODO Change to debug level
        if self.verbose:
            self.log('Received intersection pose.')

        if self.parametersChanged:
            self.log('Parameters changed.', 'info')
            self.refresh_parameters()
            self.parametersChanged = False

        trajectory = self.trajectories[self.trajectory]
        track = trajectory['track']
        tangent_angle = trajectory['tangent_angle']
        curvature = trajectory['curvature']

        # compute distance d and angle phi, same as in lane following
        d, phi, curv = self.relative_pose(int_pose, track, tangent_angle,
                                          curvature)

        # convert to LanePose() message
        lane_pose = LanePose()
        lane_pose.header.stamp = int_pose.header.stamp
        lane_pose.d = d
        lane_pose.phi = phi
        lane_pose.curvature = curv

        # publish lane pose msg
        self.pub_lanepose.publish(lane_pose)

        if self.verbose:
            self.log("distance: " + str(d))
            self.log("angle: " + str(phi))
            if self.track_publishing_skipped_iterations % 10 == 0:
                self.track_publishing_skipped_iterations = 0
                self.publish_track(track, tangent_angle)
            self.track_publishing_skipped_iterations += 1
Ejemplo n.º 10
0
    def processSegments(self, segment_list_msg):
        if not self.active:
            return

        # Step 1: predict
        current_time = rospy.get_time()
        self.filter.predict(dt=current_time - self.t_last_update,
                            v=self.velocity.v,
                            w=self.velocity.omega)
        self.t_last_update = current_time

        # Step 2: update
        self.filter.update(segment_list_msg.segments)

        # Step 3: build messages and publish things
        [d_max, phi_max] = self.filter.getEstimate()
        max_val = self.filter.getMax()
        in_lane = max_val > self.filter.min_max

        # build lane pose message to send
        lanePose = LanePose()
        lanePose.header.stamp = segment_list_msg.header.stamp
        lanePose.d = d_max
        lanePose.phi = phi_max
        lanePose.in_lane = in_lane
        lanePose.status = lanePose.NORMAL

        # publish the belief image
        bridge = CvBridge()
        belief_img = bridge.cv2_to_imgmsg(
            (255 * self.filter.belief).astype('uint8'), "mono8")
        belief_img.header.stamp = segment_list_msg.header.stamp

        self.pub_lane_pose.publish(lanePose)
        self.pub_belief_img.publish(belief_img)

        # also publishing a separate Bool for the FSM
        in_lane_msg = BoolStamped()
        in_lane_msg.header.stamp = segment_list_msg.header.stamp
        in_lane_msg.data = in_lane
        self.pub_in_lane.publish(in_lane_msg)
    def processSegments(self, segment_list_msg):
        if not self.active:
            return
        # Get actual timestamp for latency measurement
        timestamp_now = rospy.Time.now()

        # TODO-TAL double call to param server ... --> see TODO in the readme, not only is it never updated, but it is alwas 0
        # Step 0: get values from server
        if (rospy.get_param('~curvature_res') is not self.curvature_res):
            self.curvature_res = rospy.get_param('~curvature_res')
            self.filter.updateRangeArray(self.curvature_res)

        # Step 1: predict
        current_time = rospy.get_time()
        dt = current_time - self.t_last_update
        v = self.velocity.v
        w = self.velocity.omega

        self.filter.predict(dt=dt, v=v, w=w)
        self.t_last_update = current_time

        # Step 2: update

        self.filter.update(segment_list_msg.segments)

        parking_detected = BoolStamped()
        parking_detected.header.stamp = segment_list_msg.header.stamp
        parking_detected.data = self.filter.parking_detected
        self.pub_parking_detection.publish(parking_detected)

        parking_on = BoolStamped()
        parking_on.header.stamp = segment_list_msg.header.stamp
        parking_on.data = True
        self.pub_parking_on.publish(parking_on)

        if self.active_mode:
            # Step 3: build messages and publish things
            [d_max, phi_max] = self.filter.getEstimate()
            # print "d_max = ", d_max
            # print "phi_max = ", phi_max

            inlier_segments = self.filter.get_inlier_segments(
                segment_list_msg.segments, d_max, phi_max)
            inlier_segments_msg = SegmentList()
            inlier_segments_msg.header = segment_list_msg.header
            inlier_segments_msg.segments = inlier_segments
            self.pub_seglist_filtered.publish(inlier_segments_msg)

            #max_val = self.filter.getMax()
            #in_lane = max_val > self.filter.min_max
            # build lane pose message to send
            lanePose = LanePose()
            lanePose.header.stamp = segment_list_msg.header.stamp
            lanePose.d = d_max[0]
            lanePose.phi = phi_max[0]
            #lanePose.in_lane = in_lane
            # XXX: is it always NORMAL?
            lanePose.status = lanePose.NORMAL

            if self.curvature_res > 0:
                lanePose.curvature = self.filter.getCurvature(
                    d_max[1:], phi_max[1:])

            self.pub_lane_pose.publish(lanePose)

            # TODO-TAL add a debug param to not publish the image !!
            # TODO-TAL also, the CvBridge is re instantiated every time...
            # publish the belief image
            bridge = CvBridge()
            belief_img = bridge.cv2_to_imgmsg(
                np.array(255 * self.filter.beliefArray[0]).astype("uint8"),
                "mono8")
            belief_img.header.stamp = segment_list_msg.header.stamp
            self.pub_belief_img.publish(belief_img)

            # Latency of Estimation including curvature estimation
            estimation_latency_stamp = rospy.Time.now() - timestamp_now
            estimation_latency = estimation_latency_stamp.secs + estimation_latency_stamp.nsecs / 1e9
            self.latencyArray.append(estimation_latency)

            if (len(self.latencyArray) >= 20):
                self.latencyArray.pop(0)

            # print "Latency of segment list: ", segment_latency
            # print("Mean latency of Estimation:................. %s" % np.mean(self.latencyArray))

            # also publishing a separate Bool for the FSM
            in_lane_msg = BoolStamped()
            in_lane_msg.header.stamp = segment_list_msg.header.stamp
            in_lane_msg.data = self.filter.inLane
            if (self.filter.inLane):
                self.pub_in_lane.publish(in_lane_msg)
Ejemplo n.º 12
0
    def processSegments(self, segment_list_msg):
        # Get actual timestamp for latency measurement
        timestamp_now = rospy.Time.now()

        if not self.active:
            return

        # TODO-TAL double call to param server ... --> see TODO in the readme, not only is it never updated, but it is alwas 0
        # Step 0: get values from server
        if (rospy.get_param('~curvature_res') is not self.curvature_res):
            self.curvature_res = rospy.get_param('~curvature_res')
            self.filter.updateRangeArray(self.curvature_res)

        ## #######################################################
        # ########################################################
        # Parameters
        # ########################################################
        lane_width = 0.23  # [m]
        valid_rad = 0.5  # [m]
        lookahead = 0.1  # [m]
        n_test_points = 50
        max_buffer_size = 25

        # ########################################################
        # Process inputs
        # ########################################################
        # Generate an xy list from the data
        white_xy = []
        yellow_xy = []
        for segment in segment_list_msg.segments:
            # White points
            if segment.color == 0:
                white_xy.append([segment.points[0].x, segment.points[0].y])
            # Yellow points
            elif segment.color == 1:
                yellow_xy.append([segment.points[0].x, segment.points[0].y])
            else:
                print("Unrecognized segment color")

        # Turn into numpy arrays
        white_xy = np.array(white_xy)
        yellow_xy = np.array(yellow_xy)

        # ########################################################
        # Process buffer
        # ########################################################
        # Integrate the odometry
        current_time = rospy.get_time()
        dt = current_time - self.t_last_update
        self.t_last_update = current_time

        delta_x = self.velocity.v * dt
        delta_theta = self.velocity.omega * dt
        # Form the displacement
        delta_r = delta_x * np.array(
            [-np.sin(delta_theta), np.cos(delta_theta)])

        # If buffers contains data, propogate it BACKWARD
        if len(self.buffer_white) > 0:
            self.buffer_white = self.buffer_white - delta_r
        if len(self.buffer_yellow) > 0:
            self.buffer_yellow = self.buffer_yellow - delta_r

        # If new observations were received, then append them to the top of the list
        if len(white_xy) > 0:
            if len(self.buffer_white) > 0:
                self.buffer_white = np.vstack((white_xy, self.buffer_white))
            else:
                self.buffer_white = white_xy
        if len(yellow_xy) > 0:
            if len(self.buffer_yellow) > 0:
                self.buffer_yellow = np.vstack((yellow_xy, self.buffer_yellow))
            else:
                self.buffer_yellow = yellow_xy

        # ########################################################
        # Trim training points to within a valid radius
        # ########################################################
        valid_white_xy = []
        valid_yellow_xy = []

        # Select points within rad
        if len(self.buffer_white) > 0:
            tf_valid_white = self.computeRad(self.buffer_white) <= valid_rad
            valid_white_xy = self.buffer_white[tf_valid_white, :]

        if len(self.buffer_yellow) > 0:
            tf_valid_yellow = self.computeRad(self.buffer_yellow) <= valid_rad
            valid_yellow_xy = self.buffer_yellow[tf_valid_yellow, :]

        # ########################################################
        # Fit GPs
        # ########################################################
        # Set up a linspace for prediction
        if len(valid_white_xy) > 0:
            x_pred_white = np.linspace(0, np.minimum(valid_rad, np.amax(valid_white_xy[:, 0])), \
                num=n_test_points+1).reshape(-1, 1)
        else:
            x_pred_white = np.linspace(0, valid_rad,
                                       num=n_test_points + 1).reshape(-1, 1)

        if len(valid_yellow_xy) > 0:
            x_pred_yellow = np.linspace(0, np.minimum(valid_rad, np.amax(valid_yellow_xy[:, 0])), \
                num=n_test_points+1).reshape(-1, 1)
        else:
            x_pred_yellow = np.linspace(0, valid_rad,
                                        num=n_test_points + 1).reshape(-1, 1)

        # Start with the prior
        y_pred_white = -lane_width / 2.0 * np.ones((len(x_pred_white), 1))
        y_pred_yellow = lane_width / 2.0 * np.ones((len(x_pred_yellow), 1))

        t_start = time.time()
        # White GP.  Note that we're fitting y = f(x)
        gpWhite = []
        if len(valid_white_xy) > 2:
            x_train_white = valid_white_xy[:, 0].reshape(-1, 1)
            y_train_white = valid_white_xy[:, 1]
            whiteKernel = C(0.01, (-lane_width, 0)) * RBF(5, (0.3, 0.4))
            # whiteKernel = C(1.0, (1e-3, 1e3)) * RBF(5, (1e-2, 1e2))
            gpWhite = GaussianProcessRegressor(kernel=whiteKernel,
                                               optimizer=None)
            # x = f(y)
            gpWhite.fit(x_train_white, y_train_white)
            # Predict
            y_pred_white = gpWhite.predict(x_pred_white)

        # Yellow GP
        gpYellow = []
        if len(valid_yellow_xy) > 2:
            x_train_yellow = valid_yellow_xy[:, 0].reshape(-1, 1)
            y_train_yellow = valid_yellow_xy[:, 1]
            # yellowKernel = C(lane_width / 2.0, (0, lane_width)) * RBF(5, (0.3,
            # 0.4))
            yellowKernel = C(0.01, (0, lane_width)) * RBF(5, (0.3, 0.4))
            gpYellow = GaussianProcessRegressor(kernel=yellowKernel,
                                                optimizer=None)
            gpYellow.fit(x_train_yellow, y_train_yellow)
            # Predict
            y_pred_yellow = gpYellow.predict(x_pred_yellow)
        t_end = time.time()
        # print("MODEL BUILDING TIME: ", t_end - t_start)

        # Make xy point arrays
        xy_gp_white = np.hstack((x_pred_white, y_pred_white.reshape(-1, 1)))
        xy_gp_yellow = np.hstack((x_pred_yellow, y_pred_yellow.reshape(-1, 1)))

        # Trim predicted values to valid radius
        # Logical conditions
        tf_valid_white = self.computeRad(xy_gp_white) <= valid_rad
        tf_valid_yellow = self.computeRad(xy_gp_yellow) <= valid_rad

        # Select points within rad
        valid_xy_gp_white = xy_gp_white[tf_valid_white, :]
        valid_xy_gp_yellow = xy_gp_yellow[tf_valid_yellow, :]

        # ########################################################
        # Display
        # ########################################################
        self.visualizeCurves(valid_xy_gp_white, valid_xy_gp_yellow)

        # ########################################################
        # Compute d and \phi
        # ########################################################
        # Evaluate each GP at the lookahead distance and average to get d
        # Start with prior
        y_white = -lane_width / 2
        y_yellow = lane_width / 2

        if gpWhite:
            y_white = gpWhite.predict(np.array([lookahead]).reshape(-1, 1))
        if gpYellow:
            y_yellow = gpYellow.predict(np.array([lookahead]).reshape(-1, 1))

        # Take the average, and negate
        disp = np.mean((y_white, y_yellow))
        d = -disp

        # Compute phi from the lookahead distance and d
        L = np.sqrt(d**2 + lookahead**2)
        phi = disp / L

        print("D is: ", d)
        print("phi is: ", phi)

        # ########################################################
        # Publish the lanePose message
        # ########################################################
        lanePose = LanePose()
        lanePose.header.stamp = segment_list_msg.header.stamp
        lanePose.d = d
        lanePose.phi = phi
        lanePose.in_lane = True
        lanePose.status = lanePose.NORMAL
        lanePose.curvature = 0
        self.pub_lane_pose.publish(lanePose)

        # ########################################################
        # Save valid training points to buffer
        # ########################################################
        # Cap the buffer at max_buffer_size
        white_max = np.minimum(len(self.buffer_white), max_buffer_size)
        yellow_max = np.minimum(len(self.buffer_yellow), max_buffer_size)
        self.buffer_white = self.buffer_white[0:white_max - 1]
        self.buffer_yellow = self.buffer_yellow[0:yellow_max - 1]
        # print ("SIZE of white buffer: ", len(self.buffer_white))
        # print ("YELLOW of yellow buffer: ", len(self.buffer_yellow))

        ## #######################################################

        # Step 1: predict
        current_time = rospy.get_time()
        dt = current_time - self.t_last_update
        v = self.velocity.v
        w = self.velocity.omega

        self.filter.predict(dt=dt, v=v, w=w)
        # self.t_last_update = current_time

        # Step 2: update

        self.filter.update(segment_list_msg.segments)

        # Step 3: build messages and publish things
        [d_max, phi_max] = self.filter.getEstimate()
        # print "d_max = ", d_max
        # print "phi_max = ", phi_max

        inlier_segments = self.filter.get_inlier_segments(
            segment_list_msg.segments, d_max, phi_max)
        inlier_segments_msg = SegmentList()
        inlier_segments_msg.header = segment_list_msg.header
        inlier_segments_msg.segments = inlier_segments
        self.pub_seglist_filtered.publish(inlier_segments_msg)

        max_val = self.filter.getMax()
        in_lane = max_val > self.filter.min_max
        # build lane pose message to send
        # lanePose = LanePose()
        # lanePose.header.stamp = segment_list_msg.header.stamp
        # lanePose.d = d_max[0]
        # lanePose.phi = phi_max[0]
        # lanePose.in_lane = in_lane
        # # XXX: is it always NORMAL?
        # lanePose.status = lanePose.NORMAL

        # if self.curvature_res > 0:
        #     lanePose.curvature = self.filter.getCurvature(d_max[1:], phi_max[1:])

        # self.pub_lane_pose.publish(lanePose)

        # TODO-TAL add a debug param to not publish the image !!
        # TODO-TAL also, the CvBridge is re instantiated every time...
        # publish the belief image
        bridge = CvBridge()
        belief_img = bridge.cv2_to_imgmsg(
            np.array(255 * self.filter.beliefArray[0]).astype("uint8"),
            "mono8")
        belief_img.header.stamp = segment_list_msg.header.stamp
        self.pub_belief_img.publish(belief_img)

        # Latency of Estimation including curvature estimation
        estimation_latency_stamp = rospy.Time.now() - timestamp_now
        estimation_latency = estimation_latency_stamp.secs + estimation_latency_stamp.nsecs / 1e9
        self.latencyArray.append(estimation_latency)

        if (len(self.latencyArray) >= 20):
            self.latencyArray.pop(0)

        # print "Latency of segment list: ", segment_latency
        # print("Mean latency of Estimation:................. %s" % np.mean(self.latencyArray))

        # also publishing a separate Bool for the FSM
        in_lane_msg = BoolStamped()
        in_lane_msg.header.stamp = segment_list_msg.header.stamp
        in_lane_msg.data = True  #TODO-TAL change with in_lane. Is this messqge useful since it is alwas true ?
        self.pub_in_lane.publish(in_lane_msg)
Ejemplo n.º 13
0
    def processSegments(self, segment_list_msg):
        # Get actual timestamp for latency measurement
        timestamp_now = rospy.Time.now()

        if not self.active:
            return

        # Step 0: get values from server
        if (rospy.get_param('~curvature_res') is not self.curvature_res):
            self.curvature_res = rospy.get_param('~curvature_res')
            self.filter.updateRangeArray(self.curvature_res)

        # Step 1: predict
        current_time = rospy.get_time()
        dt = current_time - self.t_last_update
        v = self.velocity.v
        w = self.velocity.omega

        self.filter.predict(dt=dt, v=v, w=w)
        self.t_last_update = current_time

        # Step 2: update

        self.filter.update(segment_list_msg.segments)

        # Step 3: build messages and publish things
        [d_max, phi_max] = self.filter.getEstimate()
        # print "d_max = ", d_max
        # print "phi_max = ", phi_max

        max_val = self.filter.getMax()
        in_lane = max_val > self.filter.min_max
        # build lane pose message to send
        lanePose = LanePose()
        lanePose.header.stamp = segment_list_msg.header.stamp
        lanePose.d = d_max[0]
        lanePose.phi = phi_max[0]
        lanePose.in_lane = in_lane
        # XXX: is it always NORMAL?
        lanePose.status = lanePose.NORMAL

        if self.curvature_res > 0:
            lanePose.curvature = self.filter.getCurvature(
                d_max[1:], phi_max[1:])

        # publish the belief image
        belief_img = self.getDistributionImage(self.filter.belief,
                                               segment_list_msg.header.stamp)
        self.pub_lane_pose.publish(lanePose)
        self.pub_belief_img.publish(belief_img)

        def getDistributionImage(self, mat, stamp):
            bridge = CvBridge()
            img = bridge.cv2_to_imgmsg((255 * mat).astype('uint8'), "mono8")
            img.header.stamp = stamp
            return img

        # Latency of Estimation including curvature estimation
        estimation_latency_stamp = rospy.Time.now() - timestamp_now
        estimation_latency = estimation_latency_stamp.secs + estimation_latency_stamp.nsecs / 1e9
        self.latencyArray.append(estimation_latency)

        if (len(self.latencyArray) >= 20):
            self.latencyArray.pop(0)

        # print "Latency of segment list: ", segment_latency
        # print("Mean latency of Estimation:................. %s" % np.mean(self.latencyArray))

        # TODO (1): see above, method does not exist
        #self.pub_belief_img.publish(belief_img)

        # also publishing a separate Bool for the FSM
        in_lane_msg = BoolStamped()
        in_lane_msg.header.stamp = segment_list_msg.header.stamp
        in_lane_msg.data = True  #TODO change with in_lane
        self.pub_in_lane.publish(in_lane_msg)
    def processSegments(self, segment_list_msg):
        # Get actual timestamp for latency measurement
        timestamp_now = rospy.Time.now()

        if not self.active:
            return

        # TODO-TAL double call to param server ... --> see TODO in the readme, not only is it never updated, but it is alwas 0
        # Step 0: get values from server
        if (rospy.get_param('~curvature_res') is not self.curvature_res):
            self.curvature_res = rospy.get_param('~curvature_res')
            self.filter.updateRangeArray(self.curvature_res)

        # Step 1: predict
        current_time = rospy.get_time()
        dt = current_time - self.t_last_update
        v = self.velocity.v
        w = self.velocity.omega

        # ==== HERE THE UPPER BOUNDS ========
        ub_d = dt * self.omega_max * self.gain * 1000
        ub_phi = dt * self.v_ref * self.gain * 1000

        self.filter.predict(dt=dt, v=v, w=w)
        self.t_last_update = current_time

        # Step 2: update

        self.filter.update(segment_list_msg.segments, self.lane_offset)

        # Step 3: build messages and publish things
        [d_max, phi_max] = self.filter.getEstimate()

        inlier_segments = self.filter.get_inlier_segments(
            segment_list_msg.segments, d_max, phi_max)
        inlier_segments_msg = SegmentList()
        inlier_segments_msg.header = segment_list_msg.header
        inlier_segments_msg.segments = inlier_segments
        self.pub_seglist_filtered.publish(inlier_segments_msg)

        max_val = self.filter.getMax()
        in_lane = max_val > self.filter.min_max
        # build lane pose message to send
        lanePose = LanePose()
        lanePose.header.stamp = segment_list_msg.header.stamp

        lanePose.d = d_max[0]
        if not self.last_d:
            self.last_d = lanePose.d
        if lanePose.d - self.last_d > ub_d:
            lanePose.d = self.last_d + ub_d
            rospy.loginfo("d changed too much")
        if lanePose.d - self.last_d < -ub_d:
            lanePose.d = self.last_d - ub_d
            rospy.loginfo("d changed too much")

        lanePose.phi = phi_max[0]
        if not self.last_phi:
            self.last_phi = lanePose.phi
        if lanePose.phi - self.last_phi > ub_phi:
            lanePose.phi = self.last_phi + ub_phi
            rospy.loginfo("phi changed too much")
        if lanePose.phi - self.last_phi < -ub_phi:
            lanePose.phi = self.last_phi - ub_phi
            rospy.loginfo("phi changed too much")

        lanePose.in_lane = in_lane
        # XXX: is it always NORMAL?
        lanePose.status = lanePose.NORMAL

        if self.curvature_res > 0:
            lanePose.curvature = self.filter.getCurvature(
                d_max[1:], phi_max[1:])

        self.pub_lane_pose.publish(lanePose)

        # Save for next iteration       <=========
        self.last_d = lanePose.d
        self.last_phi = lanePose.phi

        # publish the belief image
        bridge = CvBridge()
        belief_img = bridge.cv2_to_imgmsg(
            np.array(255 * self.filter.beliefArray[0]).astype("uint8"),
            "mono8")
        belief_img.header.stamp = segment_list_msg.header.stamp
        self.pub_belief_img.publish(belief_img)

        # Latency of Estimation including curvature estimation
        estimation_latency_stamp = rospy.Time.now() - timestamp_now
        estimation_latency = estimation_latency_stamp.secs + estimation_latency_stamp.nsecs / 1e9
        self.latencyArray.append(estimation_latency)

        if (len(self.latencyArray) >= 20):
            self.latencyArray.pop(0)

        # also publishing a separate Bool for the FSM
        in_lane_msg = BoolStamped()
        in_lane_msg.header.stamp = segment_list_msg.header.stamp
        in_lane_msg.data = True  #TODO-TAL change with in_lane. Is this messqge useful since it is alwas true ?
        self.pub_in_lane.publish(in_lane_msg)
    def processSegments(self,segment_list_msg):
        # Get actual timestamp for latency measurement
        timestamp_now = rospy.Time.now()

        if not self.active:
            return

        # Step 1: predict
        current_time = rospy.get_time()
        dt = current_time - self.t_last_update
        v = self.velocity.v
        w = self.velocity.omega

        self.filter.predict(dt=dt, v=v, w=w)
        self.t_last_update = current_time

        # Step 2: update
        self.filter.update(segment_list_msg.segments)

        # Step 3: build messages and publish things
        [d_max, phi_max] = self.filter.getEstimate()

        ## Inlier segments
        inlier_segments = self.filter.get_inlier_segments(segment_list_msg.segments, d_max, phi_max)
        inlier_segments_msg = SegmentList()
        inlier_segments_msg.header = segment_list_msg.header
        inlier_segments_msg.segments = inlier_segments
        self.pub_seglist_filtered.publish(inlier_segments_msg)

        ## Lane pose
        in_lane = self.filter.isInLane()
        lanePose = LanePose()
        lanePose.header.stamp = segment_list_msg.header.stamp
        lanePose.d = d_max
        lanePose.phi = phi_max
        lanePose.in_lane = in_lane
        lanePose.status = lanePose.NORMAL

        self.pub_lane_pose.publish(lanePose)

        ## Belief image
        bridge = CvBridge()
        if self.mode == 'histogram':
            belief_img = bridge.cv2_to_imgmsg(np.array(255 * self.filter.beliefArray).astype("uint8"), "mono8")
        elif self.mode == 'particle':
            belief_img = bridge.cv2_to_imgmsg(np.array(255 * self.filter.getBeliefArray()).astype("uint8"), "mono8")
        belief_img.header.stamp = segment_list_msg.header.stamp
        self.pub_belief_img.publish(belief_img)
        
        ## Latency of Estimation
        estimation_latency_stamp = rospy.Time.now() - timestamp_now
        estimation_latency = estimation_latency_stamp.secs + estimation_latency_stamp.nsecs/1e9
        self.latencyArray.append(estimation_latency)

        if (len(self.latencyArray) >= 20):
            self.latencyArray.pop(0)

        # Separate Bool for the FSM
        in_lane_msg = BoolStamped()
        in_lane_msg.header.stamp = segment_list_msg.header.stamp
        in_lane_msg.data = True #TODO-TAL change with in_lane. Is this messqge useful since it is alwas true ?
        self.pub_in_lane.publish(in_lane_msg)
    def updateEstimate(self, segListMsg):

        self.testCount = self.testCount + 1  # Just some counter, gives an idea of the speed.

        # ######## EXTRACT ######### data from segments back into separated arrays.
        whitePointsArray, yellowPointsArray = self.segList2Array(segListMsg)
        nWhite = whitePointsArray.shape[0]
        nYellow = yellowPointsArray.shape[0]

        # ###### REMOVE WHITE POINTS OF LEFT LANE ######
        if nYellow >= 2:  # Need minimum of two points to define a line.
            self.ransacY.fit(
                np.reshape(yellowPointsArray[:, 0], (-1, 1)),
                np.array(np.reshape(yellowPointsArray[:, 1], (-1, 1))))
            zYellowOnly = np.array([
                self.ransacY.estimator_.coef_,
                self.ransacY.estimator_.intercept_
            ])
            whitePointsArray = self.removeLeftLane(zYellowOnly,
                                                   whitePointsArray)
        nWhite = whitePointsArray.shape[0]
        nYellow = yellowPointsArray.shape[0]

        # ###### FIT TWO LANES ########
        if nWhite >= 2 and nYellow >= 2:
            z = self.fitTwoLanes(whitePointsArray, yellowPointsArray)
            #self.zTwoLane = z
            m = z[0]
            c = z[1]
            zWhite = np.array([m, c])
            zYellow = np.array([m, c + self.laneWidth * np.sqrt(m**2 + 1)])
        elif nYellow >= 2:
            zWhite = zYellowOnly
        elif nWhite >= 2:
            self.ransacW.fit(np.reshape(whitePointsArray[:, 0], (-1, 1)),
                             np.reshape(whitePointsArray[:, 1], (-1, 1)))
            zWhite = np.array([
                self.ransacW.estimator_.coef_,
                self.ransacW.estimator_.intercept_
            ])

        if nWhite >= 2 or nYellow >= 2:
            # ######### Calculate d and phi from geometry ##########
            m = np.asscalar(zWhite[0])
            c = np.asscalar(zWhite[1])
            phiWhite = np.arcsin(-m / (np.sqrt(1 + m**2)))
            r_wz_b = np.array([
                [-c * m / (1 + m**2)], [c - (c * m**2 / (1 + m**2))]
            ])  # Position vector to nearest point on the line (normal to line)
            C_wb = np.array([[np.cos(phiWhite), -np.sin(phiWhite)],
                             [np.sin(phiWhite),
                              np.cos(phiWhite)]])
            r_wz_w = np.matmul(
                C_wb, r_wz_b
            )  # Distance TO white lane from duckiebot. This should be negative
            if nWhite < 2 and nYellow >= 2:  # Then the line we have is actually yellow
                r_dw_w = np.array([[0], [-self.laneWidth / 2]])
            else:
                if r_wz_w[1] >= 0:  # Then white line is on our left.
                    # This can occur in two situations
                    # 1) We are in the other lane
                    # 2) We are turning a right corner.
                    # In either case, it is actually the OTHER white line
                    r_dw_w = np.array([[0], [-1.5 * self.laneWidth]])
                else:
                    r_dw_w = np.array([[0], [self.laneWidth / 2]])
            r_zd_w = -r_wz_w - r_dw_w
            distWhite = r_zd_w[1]  # Distance TO duckiebot from desired point.

            phi = phiWhite
            d = distWhite
        else:
            d = 0
            phi = 0

        # ######### VISUALIZE ###########
        if nWhite >= 2 and nYellow >= 2:
            pa1 = self.line2PointArray(zWhite, 0, 1)
            pa2 = self.line2PointArray(zYellow, 0, 1)
            self.visualizeCurves(pa1, pa2)
        elif nWhite >= 2 or nYellow >= 2:
            pa1 = self.line2PointArray(zWhite, 0, 1)
            self.visualizeCurves(pa1)

        # ###### PUBLISH LANE POSE #######
        lanePose = LanePose()
        lanePose.header.stamp = segListMsg.header.stamp

        # Complimentary filter lol
        lanePose.d = 0.8 * float(d) + 0.2 * float(self.d_baseline)
        lanePose.phi = 0.8 * float(phi) + 0.2 * float(self.phi_baseline)
        lanePose.in_lane = True
        lanePose.status = lanePose.NORMAL

        self.correct(lanePose)  # Kalman Filter Modifications
        lanePose.d = np.asscalar(self.x_k[0][0])
        lanePose.phi = np.asscalar(self.x_k[1][0])
        self.pub_lane_pose.publish(lanePose)
    def MainLoop(self):

        if not self.active:
            return
        rate = rospy.Rate(self.rate)
        while not rospy.is_shutdown():
            # run state machine

            if self.state == self.state_dict['IDLE']:
                # waiting for FSMState to tell us that Duckiebot is at an intersection (see FSMCallback)
                pass

            elif self.state == self.state_dict['INITIALIZING_LOCALIZATION']:
                if self.InitializeLocalization():
                    self.state = self.state_dict['INITIALIZING_PATH']
                    rospy.loginfo(
                        "[%s] Initialized intersection localization, initializing path."
                        % (self.node_name))

            elif self.state == self.state_dict['INITIALIZING_PATH']:
                if self.InitializePath():
                    self.state = self.state_dict['TRAVERSING']
                    self.s = 0.0
                    self.traversing_start = rospy.Time.now()
                    rospy.loginfo(
                        "[%s] Initialized path, waiting for go signal." %
                        (self.node_name))
                else:
                    rospy.loginfo(
                        "[%s] Could not initialize path. Trying again." %
                        (self.node_name))

            elif self.state == self.state_dict['TRAVERSING']:
                '''open-loop'''
                if self.open_loop:
                    msg_cmds = Twist2DStamped()
                    msg_cmds.header.stamp = rospy.Time.now()

                    if 1.0 < (rospy.Time.now() -
                              self.traversing_start).to_sec() and self.go:

                        pos, vel = self.pathPlanner.EvaluatePath(self.s)
                        dt = 0.01
                        _, vel2 = self.pathPlanner.EvaluatePath(self.s + dt)
                        self.alpha = self.v / np.linalg.norm(vel)

                        dir = vel / np.linalg.norm(vel)
                        dir2 = vel2 / np.linalg.norm(vel2)
                        theta = np.arctan2(dir[1], dir[0])
                        theta2 = np.arctan2(dir2[1], dir2[0])
                        omega = (theta2 - theta) / dt

                        msg_cmds.v = self.v
                        msg_cmds.omega = self.alpha * omega

                        if (msg_cmds.v -
                                0.5 * math.fabs(msg_cmds.omega) * 0.1) < 0.065:
                            msg_cmds.v = 0.065 + 0.5 * math.fabs(
                                msg_cmds.omega) * 0.1
                            self.alpha = self.alpha * msg_cmds.v / self.v

                        msg_cmds.v = msg_cmds.v * self.v_scale
                        msg_cmds.omega = msg_cmds.omega * self.omega_scale

                        self.s = self.s + self.alpha * (rospy.Time.now(
                        ) - self.traversing_last_time).to_sec()

                        if (self.s > 0.99):
                            msg_cmds.v = self.v
                            msg_cmds.omega = 0.0

                            self.state = self.state_dict['DONE']
                            self.done_time = rospy.Time.now()

                    else:
                        msg_cmds.v = 0.0
                        msg_cmds.omega = 0.0

                    self.traversing_last_time = rospy.Time.now()
                    self.pub_cmds.publish(msg_cmds)

                else:
                    '''closed-loop'''
                    msg_lane_pose = LanePose()
                    msg_lane_pose.header.stamp = rospy.Time.now()

                    if 1.0 < (rospy.Time.now() -
                              self.traversing_start).to_sec() and self.go:
                        pose, _ = self.poseEstimator.PredictState(
                            msg_lane_pose.header.stamp)
                        dist, theta, curvature, self.s = self.pathPlanner.ComputeLaneError(
                            pose, self.s)
                        #rospy.loginfo("the s is: "+str(self.s))

                        if (self.s > self.s_max):

                            msg_lane_pose.v_ref = self.v
                            msg_lane_pose.d = 0.0
                            msg_lane_pose.d_ref = 0.0
                            msg_lane_pose.phi = 0.0
                            msg_lane_pose.curvature_ref = 0.0
                            self.state = self.state_dict['DONE']
                            self.done_time = rospy.Time.now()

                        else:
                            msg_lane_pose.v_ref = self.v
                            msg_lane_pose.d = dist
                            msg_lane_pose.d_ref = 0.0
                            msg_lane_pose.phi = theta
                            msg_lane_pose.curvature_ref = curvature

                    else:
                        msg_lane_pose.v_ref = 0.0
                        msg_lane_pose.d = 0.0
                        msg_lane_pose.d_ref = 0.0
                        msg_lane_pose.phi = 0.0
                        msg_lane_pose.curvature_ref = 0.0

                    self.pub_lane_pose.publish(msg_lane_pose)

            elif self.state == self.state_dict['DONE']:

                # switch back to lane following if in lane
                if self.in_lane and (rospy.Time.now() - self.in_lane_time
                                     ).to_sec() < self.in_lane_timeout:
                    msg_done = BoolStamped()
                    msg_done.header.stamp = rospy.Time.now()
                    msg_done.data = True
                    self.pub_done.publish(msg_done)
                    self.state = self.state_dict['IDLE']

                # go straight for 2.0 secs if not in lane yet. After 2 secs stop.
                else:
                    '''open-loop'''
                    if self.open_loop:
                        if (rospy.Time.now() - self.done_time
                            ).to_sec() < self.in_lane_wait_time:
                            msg_cmds = Twist2DStamped()
                            msg_cmds.header.stamp = rospy.Time.now()
                            msg_cmds.v = self.v * self.v_scale
                            msg_cmds.omega = 0.0 * self.omega_scale
                            self.pub_cmds.publish(msg_cmds)
                        else:
                            msg_cmds = Twist2DStamped()
                            msg_cmds.header.stamp = rospy.Time.now()
                            msg_cmds.v = 0.0 * self.v_scale
                            msg_cmds.omega = 0.0 * self.omega_scale
                            self.pub_cmds.publish(msg_cmds)
                            rospy.loginfo(
                                "[%s] Could not find lane. Stopping now." %
                                (self.node_name))
                            self.state = self.state_dict['ERROR']

                    else:
                        '''closed-loop'''
                        if (rospy.Time.now() - self.done_time
                            ).to_sec() < self.in_lane_wait_time:
                            msg_lane_pose = LanePose()
                            msg_lane_pose.header.stamp = rospy.Time.now()
                            msg_lane_pose.v_ref = self.v
                            msg_lane_pose.d = 0.0
                            msg_lane_pose.d_ref = 0.0
                            msg_lane_pose.phi = 0.0
                            msg_lane_pose.curvature_ref = 0.0
                            self.pub_lane_pose.publish(msg_lane_pose)
                        else:
                            msg_lane_pose = LanePose()
                            msg_lane_pose.header.stamp = rospy.Time.now()
                            msg_lane_pose.v_ref = 0.0
                            msg_lane_pose.d = 0.0
                            msg_lane_pose.d_ref = 0.0
                            msg_lane_pose.phi = 0.0
                            msg_lane_pose.curvature_ref = 0.0
                            self.pub_lane_pose.publish(msg_lane_pose)
                            rospy.loginfo(
                                "[%s] Could not find lane. Stopping now." %
                                (self.node_name))
                            self.state = self.state_dict['ERROR']

            else:
                rospy.loginfo("[%s] Something went wrong." % (self.node_name))

            rate.sleep()
Ejemplo n.º 18
0
    def processSegments(self, segment_list_msg):
        if not self.active:
            return

        # Step 1: predict
        current_time = rospy.get_time()
        dt = current_time - self.t_last_update
        v = self.velocity.v
        w = self.velocity.omega

        self.filter.predict(dt=dt, v=v, w=w)
        self.t_last_update = current_time

        # Step 2: update

        self.filter.update(segment_list_msg.segments)

        # Step 3: build messages and publish things
        [d_max, phi_max] = self.filter.getEstimateList()
        #print "d_max = ", d_max
        #print "phi_max = ", phi_max
        #        sum_phi_l = np.sum(phi_max[1:self.filter.num_belief])
        #        sum_d_l = np.sum(d_max[1:self.filter.num_belief])
        #        av_phi_l = np.average(phi_max[1:self.filter.num_belief])
        #        av_d_l = np.average(d_max[1:self.filter.num_belief])

        max_val = self.filter.getMax()
        in_lane = max_val > self.filter.min_max

        #if (sum_phi_l<-1.6 and av_d_l>0.05):
        #    print "I see a left curve"
        #elif (sum_phi_l>1.6 and av_d_l <-0.05):
        #    print "I see a right curve"
        #else:
        #    print "I am on a straight line"

        delta_dmax = np.median(d_max[1:])  # - d_max[0]
        delta_phimax = np.median(phi_max[1:])  #- phi_max[0]

        if len(self.d_median) >= 5:
            self.d_median.pop(0)
            self.phi_median.pop(0)
        self.d_median.append(delta_dmax)
        self.phi_median.append(delta_phimax)

        # build lane pose message to send
        lanePose = LanePose()
        lanePose.header.stamp = segment_list_msg.header.stamp
        lanePose.d = d_max[0]
        lanePose.phi = phi_max[0]
        lanePose.in_lane = in_lane
        # XXX: is it always NORMAL?
        lanePose.status = lanePose.NORMAL

        #print "Delta dmax", delta_dmax
        #print "Delta phimax", delta_phimax

        CURVATURE_LEFT = 0.025
        CURVATURE_RIGHT = -0.054
        CURVATURE_STRAIGHT = 0

        if np.median(self.phi_median) < -0.3 and np.median(
                self.d_median) > 0.05:
            print "left curve"
            lanePose.curvature = CURVATURE_LEFT
        elif np.median(self.phi_median) > 0.2 and np.median(
                self.d_median) < -0.02:
            print "right curve"
            lanePose.curvature = CURVATURE_RIGHT
        else:
            print "straight line"
            lanePose.curvature = CURVATURE_STRAIGHT

        # publish the belief image
        belief_img = self.getDistributionImage(self.filter.belief,
                                               segment_list_msg.header.stamp)
        self.pub_lane_pose.publish(lanePose)
        self.pub_belief_img.publish(belief_img)

        # also publishing a separate Bool for the FSM
        in_lane_msg = BoolStamped()
        in_lane_msg.header.stamp = segment_list_msg.header.stamp
        in_lane_msg.data = in_lane
        self.pub_in_lane.publish(in_lane_msg)