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
Beispiel #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
    def __init__(self):
        # Save the name of the node
        self.node_name = rospy.get_name()
        self.mode = None
        self.turn_type = -1
        self.in_lane = False
        self.lane_pose = LanePose()
        self.stop_line_reading = StopLineReading()

        self.trajectory_reparam = rospy.get_param("~trajectory_reparam",1)
        self.pub_cmd = rospy.Publisher("~car_cmd",Twist2DStamped,queue_size=1)
        self.pub_done = rospy.Publisher("~intersection_done",BoolStamped,queue_size=1)

        # Construct maneuvers
        self.maneuvers = dict()

        self.maneuvers[0] = self.getManeuver("turn_left")
        self.maneuvers[1] = self.getManeuver("turn_forward")
        self.maneuvers[2] = self.getManeuver("turn_right")
        self.maneuvers[3] = self.getManeuver("turn_back")
        # self.maneuvers[-1] = self.getManeuver("turn_stop")

        self.srv_turn_left = rospy.Service("~turn_left", Empty, self.cbSrvLeft)
        self.srv_turn_right = rospy.Service("~turn_right", Empty, self.cbSrvRight)
        self.srv_turn_forward = rospy.Service("~turn_forward", Empty, self.cbSrvForward)
        self.srv_turn_back = rospy.Service("~turn_back", Empty, self.cbSrvBack)
        self.rate = rospy.Rate(30)

        # Subscribers
        self.sub_in_lane = rospy.Subscriber("~in_lane", BoolStamped, self.cbInLane, queue_size=1)
        self.sub_turn_type = rospy.Subscriber("~turn_type", Int16, self.cbTurnType, queue_size=1)
        self.sub_mode = rospy.Subscriber("~mode", FSMState, self.cbFSMState, queue_size=1)
        self.sub_lane_pose = rospy.Subscriber("~lane_pose", LanePose, self.cbLanePose, queue_size=1)
        self.sub_stop_line = rospy.Subscriber("~stop_line_reading", StopLineReading, self.cbStopLine, queue_size=1)
Beispiel #4
0
    def __init__(self):
        self.node_name = rospy.get_name()

        self.prev_time = None

        # Publication
        self.pub_car_cmd = rospy.Publisher("~car_cmd", Twist2DStamped, queue_size=1)

        # Subscriptions
        self.sub_lane_reading = rospy.Subscriber("~lane_pose", LanePose, self.updatePose, queue_size=1)
        self.pose_msg = LanePose()

        # Setup PID gains
        self.k_P_phi = -3.5
        self.k_P_d = -2.5

        self.k_I_d = 0.5
        self.k_I_phi = 0.1

        self.k_D_d = 0.7
        self.k_D_phi = 0.4

        # Initialization
        self.prev_d_err = 0
        self.prev_phi_err = 0
        self.v_ref = 0.5
        self.v_max = 1

        self.d_offset = 0
        self.d_integral_top_cutoff = 0.3
        self.d_integral_bottom_cutoff = -0.3
        self.phi_integral_top_cutoff = 1.0
        self.phi_integral_bottom_cutoff = -1.0
Beispiel #5
0
    def __init__(self, node_name):
        # Initialize the DTROS parent class
        super(StopLineFilterNode, self).__init__(node_name=node_name, node_type=NodeType.PERCEPTION)

        # Initialize the parameters
        self.stop_distance = DTParam("~stop_distance", param_type=ParamType.FLOAT)
        self.min_segs = DTParam("~min_segs", param_type=ParamType.INT)
        self.off_time = DTParam("~off_time", param_type=ParamType.FLOAT)
        self.max_y = DTParam("~max_y", param_type=ParamType.FLOAT)

        ## params
        # self.stop_distance = self.setupParam("~stop_distance", 0.22) # distance from the stop line that we should stop
        # self.min_segs      = self.setupParam("~min_segs", 2) # minimum number of red segments that we should detect to estimate a stop
        # self.off_time      = self.setupParam("~off_time", 2)
        # self.max_y         = self.setupParam("~max_y ", 0.2) # If y value of detected red line is smaller than max_y we will not set at_stop_line true.

        ## state vars
        self.lane_pose = LanePose()

        self.state = "JOYSTICK_CONTROL"
        self.sleep = False

        ## publishers and subscribers
        self.sub_segs = rospy.Subscriber("~segment_list", SegmentList, self.cb_segments)
        self.sub_lane = rospy.Subscriber("~lane_pose", LanePose, self.cb_lane_pose)
        self.sub_mode = rospy.Subscriber("fsm_node/mode", FSMState, self.cb_state_change)
        self.pub_stop_line_reading = rospy.Publisher("~stop_line_reading", StopLineReading, queue_size=1)
        self.pub_at_stop_line = rospy.Publisher("~at_stop_line", BoolStamped, queue_size=1)
    def __init__(self):
        self.node_name = "Stop Line Filter"
        self.active = True
        ## state vars
        self.lane_pose = LanePose()

        ## params
        self.stop_distance = self.setupParam(
            "~stop_distance",
            0.2)  # distance from the stop line that we should stop
        self.min_segs = self.setupParam(
            "~min_segs", 2
        )  # minimum number of red segments that we should detect to estimate a stop
        self.off_time = self.setupParam("~off_time", 2)
        self.lanewidth = 0  # updated continuously below

        self.state = "JOYSTICK_CONTROL"
        self.sleep = False
        ## publishers and subscribers
        self.sub_segs = rospy.Subscriber("~segment_list", SegmentList,
                                         self.processSegments)
        self.sub_lane = rospy.Subscriber("~lane_pose", LanePose,
                                         self.processLanePose)
        self.sub_mode = rospy.Subscriber("fsm_node/mode", FSMState,
                                         self.processStateChange)
        self.pub_stop_line_reading = rospy.Publisher("~stop_line_reading",
                                                     StopLineReading,
                                                     queue_size=1)
        self.pub_at_stop_line = rospy.Publisher("~at_stop_line",
                                                BoolStamped,
                                                queue_size=1)

        self.params_update = rospy.Timer(rospy.Duration.from_sec(1.0),
                                         self.updateParams)
 def __init__(self, outputFile):
     f = open(outputFile, 'wt')
     self.writer = csv.writer(f)
     self.oldLP = LanePose()
     self.z = 0.0
     self.tiles_crossed = 0
     rospy.Subscriber('/teamgrapes/lane_filter_node/lane_pose', LanePose,
                      self.estimatePose)
Beispiel #8
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 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
Beispiel #10
0
 def obstacleCallback(self, obstacle_poses):
     amount_obstacles = len(obstacle_poses.poses)
     # rospy.loginfo('Number of obstacles: %d', len(obstacle_poses.poses))
     amount_obstacles_on_track = 0
     obstacle_poses_on_track = PoseArray()
     obstacle_poses_on_track.header = obstacle_poses.header
     avoidance_active = BoolStamped()
     avoidance_active.data = False
     target = LanePose()
     # target.header.frame_id = self.robot_name
     target.v_ref = 10  # max speed high, current top 0.38
     for x in range(amount_obstacles):
         # rospy.loginfo(obstacle_poses.poses[x].position.z)
         if obstacle_poses.poses[x].position.z > 0:
             # Bounding window
             # get relative coordinates
             x_obstacle = obstacle_poses.poses[x].position.x * 1000  # mm
             y_obstacle = obstacle_poses.poses[x].position.y * 1000  # mm
             # get global coordinates
             global_pos_vec = self.avoider.coordinatetransform(
                 x_obstacle, y_obstacle, self.theta_current, self.d_current)
             # rospy.loginfo('x_obstacle = %f', x_obstacle)
             # rospy.loginfo('y_obstacle = %f', y_obstacle)
             # rospy.loginfo('theta_current = %f', self.theta_current)
             # rospy.loginfo('d_current = %f', self.d_current)
             x_global = global_pos_vec[0]  # mm
             y_global = global_pos_vec[1]  # mm
             # rospy.loginfo(global_pos_vec)
             # check if obstacle is within boundaries
             if x_global < self.x_bounding_width and abs(
                     y_global) < self.y_bounding_width:
                 # rospy.loginfo('Obstacle in range - Beware')
                 obstacle_poses_on_track.poses.append(
                     obstacle_poses.poses[x])
                 amount_obstacles_on_track += 1
     if amount_obstacles_on_track == 0:  # rospy.loginfo('0 obstacles on track')
         v = 0
     elif amount_obstacles_on_track == 1:
         #ToDo: check if self.d_current can be accessed through forwarding of self
         # targets = self.avoider.avoid(obstacle_poses_on_track, self.d_current, self.theta_current)
         # target.d_ref = targets[0]
         target.v_ref = 0  # due to inaccuracies in theta, stop in any case
         # if targets[1]:  # emergency stop
         #    target.v_ref = 0
         # self.theta_target_pub.publish(targets[2]) # theta not calculated in current version
         # rospy.loginfo('1 obstacles on track')
         # rospy.loginfo('d_target= %f', targets[0])
         # rospy.loginfo('emergency_stop = %f', targets[1])
         avoidance_active.data = True
     else:
         target.v_ref = 0
         # rospy.loginfo('%d obstacles on track', amount_obstacles_on_track)
         avoidance_active.data = True
         target.v_ref = 0
         # rospy.loginfo('emergency_stop = 1')
     self.obstavoidpose_topic.publish(target)
     self.avoid_pub.publish(avoidance_active)
     # rospy.loginfo('Avoidance flag set: %s', avoidance_active.data)
     return
Beispiel #11
0
    def __init__(self):
        self.velocity_to_m_per_s = 0.67
        self.omega_to_rad_per_s = 0.45 * 2 * math.pi
        self.omega_max = 50.0
        self.omega_min = -50.0

        #PID for CTE
        self.Kp_d = 6
        self.Ki_d = 0
        self.Kd_d = 0

        #PID for phi
        self.Kp_phi = 0.8
        self.Ki_phi = 0.1
        self.Kd_phi = 0

        #Useful Info
        self.last_dt = 0
        self.t_start = rospy.get_time()
        self.prev_pose_msg = LanePose()
        self.desired_d = 0
        self.desired_phi = 0
        self.in_lane_bool = False
        self.Lane_Control = False

        self.ct_integral_top_cutoff = 0.3
        self.ct_integral_bottom_cutoff = -0.3

        # Publication
        self.pub_car_cmd = rospy.Publisher("~car_cmd",
                                           Twist2DStamped,
                                           queue_size=1)

        # Subscriptions
        self.sub_fsm_mode = rospy.Subscriber("/tyscrduckie/fsm_node/mode",
                                             FSMState,
                                             self.cbMode,
                                             queue_size=1)
        self.sub_lane_reading = rospy.Subscriber(
            "/tyscrduckie/lane_filter_node/lane_pose", LanePose,
            self.PoseHandling)

        # The PID
        self.cte_control = Controller(self.Kp_d, self.Ki_d, self.Kd_d,
                                      self.ct_integral_top_cutoff,
                                      self.ct_integral_bottom_cutoff)
        self.phi_control = Controller(self.Kp_phi, self.Ki_phi, self.Kd_phi,
                                      self.ct_integral_top_cutoff,
                                      self.ct_integral_bottom_cutoff)
    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 __init__(self):
        self.node_name = rospy.get_name()
        self.lane_reading = LanePose()
        self.car_control_lane = Twist2DStamped()
        self.car_control_joy = Twist2DStamped()
        self.safe = True
        self.in_lane = True
        self.at_stop_line = False
        self.stop = False

        # Params:
        self.max_cross_track_error = self.setupParameter(
            "~max_cross_track_error", 0.1)
        self.max_heading_error = self.setupParameter("~max_heading_error",
                                                     math.pi / 4)
        self.min_speed = self.setupParameter("~min_speed", 0.1)
        self.max_speed = self.setupParameter("~max_speed", 0.3)
        self.max_steer = self.setupParameter("~max_steer", 0.2)

        # Publicaiton
        self.pub_car_cmd = rospy.Publisher("~car_cmd",
                                           Twist2DStamped,
                                           queue_size=1)
        self.pub_safe = rospy.Publisher("~safe", Bool, queue_size=1)

        # Subscriptions
        self.sub_lane_pose = rospy.Subscriber("~lane_pose",
                                              LanePose,
                                              self.cbLanePose,
                                              queue_size=1)
        self.sub_lane_control = rospy.Subscriber("~car_cmd_lane",
                                                 Twist2DStamped,
                                                 self.cbLaneControl,
                                                 queue_size=1)
        self.sub_joy_control = rospy.Subscriber("~car_cmd_joy",
                                                Twist2DStamped,
                                                self.cbJoyControl,
                                                queue_size=1)
        self.sub_at_stop_line = rospy.Subscriber("~stop_line_reading",
                                                 StopLineReading,
                                                 self.cbStopLine,
                                                 queue_size=1)

        self.params_update = rospy.Timer(rospy.Duration.from_sec(1.0),
                                         self.updateParams)
Beispiel #14
0
    def __init__(self):
        self.node_name = rospy.get_name()
        self.lane_pose = LanePose()

        # Parameters
        self.stop_distance = 0.2  # distance from the stop line that we should stop
        self.min_segs = 1  # minimum number of red segments that we should detect to estimate a stop
        self.lane_width = 0.2

        # Topics
        self.pub_stop_line_reading = rospy.Publisher("~stop_line_reading",
                                                     StopLineReading,
                                                     queue_size=1)
        self.pub_at_stop_line = rospy.Publisher("~at_stop_line",
                                                BoolStamped,
                                                queue_size=1)
        self.sub_segs = rospy.Subscriber("~segment_list", SegmentList,
                                         self.on_segment_list)
        self.sub_lane = rospy.Subscriber("~lane_pose", LanePose,
                                         self.on_lane_pose)
    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)
Beispiel #16
0
    def __init__(self):
        self.node_name = "Lane Filter"
        self.mean_0 = [
            self.setupParam("~mean_d_0", 0),
            self.setupParam("~mean_phi_0", 0)
        ]
        self.cov_0 = [[self.setupParam("~sigma_d_0", 0.1), 0],
                      [0, self.setupParam("~sigma_phi_0", 0.01)]]
        self.delta_d = self.setupParam("~delta_d", 0.02)  # in meters
        self.delta_phi = self.setupParam("~delta_phi", 0.02)  # in radians
        self.d_max = self.setupParam("~d_max", 0.5)
        self.d_min = self.setupParam("~d_min", -0.7)
        self.phi_min = self.setupParam("~phi_min", -pi / 2)
        self.phi_max = self.setupParam("~phi_max", pi / 2)
        self.cov_v = self.setupParam("~cov_v", 0.5)  # linear velocity "input"
        self.cov_omega = self.setupParam("~cov_omega",
                                         0.01)  # angular velocity "input"
        self.linewidth_white = self.setupParam("~linewidth_white", 0.04)
        self.linewidth_yellow = self.setupParam("~linewidth_yellow", 0.02)
        self.lanewidth = self.setupParam("~lanewidth", 0.4)
        self.min_max = self.setupParam("~min_max", 0.3)  # nats

        self.d, self.phi = np.mgrid[self.d_min:self.d_max:self.delta_d,
                                    self.phi_min:self.phi_max:self.delta_phi]
        self.beliefRV = np.empty(self.d.shape)
        self.initializeBelief()
        self.lanePose = LanePose()
        self.lanePose.d = self.mean_0[0]
        self.lanePose.phi = self.mean_0[1]
        self.sub = rospy.Subscriber("~segment_list", SegmentList,
                                    self.processSegments)
        # self.sub = rospy.Subscriber("~velocity",
        self.pub_lane_pose = rospy.Publisher("~lane_pose",
                                             LanePose,
                                             queue_size=1)
        self.pub_belief_img = rospy.Publisher("~belief_img",
                                              Image,
                                              queue_size=1)
        self.pub_entropy = rospy.Publisher("~entropy", Float32, queue_size=1)
        self.pub_in_lane = rospy.Publisher("~in_lane", Bool, queue_size=1)
    def __init__(self):
        self.node_name = "Lane Filter"
        self.active = True
        self.updateParams(None)
        
        self.d,self.phi = np.mgrid[self.d_min:self.d_max:self.delta_d,self.phi_min:self.phi_max:self.delta_phi]
        self.beliefRV=np.empty(self.d.shape)
        self.initializeBelief()
        self.lanePose = LanePose()
        self.lanePose.d=self.mean_0[0]
        self.lanePose.phi=self.mean_0[1]

        self.dwa = -(self.zero_val*self.l_peak**2 + self.zero_val*self.l_max**2 - self.l_max**2*self.peak_val - 2*self.zero_val*self.l_peak*self.l_max + 2*self.l_peak*self.l_max*self.peak_val)/(self.l_peak**2*self.l_max*(self.l_peak - self.l_max)**2)
        self.dwb = (2*self.zero_val*self.l_peak**3 + self.zero_val*self.l_max**3 - self.l_max**3*self.peak_val - 3*self.zero_val*self.l_peak**2*self.l_max + 3*self.l_peak**2*self.l_max*self.peak_val)/(self.l_peak**2*self.l_max*(self.l_peak - self.l_max)**2)
        self.dwc = -(self.zero_val*self.l_peak**3 + 2*self.zero_val*self.l_max**3 - 2*self.l_max**3*self.peak_val - 3*self.zero_val*self.l_peak*self.l_max**2 + 3*self.l_peak*self.l_max**2*self.peak_val)/(self.l_peak*self.l_max*(self.l_peak - self.l_max)**2)


        self.t_last_update = rospy.get_time()
        self.v_current = 0
        self.w_current = 0
        self.v_last = 0
        self.w_last = 0
        self.v_avg = 0
        self.w_avg = 0

        # Subscribers
        if self.use_propagation:
            self.sub_velocity = rospy.Subscriber("/lane_filter_node/velocity", Twist2DStamped, self.updateVelocity)
        self.sub = rospy.Subscriber("~segment_list", SegmentList, self.processSegments, queue_size=1)

        # Publishers
        self.pub_lane_pose  = rospy.Publisher("~lane_pose", LanePose, queue_size=1)
        self.pub_belief_img = rospy.Publisher("~belief_img", Image, queue_size=1)
        self.pub_entropy    = rospy.Publisher("~entropy",Float32, queue_size=1)
    	#self.pub_prop_img = rospy.Publisher("~prop_img", Image, queue_size=1)
        self.pub_in_lane    = rospy.Publisher("~in_lane",BoolStamped, queue_size=1)
        self.sub_switch = rospy.Subscriber("~switch", BoolStamped, self.cbSwitch, queue_size=1)

        self.timer = rospy.Timer(rospy.Duration.from_sec(1.0), self.updateParams)
Beispiel #18
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)
Beispiel #19
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)
Beispiel #20
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)
Beispiel #21
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)
Beispiel #22
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)
    def __init__(self, node_name):

        # Initialize the DTROS parent class
        super(LaneControllerNode, self).__init__(node_name=node_name,
                                                 node_type=NodeType.PERCEPTION)

        # Add the node parameters to the parameters dictionary
        # TODO: MAKE TO WORK WITH NEW DTROS PARAMETERS
        self.params = dict()
        self.params['~v_bar'] = DTParam('~v_bar',
                                        param_type=ParamType.FLOAT,
                                        min_value=0.0,
                                        max_value=5.0)
        self.params['~k_d'] = DTParam('~k_d',
                                      param_type=ParamType.FLOAT,
                                      min_value=-100.0,
                                      max_value=100.0)
        self.params['~k_theta'] = DTParam('~k_theta',
                                          param_type=ParamType.FLOAT,
                                          min_value=-100.0,
                                          max_value=100.0)
        self.params['~k_Id'] = DTParam('~k_Id',
                                       param_type=ParamType.FLOAT,
                                       min_value=-100.0,
                                       max_value=100.0)
        self.params['~k_Iphi'] = DTParam('~k_Iphi',
                                         param_type=ParamType.FLOAT,
                                         min_value=-100.0,
                                         max_value=100.0)
        self.params['~theta_thres'] = rospy.get_param('~theta_thres', None)
        self.params['~d_thres'] = rospy.get_param('~d_thres', None)
        self.params['~d_offset'] = rospy.get_param('~d_offset', None)
        self.params['~integral_bounds'] = rospy.get_param(
            '~integral_bounds', None)
        self.params['~d_resolution'] = rospy.get_param('~d_resolution', None)
        self.params['~phi_resolution'] = rospy.get_param(
            '~phi_resolution', None)
        self.params['~omega_ff'] = rospy.get_param('~omega_ff', None)
        self.params['~verbose'] = rospy.get_param('~verbose', None)
        self.params['~stop_line_slowdown'] = rospy.get_param(
            '~stop_line_slowdown', None)

        # Need to create controller object before updating parameters, otherwise it will fail
        self.controller = LaneController(self.params)
        # self.updateParameters() # TODO: This needs be replaced by the new DTROS callback when it is implemented

        # Initialize variables
        self.fsm_state = None
        self.wheels_cmd_executed = WheelsCmdStamped()
        self.pose_msg = LanePose()
        self.pose_initialized = False
        self.pose_msg_dict = dict()
        self.last_s = None
        self.stop_line_distance = None
        self.stop_line_detected = False
        self.at_stop_line = False
        self.obstacle_stop_line_distance = None
        self.obstacle_stop_line_detected = False
        self.at_obstacle_stop_line = False

        self.current_pose_source = 'lane_filter'

        # Construct publishers
        self.pub_car_cmd = rospy.Publisher("~car_cmd",
                                           Twist2DStamped,
                                           queue_size=1,
                                           dt_topic_type=TopicType.CONTROL)

        # Construct subscribers
        self.sub_lane_reading = rospy.Subscriber("~lane_pose",
                                                 LanePose,
                                                 self.cbAllPoses,
                                                 "lane_filter",
                                                 queue_size=1)
        self.sub_intersection_navigation_pose = rospy.Subscriber(
            "~intersection_navigation_pose",
            LanePose,
            self.cbAllPoses,
            "intersection_navigation",
            queue_size=1)
        self.sub_wheels_cmd_executed = rospy.Subscriber(
            "~wheels_cmd",
            WheelsCmdStamped,
            self.cbWheelsCmdExecuted,
            queue_size=1)
        self.sub_stop_line = rospy.Subscriber("~stop_line_reading",
                                              StopLineReading,
                                              self.cbStopLineReading,
                                              queue_size=1)
        self.sub_obstacle_stop_line = rospy.Subscriber(
            "~obstacle_distance_reading",
            StopLineReading,
            self.cbObstacleStopLineReading,
            queue_size=1)

        self.log("Initialized!")
Beispiel #25
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)
Beispiel #26
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)
Beispiel #27
0
    def __init__(self, node_name):

        # initialize the DTROS parent class
        super(Sensor_Fusion, self).__init__(node_name=node_name,
                                            node_type=NodeType.PERCEPTION)

        # covariance noise matrices
        self.R = np.zeros((2, 3))
        self.R[0][0] = 0.01  #!
        self.R[1][1] = 0.01
        self.R[2][2] = 0.04
        self.Q = 0.005 * np.eye(2)

        # higher publishing frequency
        self.higher_pub_rate = 0  # 1 = on; 0 = off
        self.ticks_for_pub = 14  # amount of ticks between published poses

        # parameter
        self.tick_to_meter = np.pi * 0.067 / 135.  # 6.7cm diameter, 135 = #ticks per revolution
        self.wheeltrack = 0.101

        # msg
        self.msg_fusion = LanePose()
        self.msg_enc_pose = LanePose()

        # values which need to be available over multiple calls
        self.z_m = np.zeros((3, 1))  # (d_enc, phi_enc, d_cam, phi_cam)^T
        self.old_z_m0 = 0
        self.last_left_ticks, self.last_right_ticks = 0, 0
        self.time_last_image = 0
        self.first_encoder_meas_l, self.first_encoder_meas_r = 0, 0
        self.i_first_calibration = 0
        self.distance, self.old_distance = 0, 0
        self.direction_l, self.direction_r = 1, 1
        self.only_cam = 0
        self.sum_alpha, self.sum_ticks = 0, 0
        self.recalib_counter, self.recalib_old_phi = 0, 0
        self.ticks_encoder_left, self.ticks_encoder_left_old = 0, 0
        self.ticks_encoder_right, self.ticks_encoder_right_old = 0, 0

        self.x = np.zeros((2, 1))
        self.P = np.eye(2)
        self.K = np.zeros((2, 3))
        self.I = np.eye(2)
        self.H = np.array([[0, 1], [1, 0], [0, 1]])
        self.save_alpha = np.zeros((80, 1))
        self.save_timestamp = np.zeros((80, 1))
        self.save_distance = np.zeros((80, 1))

        # Publisher
        self.pub_pose_est = rospy.Publisher(
            "~fusion_lane_pose", LanePose,
            queue_size=10)  # for lane_controller_node
        #self.pub_enc_est = rospy.Publisher("~encoder_pose", LanePose, queue_size=10) ## just for test

        # Subscriber
        self.sub_camera_pose = rospy.Subscriber("lane_filter_node/lane_pose",
                                                LanePose,
                                                self.SF,
                                                queue_size=1)
        self.sub_encoder_ticks = rospy.Subscriber(
            "left_wheel_encoder_node/tick",
            WheelEncoderStamped,
            self.update_encoder_measurement_l,
            queue_size=1)
        self.sub_encoder_ticks = rospy.Subscriber(
            "right_wheel_encoder_node/tick",
            WheelEncoderStamped,
            self.update_encoder_measurement_r,
            queue_size=1)
        self.sub_encoder_ticks = rospy.Subscriber(
            "wheels_driver_node/wheels_cmd_executed",
            WheelsCmdStamped,
            self.save_wheelscmdstamped,
            queue_size=1)
Beispiel #28
0
    def setGains(self):
        self.v_bar_gain_ref = 0.5
        v_bar_fallback = 0.25  # nominal speed, 0.25m/s
        self.v_max = 1
        k_theta_fallback = -2.0
        k_d_fallback = -(k_theta_fallback**2) / (4.0 * self.v_bar_gain_ref)
        theta_thres_fallback = math.pi / 6.0
        d_thres_fallback = math.fabs(
            k_theta_fallback / k_d_fallback) * theta_thres_fallback
        d_offset_fallback = 0.0

        k_theta_fallback = k_theta_fallback
        k_d_fallback = k_d_fallback

        k_Id_fallback = 2.5
        k_Iphi_fallback = 1.25

        self.fsm_state = None
        self.cross_track_err = 0
        self.heading_err = 0
        self.cross_track_integral = 0
        self.heading_integral = 0
        self.cross_track_integral_top_cutoff = 0.3
        self.cross_track_integral_bottom_cutoff = -0.3
        self.heading_integral_top_cutoff = 1.2
        self.heading_integral_bottom_cutoff = -1.2
        #-1.2
        self.time_start_curve = 0

        use_feedforward_part_fallback = False
        self.wheels_cmd_executed = WheelsCmdStamped()

        self.actuator_limits = Twist2DStamped()
        self.actuator_limits.v = 999.0  # to make sure the limit is not hit before the message is received
        self.actuator_limits.omega = 999.0  # to make sure the limit is not hit before the message is received
        self.omega_max = 999.0  # considering radius limitation and actuator limits   # to make sure the limit is not hit before the message is received

        self.use_radius_limit_fallback = True

        self.flag_dict = {
            "obstacle_detected": False,
            "parking_stop": False,
            "fleet_planning_lane_following_override_active": False,
            "implicit_coord_velocity_limit_active": False
        }

        self.pose_msg = LanePose()
        self.pose_initialized = False
        self.pose_msg_dict = dict()
        self.v_ref_possible = dict()
        self.main_pose_source = None

        self.active = True

        self.sleepMaintenance = False

        # overwrites some of the above set default values (the ones that are already defined in the corresponding yaml-file (see launch-file of this node))

        self.v_bar = self.setupParameter("~v_bar",
                                         v_bar_fallback)  # Linear velocity
        self.k_d = self.setupParameter("~k_d", k_d_fallback)  # P gain for d
        self.k_theta = self.setupParameter(
            "~k_theta", k_theta_fallback)  # P gain for theta
        self.d_thres = self.setupParameter(
            "~d_thres", d_thres_fallback)  # Cap for error in d
        self.theta_thres = self.setupParameter(
            "~theta_thres", theta_thres_fallback)  # Maximum desire theta
        self.d_offset = self.setupParameter(
            "~d_offset",
            d_offset_fallback)  # a configurable offset from the lane position

        self.k_Id = self.setupParameter(
            "~k_Id", k_Id_fallback)  # gain for integrator of d
        self.k_Iphi = self.setupParameter(
            "~k_Iphi",
            k_Iphi_fallback)  # gain for integrator of phi (phi = theta)
        #TODO: Feedforward was not working, go away with this error source! (Julien)
        self.use_feedforward_part = self.setupParameter(
            "~use_feedforward_part", use_feedforward_part_fallback)
        self.omega_ff = self.setupParameter("~omega_ff", 0)
        self.omega_max = self.setupParameter("~omega_max", 999)
        self.omega_min = self.setupParameter("~omega_min", -999)
        self.use_radius_limit = self.setupParameter(
            "~use_radius_limit", self.use_radius_limit_fallback)
        self.min_radius = self.setupParameter("~min_rad", 0.0)

        self.d_ref = self.setupParameter("~d_ref", 0)
        self.phi_ref = self.setupParameter("~phi_ref", 0)
        self.object_detected = self.setupParameter("~object_detected", 0)
        self.v_ref_possible["default"] = self.v_max
Beispiel #29
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)
Beispiel #31
0
    def setGains(self):
        self.v_bar_gain_ref = 0.5
        self.v_max = 1

        v_bar_fallback = 0.25  # nominal speed, 0.25m/s
        k_theta_fallback = -2.0
        k_d_fallback = -(k_theta_fallback**2) / (4.0 * self.v_bar_gain_ref)

        theta_thres_fallback = math.pi / 6.0
        d_thres_fallback = math.fabs(
            k_theta_fallback / k_d_fallback) * theta_thres_fallback
        d_offset_fallback = 0.0

        k_Id_fallback = 2.5
        k_Iphi_fallback = 1.25

        self.fsm_state = None
        self.cross_track_err = 0
        self.heading_err = 0
        self.cross_track_integral = 0
        self.heading_integral = 0
        self.cross_track_integral_top_cutoff = 0.3
        self.cross_track_integral_bottom_cutoff = -0.3
        self.heading_integral_top_cutoff = 1.2
        self.heading_integral_bottom_cutoff = -1.2
        self.time_start_curve = 0

        self.wheels_cmd_executed = WheelsCmdStamped()

        self.actuator_limits = Twist2DStamped()
        self.actuator_limits.v = 999.0  # to make sure the limit is not hit before the message is received
        self.actuator_limits.omega = 999.0  # to make sure the limit is not hit before the message is received
        self.omega_max = 999.0  # considering radius limitation and actuator limits

        self.use_radius_limit_fallback = True

        self.pose_msg = LanePose()
        self.pose_initialized = False
        self.pose_msg_dict = dict()
        self.v_ref_possible = {
            'default': self.v_max,
            'main_pose': v_bar_fallback
        }
        self.main_pose_source = None

        self.active = True

        # overwrites some of the above set default values (the ones that are already defined in the corresponding yaml-file (see launch-file of this node))

        # Linear velocity
        self.v_bar = self.setupParameter("~v_bar", v_bar_fallback)
        # P gain for d
        self.k_d = self.setupParameter("~k_d", k_d_fallback)
        # P gain for theta
        self.k_theta = self.setupParameter("~k_theta", k_theta_fallback)
        # Cap for error in d
        self.d_thres = self.setupParameter("~d_thres", d_thres_fallback)
        # Maximum desired theta
        self.theta_thres = self.setupParameter("~theta_thres",
                                               theta_thres_fallback)
        # A configurable offset from the lane position
        self.d_offset = self.setupParameter("~d_offset", d_offset_fallback)

        # Gain for integrator of d
        self.k_Id = self.setupParameter("~k_Id", k_Id_fallback)
        # Gain for integrator of phi (phi = theta)
        self.k_Iphi = self.setupParameter("~k_Iphi", k_Iphi_fallback)

        # setup backward parameters
        self.k_d_back = self.setupParameter("~k_d_back", 3.0)
        self.k_theta_back = self.setupParameter("~k_theta_back", 1.0)
        self.k_Id_back = self.setupParameter("~k_Id_back", 1.0)
        self.k_Itheta_back = self.setupParameter("~k_Itheta_back", -1.0)

        self.omega_ff = self.setupParameter("~omega_ff", 0)
        self.omega_max = self.setupParameter("~omega_max", 4.7)
        self.omega_min = self.setupParameter("~omega_min", -4.7)
        self.use_radius_limit = self.setupParameter(
            "~use_radius_limit", self.use_radius_limit_fallback)
        self.min_radius = self.setupParameter("~min_rad", 0.0)

        self.d_ref = self.setupParameter("~d_ref", 0)
        self.phi_ref = self.setupParameter("~phi_ref", 0)
        self.object_detected = self.setupParameter("~object_detected", 0)
    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)