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 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)
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
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)
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
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
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)
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)
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)
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)
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)
def __init__(self): self.node_name = rospy.get_name() ## Parameters self.stop_line_max_age = self.setupParameter( "~stop_line_max_age", 1.0 ) # [sec] don't use stop_line_reading msg pairs more than this far apart self.lane_pose_max_age = self.setupParameter( "~lane_pose_max_age", 1.0 ) # [sec] don't use lane_pose msg pairs more than this far apart self.wheels_cmd_max_age = self.setupParameter( "~wheels_cmd_max_age", 2.0) # [sec] throw away wheels_cmd's older than this ## state vars self.in_lane = False self.current_wheels_cmd = WheelsCmdStamped() self.old_stop_line_msg = StopLineReading() self.old_lane_pose_msg = LanePose() self.cmd_buffer = deque() # buffer of wheel commands self.v_sample_msg = Vsample( ) # global variable because different parts of this are set in different callbacks ## publishers and subscribers self.sub_stop_line_reading = rospy.Subscriber("~stop_line_reading", StopLineReading, self.stopLineCB) self.sub_lane_pose = rospy.Subscriber("~lane_pose", LanePose, self.lanePoseCB) self.sub_wheels_cmd_executed = rospy.Subscriber( "~wheels_cmd_executed", WheelsCmdStamped, self.wheelsCmdCB) self.pub_v_sample = rospy.Publisher("~v_sample", Vsample, queue_size=1) self.pub_theta_dot_sample = rospy.Publisher("~theta_dot_sample", ThetaDotSample, queue_size=1) rospy.loginfo('[%s] Initialized' % self.node_name)
def 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)
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!")
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)
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)
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)
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
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 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)