def pubCoilsonMinefield(self):
if self.listener == None:
return
# Lookup for the coils using tf
try:
(trans,
rot) = self.listener.lookupTransform('minefield', 'left_coil',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
leftCoilX = trans[0] / self.resolution + self.numCellsX / 2.0
leftCoilY = -trans[1] / self.resolution + self.numCellsY / 2.0
try:
(trans,
rot) = self.listener.lookupTransform('minefield', 'middle_coil',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
middleCoilX = trans[0] / self.resolution + self.numCellsX / 2.0
middleCoilY = -trans[1] / self.resolution + self.numCellsY / 2.0
try:
(trans,
rot) = self.listener.lookupTransform('minefield', 'right_coil',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
rightCoilX = trans[0] / self.resolution + self.numCellsX / 2.0
rightCoilY = -trans[1] / self.resolution + self.numCellsY / 2.0
coils = [[leftCoilX, leftCoilY]]
coils.append([middleCoilX, middleCoilY])
coils.append([rightCoilX, rightCoilY])
co = 0
for x, y in coils:
coil = Coil()
coil.header.frame_id = "{}_coil".format(
["left", "middle", "right"][co])
if x <= self.mineMap.shape[
2] and x >= 0 and y <= self.mineMap.shape[1] and y >= 0:
for ch in range(3):
coil.channel.append(self.mineMap[3 * co + ch, y, x] +
random.random() * 100)
coil.zero.append(self.zeroChannel[3 * co + ch])
self.pubMineDetection.publish(coil)
co += 1
# Robot data
t = Twist()
pose = Pose()
ekfOdom = Odometry()
radStep = deg2rad(15)
transListener = None
#laser information
laserInfo = LaserScan()
#Inertial Unit
imuInfo = Imu()
#Metal detector data
leftCoil = Coil()
middleCoil = Coil()
rightCoil = Coil()
# Mine Detection Callback
def receiveCoilSignal(actualCoil):
if actualCoil.header.frame_id == "left_coil":
global leftCoil
leftCoil = actualCoil
elif actualCoil.header.frame_id == "middle_coil":
global middleCoil
middleCoil = actualCoil
elif actualCoil.header.frame_id == "right_coil":
global rightCoil
rightCoil = actualCoil
def updateRobotPose(self, newPose):
robotX, robotY, yaw = newPose
if self.listener == None:
return
# Lookup for the coils using tf
try:
(trans,
rot) = self.listener.lookupTransform('base_footprint',
'left_coil', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
leftCoilX = trans[0] * cos(yaw) - trans[1] * sin(yaw)
leftCoilY = trans[0] * sin(yaw) + trans[1] * cos(yaw)
try:
(trans,
rot) = self.listener.lookupTransform('base_footprint',
'middle_coil', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
middleCoilX = trans[0] * cos(yaw) - trans[1] * sin(yaw)
middleCoilY = trans[0] * sin(yaw) + trans[1] * cos(yaw)
try:
(trans,
rot) = self.listener.lookupTransform('base_footprint',
'right_coil', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
rightCoilX = trans[0] * cos(yaw) - trans[1] * sin(yaw)
rightCoilY = trans[0] * sin(yaw) + trans[1] * cos(yaw)
actualCoilPose = [middleCoilX, middleCoilY]
x, y = robotX, robotY
lastPose = None
if len(self.path) > 0:
lastPose = self.path[-1]
radRange = deg2rad(10)
if self.lastCoilPose == None or distance(
self.lastCoilPose, actualCoilPose) > self.cellXSize * 2 or (
lastPose != None and
(distance(lastPose, newPose) > 0.1
or not -radRange < abs(yaw - lastPose[2]) < radRange)):
self.lastCoilPose = actualCoilPose
y, x = (int((self.height / 2 - y) / self.cellYSize),
int((self.width / 2 + x) / self.cellXSize))
coils = [[
x + leftCoilX / self.cellXSize, y - leftCoilY / self.cellXSize
]]
coils.append([
x + middleCoilX / self.cellXSize,
y - middleCoilY / self.cellXSize
])
coils.append([
x + rightCoilX / self.cellXSize,
y - rightCoilY / self.cellXSize
])
coilsPose = [[leftCoilX, leftCoilY], [middleCoilX, middleCoilY],
[rightCoilX, rightCoilY]]
sensorArea = round(.18 / self.cellXSize)
radius = sensorArea #meters
y1, x1 = np.ogrid[-radius:radius, -radius:radius]
mask = x1**2 + y1**2 <= radius**2
co = 0
signals = []
for x, y in coils:
if x + radius <= self.map.shape[
1] and x - radius >= 0 and y + radius <= self.map.shape[
0] and y - radius >= 0:
self.map[y - radius:y + radius,
x - radius:x + radius][mask] = 183
coil = Coil()
coil.header.frame_id = "{}_coil".format(
["left", "middle", "right"][co])
if x <= self.mineMap.shape[
2] and x >= 0 and y <= self.mineMap.shape[1] and y >= 0:
for ch in range(3):
coil.channel.append(self.mineMap[3 * co + ch, y, x] +
random.random() * 100)
coil.zero.append(self.zeroChannel[3 * co + ch])
if (self.isSimulation):
self.pubMineDetection.publish(coil)
signals.append(coil)
co += 1
self.emitCoilSignal.emit(signals)
self.emitMap.emit([self.mineMap, self.map, coilsPose])
if lastPose == None or distance(
lastPose, newPose
) > 0.1 or not -radRange < abs(yaw - lastPose[2]) < radRange:
self.path.append(newPose)
self.receivedRobotPos.emit(self.path)
if self.pubPose != None:
pose = Pose()
pose.position.x, pose.position.y = robotX, robotY
pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w = quaternion_from_euler(
0., 0., yaw)
self.pubPose.publish(pose)
wheels = [[
robotX + .272 * cos(yaw) + .2725 * sin(yaw),
robotY - .272 * sin(yaw) + .2725 * cos(yaw)
],
[
robotX - .272 * cos(yaw) + .2725 * sin(yaw),
robotY + .272 * sin(yaw) + .2725 * cos(yaw)
],
[
robotX - .272 * cos(yaw) - .2725 * sin(yaw),
robotY + .272 * sin(yaw) - .2725 * cos(yaw)
],
[
robotX + .272 * cos(yaw) - .2725 * sin(yaw),
robotY - .272 * sin(yaw) - .2725 * cos(yaw)
]]
for mine in self.mines:
for wheel in wheels:
if distance(wheel, mine) < self.maxDistExplosion:
if not mine in self.minesExploded:
self.minesExploded.append(mine)
self.receivedMineExplodedPos.emit(
self.minesExploded)
transListener = None
# Robot data
robotTwist = Twist()
robotPose = PoseStamped()
leftCoilPose = PoseStamped()
#laser information
laserInfo = LaserScan()
laserInfoHokuyo = LaserScan()
#Inertial Unit
imuInfo = Imu()
#Metal detector data
coils = Coil()
######################### AUXILIARY FUNCTIONS ############################
# Get a transformation matrix from a geometry_msgs/Pose
def matrix_from_pose_msg(pose):
t = transformations.translation_matrix(
(pose.position.x, pose.position.y, pose.position.z))
q = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
r = transformations.quaternion_matrix(q)
return transformations.concatenate_matrices(t, r)