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 processTimer(self, msgg):
if self.active: # fsm switch on
if (self.state == "LANE_FOLLOWING_TURN_RIGHT") or (
self.state == "LANE_FOLLOWING_TURN_LEFT"
): # if in fsm state "left turn" or "right turn""
if not self.start: # if timer not start yet
self.timer_start = time.time() # record start time
self.start = True # change timer state to start
print "start time: ", self.timer_start
self.timer_end = time.time() # record time now
print "end time: ", self.timer_end
if (
self.timer_end - self.timer_start
) > 4: #if time duration between start time and time now bigger than 2 seconsds
# publish time is up
msg = BoolStamped()
msg.data = True
self.pub_time_is_up.publish(msg)
print "time is up"
if not self.active: # fsm switch off
# publish timer is off and reset timer state to not starting
msg = BoolStamped()
msg.data = False
self.pub_time_is_up.publish(msg)
self.start = False
def JoyButton(self, joy_msg):
if(joy_msg.buttons[0] == 1):
rospy.loginfo("[%s] You Press Button A " %(self.node_name))
state = BoolStamped()
state.data = False
self.pub_A.publish(state)
elif(joy_msg.buttons[1] == 1):
rospy.loginfo("[%s] You Press Button B " %(self.node_name))
state = BoolStamped()
state.data = True
self.pub_B.publish(state)
elif(joy_msg.buttons[2] == 1):
rospy.loginfo("[%s] You Press Button X " %(self.node_name))
state = BoolStamped()
state.data = True
self.pub_X.publish(state)
elif(joy_msg.buttons[3] == 1):
rospy.loginfo("[%s] You Press Button Y " %(self.node_name))
state = BoolStamped()
state.data = True
self.pub_Y.publish(state)
else:
some_active = sum(joy_msg.buttons)
if some_active:
rospy.loginfo("[%s] No binding buttons " %(self.node_name))
def next_task(self):
# Reset switch
self.path_finish = False
self.move_finish = False
# Pop next task
try:
self.current_task = self.current_macro_task.pop()
#self.current_task = self.macro_tasks['macro_task_0'].pop()
rospy.loginfo("[%s] pop next task" %(self.node_name))
# Publish finish message
finish_msg = BoolStamped()
finish_msg.header.stamp = rospy.Time.now()
finish_msg.data = True
self.pub_finish.publish(finish_msg)
except:
return
debug = False
if debug:
self.current_task = self.default_task
rospy.loginfo("[%s] pop default task" %(self.node_name))
# Publish finish message
finish_msg = BoolStamped()
finish_msg.header.stamp = rospy.Time.now()
finish_msg.data = True
self.pub_finish.publish(finish_msg)
def lane_follow_override_cb(self, data):
# this should check and also set the block incomming flag
if data.data == True and self.blockLfOverrideMsg == False:
# prevent other msgs from invoking remote control node
self.blockLfOverrideMsg = True
# publish message to stop lane follow
override_msg = BoolStamped()
override_msg.header.stamp = data.header.stamp
override_msg.data = False
self.pub_joy_override.publish(override_msg)
# publish message to stop the bot
car_msg = Twist2DStamped()
car_msg.v = 0.0
car_msg.omega = 0.0
self.pub_car_cmd.publish(car_msg)
# Consider creating a new message to notify romote
# control to start and send send it the Pose2D from
# the object detector, instead of object detection
# sending it to two different nodes. This would
# eliminate possible race condition..
# msg: BoolStamped
# Posed2D
#
# Note this if implemented, this would replace the
# message below..
# publish message to start remote control mode.
remote_ctrl_msg = BoolStamped()
remote_ctrl_msg.header.stamp = data.header.stamp
remote_ctrl_msg.data = data.data
self.pub_steering.publish(remote_ctrl_msg)
def processButtons(self, joy_msg):
if (joy_msg.buttons[6] == 1): # The back button
override_msg = BoolStamped()
override_msg.header.stamp = self.joy.header.stamp
override_msg.data = True
rospy.loginfo('joystick_control = True')
self.joy_control = 1
self.ncs_control = 0
self.pub_joy_override.publish(override_msg)
elif (joy_msg.buttons[7] == 1): # the start button
override_msg = BoolStamped()
override_msg.header.stamp = self.joy.header.stamp
override_msg.data = False
rospy.loginfo('deep_lanefollowing = True')
self.joy_control = 0
self.ncs_control = 1
self.pub_joy_override.publish(override_msg)
elif (joy_msg.buttons[3] == 1):
anti_instagram_msg = BoolStamped()
anti_instagram_msg.header.stamp = self.joy.header.stamp
anti_instagram_msg.data = True
rospy.loginfo('anti_instagram message')
self.pub_anti_instagram.publish(anti_instagram_msg)
def processButtons(self, joy_msg):
if (joy_msg.buttons[6] == 1): #The back button
override_msg = BoolStamped()
override_msg.header.stamp = self.joy.header.stamp
override_msg.data = True
self.pub_joy_override.publish(override_msg)
elif (joy_msg.buttons[7] == 1): #the start button
override_msg = BoolStamped()
override_msg.header.stamp = self.joy.header.stamp
override_msg.data = False
self.pub_joy_override.publish(override_msg)
def joy_cb(self, joy_msg):
"""
Callback that process the received :obj:`Joy` messages.
Args:
joy_msg (:obj:`Joy`): the joystick message to process.
"""
# Navigation buttons
car_cmd_msg = Twist2DStamped()
car_cmd_msg.header.stamp = rospy.get_rostime()
# Left stick V-axis. Up is positive
car_cmd_msg.v = joy_msg.axes[1] * self._speed_gain
if self._bicycle_kinematics:
# Implements Bicycle Kinematics - Nonholonomic Kinematics
# see https://inst.eecs.berkeley.edu/~ee192/sp13/pdf/steer-control.pdf
steering_angle = joy_msg.axes[3] * self._steer_gain
car_cmd_msg.omega = \
car_cmd_msg.v / self._simulated_vehicle_length * \
math.tan(steering_angle)
else:
# Holonomic Kinematics for Normal Driving
car_cmd_msg.omega = joy_msg.axes[3] * self._steer_gain
self.pub_car_cmd.publish(car_cmd_msg)
# Back button: Stop LF
if joy_msg.buttons[6] == 1:
override_msg = BoolStamped()
override_msg.header.stamp = joy_msg.header.stamp
override_msg.data = True
self.log('override_msg = True')
self.pub_joy_override.publish(override_msg)
# Start button: Start LF
elif joy_msg.buttons[7] == 1:
override_msg = BoolStamped()
override_msg.header.stamp = joy_msg.header.stamp
override_msg.data = False
self.log('override_msg = False')
self.pub_joy_override.publish(override_msg)
# Y button: Emergency Stop
elif joy_msg.buttons[3] == 1:
self.e_stop = not self.e_stop
estop_msg = BoolStamped()
estop_msg.header.stamp = joy_msg.header.stamp
estop_msg.data = self.e_stop
self.pub_e_stop.publish(estop_msg)
else:
some_active = sum(joy_msg.buttons) > 0
if some_active:
self.logwarn('No binding for joy_msg.buttons = %s' %
str(joy_msg.buttons))
def processSegments(self, segment_list_msg):
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
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)
### 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
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.max_y #Only detect redline if y is within max_y distance
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 cbAndSwitch(self, msg):
if msg.data == False: #The back button
override_msg = BoolStamped()
override_msg.header.stamp = rospy.Time.now()
override_msg.data = True
rospy.loginfo('override_msg = True')
self.pub_joy_override.publish(override_msg)
elif msg.data == True: #the start button
override_msg = BoolStamped()
override_msg.header.stamp = rospy.Time.now()
override_msg.data = False
rospy.loginfo('override_msg = False')
self.pub_joy_override.publish(override_msg)
def JoyButton(self, joy_msg):
if (joy_msg.buttons[0] == 1):
rospy.loginfo("[%s] You Press Button A " % (self.node_name))
state = BoolStamped()
state.data = False
self.pub_A.publish(state)
elif (joy_msg.buttons[1] == 1):
rospy.loginfo("[%s] You Press Button B " % (self.node_name))
state = BoolStamped()
state.data = True
self.pub_B.publish(state)
elif (joy_msg.buttons[2] == 1):
rospy.loginfo("[%s] You Press Button X " % (self.node_name))
state = BoolStamped()
state.data = True
self.pub_X.publish(state)
elif (joy_msg.buttons[3] == 1):
rospy.loginfo("[%s] You Press Button Y " % (self.node_name))
state = BoolStamped()
state.data = True
self.pub_Y.publish(state)
else:
some_active = sum(joy_msg.buttons)
if some_active:
rospy.loginfo("[%s] No binding buttons " % (self.node_name))
#gripperrrrrr_cmd
if (joy_msg.buttons[0] == 1):
rospy.loginfo("Grip Open")
g_cmd = Pixel()
g_cmd.u = 1
self.pub_Grip.publish(g_cmd)
elif (joy_msg.buttons[1] == 1):
rospy.loginfo("Grip Close")
g_cmd = Pixel()
g_cmd.u = 2
self.pub_Grip.publish(g_cmd)
else:
rospy.loginfo("Grip Stop")
g_cmd = Pixel()
g_cmd.u = 0
self.pub_Grip.publish(g_cmd)
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 processStateChange(self, msg):
if msg.state == "START_TO_TURN":
if self.turnright == True:
turn_right = BoolStamped()
turn_right.data = True
self.pub_turn_right.publish(turn_right)
elif self.turnleft == True:
turn_left = BoolStamped()
turn_left.data = True
self.pub_turn_left.publish(turn_left)
elif self.turnaround == True:
turn_around = BoolStamped()
turn_around.data = True
self.pub_turn_around.publish(turn_around)
self.state = msg.state
def processButtons(self, joy_msg):
if (joy_msg.buttons[6] == 1): #The back button
override_msg = BoolStamped()
override_msg.header.stamp = self.joy.header.stamp
override_msg.data = True
self.get_logger().info('override_msg = True')
self.pub_joy_override.publish(override_msg)
elif (joy_msg.buttons[7] == 1): #the start button
override_msg = BoolStamped()
override_msg.header.stamp = self.joy.header.stamp
override_msg.data = False
self.get_logger().info('override_msg = False')
self.pub_joy_override.publish(override_msg)
elif (joy_msg.buttons[5] == 1): # Right back button
self.state_verbose ^= True
self.get_logger().info('state_verbose = %s' % self.state_verbose)
#rospy.set_param('line_detector_node/verbose', self.state_verbose) # bad - should be published for all to hear - not set a specific param
elif (joy_msg.buttons[4] == 1): #Left back button
self.state_parallel_autonomy ^= True
self.get_logger().info('state_parallel_autonomy = %s' % self.state_parallel_autonomy)
parallel_autonomy_msg = BoolStamped()
parallel_autonomy_msg.header.stamp = self.joy.header.stamp
parallel_autonomy_msg.data = self.state_parallel_autonomy
self.pub_parallel_autonomy.publish(parallel_autonomy_msg)
elif (joy_msg.buttons[3] == 1):
anti_instagram_msg = BoolStamped()
anti_instagram_msg.header.stamp = self.joy.header.stamp
anti_instagram_msg.data = True
self.get_logger().info('anti_instagram message')
self.pub_anti_instagram.publish(anti_instagram_msg)
#elif (joy_msg.buttons[8] == 1): #power button (middle)
#e_stop_msg = BoolStamped()
#e_stop_msg.header.stamp = self.joy.header.stamp
#e_stop_msg.data = True # note that this is toggle (actual value doesn't matter)
#self.get_logger().info('E-stop message')
#self.pub_e_stop.publish(e_stop_msg)
elif (joy_msg.buttons[9] == 1): #push left joystick button
avoidance_msg = BoolStamped()
self.get_logger().info('start lane following with avoidance mode')
avoidance_msg.header.stamp = self.joy.header.stamp
avoidance_msg.data = True
self.pub_avoidance.publish(avoidance_msg)
else:
some_active = sum(joy_msg.buttons) > 0
if some_active:
self.get_logger().info('No binding for joy_msg.buttons = %s' % str(joy_msg.buttons))
def image_cb(self, image_msg):
if not self.initialized:
return
# TODO to get better hz, you might want to only call your wrapper's predict function only once ever 4-5 images?
# This way, you're not calling the model again for two practically identical images. Experiment to find a good number of skipped
# images.
# Decode from compressed image with OpenCV
try:
image = self.bridge.compressed_imgmsg_to_cv2(image_msg)
except ValueError as e:
self.logerr('Could not decode image: %s' % e)
return
# Perform color correction
if self.ai_thresholds_received:
image = self.ai.apply_color_balance(
self.anti_instagram_thresholds["lower"],
self.anti_instagram_thresholds["higher"], image)
image = cv2.resize(image, (224, 224))
bboxes, classes, scores = self.model_wrapper.predict(image)
msg = BoolStamped()
msg.header = image_msg.header
msg.data = self.det2bool(
bboxes[0],
classes[0]) # [0] because our batch size given to the wrapper is 1
self.pub_obj_dets.publish(msg)
def getOutputMsg(self, gate_name, inputs):
bool_list = list()
latest_time_stamp = rospy.Time(0)
for event_name, event_msg in self.event_msg_dict.items():
if event_msg is None:
return None
if event_name in inputs:
if (event_msg.data == self.event_trigger_dict[event_name]):
bool_list.append(True)
else:
bool_list.append(False)
# Keeps track of latest timestamp
if event_msg.header.stamp >= latest_time_stamp:
latest_time_stamp = event_msg.header.stamp
# Perform logic operation
msg = BoolStamped()
msg.header.stamp = latest_time_stamp
gate = self.gates_dict.get(gate_name)
gate_type = gate.get("gate_type")
if gate_type == "AND":
msg.data = all(bool_list)
elif gate_type == "OR":
msg.data = any(bool_list)
print bool_list
return msg
def cbFinishCoord(self, msg):
#if not self.active:
# return
# Finish coordination switch to next macro task
if msg.data:
# I receive positive confirm, switch to next task
try:
# Try to pop the next macrotask
self.macro_task_index += 1
self.macro_tasks = rospy.get_param("~macro_tasks")
self.current_macro_task = self.macro_tasks[str('macro_task_' + str(self.macro_task_index))][:]
self.current_task = self.current_macro_task.pop()
except:
# Or its the last task
self.macro_task_index = 0
self.macro_tasks = rospy.get_param("~macro_tasks")
self.current_macro_task = self.macro_tasks[str('macro_task_' + str(self.macro_task_index))][:]
self.current_task = self.current_macro_task.pop()
return
else:
# else continue precious macro task
#print self.macro_task_index
self.macro_tasks = rospy.get_param("~macro_tasks")
self.current_macro_task = self.macro_tasks[str('macro_task_' + str(self.macro_task_index))][:]
self.current_task = self.current_macro_task.pop()
# Publish finish message to enter planning state
finish_msg = BoolStamped()
finish_msg.header.stamp = rospy.Time.now()
finish_msg.data = True
self.pub_finish.publish(finish_msg)
def CSMA(self):
print "entering csma"
while self.active:
rospy.loginfo("[%s] activated" % (self.node_name))
flag = BoolStamped()
backoff_time = 0.0 # in seconds
time.sleep(0.5)
if self.DetectPotCollision():
rospy.loginfo("[%s] potential collision" % (self.node_name))
if self.iteration > 0:
backoff_time = randrange(0, 5)
flag.data = False
self.pub_implicit_coordination.publish(flag)
time.sleep(backoff_time)
self.iteration += 1
if self.iteration > self.iteration_threshold:
flag.data = True
self.pub_implicit_coordination.publish(flag)
self.active = False
if self.detected_bots == None or not self.DetectPotCollision():
rospy.loginfo("[%s] no potential" % (self.node_name))
self.iteration = 0
flag.data = True
self.pub_implicit_coordination.publish(flag)
self.active = False
def processImage(self, image_msg):
if self.lock.testandset():
vehicle_detected_msg_out = BoolStamped()
try:
image_cv = self.bridge.imgmsg_to_cv2(image_msg, "bgr8")
except CvBridgeError as e:
print e
start = rospy.Time.now()
params = cv2.SimpleBlobDetector_Params()
params.minArea = self.blobdetector_min_area
params.minDistBetweenBlobs = self.blobdetector_min_dist_between_blobs
simple_blob_detector = cv2.SimpleBlobDetector_create(params)
(detection,
corners) = cv2.findCirclesGrid(image_cv,
self.circlepattern_dims,
flags=cv2.CALIB_CB_SYMMETRIC_GRID,
blobDetector=simple_blob_detector)
elapsed_time = (rospy.Time.now() - start).to_sec()
self.pub_time_elapsed.publish(elapsed_time)
vehicle_detected_msg_out.data = detection
self.pub_detection.publish(vehicle_detected_msg_out)
if self.publish_circles:
cv2.drawChessboardCorners(image_cv, self.circlepattern_dims,
corners, detection)
image_msg_out = self.bridge.cv2_to_imgmsg(image_cv, "bgr8")
self.pub_circlepattern_image.publish(image_msg_out)
self.lock.unlock()
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 image_cb(self, image_msg):
if not self.initialized:
return
if self.frame_id != 0:
return
self.frame_id += 1
from integration import NUMBER_FRAMES_SKIPPED
self.frame_id = self.frame_id % (1 + NUMBER_FRAMES_SKIPPED())
# Decode from compressed image with OpenCV
try:
image = self.bridge.compressed_imgmsg_to_cv2(image_msg)
except ValueError as e:
self.logerr('Could not decode image: %s' % e)
return
# Perform color correction
if self.ai_thresholds_received:
image = self.ai.apply_color_balance(
self.anti_instagram_thresholds["lower"],
self.anti_instagram_thresholds["higher"], image)
image = cv2.resize(image, (416, 416))
bboxes, classes, scores = self.model_wrapper.predict(image)
msg = BoolStamped()
msg.header = image_msg.header
msg.data = self.det2bool(bboxes, classes, scores)
self.pub_obj_dets.publish(msg)
def publishDoneMsg(self):
if self.mode == "INTERSECTION_CONTROL":
msg = BoolStamped()
msg.header.stamp = rospy.Time.now()
msg.data = True
self.pub_done.publish(msg)
rospy.loginfo("[%s] interesction_done!" % (self.node_name))
def pose2(data):
global min_dist_road, time_out, flag, pub_too_close
# Get the distance away from the duckiebot
dis = data.position.z
rospy.loginfo('Tag 2 (Obstacle blocking road) detected.')
# If we aren't too close yet, its okay
if dis > min_dist_road:
rospy.loginfo('Tag 2 is still far away enough: %s m', dis)
# If we are too close, publish true to too_close
else:
# Set up a BoolStamped to publish
b = BoolStamped()
b.data = True
# Print a message
rospy.loginfo('Tag 2 is too close: %s m', dis)
# Publish to the too_close topic
pub_too_close.publish(b)
# Reset the timer and tell timer to start counting
time_out = 0
flag = True
def cbNextGoal(self, msg):
if not msg.data:
return
if self.fsm_state != "LANE_FOLLOWING":
return
# Pop next goal on path
try:
# If we can pop another goal
print "pop!"
self.goal = self.path.pop()
goal_msg = Pose2DStamped()
goal_msg.header.stamp = rospy.Time.now()
goal_msg.x = self.goal[0]
goal_msg.y = self.goal[1]
goal_msg.theta = self.goal[2]
self.pub_goal.publish(goal_msg)
#self.Rate.sleep()
except:
# Cannot pop another goal, reach destination
# Stop the robot and wait for coordination
goal_msg = Pose2DStamped()
goal_msg.header.stamp = rospy.Time.now()
goal_msg.x = 0.0
goal_msg.y = 0.0
goal_msg.theta = 0.0
self.pub_goal.publish(goal_msg)
debug = False
if debug:
self.path = [(0.0, 0.0, 0.0), (1.0, 0.0, 0.0), (1.0, 1.0, 0.0),
(0.0, 1.0, 0.0)]
else:
reach_msg = BoolStamped()
reach_msg.header.stamp = rospy.Time.now()
reach_msg.data = True
self.pub_reach_dest.publish(reach_msg)
def lcd_off(self):
override_msg = BoolStamped()
override_msg.data = False
self.pub_lcd_X.publish(override_msg)
self.pub_lcd_A.publish(override_msg)
self.pub_lcd_B.publish(override_msg)
self.pub_lcd_Y.publish(override_msg)
def timer():
global time_out, flag, pub_too_close
b = BoolStamped()
b.data = False
# Essentially a while(true) loop
while not rospy.is_shutdown():
# Sleep 1 second
rospy.sleep(1)
# If the last message published was true
if flag == True:
# Increase the time
time_out += 1
rospy.loginfo('Incrementing time out!')
# If no message has been published in 5 seconds
# publish "false" to the topic "too close"
if time_out >= 5:
pub_too_close.publish(b)
time_out = 0
flag = False
rospy.loginfo('Timed out!')
def cbMode(self, msg):
if msg.state == "SLIM_PARKING":
rospy.set_param('/articuno/lane_controller_node/v_bar', 0.14)
rospy.set_param('/articuno/lane_controller_node/k_theta', -0.5)
rospy.set_param('/articuno/stop_line_filter_node/stop_distance',
0.15)
self.active_mode = True
#if msg.state == "LOOKING_FOR_PARKING":
if msg.state == "PARKED":
rospy.sleep(self.park_timeout / 2)
park_timeout_expired = BoolStamped()
#park_timeout_expired.header.stamp = 0
park_timeout_expired.data = True
self.pub_exit_from_parking.publish(park_timeout_expired)
#self.wait(self.park_timeout)
if msg.state == "EXITING_FROM_PARKING":
rospy.set_param('/articuno/lane_controller_node/v_bar', -0.05)
rospy.set_param('/articuno/lane_controller_node/k_theta', -0.04)
if msg.state == "LANE_FOLLOWING":
rospy.set_param('/articuno/lane_controller_node/v_bar', 0.23)
rospy.set_param('/articuno/lane_controller_node/k_theta', -1)
rospy.set_param('/articuno/stop_line_filter_node/stop_distance',
0.22)
self.filter.parking_detected = False
self.active_mode = False
def publish_topics(self):
now = rospy.Time.now()
# Publish LEDs
self.roof_light_pub.publish(self.roof_light)
# Clearance to go
self.clearance_to_go_pub.publish(
CoordinationClearance(status=self.clearance_to_go))
# Publish intersection_go flag
# rospy.loginfo("clearance_to_go is "+str(self.clearance_to_go) + " and CoordinationClearance.GO is "+str(CoordinationClearance.GO))
if self.clearance_to_go == CoordinationClearance.GO and not self.intersection_go_published:
msg = BoolStamped()
msg.header.stamp = now
msg.data = True
self.pub_intersection_go.publish(msg)
self.intersection_go_published = True
rospy.loginfo('[%s] Go!' % (self.node_name))
# Publish LEDs
# self.roof_light_pub.publish(self.roof_light)
car_cmd_msg = Twist2DStamped(v=0.0, omega=0.0)
car_cmd_msg.header.stamp = now
self.pub_coord_cmd.publish(car_cmd_msg)
self.coordination_state_pub.publish(data=self.state)
def InitializePath(self):
# waiting for instructions where to go
#if self.turn_type < 0 or (rospy.Time.now() - self.turn_type_time).to_sec() > self.turn_type_timeout:
# rospy.loginfo("[%s] No current turn type information." % (self.node_name))
# return False
#TODO uncomment
pose_init, _ = self.poseEstimator.PredictState(rospy.Time.now())
rospy.loginfo("The duck will go: " + str(self.turn_type))
pose_final = self.ComputeFinalPose(self.current_tag_info,
self.turn_type)
rospy.loginfo(
"[%s] Planning path from (%f,%f,%f) to (%f,%f,%f)." %
(self.node_name, pose_init[0], pose_init[1], pose_init[2],
pose_final[0], pose_final[1], pose_final[2]))
if not self.pathPlanner.PlanPath(pose_init, pose_final):
rospy.loginfo("[%s] Could not compute feasible path." %
(self.node_name))
return False
else:
msg_path_computed = BoolStamped()
msg_path_computed.header.stamp = rospy.Time.now()
msg_path_computed.data = True
self.pub_path_computed.publish(msg_path_computed)
msg_path_computed.data = False
return True
def control_car(data):
# Only run when the road is blocked
if data.data == True:
global pub_road_blocked, pub_cmd
# Array of the t,v,w commands
turns = [[2, .15, 3]]
# Create a list of the t,v,w commands to go around
manuever = list()
for turn in turns:
manuever.append((turn[0], Twist2DStamped(v=turn[1],
omega=turn[2])))
# Run each manuever (from open_loop_control_intersection_node code)
for index, pair in enumerate(manuever):
cmd = copy.deepcopy(pair[1])
start_time = rospy.Time.now()
end_time = start_time + rospy.Duration.from_sec(pair[0])
while rospy.Time.now() < end_time:
cmd.header.stamp = rospy.Time.now()
pub_cmd.publish(cmd)
# Tell fsm that we are done turning around
b = BoolStamped()
b.header.stamp = rospy.Time.now()
b.data = False
# Publish to the road_blocked topic to be read by fsm state
pub_road_blocked.publish(b)
