def publish_stop_line_msg(self,
header,
detected=False,
at=False,
x=0.0,
y=0.0):
"""
Makes and publishes a stop line message.
Args:
header: header of a ROS message, usually copied from an incoming message
detected (`bool`): whether a vehicle has been detected
at (`bool`): whether we are closer than the desired distance
x (`float`): distance to the vehicle
y (`float`): lateral offset from the vehicle (not used, always 0)
"""
stop_line_msg = StopLineReading()
stop_line_msg.header = header
stop_line_msg.stop_line_detected = bool(detected)
stop_line_msg.at_stop_line = bool(at)
stop_line_msg.stop_line_point.x = int(x)
stop_line_msg.stop_line_point.y = int(y)
self.pub_virtual_stop_line.publish(stop_line_msg)
"""
Remove once have the road anomaly watcher node
"""
stopped_flag = BoolStamped()
stopped_flag.header = header
stopped_flag.data = bool(at)
self.pub_stopped_flag.publish(stopped_flag)
def car_read_action(self):
while True:
strhttp = 'http://192.168.0.100/tsp/read_car_action.php?car_id=1'
req = urllib2.Request(strhttp)
response = urllib2.urlopen(req)
the_page = response.read()
if (self.car_action_check == 1 and int(the_page) == 0
and self.last_commodity_tag != 0):
self.car_action_check = int(the_page)
e_stop_msg = BoolStamped()
e_stop_msg.data = int(the_page)
self.pub_lanefollowing.publish(e_stop_msg)
if self.last_action == "B":
stop_line_reading_msg = StopLineReading()
stop_line_reading_msg.stop_line_detected = True
stop_line_reading_msg.at_stop_line = 1
self.pub_stop_line_reading.publish(stop_line_reading_msg)
stop_msg = BoolStamped()
stop_msg.data = True
self.pub_at_stop_back.publish(stop_msg)
time.sleep(1)
self.turn = 3
self.type_back.publish(self.turn)
self.last_commodity_tag = 0
self.car_action_check = int(the_page)
e_stop_msg = BoolStamped()
e_stop_msg.data = int(the_page)
self.pub_lanefollowing.publish(e_stop_msg)
time.sleep(1)
def after_intersection_work(self):
self.loginfo("Blocking stop line detection after the intersection")
stop_line_reading_msg = StopLineReading()
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)
self.sleep = True
rospy.sleep(self.off_time.value)
self.sleep = False
self.loginfo("Resuming stop line detection after the intersection")
def afterIntersectionWork(self):
rospy.loginfo("stop line sleep start")
stop_line_reading_msg = StopLineReading()
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)
self.sleep = True
rospy.sleep(self.off_time)
self.sleep = False
rospy.loginfo("stop line sleep end")
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 car_read_action(self):
while True:
strhttp='http://192.168.0.100/tsp/read_car_action.php?car_id=1'
req = urllib2.Request(strhttp)
response = urllib2.urlopen(req)
the_page = response.read()
#self.car_action_check=int(the_page)
"""
if(self.car_action_check!=int(the_page)):
self.car_action_check=int(the_page)
e_stop_msg=BoolStamped()
e_stop_msg.data=int(the_page)
self.pub_lanefollowing.publish(e_stop_msg)
"""
#print("yyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyy")
#print(self.last_commodity_tag)
if(self.car_action_check==1 and int(the_page)==0 and self.last_commodity_tag!=0):
self.car_action_check=int(the_page)
e_stop_msg=BoolStamped()
e_stop_msg.data=int(the_page)
self.pub_lanefollowing.publish(e_stop_msg)
print("lllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllll")
#print(self.last_commodity_tag)
#strhttp='http://192.168.0.100/tsp/car_recode_navigation.php?car_id=1&tag_id='+str(self.last_commodity_tag)
#req = urllib2.Request(strhttp)
#response = urllib2.urlopen(req)
#the_page_1 = response.read()
#print(the_page_1)
print(self.last_action)
#print("Sssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssss")
if self.last_action=="B":
stop_line_reading_msg = StopLineReading()
stop_line_reading_msg.stop_line_detected = True
stop_line_reading_msg.at_stop_line = 1
self.pub_stop_line_reading.publish(stop_line_reading_msg)
print("ooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo")
stop_msg=BoolStamped()
stop_msg.data=True
self.pub_at_stop_back.publish(stop_msg)
time.sleep(5)
#print("ttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttt")
self.turn=3
self.type_back.publish(self.turn)
self.last_commodity_tag=0
if(self.car_action_check!=int(the_page)):
#if(True):
self.car_action_check=int(the_page)
e_stop_msg=BoolStamped()
e_stop_msg.data=int(the_page)
self.pub_lanefollowing.publish(e_stop_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 __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 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 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 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)
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)
if(self.car_action_check==1 and int(the_page)==0 and self.last_commodity_tag!=0):
self.car_action_check=int(the_page)
e_stop_msg=BoolStamped()
e_stop_msg.data=int(the_page)
self.pub_lanefollowing.publish(e_stop_msg)
print("lllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllll")
#print(self.last_commodity_tag)
#strhttp='http://192.168.0.100/tsp/car_recode_navigation.php?car_id=1&tag_id='+str(self.last_commodity_tag)
#req = urllib2.Request(strhttp)
#response = urllib2.urlopen(req)
#the_page_1 = response.read()
#print(the_page_1)
print(self.last_action)
#print("Sssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssss")
if self.last_action=="B":
stop_line_reading_msg = StopLineReading()
stop_line_reading_msg.stop_line_detected = True
stop_line_reading_msg.at_stop_line = 1
self.pub_stop_line_reading.publish(stop_line_reading_msg)
print("ooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo")
stop_msg=BoolStamped()
stop_msg.data=True
self.pub_at_stop_back.publish(stop_msg)
time.sleep(5)
#print("ttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttt")
self.turn=3
self.type_back.publish(self.turn)
self.last_commodity_tag=0
if(self.car_action_check!=int(the_page)):
#if(True):
self.car_action_check=int(the_page)