except:
print("Assign appropriate path of waypoint csv file!")
wpts_x = csv_data.values[:,0]
wpts_y = csv_data.values[:,1]
wpts_length = len(wpts_x)
# ROS init
rospy.init_node('wpt_loader')
marker_pub = rospy.Publisher('waypoint_markers', Marker, queue_size=10)
listener = tf.TransformListener()
rate = rospy.Rate(1.0)
while not rospy.is_shutdown():
# Publish wpt as MarkerArray
msg_wpts = Marker()
msg_wpts.header.frame_id= "/map"
msg_wpts.header.stamp= rospy.Time.now()
msg_wpts.ns= "spheres"
msg_wpts.action= Marker.ADD
msg_wpts.pose.orientation.w= 1.0
msg_wpts.id = 0
msg_wpts.type = Marker.SPHERE_LIST
# POINTS markers use x and y scale for width/height respectively
msg_wpts.scale.x = 0.1
msg_wpts.scale.y = 0.1
msg_wpts.scale.z = 0.1
def update(self, x_pos, y_pos, yaw):
global waypoints, waypoint_index, station_keep_flag, stable, lock, lock_aux_pointx, lock_aux_pointy
self.pos_SetPointx = waypoints[waypoint_index][0]
self.pos_SetPointy = waypoints[waypoint_index][1]
# get waypoint index
last_waypoint_index = waypoint_index
if np.sqrt((waypoints[waypoint_index][0] - x_pos) *
(waypoints[waypoint_index][0] - x_pos) +
(waypoints[waypoint_index][1] - y_pos) *
(waypoints[waypoint_index][1] - y_pos)) < 5:
if waypoint_index == waypoints.shape[0] - 1:
waypoint_index = waypoint_index
else:
waypoint_index = waypoint_index + 1
if waypoint_index == waypoints.shape[0] - 1 and np.sqrt(
(waypoints[waypoint_index][0] - x_pos) *
(waypoints[waypoint_index][0] - x_pos) +
(waypoints[waypoint_index][1] - y_pos) *
(waypoints[waypoint_index][1] - y_pos)) < 5:
station_keep_flag = 1
else:
station_keep_flag = 0
if time.time() - self.wait_start > 10:
self.wait_flag = 0
#print "delta t", time.time() - self.wait_start
pos_error = np.sqrt((self.pos_SetPointx - x_pos) *
(self.pos_SetPointx - x_pos) +
(self.pos_SetPointy - y_pos) *
(self.pos_SetPointy - y_pos))
waypoint_error = np.sqrt((waypoints[waypoint_index][0] - x_pos) *
(waypoints[waypoint_index][0] - x_pos) +
(waypoints[waypoint_index][1] - y_pos) *
(waypoints[waypoint_index][1] - y_pos))
if np.abs(
waypoint_error) < 5 and station_keep_flag == 1 and stable == 1:
print "turn to desired yaw"
# lock yaw
if waypoints[waypoint_index][2] != -10:
error = waypoints[waypoint_index][2] - yaw
else:
error = np.arctan2((waypoints[waypoint_index][1] -
waypoints[waypoint_index - 1][1]),
(waypoints[waypoint_index][0] -
waypoints[waypoint_index - 1][0])) - yaw
else:
error = np.arctan2((self.SetPointy - y_pos),
(self.SetPointx - x_pos)) - yaw
if waypoint_index >= 1:
last_waypointx = waypoints[waypoint_index - 1][0]
last_waypointy = waypoints[waypoint_index - 1][1]
else:
if lock_aux_pointx == 0:
lock_aux_pointx = x_pos
lock_aux_pointy = y_pos
last_waypointx = lock_aux_pointx
last_waypointy = lock_aux_pointy
if np.abs(waypoints[waypoint_index][0] - last_waypointx) < 1:
dist_to_line = np.abs(x_pos - last_waypointx)
tmp_x = last_waypointx
tmp_y = y_pos
dir_vectorx = 0
dir_vectory = 1
else:
line_slope = (waypoints[waypoint_index][1] - last_waypointy) / (
waypoints[waypoint_index][0] - last_waypointx)
line_intercept = waypoints[waypoint_index][
1] - line_slope * waypoints[waypoint_index][0]
dist_to_line = np.abs(line_slope * x_pos + line_intercept -
y_pos) / np.sqrt(line_slope * line_slope + 1)
tmp_x = x_pos + (line_slope / np.sqrt(line_slope * line_slope + 1)
) * dist_to_line
tmp_y = y_pos - dist_to_line
#print line_slope, line_intercept, dist_to_line
if np.abs(line_slope * tmp_x + line_intercept - tmp_y) / np.sqrt(
line_slope * line_slope + 1) > dist_to_line:
tmp_x = x_pos - (line_slope / np.sqrt(line_slope * line_slope +
1)) * dist_to_line
tmp_y = y_pos + dist_to_line
dir_vectorx = 1 / np.sqrt(line_slope * line_slope + 1)
dir_vectory = line_slope / np.sqrt(line_slope * line_slope + 1)
#print dir_vectorx, dir_vectory
#print dist_to_line
proc_dist = np.sqrt(5 * 5 - dist_to_line * dist_to_line)
aux_pointx = tmp_x + dir_vectorx * proc_dist
aux_point3x = tmp_x + 3 * dir_vectorx * proc_dist
aux_pointy = tmp_y + dir_vectory * proc_dist
aux_point3y = tmp_y + 3 * dir_vectory * proc_dist
if np.abs((waypoints[waypoint_index][0] - tmp_x) *
(waypoints[waypoint_index][0] - tmp_x) +
(waypoints[waypoint_index][1] - tmp_y) *
(waypoints[waypoint_index][1] - tmp_y)) < np.abs(
(waypoints[waypoint_index][0] - aux_pointx) *
(waypoints[waypoint_index][0] - aux_pointx) +
(waypoints[waypoint_index][1] - aux_pointy) *
(waypoints[waypoint_index][1] - aux_pointy)):
aux_pointx = tmp_x - dir_vectorx * proc_dist
aux_point3x = tmp_x - 3 * dir_vectorx * proc_dist
aux_pointy = tmp_y - dir_vectory * proc_dist
aux_point3y = tmp_y - 3 * dir_vectory * proc_dist
#print np.sqrt((tmp_x - x_pos)*(tmp_x - x_pos)+(tmp_y - y_pos)*(tmp_y - y_pos))
if np.sqrt((tmp_x - x_pos) * (tmp_x - x_pos) + (tmp_y - y_pos) *
(tmp_y - y_pos)) < 5:
if station_keep_flag == 1 and pos_error < 5:
if lock == 0:
self.SetPointx = aux_point3x
self.SetPointy = aux_point3y
aux_pointx = aux_point3x
aux_pointy = aux_point3y
lock = 1
aux_pointx = self.SetPointx
aux_pointy = self.SetPointy
else:
self.SetPointx = aux_pointx
self.SetPointy = aux_pointy
lock = 0
#print "aux", aux_pointx, aux_pointy
wpoints = []
for i in range(1):
p = Point()
p.x = aux_pointx
p.y = aux_pointy
p.z = 0
wpoints.append(p)
marker = Marker()
marker.header.frame_id = "/odom"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.pose.orientation.w = 1
marker.points = wpoints
t = rospy.Duration()
marker.lifetime = t
marker.scale.x = 0.4
marker.scale.y = 0.4
marker.scale.z = 0.4
marker.color.a = 1.0
marker.color.r = 0
marker.color.g = 0
marker.color.b = 1.0
self.pub_aux.publish(marker)
else:
lock = 0
if station_keep_flag == 1:
self.SetPointx = waypoints[waypoint_index][0]
self.SetPointy = waypoints[waypoint_index][1]
tmp_x = waypoints[waypoint_index][0]
tmp_y = waypoints[waypoint_index][1]
else:
if (last_waypointx - x_pos) * (
waypoints[waypoint_index][0] -
last_waypointx) + (last_waypointy - y_pos) * (
waypoints[waypoint_index][1] - last_waypointy) > 0:
self.SetPointx = tmp_x
self.SetPointy = tmp_y
else:
self.SetPointx = waypoints[waypoint_index][0]
self.SetPointy = waypoints[waypoint_index][1]
print "tmp", tmp_x, tmp_y
wpoints = []
for i in range(1):
p = Point()
p.x = tmp_x
p.y = tmp_y
p.z = 0
wpoints.append(p)
marker = Marker()
marker.header.frame_id = "/odom"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.pose.orientation.w = 1
marker.points = wpoints
t = rospy.Duration()
marker.lifetime = t
marker.scale.x = 0.4
marker.scale.y = 0.4
marker.scale.z = 0.4
marker.color.a = 1.0
marker.color.r = 0
marker.color.g = 1.0
marker.color.b = 0
self.pub_aux.publish(marker)
wpoints = []
for i in range(1):
p = Point()
p.x = waypoints[waypoint_index][0]
p.y = waypoints[waypoint_index][1]
p.z = 0
wpoints.append(p)
marker2 = Marker()
marker2.header.frame_id = "/odom"
marker2.type = marker.POINTS
marker2.action = marker.ADD
marker2.pose.orientation.w = 1
marker2.points = wpoints
t = rospy.Duration()
marker2.lifetime = t
marker2.scale.x = 0.4
marker2.scale.y = 0.4
marker2.scale.z = 0.4
marker2.color.a = 1.0
marker2.color.r = 0.5
marker2.color.g = 0
marker2.color.b = 0.5
self.pub_tgwp.publish(marker2)
wpoints = []
for i in range(1):
p = Point()
p.x = last_waypointx
p.y = last_waypointy
p.z = 0
wpoints.append(p)
marker3 = Marker()
marker3.header.frame_id = "/odom"
marker3.type = marker.POINTS
marker3.action = marker.ADD
marker3.pose.orientation.w = 1
marker3.points = wpoints
t = rospy.Duration()
marker3.lifetime = t
marker3.scale.x = 0.4
marker3.scale.y = 0.4
marker3.scale.z = 0.4
marker3.color.a = 1.0
marker3.color.r = 0
marker3.color.g = 0.5
marker3.color.b = 0.5
self.pub_lwp.publish(marker3)
'''
#print np.abs(waypoint_error)
if np.abs(waypoint_error) < 5 and waypoints is not None:
if waypoint_index < waypoints.shape[0]-1:
if self.wait_flag == 0 and station_keep_flag == 0:
waypoint_index = waypoint_index + 1
self.wait_flag = 1
self.wait_start = time.time()
print "target waypoint", waypoint_index
#print waypoint_index, waypoints.shape[0]
else:
station_keep_flag = 1
print "station keep in last waypoint"
'''
#print "error", np.arctan((self.pos_SetPointy - y_pos)/(self.pos_SetPointx - x_pos)) - yaw
if np.abs(error) > np.pi:
if error > 0:
error = error - 2 * np.pi
else:
error = error + 2 * np.pi
if station_keep_flag == 1 and np.abs(waypoint_error) > 5:
if np.abs(error) > np.pi / 2:
if error < 0:
error = error + np.pi
else:
error = error - np.pi
#print "error", error
#print "yaw", yaw
#print "target", np.arctan2((self.pos_SetPointy - y_pos), (self.pos_SetPointx - x_pos))
self.current_time = time.time()
delta_time = self.current_time - self.last_time
delta_error = error - self.last_error
if (delta_time >= self.sample_time):
self.PTerm = self.Kp * error
self.ITerm += error * delta_time
if (self.ITerm < -self.windup_guard):
self.ITerm = -self.windup_guard
elif (self.ITerm > self.windup_guard):
self.ITerm = self.windup_guard
self.DTerm = 0.0
if delta_time > 0:
self.DTerm = delta_error / delta_time
self.last_time = self.current_time
self.last_error = error
self.output = self.PTerm + (self.Ki * self.ITerm) + (self.Kd *
self.DTerm)
def marker_clear_all(frame_id):
# Create a marker which clears all.
marker = Marker()
marker.header.frame_id = frame_id
marker.action = 3 # DELETEALL action.
return marker
def callback(bbox_data, laserScan_data):
nameArray = []
xMinArray = []
yMinArray = []
xMaxArray = []
yMaxArray = []
detected_Objects = [[]]
global id_stock
global markerArray
global subArray
i = 0
#Bounding box verisini kontrol el
if (bbox_data is not None):
for i in range(len(bbox_data.bounding_boxes)):
try:
tmp = bbox_data.bounding_boxes[i].Class
nameArray.append(bbox_data.bounding_boxes[i].Class)
xMinArray.append(bbox_data.bounding_boxes[i].xmin)
yMinArray.append(bbox_data.bounding_boxes[i].ymin)
xMaxArray.append(bbox_data.bounding_boxes[i].xmax)
yMaxArray.append(bbox_data.bounding_boxes[i].ymax)
i += 1
except:
print("hata")
objCount = i
counter = 0
#Laser scan verisinden obje uzakligini bul
for counter in range(objCount):
rangeMin = 380 + ((640 - xMaxArray[counter]) / 2)
rangeMax = 700 - (xMinArray[counter] / 2)
sum = 0
k = 0
'''
for j in range (int(round(rangeMax-rangeMin))):
sum += laserScan_data.ranges[rangeMin+j]
mean = sum/(rangeMax-rangeMin)
'''
midPoint = int(round((rangeMin + rangeMax) / 2))
mean = laserScan_data.ranges[midPoint]
detected_Objects.append([nameArray[counter], mean, midPoint])
i = 1
print("Bulunan obje sayisi: " + str(len(detected_Objects)))
print(detected_Objects)
print(" | Class " + " | Dist | " + "pos |")
while (i < len(detected_Objects)): #Bulunan objeleri yazdir
text = " "
text += str(i) + ".| "
text += str(detected_Objects[i][0])
text += " | "
text += "{:.2f}".format(detected_Objects[i][1])
text += "m | "
text += str(detected_Objects[i][2])
text += " | "
print(text)
#Marker olustur
marker = Marker()
marker.header.frame_id = "/robot/odom"
marker.header.stamp = rospy.get_rostime()
marker.id = id_stock[0]
id_stock.pop(0)
marker.ns = str(detected_Objects[i][0])
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 0.1
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.pose.orientation.w = 1.0
#Robotun global koordinatlarini bul
(trans, rot) = listener.lookupTransform('/robot/odom',
'/robot/base_link',
rospy.Time(0))
tX = trans[0]
tY = trans[1]
tZ = trans[2]
rX = rot[0]
rY = rot[1]
rZ = rot[2]
rW = rot[3]
quaternion = (rX, rY, rZ, rW)
euler = tf.transformations.euler_from_quaternion(quaternion)
print("Trans: " + "{:.2f}".format(tX) + " {:.2f}".format(tY) +
" {:.2f}".format(tZ))
print("Roll: " + "{:.2f}".format(degrees(euler[0])) + " Pitch: " +
"{:.2f}".format(degrees(euler[1])) + " Yaw: " +
"{:.2f}".format(degrees(euler[2])))
#Kameranin merkezine gore obje acisini bul
if (detected_Objects[i][2] > 539):
degree = (float(detected_Objects[i][2]) -
539) * laserScan_data.angle_increment
else:
degree = (539 - float(
detected_Objects[i][2])) * laserScan_data.angle_increment
degree = degrees(degree)
#Global aciyi bul
if (detected_Objects[i][2] < 540):
degree = degree * (-1)
yaw = round(degrees(euler[2]), 2)
globalYaw = yaw + degree
print("ObjeDegree: " + str(degree) + " Yaw: " + str(yaw) +
" Global Yaw: " + str(globalYaw))
xAdd = detected_Objects[i][1] * cos(radians(globalYaw))
yAdd = detected_Objects[i][1] * sin(radians(globalYaw))
marker.pose.position.x = trans[0]
marker.pose.position.y = trans[1]
marker.pose.position.z = 0.2
marker.pose.position.x += xAdd
marker.pose.position.y += yAdd
j = 0
different = True
sameID = -1
pointIncRate = 0.1
pointDecRate = 0.03
#Bulunan obje daha once isaretlenmis mi diye kontrol et
while (j < (len(subArray.markers))):
if (subArray.markers[j].ns == str(detected_Objects[i][0])):
x1 = subArray.markers[j].pose.position.x
x2 = marker.pose.position.x
y1 = subArray.markers[j].pose.position.y
y2 = marker.pose.position.y
distanceBetween = sqrt(pow((x1 - x2), 2) + pow((y1 - y2), 2))
print("Distance between:" + str(distanceBetween))
if (distanceBetween != float("inf")):
if (subArray.markers[j].color.a < 1.0):
if (distanceBetween < 0.1):
print("Color.a " +
str(subArray.markers[j].color.a))
different = False
sameID = j
else:
if (distanceBetween < 1):
print("Color.a2 " +
str(subArray.markers[j].color.a))
different = False
sameID = j
j += 1
if (different):
#marker.id = id_stock[len(id_stock)-1]
#id_stock.pop(len(id_stock)-1)
subArray.markers.append(marker)
else:
id_stock.append(marker.id)
if (subArray.markers[sameID].color.a < 1.0):
subArray.markers[sameID].color.a += pointIncRate
if (subArray.markers[sameID].color.a >= 1.0):
#subArray.markers[sameID].text= str(detected_Objects[i][0])
tmpMarker = Marker()
tmpMarker.header.frame_id = "/robot/odom"
tmpMarker.header.stamp = rospy.get_rostime()
tmpMarker.id = subArray.markers[sameID].id
tmpMarker.ns = "textmarker"
tmpMarker.type = marker.TEXT_VIEW_FACING
tmpMarker.action = marker.ADD
tmpMarker.scale.x = 0.2
tmpMarker.scale.y = 0.2
tmpMarker.scale.z = 0.2
tmpMarker.color.a = 1.0
tmpMarker.color.r = 0.0
tmpMarker.color.g = 0.0
tmpMarker.color.b = 1.0
#print("MarkerPoseZ: " + str(subArray.markers[sameID].pose.position.z) + " TextPoseZ: " + str(tmpMarker.pose.position.z))
tmpMarker.pose.orientation.w = 1.0
tmpMarker.pose.position.x = subArray.markers[
sameID].pose.position.x
tmpMarker.pose.position.y = subArray.markers[
sameID].pose.position.y
tmpMarker.pose.position.z = subArray.markers[
sameID].pose.position.z
tmpMarker.pose.position.z += 0.2
if (str(subArray.markers[sameID].ns) == "QR"):
tmpMarker.text = "QR_"
print("i: " + str(i))
tmp_xMin = xMinArray[i - 1]
tmp_yMin = yMinArray[i - 1]
tmp_xMax = xMaxArray[i - 1]
tmp_yMax = yMaxArray[i - 1]
img = rospy.wait_for_message(
"/robot/camera/rgb/image_raw", Image)
cv2_img = bridge.imgmsg_to_cv2(img, "bgr8")
#cv2.imshow("asf",cv2_img)
#time.sleep(1)
#time.sleep(10)
#print("Xmin: " +str(tmp_xMin)+ " yMin: " +str(tmp_yMin)+ " tmp_xMax: " +str(tmp_xMax)+ " tmp_yMax: "+str(tmp_yMax))
if (tmp_xMin > 20):
tmp_xMin -= 20
if (tmp_yMin > 20):
tmp_yMin -= 20
if (tmp_xMax < 620):
tmp_xMax += 20
if (tmp_yMax < 460):
tmp_yMax += 20
#print("Xmin: " +str(tmp_xMin)+ " yMin: " +str(tmp_yMin)+ " tmp_xMax: " +str(tmp_xMax)+ " tmp_yMax: "+str(tmp_yMax))
#print("tmp_yMin: "+ str(tmp_yMin) + " tmp_yMax-tmp_yMin: "+ str(tmp_yMax-tmp_yMin) + "tmp_xMin: "+ str(tmp_xMin) + " tmp_xMax-tmp_xMin" + str(tmp_xMax-tmp_xMin))
cv2_img = cv2_img[tmp_yMin:tmp_yMax, tmp_xMin:tmp_xMax]
#qr = qrtools.QR()
#qr.decode(cv2_img)
qrData = pyzbar.decode(cv2_img)
#print("QR data:" + str(qrData[0][0]))
#time.sleep(10)
#print(cv2_img)
#cv2.imshow("QRimg",cv2_img)
#cv2.waitKey(1)
#img = rospy.Subscriber("/robot/camera/rgb/image_raw", Image, img_callback)
#print("QR hata")
#cv2.imshow(img)
#cv2.waitKey(1)
#time.sleep(10)
if (qrData):
subArray.markers[sameID].color.a = 1.0
subArray.markers[sameID].color.r = 0.0
subArray.markers[sameID].color.g = 1.0
tmpMarker.text += str(qrData[0][0])
subArray.markers.append(tmpMarker)
#markerArray.markers.append(tmpMarker)
print("qr okundu")
else:
subArray.markers[sameID].color.a = 0.2
print("qr okunamadi")
#time.sleep(10)
#time.sleep(10)
else:
subArray.markers[sameID].color.a = 1.0
subArray.markers[sameID].color.r = 0.0
subArray.markers[sameID].color.g = 1.0
tmpMarker.text = subArray.markers[sameID].ns
subArray.markers.append(tmpMarker)
#markerArray.markers.append(tmpMarker)
#print("MarkerPoseZ: " + str(subArray.markers[sameID].pose.position.z) + " TextPoseZ: " + str(tmpMarker.pose.position.z))
#time.sleep(10)
#markerArray.markers.append(subArray.markers[sameID])
print(subArray.markers[sameID])
j = 0
while (j < (len(subArray.markers))):
if (subArray.markers[j].ns == str(
detected_Objects[i][0])):
x1 = subArray.markers[j].pose.position.x
x2 = marker.pose.position.x
y1 = subArray.markers[j].pose.position.y
y2 = marker.pose.position.y
distanceBetween = sqrt(
pow((x1 - x2), 2) + pow((y1 - y2), 2))
if (distanceBetween != float("inf")):
if ((subArray.markers[j].color.a < 1.0)
and (distanceBetween < 1)):
subArray.markers[j].color.a = 0.0
j += 1
i += 1
print("\n")
j = 0
deleteArray = []
marker_pub.publish(subArray)
while j < (len(subArray.markers)):
if ((subArray.markers[j].ns == "QR")
and (subArray.markers[j].color.a >= 1.0)):
print("lensub" + str(len(subArray.markers)) + " id: " +
str(subArray.markers[j].id) + " NS: " +
str(subArray.markers[j].ns) + " text:" +
str(subArray.markers[j].text) + " color.a: " +
str(subArray.markers[j].color.a))
if (subArray.markers[j].color.a < 1.0):
print("dec")
print("lensub" + str(len(subArray.markers)) + " id: " +
str(subArray.markers[j].id) + " NS: " +
str(subArray.markers[j].ns) + " text:" +
str(subArray.markers[j].text) + " color.a: " +
str(subArray.markers[j].color.a))
if (subArray.markers[j] > 0.0):
subArray.markers[j].color.a -= pointDecRate
if (subArray.markers[j].color.a <= 0.0):
deleteArray.append(subArray.markers[j])
j += 1
if (deleteArray is not None):
deleteArray.sort(reverse=True)
for j in range(len(deleteArray)):
id_stock.append(deleteArray[j].id)
subArray.markers.remove(deleteArray[j])
#marker_pub.publish(subArray)
#marker_pub.publish(markerArray)
print("Marker sayisi: " + str(len(markerArray.markers)))
print("id_stock: " + str(len(id_stock)))
def __init__(self, color_saturation=1.0, alpha=1.0, scale=0.1):
"""
The color saturation ranges from 0 to 1. Alpha sets the transparency
and scale scales the size of the ball
"""
reference_frame = rospy.get_param('reference_frame', '/map')
self.marker_pub = rospy.Publisher("visualization_marker",
Marker,
queue_size=10)
self.markerx = Marker()
self.markery = Marker()
self.markerz = Marker()
self.markerx.header.frame_id = reference_frame
self.markery.header.frame_id = reference_frame
self.markerz.header.frame_id = reference_frame
self.markerx.ns = "frame_markers"
self.markery.ns = "frame_markers"
self.markerz.ns = "frame_markers"
self.markerx.id = FrameMarker.id
FrameMarker.id += 1
self.markery.id = FrameMarker.id
FrameMarker.id += 1
self.markerz.id = FrameMarker.id
FrameMarker.id += 1
self.markerx.type = self.markerx.ARROW
self.markery.type = self.markery.ARROW
self.markerz.type = self.markerz.ARROW
self.markerx.action = self.markerx.ADD
self.markery.action = self.markery.ADD
self.markerz.action = self.markerz.ADD
self.markerx.pose.position.x = 0.0
self.markerx.pose.position.y = 0.0
self.markerx.pose.position.z = 0.0
self.markerx.pose.orientation.w = 1.0
self.markerx.pose.orientation.x = 0.0
self.markerx.pose.orientation.y = 0.0
self.markerx.pose.orientation.z = 0.0
self.markery.pose.position.x = 0.0
self.markery.pose.position.y = 0.0
self.markery.pose.position.z = 0.0
self.markery.pose.orientation.w = np.cos(np.pi / 4.0)
self.markery.pose.orientation.x = 0.0
self.markery.pose.orientation.y = 0.0
self.markery.pose.orientation.z = np.sin(np.pi / 4.0)
self.markerz.pose.position.x = 0.0
self.markerz.pose.position.y = 0.0
self.markerz.pose.position.z = 0.0
self.markerz.pose.orientation.w = np.cos(-np.pi / 4.0)
self.markerz.pose.orientation.x = 0.0
self.markerz.pose.orientation.y = np.sin(-np.pi / 4.0)
self.markerz.pose.orientation.z = 0.0
self.markerx.scale.x = scale
self.markerx.scale.y = 0.01
self.markerx.scale.z = 0.01
self.markery.scale.x = scale
self.markery.scale.y = 0.01
self.markery.scale.z = 0.01
self.markerz.scale.x = scale
self.markerz.scale.y = 0.01
self.markerz.scale.z = 0.01
self.markerx.color.r = color_saturation
self.markerx.color.g = 0.0
self.markerx.color.b = 0.0
self.markerx.color.a = alpha
self.markery.color.r = 0.0
self.markery.color.g = color_saturation
self.markery.color.b = 0.0
self.markery.color.a = alpha
self.markerz.color.r = 0.0
self.markerz.color.g = 0.0
self.markerz.color.b = color_saturation
self.markerz.color.a = alpha
self.markerx.lifetime = rospy.Duration()
self.markery.lifetime = rospy.Duration()
self.markerz.lifetime = rospy.Duration()
def callback(msg, tmp_list):
""""""
global old_yaw
[
particle_filter, publisher_position, publisher_mavros,
publisher_particles, broadcaster, publisher_marker
] = tmp_list
# particle filter algorithm
particle_filter.predict() # move particles
# get length of message
num_meas = len(msg.detections)
orientation_yaw_pitch_roll = np.zeros((num_meas, 3))
# if new measurement: update particles
if num_meas >= 1:
measurements = np.zeros((num_meas, 1 + PART_DIM))
# get data from topic /tag_detection
if rviz:
markerArray = MarkerArray()
for i, tag in enumerate(msg.detections):
tag_id = int(tag.id[0])
distance = np.array(([
tag.pose.pose.pose.position.x, tag.pose.pose.pose.position.y,
tag.pose.pose.pose.position.z
]))
measurements[i, 0] = np.linalg.norm(distance)
tmpquat = Quaternion(w=tag.pose.pose.pose.orientation.w,
x=tag.pose.pose.pose.orientation.x,
y=tag.pose.pose.pose.orientation.y,
z=tag.pose.pose.pose.orientation.z)
orientation_yaw_pitch_roll[i, :] = tmpquat.inverse.yaw_pitch_roll
index = np.where(tags[:, 0] == tag_id)
measurements[i, 1:4] = tags[index, 1:4]
# print(measurements[i, 1:4])
if rviz:
marker = Marker()
marker.header.frame_id = "global_tank"
marker.id = i
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = measurements[
i, 0] * 2 # r*2 of distance to camera from tag_14
marker.scale.y = measurements[i, 0] * 2
marker.scale.z = measurements[i, 0] * 2
marker.color.g = 1
marker.color.a = 0.1 # transparency
marker.pose.orientation.w = 1.0
marker.pose.position.x = tags[index, 1][0] # x
marker.pose.position.y = tags[index, 2][0] # y
marker.pose.position.z = tags[index, 3][0] # z
markerArray.markers.append(marker)
if rviz:
# print(len(markerArray.markers))
publisher_marker.publish(markerArray)
# print(index)
particle_filter.update(measurements)
yaw = np.mean(orientation_yaw_pitch_roll[:, 0])
pitch = np.mean(orientation_yaw_pitch_roll[:, 1])
roll = np.mean(orientation_yaw_pitch_roll[:, 2])
else:
yaw = old_yaw
old_yaw = yaw
# print "reale messungen: " + str(measurements)
print("Angle yaw:", yaw * 180 / np.pi)
estimated_orientation = yaw_pitch_roll_to_quat(-(yaw - np.pi / 2), 0, 0)
# estimated_orientation = yaw_pitch_roll_to_quat(yaw, 0, 0)#evtl wrong
# calculate position as mean of particle positions
estimated_position = particle_filter.get_position_estimate()
# [mm]
x_mean_ned = estimated_position[
0] * 1000 # global Tank Coordinate System(NED)
y_mean_ned = estimated_position[1] * 1000
z_mean_ned = estimated_position[2] * 1000
# publish estimated_pose [m] in mavros to /mavros/vision_pose/pose
# this pose needs to be in ENU
mavros_position = PoseStamped()
mavros_position.header.stamp = rospy.Time.now()
mavros_position.header.frame_id = "map"
mavros_position.pose.position.x = y_mean_ned / 1000 # NED Coordinate to ENU(ROS)
mavros_position.pose.position.y = x_mean_ned / 1000
mavros_position.pose.position.z = -z_mean_ned / 1000
mavros_position.pose.orientation.w = estimated_orientation.w
mavros_position.pose.orientation.x = estimated_orientation.x
mavros_position.pose.orientation.y = estimated_orientation.y
mavros_position.pose.orientation.z = estimated_orientation.z
# publish estimated_pose [m]
position = PoseStamped()
position.header.stamp = rospy.Time.now()
position.header.frame_id = "global_tank" # ned
position.pose.position.x = x_mean_ned / 1000
position.pose.position.y = y_mean_ned / 1000
position.pose.position.z = z_mean_ned / 1000
estimated_orientation = yaw_pitch_roll_to_quat(yaw, 0, 0)
position.pose.orientation.w = estimated_orientation.w
position.pose.orientation.x = estimated_orientation.x
position.pose.orientation.y = estimated_orientation.y
position.pose.orientation.z = estimated_orientation.z
publisher_position.publish(position)
# yaw = 0 / 180.0 * np.pi
# tmp = yaw_pitch_roll_to_quat(-(yaw-np.pi/2), 0, 0)
# print(tmp)
# mavros_position.pose.orientation.w = tmp.w
# mavros_position.pose.orientation.x = tmp.x
# mavros_position.pose.orientation.y = tmp.y
# mavros_position.pose.orientation.z = tmp.z
publisher_mavros.publish(mavros_position)
# For Debugging
# mavros_position = PoseStamped()
# mavros_position.header.stamp = rospy.Time.now()
# mavros_position.header.frame_id = "map"
# mavros_position.pose.position.x = 1.0 + np.random.normal(0, 0.01)
# mavros_position.pose.position.y = 2.0 + np.random.normal(0, 0.01)
# mavros_position.pose.position.z = 3.0 + np.random.normal(0, 0.01)
#
# mavros_position.pose.orientation.w = 1.0
# mavros_position.pose.orientation.x = 2.0
# mavros_position.pose.orientation.y = 3.0
# mavros_position.pose.orientation.z = 4.0
# publisher_mavros.publish(mavros_position)
"""
# publish transform
broadcaster.sendTransform((estimated_position[0], estimated_position[1], estimated_position[2]),
(1.0, 0, 0, 0),
rospy.Time.now(),
"TestPose",
"world")
"""
if rviz:
# publish particles as PoseArray
pose_array = PoseArray()
pose_array.header.stamp = rospy.Time.now()
pose_array.header.frame_id = "global_tank"
# for i in range(num_meas):
# print(orientation_yaw_pitch_roll[i, 0] * 180 / np.pi, orientation_yaw_pitch_roll[i, 1] * 180 / np.pi,
# orientation_yaw_pitch_roll[i, 2] * 180 / np.pi)
print("done")
for i in range(particle_filter.NUM_P):
pose = Pose()
pose.position.x = particle_filter.particles[i, 0]
pose.position.y = particle_filter.particles[i, 1]
pose.position.z = particle_filter.particles[i, 2]
# pose.orientation.x =
# pose.orientation.y =
# pose.orientation.z =
# pose.orientation.w =
pose_array.poses.append(pose)
publisher_particles.publish(pose_array)
def publish(self, target_frame, timestamp):
pose = PoseStamped()
pose.header.frame_id = target_frame
pose.header.stamp = timestamp
pose.pose.position.x = self.X[0, 0]
pose.pose.position.y = self.X[1, 0]
pose.pose.position.z = 0.0
Q = tf.transformations.quaternion_from_euler(0, 0, self.X[2, 0])
pose.pose.orientation.x = Q[0]
pose.pose.orientation.y = Q[1]
pose.pose.orientation.z = Q[2]
pose.pose.orientation.w = Q[3]
self.pose_pub.publish(pose)
ma = MarkerArray()
marker = Marker()
marker.header = pose.header
marker.ns = "kf_uncertainty"
marker.id = 5000
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.pose = pose.pose
marker.pose.position.z = -0.1
marker.scale.x = 3 * sqrt(self.P[0, 0])
marker.scale.y = 3 * sqrt(self.P[1, 1])
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 1.0
ma.markers.append(marker)
for id in self.idx.iterkeys():
marker = Marker()
marker.header.stamp = timestamp
marker.header.frame_id = target_frame
marker.ns = "landmark_kf"
marker.id = id
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
l = self.idx[id]
marker.pose.position.x = self.X[l, 0]
marker.pose.position.y = self.X[l + 1, 0]
marker.pose.position.z = -0.1
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 1
marker.pose.orientation.w = 0
marker.scale.x = 3 * sqrt(self.P[l, l])
marker.scale.y = 3 * sqrt(self.P[l + 1, l + 1])
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.lifetime.secs = 3.0
ma.markers.append(marker)
marker = Marker()
marker.header.stamp = timestamp
marker.header.frame_id = target_frame
marker.ns = "landmark_kf"
marker.id = 1000 + id
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.ADD
marker.pose.position.x = self.X[l + 0, 0]
marker.pose.position.y = self.X[l + 1, 0]
marker.pose.position.z = 1.0
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 1
marker.pose.orientation.w = 0
marker.text = str(id)
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.lifetime.secs = 3.0
ma.markers.append(marker)
self.marker_pub.publish(ma)
def callback(msg):
markerArray = MarkerArray()
count = 0
MARKERS_MAX = 1
face_detection = msg.data
arr[1] = arr[0]
arr[0] = face_detection
print arr
marker = Marker()
marker.header.frame_id = "/world"
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = scale
marker.scale.y = scale
marker.scale.z = scale
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.pose.orientation.w = 1.0
##PORTRAIT ONE##
if arr == [1,1]:
marker.pose.position.x = 4.739
marker.pose.position.y = -0.15
marker.pose.position.z = 0.3
# We add the new marker to the MarkerArray, removing the oldest
# marker from it when necessary
if(count > MARKERS_MAX):
markerArray.markers.pop(0)
markerArray.markers.append(marker)
# Renumber the marker IDs
id = 0
for m in markerArray.markers:
m.id = id
id += 1
# Publish the MarkerArray
publisher.publish(markerArray)
count += 1
rospy.sleep(0.01)
#PORTRAIT 2##
if arr == [5,5]:
marker.pose.position.x = -3.156
marker.pose.position.y = -5.9088
marker.pose.position.z = 0.3
# We add the new marker to the MarkerArray, removing the oldest
# marker from it when necessary
if(count > MARKERS_MAX):
markerArray.markers.pop(0)
markerArray.markers.append(marker)
# Renumber the marker IDs
id = 0
for m in markerArray.markers:
m.id = id
id += 1
# Publish the MarkerArray
publisher.publish(markerArray)
count += 1
rospy.sleep(0.01)
##PORTRAIT 3##
if arr == [2,2]:
marker.pose.position.x = 4.7317
marker.pose.position.y = -4.51
marker.pose.position.z = 0.3
# We add the new marker to the MarkerArray, removing the oldest
# marker from it when necessary
if(count > MARKERS_MAX):
markerArray.markers.pop(0)
markerArray.markers.append(marker)
# Renumber the marker IDs
id = 0
for m in markerArray.markers:
m.id = id
id += 1
# Publish the MarkerArray
publisher.publish(markerArray)
count += 1
rospy.sleep(0.01)
##PORTRAIT 4##
if arr == [3,3]:
marker.pose.position.x = 2.3205
marker.pose.position.y = -7.08034
marker.pose.position.z = 0.3
# We add the new marker to the MarkerArray, removing the oldest
# marker from it when necessary
if(count > MARKERS_MAX):
markerArray.markers.pop(0)
markerArray.markers.append(marker)
# Renumber the marker IDs
id = 0
for m in markerArray.markers:
m.id = id
id += 1
# Publish the MarkerArray
publisher.publish(markerArray)
count += 1
rospy.sleep(0.01)
##PORTRAIT 5##
if arr == [4,4]:
marker.pose.position.x = -0.227
marker.pose.position.y = -13.6657
marker.pose.position.z = 0.3
# We add the new marker to the MarkerArray, removing the oldest
# marker from it when necessary
if(count > MARKERS_MAX):
markerArray.markers.pop(0)
markerArray.markers.append(marker)
# Renumber the marker IDs
id = 0
for m in markerArray.markers:
m.id = id
id += 1
# Publish the MarkerArray
publisher.publish(markerArray)
count += 1
rospy.sleep(0.01)
marker.scale.x = 0.01
marker.scale.y = 0.01
marker.scale.z = 0.01
marker.pose.position.x = 1000
marker.pose.position.y = 1000
marker.pose.position.z = 1000
# We add the new marker to the MarkerArray, removing the oldest
# marker from it when necessary
if(count > MARKERS_MAX):
markerArray.markers.pop(0)
markerArray.markers.append(marker)
# Renumber the marker IDs
id = 0
for m in markerArray.markers:
m.id = id
id += 1
# Publish the MarkerArray
publisher.publish(markerArray)
count += 1
rospy.sleep(0.01)
from visualization_msgs.msg import MarkerArray
rospy.init_node("marker_mesh_sample")
pub = rospy.Publisher('marker', Marker, queue_size=1)
pub2 = rospy.Publisher('marker2', Marker, queue_size=1)
pub3 = rospy.Publisher('marker3', Marker, queue_size=1)
pub4 = rospy.Publisher('marker4', Marker, queue_size=1)
moonwalk_markers_pub = rospy.Publisher('markers', MarkerArray, queue_size=1)
r = rospy.Rate(10)
green = 0
while not rospy.is_shutdown():
mesh_marker = Marker()
mesh_marker.header.frame_id = "nil_link"
mesh_marker.header.stamp = rospy.Time.now()
mesh_marker.pose.position.x = 1.0
mesh_marker.pose.orientation.w = 1.0
mesh_marker.scale.x = 1.0
mesh_marker.scale.y = 1.0
mesh_marker.scale.z = 1.0
mesh_marker.type = Marker.MESH_RESOURCE
#mesh_marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae"
mesh_marker.mesh_resource = "package://rviz_collada_marker/nil_link_mesh.dae"
mesh_marker.mesh_use_embedded_materials = True
pub.publish(mesh_marker)
mesh_marker.mesh_resource = "package://rviz_collada_marker/base.dae"
mesh_marker.pose.position.y = 2.0
def main():
rospy.init_node('simple_marker')
wait_for_time()
global pub_init_pose
pub_init_pose = rospy.Publisher('initialpose',
PoseWithCovarianceStamped,
queue_size=1)
pub = rospy.Publisher('odometry_goal', Point, queue_size=5)
hostname = socket.gethostname()
marker_publisher = rospy.Publisher('visualization_marker',
Marker,
queue_size=5)
marker_publisher_host = rospy.Publisher(hostname + '/visualization_marker',
Marker,
queue_size=5)
waypoints_publisher = rospy.Publisher('visualization_marker_array',
MarkerArray,
queue_size=5)
waypoints_publisher_text = rospy.Publisher(
'visualization_marker_array_text', MarkerArray, queue_size=5)
rospy.sleep(0.5)
server = InteractiveMarkerServer('simple_marker')
marker1 = DestinationMarker(server, 0, 0, 'dest1', pub)
global pose_marker
pose_marker = DestinationMarker(server, 1, 0, 'pose estimate', pub)
global waypoint_marker
waypoint_marker = DestinationMarker(server, 2, -1, 'waypoint', pub)
marker1.start()
marker1.markerOff()
pose_marker.start()
waypoint_marker.start()
global waypoint_list
#marker1.markerOff()
waypoint_marker.markerOff()
pose_marker.markerOff()
rospy.Subscriber("robot_pose", PoseStamped, robot_marker_callback)
service_waypoint = rospy.Service('waypoint_markers_service', waypoint,
turn_waypoint_on_off)
service_pose_estimate = rospy.Service('poseestimate_markers_service',
poseestimate,
turn_poseestimate_on_off)
service_start_slam = rospy.Service('start_slam', slamsrv, turn_slam_on_off)
service_start_nav = rospy.Service('start_navigation', navigatesrv,
turn_nav_on_off)
set_pose = rospy.Service('set_pose', poseestimate, set_post)
get_map = rospy.Service('get_map', maps, get_maps)
getpose = rospy.Service('get_pose', getpost, get_pose)
get_map = rospy.Service('save_map', savemaps, save_maps)
get_waypoint = rospy.Service('get_waypoint', waypointsarray, getwaypoints)
get_a_waypoint = rospy.Service('get_a_waypoint', awaypoint,
getwayAwaypoint)
get_waypoint_name = rospy.Service('get_waypoint_name', waypointname,
getWaypointname)
delete_map = rospy.Service('delete_map', deletemap, deleteMap)
delete_waypoint = rospy.Service('delete_waypoint', deletewaypoint,
deleteWaypoint)
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
rate = rospy.Rate(20.0)
is_slam_running = False
is_nav_running = False
wpts = []
p_wpts = []
markerArray2 = MarkerArray()
markerArray3 = MarkerArray()
p_waypoint_length = 0
print("Start loop")
while not rospy.is_shutdown():
if not q_slam.empty():
status = q_slam.get()
if status == 1:
if is_slam_running == False:
uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid)
launch_slam = roslaunch.parent.ROSLaunchParent(
uuid, [
"/home/pi/linorobot_ws/src/linorobot/launch/slam.launch"
])
launch_slam.start()
is_slam_running = True
print("start")
else:
if is_slam_running == True:
launch_slam.shutdown()
is_slam_running = False
print("stop")
if not q_nav.empty():
nav_status = q_nav.get()
if nav_status["sw"] == 1:
if is_slam_running == False:
uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid)
hostname = socket.gethostname()
cli_args = [
"/home/pi/linorobot_ws/src/linorobot/launch/navigate.launch",
"map_file:=/home/pi/linorobot_ws/src/linorobot/maps/" +
nav_status["mapname"] + ".yaml"
]
roslaunch_args = cli_args[1:]
roslaunch_file = [(
roslaunch.rlutil.resolve_launch_arguments(cli_args)[0],
roslaunch_args)]
launch_nav = roslaunch.parent.ROSLaunchParent(
uuid, roslaunch_file) #map_file
launch_nav.start()
print(hostname)
#uuid2 = roslaunch.rlutil.get_or_generate_uuid(None, False)
#roslaunch.configure_logging(uuid2)
#hostname = socket.gethostname()
#cli_args2 = ["/home/pi/linorobot_ws/src/mqtt_bridge/launch/demo.launch"]
#roslaunch_args2 = cli_args2[1:]
#roslaunch_file2 = [(roslaunch.rlutil.resolve_launch_arguments(cli_args2)[0], roslaunch_args2)]
#launch_nav2 = roslaunch.parent.ROSLaunchParent(uuid2, roslaunch_file2) #map_file
is_nav_running = True
print("start")
else:
if is_nav_running == True:
launch_nav.shutdown()
#launch_nav2.shutdown()
is_nav_running = False
print("stop")
if not q_map_name.empty():
mapname = q_map_name.get()
bin_cmd = 'rosrun map_server map_saver -f /home/pi/linorobot_ws/src/linorobot/maps/' + mapname
print bin_cmd
os.system(bin_cmd)
data = {}
with open(os.path.join(MAP_PATH, mapname + '.json'), 'w') as fp:
json.dump(data, fp)
#subprocess.run(["rosrun", bin_cmd])
if not q_wayponts.empty():
wpts = q_wayponts.get()
wp_names = q_wayponts_names.get()
print "========>"
print len(wpts)
#print wpts
mk_length = len(markerArray2.markers)
id = 0
id_t = len(markerArray2.markers) + 1
i = 0
del markerArray2.markers[:]
del markerArray3.markers[:]
for x in range(0, mk_length):
#marker =
markerArray2.markers.append(
Marker(type=Marker.ARROW,
id=id,
action=2,
scale=Vector3(0.2, 0.2, 0.2),
header=Header(frame_id='map'),
color=ColorRGBA(1.0, 1.0, 0.0, 1.0),
text="AGV"))
markerArray3.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
id=id_t,
action=2,
scale=Vector3(0.3, 0.3, 0.3),
header=Header(frame_id='map'),
color=ColorRGBA(0.0, 0.1, 1.0, 1.0),
text="null"))
i = i + 1
id += 1
id_t += 1
waypoints_publisher.publish(markerArray2)
waypoints_publisher_text.publish(markerArray3)
print "clear========>"
del markerArray2.markers[:]
del markerArray3.markers[:]
#markerArray2.markers = []
#markerArray3.markers = []
id = 0
id_t = len(wpts) + 1
i = 0
for x in wpts:
#marker =
markerArray2.markers.append(
Marker(type=Marker.ARROW,
id=id,
pose=x,
lifetime=rospy.Duration.from_sec(1),
scale=Vector3(0.2, 0.2, 0.2),
header=Header(frame_id='map'),
color=ColorRGBA(1.0, 1.0, 0.0, 1.0),
text="AGV"))
ik_pose = Pose()
ik_pose.position.x = x.position.x
ik_pose.position.y = x.position.y
ik_pose.position.z = x.position.z + 0.1
ik_pose.orientation.x = x.orientation.x
ik_pose.orientation.y = x.orientation.y
ik_pose.orientation.z = x.orientation.z
ik_pose.orientation.w = x.orientation.w
markerArray3.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
id=id_t,
pose=ik_pose,
scale=Vector3(0.3, 0.3, 0.3),
header=Header(frame_id='map'),
color=ColorRGBA(0.0, 0.1, 1.0, 1.0),
text=wp_names[i]))
i = i + 1
id += 1
id_t += 1
p_waypoint_length = len(wpts)
p_wpts = wpts
#print markerArray3
waypoints_publisher.publish(markerArray2)
waypoints_publisher_text.publish(markerArray3)
try:
point_odom = tfBuffer.lookup_transform('map', 'base_footprint',
rospy.Time(0))
hhname = socket.gethostname()
marker = Marker(type=Marker.ARROW,
id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(point_odom.transform.translation,
point_odom.transform.rotation),
scale=Vector3(0.2, 0.2, 0.2),
header=Header(frame_id='map'),
color=ColorRGBA(0.0, 1.0, 0.0, 1.0),
text=hhname)
marker_publisher.publish(marker)
marker_publisher_host.publish(marker)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rate.sleep()
#continue
#print("loop")
rate.sleep()
def initialize_markers(
self): #initialize markers for each place at the beginning
Markersini = MarkerArray()
for elem in self.rooms:
SMarker = Marker()
SMarker.header.frame_id = "map"
SMarker.type = 1
SMarker.pose.orientation.w = 1.0
SMarker.lifetime = rospy.Duration(0)
SMarker.action = SMarker.ADD
SMarker.pose.position.z = 0
SMarker.scale.z = 0.1
SMarker.color.r = 0.65
SMarker.color.g = 0.65
SMarker.color.b = 0.65
SMarker.color.a = 0.65
SMarker.frame_locked = True
SMarker.id = 1
SMarker.header.stamp = rospy.Time.now()
SMarker.ns = '{}-{}'.format(elem.xcenter, elem.ycenter)
SMarker.pose.position.x = elem.xcenter
SMarker.pose.position.y = elem.ycenter
SMarker.scale.x = abs(elem.xmax - elem.xmin)
SMarker.scale.y = abs(elem.ymax - elem.ymin)
Markersini.markers.append(copy.deepcopy(SMarker))
print 'inserted room x:{} y:{} marker'.format(
elem.xcenter, elem.ycenter)
for elem in self.rooms:
Text_Marker = Marker()
Text_Marker.header.frame_id = "map"
Text_Marker.id = 1
Text_Marker.type = Text_Marker.TEXT_VIEW_FACING
Text_Marker.action = 0
Text_Marker.pose.orientation.w = 1.0
Text_Marker.scale.z = 0.45 #/0.65
Text_Marker.color.r = 0
Text_Marker.color.g = 0
Text_Marker.color.b = 0
Text_Marker.color.a = 1
Text_Marker.lifetime = rospy.Duration(0)
Text_Marker.frame_locked = True
Text_Marker.pose.position.z = 0.2
Text_Marker.header.stamp = rospy.Time.now()
Text_Marker.ns = 'txt{}-{}'.format(elem.xcenter, elem.ycenter)
Text_Marker.pose.position.x = elem.xcenter
Text_Marker.pose.position.y = elem.ycenter
Text_Marker.text = '{}: Unknown category!'.format(elem.id)
Markersini.markers.append(copy.deepcopy(Text_Marker))
print 'inserted room x:{} y:{} text marker'.format(
elem.xcenter, elem.ycenter)
rospy.sleep(0.1)
for elem in self.rooms:
Line_List = Marker()
Line_List.header.frame_id = "map"
Line_List.type = 4
Line_List.pose.orientation.w = 1.0
Line_List.lifetime = rospy.Duration(0)
Line_List.action = SMarker.ADD
Line_List.scale.x = 0.1
Line_List.color.r = 1
Line_List.color.g = 0.0
Line_List.color.b = 0.0
Line_List.color.a = 0.9
Line_List.frame_locked = True
Line_List.id = 1
Line_List.header.stamp = rospy.Time.now()
Line_List.ns = 'line{}-{}'.format(elem.xcenter, elem.ycenter)
xmin_ymin = Point()
xmin_ymin.x = elem.xmin
xmin_ymin.y = elem.ymin
xmin_ymin.z = 0.10
Line_List.points.append(xmin_ymin)
xmax_ymin = Point()
xmax_ymin.x = elem.xmax
xmax_ymin.y = elem.ymin
xmax_ymin.z = 0.10
Line_List.points.append(xmax_ymin)
xmax_ymax = Point()
xmax_ymax.x = elem.xmax
xmax_ymax.y = elem.ymax
xmax_ymax.z = 0.10
Line_List.points.append(xmax_ymax)
xmin_ymax = Point()
xmin_ymax.x = elem.xmin
xmin_ymax.y = elem.ymax
xmin_ymax.z = 0.10
Line_List.points.append(xmin_ymax)
Line_List.points.append(xmin_ymin)
Markersini.markers.append(copy.deepcopy(Line_List))
rospy.sleep(0.1)
self.marker_pub.publish(Markersini)
rospy.sleep(0.1)
def publish(self, timestamp):
pose = PoseStamped()
pose.header.frame_id = self.target_frame
pose.header.stamp = timestamp
pose.pose.position.x = self.X[0, 0]
pose.pose.position.y = self.X[1, 0]
pose.pose.position.z = 0.0
Q = quaternion_from_euler(0, 0, self.X[2, 0])
pose.pose.orientation.x = Q[0]
pose.pose.orientation.y = Q[1]
pose.pose.orientation.z = Q[2]
pose.pose.orientation.w = Q[3]
self.pose_pub.publish(pose)
ma = MarkerArray()
marker = Marker()
marker.header = pose.header
marker.ns = "kf_uncertainty"
marker.id = 5000
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.pose = pose.pose
marker.pose.position.z = -0.1
try:
marker.scale.x = 3 * sqrt(self.P[0, 0])
marker.scale.y = 3 * sqrt(self.P[1, 1])
except:
print 'Error with self.P\n: ' + repr(self.P)
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 1.0
ma.markers.append(marker)
for id in self.idx.iterkeys():
list = self.idx[id]
for entry in range(0, len(list)):
l = list[entry]
marker = Marker()
marker.header.stamp = timestamp
marker.header.frame_id = self.target_frame
marker.ns = "landmark_kf"
marker.id = id + 100 * entry
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.pose.position.x = self.X[l, 0]
marker.pose.position.y = self.X[l + 1, 0]
marker.pose.position.z = -0.1
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 1
marker.pose.orientation.w = 0
marker.scale.x = 0.2
#3*sqrt(self.P[l,l])
marker.scale.y = 0.2
#3*sqrt(self.P[l+1,l+1]);
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 0.25
marker.color.g = 0
marker.color.b = 0.25
marker.lifetime.secs = 3.0
ma.markers.append(marker)
marker = Marker()
marker.header.stamp = timestamp
marker.header.frame_id = self.target_frame
marker.ns = "landmark_kf"
marker.id = 1000 + id + 100 * entry
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.ADD
marker.pose.position.x = self.X[l + 0, 0]
marker.pose.position.y = self.X[l + 1, 0]
marker.pose.position.z = 1.0
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 1
marker.pose.orientation.w = 0
marker.text = str(id)
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.lifetime.secs = 3.0
ma.markers.append(marker)
self.marker_pub.publish(ma)
def talker():
#XBee config
ser = serial.Serial("/dev/ttyUSB0", 9600)
ser.write('\n')
ser.write('b\n')
ser.close()
ser = serial.Serial("/dev/ttyUSB0", 115200)
sourceaddrlist = {} # node set dictionary
pubMarker = {} # publisher dictionary
pubText = {} # publisher dictionary
xbradio1 = xbee.ZigBee(ser) # the DAQ radio
rospy.init_node('RelocNode')
pubIRdata1 = rospy.Publisher('/IRdebug/point1', Point)
pubIRdata2 = rospy.Publisher('/IRdebug/point2', Point)
pubIRdata3 = rospy.Publisher('/IRdebug/point3', Point)
pubIRdata4 = rospy.Publisher('/IRdebug/point4', Point)
pubUSdata = rospy.Publisher('/USdebug', Quaternion)
br = tf.TransformBroadcaster()
robId = "??"
IRx1 = 0
IRy1 = 0
IRx2 = 0
IRy2 = 0
IRx3 = 0
IRy3 = 0
IRx4 = 0
IRy4 = 0
IRang = 0
US1 = 0
US2 = 0
US3 = 0
US4 = 0
while not rospy.is_shutdown():
response = xbradio1.wait_read_frame() #response is a python dictionary
#sorce address
name = binascii.hexlify(response['source_addr_long'])
sourceaddr = name.decode("utf-8")
# check if its a new source address and initialize publishers
nodeseen = 0
for j in range(len(sourceaddrlist)):
if sourceaddrlist[j + 1] == sourceaddr:
nodeseen = 1
Rfdata = response['rf_data'].decode("utf-8")
str2 = Rfdata.split(',')
#packet data extraction
range_flag = 0
bearing_flag = 0
packetValid = 0
try:
for i in range(len(str2)):
if str2[i][0:2] == "RL":
robID = str2[i][2:]
if str2[i] == "IR":
IRx1 = int(str2[i + 1])
IRy1 = int(str2[i + 2])
IRx2 = int(str2[i + 3])
IRy2 = int(str2[i + 4])
IRx3 = int(str2[i + 5])
IRy3 = int(str2[i + 6])
IRx4 = int(str2[i + 7])
IRy4 = int(str2[i + 8])
bearing_flag = 1
if str2[i] == "ANG":
IRang = int(
str2[i + 1][0:len(str2[i + 1]) - 2]) - 90
#print IRang/180.0*math.pi
br.sendTransform(
(0, 0, 0.3),
tf.transformations.quaternion_from_euler(
0, 0, -IRang / 180.0 * math.pi),
rospy.Time.now(), "/pioneer1/IRcam",
"/pioneer1/base_link")
if str2[i] == "US":
US1 = float(str2[i + 1])
US2 = float(str2[i + 2])
US3 = float(str2[i + 3])
US4 = float(str2[i + 4])
range_flag = 1
packetValid = 1
except:
#print "Bad Packet\n"
packetValid = 0
#Measurement processing
if packetValid == 1:
slog = "No Data"
USmin = 0
if range_flag == 1 and bearing_flag == 0:
IRx = 0
IRy = 0
USmin = min(US1, US2, US3, US4) / 1000.0
fc = 1380
slog = "Range only"
if USmin == 10725:
slog = "No Data"
USmin = 0
if range_flag == 0 and bearing_flag == 1:
IRx = IRx1 - 1024 / 2
IRy = -IRy1 + 768 / 2
USmin = 10
fc = 1380
slog = "Bearing only"
if range_flag == 1 and bearing_flag == 1:
IRx = IRx1 - 1024 / 2
IRy = -IRy1 + 768 / 2
USmin = min(US1, US2, US3, US4) / 1000.0
fc = 1380
slog = "Range Bearing"
#print USmin
#print IRx1
if IRx1 == 1023:
robx = 0
roby = 0
robz = 0
else:
rpx = math.sqrt(pow(IRx, 2) + pow(IRy, 2) + pow(fc, 2))
robx = fc / rpx * USmin
roby = IRx / rpx * USmin
robz = IRy / rpx * USmin
if USmin == 0 or USmin > 9:
USmin = 0.1
#Data publish
text_marker = Marker()
text_marker.header.frame_id = "/pioneer1/base_link"
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.text = "RobID : %s\nx : %f\ny : %f\nz : %f\ntheta :\n %s" % (
robID, robx, roby, robz, slog)
text_marker.pose.position.x = robx
text_marker.pose.position.y = roby
text_marker.pose.position.z = robz
text_marker.scale.z = 0.1
text_marker.color.r = 0.0
text_marker.color.g = 0.5
text_marker.color.b = 0.5
text_marker.color.a = 1
if slog == "Range only":
arrow_marker = Marker()
arrow_marker.header.frame_id = "/pioneer1/base_link"
arrow_marker.type = Marker.CYLINDER
arrow_marker.scale.x = (USmin + 0.05) * 2
arrow_marker.scale.y = (USmin + 0.05) * 2
arrow_marker.scale.z = 0.1
arrow_marker.color.r = 0.0
arrow_marker.color.g = 0.5
arrow_marker.color.b = 0.5
arrow_marker.color.a = 0.5
else:
arrow_marker = Marker()
arrow_marker.header.frame_id = "/pioneer1/base_link"
arrow_marker.type = Marker.ARROW
arrow_marker.points = {
Point(0, 0, 0),
Point(robx, roby, robz)
}
arrow_marker.scale.x = 0.1
arrow_marker.scale.y = 0.1
arrow_marker.scale.z = 0.2
arrow_marker.color.r = 1.0
arrow_marker.color.g = 0.5
arrow_marker.color.b = 0.5
arrow_marker.color.a = 0.5
IRdebug1 = Point()
IRdebug1.x = IRx1
IRdebug1.y = IRy1
IRdebug2 = Point()
IRdebug2.x = IRx2
IRdebug2.y = IRy2
IRdebug3 = Point()
IRdebug3.x = IRx3
IRdebug3.y = IRy3
IRdebug4 = Point()
IRdebug4.x = IRx4
IRdebug4.y = IRy4
USdebug = Quaternion()
USdebug.x = US1
USdebug.y = US2
USdebug.z = US3
USdebug.w = US4
pubIRdata1.publish(IRdebug1)
pubIRdata2.publish(IRdebug2)
pubIRdata3.publish(IRdebug3)
pubIRdata4.publish(IRdebug4)
pubUSdata.publish(USdebug)
pubText[j + 1].publish(text_marker)
pubMarker[j + 1].publish(arrow_marker)
if str2[0][0:5] == "RL002":
print str2
#print str2
if nodeseen == 0: # New nodes handling
sourceaddrlist[len(sourceaddrlist) + 1] = sourceaddr
pubText[len(pubText) + 1] = rospy.Publisher(
'/relocNode' + str(len(pubText) + 1) + '/relocVizDataText',
Marker)
pubMarker[len(pubMarker) + 1] = rospy.Publisher(
'/relocNode' + str(len(pubMarker) + 1) + '/relocVizData',
Marker)
print "New node registered:" + sourceaddr
#publish data of new node
Rfdata = response['rf_data'].decode("utf-8")
#print(sourceaddr+' :'+Rfdata)
#Publish all topics
rospy.sleep(0.01)
rospy.Subscriber(vehiclePosTopicName, PoseStamped, callbackVehiclePos)
else:
raise AssertionError('Vehicle postion message type not correct!')
estPosPublisher = rospy.Publisher("phone_position",
phone_pos_est,
queue_size=10)
estPhonePosMsg = phone_pos_est()
estPosReportPublisher = rospy.Publisher("bluetooth_phone_report",
filtered_art,
queue_size=10)
estPhoneReportPosMsg = filtered_art()
estPosMarkerPublisher = rospy.Publisher("phone_marker", Marker, queue_size=10)
estPhonePosMarker = Marker()
loopRate = rospy.Rate(loopFrequency)
threadRosSpin = threading.Thread(name='rosSpin', target=rosSpin)
threadRosSpin.start()
lock = threading.Lock(
) #use lock to prevent changing the currentBTdeviceList at the same time
threadDeviceDiscovery = threading.Thread(name='discovery',
target=deviceDiscovery,
args=(lock, ))
threadDeviceDiscovery.start()
while not rospy.is_shutdown():
def initPathMarkers():
# cannot write in one equation!!!
sourcePoint = Marker()
goalPoint = Marker()
neighbourPoint = Marker()
finalPath = Marker()
sourcePoint.header.frame_id = 'path_planner'
goalPoint.header.frame_id = 'path_planner'
neighbourPoint.header.frame_id = 'path_planner'
finalPath.header.frame_id = 'path_planner'
sourcePoint.header.stamp = rospy.get_rostime()
goalPoint.header.stamp = rospy.get_rostime()
neighbourPoint.header.stamp = rospy.get_rostime()
finalPath.header.stamp = rospy.get_rostime()
sourcePoint.ns = "path_planner"
goalPoint.ns = "path_planner"
neighbourPoint.ns = "path_planner"
finalPath.ns = "path_planner"
sourcePoint.action = 0 # add/modify an object
goalPoint.action = 0
neighbourPoint.action = 0
finalPath.action = 0
sourcePoint.id = 0
goalPoint.id = 1
neighbourPoint.id = 2
finalPath.id = 3
# sourcePoint.text = 'sourcePoint'
# goalPoint.text = 'goalPoint'
# neighbourPoint.text = 'neighbourPoint'
# finalPath.text = 'finalPath'
sourcePoint.type = 2 # Sphere
goalPoint.type = 2
neighbourPoint.type = 8 # Points
finalPath.type = 4 # Line Strip
sourcePoint.pose.orientation.w = 1.0
goalPoint.pose.orientation.w = 1.0
neighbourPoint.pose.orientation.w = 1.0
finalPath.pose.orientation.w = 1.0
sourcePoint.pose.position.x = 0.0
sourcePoint.pose.position.y = 0.0
sourcePoint.pose.position.z = 0.0
goalPoint.pose.position.x = 10.0
goalPoint.pose.position.y = 10.0
goalPoint.pose.position.z = 0.0
neighbourPoint.pose.position.x = 0.0
neighbourPoint.pose.position.y = 0.0
neighbourPoint.pose.position.z = 0.0
sourcePoint.scale.x = sourcePoint.scale.y = sourcePoint.scale.z = 1.0
goalPoint.scale.x = goalPoint.scale.y = goalPoint.scale.z = 1.0
neighbourPoint.scale.x = neighbourPoint.scale.y = neighbourPoint.scale.z = 0.1
finalPath.scale.x = 0.5 # scale.x controls the width of the line segments
sourcePoint.color.g = 1.0
goalPoint.color.r = 1.0
neighbourPoint.color.r = 0.8
neighbourPoint.color.g = 0.4
finalPath.color.r = 0.2
finalPath.color.g = 0.2
finalPath.color.b = 1.0
sourcePoint.color.a = 1.0
goalPoint.color.a = 1.0
neighbourPoint.color.a = 0.5
finalPath.color.a = 1.0
return (sourcePoint, goalPoint, neighbourPoint, finalPath)
def callback(msg, tmp_list):
""""""
global old_yaw
[ekf, publisher_position, publisher_mavros, broadcaster,
publisher_marker] = tmp_list
# ekf algorithm
ekf.prediction()
# get length of message
num_meas = len(msg.detections)
orientation_yaw_pitch_roll = np.zeros((num_meas, 3))
# if new measurement: update particles
if num_meas >= 1:
measurements = np.zeros((num_meas, 1 + state_dim))
# get data from topic /tag_detection
if rviz:
markerArray = MarkerArray()
for i, tag in enumerate(msg.detections):
tag_id = int(tag.id[0])
tag_distance_cam = np.array(([
tag.pose.pose.pose.position.x * 1.05,
tag.pose.pose.pose.position.y * 1.1,
tag.pose.pose.pose.position.z
]))
measurements[i, 0] = np.linalg.norm(tag_distance_cam)
tmpquat = Quaternion(w=tag.pose.pose.pose.orientation.w,
x=tag.pose.pose.pose.orientation.x,
y=tag.pose.pose.pose.orientation.y,
z=tag.pose.pose.pose.orientation.z)
orientation_yaw_pitch_roll[i, :] = tmpquat.inverse.yaw_pitch_roll
index = np.where(tags[:, 0] == tag_id)
measurements[i, 1:4] = tags[index, 1:4]
# print(measurements[i, 1:4])
if rviz:
marker = Marker()
marker.header.frame_id = "global_tank"
marker.id = i
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = measurements[
i, 0] * 2 # r*2 of distance to camera from tag_14
marker.scale.y = measurements[i, 0] * 2
marker.scale.z = measurements[i, 0] * 2
marker.color.g = 1
marker.color.a = 0.1 # transparency
marker.pose.orientation.w = 1.0
marker.pose.position.x = tags[index, 1][0] # x
marker.pose.position.y = tags[index, 2][0] # y
marker.pose.position.z = tags[index, 3][0] # z
markerArray.markers.append(marker)
if rviz:
# print(len(markerArray.markers))
publisher_marker.publish(markerArray)
# print(index)
# ekf update step
ekf.update(measurements)
yaw = np.mean(orientation_yaw_pitch_roll[:, 0])
pitch = np.mean(orientation_yaw_pitch_roll[:, 1])
roll = np.mean(orientation_yaw_pitch_roll[:, 2])
else:
yaw = old_yaw
old_yaw = yaw
# print "reale messungen: " + str(measurements)
print("Angle yaw: " + str(np.round(yaw * 180 / np.pi, decimals=2)) +
", x_est = " + str(ekf.get_x_est().transpose()))
estimated_orientation = yaw_pitch_roll_to_quat(-(yaw - np.pi / 2), 0, 0)
# estimated_orientation = yaw_pitch_roll_to_quat(yaw, 0, 0)#evtl wrong
# calculate position as mean of particle positions
estimated_position = ekf.get_x_est()
# [mm]
x_mean_ned = estimated_position[
0] * 1000 # global Tank Koordinate System(NED)
y_mean_ned = estimated_position[1] * 1000
z_mean_ned = estimated_position[2] * 1000
# publish estimated_pose [m] in mavros to /mavros/vision_pose/pose
# this pose needs to be in ENU
mavros_position = PoseStamped()
mavros_position.header.stamp = rospy.Time.now()
mavros_position.header.frame_id = "map"
mavros_position.pose.position.x = y_mean_ned / 1000 # NED Coordinate to ENU(ROS)
mavros_position.pose.position.y = x_mean_ned / 1000
mavros_position.pose.position.z = -z_mean_ned / 1000
mavros_position.pose.orientation.w = estimated_orientation.w
mavros_position.pose.orientation.x = estimated_orientation.x
mavros_position.pose.orientation.y = estimated_orientation.y
mavros_position.pose.orientation.z = estimated_orientation.z
# publish estimated_pose [m]
position = PoseStamped()
position.header.stamp = rospy.Time.now()
position.header.frame_id = "global_tank" # ned
position.pose.position.x = x_mean_ned / 1000
position.pose.position.y = y_mean_ned / 1000
position.pose.position.z = z_mean_ned / 1000
estimated_orientation = yaw_pitch_roll_to_quat(yaw, 0, 0)
position.pose.orientation.w = estimated_orientation.w
position.pose.orientation.x = estimated_orientation.x
position.pose.orientation.y = estimated_orientation.y
position.pose.orientation.z = estimated_orientation.z
publisher_position.publish(position)
# yaw = 0 / 180.0 * np.pi
# tmp = yaw_pitch_roll_to_quat(-(yaw-np.pi/2), 0, 0)
# print(tmp)
# mavros_position.pose.orientation.w = tmp.w
# mavros_position.pose.orientation.x = tmp.x
# mavros_position.pose.orientation.y = tmp.y
# mavros_position.pose.orientation.z = tmp.z
publisher_mavros.publish(mavros_position)
# For Debugging
# mavros_position = PoseStamped()
# mavros_position.header.stamp = rospy.Time.now()
# mavros_position.header.frame_id = "map"
# mavros_position.pose.position.x = 1.0 + np.random.normal(0, 0.01)
# mavros_position.pose.position.y = 2.0 + np.random.normal(0, 0.01)
# mavros_position.pose.position.z = 3.0 + np.random.normal(0, 0.01)
#
# mavros_position.pose.orientation.w = 1.0
# mavros_position.pose.orientation.x = 2.0
# mavros_position.pose.orientation.y = 3.0
# mavros_position.pose.orientation.z = 4.0
# publisher_mavros.publish(mavros_position)
"""
def visualizer():
rospy.Subscriber("baro/pressure", FluidPressure, update_pressure)
rospy.Subscriber("gps", GPS, update_gps)
rate = rospy.Rate(1)
trace_pub = rospy.Publisher("viz/gps_trace", Marker)
primary_geofence_pub = rospy.Publisher("viz/primary_geofence", Marker)
secondary_geofence_pub = rospy.Publisher("viz/secondary_geofence", Marker)
trace_points = []
# initialize parameters
secondary_geofence_width = rospy.get_param("secondary_geofence/width",
0.00002)
min_lon = 0
max_lon = 0
min_lat = 0
max_lat = 0
try:
min_lon = rospy.get_param("geofence/lon/min")
max_lon = rospy.get_param("geofence/lon/max")
min_lat = rospy.get_param("geofence/lat/max")
max_lat = rospy.get_param("geofence/lat/min")
except KeyError:
rospy.logfatal(
"Geofence was not configured on the parameter server! Shutting down"
)
sys.exit(1)
# get parameters with defaults that require the aforementioned non-default parameters
secondary_min_lat = rospy.get_param("secondary_geofence/lat/min",
min_lat + secondary_geofence_width)
secondary_max_lat = rospy.get_param("secondary_geofence/lat/max",
max_lat - secondary_geofence_width)
secondary_min_lon = rospy.get_param("secondary_geofence/lon/min",
min_lon - secondary_geofence_width)
secondary_max_lon = rospy.get_param("secondary_geofence/lon/max",
max_lon + secondary_geofence_width)
while not rospy.is_shutdown():
if fix and pressure_offset != -1:
gps_divisor = rospy.get_param("~gps_divisor", 0.00001)
try:
primary_geofence_pts = (Point(
0, 0, 0), Point(0, (max_lat - min_lat) / gps_divisor, 0),
Point(
(max_lon - min_lon) / gps_divisor,
(max_lat - min_lat) / gps_divisor,
0),
Point(
(max_lon - min_lon) / gps_divisor,
0, 0), Point(0, 0, 0))
secondary_geofence_pts = (
Point((secondary_min_lon - min_lon) / gps_divisor,
(secondary_min_lat - min_lat) / gps_divisor, 0),
Point((secondary_min_lon - min_lon) / gps_divisor,
(secondary_max_lat - min_lat) / gps_divisor, 0),
Point((secondary_max_lon - min_lon) / gps_divisor,
(secondary_max_lat - min_lat) / gps_divisor, 0),
Point((secondary_max_lon - min_lon) / gps_divisor,
(secondary_min_lat - min_lat) / gps_divisor, 0),
Point((secondary_min_lon - min_lon) / gps_divisor,
(secondary_min_lat - min_lat) / gps_divisor, 0))
trace_points.append(
Point((lon - min_lon) / gps_divisor,
(lat - min_lat) / gps_divisor,
relative_alt / rospy.get_param("~baro_divisor", 10)))
trace_color = rospy.get_param("~trace_color", {
"r": 0,
"g": 1,
"b": 0,
"a": 1
})
viz_trace_color = ColorRGBA(trace_color['r'], trace_color['g'],
trace_color['b'], trace_color['a'])
primary_geofence_color = rospy.get_param(
"~primary_geofence_color", {
"r": 1,
"g": 1,
"b": 0,
"a": 1
})
viz_primary_geofence_color = ColorRGBA(
primary_geofence_color['r'], primary_geofence_color['g'],
primary_geofence_color['b'], primary_geofence_color['a'])
secondary_geofence_color = rospy.get_param(
"~secondary_geofence_color", {
"r": 1,
"g": 0,
"b": 0,
"a": 1
})
viz_secondary_geofence_color = ColorRGBA(
secondary_geofence_color['r'],
secondary_geofence_color['g'],
secondary_geofence_color['b'],
secondary_geofence_color['a'])
scale = rospy.get_param("~scale", 0.25)
scale_vec = Vector3(scale, 0, 0)
trace_ns = rospy.get_param("~trace_ns", "moby_trace")
primary_geofence_ns = rospy.get_param("~primary_geofence_ns",
"moby_primary_geofence")
secondary_geofence_ns = rospy.get_param(
"~secondary_geofence_ns", "moby_secondary_geofence")
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "map"
trace_msg = Marker(header=header,
ns=trace_ns,
id=0,
type=4,
action=0,
scale=scale_vec,
color=viz_trace_color,
lifetime=rospy.Duration(),
frame_locked=False,
points=trace_points)
trace_msg.pose.orientation.w = 1.0
trace_pub.publish(trace_msg)
primary_geofence_msg = Marker(header=header,
ns=primary_geofence_ns,
id=0,
type=4,
action=0,
scale=scale_vec,
color=viz_primary_geofence_color,
lifetime=rospy.Duration(),
frame_locked=False,
points=primary_geofence_pts)
primary_geofence_pub.publish(primary_geofence_msg)
secondary_geofence_msg = Marker(
header=header,
ns=secondary_geofence_ns,
id=0,
type=4,
action=0,
scale=scale_vec,
color=viz_secondary_geofence_color,
lifetime=rospy.Duration(),
frame_locked=False,
points=secondary_geofence_pts)
secondary_geofence_pub.publish(secondary_geofence_msg)
except KeyError:
rospy.logerr(
"Visualization is enabled but zero point(s) are not set!")
rate.sleep()
def _create_visualization_marker(self, obj, obj_type):
# Text marker to display object name
text_marker = Marker()
text_marker.header.frame_id = "map"
text_marker.header.stamp = rospy.Time() # Time zero, always show
text_marker.ns = obj
text_marker.id = 0
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.action = Marker.ADD
text_marker.text = obj
text_marker.pose.orientation.x = 0.0
text_marker.pose.orientation.y = 0.0
text_marker.pose.orientation.z = 0.0
text_marker.pose.orientation.w = 1.0
text_marker.color.a = 1.0
text_marker.color.r = 1.0
text_marker.color.g = 1.0
text_marker.color.b = 1.0
text_marker.scale.x = 0.2
text_marker.scale.y = 0.2
text_marker.scale.z = 0.1
text_marker.lifetime = rospy.Time(3)
#Set position, depends on type
if obj_type in ["drone", "turtlebot"]:
text_marker.header.frame_id = "/%s/base_link" % obj if obj_type == "drone" else "/base_link"
text_marker.frame_locked = True
text_marker.pose.position.x = 0.0
text_marker.pose.position.y = 0.0
text_marker.pose.position.z = 0.5*text_marker.scale.z
elif obj_type == "person":
wp_pos = self.waypoint_positions[self.obj2loc[obj]]
text_marker.pose.position.x = wp_pos['x']
text_marker.pose.position.y = wp_pos['y']
text_marker.pose.position.z = 0.5*text_marker.scale.z
elif obj_type == "waypoint":
wp_pos = self.waypoint_positions[obj]
text_marker.pose.position.x = wp_pos['x']
text_marker.pose.position.y = wp_pos['y']
text_marker.pose.position.z = 0.5*text_marker.scale.z
text_marker.color.a = 0.2
#Early exit for types that only needs text
if obj_type == "turtlebot":
self.vis_markers[obj] = [text_marker]
return
# Colored marker
marker = deepcopy(text_marker)
marker.id = 1
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.1
marker.color.a = 0.8
if obj_type == "drone":
marker.type = Marker.SPHERE
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
elif obj_type == "box":
marker.type = Marker.CUBE
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
elif obj_type == "waypoint":
marker.type = Marker.CYLINDER
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
marker.scale.x = 0.4
marker.scale.y = 0.4
marker.scale.z = 0.01
marker.color.a = 0.2
elif obj_type == "person":
marker.type = Marker.CYLINDER
#Set color in update step
self.vis_markers[obj] = [text_marker,marker]
def publish_rviz_marker(self,
stamp,
ros_publisher,
classes,
positions,
pos_covariances=None,
track_ids=None):
#remove all rviz markers before we publish the new ones
self.delete_all_markers(ros_publisher)
markers = MarkerArray()
for i in range(len(classes)):
cla = classes[i]
#setup marker
marker = Marker()
marker.header.stamp = stamp
marker.header.frame_id = positions[i]["frame_id"]
if track_ids is not None:
marker.id = track_ids[i]
else:
marker.id = i
marker.ns = "mobility_aids"
marker.type = Marker.SPHERE
marker.action = Marker.MODIFY
marker.lifetime = rospy.Duration()
#maker position
marker.pose.position.x = positions[i]["x"]
marker.pose.position.y = positions[i]["y"]
marker.pose.position.z = positions[i]["z"]
#marker color
color_box = self.viz_helper.colors_box[cla]
marker.color.b = float(color_box[2])
marker.color.g = float(color_box[1])
marker.color.r = float(color_box[0])
marker.color.a = 1.0
#get error ellipse
width, height, scale, angle = 0.5, 0.5, 0.5, 0.0
#if a pose covariance is given, like for tracking, plot ellipse marker
if pos_covariances is not None:
width, height, angle = Visualizer.get_error_ellipse(
pos_covariances[i])
angle = angle + np.pi / 2
scale = 0.1
quat = tf.transformations.quaternion_from_euler(0, 0, angle)
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.scale.x = height
marker.scale.y = width
marker.scale.z = scale
markers.markers.append(marker)
ros_publisher.publish(markers)
def handle_velodyne_msg(msg, arg=None):
t_start = time.time()
global tf_segmenter_graph
global last_known_position, last_known_box_size, last_known_yaw
if segmenter_model is None:
init_segmenter(args.segmenter_model)
if localizer_model is None:
init_localizer(args.localizer_model)
assert msg._type == 'sensor_msgs/PointCloud2'
# PointCloud2 reference http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html
if verbose: print 'stamp: %s' % msg.header.stamp
#print 'number of points: %i' % msg.width * msg.height
# PERFORMANCE WARNING START
# this preparation code is super slow b/c it uses generator, ideally the code should receive 3 arrays:
# lidar_d lidar_h lidar_i already preprocessed in C++
# points = 0
# for x, y, z, intensity, ring in pc2.read_points(msg):
# cloud[points] = x, y, z, intensity, ring
# points += 1
cloud = pcl.msg2cloud(msg)
lidar, lidar_int, angle_at_edge = DidiTracklet.filter_lidar_rings(
#cloud[:points],
cloud,
rings,
points_per_ring,
clip=CLIP_DIST,
clip_h=CLIP_HEIGHT,
return_lidar_interpolated=True,
return_angle_at_edges=True)
points_per_sector = points_per_ring // sectors
_sectors = 2 * sectors if segmenter_phased else sectors
lidar_d = np.empty((_sectors, points_per_sector, len(rings)),
dtype=np.float32)
lidar_h = np.empty((_sectors, points_per_sector, len(rings)),
dtype=np.float32)
lidar_i = np.empty((_sectors, points_per_sector, len(rings)),
dtype=np.float32)
s_start = 0
for sector in range(sectors):
s_end = s_start + points_per_sector
for ring in range(len(rings)):
lidar_d[sector, :, ring] = lidar[ring, s_start:s_end, 0]
lidar_h[sector, :, ring] = lidar[ring, s_start:s_end, 1]
lidar_i[sector, :, ring] = lidar[ring, s_start:s_end, 2]
s_start = s_end
if segmenter_phased:
s_start = points_per_sector // 2
for sector in range(sectors - 1):
_sector = sectors + sector
s_end = s_start + points_per_sector
for ring in range(len(rings)):
lidar_d[_sector, :, ring] = lidar[ring, s_start:s_end, 0]
lidar_h[_sector, :, ring] = lidar[ring, s_start:s_end, 1]
lidar_i[_sector, :, ring] = lidar[ring, s_start:s_end, 2]
s_start = s_end
for ring in range(len(rings)):
lidar_d[_sectors - 1, :points_per_sector // 2,
ring] = lidar[ring, :points_per_sector // 2, 0]
lidar_d[_sectors - 1, points_per_sector // 2:,
ring] = lidar[ring,
points_per_ring - points_per_sector // 2:, 0]
lidar_h[_sectors - 1, :points_per_sector // 2,
ring] = lidar[ring, :points_per_sector // 2, 1]
lidar_h[_sectors - 1, points_per_sector // 2:,
ring] = lidar[ring,
points_per_ring - points_per_sector // 2:, 1]
lidar_i[_sectors - 1, :points_per_sector // 2,
ring] = lidar[ring, :points_per_sector // 2, 2]
lidar_i[_sectors - 1, points_per_sector // 2:,
ring] = lidar[ring,
points_per_ring - points_per_sector // 2:, 2]
# PERFORMANCE WARNING END
if K._backend == 'tensorflow':
with tf_segmenter_graph.as_default():
time_seg_infe_start = time.time()
class_predictions_by_angle = segmenter_model.predict(
[lidar_d, lidar_h, lidar_i], batch_size=_sectors)
time_seg_infe_end = time.time()
else:
time_seg_infe_start = time.time()
class_predictions_by_angle = segmenter_model.predict(
[lidar_d, lidar_h, lidar_i], batch_size=_sectors)
time_seg_infe_end = time.time()
if verbose:
print ' Seg inference: %0.3f ms' % (
(time_seg_infe_end - time_seg_infe_start) * 1000.0)
if segmenter_phased:
_class_predictions_by_angle = class_predictions_by_angle.reshape(
(-1, points_per_ring, len(rings)))
class_predictions_by_angle = np.copy(_class_predictions_by_angle[0, :])
class_predictions_by_angle[points_per_sector // 2 : points_per_ring - (points_per_sector//2)] += \
_class_predictions_by_angle[1 , : points_per_ring - points_per_sector]
class_predictions_by_angle[0 : points_per_sector // 2 ] += \
_class_predictions_by_angle[1 , points_per_ring - points_per_sector : points_per_ring - (points_per_sector // 2)]
class_predictions_by_angle[points_per_ring - (points_per_sector // 2) : ] += \
_class_predictions_by_angle[1 , points_per_ring - (points_per_sector // 2): ]
class_predictions_by_angle_idx = np.argwhere(
class_predictions_by_angle >= (2 * segmenter_threshold))
else:
class_predictions_by_angle = np.squeeze(
class_predictions_by_angle.reshape(
(-1, points_per_ring, len(rings))),
axis=0)
if segmenter_threshold == 0.:
# dynamic thresholding
number_of_segmented_points = 0
dyn_threshold = 0.7
while (number_of_segmented_points < 100) and dyn_threshold >= 0.2:
class_predictions_by_angle_thresholded = (
class_predictions_by_angle >= dyn_threshold)
number_of_segmented_points = np.sum(
class_predictions_by_angle_thresholded)
dyn_threshold -= 0.1
class_predictions_by_angle_idx = np.argwhere(
class_predictions_by_angle_thresholded)
# print(dyn_threshold + 0.1, number_of_segmented_points)
else:
#for prob in [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8]:
# print(prob, np.sum(class_predictions_by_angle >= prob))
class_predictions_by_angle_idx = np.argwhere(
class_predictions_by_angle >= segmenter_threshold)
filtered_points_xyz = np.empty((0, 3))
if (class_predictions_by_angle_idx.shape[0] > 0):
# the idea of de-interpolation is to remove artifacts created by same-neighbor interpolation
# by checking repeated values (which are going to be nearest-neighbor interpolated values with high prob)
# for code convenience, we'e just taking the unique indexes as returned by np.unique but we
# could further improve this by calculating the center of mass on the X axis of the prediction
# vector (with the unique elements only), and take the index closest to the center for each duplicated stride.
if deinterpolate:
#((a.shape[0] - 1 - i_l ) + (i_f)) // 2
deinterpolated_class_predictions_by_angle_idx = np.empty((0, 2))
lidar_d_interpolated = lidar_d.reshape(
(-1, points_per_ring, len(rings)))[0]
for ring in range(len(rings)):
predictions_idx_in_ring = class_predictions_by_angle_idx[
class_predictions_by_angle_idx[:, 1] == ring]
if predictions_idx_in_ring.shape[0] > 1:
lidar_d_predictions_in_ring = lidar_d_interpolated[
predictions_idx_in_ring[:, 0], ring]
_, lidar_d_predictions_in_ring_unique_idx_first = np.unique(
lidar_d_predictions_in_ring, return_index=True)
_, lidar_d_predictions_in_ring_unique_idx_last = np.unique(
lidar_d_predictions_in_ring[::-1], return_index=True)
lidar_d_predictions_in_ring_unique_idx = \
(lidar_d_predictions_in_ring.shape[0] - 1 - lidar_d_predictions_in_ring_unique_idx_last + lidar_d_predictions_in_ring_unique_idx_first ) // 2
deinterpolated_class_predictions_by_angle_idx_this_ring = \
predictions_idx_in_ring[lidar_d_predictions_in_ring_unique_idx]
deinterpolated_class_predictions_by_angle_idx = np.concatenate(
(deinterpolated_class_predictions_by_angle_idx,
deinterpolated_class_predictions_by_angle_idx_this_ring
))
class_predictions_by_angle_idx = deinterpolated_class_predictions_by_angle_idx.astype(
int)
segmented_points = lidar_int[class_predictions_by_angle_idx[:, 0] +
points_per_ring *
class_predictions_by_angle_idx[:, 1]]
# remove capture vehicle, helps in ford01.bag (and doesn't hurt)
segmented_points = DidiTracklet._remove_capture_vehicle(
segmented_points)
# TODO: use PREDICTED position instead of last known for false positive rejection
if reject_false_positives and last_known_position is not None and segmented_points.shape[
0] > 2:
original_number_of_points = segmented_points.shape[0]
time_start = time.time()
rfp_implementation = 2
if rfp_implementation == 1:
import hdbscan
clusterer = hdbscan.HDBSCAN(allow_single_cluster=True,
metric='l2',
min_cluster_size=50)
cluster_labels = clusterer.fit_predict(segmented_points[:, :2])
number_of_clusters = len(
set(cluster_labels)) - (1 if -1 in cluster_labels else 0)
if verbose:
print("Clusters " + str(number_of_clusters) + ' from ' +
str(segmented_points.shape[0]) + ' points')
if number_of_clusters > 1:
unique_clusters = set(cluster_labels)
closest_cluster_center_of_mass = np.array([1e20, 1e20])
best_cluster = None
for cluster in unique_clusters:
points_in_cluster = segmented_points[cluster_labels ==
cluster]
points_in_cluster_center_of_mass = np.mean(
points_in_cluster[:, :2], axis=0)
if verbose:
print(cluster, points_in_cluster.shape,
points_in_cluster_center_of_mass)
if cluster != -1:
if np.linalg.norm(closest_cluster_center_of_mass - last_known_position[:2]) >\
np.linalg.norm(points_in_cluster_center_of_mass - last_known_position[:2]):
closest_cluster_center_of_mass = points_in_cluster_center_of_mass
best_cluster = cluster
#if verbose: print("best cluster", best_cluster, 'last_known_position at ', last_known_position)
selected_clusters = [best_cluster]
for cluster in unique_clusters:
if (cluster != -1) and (cluster != best_cluster):
points_in_cluster = segmented_points[cluster_labels
== cluster]
distances_from_last_known_position = np.linalg.norm(
points_in_cluster[:, :2] -
closest_cluster_center_of_mass,
axis=1)
if np.all(distances_from_last_known_position < 2.
): # TODO ADJUST
selected_clusters.append(cluster)
filtered_points_xyz = segmented_points[np.in1d(
cluster_labels, selected_clusters, invert=True), :3]
segmented_points = segmented_points[np.in1d(
cluster_labels, selected_clusters)]
if verbose:
print('selected_clusters: ' + str(selected_clusters) +
' with points ' +
str(segmented_points.shape[0]) + '/' +
str(original_number_of_points))
else:
within_tolerance_idx = (
((segmented_points[:, 0] - last_known_position[0])**2 +
(segmented_points[:, 0] - last_known_position[0])**2) <=
(5**2))
filtered_points_xyz = segmented_points[
~within_tolerance_idx, :3]
if filtered_points_xyz.shape[0] < original_number_of_points:
segmented_points = segmented_points[within_tolerance_idx]
if verbose:
print 'clustering filter: {:.2f}ms'.format(
1e3 * (time.time() - time_start))
else:
segmented_points = np.empty((0, 3))
detection = 0
pose = np.zeros((3))
box_size = np.zeros((3))
yaw = np.zeros((1))
if segmented_points.shape[0] >= localizer_points_threshold:
detection = 1
# filter outlier points
if True:
time_start = time.time()
cloud_orig = pcl.PointCloud(segmented_points[:, :3].astype(
np.float32))
fil = cloud_orig.make_statistical_outlier_filter()
fil.set_mean_k(50)
fil.set_std_dev_mul_thresh(1.0)
inlier_inds = fil.filter_ind()
segmented_points = segmented_points[inlier_inds, :]
if verbose:
print 'point cloud filter: {:.2f}ms'.format(
1e3 * (time.time() - time_start))
#filtered_points_xyz = segmented_points[:,:3]
segmented_points_mean = np.mean(segmented_points[:, :3], axis=0)
angle = np.arctan2(segmented_points_mean[1], segmented_points_mean[0])
aligned_points = point_utils.rotZ(segmented_points, angle)
segmented_and_aligned_points_mean = np.mean(aligned_points[:, :3],
axis=0)
aligned_points[:, :3] -= segmented_and_aligned_points_mean
distance_to_segmented_and_aligned_points = np.linalg.norm(
segmented_and_aligned_points_mean[:2])
aligned_points_resampled = DidiTracklet.resample_lidar(
aligned_points[:, :4], pointnet_points)
aligned_points_resampled = np.expand_dims(aligned_points_resampled,
axis=0)
distance_to_segmented_and_aligned_points = np.expand_dims(
distance_to_segmented_and_aligned_points, axis=0)
if K._backend == 'tensorflow':
with tf_localizer_graph.as_default():
time_loc_infe_start = time.time()
pose, box_size, yaw = localizer_model.predict_on_batch([
aligned_points_resampled,
distance_to_segmented_and_aligned_points
])
time_loc_infe_end = time.time()
else:
time_loc_infe_start = time.time()
pose, box_size, yaw = localizer_model.predict_on_batch([
aligned_points_resampled,
distance_to_segmented_and_aligned_points
])
time_loc_infe_end = time.time()
pose = np.squeeze(pose, axis=0)
box_size = np.squeeze(box_size, axis=0)
yaw = np.squeeze(yaw, axis=0)
if verbose:
print ' Loc inference: %0.3f ms' % (
(time_loc_infe_end - time_loc_infe_start) * 1000.0)
pose += segmented_and_aligned_points_mean
pose = point_utils.rotZ(pose, -angle)
yaw = point_utils.remove_orientation(yaw + angle)
pose_angle = np.arctan2(pose[1], pose[0])
angle_diff = angle_at_edge - pose_angle
if angle_diff < 0.:
angle_diff += 2 * np.pi
# ALI => delta_time is the time difference in milliseconds (0-100) from the start of the lidar
# scan to the time the object was detected, i don' know if the lidar msg is referenced to be
# beginning of the scan or the end... so basically adjust the lidar observation for cases which
# we need to test:
# observation_time = msg.header.stamp.to_sec() + delta_time
# observation_time = msg.header.stamp.to_sec() - delta_time
# observation_time = msg.header.stamp.to_sec() + 100 msecs - delta_time
delta_time = 0.1 * angle_diff / (2 * np.pi)
if verbose: print(angle_at_edge, pose_angle, angle_diff)
if verbose: print(pose, box_size, yaw, delta_time)
if delta_time < 0:
print(angle_at_edge, pose_angle, angle_diff, delta_time)
# fix lidar static tilt
Rx = rotXMat(np.deg2rad(g_roll_correction))
Ry = rotYMat(np.deg2rad(g_pitch_correction))
Rz = rotZMat(np.deg2rad(g_yaw_correction))
pose = Rz.dot(Ry.dot(Rx.dot([pose[0], pose[1], pose[2]])))
pose[2] += g_z_correction
# scale bbox size
box_size[2] = g_bbox_scale_l * box_size[2]
box_size[1] = g_bbox_scale_w * box_size[1]
box_size[0] = g_bbox_scale_h * box_size[0]
last_known_position = pose
last_known_box_size = box_size
# FUSION
with g_fusion_lock:
observation = LidarObservation(msg.header.stamp.to_sec(), pose[0],
pose[1], pose[2], yaw)
g_fusion.filter(observation)
segmented_points_cloud_msg = pc2.create_cloud_xyz32(
msg.header, segmented_points[:, :3])
# publish car prediction data as separate regular ROS messages just for vizualization (dunno how to visualize custom messages in rviz)
publish_rviz_topics = False
if publish_rviz_topics and detection > 0:
# point cloud
seg_pnt_pub = rospy.Publisher(name='segmented_obj',
data_class=PointCloud2,
queue_size=1)
seg_msg = PointCloud2()
seg_pnt_pub.publish(segmented_points_cloud_msg)
# car pose frame
yaw_q = ros_tf.transformations.quaternion_from_euler(0, 0, yaw)
br = ros_tf.TransformBroadcaster()
br.sendTransform(tuple(pose), tuple(yaw_q), rospy.Time.now(),
'obj_lidar_centroid', 'velodyne')
# give bbox different color, depending on the predicted object class
if detection == 1: # car
bbox_color = ColorRGBA(r=1., g=1., b=0., a=0.5)
else: # ped
bbox_color = ColorRGBA(r=0., g=0., b=1., a=0.5)
# bounding box
marker = Marker()
marker.header.frame_id = "obj_lidar_centroid"
marker.header.stamp = rospy.Time.now()
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.scale.x = box_size[2]
marker.scale.y = box_size[1]
marker.scale.z = box_size[0]
marker.color = bbox_color
marker.lifetime = rospy.Duration()
pub = rospy.Publisher("obj_lidar_bbox", Marker, queue_size=10)
pub.publish(marker)
# filtered point cloud
fil_points_msg = pc2.create_cloud_xyz32(msg.header,
filtered_points_xyz)
rospy.Publisher(name='segmented_filt_obj',
data_class=PointCloud2,
queue_size=1).publish(fil_points_msg)
print "TIME: ", time.time() - t_start
return {
'detection': detection,
'x': pose[0],
'y': pose[1],
'z': pose[2],
'l': box_size[2],
'w': box_size[1],
'h': box_size[0],
'yaw': np.squeeze(yaw)
}
def addEllipsoid(self, ellipsoid_type='collision'):
if ellipsoid_type == 'collision':
id = 999
color = ColorRGBA(r=0, g=1, b=0, a=0.5)
cube = Marker(header=Header(stamp=rospy.Time.now(),
frame_id=self.frame_id),
pose=self.collision_ellipsoid_origin,
type=Marker.SPHERE,
color=color,
id=id,
scale=Vector3(self.collision_ellipsoid_size_x,
self.collision_ellipsoid_size_y,
self.collision_ellipsoid_size_z),
action=Marker.ADD)
self.models[id] = cube
# self.deleteEllipsoid(ellipsoid_type='reaching')
elif ellipsoid_type == 'reaching':
id = 1000
color = ColorRGBA(r=1, g=0, b=1, a=1)
cube = Marker(header=Header(stamp=rospy.Time.now(),
frame_id=self.frame_id),
pose=self.reaching_ellipsoid_origin,
type=Marker.SPHERE,
color=color,
id=id,
scale=Vector3(self.reaching_ellipsoid_size_x,
self.reaching_ellipsoid_size_y,
self.reaching_ellipsoid_size_z),
action=Marker.ADD)
self.models[id] = cube
# self.deleteEllipsoid(ellipsoid_type='collision')
elif ellipsoid_type == 'all':
id = 999
color = ColorRGBA(r=0, g=1, b=0, a=0.5)
cube = Marker(header=Header(stamp=rospy.Time.now(),
frame_id=self.frame_id),
pose=self.collision_ellipsoid_origin,
type=Marker.SPHERE,
color=color,
id=id,
scale=Vector3(self.collision_ellipsoid_size_x,
self.collision_ellipsoid_size_y,
self.collision_ellipsoid_size_z),
action=Marker.ADD)
self.models[id] = cube
id = 1000
color = ColorRGBA(r=1, g=0, b=1, a=1)
cube = Marker(header=Header(stamp=rospy.Time.now(),
frame_id=self.frame_id),
pose=self.reaching_ellipsoid_origin,
type=Marker.SPHERE,
color=color,
id=id,
scale=Vector3(self.reaching_ellipsoid_size_x,
self.reaching_ellipsoid_size_y,
self.reaching_ellipsoid_size_z),
action=Marker.ADD)
self.models[id] = cube
def node():
global frontiers, mapData, global1, global2, global3, globalmaps, litraIndx, n_robots, namespace_init_count
rospy.init_node('filter', anonymous=False)
#rospy.loginfo('filter and send: I am here')
# fetching all parameters
map_topic = rospy.get_param('~map_topic', '/map')
threshold = rospy.get_param('~costmap_clearing_threshold', 70)
# this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate
info_radius = rospy.get_param('~info_radius', 1.0)
goals_topic = rospy.get_param('~goals_topic', '/detected_points')
n_robots = rospy.get_param('~n_robots', 1)
namespace = rospy.get_param('~namespace', '')
namespace_init_count = rospy.get_param('namespace_init_count', 1)
rateHz = rospy.get_param('~rate', 100)
global_costmap_topic = rospy.get_param(
'~global_costmap_topic', '/move_base/global_costmap/costmap'
) #Initially /move_base_node/global_costmap/costmap
robot_frame = rospy.get_param('~robot_frame', 'base_link')
litraIndx = len(namespace)
rate = rospy.Rate(rateHz)
# -------------------------------------------
rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack)
# ---------------------------------------------------------------------------------------------------------------
for i in range(0, n_robots):
globalmaps.append(OccupancyGrid())
if len(namespace) > 0:
for i in range(0, n_robots):
rospy.Subscriber(
namespace + str(i + namespace_init_count) +
global_costmap_topic, OccupancyGrid, globalMap)
elif len(namespace) == 0:
rospy.Subscriber(global_costmap_topic, OccupancyGrid, globalMap)
# wait if map is not received yet
while (len(mapData.data) < 1):
rospy.loginfo('Waiting for the map')
rospy.sleep(0.1)
pass
# wait if any of robots' global costmap map is not received yet
for i in range(0, n_robots):
while (len(globalmaps[i].data) < 1):
rospy.loginfo('Waiting for the global costmap')
rospy.sleep(0.1)
pass
global_frame = "/" + mapData.header.frame_id
tfLisn = tf.TransformListener()
if len(namespace) > 0:
for i in range(0, n_robots):
tfLisn.waitForTransform(
global_frame[1:],
namespace + str(i + namespace_init_count) + '/' + robot_frame,
rospy.Time(0), rospy.Duration(10.0))
elif len(namespace) == 0:
tfLisn.waitForTransform(global_frame[1:], '/' + robot_frame,
rospy.Time(0), rospy.Duration(10.0))
rospy.Subscriber(goals_topic,
PointStamped,
callback=callBack,
callback_args=[tfLisn, global_frame[1:]])
pub = rospy.Publisher('frontiers', Marker, queue_size=10)
pub2 = rospy.Publisher('centroids', Marker, queue_size=10)
filterpub = rospy.Publisher('filtered_points', PointArray, queue_size=10)
rospy.loginfo("the map and global costmaps are received")
# wait if no frontier is received yet
while len(frontiers) < 1:
pass
#rospy.loginfo('filter and send: I am here')
points = Marker()
points_clust = Marker()
# Set the frame ID and timestamp. See the TF tutorials for information on these.
points.header.frame_id = mapData.header.frame_id
points.header.stamp = rospy.Time.now()
points.ns = "markers2"
points.id = 0
points.type = Marker.POINTS
# Set the marker action for latched frontiers. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
points.action = Marker.ADD
points.pose.orientation.w = 1.0
points.scale.x = 0.2
points.scale.y = 0.2
points.color.r = 255.0 / 255.0
points.color.g = 255.0 / 255.0
points.color.b = 0.0 / 255.0
points.color.a = 1
points.lifetime = rospy.Duration()
p = Point()
p.z = 0
pp = []
pl = []
points_clust.header.frame_id = mapData.header.frame_id
points_clust.header.stamp = rospy.Time.now()
points_clust.ns = "markers3"
points_clust.id = 4
points_clust.type = Marker.POINTS
# Set the marker action for centroids. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
points_clust.action = Marker.ADD
points_clust.pose.orientation.w = 1.0
points_clust.scale.x = 0.2
points_clust.scale.y = 0.2
points_clust.color.r = 0.0 / 255.0
points_clust.color.g = 255.0 / 255.0
points_clust.color.b = 0.0 / 255.0
points_clust.color.a = 1
points_clust.lifetime = rospy.Duration()
temppoint = PointStamped()
temppoint.header.frame_id = mapData.header.frame_id
temppoint.header.stamp = rospy.Time(0)
temppoint.point.z = 0.0
arraypoints = PointArray()
tempPoint = Point()
tempPoint.z = 0.0
# -------------------------------------------------------------------------
# --------------------- Main Loop -------------------------------
# -------------------------------------------------------------------------
while not rospy.is_shutdown():
# -------------------------------------------------------------------------
# Clustering frontier points
centroids = []
front = copy(frontiers)
if len(front) > 1:
ms = MeanShift(bandwidth=0.3)
ms.fit(front)
centroids = ms.cluster_centers_ # centroids array is the centers of each cluster
# if there is only one frontier no need for clustering, i.e. centroids=frontiers
if len(front) == 1:
centroids = front
# -------------------------------------------------------------------------
# clearing old frontiers
z = 0
while z < len(centroids):
cond = False
temppoint.point.x = centroids[z][0]
temppoint.point.y = centroids[z][1]
for i in range(0, n_robots):
transformedPoint = tfLisn.transformPoint(
globalmaps[i].header.frame_id, temppoint)
x = array([transformedPoint.point.x, transformedPoint.point.y])
cond = (gridValue(globalmaps[i], x) > threshold) or cond
if (cond or
(informationGain(mapData, [centroids[z][0], centroids[z][1]],
info_radius * 0.5)) < 0.2):
centroids = delete(centroids, (z), axis=0)
z = z - 1
z += 1
frontiers = copy(centroids) #Change this to after the cleannup
# -------------------------------------------------------------------------
# publishing
arraypoints.points = []
for i in centroids:
tempPoint.x = i[0]
tempPoint.y = i[1]
arraypoints.points.append(copy(tempPoint))
filterpub.publish(arraypoints)
pp = []
for q in range(0, len(frontiers)):
p.x = frontiers[q][0]
p.y = frontiers[q][1]
pp.append(copy(p))
points.points = pp
pp = []
for q in range(0, len(centroids)):
p.x = centroids[q][0]
p.y = centroids[q][1]
pp.append(copy(p))
points_clust.points = pp
pub.publish(points)
pub2.publish(points_clust)
rate.sleep()
import rospy
from sailboat_message.msg import WTST_msg
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped, Point
from visualization_msgs.msg import Marker
## fleet race
# target_x = [0, 58.7091157621, 64.8362530808, 250.098605093, 213.669988671]
# target_y = [0, 115.264402922, 91.6514720561, 112.199262859, 207.218604858], e5: 366.770497586
## station_keeping
target_x = [0, -151.061785525]
target_y = [0, 366.770497586]
path = Path()
marker = Marker()
marker.header.frame_id = 'WTST'
marker.type = marker.POINTS
marker.action = marker.ADD
marker.pose.orientation.w = 1
marker.scale.x = 5.0
marker.scale.y = 5.0
marker.scale.z = 5.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0
marker.color.a = 0.5
for xx, yy in zip(target_x, target_y):
p = Point()
p.x = xx
p.y = yy
def __init__(self):
rospy.init_node('pub_3d_target')
rate = rospy.get_param('~rate', 10)
r = rospy.Rate(rate)
# Remap this frame in the launch file or command line if necessary
#target_frame = 'base_link'
#target_frame = 'kinect2_link'
target_topic = 'target_point'
#target_topic = 'clicked_point'
# Parameters for publishing the RViz target marker
marker_scale = rospy.get_param('~marker_scale', 0.1)
marker_lifetime = rospy.get_param('~marker_lifetime',
1) # 0 is forever
marker_ns = rospy.get_param('~marker_ns', 'target_point')
marker_id = rospy.get_param('~marker_id', 0)
marker_color = rospy.get_param('~marker_color', {
'r': 0.0,
'g': 1.0,
'b': 0.0,
'a': 1.0
})
# Define a marker publisher
self.marker_pub = rospy.Publisher('target_point_marker',
Marker,
queue_size=10)
self.clicked_sub = rospy.Subscriber('clicked_point', PointStamped,
self.clicked_callback)
#target_pub = rospy.Publisher(target_topic, PointStamped, queue_size=10)
# Initialize the marker
self.marker = Marker()
self.marker.ns = marker_ns
self.marker.id = marker_id
self.marker.type = Marker.SPHERE
self.marker.action = Marker.ADD
self.marker.lifetime = rospy.Duration(marker_lifetime)
self.marker.scale.x = marker_scale
self.marker.scale.y = marker_scale
self.marker.scale.z = marker_scale
self.marker.color.r = marker_color['r']
self.marker.color.g = marker_color['g']
self.marker.color.b = marker_color['b']
self.marker.color.a = marker_color['a']
# Define the target as a PointStamped message
self.target = PointStamped()
self.target.point.x = 100.0
self.target.point.y = 0.0
self.target.point.z = 0.0
self.target.header.stamp = rospy.Time.now()
self.target.header.frame_id = 'base_link'
# Define the target publisher
self.target_pub = rospy.Publisher(target_topic,
PointStamped,
queue_size=10)
# Initialize tf listener
self.tf = tf.TransformListener()
# Give the tf buffer a chance to fill up
rospy.sleep(5.0)
rospy.loginfo(
"Publishing target point on frame ") # + str(target_frame))
while not rospy.is_shutdown():
r.sleep()
while not rospy.is_shutdown():
rospy.sleep(0.1)
waypoints = new_waypoints
wpoints = []
while waypoints is None or start_flag == 0:
if waypoints is None:
print "no waypoints"
lock_aux_pointx = 0
waypoints = new_waypoints
for i in range(waypoints.shape[0]):
p = Point()
p.x = waypoints[i][0]
p.y = waypoints[i][1]
p.z = 0
wpoints.append(p)
marker = Marker()
marker.header.frame_id = "/odom"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.pose.orientation.w = 1
marker.points = wpoints
t = rospy.Duration()
marker.lifetime = t
marker.scale.x = 0.4
marker.scale.y = 0.4
marker.scale.z = 0.4
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
def plotPolygons(self):
# https://answers.ros.org/question/203782/rviz-marker-line_strip-is-not-displayed/
marker = Marker()
marker.header.frame_id = "yumi_base_link"
marker.type = marker.LINE_LIST
marker.action = marker.ADD
# marker scale
marker.scale.x = 0.002
marker.scale.y = 0.002
marker.scale.z = 0.002
# marker color
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
# marker orientaiton
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
# marker position
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
objectList = self.objects
# marker line points
marker.points = []
for j in range(len(objectList)):
for ii in range(objectList[j].lines.shape[0]):
lineIndex = objectList[j].lines[ii]
lineVerticeis = objectList[j].verticesTransformed[lineIndex]
height = objectList[j].heightTransformed
x_data = lineVerticeis[:, 0].tolist()
y_data = lineVerticeis[:, 1].tolist()
point1 = Point()
point1.x = x_data[0]
point1.y = y_data[0]
point1.z = height[0]
marker.points.append(point1)
point2 = Point()
point2.x = x_data[1]
point2.y = y_data[1]
point2.z = height[0]
marker.points.append(point2)
point3 = Point()
point3.x = x_data[0]
point3.y = y_data[0]
point3.z = height[1]
marker.points.append(point1)
marker.points.append(point3)
point4 = Point()
point4.x = x_data[1]
point4.y = y_data[1]
point4.z = height[1]
marker.points.append(point2)
marker.points.append(point4)
point5 = Point()
point5.x = x_data[0]
point5.y = y_data[0]
point5.z = height[1]
marker.points.append(point5)
point6 = Point()
point6.x = x_data[1]
point6.y = y_data[1]
point6.z = height[1]
marker.points.append(point6)
self.linePub.publish(marker)
def gnd_marker_pub(gnd_label, marker_pub, cfg, color = "red"):
length = int(cfg.grid_range[2] - cfg.grid_range[0]) # x direction
width = int(cfg.grid_range[3] - cfg.grid_range[1]) # y direction
gnd_marker = Marker()
gnd_marker.header.frame_id = "/kitti/base_link"
gnd_marker.header.stamp = rospy.Time.now()
gnd_marker.type = gnd_marker.LINE_LIST
gnd_marker.action = gnd_marker.ADD
gnd_marker.scale.x = 0.05
gnd_marker.scale.y = 0.05
gnd_marker.scale.z = 0.05
if(color == "red"):
gnd_marker.color.a = 1.0
gnd_marker.color.r = 1.0
gnd_marker.color.g = 0.0
gnd_marker.color.b = 0.0
if(color == "green"):
gnd_marker.color.a = 1.0
gnd_marker.color.r = 0.0
gnd_marker.color.g = 1.0
gnd_marker.color.b = 0.0
gnd_marker.points = []
# gnd_labels are arranged in reverse order
for j in range(gnd_label.shape[0]):
for i in range(gnd_label.shape[1]):
pt1 = Point()
pt1.x = i + cfg.grid_range[0]
pt1.y = j + cfg.grid_range[1]
pt1.z = gnd_label[j,i]
if j>0 :
pt2 = Point()
pt2.x = i + cfg.grid_range[0]
pt2.y = j-1 +cfg.grid_range[1]
pt2.z = gnd_label[j-1, i]
gnd_marker.points.append(pt1)
gnd_marker.points.append(pt2)
if i>0 :
pt2 = Point()
pt2.x = i -1 + cfg.grid_range[0]
pt2.y = j + cfg.grid_range[1]
pt2.z = gnd_label[j, i-1]
gnd_marker.points.append(pt1)
gnd_marker.points.append(pt2)
if j < width-1 :
pt2 = Point()
pt2.x = i + cfg.grid_range[0]
pt2.y = j + 1 + cfg.grid_range[1]
pt2.z = gnd_label[j+1, i]
gnd_marker.points.append(pt1)
gnd_marker.points.append(pt2)
if i < length-1 :
pt2 = Point()
pt2.x = i + 1 + cfg.grid_range[0]
pt2.y = j + cfg.grid_range[1]
pt2.z = gnd_label[j, i+1]
gnd_marker.points.append(pt1)
gnd_marker.points.append(pt2)
marker_pub.publish(gnd_marker)
def forward_kinematics(data):
if not correct(data):
rospy.logerr('Incorrect position! ' + str(data))
return
a, d, al, th = params['i1']
al, a, d, th = float(al), float(a), float(d), float(th)
tz = translation_matrix((0, 0, d))
rz = rotation_matrix(data.position[0], zaxis)
tx = translation_matrix((a, 0, 0))
rx = rotation_matrix(al, xaxis)
T1 = concatenate_matrices(rx, tx, rz, tz)
a, d, al, th = params['i2']
al, a, d, th = float(al), float(a), float(d), float(th)
tz = translation_matrix((0, 0, d))
rz = rotation_matrix(data.position[1], zaxis)
tx = translation_matrix((a, 0, 0))
rx = rotation_matrix(al, xaxis)
T2 = concatenate_matrices(rx, tx, rz, tz)
a, d, al, th = params['i3']
al, a, d, th = float(al), float(a), float(d), float(th)
tz = translation_matrix((0, 0, data.position[2]))
rz = rotation_matrix(th, zaxis)
tx = translation_matrix((a, 0, 0))
rx = rotation_matrix(al, xaxis)
T3 = concatenate_matrices(rx, tx, rz, tz)
Tk = concatenate_matrices(T1, T2, T3)
x, y, z = translation_from_matrix(Tk)
qx, qy, qz, qw = quaternion_from_matrix(Tk)
pose = PoseStamped()
pose.header.frame_id = 'base_link'
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
pose.pose.orientation.x = qx
pose.pose.orientation.y = qy
pose.pose.orientation.z = qz
pose.pose.orientation.w = qw
pub.publish(pose)
marker = Marker()
marker.header.frame_id = 'base_link'
marker.type = marker.CUBE
marker.action = marker.ADD
marker.pose.orientation.w = 1
time = rospy.Duration()
marker.lifetime = time
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.pose.position.x = x
marker.pose.position.y = y
marker.pose.position.z = z
marker.pose.orientation.x = qx
marker.pose.orientation.y = qy
marker.pose.orientation.z = qz
marker.pose.orientation.w = qw
marker.color.a = 0.7
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
marker_pub.publish(marker)
def publish_3dbox(box3d_pub, corners_3d_velos, types, track_ids): marker_array = MarkerArray() for track_id, corners_3d_velo in enumerate(corners_3d_velos): marker = Marker() marker.header.frame_id = FRAME_ID marker.header.stamp = rospy.Time.now() marker.id = track_id marker.action = Marker.ADD marker.lifetime = rospy.Duration(LIFETIME) marker.type = Marker.LINE_LIST b, g, r = DETECTION_COLOR_MAP[types[track_id]] marker.color.r = r/255.0 marker.color.g = g/255.0 marker.color.b = b/255.0 marker.color.a = 1.0 marker.scale.x = 0.1 marker.points = [] for l in LINES: p1 = corners_3d_velo[l[0]] marker.points.append(Point(p1[0], p1[1], p1[2])) p2 = corners_3d_velo[l[1]] marker.points.append(Point(p2[0], p2[1], p2[2])) marker_array.markers.append(marker) text_marker = Marker() text_marker.header.frame_id = FRAME_ID text_marker.header.stamp = rospy.Time.now() text_marker.id = track_id + 1000 #avoid rpeat same id text_marker.action = Marker.ADD text_marker.lifetime = rospy.Duration(LIFETIME) text_marker.type = Marker.TEXT_VIEW_FACING p4 = corners_3d_velo[4]# upper front left corner text_marker.pose.position.x = p4[0] text_marker.pose.position.y = p4[1] text_marker.pose.position.z = p4[2] + 0.5 text_marker.text = str(track_ids[track_id]) text_marker.scale.x = 1 text_marker.scale.y = 1 text_marker.scale.z = 1 b, g, r = DETECTION_COLOR_MAP[types[track_id]] text_marker.color.r = r/255.0 text_marker.color.g = g/255.0 text_marker.color.b = b/255.0 text_marker.color.a = 1.0 marker_array.markers.append(text_marker) marker = Marker() marker.header.frame_id = FRAME_ID marker.header.stamp = rospy.Time.now() marker.id = -1 marker.action = Marker.ADD marker.lifetime = rospy.Duration(LIFETIME) marker.type = Marker.LINE_LIST marker.color.r = 0 marker.color.g = 1 marker.color.b = 1 marker.color.a = 1.0 marker.scale.x = 0.1 marker.points = [] for l in LINES: print(l) p1 = EGOCAR[l[0]] print(p1) marker.points.append(Point(p1[0], p1[1], p1[2])) p2 = EGOCAR[l[1]] print(p2) marker.points.append(Point(p2[0], p2[1], p2[2])) marker_array.markers.append(marker) text_marker = Marker() text_marker.header.frame_id = FRAME_ID text_marker.header.stamp = rospy.Time.now() text_marker.id = -2 text_marker.action = Marker.ADD text_marker.lifetime = rospy.Duration(LIFETIME) text_marker.type = Marker.TEXT_VIEW_FACING p4 = EGOCAR[4]# upper front left corner text_marker.pose.position.x = p4[0] text_marker.pose.position.y = p4[1] text_marker.pose.position.z = p4[2] + 0.5 text_marker.text = 'EGOCAR' text_marker.scale.x = 1 text_marker.scale.y = 1 text_marker.scale.z = 1 b, g, r = DETECTION_COLOR_MAP['Car'] text_marker.color.r = r/255.0 text_marker.color.g = g/255.0 text_marker.color.b = b/255.0 text_marker.color.a = 1.0 marker_array.markers.append(text_marker) box3d_pub.publish(marker_array)
class MissionPlanner(object):
x0, y0, yaw0 = 0, 0, 0
enemy_pos = []
## Movement PID constants
del_T = 100.0 # time step in ms
p_ang = 140.0 #140, 140
i_ang = 0.02 #0.013, 0.01
d_ang = 55000.0 #50000, 56000
p_x = 450.0 #350
i_x = 0.1 #0.2
d_x = 100.0 #200, 300
p_y = 850.0 #600
i_y = 0.1 #0.1
d_y = 120.0 #120, 180
x_lin_vel_thres = 660.0 # max 660
y_lin_vel_thres = 660
ang_vel_thres = 660.0 # max 660
bias = 1024.0
pre_ang_error = 0.0
pre_x_error = 0.0
pre_y_error = 0.0
ang_integral = 0.0
x_integral = 0.0
y_integral = 0.0
lin_integral_threshold = 50.0
ang_integral_threshold = 20.0
# path function parameters
T_step = 100.0 #time step in ms, 10Hz
Ax = 1.0 #amplitude
Ay = 1.0
Px = 2500 #period in ms
Py = 5000
Lx = 1000.0 #phase lag
Ly = 0.0
t = 0
path = 1
counter = 1
#preferred direction of active dodging
isleft = True
#turret control params
state_x = 0.0
state_y = 0.0
statep_x = state_x
statep_y = state_y
stash = []
prevsh = 0
updatetime = time()
#camera parameters
# xMax = rospy.get_param('/usb_cam/image_width') / 10.0
# yMax = rospy.get_param('/usb_cam/image_height') / 10.0
xMax = 1024.0 / 10.0
yMax = 576.0 / 10.0
error_x = xMax / 2.0
#heatmap to publish
img = Image()
img.header.frame_id = '/heatmap'
img.height = int(yMax)
img.width = int(xMax)
img.encoding = 'mono8'
img.step = int(xMax)
Kp_x = 1.3
Ki_x = 0
Kd_x = 1.5
Kp_y = 1.5
Ki_y = 0
Kd_y = 0.8
target_x = xMax / 2.0
target_y = yMax / 2.0
errorp_x = 0
errorp_y = 0
#cmd vel params
cmd_x = bias
cmd_y = bias
cmd_yaw = bias
cmd_yaw_turret = bias
cmd_pitch = bias
cmd_shoot = 0
startshoottime = 0
# path_marker params
path_marker = []
path_marker_done = []
# ref_marker
ref_marker = Marker()
def __init__(self, nodename):
rospy.init_node(nodename, anonymous=False)
print self.d_ang
rospy.Subscriber("/odometry",
Odometry,
self.odom_callback,
queue_size=50)
rospy.Subscriber("/enemy_yolo",
Marker,
self.enemy_callback,
queue_size=50)
rospy.Subscriber('/roi',
RegionOfInterest,
self.armor_callback,
queue_size=50)
self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Joy, queue_size=10)
self.pubimg = rospy.Publisher('/heatmap', Image, queue_size=1)
self.pubpath_marker = rospy.Publisher('path_marker',
Marker,
queue_size=5)
self.pubref_marker = rospy.Publisher('ref_marker',
Marker,
queue_size=5)
# path_marker
for i in range(6):
self.path_marker_done.append(False)
self.path_marker.append(Marker())
self.path_marker[i].header.stamp = rospy.get_rostime()
self.path_marker[i].header.frame_id = "odom"
self.path_marker[i].ns = "points"
self.path_marker[i].type = self.path_marker[i].POINTS
self.path_marker[i].action = self.path_marker[i].ADD
self.path_marker[i].pose.orientation.w = 1.0
self.path_marker[i].id = 0
self.path_marker[i].scale.x = 0.02
self.path_marker[i].scale.y = 0.02
self.path_marker[i].scale.z = 0.02
self.path_marker[i].color.g = 1.0
self.path_marker[i].color.b = 1.0
self.path_marker[i].color.a = 1.0
# ref_marker
self.ref_marker.header.stamp = rospy.get_rostime()
self.ref_marker.header.frame_id = "odom"
self.ref_marker.ns = "points"
self.ref_marker.type = self.ref_marker.POINTS
self.ref_marker.action = self.ref_marker.ADD
self.ref_marker.pose.orientation.w = 1.0
self.ref_marker.id = 0
self.ref_marker.scale.x = 0.02
self.ref_marker.scale.y = 0.02
self.ref_marker.scale.z = 0.02
self.ref_marker.color.r = 1.0
self.ref_marker.color.a = 1.0
self.pitch_up()
rate = rospy.Rate(10)
msg = Joy()
heading_threshold = 20 * math.pi / 180
while not rospy.is_shutdown():
#self.translate(0, 0, 0)
#self.passive_dodge()
# if abs(self.yaw0-0)>heading_threshold:
# self.rotate(0)
# else:
# #else translate to goal
# self.translate(0, 0, 0)
if len(self.enemy_pos) == 1:
#if only one, active dodging
self.active_dodge()
else:
#more than one, passive dodge
self.passive_dodge_new()
# msg=Joy()
#do priority that governs yawing (lidar or vision)
if time() - self.updatetime < 1 and (
(self.state_x - self.statep_x < self.xMax / 5
and self.state_y - self.statep_y < self.yMax / 2)
or not self.stash) and abs(self.error_x) < self.xMax / 4:
# print("shoot")
#update time is only updated in shooting mode. armor detected
if self.cmd_shoot == 2 and time() - self.startshoottime < 3:
self.cmd_x = self.bias
self.cmd_y = self.bias
# if time()-self.startshoottime>3 and time()-self.startshoottime <4:
# self.cmd_pitch=1524 #pitch up
msg.buttons = [
int(self.cmd_x),
int(self.cmd_y),
int(self.cmd_yaw_turret),
int(self.cmd_pitch),
int(self.cmd_shoot)
]
else:
#no shooting, dodge and reset turret pid
if time() - self.updatetime > 2:
self.stash = []
heatmap = np.zeros((int(self.xMax), int(self.yMax)),
dtype=np.uint8)
self.img.data = np.resize(
np.ravel(heatmap.T, order='C'),
int(self.xMax) * int(self.yMax)).astype(np.uint8).tolist()
self.pubimg.publish(self.img)
self.state_x = 0
self.state_y = 0
self.cmd_shoot = 0
self.cmd_pitch = self.bias
msg.buttons = [
int(self.cmd_x),
int(self.cmd_y),
int(self.cmd_yaw),
int(self.cmd_pitch),
int(self.cmd_shoot)
]
#msg.buttons=[int(self.cmd_x), int(self.cmd_y), int(self.cmd_yaw), int(self.cmd_pitch), int(self.cmd_shoot)]
#print(msg)
self.cmd_vel_pub.publish(msg)
rate.sleep()
self.stop()
def generate_path(self):
pass
def pitch_up(self):
msg = Joy()
#pitch up to ensure armor in field of view
self.cmd_pitch = 1524
msg.buttons = [self.bias, self.bias, self.bias, int(self.cmd_pitch), 0]
def stop(self):
msg = Joy()
msg.buttons = [self.bias, self.bias, self.bias, self.bias, 0]
self.cmd_vel_pub.publish(msg)
def active_dodge(self):
target = self.enemy_pos[0]
#rotate to face target
heading = math.atan2(target[1] - self.y0, target[0] - self.x0)
heading_threshold = 40 * math.pi / 180
difference = abs(
math.atan2(math.sin(self.yaw0 - heading),
math.cos(self.yaw0 - heading)))
if difference > heading_threshold:
#print("rotate")
print(
math.atan2(math.sin(self.yaw0 - heading),
math.cos(self.yaw0 - heading)) * 180 / math.pi)
self.rotate(heading)
else:
#print("translate")
#the higher d, the faster
d = 0.6
#direction to the left
beta1 = heading + math.pi / 2
#direction to the right
beta2 = heading - math.pi / 2
#check if out of radius, assume middle of the arena is origin
if math.sqrt(self.x0**2 + self.y0**2) > 0.7:
#add to origin vector
delta = math.atan2(-self.y0, -self.x0)
#print(delta*180/math.pi)
pred1 = [
self.x0 + d * math.cos(beta1) + 0.2 * math.cos(delta),
self.y0 + d * math.sin(beta1) + 0.2 * math.sin(delta)
]
pred2 = [
self.x0 + d * math.cos(beta2) + 0.2 * math.cos(delta),
self.y0 + d * math.sin(beta2) + 0.2 * math.sin(delta)
]
else:
#predict position a timestep ahead
pred1 = [
self.x0 + d * math.cos(beta1),
self.y0 + d * math.sin(beta1)
]
pred2 = [
self.x0 + d * math.cos(beta2),
self.y0 + d * math.sin(beta2)
]
heading1 = math.atan2(target[1] - pred1[1], target[0] - pred1[0])
heading2 = math.atan2(target[1] - pred2[1], target[0] - pred2[0])
if self.inside_arena(pred1) == True and self.inside_arena(
pred2) == True:
#go to preferred direction
if self.isleft == True:
self.translate(pred1[0], pred1[1], heading1)
else:
self.translate(pred2[0], pred2[1], heading2)
elif self.inside_arena(pred1) == True and self.inside_arena(
pred2) == False:
self.translate(pred1[0], pred1[1], heading1)
self.isleft = True
elif self.inside_arena(pred1) == False and self.inside_arena(
pred2) == True:
self.translate(pred2[0], pred2[1], heading2)
self.isleft = False
else:
#stuck in corner, translate to origin
self.translate(0, 0, self.yaw0)
def x_plot(self, t, Lx, Ay, Ax):
#print("Ax : ",Ax)
#print("Lx : ",Lx)
#print("Ay : ",Ay)
return Ax * math.sin(
3 / 2 * math.pi * t * self.T_step / self.Px +
Lx) #*math.sin(1*math.pi*t*self.T_step/self.Px + Lx)
def y_plot(self, t, Lx, Ay, Ax):
#print("Ax : ",Ax)
#print("Lx : ",Lx)
#print("Ay : ",Ay)
return Ay * math.cos(
2 * math.pi * t * self.T_step / self.Py +
self.Ly) #*math.sin(1*math.pi*t*self.T_step/self.Px + Lx)
def passive_dodge(self):
if self.path == 1:
# ref_x = self.x_plot(self.t,0,0.5,0.25)
# ref_y = self.y_plot(self.t,0,0.5,0.25)
ref_x = self.x_plot(self.t, 0, 0.55, 0.55)
ref_y = self.y_plot(self.t, 0, 0.55, 0.55)
if self.path_marker_done[self.path - 1] == False:
points = []
for i in range(0, 1000):
p = Point()
# p.x = self.x_plot(i,0,0.5,0.25)
# p.y = self.y_plot(i,0,0.5,0.25)
p.x = self.x_plot(i, 0, 0.55, 0.55)
p.y = self.y_plot(i, 0, 0.55, 0.55)
p.z = 0.0
points.append(p)
self.path_marker[self.path - 1].points = points
self.path_marker_done[self.path - 1] = True
elif self.path == 2:
# ref_x = self.x_plot(self.t,-26,-0.45,-0.4)
# ref_y = self.y_plot(self.t,26,0.45,0.4)
ref_x = self.x_plot(self.t, -0, -0.45, -0.45)
ref_y = self.y_plot(self.t, 0, 0.45, 0.45)
if self.path_marker_done[self.path - 1] == False:
points = []
for i in range(0, 1000):
p = Point()
# p.x = self.x_plot(i,-26,-0.45,-0.4)
# p.y = self.y_plot(i,26,0.45,0.4)
p.x = self.x_plot(i, -0, -0.45, -0.45)
p.y = self.y_plot(i, 0, 0.45, 0.45)
p.z = 0.0
points.append(p)
self.path_marker[self.path - 1].points = points
self.path_marker_done[self.path - 1] = True
elif self.path == 3:
# ref_x = self.x_plot(self.t,-26,0.45,0.4)
# ref_y = self.y_plot(self.t,-26,0.45,0.4)
ref_x = self.x_plot(self.t, -0, 0.6, 0.6)
ref_y = self.y_plot(self.t, -0, 0.6, 0.6)
if self.path_marker_done[self.path - 1] == False:
points = []
for i in range(0, 1000):
p = Point()
# p.x = self.x_plot(i,-26,0.45,0.4)
# p.y = self.y_plot(i,-26,0.45,0.4)
p.x = self.x_plot(i, -0, 0.6, 0.6)
p.y = self.y_plot(i, -0, 0.6, 0.6)
p.z = 0.0
points.append(p)
self.path_marker[self.path - 1].points = points
self.path_marker_done[self.path - 1] = True
elif self.path == 4:
# ref_x = self.x_plot(self.t,0,-0.5,-0.25)
# ref_y = self.y_plot(self.t,0,0.5,0.25)
ref_x = self.x_plot(self.t, 0, -0.55, -0.55)
ref_y = self.y_plot(self.t, 0, 0.55, 0.55)
if self.path_marker_done[self.path - 1] == False:
points = []
for i in range(0, 1000):
p = Point()
# p.x = self.x_plot(i,0,-0.5,-0.25)
# p.y = self.y_plot(i,0,0.5,0.25)
p.x = self.x_plot(i, 0, -0.55, -0.55)
p.y = self.y_plot(i, 0, 0.55, 0.55)
p.z = 0.0
points.append(p)
self.path_marker[self.path - 1].points = points
self.path_marker_done[self.path - 1] = True
elif self.path == 5:
# ref_x = self.x_plot(self.t,26,0.45,0.4)
# ref_y = self.y_plot(self.t,26,0.45,0.4)
ref_x = self.x_plot(self.t, 0, 0.6, 0.6)
ref_y = self.y_plot(self.t, 0, 0.6, 0.6)
if self.path_marker_done[self.path - 1] == False:
points = []
for i in range(0, 1000):
p = Point()
# p.x = self.x_plot(i,26,0.45,0.4)
# p.y = self.y_plot(i,26,0.45,0.4)
p.x = self.x_plot(i, 0, 0.6, 0.6)
p.y = self.y_plot(i, 0, 0.6, 0.6)
p.z = 0.0
points.append(p)
self.path_marker[self.path - 1].points = points
self.path_marker_done[self.path - 1] = True
elif self.path == 6:
# ref_x = self.x_plot(self.t,26,-0.45,-0.4)
# ref_y = self.y_plot(self.t,-26,0.45,0.4)
ref_x = self.x_plot(self.t, 0, -0.5, -0.5)
ref_y = self.y_plot(self.t, -0, 0.5, 0.5)
if self.path_marker_done[self.path - 1] == False:
points = []
for i in range(0, 1000):
p = Point()
# p.x = self.x_plot(i,26,-0.45,-0.4)
# p.y = self.y_plot(i,-26,0.45,0.4)
p.x = self.x_plot(i, 0, -0.5, -0.5)
p.y = self.y_plot(i, -0, 0.5, 0.5)
p.z = 0.0
points.append(p)
self.path_marker[self.path - 1].points = points
self.path_marker_done[self.path - 1] = True
self.pubpath_marker.publish(self.path_marker[self.path - 1])
points = Point()
points.x = ref_x
points.y = ref_y
self.ref_marker.points = [points]
self.pubref_marker.publish(self.ref_marker)
if self.t > self.counter * 100:
self.path += 1
self.counter += 1
if self.path > 6:
self.path = 1
self.t += 1
#print("Path : ",self.path)
#print("Time : ",self.t)
#print("Counter : ",self.counter)
if math.sqrt(self.x0**2 + self.y0**2) > 0.6:
print("return")
self.translate(0, 0, self.yaw0)
else:
if self.inside_arena([ref_x, ref_y]) == True:
#if target is inside arena
self.translate(
ref_x, ref_y,
self.yaw0 + (self.path - 3) * 30 * math.pi / 180)
def passive_dodge_new(self):
#from current position, random a direction to append radius to, limit x and y afterwards, once at the edges, add to-origin vector
beta = random.uniform(-math.pi, math.pi)
d = 0.5
pred = [self.x0 + d * math.cos(beta), self.y0 + d * math.sin(beta)]
#constrain
if abs(pred[0]) > 0.7:
pred[0] = pred[0] * 0.7 / abs(pred[0])
elif abs(pred[1]) > 0.7:
pred[1] = pred[1] * 0.7 / abs(pred[1])
if math.sqrt(self.x0**2 + self.y0**2) > 0.5:
#add to origin vector
delta = math.atan2(-self.y0, -self.x0)
#print(delta*180/math.pi)
pred = [
pred[0] + 0.2 * math.cos(delta),
pred[1] + 0.2 * math.sin(delta)
]
direction = random.uniform(-math.pi / 4, math.pi / 4)
self.translate(pred[0], pred[1], self.yaw0 + direction)
def inside_arena(self, pos):
#check whether pos is inside arena, assuming origin 0,0 in middle
#border [x_min, x_max, y_min, y_max]
borders = [-0.7, 0.7, -0.7, 0.7]
if pos[0] < borders[0] or pos[0] > borders[1] or pos[1] < borders[
2] or pos[1] > borders[3]:
return False
return True
def translate(self, x_target, y_target, angle):
x_error = (x_target - self.x0) * math.cos(
self.yaw0) + (y_target - self.y0) * math.sin(self.yaw0)
y_error = -(x_target - self.x0) * math.sin(
self.yaw0) + (y_target - self.y0) * math.cos(self.yaw0)
ang_error = math.atan2(math.sin(angle - self.yaw0),
math.cos(angle - self.yaw0))
x_derivative = (x_error - self.pre_x_error) / self.del_T
y_derivative = (y_error - self.pre_y_error) / self.del_T
ang_derivative = (ang_error - self.pre_ang_error) / self.del_T
# integrals (PID)
self.x_integral += x_error * self.del_T
if self.x_integral > self.lin_integral_threshold:
self.x_integral = self.lin_integral_threshold
elif self.x_integral < -self.lin_integral_threshold:
self.x_integral = -self.lin_integral_threshold
self.y_integral += y_error * self.del_T
if self.y_integral > self.lin_integral_threshold:
self.y_integral = self.lin_integral_threshold
elif self.y_integral < -self.lin_integral_threshold:
self.y_integral = -self.lin_integral_threshold
self.ang_integral += ang_error * self.del_T
if self.ang_integral > self.ang_integral_threshold:
self.ang_integral = self.ang_integral_threshold
elif self.ang_integral < -self.ang_integral_threshold:
self.ang_integral = -self.ang_integral_threshold
# output velocities
x_linear_vel = (self.p_x * x_error) + (self.d_x * x_derivative) + (
self.i_x * self.x_integral)
if abs(x_linear_vel) > self.x_lin_vel_thres:
x_linear_vel = x_linear_vel * self.x_lin_vel_thres / abs(
x_linear_vel)
self.cmd_x = self.bias + x_linear_vel
y_linear_vel = (self.p_y * y_error) + (self.d_y * y_derivative) + (
self.i_y * self.y_integral)
if abs(y_linear_vel) > self.y_lin_vel_thres:
y_linear_vel = y_linear_vel * self.y_lin_vel_thres / abs(
y_linear_vel)
self.cmd_y = self.bias - y_linear_vel
print(self.d_ang * ang_derivative)
print(self.p_ang * ang_error)
angular_vel = (self.p_ang * ang_error) + (
self.d_ang * ang_derivative) + (self.i_ang * self.ang_integral)
if abs(angular_vel) > self.ang_vel_thres:
angular_vel = angular_vel * self.ang_vel_thres / abs(angular_vel)
self.cmd_yaw = self.bias - angular_vel
self.pre_x_error = x_error
self.pre_y_error = y_error
self.pre_ang_error = ang_error
def rotate(self, angle):
ang_error = math.atan2(math.sin(angle - self.yaw0),
math.cos(angle - self.yaw0))
derivative = (ang_error - self.pre_ang_error) / self.del_T
self.ang_integral += ang_error * self.del_T
if self.ang_integral > self.ang_integral_threshold:
self.ang_integral = self.ang_integral_threshold
elif self.ang_integral < -self.ang_integral_threshold:
self.ang_integral = -self.ang_integral_threshold
print(self.d_ang * derivative)
angular_vel = (self.p_ang * ang_error) + (self.d_ang * derivative) + (
self.i_ang * self.ang_integral)
if abs(angular_vel) > self.ang_vel_thres:
angular_vel = angular_vel * self.ang_vel_thres / abs(angular_vel)
self.cmd_x = self.bias
self.cmd_y = self.bias
self.cmd_yaw = int(self.bias - angular_vel)
self.pre_ang_error = ang_error
def enemy_callback(self, msg):
self.enemy_pos = []
for point in msg.points:
self.enemy_pos.append([point.x, point.y])
def armor_callback(self, msg):
#calculate center and size of roi
roi = [0.0, 0.0, 0.0] #center_x, center_y, size
roi[0] = (msg.x_offset + msg.width / 2) / 10.0
roi[1] = (msg.y_offset + msg.height / 2) / 10.0
roi[2] = msg.width * msg.height / 10.0
if len(self.stash) == 5:
del self.stash[0]
self.stash.append(roi)
heatmap = np.zeros((int(self.xMax), int(self.yMax)), dtype=np.uint8)
for obj in self.stash:
heatmap[int(obj[0]), int(obj[1])] += 0.1 * obj[2]
self.img.data = np.resize(heatmap,
int(self.xMax) * int(self.yMax)).astype(
np.uint8).tolist()
self.pubimg.publish(self.img)
self.statep_x = self.state_x
self.statep_y = self.state_y
self.state_x, self.state_y = np.unravel_index(heatmap.argmax(),
heatmap.shape)
self.error_x = self.target_x - self.state_x
output_x = self.Kp_x * self.error_x + self.Ki_x * (
self.error_x + self.errorp_x) + self.Kd_x * (self.error_x -
self.errorp_x)
self.cmd_yaw_turret = self.bias - output_x
error_y = self.state_y - self.target_y
output_y = self.Kp_y * error_y + self.Ki_y * (
error_y + self.errorp_y) + self.Kd_y * (error_y - self.errorp_y)
self.cmd_pitch = self.bias - output_y
self.errorp_x = self.error_x
self.errorp_y = error_y
if abs(self.error_x) < self.xMax / 10 and abs(error_y) < self.yMax / 6:
self.cmd_shoot = 2
if self.prevsh == 0:
self.startshoottime = time()
self.prevsh = 1
else:
self.cmd_shoot = 1
self.prevsh = 0
self.updatetime = time()
def odom_callback(self, msg):
self.x0 = msg.pose.pose.position.x
self.y0 = msg.pose.pose.position.y
_, _, self.yaw0 = euler_from_quaternion(
(msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w))
self.odom_received = True
