def callback(self, msg):
patrol_bot = PatrolBot()
patrol_bot.name = self.veh
#detect_msg = BoolStamped()
#detect_msg.data = True
#self.pub_detected.publish(detect_msg)
# Load tag detections message
for detection in msg.detections:
# ------ start tag info processing
new_info = TagInfo()
new_info.id = int(detection.id)
id_info = self.tags_dict[new_info.id]
if detection.pose.pose.orientation.y <= 0.5:
patrol_bot.direction = "cw"
else:
patrol_bot.direction = "ccw"
patrol_bot.id = new_info.id
print "detect id ", new_info.id
'''if new_info.id == 1:
patrol_bot.direction = "cw"
patrol_bot.id = 1
print "detect tag 1"
elif new_info.id == 2:
patrol_bot.direction = "cw"
patrol_bot.id = 2
print "detect tag 2"
elif new_info.id == 3:
patrol_bot.direction = "cw"
patrol_bot.id = 3
print "detect tag 3"
elif new_info.id == 4:
patrol_bot.direction = "cw"
patrol_bot.id = 4
print "detect tag 4"
elif new_info.id == 5:
patrol_bot.direction = "ccw"
patrol_bot.id = 1
print "detect tag 5"
elif new_info.id == 6:
patrol_bot.direction = "ccw"
patrol_bot.id = 2
print "detect tag 6"
elif new_info.id == 7:
patrol_bot.direction = "ccw"
patrol_bot.id = 3
print "detect tag 7"
elif new_info.id == 8:
patrol_bot.direction = "ccw"
patrol_bot.id = 4
print "detect tag 8"
else:
continue'''
self.pub_info.publish(patrol_bot)
def __init__(self):
""" """
self.node_name = rospy.get_name()
self.veh = rospy.get_param('~veh')
self.node_name = "apriltags_follow_node"
# -------- Start adding back the tag info stuff --------
rospack = rospkg.RosPack()
self.pkg_path = rospack.get_path('apriltags_ros')
tags_filepath = self.setupParam(
"~tags_file", self.pkg_path + "/apriltagsDB/apriltagsDB.yaml"
) # No tags_file input atm., so default value is used
self.loc = self.setupParam("~loc", -1) # -1 if no location is given
tags_file = open(tags_filepath, 'r')
self.tags_dict = yaml.load(tags_file)
tags_file.close()
self.info = TagInfo()
# -------- publisher --------
self.sub_prePros = rospy.Subscriber("~apriltags_in",
AprilTags,
self.callback,
queue_size=1)
# -------- Subscriber --------
self.pub_pose = rospy.Publisher("~target_pose",
VehiclePose,
queue_size=1)
def __init__(self):
""" """
self.veh = rospy.get_param('~veh')
self.node_name = "apriltags_postprocessing_node"
# -------- Start adding back the tag info stuff
rospack = rospkg.RosPack()
self.pkg_path = rospack.get_path('apriltags_ros')
tags_filepath = self.setupParam(
"~tags_file", self.pkg_path + "/apriltagsDB/apriltagsDB.yaml"
) # No tags_file input atm., so default value is used
self.loc = self.setupParam("~loc", -1) # -1 if no location is given
tags_file = open(tags_filepath, 'r')
self.tags_dict = yaml.load(tags_file)
tags_file.close()
self.info = TagInfo()
# ---- end tag info stuff
#self.sub_mode = rospy.Subscriber("~mode",FSMState, self.processStateChange)
self.pub_info = rospy.Publisher("~info", PatrolBot, queue_size=1)
#self.pub_detected = rospy.Publisher("~detected", BoolStamped, queue_size=1)
self.sub_prePros = rospy.Subscriber("~apriltags_in",
AprilTagDetectionArray,
self.callback,
queue_size=1)
def __init__(self):
""" """
self.node_name = "apriltag_postprocessing_node"
# Load parameters
self.camera_x = self.setupParam("~camera_x", 0.065)
self.camera_y = self.setupParam("~camera_y", 0.0)
self.camera_z = self.setupParam("~camera_z", 0.11)
self.camera_theta = self.setupParam("~camera_theta", 19.0)
self.scale_x = self.setupParam("~scale_x", 1)
self.scale_y = self.setupParam("~scale_y", 1)
self.scale_z = self.setupParam("~scale_z", 1)
# -------- Start adding back the tag info stuff
tags_filepath = self.setupParam("~tags_file")
self.loc = self.setupParam("~loc", -1) # -1 if no location is given
tags_file = open(tags_filepath, 'r')
self.tags_dict = yaml.load(tags_file)
tags_file.close()
self.info = TagInfo()
self.sign_types = {"StreetName": self.info.S_NAME,
"TrafficSign": self.info.SIGN,
"Light": self.info.LIGHT,
"Localization": self.info.LOCALIZE,
"Vehicle": self.info.VEHICLE}
self.traffic_sign_types = {"stop": self.info.STOP,
"yield": self.info.YIELD,
"no-right-turn": self.info.NO_RIGHT_TURN,
"no-left-turn": self.info.NO_LEFT_TURN,
"oneway-right": self.info.ONEWAY_RIGHT,
"oneway-left": self.info.ONEWAY_LEFT,
"4-way-intersect": self.info.FOUR_WAY,
"right-T-intersect": self.info.RIGHT_T_INTERSECT,
"left-T-intersect": self.info.LEFT_T_INTERSECT,
"T-intersection": self.info.T_INTERSECTION,
"do-not-enter": self.info.DO_NOT_ENTER,
"pedestrian": self.info.PEDESTRIAN,
"t-light-ahead": self.info.T_LIGHT_AHEAD,
"duck-crossing": self.info.DUCK_CROSSING,
"parking": self.info.PARKING}
# ---- end tag info stuff
self.sub_prePros = rospy.Subscriber("~apriltags_in", AprilTagDetectionArray, self.callback, queue_size=1)
self.pub_postPros = rospy.Publisher("~apriltags_out", AprilTagsWithInfos, queue_size=1)
self.pub_visualize = rospy.Publisher("~tag_pose", PoseStamped, queue_size=1)
# topics for state machine
self.pub_postPros_parking = rospy.Publisher("~apriltags_parking", BoolStamped, queue_size=1)
self.pub_postPros_intersection = rospy.Publisher("~apriltags_intersection", BoolStamped, queue_size=1)
rospy.loginfo("[%s] has started", self.node_name)
def callback(self, msg):
patrol_bot = PatrolBot()
patrol_bot.name = self.veh
#detect_msg = BoolStamped()
#detect_msg.data = True
#self.pub_detected.publish(detect_msg)
# Load tag detections message
for detection in msg.detections:
# ------ start tag info processing
new_info = TagInfo()
new_info.id = int(detection.id)
id_info = self.tags_dict[new_info.id]
if detection.pose.pose.orientation.y <= 0.5:
patrol_bot.direction = "cw"
else:
patrol_bot.direction = "ccw"
patrol_bot.id = new_info.id
print "detect id ", new_info.id
self.pub_info.publish(patrol_bot)
def __init__(self):
""" """
self.node_name = rospy.get_name()
rospack = rospkg.RosPack()
self.pkg_path = rospack.get_path('apriltags')
tags_filepath = self.setupParam(
"~tags_file", self.pkg_path + "/apriltagsDB/apriltagsDB.yaml"
) # No tags_file input atm., so default value is used
self.loc = self.setupParam("~loc", -1) # -1 if no location is given
tags_file = open(tags_filepath, 'r')
self.tags_dict = yaml.load(tags_file)
tags_file.close()
self.info = TagInfo()
self.sign_types = {
"StreetName": self.info.S_NAME,
"TrafficSign": self.info.SIGN,
"Light": self.info.LIGHT,
"Localization": self.info.LOCALIZE,
"Vehicle": self.info.VEHICLE
}
self.traffic_sign_types = {
"stop": self.info.STOP,
"yield": self.info.YIELD,
"no-right-turn": self.info.NO_RIGHT_TURN,
"no-left-turn": self.info.NO_LEFT_TURN,
"oneway-right": self.info.ONEWAY_RIGHT,
"oneway-left": self.info.ONEWAY_LEFT,
"4-way-intersect": self.info.FOUR_WAY,
"right-T-intersect": self.info.RIGHT_T_INTERSECT,
"left-T-intersect": self.info.LEFT_T_INTERSECT,
"T-intersection": self.info.T_INTERSECTION,
"do-not-enter": self.info.DO_NOT_ENTER,
"pedestrian": self.info.PEDESTRIAN,
"t-light-ahead": self.info.T_LIGHT_AHEAD,
"duck-crossing": self.info.DUCK_CROSSING
}
self.sub_prePros = rospy.Subscriber("~apriltags_in",
AprilTags,
self.callback,
queue_size=1)
self.pub_postPros = rospy.Publisher("~apriltags_out",
AprilTags,
queue_size=1)
def __init__(self):
""" """
self.node_name = "apriltags_postprocessing_node"
self.turnright = False
self.turnleft = False
self.turnaround = False
# Load parameters
self.camera_x = self.setupParam("~camera_x", 0.065)
self.camera_y = self.setupParam("~camera_y", 0.0)
self.camera_z = self.setupParam("~camera_z", 0.11)
self.camera_theta = self.setupParam("~camera_theta", 19.0)
self.scale_x = self.setupParam("~scale_x", 1)
self.scale_y = self.setupParam("~scale_y", 1)
self.scale_z = self.setupParam("~scale_z", 1)
# -------- Start adding back the tag info stuff
rospack = rospkg.RosPack()
self.pkg_path = rospack.get_path('apriltags_ros')
tags_filepath = self.setupParam(
"~tags_file", self.pkg_path + "/apriltagsDB/apriltagsDB.yaml"
) # No tags_file input atm., so default value is used
self.loc = self.setupParam("~loc", -1) # -1 if no location is given
tags_file = open(tags_filepath, 'r')
self.tags_dict = yaml.load(tags_file)
tags_file.close()
self.info = TagInfo()
self.sign_types = {
"StreetName": self.info.S_NAME,
"TrafficSign": self.info.SIGN,
"Light": self.info.LIGHT,
"Localization": self.info.LOCALIZE,
"Vehicle": self.info.VEHICLE
}
self.traffic_sign_types = {
"stop": self.info.STOP,
"yield": self.info.YIELD,
"no-right-turn": self.info.NO_RIGHT_TURN,
"no-left-turn": self.info.NO_LEFT_TURN,
"oneway-right": self.info.ONEWAY_RIGHT,
"oneway-left": self.info.ONEWAY_LEFT,
"4-way-intersect": self.info.FOUR_WAY,
"right-T-intersect": self.info.RIGHT_T_INTERSECT,
"left-T-intersect": self.info.LEFT_T_INTERSECT,
"T-intersection": self.info.T_INTERSECTION,
"do-not-enter": self.info.DO_NOT_ENTER,
"pedestrian": self.info.PEDESTRIAN,
"t-light-ahead": self.info.T_LIGHT_AHEAD,
"duck-crossing": self.info.DUCK_CROSSING
}
# ---- end tag info stuff
self.sub_mode = rospy.Subscriber("~mode", FSMState,
self.processStateChange)
self.pub_turn_right = rospy.Publisher("~turn_right",
BoolStamped,
queue_size=1)
self.pub_turn_left = rospy.Publisher("~turn_left",
BoolStamped,
queue_size=1)
self.pub_turn_around = rospy.Publisher("~turn_around",
BoolStamped,
queue_size=1)
self.pub_detected = rospy.Publisher("~detected",
BoolStamped,
queue_size=1)
self.sub_prePros = rospy.Subscriber("~apriltags_in",
AprilTagDetectionArray,
self.callback,
queue_size=1)
self.pub_postPros = rospy.Publisher("~apriltags_out",
AprilTagsWithInfos,
queue_size=1)
self.pub_visualize = rospy.Publisher("~tag_pose",
PoseStamped,
queue_size=1)
#self.sub_my = rospy.Subscriber("~apriltags_out", AprilTagsWithInfos, self.cb, queue_size=1)
rospy.loginfo("[%s] has started", self.node_name)
def callback(self, msg):
detect_msg = BoolStamped()
detect_msg.data = True
self.pub_detected.publish(detect_msg)
tag_infos = []
# Load tag detections message
for detection in msg.detections:
# ------ start tag info processing
new_info = TagInfo()
new_info.id = int(detection.id)
id_info = self.tags_dict[new_info.id]
if new_info.id == 1:
self.turnright = True
self.turnleft = False
self.turnaround = False
#print "---------Tag 1---------Turn Right-----------"
elif new_info.id == 2:
self.turnright = False
self.turnleft = True
self.turnaround = False
#print "---------Tag 2---------Turn LEFT-----------"
elif new_info.id == 3:
self.turnright = False
self.turnleft = False
self.turnaround = True
print "---------Tag 3---------Turn around-----------"
# Check yaml file to fill in ID-specific information
new_info.tag_type = self.sign_types[id_info['tag_type']]
if new_info.tag_type == self.info.S_NAME:
new_info.street_name = id_info['street_name']
elif new_info.tag_type == self.info.SIGN:
new_info.traffic_sign_type = self.traffic_sign_types[
id_info['traffic_sign_type']]
elif new_info.tag_type == self.info.VEHICLE:
new_info.vehicle_name = id_info['vehicle_name']
# TODO: Implement location more than just a float like it is now.
# location is now 0.0 if no location is set which is probably not that smart
if self.loc == 226:
l = (id_info['location_226'])
if l is not None:
new_info.location = l
elif self.loc == 316:
l = (id_info['location_316'])
if l is not None:
new_info.location = l
tag_infos.append(new_info)
# --- end tag info processing
# Define the transforms
veh_t_camxout = tr.translation_matrix(
(self.camera_x, self.camera_y, self.camera_z))
veh_R_camxout = tr.euler_matrix(0, self.camera_theta * np.pi / 180,
0, 'rxyz')
veh_T_camxout = tr.concatenate_matrices(
veh_t_camxout,
veh_R_camxout) # 4x4 Homogeneous Transform Matrix
camxout_T_camzout = tr.euler_matrix(-np.pi / 2, 0, -np.pi / 2,
'rzyx')
veh_T_camzout = tr.concatenate_matrices(veh_T_camxout,
camxout_T_camzout)
tagzout_T_tagxout = tr.euler_matrix(-np.pi / 2, 0, np.pi / 2,
'rxyz')
#Load translation
trans = detection.pose.pose.position
rot = detection.pose.pose.orientation
camzout_t_tagzout = tr.translation_matrix(
(trans.x * self.scale_x, trans.y * self.scale_y,
trans.z * self.scale_z))
camzout_R_tagzout = tr.quaternion_matrix(
(rot.x, rot.y, rot.z, rot.w))
camzout_T_tagzout = tr.concatenate_matrices(
camzout_t_tagzout, camzout_R_tagzout)
veh_T_tagxout = tr.concatenate_matrices(veh_T_camzout,
camzout_T_tagzout,
tagzout_T_tagxout)
# Overwrite transformed value
(trans.x, trans.y,
trans.z) = tr.translation_from_matrix(veh_T_tagxout)
(rot.x, rot.y, rot.z,
rot.w) = tr.quaternion_from_matrix(veh_T_tagxout)
detection.pose.pose.position = trans
detection.pose.pose.orientation = rot
new_tag_data = AprilTagsWithInfos()
new_tag_data.detections = msg.detections
new_tag_data.infos = tag_infos
# Publish Message
self.pub_postPros.publish(new_tag_data)
def callback(self, msg):
tag_infos = []
new_tag_data = AprilTagsWithInfos()
# Load tag detections message
for detection in msg.detections:
# ------ start tag info processing
new_info = TagInfo()
#Can use id 1 as long as no bundles are used
new_info.id = int(detection.id[0])
id_info = self.tags_dict[new_info.id]
# Check yaml file to fill in ID-specific information
new_info.tag_type = self.sign_types[id_info['tag_type']]
if new_info.tag_type == self.info.S_NAME:
new_info.street_name = id_info['street_name']
elif new_info.tag_type == self.info.SIGN:
new_info.traffic_sign_type = self.traffic_sign_types[
id_info['traffic_sign_type']]
# publish for FSM
# parking apriltag event
msg_parking = BoolStamped()
msg_parking.header.stamp = rospy.Time(0)
if new_info.traffic_sign_type == TagInfo.PARKING:
msg_parking.data = True
else:
msg_parking.data = False
self.pub_postPros_parking.publish(msg_parking)
# intersection apriltag event
msg_intersection = BoolStamped()
msg_intersection.header.stamp = rospy.Time(0)
if (new_info.traffic_sign_type == TagInfo.FOUR_WAY) or (
new_info.traffic_sign_type == TagInfo.RIGHT_T_INTERSECT
) or (new_info.traffic_sign_type == TagInfo.LEFT_T_INTERSECT
) or (new_info.traffic_sign_type
== TagInfo.T_INTERSECTION):
msg_intersection.data = True
else:
msg_intersection.data = False
self.pub_postPros_intersection.publish(msg_intersection)
elif new_info.tag_type == self.info.VEHICLE:
new_info.vehicle_name = id_info['vehicle_name']
# TODO: Implement location more than just a float like it is now.
# location is now 0.0 if no location is set which is probably not that smart
if self.loc == 226:
l = (id_info['location_226'])
if l is not None:
new_info.location = l
elif self.loc == 316:
l = (id_info['location_316'])
if l is not None:
new_info.location = l
tag_infos.append(new_info)
# --- end tag info processing
# Define the transforms
veh_t_camxout = tr.translation_matrix(
(self.camera_x, self.camera_y, self.camera_z))
veh_R_camxout = tr.euler_matrix(0, self.camera_theta * np.pi / 180,
0, 'rxyz')
veh_T_camxout = tr.concatenate_matrices(
veh_t_camxout,
veh_R_camxout) # 4x4 Homogeneous Transform Matrix
camxout_T_camzout = tr.euler_matrix(-np.pi / 2, 0, -np.pi / 2,
'rzyx')
veh_T_camzout = tr.concatenate_matrices(veh_T_camxout,
camxout_T_camzout)
tagzout_T_tagxout = tr.euler_matrix(-np.pi / 2, 0, np.pi / 2,
'rxyz')
#Load translation
trans = detection.pose.pose.pose.position
rot = detection.pose.pose.pose.orientation
header = detection.pose.header
camzout_t_tagzout = tr.translation_matrix(
(trans.x * self.scale_x, trans.y * self.scale_y,
trans.z * self.scale_z))
camzout_R_tagzout = tr.quaternion_matrix(
(rot.x, rot.y, rot.z, rot.w))
camzout_T_tagzout = tr.concatenate_matrices(
camzout_t_tagzout, camzout_R_tagzout)
veh_T_tagxout = tr.concatenate_matrices(veh_T_camzout,
camzout_T_tagzout,
tagzout_T_tagxout)
# Overwrite transformed value
(trans.x, trans.y,
trans.z) = tr.translation_from_matrix(veh_T_tagxout)
(rot.x, rot.y, rot.z,
rot.w) = tr.quaternion_from_matrix(veh_T_tagxout)
new_tag_data.detections.append(
AprilTagDetection(int(detection.id[0]),
float(detection.size[0]),
PoseStamped(header, Pose(trans, rot))))
new_tag_data.infos = tag_infos
# Publish Message
self.pub_postPros.publish(new_tag_data)
def callback(self, msg):
tag_infos = []
# Load tag detections message
for detection in msg.detections:
# ------ start tag info processing
new_info = TagInfo()
new_info.id = int(detection.id)
id_info = self.tags_dict[new_info.id]
# Check yaml file to fill in ID-specific information
new_info.tag_type = self.sign_types[id_info['tag_type']]
if new_info.tag_type == self.info.S_NAME:
new_info.street_name = id_info['street_name']
elif new_info.tag_type == self.info.SIGN:
new_info.traffic_sign_type = self.traffic_sign_types[id_info['traffic_sign_type']]
elif new_info.tag_type == self.info.VEHICLE:
new_info.vehicle_name = id_info['vehicle_name']
# TODO: Implement location more than just a float like it is now.
# location is now 0.0 if no location is set which is probably not that smart
if self.loc == 226:
l = (id_info['location_226'])
if l is not None:
new_info.location = l
elif self.loc == 316:
l = (id_info['location_316'])
if l is not None:
new_info.location = l
tag_infos.append(new_info)
# --- end tag info processing
# Define the transforms
veh_t_camxout = tr.translation_matrix((self.camera_x, self.camera_y, self.camera_z))
veh_R_camxout = tr.euler_matrix(0, self.camera_theta*np.pi/180, 0, 'rxyz')
veh_T_camxout = tr.concatenate_matrices(veh_t_camxout, veh_R_camxout) # 4x4 Homogeneous Transform Matrix
camxout_T_camzout = tr.euler_matrix(-np.pi/2,0,-np.pi/2,'rzyx')
veh_T_camzout = tr.concatenate_matrices(veh_T_camxout, camxout_T_camzout)
tagzout_T_tagxout = tr.euler_matrix(-np.pi/2, 0, np.pi/2, 'rxyz')
#Load translation
trans = detection.pose.pose.position
rot = detection.pose.pose.orientation
camzout_t_tagzout = tr.translation_matrix((trans.x*self.scale_x, trans.y*self.scale_y, trans.z*self.scale_z))
camzout_R_tagzout = tr.quaternion_matrix((rot.x, rot.y, rot.z, rot.w))
camzout_T_tagzout = tr.concatenate_matrices(camzout_t_tagzout, camzout_R_tagzout)
veh_T_tagxout = tr.concatenate_matrices(veh_T_camzout, camzout_T_tagzout, tagzout_T_tagxout)
# Overwrite transformed value
(trans.x, trans.y, trans.z) = tr.translation_from_matrix(veh_T_tagxout)
(rot.x, rot.y, rot.z, rot.w) = tr.quaternion_from_matrix(veh_T_tagxout)
detection.pose.pose.position = trans
detection.pose.pose.orientation = rot
new_tag_data = AprilTagsWithInfos()
new_tag_data.detections = msg.detections
new_tag_data.infos = tag_infos
# Publish Message
self.pub_postPros.publish(new_tag_data)
def __init__(self):
# save the name of the node
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing." % (self.node_name))
self.active = True
# read parameters
self.veh = self.SetupParameter("~veh", "daisy")
# set up path planner, state estimator and localizer
self.intersectionLocalizer = IntersectionLocalizer(self.veh)
self.pathPlanner = PathPlanner(self.veh)
self.poseEstimator = PoseEstimator()
# open-loop / closed-loop
# Open-loop: Path is tracked by its parametrization s.
# CLosed-loop: Path is tracked by the controller designed by devel-controllers group. However the controller
# has been adapted by us. Check lane-controller node.
# If true remap in 00-infrastructure/duckietown/config/baseline/dagu_car/car_cmd_switch_node/default.yaml
self.open_loop = False
# main logic parameters
self.rate = 10 # main logic runs at 10Hz
self.timeout = 1.0
# fsm
self.go = False
# turn type
self.turn_type = -1
self.turn_type_time = rospy.Time()
self.turn_type_timeout = 10.0
# in lane
self.in_lane = False
self.in_lane_time = rospy.Time()
self.in_lane_timeout = 0.5
self.in_lane_wait_time = 1.0
self.state_dict = dict()
for counter, key in enumerate([
'IDLE', 'INITIALIZING_LOCALIZATION', 'INITIALIZING_PATH',
'TRAVERSING', 'DONE', 'ERROR'
]):
self.state_dict.update({key: counter})
self.state = self.state_dict['IDLE']
# auxiliary variables
self.tag_info = TagInfo()
self.intersection_signs = [
self.tag_info.FOUR_WAY, self.tag_info.RIGHT_T_INTERSECT,
self.tag_info.LEFT_T_INTERSECT, self.tag_info.T_INTERSECTION
]
self.intersection_type = 0 # 0: three way intersection, 1: four way intersection
self.current_tag_info = None
# Velocities conversion parameters
self.v_scale = 1.53
self.omega_scale = 4.75
# Set constant velocity
if self.open_loop:
self.v = 0.1
else:
self.v = 0.2 #TODO was 0.2 before
# nominal stop positions: centered in lane, 0.16m in front of center of red stop line,
# 0 relative orientation error
self.nominal_start_positions = {
self.tag_info.FOUR_WAY: [0.400, -0.135, 0.5 * np.pi],
self.tag_info.LEFT_T_INTERSECT: [0.694, 0.400, np.pi],
self.tag_info.RIGHT_T_INTERSECT: [-0.135, 0.121, 0.0 * np.pi],
self.tag_info.T_INTERSECTION: [0.400, -0.135, 0.5 * np.pi]
}
self.nominal_final_positions = [[0.159, 0.0508, -0.5 * np.pi],
[0.508, 0.159, 0.0],
[0.400, 0.508, 0.5 * np.pi],
[0.0508, 0.400, np.pi]]
# set up subscribers
self.sub_fsm = rospy.Subscriber("~fsm",
FSMState,
self.FSMCallback,
queue_size=1)
self.sub_turn_type = rospy.Subscriber("~turn_type",
Int16,
self.TurnTypeCallback,
queue_size=1)
self.sub_img = rospy.Subscriber("~img",
CompressedImage,
self.ImageCallback,
queue_size=1,
buff_size=2**24)
self.sub_pose = rospy.Subscriber("~pose_in",
IntersectionPose,
self.PoseCallback,
queue_size=1)
self.sub_cmd = rospy.Subscriber("~cmds",
Twist2DStamped,
self.CmdCallback,
queue_size=30)
self.sub_in_lane = rospy.Subscriber("~in_lane",
BoolStamped,
self.InLaneCallback,
queue_size=1)
# set up publishers
self.pub_intersection_pose_img = rospy.Publisher("~pose_img_out",
IntersectionPoseImg,
queue_size=1)
self.pub_lane_pose = rospy.Publisher("~intersection_navigation_pose",
LanePose,
queue_size=1)
self.pub_done = rospy.Publisher("~intersection_done",
BoolStamped,
queue_size=1)
# Needed if open loop
self.pub_cmds = rospy.Publisher("~cmds_out",
Twist2DStamped,
queue_size=1)
self.pub_path_computed = rospy.Publisher("~path_computed",
BoolStamped,
queue_size=1)
rospy.loginfo("[%s] Initialized." % (self.node_name))
rospy.set_param("~v_inters", 0.2)
rospy.set_param("~s_max", 0.5)
self.param_timer = rospy.Timer(rospy.Duration.from_sec(1.0),
self.updateParameter)
def callback(self, msg):
""" """
# Load tag detections message
tag_infos = []
for detection in msg.detections:
new_info = TagInfo()
#new_location_info = Vector2D()
new_info.id = int(detection.id)
id_info = self.tags_dict[new_info.id]
# Check yaml file to fill in ID-specific information
new_info.tag_type = self.sign_types[id_info['tag_type']]
if new_info.tag_type == self.info.S_NAME:
new_info.street_name = id_info['street_name']
elif new_info.tag_type == self.info.SIGN:
new_info.traffic_sign_type = self.traffic_sign_types[
id_info['traffic_sign_type']]
elif new_info.tag_type == self.info.VEHICLE:
new_info.vehicle_name = id_info['vehicle_name']
# TODO: Implement location more than just a float like it is now.
# location is now 0.0 if no location is set which is probably not that smart
if self.loc == 226:
l = (id_info['location_226'])
if l is not None:
new_info.location = l
elif self.loc == 316:
l = (id_info['location_316'])
if l is not None:
new_info.location = l
#######################
# Localization stuff
#######################
"""
camera_x = 0.05 # x distance from wheel center
camera_y = 0.0 #
camera_z = 0.1 # height of camera from ground
camera_theta = 15 # degree of rotation arround y axis
"""
camera_x = rospy.get_param("~camera_x")
camera_y = rospy.get_param("~camera_y")
camera_z = rospy.get_param("~camera_z")
camera_theta = rospy.get_param("~camera_theta")
#Load translation
x = detection.transform.translation.x
y = detection.transform.translation.y
z = detection.transform.translation.z
t_tc_Fc = k.Vector(
x, y, z
) # translation tags(t) w/ camera(c) expressed in camera frame (Fc)
#Load rotation
x = detection.transform.rotation.x
y = detection.transform.rotation.y
z = detection.transform.rotation.z
w = detection.transform.rotation.w
e = k.Vector(x, y, z)
Q_Ftag_Fold = k.Quaternion(e, w)
# New tag orientation reference (zero when facing camera) w/ to old tag ref used by the lib
C_Ft_Ftag = k.RotationMatrix(
np.matrix([[0, 0, -1], [-1, 0, 0], [0, 1, 0]]))
Q_Ft_Ftag = C_Ft_Ftag.toQuaternion()
# Rotation of old ref frame used by the lib w/ to camera frame
C_Fold_Fc = k.RotationMatrix(
np.matrix([[0, -1, 0], [0, 0, -1], [1, 0, 0]]))
Q_Fold_Fc = C_Fold_Fc.toQuaternion()
# Camera localization
t_cv_Fv = k.Vector(
camera_x, camera_y, camera_z
) # translation of camera w/ vehicle origin in vehicle frame
C_Fc_Fv = k.euler2RotationMatrix(
0, camera_theta,
0) # Rotation of camera frame w/ vehicle frame
Q_Fc_Fv = C_Fc_Fv.toQuaternion()
# Compute tag orientation in vehicle frame
Q_Ft_Fv = Q_Fc_Fv * Q_Fold_Fc * Q_Ftag_Fold * Q_Ft_Ftag
# Compute position of tag in vehicle frame expressed in vehicle frame
C_Fv_Fc = -C_Fc_Fv # inverse transform
t_tc_Fv = C_Fv_Fc * t_tc_Fc
t_tv_Fv = t_tc_Fv + t_cv_Fv
# Overwrite transformed value
detection.transform.translation.x = t_tv_Fv.x
detection.transform.translation.y = t_tv_Fv.y
detection.transform.translation.z = t_tv_Fv.z
detection.transform.rotation.x = Q_Ft_Fv.e.x
detection.transform.rotation.y = Q_Ft_Fv.e.y
detection.transform.rotation.z = Q_Ft_Fv.e.z
detection.transform.rotation.w = Q_Ft_Fv.n
# Debug Print
#A_read = Q_Ftag_Fold.toAngleAxis()
#A_Ft_Fv = Q_Ft_Fv.toAngleAxis()
#print 'Rotation Read'
#A_read()
#print 'Rotation in Vehicle Frame'
#A_Ft_Fv()
tag_infos.append(new_info)
new_tag_data = AprilTags()
new_tag_data.detections = msg.detections
new_tag_data.infos = tag_infos
# Publish Message
self.pub_postPros.publish(new_tag_data)
