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 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 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 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)
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)