def cb_segments(self, segment_list_msg): if not self.switch or self.sleep: return good_seg_count = 0 stop_line_x_accumulator = 0.0 stop_line_y_accumulator = 0.0 for segment in segment_list_msg.segments: if segment.color != segment.RED: continue if segment.points[0].x < 0 or segment.points[1].x < 0: # the point is behind us continue p1_lane = self.to_lane_frame(segment.points[0]) p2_lane = self.to_lane_frame(segment.points[1]) avg_x = 0.5 * (p1_lane[0] + p2_lane[0]) avg_y = 0.5 * (p1_lane[1] + p2_lane[1]) stop_line_x_accumulator += avg_x stop_line_y_accumulator += avg_y # TODO output covariance and not just mean good_seg_count += 1.0 stop_line_reading_msg = StopLineReading() stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp if good_seg_count < self.min_segs.value: stop_line_reading_msg.stop_line_detected = False stop_line_reading_msg.at_stop_line = False self.pub_stop_line_reading.publish(stop_line_reading_msg) # ### CRITICAL: publish false to at stop line output_topic # msg = BoolStamped() # msg.header.stamp = stop_line_reading_msg.header.stamp # msg.data = False # self.pub_at_stop_line.publish(msg) # ### CRITICAL END else: stop_line_reading_msg.stop_line_detected = True stop_line_point = Point() stop_line_point.x = stop_line_x_accumulator / good_seg_count stop_line_point.y = stop_line_y_accumulator / good_seg_count stop_line_reading_msg.stop_line_point = stop_line_point # Only detect redline if y is within max_y distance: stop_line_reading_msg.at_stop_line = ( stop_line_point.x < self.stop_distance.value and np.abs(stop_line_point.y) < self.max_y.value ) self.pub_stop_line_reading.publish(stop_line_reading_msg) if stop_line_reading_msg.at_stop_line: msg = BoolStamped() msg.header.stamp = stop_line_reading_msg.header.stamp msg.data = True self.pub_at_stop_line.publish(msg)
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 processSegments( self, segment_list_msg ): #Process segment list and decide whether there is a stop line. if not self.active or self.sleep: return good_seg_count = 0 stop_line_x_accumulator = 0.0 stop_line_y_accumulator = 0.0 for segment in segment_list_msg.segments: if segment.color != segment.RED: continue if segment.points[0].x < 0 or segment.points[ 1].x < 0: # the point is behind us continue ###Transform segment list to lane pose frame p1_lane = self.to_lane_frame(segment.points[0]) p2_lane = self.to_lane_frame(segment.points[1]) ###Find average x and y of the red line segment avg_x = 0.5 * (p1_lane[0] + p2_lane[0]) avg_y = 0.5 * (p1_lane[1] + p2_lane[1]) stop_line_x_accumulator += avg_x stop_line_y_accumulator += avg_y # TODO output covariance and not just mean good_seg_count += 1.0 stop_line_reading_msg = StopLineReading() stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp if ( good_seg_count < self.min_segs ): #If red line segment is lower that threshold, then, there is no stop line stop_line_reading_msg.stop_line_detected = False stop_line_reading_msg.at_stop_line = False self.pub_stop_line_reading.publish(stop_line_reading_msg) return else: stop_line_reading_msg.stop_line_detected = True stop_line_point = Point() stop_line_point.x = stop_line_x_accumulator / good_seg_count stop_line_point.y = stop_line_y_accumulator / good_seg_count stop_line_reading_msg.stop_line_point = stop_line_point stop_line_reading_msg.at_stop_line = stop_line_point.x < self.stop_distance and math.fabs( stop_line_point.y) < self.lanewidth / 4 self.pub_stop_line_reading.publish(stop_line_reading_msg) if stop_line_reading_msg.at_stop_line: ###Task: assign fsm stop line message, if message data is true, then it will trigger stop event ###Problem msg = BoolStamped() #Declaration msg.header.stamp = segment_list_msg.header.stamp #Assign segment_list time stamp msg.data = true #If you want to trigger the event at_stop_line, message data must be "true" print "Event stop line fiter is triggered ? :" + msg.data self.pub_at_stop_line.publish(msg)
def processSegments(self, segment_list_msg): #print ("-*/*/*/*/*&&&&&&&&&&*/*/*/*/-") if not self.active or self.sleep: return good_seg_count = 0 stop_line_x_accumulator = 0.0 stop_line_y_accumulator = 0.0 for segment in segment_list_msg.segments: if segment.color != segment.RED and segment.color != segment.BLUE: continue if segment.points[0].x < 0 or segment.points[ 1].x < 0: # the point is behind us continue p1_lane = self.to_lane_frame(segment.points[0]) p2_lane = self.to_lane_frame(segment.points[1]) avg_x = 0.5 * (p1_lane[0] + p2_lane[0]) avg_y = 0.5 * (p1_lane[1] + p2_lane[1]) stop_line_x_accumulator += avg_x stop_line_y_accumulator += avg_y # TODO output covariance and not just mean good_seg_count += 1.0 stop_line_reading_msg = StopLineReading() stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp if (good_seg_count < self.min_segs): stop_line_reading_msg.stop_line_detected = False stop_line_reading_msg.at_stop_line = False self.pub_stop_line_reading.publish(stop_line_reading_msg) return stop_line_reading_msg.stop_line_detected = True stop_line_point = Point() stop_line_point.x = stop_line_x_accumulator / good_seg_count stop_line_point.y = stop_line_y_accumulator / good_seg_count stop_line_reading_msg.stop_line_point = stop_line_point stop_line_reading_msg.at_stop_line = stop_line_point.x < self.stop_distance and math.fabs( stop_line_point.y) < self.lanewidth / 4 self.pub_stop_line_reading.publish(stop_line_reading_msg) if stop_line_reading_msg.at_stop_line: msg = BoolStamped() msg.header.stamp = stop_line_reading_msg.header.stamp msg.data = True print "-------at stop line-------" if segment.color == segment.BLUE: self.pub_sendmessage = rospy.Publisher( "/arg2/stop_line_filter_node/send", BoolStamped, queue_size=1) self.pub_sendmessage.publish(msg) elif segment.color == segment.RED: self.pub_at_stop_line.publish(msg)
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 processSegments(self, segment_list_msg): good_seg_count = 0 stop_line_x_accumulator = 0.0 stop_line_y_accumulator = 0.0 for segment in segment_list_msg.segments: if segment.color != segment.RED: continue if segment.points[0].x < 0 or segment.points[ 1].x < 0: # the point is behind us continue if not self.active or self.sleep: continue p1_lane = self.to_lane_frame(segment.points[0]) p2_lane = self.to_lane_frame(segment.points[1]) avg_x = 0.5 * (p1_lane[0] + p2_lane[0]) avg_y = 0.5 * (p1_lane[1] + p2_lane[1]) stop_line_x_accumulator += avg_x stop_line_y_accumulator += avg_y # TODO output covariance and not just mean good_seg_count += 1.0 stop_line_reading_msg = StopLineReading() stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp if (good_seg_count < self.min_segs): stop_line_reading_msg.stop_line_detected = False stop_line_reading_msg.at_stop_line = False self.pub_stop_line_reading.publish(stop_line_reading_msg) return stop_line_reading_msg.stop_line_detected = True stop_line_point = Point() stop_line_point.x = stop_line_x_accumulator / good_seg_count stop_line_point.y = stop_line_y_accumulator / good_seg_count stop_line_reading_msg.stop_line_point = stop_line_point stop_line_reading_msg.at_stop_line = stop_line_point.x < self.stop_distance and math.fabs( stop_line_point.y) < self.lanewidth / 4 self.pub_stop_line_reading.publish(stop_line_reading_msg) if stop_line_reading_msg.at_stop_line: msg = BoolStamped() msg.header.stamp = stop_line_reading_msg.header.stamp msg.data = True self.pub_at_stop_line.publish(msg) else: msg = BoolStamped() msg.header.stamp = stop_line_reading_msg.header.stamp msg.data = False self.pub_at_stop_line.publish(msg)
def processSegments(self, segment_list_msg): good_seg_count=0 stop_line_x_accumulator=0.0 stop_line_y_accumulator=0.0 for segment in segment_list_msg.segments: if segment.color != segment.RED: continue if segment.points[0].x < 0 or segment.points[1].x < 0: # the point is behind us continue if not self.active or self.sleep: continue p1_lane = self.to_lane_frame(segment.points[0]) p2_lane = self.to_lane_frame(segment.points[1]) avg_x = 0.5*(p1_lane[0] + p2_lane[0]) avg_y = 0.5*(p1_lane[1] + p2_lane[1]) stop_line_x_accumulator += avg_x stop_line_y_accumulator += avg_y # TODO output covariance and not just mean good_seg_count += 1.0 stop_line_reading_msg = StopLineReading() stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp if (good_seg_count < self.min_segs): stop_line_reading_msg.stop_line_detected = False stop_line_reading_msg.at_stop_line = False self.pub_stop_line_reading.publish(stop_line_reading_msg) return stop_line_reading_msg.stop_line_detected = True stop_line_point = Point() stop_line_point.x = stop_line_x_accumulator/good_seg_count stop_line_point.y = stop_line_y_accumulator/good_seg_count stop_line_reading_msg.stop_line_point = stop_line_point stop_line_reading_msg.at_stop_line = stop_line_point.x < self.stop_distance and math.fabs(stop_line_point.y) < self.lanewidth/4 self.pub_stop_line_reading.publish(stop_line_reading_msg) if stop_line_reading_msg.at_stop_line: msg = BoolStamped() msg.header.stamp = stop_line_reading_msg.header.stamp msg.data = True self.pub_at_stop_line.publish(msg) else: msg = BoolStamped() msg.header.stamp = stop_line_reading_msg.header.stamp msg.data = False self.pub_at_stop_line.publish(msg)