def add_marker(self, array, track, color, tid):
m1 = Marker()
m1.header.frame_id = "camera_rgb_optical_frame"
m1.ns = "line"
m1.id = tid
m1.type = 4 #lines
m1.action = 0
m1.scale.x = .002
m1.color.a = 1.
m1.color.r = color[0]/255.
m1.color.g = color[1]/255.
m1.color.b = color[2]/255.
m1.points = track
array.append(m1)
m2 = Marker()
m2.header.frame_id = "camera_rgb_optical_frame"
m2.ns = "point"
m2.id = tid
m2.type = 8 #points
m2.action = 0
m2.scale.x = .008
m2.scale.y = .008
m2.color.a = 1.
m2.color.r = color[0]/255.
m2.color.g = color[1]/255.
m2.color.b = color[2]/255.
m2.points = [ track[-1] ]
array.append(m2)
def publish_debug_info(self, footprint_point, distance, scan_point):
array = MarkerArray()
fp = Marker()
fp.id = 1
fp.type = fp.SPHERE
scale = 0.1
fp.pose.position.x,fp.pose.position.y, fp.pose.position.z = footprint_point[0], footprint_point[1], 0
fp.scale.x, fp.scale.y, fp.scale.z = scale, scale, scale
fp.color.r, fp.color.g, fp.color.b, fp.color.a = (0, 0, 1, 1)
fp.header.frame_id = "{}/base_link".format(self.robot.robot_name)
fp.frame_locked = True
fp.action = fp.ADD
fp.ns = "door_opening"
array.markers += [fp]
sp = Marker()
sp.id = 2
sp.type = sp.SPHERE
scale = 0.1
sp.pose.position.x,sp.pose.position.y, sp.pose.position.z = scan_point[0], scan_point[1], 0
sp.scale.x, sp.scale.y, sp.scale.z = scale, scale, scale
sp.color.r, sp.color.g, sp.color.b, sp.color.a = (1, 0, 0, 1)
sp.header.frame_id = "{}/base_link".format(self.robot.robot_name)
sp.frame_locked = False
sp.action = sp.ADD
sp.ns = "door_opening"
array.markers += [sp]
self.debug_vizualizer.publish(array)
def publishWaypointMarker(self, wp, delete = False): marker = Marker() marker.header.frame_id = "world" marker.header.stamp = rospy.Time.now() marker.ns = "waypoints" marker.id = self.wp_ids[wp] marker.lifetime = rospy.Duration(0) if delete: marker.action = marker.DELETE else: marker.type = marker.SPHERE marker.action = marker.ADD marker.pose.position.x = wp.x marker.pose.position.y = wp.y marker.pose.orientation.w = 1.0 marker.scale.y = 0.02 marker.scale.x = 0.02 marker.scale.z = 0.01 marker.color.a = 1.0 if wp.action == Waypoint.VIEW: marker.color.g = 1.0 else: marker.color.r = 1.0 # print marker self.vizPub.publish(marker)
def plot_3d_vel(self, p_arr, v_arr, v_scale=1.0):
marker_array = MarkerArray()
for i in xrange(len(p_arr)):
p = p_arr[i]
v = vx,vy,vz = v_arr[i]
marker = Marker()
marker.header.frame_id = "/openni_rgb_optical_frame"
marker.type = marker.ARROW
marker.action = marker.ADD
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.points.append(Point(p[0],p[1],p[2]))
marker.points.append(Point(p[0]+vx*v_scale,p[1]+vy*v_scale,p[2]+vz*v_scale))
marker.scale.x=0.05
marker.scale.y=0.1
marker.id = i
marker_array.markers.append(marker)
for i in xrange(len(p_arr),self.max_features):
marker = Marker()
marker.action = marker.DELETE
marker.id = i
marker_array.markers.append(marker)
self.marker_pub.publish(marker_array)
def publishPositions(self):
if not self.initialized:
return
nr = 0
markerArray = MarkerArray()
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.MESH_RESOURCE
marker.action = marker.ADD
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.mesh_use_embedded_materials = True
marker.mesh_resource = "package://pacman_controller/meshes/pacman.dae"
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.pose.orientation = self.pacman["orientation"]
marker.pose.position.x = self.pacman["x"]
marker.pose.position.y = self.pacman["y"]
marker.pose.position.z = 0.0
marker.id = nr
markerArray.markers.append(marker)
for ghost in self.ghosts:
curGhost = self.ghosts[ghost]
if not curGhost["initialized"]:
continue
nr += 1
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.MESH_RESOURCE
marker.action = marker.ADD
marker.scale.x = 0.3
marker.scale.y = 0.3
marker.scale.z = 0.3
marker.mesh_use_embedded_materials = True
if curGhost["eaten"]:
marker.mesh_resource = "package://pacman_controller/meshes/dead.dae"
elif self.state == State.FLEEING:
marker.mesh_resource = "package://pacman_controller/meshes/ghost_catchable.dae"
else:
marker.mesh_resource = "package://pacman_controller/meshes/%s.dae" % ghost
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.pose.orientation = curGhost["orientation"]
marker.pose.position.x = curGhost["x"]
marker.pose.position.y = curGhost["y"]
marker.pose.position.z = 0.0
marker.id = nr
markerArray.markers.append(marker)
self.pubPositions.publish(markerArray)
return
def publishMap(self):
markerArray = MarkerArray()
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.SPHERE_LIST
marker.action = marker.ADD
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
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
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
marker.id = 0
for p in self.mapPoints:
if p["eaten"]:
continue
if p["powerup"]:
continue
point = Point()
point.x = p["x"]
point.y = p["y"]
point.z = 0.15
marker.points.append(point)
markerArray.markers.append(marker)
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.SPHERE_LIST
marker.action = marker.ADD
marker.scale.x = 0.3
marker.scale.y = 0.3
marker.scale.z = 0.3
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
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
marker.id = 1
for p in self.mapPoints:
if p["eaten"]:
continue
if not p["powerup"]:
continue
point = Point()
point.x = p["x"]
point.y = p["y"]
point.z = 0.2
marker.points.append(point)
markerArray.markers.append(marker)
self.pubMap.publish(markerArray)
def publish_cluster(publisher, points, frame_id, namespace, cluster_id):
"""Publishes a marker representing a cluster.
The x and y arguments specify the center of the target.
Args:
publisher: A visualization_msgs/Marker publisher
points: A list of geometry_msgs/Point
frame_id: The coordinate frame in which to interpret the points.
namespace: string, a unique name for a group of clusters.
cluster_id: int, a unique number for this cluster in the given namespace.
"""
marker = Marker()
# TODO(jstn): Once the point clouds have the correct frame_id,
# use them here.
marker.header.frame_id = frame_id
marker.header.stamp = rospy.Time().now()
marker.ns = namespace
marker.id = 2 * cluster_id
marker.type = Marker.POINTS
marker.action = Marker.ADD
marker.color.r = random.random()
marker.color.g = random.random()
marker.color.b = random.random()
marker.color.a = 0.5
marker.points = points
marker.scale.x = 0.002
marker.scale.y = 0.002
marker.lifetime = rospy.Duration()
center = [0, 0, 0]
for point in points:
center[0] += point.x
center[1] += point.y
center[2] += point.z
center[0] /= len(points)
center[1] /= len(points)
center[2] /= len(points)
text_marker = Marker()
text_marker.header.frame_id = frame_id
text_marker.header.stamp = rospy.Time().now()
text_marker.ns = namespace
text_marker.id = 2 * cluster_id + 1
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.action = Marker.ADD
text_marker.pose.position.x = center[0] - 0.1
text_marker.pose.position.y = center[1]
text_marker.pose.position.z = center[2]
text_marker.color.r = 1
text_marker.color.g = 1
text_marker.color.b = 1
text_marker.color.a = 1
text_marker.scale.z = 0.05
text_marker.text = '{}'.format(cluster_id)
text_marker.lifetime = rospy.Duration()
_publish(publisher, marker, "cluster")
_publish(publisher, text_marker, "text_marker")
return marker
def visualize_cluster(self, cluster, label=None):
points = pc2.read_points(cluster.pointcloud, skip_nans=True)
point_list = [Point(x=x, y=y-0.3, z=z) for x, y, z, rgb in points]
if len(point_list) == 0:
rospy.logwarn('Point list was of size 0, skipping.')
return
marker_id = len(self._current_markers)
marker = Marker()
marker.header.frame_id = 'map'
marker.header.stamp = rospy.Time().now()
marker.ns = 'clusters'
marker.id = marker_id
marker.type = Marker.POINTS
marker.action = Marker.ADD
marker.color.r = random.random()
marker.color.g = random.random()
marker.color.b = random.random()
marker.color.a = 0.5 + random.random()
marker.points = point_list
marker.scale.x = 0.002
marker.scale.y = 0.002
marker.lifetime = rospy.Duration()
self.visualize_marker(marker)
if label is not None:
center = [0, 0, 0]
for point in point_list:
center[0] += point.x
center[1] += point.y
center[2] += point.z
center[0] /= len(point_list)
center[1] /= len(point_list)
center[2] /= len(point_list)
text_marker = Marker()
text_marker.header.frame_id = 'map'
text_marker.header.stamp = rospy.Time().now()
text_marker.ns = 'labels'
text_marker.id = marker_id + 1
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.action = Marker.ADD
text_marker.pose.position.x = center[1] - 0.05
text_marker.pose.position.y = center[1]
text_marker.pose.position.z = center[2]
text_marker.color.r = 1
text_marker.color.g = 1
text_marker.color.b = 1
text_marker.color.a = 1
text_marker.scale.z = 0.05
text_marker.text = label
text_marker.lifetime = rospy.Duration()
self.visualize_marker(text_marker)
def publish(self, target_frame, timestamp):
ma = MarkerArray()
for id in self.marker_list:
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
Lkf = self.marker_list[id]
marker.pose.position.x = Lkf.L[0,0]
marker.pose.position.y = Lkf.L[1,0]
marker.pose.position.z = 0
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 1
marker.pose.orientation.w = 0
marker.scale.x = max(3*sqrt(Lkf.P[0,0]),0.05)
marker.scale.y = max(3*sqrt(Lkf.P[1,1]),0.05)
marker.scale.z = 0.5;
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
Lkf = self.marker_list[id]
marker.pose.position.x = Lkf.L[0,0]
marker.pose.position.y = Lkf.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 pubViz(self, ast, bst):
rate = rospy.Rate(10)
for i in range(self.winSize):
msgs = MarkerArray()
#use1について
msg = Marker()
#markerのプロパティ
msg.header.frame_id = 'camera_link'
msg.header.stamp = rospy.Time.now()
msg.ns = 'j1'
msg.action = 0
msg.id = 1
msg.type = 8
msg.scale.x = 0.1
msg.scale.y = 0.1
msg.color = self.carray[2]
#ジョイントポイントを入れる処理
for j1 in range(self.jointSize):
point = Point()
point.x = self.pdata[0][ast+i][j1][0]
point.y = self.pdata[0][ast+i][j1][1]
point.z = self.pdata[0][ast+i][j1][2]
msg.points.append(point)
msg.pose.orientation.w = 1.0
msgs.markers.append(msg)
msg = Marker()
msg.header.frame_id = 'camera_link'
msg.header.stamp = rospy.Time.now()
msg.ns = 'j2'
msg.action = 0
msg.id = 2
msg.type = 8
msg.scale.x = 0.1
msg.scale.y = 0.1
msg.color = self.carray[1]
for j2 in range(self.jointSize):
point = Point()
point.x = self.pdata[1][bst+i][j2][0]
point.y = self.pdata[1][bst+i][j2][1]
point.z = self.pdata[1][bst+i][j2][2]
msg.points.append(point)
msg.pose.orientation.w = 1.0
msgs.markers.append(msg)
self.mpub.publish(msgs)
rate.sleep()
def publishState(self):
self.pubState.publish(str(self.state))
marker = Marker()
marker.header.frame_id = "/map"
marker.header.stamp = rospy.get_rostime()
marker.type = marker.TEXT_VIEW_FACING
marker.action = marker.ADD
marker.scale.x = 1.0
marker.scale.y = 0.2
marker.scale.z = 0.5
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = self.infoLoc["x"]
marker.pose.position.y = self.infoLoc["y"]
marker.pose.position.z = 0.0
marker.id = 0
if self.state in [State.RUNNING]:
if not self.sendStart:
marker.text = "Start"
self.sendStart = True
self.timeSend = rospy.get_rostime()
elif (rospy.get_rostime() - self.timeSend).to_sec() > 0.2:
# marker.type = marker.SPHERE
marker.action = marker.DELETE
else:
marker.text = "Start"
if self.state == State.FLEEING:
timeFleeing = TIME_FLEEING - (rospy.get_rostime() - self.timeStartFleeing).to_sec()
if timeFleeing > 0:
marker.text = str(timeFleeing)
else:
marker.action = marker.DELETE
self.state = State.RUNNING
if self.state in [State.GAME_OVER, State.STOPPED, State.SETUP]:
marker.text = "GAME OVER!"
if self.state in [State.INIT]:
marker.text = "Initializing Game"
if self.state in [State.PAUSED]:
marker.text = "Game Paused"
if self.state in [State.WON]:
marker.text = "Congratulations, you have won the Game! Now get back to you real job!"
self.pubInfo.publish(marker)
def pubRviz(self, pos, joints):
msgs = MarkerArray()
for p in range(len(pos)):
msg = Marker()
msg.header.frame_id = 'camera_link'
msg.header.stamp = rospy.Time.now()
msg.ns = 'marker'
msg.action = 0
msg.id = p
msg.type = 4
msg.scale.x = 0.1
msg.scale.y = 0.1
msg.color = self.carray[2]
for i in range(len(pos[p])):
point = Point()
point.x = pos[p][i][0]
point.y = pos[p][i][1]
point.z = pos[p][i][2]
msg.points.append(point)
msg.pose.orientation.w = 1.0
msgs.markers.append(msg)
for j in range(len(joints)):
msg = Marker()
msg.header.frame_id = 'camera_link'
msg.header.stamp = rospy.Time.now()
msg.ns = 'joints'
msg.action = 0
msg.id = j
msg.type = 8
msg.scale.x = 0.1
msg.scale.y = 0.1
msg.color = self.carray[j]
#print "joints len:"+str(len(joints[j]))
for i in range(len(joints[j])):
point = Point()
point.x = joints[j][i][0]
point.y = joints[j][i][1]
point.z = joints[j][i][2]
msg.points.append(point)
msg.pose.orientation.w = 1.0
msgs.markers.append(msg)
self.mpub.publish(msgs)
def publish(anns, data):
wall_list = WallList()
marker_list = MarkerArray()
marker_id = 1
for a, d in zip(anns, data):
# Walls
object = deserialize_msg(d.data, Wall)
wall_list.obstacles.append(object)
# Markers
marker = Marker()
marker.id = marker_id
marker.header = a.pose.header
marker.type = a.shape
marker.ns = "wall_obstacles"
marker.action = Marker.ADD
marker.lifetime = rospy.Duration.from_sec(0)
marker.pose = copy.deepcopy(a.pose.pose.pose)
marker.scale = a.size
marker.color = a.color
marker_list.markers.append(marker)
marker_id = marker_id + 1
marker_pub = rospy.Publisher('wall_marker', MarkerArray, latch=True, queue_size=1)
wall_pub = rospy.Publisher('wall_pose_list', WallList, latch=True, queue_size=1)
wall_pub.publish(wall_list)
marker_pub.publish(marker_list)
return
def addPolygonFilled(self, points):
oid = self.oid+1
name = "/polygonFilled"+str(self.oid)
marker = Marker()
marker.id = self.oid
marker.ns = "/polygonFilled"
marker.header.frame_id = name
marker.type = marker.TRIANGLE_LIST
marker.action = marker.ADD
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.points = []
for i in range(0,len(points)-2,1):
pt = Point(points[0][0], points[0][1], points[0][2])
marker.points.append(pt)
pt = Point(points[i+1][0], points[i+1][1], points[i+1][2])
marker.points.append(pt)
pt = Point(points[i+2][0], points[i+2][1], points[i+2][2])
marker.points.append(pt)
self.markerArray.markers.append(marker)
def input_handler(space):
global marker_color
global marker_pub
global matches
for str in matches:
if space.aux_payload.find(str) == -1:
# print "FAIL: str='%s' aux_payload='%s'" % (str, space.aux_payload)
return
# print space
# print
marker = Marker()
marker.header.frame_id = "/map"
marker.header.stamp = rospy.Time()
marker.ns = ""
marker.id = 0
marker.type = Marker.POINTS
marker.action = Marker.ADD
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 0
marker.pose.orientation.w = 0
marker.scale.x = .1
marker.scale.y = .1
marker.scale.z = .1
marker.color = marker_color
marker.points = []
# this line cuts off anything before the string "convex_set: " in the aux_payload
convex_set = space.aux_payload[ space.aux_payload.find("convex_set: ") :]
# this line cuts off that string at the next newline character
convex_set = convex_set.split('\n')[0]
# this line splits the string into pieces delineated by spaces
convex_set = convex_set.split(' ')
# print convex_set
for candidate in convex_set:
if len(candidate) == 0 or candidate[0] != '(':
continue
coords = candidate[1:-1].split(',')
# print coords
marker.points.append(Point(float(coords[0]), float(coords[1]), float(coords[2])))
# print marker.points[-1]
# print
# print marker.points
# print
marker_pub.publish(marker)
def __init__(self):
self.layout = rospy.get_param('/thruster_layout/thrusters') # Add thruster layout from ROS param set by mapper
assert self.layout is not None, 'Could not load thruster layout from ROS param'
'''
Create MarkerArray with an arrow marker for each thruster at index node_id.
The position of the marker is as specified in the layout, but the length of the arrow
will be set at each thrust callback.
'''
self.markers = MarkerArray()
for i in xrange(len(self.layout)):
# Initialize each marker (color, scale, namespace, frame)
m = Marker()
m.header.frame_id = '/base_link'
m.type = Marker.ARROW
m.ns = 'thrusters'
m.color.a = 0.8
m.scale.x = 0.01 # Shaft diameter
m.scale.y = 0.05 # Head diameter
m.action = Marker.DELETE
m.lifetime = rospy.Duration(0.1)
self.markers.markers.append(m)
for key, layout in self.layout.iteritems():
# Set position and id of marker from thruster layout
idx = layout['node_id']
pt = numpy_to_point(layout['position'])
self.markers.markers[idx].points.append(pt)
self.markers.markers[idx].points.append(pt)
self.markers.markers[idx].id = idx
# Create publisher for marker and subscribe to thrust
self.pub = rospy.Publisher('/thrusters/markers', MarkerArray, queue_size=5)
self.thrust_sub = rospy.Subscriber('/thrusters/thrust', Thrust, self.thrust_cb, queue_size=5)
def new_marker(self, ns="/debug", frame="enu", time=None, type = Marker.CUBE , position=(0,0,0), orientation=(0,0,0,1), color=(1,0,0)):
marker = Marker()
marker.ns = ns
if time != None:
marker.header.stamp = time
marker.header.frame_id = frame
marker.type = type
marker.action = marker.ADD
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.color.a = 1.0
marker.id = self.last_id
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
marker.pose.position.x = position[0]
marker.pose.position.y = position[1]
marker.pose.position.z = position[2]
self.last_id += 1
self.markers.markers.append(marker)
def to_marker(self):
"""
:return: a marker representing the map.
:type: Marker
"""
marker = Marker()
marker.type = Marker.CUBE_LIST
for x in range(0, len(self.field)):
for y in range(0, len(self.field[0])):
marker.header.frame_id = '/odom_combined'
marker.ns = 'suturo_planning/map'
marker.id = 23
marker.action = Marker.ADD
(x_i, y_i) = self.index_to_coordinates(x, y)
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.pose.orientation.w = 1
marker.scale.x = self.cell_size
marker.scale.y = self.cell_size
marker.scale.z = 0.01
p = Point()
p.x = x_i
p.y = y_i
marker.points.append(p)
c = self.get_cell_by_index(x, y)
marker.colors.append(self.__cell_to_color(c))
marker.lifetime = rospy.Time(0)
return marker
def create_marker(self, markerArray, view, pose, view_values):
marker1 = Marker()
marker1.id = self.marker_id
self.marker_id += 1
marker1.header.frame_id = "/map"
marker1.type = marker1.TRIANGLE_LIST
marker1.action = marker1.ADD
marker1.scale.x = 1
marker1.scale.y = 1
marker1.scale.z = 2
marker1.color.a = 0.25
vals = view_values.values()
max_val = max(vals)
non_zero_vals = filter(lambda a: a != 0, vals)
min_val = min(non_zero_vals)
print min_val, max_val, view_values[view.ID]
marker1.color.r = r_func( float((view_values[view.ID] - min_val)) / float((max_val - min_val + 1)))
marker1.color.g = g_func( float((view_values[view.ID] - min_val)) / float((max_val - min_val + 1)))
marker1.color.b = b_func( float((view_values[view.ID] - min_val)) / float((max_val - min_val + 1)))
marker1.pose.orientation = pose.orientation
marker1.pose.position = pose.position
marker1.points = [Point(0,0,0.01),Point(2.5,-1.39,0.01),Point(2.5,1.39,0.01)]
markerArray.markers.append(marker1)
def publish_markerArray(publisher, points, rgba=(1, 0, 0, 1), shape=Marker.CUBE, duration=rospy.Duration(360), ns='ns'):
#points is expected to be a list of tuples (x,y)
#It's recommended that the publisher is created with latch=True
_id = 0
ma = MarkerArray()
for p in points:
m = Marker()
m.header.frame_id = '/map'
m.header.stamp = rospy.get_rostime()
m.ns = ns
m.id = _id
m.type = shape
m.action = m.ADD
m.pose.position.x = p[0]
m.pose.position.y = p[1]
m.pose.orientation.w = 1
m.scale.x = 0.5
m.scale.y = 0.5
m.scale.z = 0.5
m.color.r = rgba[0]
m.color.g = rgba[1]
m.color.b = rgba[2]
m.color.a = rgba[3]
m.lifetime = duration
ma.markers.append(m)
_id += 1
publisher.publish(ma)
def publish_prob(self, waypoints, objects, probs):
prob_msg = MarkerArray()
i = 0
n_waypoints = len(waypoints)
n_objects = len(objects)
scaling_factor = max(probs)
for node in self._topo_map:
if node.name in waypoints:
marker = Marker()
marker.header.frame_id = 'map'
marker.id = i
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.pose = node.pose
prob = 1
for j in range(0, n_objects):
prob = prob*probs[n_objects*i + j]
prob = prob/(scaling_factor**2)
print prob
marker.scale.x = 1*prob
marker.scale.y = 1*prob
marker.scale.z = 1
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
prob_msg.markers.append(marker)
i = i + 1
self._prob_marker_pub.publish(prob_msg)
def __publish_marker_for_object(self, object, txt):
marker = Marker()
marker.header.frame_id = object.object.header.frame_id
marker.header.stamp = rospy.Time(0)
marker.ns = "scene_classifier_marker"
self.marker_id = self.marker_id + 1
marker.id = self.marker_id
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.ADD
marker.pose.position.x = object.c_centroid.x
marker.pose.position.y = object.c_centroid.y
marker.pose.position.z = object.c_centroid.z
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.scale.x = 0.5
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 = 0.0
marker.text = txt
marker.lifetime = rospy.Duration.from_sec(10)
self.marker_publisher.publish(marker)
def pose_cb(self, pose):
pose = self.listener.transformPose(self.reference_frame,pose)
q = pose.pose.orientation
qvec = [q.x,q.y,q.z,q.w]
euler = euler_from_quaternion(qvec)
self.goal_x = pose.pose.position.x
self.goal_y = pose.pose.position.y
self.goal_theta = euler[2]
(ex,ey,etheta) = self.compute_error()
if ex < -self.dist_threshold:
self.goal_theta = norm_angle(etheta + pi)
print "New goal: %.2f %.2f %.2f" % (self.goal_x, self.goal_y, self.goal_theta)
marker = Marker()
marker.header = pose.header
marker.ns = "goal_marker"
marker.id = 10
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.pose = pose.pose
marker.scale.x = 0.5
marker.scale.y = 0.5
marker.scale.z = 1.0;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.lifetime.secs=-1.0
self.marker_pub.publish(marker)
def publish_prob2(self, waypoints, objects, probs):
prob_msg = MarkerArray()
i = 0
idx = 0
n_waypoints = len(waypoints)
n_objects = len(objects)
scaling_factor = max(probs)
current_probs = [0 for foo in objects]
for node in self._topo_map:
if node.name in waypoints:
for j in range(0, n_objects):
marker = Marker()
marker.header.frame_id = 'map'
marker.id = idx
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.pose = node.pose
prob = probs[n_objects*i + j]
prob = prob/(scaling_factor)
print "AHAHHAHBHBHBHBHBHB", prob
marker.pose.position.z = marker.pose.position.z + current_probs[j]
marker.scale.x = 1*prob
marker.scale.y = 1*prob
marker.scale.z = 1*prob
current_probs[j] = current_probs[j] + prob + 0.1
marker.color.a = 1.0
marker.color.r = 1.0*prob
marker.color.g = 1.0*prob
marker.color.b = 1.0*prob
prob_msg.markers.append(marker)
idx = idx + 1
i = i + 1
self._prob_marker_pub.publish(prob_msg)
def publish_marker(self): # create marker marker = Marker() marker.header.frame_id = "/base_link" marker.header.stamp = rospy.Time.now() marker.ns = "color" marker.id = 0 marker.type = 2 # SPHERE marker.action = 0 # ADD marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 1.5 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.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = self.color.a #Transparency marker.color.r = self.color.r marker.color.g = self.color.g marker.color.b = self.color.b # publish marker self.pub_marker.publish(marker)
def publish(anns, data):
ar_mk_list = AlvarMarkers()
marker_list = MarkerArray()
marker_id = 1
for a, d in zip(anns, data):
# AR markers
object = deserialize_msg(d.data, AlvarMarker)
ar_mk_list.markers.append(object)
# Visual markers
marker = Marker()
marker.id = marker_id
marker.header = a.pose.header
marker.type = a.shape
marker.ns = "ar_mk_obstacles"
marker.action = Marker.ADD
marker.lifetime = rospy.Duration.from_sec(0)
marker.pose = copy.deepcopy(a.pose.pose.pose)
marker.scale = a.size
marker.color = a.color
marker_list.markers.append(marker)
marker_id = marker_id + 1
marker_pub = rospy.Publisher('ar_mk_marker', MarkerArray, latch=True, queue_size=1)
ar_mk_pub = rospy.Publisher('ar_mk_pose_list', AlvarMarkers,latch=True, queue_size=1)
ar_mk_pub.publish(ar_mk_list)
marker_pub.publish(marker_list)
return
def drawlandmark(pub):
global pt_count
points_tag = Marker()
points_tag.header.frame_id = "/base_link"
points_tag.action = Marker.ADD
points_tag.header.stamp = rospy.Time.now()
points_tag.lifetime = rospy.Time(0)
points_tag.scale.x = 0.1;
points_tag.scale.y = 0.1;
points_tag.scale.z = 0.1;
points_tag.color.a = 1.0;
points_tag.color.r = 1.0;
points_tag.color.g = 0.0;
points_tag.color.b = 0.0;
points_tag.ns = "pts_line"
pt_count+=1
points_tag.id = pt_count
points_tag.type = Marker.POINTS
for x in range(6):
p1 = Point()
p1.x = tagnum_x[x]/100
p1.y = tagnum_y[x]/100
p1.z = 0
points_tag.points.append(p1)
pub.publish(points_tag)
def draw_box_marker(
self, id, x, y, z, dimx, dimy, dimz, color=(1.0, 1.0, 1.0, 0.0), duration=0.0, frame_id="base_link"
):
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.ns = "object_detector"
marker.type = Marker.CUBE
marker.action = marker.ADD
marker.id = id
marker.header.frame_id = "base_link"
marker.pose.position.x = x
marker.pose.position.y = y
marker.pose.position.z = z
marker.pose.position.x = x
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.scale.x = dimx
marker.scale.y = dimy
marker.scale.z = dimz
marker.color.a = color[0]
marker.color.r = color[1]
marker.color.g = color[2]
marker.color.b = color[3]
marker.lifetime = rospy.Duration(duration)
self.box_drawer.publish(marker)
def set_position_callback(self,marker,joy): print self.position_marker.ns print joy.buttons[3] == 1 print marker.ns == "PEOPLE" if (self.position_marker.ns == "NONE" and joy.buttons[3] == 1 and marker.ns == "PEOPLE"): self.position_marker = marker print "set position" ref_marker = Marker() ref_marker.header.frame_id = "base_footprint" ref_marker.header.stamp = rospy.get_rostime() ref_marker.ns = "robot" ref_marker.type = 9 ref_marker.action = 0 ref_marker.pose.position.x = self.position_marker.pose.position.x ref_marker.pose.position.y = self.position_marker.pose.position.y ref_marker.pose.position.z = self.position_marker.pose.position.z ref_marker.scale.x = .25 ref_marker.scale.y = .25 ref_marker.scale.z = .25 ref_marker.color.r = 1.0 ref_marker.color.g = 0.0 ref_marker.color.a = 1.0 ref_marker.lifetime = rospy.Duration(0) self.ref_viz_pub.publish(ref_marker) else: pass
def __setTextMarker(self, id, waypoint, colors = [1,0,0,0]):
try:
marker = Marker()
marker.header.frame_id = '/map'
marker.header.stamp = rospy.Time.now()
marker.ns = 'patrol'
marker.id = id + len(self.__waypoints)
marker.type = marker.TEXT_VIEW_FACING
marker.action = marker.ADD
marker.text = str(id)
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = colors[0]
marker.color.r = colors[1]
marker.color.b = colors[2]
marker.color.g = colors[3]
marker.pose.orientation.w = 1.0
marker.pose.position.x = waypoint.target_pose.pose.position.x
marker.pose.position.y = waypoint.target_pose.pose.position.y
marker.pose.position.z = waypoint.target_pose.pose.position.z
return marker
except Exception as ex:
rospy.logwarn('PatrolNode.__setMarker- ', ex.message)
-0.00798111114703, -0.00240289023425, 0.973910607185, 3.55992334659e-05,
2.53591989399e-05, 0.707060385227, 0.707153172752
]
manipPose1 = [
0.441024004555, -0.0157218288562, 0.402612554019, 0.527145535299,
0.477387854476, 0.486045011226, 0.50791600494
]
manipPose2 = [0.01, 0.3, 0.55, 0, 0, 0, 0.707]
pub_line_min_dist = rospy.Publisher('visualization_marker',
Marker,
queue_size=1)
marker = Marker()
marker.header.frame_id = "/chassis"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.id = 0
# marker scale
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
# marker color
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 1.0
# marker orientaitn
marker.pose.orientation.x = 0.0
results = json.load(resultsf)
count = 0
tstamp = 0
markerArray = MarkerArray()
for result in results:
marker = Marker()
marker.header.frame_id = 'map'
marker.header.stamp = rospy.Time.from_sec(result['timestamp'] /
100000000.0)
marker.ns = 'object'
marker.id = count
marker.lifetime = rospy.Duration(
0.23
) #The lifetime of the bounding-box, you can modify it according to the power of your machine.
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.scale.x = result['length']
marker.scale.y = result['width']
marker.scale.z = result['height']
marker.color.b = 1.0
marker.color.a = 0.5 #The alpha of the bounding-box
marker.pose.position.x = result['center']['x']
marker.pose.position.y = result['center']['y']
marker.pose.position.z = result['center']['z']
marker.pose.orientation.w = result['rotation']['w']
marker.pose.orientation.x = result['rotation']['x']
marker.pose.orientation.y = result['rotation']['y']
marker.pose.orientation.z = result['rotation']['z']
marker1 = Marker()
marker1.header.frame_id = 'map'
marker1.header.stamp = rospy.Time.from_sec(
def recorder():
rospy.init_node('recorder', anonymous = True)
rate = rospy.Rate(10)
header = Header()
data_path = "/Pandaset/data"
bag_path = "/Pandaset/bagfiles/"
dataset = DataSet(data_path)
for files in dataset.sequences(with_semseg = True):
#if files != "024" :
# continue
f = dataset[files]
f.load()
print("===file", files, "open===")
bag_name = "data_" + files + ".bag"
full_bag_path = bag_path + bag_name
bag = rosbag.Bag(full_bag_path, 'w')
###lidar visualization
lidar_obj = f.lidar
for i, pc0 in enumerate(f.lidar):
semseg = np.array(f.semseg[i])
lidar = PointCloud()
lidar_set = PointSet()
lidar_set.header = header
lidar_set.header.frame_id = "/base_link"
lidar_set.header.stamp.secs = int(lidar_obj.timestamps[i])
lidar_set.header.stamp.nsecs = int((lidar_obj.timestamps[i] - lidar.header.stamp.secs) * (10**9))
lidar.header = header
lidar.header.frame_id = "/base_link"
lidar.header.stamp.secs = int(lidar_obj.timestamps[i])
lidar.header.stamp.nsecs = int((lidar_obj.timestamps[i] - lidar.header.stamp.secs) * (10**9))
for index, lidar_points in enumerate(pc0.values):
lidar.points.append(Point32(lidar_points[0], lidar_points[1], lidar_points[2]))
lidar_set.point.append(LidarPoint(lidar_points[0]
,lidar_points[1]
,lidar_points[2]
,lidar_points[3]
,semseg[index][0]))
bag.write("lidar_data", lidar, lidar.header.stamp)
bag.write("lidar_info", lidar_set, lidar_set.header.stamp)
rate.sleep()
rospy.loginfo("===lidar_complete===")
for i, cuboid in enumerate(f.cuboids):
lidar_label = MarkerArray()
header = Header()
header.frame_id = "base_link"
header.stamp.secs = int(f.timestamps[i])
header.stamp.nsecs = int((f.timestamps[i] - header.stamp.secs) * (10**9))
POSITION_X = cuboid["position.x"]
POSITION_Y = cuboid["position.y"]
POSITION_Z = cuboid["position.z"]
DIMENSIONS_X = cuboid["dimensions.x"]
DIMENSIONS_Y = cuboid["dimensions.y"]
DIMENSIONS_Z = cuboid["dimensions.z"]
YAW = cuboid.yaw
LABEL = cuboid.label
for idx, UUID in enumerate(cuboid.uuid):
label_marker = Marker()
label_marker.header = header
label_marker.type = Marker.CUBE
label_marker.action = Marker.ADD
ID = int(UUID[len(UUID)-8:len(UUID)].encode("utf-8").hex(), 16)
label_marker.id = np.int32(ID)
label_marker.lifetime = rospy.Duration(0.1)
label_marker.pose.position.x = POSITION_X[idx]
label_marker.pose.position.y = POSITION_Y[idx]
label_marker.pose.position.z = POSITION_Z[idx]
label_marker.scale.x = DIMENSIONS_X[idx]
label_marker.scale.y = DIMENSIONS_Y[idx]
label_marker.scale.z = DIMENSIONS_Z[idx]
quaternion = quaternion_from_euler(0, 0, YAW[idx])
label_marker.pose.orientation.x = quaternion[1]
label_marker.pose.orientation.y = quaternion[2]
label_marker.pose.orientation.z = quaternion[3]
label_marker.pose.orientation.w = quaternion[0]
label_marker.color.a = 0.5
if LABEL[idx] == "Car":
label_marker.color.b = Car[0]
label_marker.color.g = Car[1]
label_marker.color.r = Car[2]
label_marker.ns = "Car"
elif LABEL[idx] == "Motorized Scooter":
label_marker.color.b = Motorized_Scooter[0]
label_marker.color.g = Motorized_Scooter[1]
label_marker.color.r = Motorized_Scooter[2]
label_marker.ns = "Motorized Scooter"
elif LABEL[idx] == "Pickup Truck":
label_marker.color.b = Pickup_Truck[0]
label_marker.color.g = Pickup_Truck[1]
label_marker.color.r = Pickup_Truck[2]
label_marker.ns = "Pickup Truck"
elif LABEL[idx] == "Pedestrian":
label_marker.color.b = Pedestrian[0]
label_marker.color.g = Pedestrian[1]
label_marker.color.r = Pedestrian[2]
label_marker.ns = "Pedestrian"
elif LABEL[idx] == "Signs":
label_marker.color.b = Signs[0]
label_marker.color.g = Signs[1]
label_marker.color.r = Signs[2]
label_marker.ns = "Signs"
elif LABEL[idx] == "Bicycle":
label_marker.color.b = Bicycle[0]
label_marker.color.g = Bicycle[1]
label_marker.color.r = Bicycle[2]
label_marker.ns = "Bicycle"
lidar_label.markers.append(label_marker)
bag.write("/lidar_label", lidar_label, header.stamp)
rate.sleep()
rospy.loginfo("===cuboids_complete===")
###camera visualization
back_camera = f.camera['back_camera']
front_camera = f.camera['front_camera']
front_left_camera = f.camera['front_left_camera']
front_right_camera = f.camera['front_right_camera']
left_camera = f.camera['left_camera']
right_camera = f.camera['right_camera']
for index, back_img in enumerate(back_camera):
camera_setting(index, back_img, back_camera, lidar_obj, f, "back_camera", bag, rate)
rospy.loginfo("===back_camera_complete===")
for index, front_img in enumerate(front_camera):
camera_setting(index, front_img, front_camera, lidar_obj, f, "front_camera", bag, rate)
rospy.loginfo("===front_camera_complete===")
for index, front_left_img in enumerate(front_left_camera):
camera_setting(index, front_left_img, front_left_camera, lidar_obj, f, "front_left_camera", bag, rate)
rospy.loginfo("===front_left_camera_complete===")
for index, front_right_img in enumerate(front_right_camera):
camera_setting(index, front_right_img, front_right_camera, lidar_obj, f, "front_right_camera", bag, rate)
rospy.loginfo("===front_right_camera_complete===")
for index, left_img in enumerate(left_camera):
camera_setting(index, left_img, left_camera, lidar_obj, f, "left_camera", bag, rate)
rospy.loginfo("===left_camera_complete===")
for index, right_img in enumerate(right_camera):
camera_setting(index, right_img, right_camera, lidar_obj, f, "right_camera", bag, rate)
rospy.loginfo("===right_camera_complete===")
bag.close()
rospy.loginfo("===all complete===")
print()
def callback(data, list):
pub, ekf, publisher_marker = list
# print("Start of current picture")
# print(ekf.get_x_est())
# print(ekf.get_p_mat())
# print(image.encoding)
# brige = CvBridge()
# try:
# frame = brige.imgmsg_to_cv2(image, "passthrough")
# # frame = brige.compressed_imgmsg_to_cv2(image, "passthrough")
# except CvBridgeError as e:
# print(e)
#
# print(frame.shape)
# point_data_3d = np.zeros((frame.shape[0] * frame.shape[1], 3))
# print(point_data_3d.shape)
# for i in range(point_data_3d.shape[0]):
# # print(i)
# # print(np.mod(i, 480))
# # print(i/640)
# point_data_3d[i, 0] = np.mod(i, 480)/300.0
# point_data_3d[i, 1] = i/640/300.0
# point_data_3d[i, 2] = frame[np.mod(i, 480), i/640]
# point_data_3d = np.float32(point_data_3d)
# #print(point_data_3d[1000,:])
pc = ros_numpy.numpify(data)
points = np.zeros((pc.shape[0], 3))
points[:, 0] = pc['x']
points[:, 1] = pc['y']
points[:, 2] = pc['z']
# print(points.shape)
points = points[::10, :]
# print(points.shape)
points = np.float32(points)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10, 1.0)
K = 10
ret, label, center = cv2.kmeans(points, K, None, criteria, 10,
cv2.KMEANS_RANDOM_CENTERS)
# print(center)
best_match = np.argmin(abs(center[:, 2] - 1))
# print(best_match)
A = points[label.ravel() == best_match]
# B = points[label.ravel() == 1]
# C = points[label.ravel() == 2]
# print(A.shape)
A = A[::10, :]
# print(A.shape)
points = []
lim = 8
# publish point cloud
# CALCULATE THE DIFFERENT SIDES LEFT AND RIGHT
print("center", center[best_match, :])
median_center = np.zeros((3))
# from koordinate system (z raus) zu x aus der kamera raus
median_center[0] = center[best_match, 2]
median_center[1] = -center[best_match, 0]
median_center[2] = -center[best_match, 1]
print("median_center", median_center)
current_mean_angle = np.arctan2(median_center[1], median_center[0])
print("current_mean_angle", current_mean_angle)
left_segment = []
right_segment = []
for i in range(A.shape[0]):
x = A[i, 2]
y = -A[i, 0]
if np.arctan2(y, x) > current_mean_angle:
left_segment.append(A[i, :])
else:
right_segment.append(A[i, :])
left_segment = np.asarray(left_segment)
print("left_segment", left_segment.shape)
right_segment = np.asarray(right_segment)
print("right_segment", right_segment.shape)
current_segment = left_segment
#update EKF
ekf.prediction()
print("EKF Update:")
ekf.update(current_segment)
current_state_ekf = ekf.get_x_est()
# print("x_est", current_state_ekf)
# calculat 3 vectors for ebene representation such that p=s_v+mu*r_v_1+theta*r_v_1
s_v = np.asarray([[0, 0, current_state_ekf[3] / current_state_ekf[2]]],
dtype="float32")
r_v_1 = np.asarray([[
0.5, 0, (current_state_ekf[3] - 0.5 * current_state_ekf[0]) /
current_state_ekf[2]
]],
dtype="float32")
r_v_2 = np.cross(r_v_1, np.transpose(current_state_ekf[0:3]))
r_v_2 = np.asarray(r_v_2, dtype="float32")
r_v_2 = normalize_vector(r_v_2)
r_v_1 = normalize_vector(r_v_1)
print("current_state_ekf", current_state_ekf)
# PUBLISH CURRENT POINT CLOUD
A = current_segment
for i in range(A.shape[0]):
x = A[i, 2]
y = -A[i, 0]
z = -A[i, 1]
r = 255
g = 0
b = 0
a = 150
rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0]
pt = [x, y, z, rgb]
points.append(pt)
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
# PointField('rgb', 12, PointField.UINT32, 1),
PointField('rgba', 12, PointField.UINT32, 1),
]
# print points
header = Header()
header.frame_id = "d435i_link"
pc2 = point_cloud2.create_cloud(header, fields, points)
pub.publish(pc2)
rviz = True
if rviz:
markerArray = MarkerArray()
i = 1
for mu in np.linspace(-1, 1, 10):
for theta in np.linspace(-1, 1, 10):
current_point = np.transpose(s_v) + mu * np.transpose(
r_v_1) + theta * np.transpose(r_v_2)
r = 0.02
marker = Marker()
marker.header.frame_id = "d435i_link"
marker.id = i
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = r * 2 # r*2
marker.scale.y = r * 2
marker.scale.z = r * 2
marker.color.r = 1
marker.color.g = 1
marker.color.a = 1 # transparency
marker.pose.orientation.w = 1.0
marker.pose.position.x = current_point[2] # x
marker.pose.position.y = -current_point[0] # y
marker.pose.position.z = -current_point[1] # z
markerArray.markers.append(marker)
i = i + 1
publisher_marker.publish(markerArray)
def node():
global frontiers, mapData, global1, global2, global3, globalmaps, litraIndx, n_robots, namespace_init_count
rospy.init_node('filter', anonymous=False)
# fetching all parameters
map_topic = rospy.get_param('~map_topic', '/map')
threshold = rospy.get_param('~costmap_clearing_threshold', 70)
info_radius = rospy.get_param(
'~info_radius', 1.0
) #this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate
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)
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) +
'/move_base_node/global_costmap/costmap', OccupancyGrid,
globalMap)
elif len(namespace) == 0:
rospy.Subscriber('/move_base_node/global_costmap/costmap',
OccupancyGrid, globalMap)
#wait if map is not received yet
while (len(mapData.data) < 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):
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) + '/base_link',
rospy.Time(0), rospy.Duration(10.0))
elif len(namespace) == 0:
tfLisn.waitForTransform(global_frame[1:], '/base_link', 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
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
frontiers = copy(centroids)
#-------------------------------------------------------------------------
#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
#-------------------------------------------------------------------------
#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()
def publish_tracked_people(self, now):
"""
Publish markers of tracked people to Rviz and to <people_tracked> topic
"""
people_tracked_msg = PersonArray()
people_tracked_msg.header.stamp = now
people_tracked_msg.header.frame_id = self.publish_people_frame
marker_id = 0
# Make sure we can get the required transform first:
if self.use_scan_header_stamp_for_tfs:
tf_time = now
try:
self.listener.waitForTransform(self.publish_people_frame,
self.fixed_frame, tf_time,
rospy.Duration(1.0))
transform_available = True
except:
transform_available = False
else:
tf_time = rospy.Time(0)
transform_available = self.listener.canTransform(
self.publish_people_frame, self.fixed_frame, tf_time)
marker_id = 0
if not transform_available:
rospy.loginfo(
"Person tracker: tf not avaiable. Not publishing people")
else:
for person in self.objects_tracked:
if person.is_person == True:
if self.publish_occluded or person.seen_in_current_scan: # Only publish people who have been seen in current scan, unless we want to publish occluded people
# Get position in the <self.publish_people_frame> frame
ps = PointStamped()
ps.header.frame_id = self.fixed_frame
ps.header.stamp = tf_time
ps.point.x = person.pos_x
ps.point.y = person.pos_y
try:
ps = self.listener.transformPoint(
self.publish_people_frame, ps)
except:
rospy.logerr(
"Not publishing people due to no transform from fixed_frame-->publish_people_frame"
)
continue
# publish to people_tracked topic
new_person = Person()
new_person.pose.position.x = ps.point.x
new_person.pose.position.y = ps.point.y
yaw = math.atan2(person.vel_y, person.vel_x)
quaternion = tf.transformations.quaternion_from_euler(
0, 0, yaw)
new_person.pose.orientation.x = quaternion[0]
new_person.pose.orientation.y = quaternion[1]
new_person.pose.orientation.z = quaternion[2]
new_person.pose.orientation.w = quaternion[3]
new_person.id = person.id_num
people_tracked_msg.people.append(new_person)
# publish rviz markers
# Cylinder for body
marker = Marker()
marker.header.frame_id = self.publish_people_frame
marker.header.stamp = now
marker.ns = "People_tracked"
marker.color.r = person.colour[0]
marker.color.g = person.colour[1]
marker.color.b = person.colour[2]
marker.color.a = (
rospy.Duration(3) -
(rospy.get_rostime() - person.last_seen)
).to_sec() / rospy.Duration(3).to_sec() + 0.1
marker.pose.position.x = ps.point.x
marker.pose.position.y = ps.point.y
marker.id = marker_id
marker_id += 1
marker.type = Marker.CYLINDER
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 1.2
marker.pose.position.z = 0.8
self.marker_pub.publish(marker)
# Sphere for head shape
marker.type = Marker.SPHERE
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.pose.position.z = 1.5
marker.id = marker_id
marker_id += 1
self.marker_pub.publish(marker)
# Text showing person's ID number
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
marker.id = marker_id
marker_id += 1
marker.type = Marker.TEXT_VIEW_FACING
marker.text = str(person.id_num)
marker.scale.z = 0.2
marker.pose.position.z = 1.7
self.marker_pub.publish(marker)
# Arrow pointing in direction they're facing with magnitude proportional to speed
marker.color.r = person.colour[0]
marker.color.g = person.colour[1]
marker.color.b = person.colour[2]
marker.color.a = (
rospy.Duration(3) -
(rospy.get_rostime() - person.last_seen)
).to_sec() / rospy.Duration(3).to_sec() + 0.1
start_point = Point()
end_point = Point()
start_point.x = marker.pose.position.x
start_point.y = marker.pose.position.y
end_point.x = start_point.x + 0.5 * person.vel_x
end_point.y = start_point.y + 0.5 * person.vel_y
marker.pose.position.x = 0.
marker.pose.position.y = 0.
marker.pose.position.z = 0.1
marker.id = marker_id
marker_id += 1
marker.type = Marker.ARROW
marker.points.append(start_point)
marker.points.append(end_point)
marker.scale.x = 0.05
marker.scale.y = 0.1
marker.scale.z = 0.2
self.marker_pub.publish(marker)
# <self.confidence_percentile>% confidence bounds of person's position as an ellipse:
cov = person.filtered_state_covariances[0][
0] + person.var_obs # cov_xx == cov_yy == cov
std = cov**(1. / 2.)
gate_dist_euclid = scipy.stats.norm.ppf(
1.0 - (1.0 - self.confidence_percentile) / 2., 0,
std)
marker.pose.position.x = ps.point.x
marker.pose.position.y = ps.point.y
marker.type = Marker.SPHERE
marker.scale.x = 2 * gate_dist_euclid
marker.scale.y = 2 * gate_dist_euclid
marker.scale.z = 0.01
marker.color.r = person.colour[0]
marker.color.g = person.colour[1]
marker.color.b = person.colour[2]
marker.color.a = 0.1
marker.pose.position.z = 0.0
marker.id = marker_id
marker_id += 1
self.marker_pub.publish(marker)
# Clear previously published people markers
for m_id in xrange(marker_id, self.prev_person_marker_id):
marker = Marker()
marker.header.stamp = now
marker.header.frame_id = self.publish_people_frame
marker.ns = "People_tracked"
marker.id = m_id
marker.action = marker.DELETE
self.marker_pub.publish(marker)
self.prev_person_marker_id = marker_id
# Publish people tracked message
self.people_tracked_pub.publish(people_tracked_msg)
import rospy
from visualization_msgs.msg import Marker
rospy.init_node("marker_pub")
pub = rospy.Publisher("visualization_marker", Marker, queue_size=10)
rate = rospy.Rate(25)
while not rospy.is_shutdown():
marker_data = Marker()
marker_data.header.frame_id = "map"
marker_data.header.stamp = rospy.Time.now()
marker_data.ns = "basic_shapes"
marker_data.id = 0
marker_data.action = Marker.ADD
marker_data.pose.position.x = 0.0
marker_data.pose.position.y = 0.0
marker_data.pose.position.z = 0.0
marker_data.pose.orientation.x = 0.0
marker_data.pose.orientation.y = 0.0
marker_data.pose.orientation.z = 1.0
marker_data.pose.orientation.w = 0.0
marker_data.color.r = 1.0
marker_data.color.g = 0.0
marker_data.color.b = 0.0
marker_data.color.a = 1.0
y = float(line.strip().split()[1])
if len(line.strip().split()) >= 3:
mode = int(line.strip().split()[2])
path_x.append(x)
path_y.append(y)
modes.append(mode)
path_len += 1
print(x, y, mode)
rospy.sleep(1)
while path_len > count:
marker = Marker()
marker.header.frame_id = "/base_link"
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.3
marker.scale.y = 0.3
marker.scale.z = 0.3
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
if modes[count]%2 == 0:
marker.color.r = 1
marker.color.g = 0.5
marker.color.b = 0
def lidar_cb(self, msg):
rospy.loginfo("starting lidar cb")
if self.road_map is None:
return
rospy.loginfo("got map")
ranges = np.array(msg.ranges)
num_ranges = len(ranges)
angles = (np.ones(num_ranges) * msg.angle_min) + (
np.arange(num_ranges) * msg.angle_increment)
# Discard extraneous data (< range_min or > range_max)
angles = angles[ranges > msg.range_min]
ranges = ranges[ranges > msg.range_min]
angles = angles[ranges < msg.range_max]
ranges = ranges[ranges < msg.range_max]
# Remove data points not in the viable range in front of the car, ie theta E (-viable_angle_range, viable_angle_range)
ranges = ranges[angles > -self.viable_angle_range]
angles = angles[angles > -self.viable_angle_range]
ranges = ranges[angles < self.viable_angle_range]
angles = angles[angles < self.viable_angle_range]
# Remove data points outside self.in_range
angles = angles[ranges < self.in_range]
ranges = ranges[ranges < self.in_range]
# Convert to cartesian, LIDAR-frame positions
x = ranges * np.cos(angles)
y = ranges * np.sin(angles)
new_num_ranges = len(x)
arr = np.vstack((x, y))
# Find transform from LIDAR frame to map frame
try:
(trans,
rot) = self.listener.lookupTransform(msg.header.frame_id,
self.road_map.frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rate.sleep()
return
# Convert translation + quaternion to homogeneous transformation matrix
T_lidar_map = transformations.quaternion_matrix(rot)
T_lidar_map = T_lidar_map.T
TEMP = transformations.quaternion_matrix(rot)
TEMP = np.linalg.inv(TEMP[:3, :3])
T_lidar_map[:3, 3] = np.matmul(TEMP, -np.array(trans).T)
#T_lidar_map = T_lidar_map.T
# Convert LIDAR points from LIDAR frame to map frame
arr_LIDAR = np.vstack(
(x, y, np.zeros(new_num_ranges), np.ones(new_num_ranges)))
arr_MAP = np.matmul(T_lidar_map, arr_LIDAR)
print(T_lidar_map)
arr_MAP = arr_MAP[:2]
x_map = arr_MAP[0]
y_map = arr_MAP[1]
# Find which points are on the roads
on_road = self.road_map.check_free(arr_MAP)
x_obs = x_map[on_road]
y_obs = y_map[on_road]
"""
# Find occupancy grid matrix with detected obstacles filled in
new_occ_matrix = self.road_map.add_obstructions(np.vstack((x_obs, y_obs)))
# Creates and publishes OccupancyGrid message with obstacles to /obstacles
obs_msg = OccupancyGrid()
# now = rospy.get_time()
# obs_msg.header.stamp = now
obs_msg.header.frame_id = self.road_map.frame
obs_msg.info = self.road_map.msg.info
# obs_msg.info.map_load_time = rospy.Time(0) # Probably unnecessary?
obs_msg.data = np.ravel(new_occ_matrix)
rospy.loginfo("publishing")
self.obs_pub.publish(obs_msg)
print("test")
"""
marker = Marker()
marker.header.frame_id = "map"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.scale.x = 0.5
marker.scale.y = 0.5
for i in range(len(x_obs)):
TEMP = Point()
TEMP.x = x_obs[i]
TEMP.y = y_obs[i]
TEMP.z = .05
marker.points.append(TEMP)
TEMP = ColorRGBA()
TEMP.r = 1
TEMP.g = 0
TEMP.b = 0
TEMP.a = 1
marker.colors.append(TEMP)
self.vis_pub.publish(marker)
#painting all the syntetic points
for i in xrange(10, 5000):
p = numpy.random.multivariate_normal(meanModel, covMatModel)
if syntetic_samples == None:
syntetic_samples = [p]
else:
syntetic_samples = concatenate((syntetic_samples, [p]), axis=0)
marker = Marker()
marker.header.frame_id = "/root"
marker.header.stamp = rospy.Time.now()
marker.ns = "my_namespace2"
marker.id = i
marker.type = visualization_msgs.msg.Marker.SPHERE
marker.action = visualization_msgs.msg.Marker.ADD
marker.pose.position.x = p[0]
marker.pose.position.y = p[1]
marker.pose.position.z = p[2]
marker.pose.orientation.x = 1
marker.pose.orientation.y = 1
marker.pose.orientation.z = 1
marker.pose.orientation.w = 1
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 0.0
points_pub.publish(marker)
from geometry_msgs.msg import Point
import tf
import numpy as np
rospy.init_node("marker_node", anonymous=True)
pub = rospy.Publisher("marker_test", Marker, queue_size=10)
rate = rospy.Rate(25)
while not rospy.is_shutdown():
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time.now()
marker.id = 0
marker.action = Marker.ADD
marker.lifetime = rospy.Duration()
marker.type = Marker.MESH_RESOURCE
marker.mesh_resource = "file:///media/psf/Home/Downloads/ros-kitti-project/src/ROS-notes-by-kwea123/kitti/bmw_x5/BMW X5 4.dae"
# marker.mesh_use_embedded_materials = True
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = -1.73
q = tf.transformations.quaternion_from_euler(np.pi/2, 0, np.pi);
marker.pose.orientation.x = q[0]
marker.pose.orientation.y = q[1]
marker.pose.orientation.z = q[2]
marker.pose.orientation.w = q[3]
def lidar_cb(self, msg):
if msg.header.stamp - self.last_update < self.min_time:
return
#read in ranges from lidar only for a given range
#global r
r = np.array(msg.ranges, dtype=np.float64)
rsmall = r[(a >= angle_from) * (a <= angle_to)]
asmall = a[(a >= angle_from) * (a <= angle_to)]
#a = np.array(msg.angle, dtype=mp.float64)
t = msg.header.stamp #rospy.Time(0)
if self.tf_listener.canTransform('map', 'laser', t):
(self.lx, self.ly,
_), rot = self.tf_listener.lookupTransform('map', 'laser', t)
_, _, self.ltheta = euler_from_quaternion(rot)
(self.x, self.y,
_), rot = self.tf_listener.lookupTransform('map', 'base', t)
_, _, self.theta = euler_from_quaternion(rot)
log.info('set pose: %.2f, %.2f, %.2f', self.lx, self.ly,
self.ltheta)
else:
log.warn('did not find current pose, ignoring update')
return
with timed_code('update'):
h, w = self.log_map.shape
rsmall[rsmall > VIEW_DISTANCE] = VIEW_DISTANCE
points = np.c_[rsmall, -asmall + self.ltheta]
global xs, ys
xs, ys = np.meshgrid(
si(np.arange(w)) - self.x0,
si(np.arange(h)) - self.y0)
with timed_code('-beam map'):
hits = beam_map(self.lx, self.ly, points, self.x, self.y,
self.theta)
self.log_map[hits > 0] += (l_view - l_0)
#translate log map to occupancygrid map (0-100)
self.explore_map[:, :] = 1.0 - 1.0 / (1.0 + np.exp(self.log_map))
#self.getnewexploregoal(hitslidar)
self.last_update = msg.header.stamp
#**********************************************
# get cool new point *****************************
hits = np.zeros(xs.shape, dtype=np.uint8)
#ris = [i for i in range(len(r)) if (0.15 < r[i] < 4)]
ris = list(range(0, len(r), 2))
sample = [i for i in ris if r[i] > 0.12]
#sample = np.random.choice(sample, 100)
randomr = [uniform(0.12, r[i] - 0.05) for i in sample]
randomx = [
self.lx + randomr[j] * np.cos(-a[i] + self.ltheta)
for j, i in enumerate(sample)
]
randomy = [
self.ly + randomr[j] * np.sin(-a[i] + self.ltheta)
for j, i in enumerate(sample)
]
costs = np.zeros(len(randomx))
countfree = 0
for i in range(len(randomx)):
if ((randomx[i] > np.min(xs)) * (randomx[i] < np.max(xs)) *
(randomy[i] > np.min(ys)) * (randomy[i] < np.max(ys))):
costs[i] += np.sqrt((self.x - randomx[i])**2 +
(self.y - randomy[i])**2) * 1
angle = (-a[i] + self.ltheta - self.theta) * 180 / np.pi
costs[i] += np.abs((np.mod(angle + 180, 360) - 180)) * 100
#info gain
hits = beam_map2(randomx[i], randomy[i], -a[i] + self.ltheta)
costs[i] += np.sum(self.explore_map[hits == HIT_VIEW]) * 1
#self.explore_map[hits == HIT_VIEW]=1
#penelty for point we have been to
if (self.explore_map[gu(randomy[i] + self.y0),
gu(randomx[i] + self.x0)] > 0.2):
costs[i] += 1000
countfree += 1
if (self.occ_map[gu(randomy[i] + self.y0),
gu(randomx[i] + self.x0)] > 0.2):
costs[i] += 10e3
countfree += 1
elif (self.occ_map.line_max(
(self.x, self.y),
(randomx[i] + self.x0, randomy[i] + self.y0)) > 0.2):
costs[i] += 1000
countfree += 1
else:
costs[i] = np.inf
#new points as we are in a deadend
if False and (countfree > len(randomx) - 2):
costs = np.zeros(len(randomx))
randomx = [uniform(self.xmin, self.xmax) for i in range(0, len(r))]
randomy = [uniform(self.ymin, self.ymax) for i in range(0, len(r))]
randoma = [uniform(-np.pi, np.pi) for i in range(0, len(r))]
costs = np.zeros(len(randomx))
countfree = 0
for i in range(len(randomx)):
if ((randomx[i] > np.min(xs)) * (randomx[i] < np.max(xs)) *
(randomy[i] > np.min(ys)) * (randomy[i] < np.max(ys))):
costs[i] += np.sqrt((self.x - randomx[i])**2 +
(self.y - randomy[i])**2) * 1
angle = (-a[i] + self.ltheta - self.theta) * 180 / np.pi
costs[i] += np.abs((np.mod(angle + 180, 360) - 180)) * 1
hits = beam_map2(randomx[i], randomy[i], randoma[i])
costs[i] += np.sum(self.explore_map[hits == HIT_VIEW]) * 1
if (self.explore_map[gu(randomy[i] + self.y0),
gu(randomx[i] + self.x0)] > 0.2):
costs[i] += 1000
if (self.occ_map[gu(randomy[i] + self.y0),
gu(randomx[i] + self.x0)] > 0.2):
costs[i] += 10000
#weights = 1.0/costs
#ps = weights/weights.sum()
#j = np.random.choice(len(costs), p=ps)
j = np.argmin(costs)
msg = PoseStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "map"
msg.pose.orientation.w = 1.0
msg.pose.position.x = randomx[j]
msg.pose.position.y = randomy[j]
msg.pose.orientation.z = a[j]
#pub.publish(msg)
self.pub_goal.publish(msg)
print(randomx[j])
print(randomy[j])
print(a[j] * 180 / np.pi)
print(costs[j])
marker = Marker()
marker.header.frame_id = "/map"
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 = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = randomx[j]
marker.pose.position.y = randomy[j]
self.pub_point.publish(marker)
markerArray = MarkerArray()
for i in range(len(randomx)):
markera = Marker()
markera.header.frame_id = "/map"
markera.type = marker.SPHERE
markera.action = marker.ADD
markera.scale.x = 0.05
markera.scale.y = 0.05
markera.scale.z = 0.05
markera.color.a = 1.0
markera.color.r = 0.0
markera.color.g = 1.0
markera.color.b = 0.0
markera.pose.orientation.w = 1.0
markera.id = i
markera.pose.position.x = randomx[i]
markera.pose.position.y = randomy[i]
markerArray.markers.append(markera)
self.pub_allpoints.publish(markerArray)
def create_frustum_marker(self, markerArray, view, pose, view_values):
marker1 = Marker()
marker1.id = self.marker_id
self.marker_id += 1
marker1.header.frame_id = "/map"
marker1.type = marker1.LINE_LIST
marker1.action = marker1.ADD
marker1.scale.x = 0.05
marker1.color.a = 0.3
vals = view_values.values()
max_val = max(vals)
non_zero_vals = filter(lambda a: a != 0, vals)
min_val = min(non_zero_vals)
print min_val, max_val, view_values[view.ID]
marker1.color.r = r_func(
float((view_values[view.ID] - min_val)) / float(
(max_val - min_val + 1)))
marker1.color.g = g_func(
float((view_values[view.ID] - min_val)) / float(
(max_val - min_val + 1)))
marker1.color.b = b_func(
float((view_values[view.ID] - min_val)) / float(
(max_val - min_val + 1)))
marker1.pose.orientation = pose.orientation
marker1.pose.position = pose.position
points = view.get_frustum()
marker1.points.append(points[0])
marker1.points.append(points[1])
marker1.points.append(points[2])
marker1.points.append(points[3])
marker1.points.append(points[0])
marker1.points.append(points[2])
marker1.points.append(points[1])
marker1.points.append(points[3])
marker1.points.append(points[4])
marker1.points.append(points[5])
marker1.points.append(points[6])
marker1.points.append(points[7])
marker1.points.append(points[4])
marker1.points.append(points[6])
marker1.points.append(points[5])
marker1.points.append(points[7])
marker1.points.append(points[0])
marker1.points.append(points[4])
marker1.points.append(points[2])
marker1.points.append(points[6])
marker1.points.append(points[1])
marker1.points.append(points[5])
marker1.points.append(points[3])
marker1.points.append(points[7])
markerArray.markers.append(marker1)
def sim():
rospy.init_node('optimization')
o = Optimizer()
c = Cfs(21, 0)
rospy.Subscriber('/odom', Odometry, o.odom_callback)
rospy.Subscriber('/filteredGPS', Point, o.filter_callback)
rospy.Subscriber('/obstacle1_poly', PolygonStamped,
o.obstacle_poly_callback) # from fake_human nodes
rospy.Subscriber('/obstacle1_v', Twist, o.obstacle_vel_callback)
rospy.Subscriber('/obstacle2_poly', Polygons,
o.lobstacle_poly_callback) # from lidar
#rospy.Subscriber('/obstacle2_v', Twist, o.lobstacle_vel_callback)
rospy.Subscriber('/human', Point32, o.human_callback)
rospy.Subscriber('/goal_state', Point32, o.goal_callback)
path_pub = rospy.Publisher('/path', Path, queue_size=100)
margin_rviz_pub = rospy.Publisher('/margins',
PolygonStamped,
queue_size=100)
goal_pub = rospy.Publisher('/goalrviz', PointStamped, queue_size=100)
line_viz = rospy.Publisher('/line', Marker, queue_size=50)
cmv_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
rate = rospy.Rate(10)
try:
barrier = rospy.get_param('~barrier_radius')
except:
barrier = 1.5
c.set_barrier(barrier)
while not rospy.is_shutdown():
o.move_pred()
c.set_margin(o.margin)
obstacles = o.obstacles + o.lidar_obs
vels = o.vels + o.lidar_vels
refpath, linegen = c.optimize(
obstacles, vels, o.x, o.goal,
human=o.human) # o.human if tracking human
marker = Marker()
marker.header.frame_id = "/odom"
marker.type = marker.LINE_STRIP
marker.action = marker.ADD
marker.scale.x = 0.03
marker.scale.y = 0.03
marker.scale.z = 0.03
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0
pts = []
for i in range(-10, 10):
add = Point()
add.x = i
add.y = linegen[0] * i + linegen[1]
pts.append(add)
marker.points = pts
line_viz.publish(marker)
if stopCheck(refpath, 0.2):
path = to_path(refpath)
path_pub.publish(path)
else:
path = to_path([])
path_pub.publish(path)
margin = o.get_margin_polygon()
margin_rviz_pub.publish(margin)
goal_pub.publish(o.get_goal())
rate.sleep()
audio_pub = rospy.Publisher('train_sound', AudioData, queue_size=100)
marker_pub = rospy.Publisher('train_ground_truth', MarkerArray, queue_size=1)
assert sample_rate % MSG_FREQ == 0
frames_per_msg = sample_rate / MSG_FREQ
for sounds, positions in gen:
if rospy.is_shutdown():
break
sounds = sounds[0]
positions = positions[0]
rate = rospy.Rate(MSG_FREQ)
sound = np.int16(sounds.mean(0) * 32768)
delete_all = Marker()
delete_all.action = Marker.DELETEALL
delete_all.ns = 'sound_sources'
delete_all = MarkerArray(markers=[delete_all])
for t in xrange((sound.shape[0]+frames_per_msg-1)/frames_per_msg):
begin = t*frames_per_msg
end = min((t+1)*frames_per_msg, sound.shape[0])
audio_pub.publish(AudioData(data = sound[begin:end].tostring()))
markers = MarkerArray()
for i, (snd, pos) in enumerate(zip(sounds, positions)):
scale = np.abs(snd[begin:end]).max()
if scale == 0.:
continue
marker = Marker()
marker.header.stamp = rospy.Time.now()
def publish_tracked_objects(self, now):
"""
Publish markers of tracked objects to Rviz
"""
# Make sure we can get the required transform first:
if self.use_scan_header_stamp_for_tfs:
tf_time = now
try:
self.listener.waitForTransform(self.publish_people_frame,
self.fixed_frame, tf_time,
rospy.Duration(1.0))
transform_available = True
except:
transform_available = False
else:
tf_time = rospy.Time(0)
transform_available = self.listener.canTransform(
self.publish_people_frame, self.fixed_frame, tf_time)
marker_id = 0
if not transform_available:
rospy.loginfo(
"Person tracker: tf not avaiable. Not publishing people")
else:
for track in self.objects_tracked:
if track.is_person:
continue
if self.publish_occluded or track.seen_in_current_scan: # Only publish people who have been seen in current scan, unless we want to publish occluded people
# Get the track position in the <self.publish_people_frame> frame
ps = PointStamped()
ps.header.frame_id = self.fixed_frame
ps.header.stamp = tf_time
ps.point.x = track.pos_x
ps.point.y = track.pos_y
try:
ps = self.listener.transformPoint(
self.publish_people_frame, ps)
except:
continue
# publish rviz markers
marker = Marker()
marker.header.frame_id = self.publish_people_frame
marker.header.stamp = now
marker.ns = "objects_tracked"
if track.in_free_space < self.in_free_space_threshold:
marker.color.r = track.colour[0]
marker.color.g = track.colour[1]
marker.color.b = track.colour[2]
else:
marker.color.r = 0
marker.color.g = 0
marker.color.b = 0
marker.color.a = 1
marker.pose.position.x = ps.point.x
marker.pose.position.y = ps.point.y
marker.id = marker_id
marker_id += 1
marker.type = Marker.CYLINDER
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.2
marker.pose.position.z = 0.15
self.marker_pub.publish(marker)
# # Publish a marker showing distance travelled:
# if track.dist_travelled > 1:
# marker.color.r = 1.0
# marker.color.g = 1.0
# marker.color.b = 1.0
# marker.color.a = 1.0
# marker.id = marker_id
# marker_id += 1
# marker.type = Marker.TEXT_VIEW_FACING
# marker.text = str(round(track.dist_travelled,1))
# marker.scale.z = 0.1
# marker.pose.position.z = 0.6
# self.marker_pub.publish(marker)
# Publish <self.confidence_percentile>% confidence bounds of track as an ellipse:
# cov = track.filtered_state_covariances[0][0] + track.var_obs # cov_xx == cov_yy == cov
# std = cov**(1./2.)
# gate_dist_euclid = scipy.stats.norm.ppf(1.0 - (1.0-self.confidence_percentile)/2., 0, std)
# marker.type = Marker.SPHERE
# marker.scale.x = 2*gate_dist_euclid
# marker.scale.y = 2*gate_dist_euclid
# marker.scale.z = 0.01
# marker.color.r = 1.0
# marker.color.g = 1.0
# marker.color.b = 1.0
# marker.color.a = 0.1
# marker.pose.position.z = 0.0
# marker.id = marker_id
# marker_id += 1
# self.marker_pub.publish(marker)
# Clear previously published track markers
for m_id in xrange(marker_id, self.prev_track_marker_id):
marker = Marker()
marker.header.stamp = now
marker.header.frame_id = self.publish_people_frame
marker.ns = "objects_tracked"
marker.id = m_id
marker.action = marker.DELETE
self.marker_pub.publish(marker)
self.prev_track_marker_id = marker_id
def create_axis_marker(self,
axis,
id_num,
rgba=None,
name=None,
axes_scale=1.0):
marker = Marker()
marker.header.frame_id = self.frame_id
marker.header.stamp = self.timestamp
marker.id = id_num
marker.type = marker.ARROW
marker.action = marker.ADD
marker.lifetime = rospy.Duration(1.0)
if name is not None:
# although useful, this causes a warning and rviz and goes
# against the documentation "NOTE: only used for text markers
# string text"
marker.text = name
# axis_arrow = {'head_diameter': 0.02,
# 'shaft_diameter': 0.012,
# 'head_length': 0.012,
# 'length': 0.08}
axis_arrow = {
'head_diameter': 0.02 * axes_scale,
'shaft_diameter': 0.012 * axes_scale,
'head_length': 0.012 * axes_scale,
'length': 0.08 * axes_scale
}
# "scale.x is the shaft diameter, and scale.y is the
# head diameter. If scale.z is not zero, it specifies
# the head length." -
# http://wiki.ros.org/rviz/DisplayTypes/Marker#Arrow_.28ARROW.3D0.29
marker.scale.x = axis_arrow['shaft_diameter']
marker.scale.y = axis_arrow['head_diameter']
marker.scale.z = axis_arrow['head_length']
if rgba is None:
# make as bright as possible
den = float(np.max(self.id_color))
marker.color.r = self.id_color[2] / den #1.0
marker.color.g = self.id_color[1] / den #0.0
marker.color.b = self.id_color[0] / den #0.0
marker.color.a = 0.5
else:
c = marker.color
c.r, c.g, c.b, c.a = rgba
start_point = Point()
x = self.marker_position[0]
y = self.marker_position[1]
z = self.marker_position[2]
start_point.x = x
start_point.y = y
start_point.z = z
end_point = Point()
length = axis_arrow['length']
end_point.x = x + (axis[0] * length)
end_point.y = y + (axis[1] * length)
end_point.z = z + (axis[2] * length)
marker.points = [start_point, end_point]
return marker
def create_marker(pos,
orientation=1.0,
color=(1.0, 1.0, 1.0),
m_scale=0.5,
frame_id="/velodyneVPL",
duration=10,
marker_id=0,
mesh_resource=None,
marker_type=2,
marker_text=""):
"""Create marker object using the map information and the node position
:param pos: list of 3d postion for the marker
:param orientation: orientation of the maker (1 for no orientation)
:param color: a 3 vector of 0-1 rgb values
:param m_scale: scale of the marker (1.0) for normal scale
:param frame_id: ROS frame id
:param duration: duration in seconds for this marker dissapearance
:param marker_id:
:param mesh_resource:
:param marker_type: one of the following types (use the int value)
http://wiki.ros.org/rviz/DisplayTypes/Marker
ARROW = 0
CUBE = 1
SPHERE = 2
CYLINDER = 3
LINE_STRIP = 4
LINE_LIST = 5
CUBE_LIST = 6
SPHERE_LIST = 7
POINTS = 8
TEXT_VIEW_FACING = 9
MESH_RESOURCE = 10
TRIANGLE_LIST = 11
:param marker_text: text string used for the marker
:return:
"""
marker = Marker()
marker.header.frame_id = frame_id
marker.id = marker_id
if mesh_resource:
marker.type = marker.MESH_RESOURCE
marker.mesh_resource = mesh_resource
else:
marker.type = marker_type
marker.action = marker.ADD
marker.scale.x = m_scale
marker.scale.y = m_scale
marker.scale.z = m_scale
marker.color.a = 1.0
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.pose.orientation.w = orientation
marker.text = marker_text
marker.pose.position.x = pos[0]
marker.pose.position.y = pos[1]
marker.pose.position.z = pos[2]
d = rospy.Duration.from_sec(duration)
marker.lifetime = d
return marker
def people_tracked_callback(self, people_tracked_msg):
toc = timeit.default_timer()
# Don't start keeping tracking of tracking time until at least one person has started to be tracked
# This is because the tracking programs are often slow to startup when no tfs have been published yet
if len(people_tracked_msg.people) > 0:
self.tracking_started = True
if self.tracking_started:
tracking_time = toc - self.tic
self.tracking_time_cum += tracking_time
self.tracking_msg_count += 1
if tracking_time > self.worst_tracking_time:
self.worst_tracking_time = tracking_time
rospy.loginfo(
"***************************Worst tracking time so far: %.2f***************************",
self.worst_tracking_time)
if self.savebag_filename is not None:
save_time = (self.next_scan_msg_time -
self.cur_scan_msg_time) / 2. + self.cur_scan_msg_time
people_tracked_msg.header.stamp = save_time
self.savebag.write('/people_tracked', people_tracked_msg,
save_time)
# save rviz markers
marker_id = 0
for person in people_tracked_msg.people:
self.tracking_started = True
if person.id not in self.person_colours:
self.person_colours[person.id] = (random.random(),
random.random(),
random.random())
marker = Marker()
marker.header.frame_id = people_tracked_msg.header.frame_id
marker.header.stamp = save_time
marker.ns = "playback_and_record_tracked"
marker.color.r = self.person_colours[person.id][0]
marker.color.g = self.person_colours[person.id][1]
marker.color.b = self.person_colours[person.id][2]
marker.color.a = 1
marker.pose.position.x = person.pose.position.x
marker.pose.position.y = person.pose.position.y
for i in xrange(
2
): # publish two markers per person: one for body and one for head
marker.id = marker_id
marker_id += 1
if i == 0: # cylinder for body shape
marker.type = Marker.CYLINDER
marker.scale.x = 0.15
marker.scale.y = 0.15
marker.scale.z = 0.6
marker.pose.position.z = 0.6
else: # sphere for head shape
marker.type = Marker.SPHERE
marker.scale.x = 0.15
marker.scale.y = 0.15
marker.scale.z = 0.15
marker.pose.position.z = 0.975
self.savebag.write('/visualization_marker', marker,
save_time)
# Text showing person's ID number
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
marker.id = marker_id
marker_id += 1
marker.type = Marker.TEXT_VIEW_FACING
marker.text = str(person.id)
marker.scale.z = 0.2
marker.pose.position.z = 1.3
self.savebag.write('/visualization_marker', marker, save_time)
# Clear previously published people markers
for m_id in xrange(marker_id, self.prev_person_marker_id):
marker = Marker()
marker.header.stamp = people_tracked_msg.header.stamp
marker.header.frame_id = people_tracked_msg.header.frame_id
marker.ns = "playback_and_record_tracked"
marker.id = m_id
marker.action = marker.DELETE
self.savebag.write('/visualization_marker', marker, save_time)
self.prev_person_marker_id = marker_id
self.nextScanMsg()
def main ():
SUFFIX = ''
ONE_BY_ONE = False
# Whether to visualize normals
VIS_NORMS = True
rospy.init_node ('visualize_contacts', anonymous=True)
vis_pub = rospy.Publisher ('visualization_marker', Marker, queue_size=5)
vis_arr_pub = rospy.Publisher ('visualization_marker_array', MarkerArray,
queue_size=5)
objs = ConfigReadYAML.read_scene_paths ()
# List of strings
obj_names = objs [ConfigReadYAML.NAME_IDX]
# For marker color based on grasp quality
ENERGY_MIN = -1.8 #0.7 #0.4
ENERGY_MAX = -0.8 #1.6 #0.6
# Number of segments to calculate colormap
N_COLOR_SEG = 10
#SEG_WIDTH = (ENERGY_MAX - ENERGY_MIN) / N_COLOR_SEG
mesh_dir = 'package://grasp_collection/graspit_input/models/objects/dexnet/'
# Marker size in meters
CONTACT_SIZE = 0.003
# Length of normal vector
NORM_LEN = 0.01
terminate = False
# Loop through each object
for o_i in range (len (obj_names)):
# Delete all markers so next round starts clean
del_mkr = Marker ()
create_marker (Marker.SPHERE_LIST, 'contacts', '/world', 0,
0, 0, 0, 0, 0, 0, 0.5, CONTACT_SIZE, CONTACT_SIZE, CONTACT_SIZE,
del_mkr, duration=0)
del_mkr.action = Marker.DELETEALL
vis_pub.publish (del_mkr)
# File name of world XML, one file per object
world_fname = os.path.join (world_subdir, obj_names [o_i])
# A set of contacts per object
mesh_mkr = Marker ()
create_marker (Marker.MESH_RESOURCE, 'mesh', '/world', 0,
0, 0, 0, 1, 1, 1, 0.5, 1, 1, 1,
mesh_mkr, duration=0)
# OBJ file path
mesh_mkr.mesh_resource = mesh_dir + objects [o_i]
# nGrasps x 3
contacts_m, normals_m = GraspIO.read_contacts (os.path.basename (
world_fname), SUFFIX)
# Contacts meta tells how many contacts per grasp
cmeta = GraspIO.read_contact_meta (os.path.basename (world_fname), SUFFIX)
# List of nGrasps elts
energies = GraspIO.read_energies (os.path.basename (world_fname),
ENERGY_ABBREV, SUFFIX)
print ('Energy min: %g, max: %g, mean: %g, median: %g' % (np.min (energies),
np.max (energies), np.mean (energies), np.median (energies)))
# A set of contacts per object
o_contacts_mkr = Marker ()
create_marker (Marker.SPHERE_LIST, 'contacts', '/world', 0,
0, 0, 0, 0, 0, 0, 0.5, CONTACT_SIZE, CONTACT_SIZE, CONTACT_SIZE,
o_contacts_mkr, duration=0)
o_normals_arr = MarkerArray ()
# Contact index for current grasp
ct_start_i = 0
# Loop through each grasp
for g_i in range (len (energies)):
# A set of contacts per object
contacts_mkr = Marker ()
create_marker (Marker.SPHERE_LIST, 'contacts', '/world', 0,
0, 0, 0, 0, 0, 0, 0.5, CONTACT_SIZE, CONTACT_SIZE, CONTACT_SIZE,
contacts_mkr, duration=0)
normals_arr = MarkerArray ()
# Energies are negative. Use absolute value
#fraction = (abs (energies [g_i]) - ENERGY_MIN) / (ENERGY_MAX - ENERGY_MIN)
fraction = 1 - (energies [g_i] - ENERGY_MIN) / (ENERGY_MAX - ENERGY_MIN)
color_tuple = mpl_color (fraction, N_COLOR_SEG, colormap_name='jet')
print ('energy: %g, fraction: %g' % (energies [g_i], fraction))
color = ColorRGBA ()
color.r = color_tuple [0]
color.g = color_tuple [1]
color.b = color_tuple [2]
color.a = 0.8
# Loop through each contact point x y z in this grasp
for p_i in range (ct_start_i, ct_start_i + cmeta [g_i]):
# Contact point
pt = Point ()
pt.x = contacts_m [p_i, 0]
pt.y = contacts_m [p_i, 1]
pt.z = contacts_m [p_i, 2]
contacts_mkr.points.append (pt)
contacts_mkr.colors.append (color)
o_contacts_mkr.points.append (pt)
o_contacts_mkr.colors.append (color)
# Contact normal vector
norm_vec = normals_m [p_i, :] - contacts_m [p_i, :]
norm_vec /= np.linalg.norm (norm_vec)
norm_vec *= NORM_LEN
normals_mkr = Marker ()
create_marker (Marker.ARROW, 'normals', '/world', p_i,
# sx: shaft diameter, sy: arrowhead diameter
0, 0, 0, color.r, color.g, color.b, color.a, 0.001, 0.002, 0,
normals_mkr, duration=0)
# Start at contact point
norm_s = Point ()
norm_s.x = contacts_m [p_i, 0]
norm_s.y = contacts_m [p_i, 1]
norm_s.z = contacts_m [p_i, 2]
normals_mkr.points.append (norm_s)
# End at endpoint of vector
norm_t = Point ()
norm_t.x = contacts_m [p_i, 0] + norm_vec [0]
norm_t.y = contacts_m [p_i, 1] + norm_vec [1]
norm_t.z = contacts_m [p_i, 2] + norm_vec [2]
normals_mkr.points.append (norm_t)
normals_arr.markers.append (normals_mkr)
o_normals_arr.markers.append (normals_mkr)
#print ('Normal length: %g' % (np.linalg.norm (normals_m [p_i, :] - contacts_m [p_i, :])))
# Update for next grasp
ct_start_i += cmeta [g_i]
if ONE_BY_ONE:
for i in range (5):
vis_pub.publish (mesh_mkr)
vis_pub.publish (contacts_mkr)
if VIS_NORMS:
vis_arr_pub.publish (normals_arr)
rospy.sleep (0.5)
uinput = raw_input ('Press enter to go to next grasp, s to skip to next object, q to quit: ')
if uinput.lower () == 's':
break
elif uinput.lower () == 'q':
terminate = True
break
# Delete all markers so next round starts clean
del_mkr = Marker ()
create_marker (Marker.SPHERE_LIST, 'contacts', '/world', 0,
0, 0, 0, 0, 0, 0, 0.5, CONTACT_SIZE, CONTACT_SIZE, CONTACT_SIZE,
del_mkr, duration=0)
del_mkr.action = Marker.DELETEALL
vis_pub.publish (del_mkr)
#print (o_contacts_mkr.points)
#print (o_contacts_mkr.colors)
if terminate:
break
# Cumulative per-object contacts
for i in range (5):
vis_pub.publish (mesh_mkr)
vis_pub.publish (o_contacts_mkr)
if VIS_NORMS:
vis_arr_pub.publish (o_normals_arr)
rospy.sleep (0.5)
uinput = raw_input ('Press enter to go to next object, q to quit: ')
if uinput.lower () == 'q':
break
def det_result_cb(self, msg):
dets_list = None
info_list = None
for idx, det in enumerate(msg.dets_list):
if dets_list is None:
dets_list = np.array([det.x, det.y, det.radius],
dtype=np.float32)
info_list = np.array([det.confidence, det.class_id],
dtype=np.float32)
else:
dets_list = np.vstack([dets_list, [det.x, det.y, det.radius]])
info_list = np.vstack(
[info_list, [det.confidence, det.class_id]])
# if len(dets_list.shape) == 1: dets_list = np.expand_dims(dets_list, axis=0)
# if len(info_list.shape) == 1: info_list = np.expand_dims(info_list, axis=0)
# if dets_list.shape[1] == 0:
# # If there is no detection, just pack the laserscan topic for localmap creation
# trk3d_array = Trk3DArray()
# trk3d_array.header.frame_id = msg.header.frame_id
# trk3d_array.header.stamp = rospy.Time.now()
# trk3d_array.scan = msg.scan
# self.pub_trk3d_result.publish(trk3d_array)
# return
# rospy.loginfo('number of alive trks: {}'.format(len(self.mot_tracker.trackers)))
if dets_list is not None:
if len(dets_list.shape) == 1:
dets_list = np.expand_dims(dets_list, axis=0)
if len(info_list.shape) == 1:
info_list = np.expand_dims(info_list, axis=0)
dets_all = {'dets': dets_list, 'info': info_list}
trackers = self.mot_tracker.update(dets_all)
else:
# if no detection in a sequence
trackers = self.mot_tracker.update_with_no_dets()
# Skip the visualization at first callback
if self.last_time is None:
self.last_time = rospy.Time.now()
# self.last_time = time.time()
return
# saving results, loop over each tracklet
marker_array = MarkerArray()
trk3d_array = Trk3DArray()
time_now = rospy.Time.now()
delta_t = (time_now - self.last_time).to_sec()
if delta_t < 0.1:
delta_t = 0.1
# small_time = (0.1 - delta_t) * 0.92
# delta_t += small_time
# rospy.sleep(small_time)
# sys.stdout.write("{:.4f} s \r".format(delta_t))
# sys.stdout.flush()
self.last_time = time_now
for idx, d in enumerate(trackers):
'''
x, y, r, vx, vy, id, confidence, class_id
'''
d_now = np.sqrt(d[0]**2 + d[1]**2)
#print(d[0])
#print(d[1])
# vx, vy = np.array([d[3], d[4]]) / (time.time() - self.last_time)
vx, vy = np.array(
[d[3], d[4]]
) / delta_t #- np.array([self.ego_velocity.x, self.ego_velocity.y])-np.array([d_now*math.sin(self.ego_theta.z),d_now*math.cos(self.ego_theta.z)])-
speed = np.sqrt(
vx**2 + vy**
2) # Note: reconstruct speed by multipling the sampling rate
yaw = np.arctan2(vy, vx)
if (speed > 2):
speed = 2
if (speed > 0.5):
dangerous = 2 * speed / (
1 + np.exp(0.3 * np.sqrt(d[0]**2 + d[1]**2)))
#print("peolple dangerous:",dangerous)
else:
dangerous = 2 * 0.5 / (
1 + np.exp(0.3 * np.sqrt(d[0]**2 + d[1]**2)))
#print("peolple dangerous:",dangerous)
#print(speed)
# Custom ROS message
trk3d_msg = Trk3D()
trk3d_msg.x, trk3d_msg.y = d[0], d[1]
# print("peolple x:",trk3d_msg.x)
# print("peolple y:",trk3d_msg.y)
trk3d_msg.radius = d[2]
trk3d_msg.vx, trk3d_msg.vy = vx, vy
trk3d_msg.yaw = yaw
trk3d_msg.confidence = d[6]
trk3d_msg.class_id = int(d[7])
trk3d_msg.dangerous = dangerous
trk3d_array.trks_list.append(trk3d_msg)
#print("peolple speed:",speed)
# Visualization
marker = Marker()
marker.header.frame_id = 'odom_filtered'
marker.header.stamp = rospy.Time()
marker.ns = 'object'
marker.id = idx
marker.lifetime = rospy.Duration(
MARKER_LIFETIME
) #The lifetime of the bounding-box, you can modify it according to the power of your machine.
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.scale.x = d[2] * 2
marker.scale.y = d[2] * 2
marker.scale.z = 1.0
marker.color.b = 1.0
marker.color.a = 0.5 #The alpha of the bounding-box
marker.pose.position.x = d[0]
marker.pose.position.y = d[1]
marker.pose.position.z = 0.5
q = euler_to_quaternion(0, 0, yaw)
marker.pose.orientation.x = q[0]
marker.pose.orientation.y = q[1]
marker.pose.orientation.z = q[2]
marker.pose.orientation.w = q[3]
marker_array.markers.append(marker)
# Show tracking ID
str_marker = Marker()
str_marker.header.frame_id = 'odom_filtered'
str_marker.header.stamp = rospy.Time()
str_marker.ns = 'text'
str_marker.id = idx
str_marker.scale.z = 0.4 #The size of the text
str_marker.color.b = 1.0
str_marker.color.g = 1.0
str_marker.color.r = 1.0
str_marker.color.a = 1.0
str_marker.pose.position.x = d[0]
str_marker.pose.position.y = d[1]
str_marker.pose.position.z = 0.5
str_marker.lifetime = rospy.Duration(MARKER_LIFETIME)
str_marker.type = Marker.TEXT_VIEW_FACING
str_marker.action = Marker.ADD
str_marker.text = "{}".format(int(d[5])) # str(d[5])
marker_array.markers.append(str_marker)
# Show direction
arrow_marker = copy.deepcopy(marker)
arrow_marker.type = Marker.ARROW
arrow_marker.ns = 'direction'
arrow_marker.scale.x = 2 * (speed / 1.5)
arrow_marker.scale.y = 0.2
arrow_marker.scale.z = 0.2
marker_array.markers.append(arrow_marker)
self.pub_trk3d_vis.publish(marker_array)
trk3d_array.header.frame_id = msg.header.frame_id
trk3d_array.header.stamp = rospy.Time().now()
trk3d_array.pointcloud = msg.pointcloud
self.pub_trk3d_result.publish(trk3d_array)
def nextScanMsg(self):
self.mutex.acquire() # TODO necessary?
# First display and save <next_scan_msg>
# It is now referred to as the cur_scan_msg
# And we'll iterate through the rosbag until we reach the next_scan_msg
if self.next_scan_msg is not None:
topic, msg, time = self.next_scan_msg
self.cur_scan_msg_time = time
self.next_scan_msg = None
if self.savebag_filename is not None:
self.savebag.write(topic, msg, time)
self.tic = timeit.default_timer()
self.laser_pub.publish(msg)
if self.bag_empty:
# Shut down and close rosbags safetly
rospy.loginfo("End of readbag file, shutting down")
rospy.signal_shutdown("End of readbag file, shutting down")
marker_id = 0
topic = None
while topic != self.scan_topic and not self.bag_empty:
try:
# Get the next message from the rosbag
topic, msg, time = next(self.msg_gen)
if topic == self.scan_topic:
self.next_scan_msg = (topic, msg, time)
self.next_scan_msg_time = time
else:
# Save message to savebag
if self.savebag_filename is not None:
self.savebag.write(topic, msg, time)
# Publish them
if topic == "/tf":
self.tf_pub.publish(msg)
elif topic == "/visualization_marker":
marker = Marker()
marker.header = msg.header
marker.ns = msg.ns
marker.id = msg.id
marker.type = msg.type
marker.action = msg.action
marker.pose = msg.pose
marker.scale = msg.scale
marker.color = msg.color
marker.lifetime = msg.lifetime
marker.frame_locked = msg.frame_locked
marker.points = msg.points
marker.colors = msg.colors
marker.text = msg.text
marker.mesh_resource = msg.mesh_resource
marker.mesh_use_embedded_materials = msg.mesh_use_embedded_materials
self.marker_pub.publish(marker)
except StopIteration:
# Passed the last message in the rosbag
self.bag_empty = True
# Clear previously published people markers
if self.prev_marker_id > marker_id:
for m_id in xrange(marker_id, self.prev_marker_id):
marker = Marker()
marker.header.frame_id = self.fixed_frame
marker.ns = "clear_mot_display"
marker.id = m_id
marker.action = marker.DELETE
self.marker_pub.publish(marker)
self.prev_marker_id = marker_id
self.mutex.release()
[[0, 0, 0, px], [0, 0, 0, py], [0, 0, 0, pz], [0, 0, 0, 0]])
T_base_d = T_base_tl6 * T_tl6_cam * T_cam_opt * T_opt_d
door_points = door_pos_points
base_door_points = []
for i in range(0, door_points.shape[0]):
p = numpy.matrix([[door_points[i, 0]], [door_points[i, 1]], [0.0],
[1.0]])
base_door_points.append(T_base_d * p)
marker = Marker()
marker.header.frame_id = "/tl_base"
marker.type = marker.TRIANGLE_LIST
marker.action = marker.ADD
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
marker.color.a = 1.0
marker.color.g = 1.0
marker.pose.orientation.w = 1.0
p = Point()
p.x = base_door_points[0][0]
p.y = base_door_points[0][1]
p.z = base_door_points[0][2]
p1 = Point()
p1.x = base_door_points[1][0]
p1.y = base_door_points[1][1]
p1.z = base_door_points[1][2]
markerArray = create_delta_simulation()
initializedMarkerArray = set_initial_delta_pose(markerArray)
idcount = 100
pointwidth = 0.005
with open('valid_points_generated.txt', 'rb') as f:
reader = csv.reader(f)
for row in reader:
if idcount < 50000:
if len(row) > 0:
testpoint = Marker()
testpoint.header.frame_id = "/map"
testpoint.type = testpoint.SPHERE
testpoint.action = testpoint.ADD
testpoint.scale.x = pointwidth
testpoint.scale.y = pointwidth
testpoint.scale.z = pointwidth
testpoint.color.a = 1.0
testpoint.color.r = 1.0
testpoint.color.g = 1.0
testpoint.color.b = 1.0
testpoint.pose.orientation.w = 1.0
testpoint.pose.position.x = float(row[0])
testpoint.pose.position.y = float(row[1])
testpoint.pose.position.z = float(row[2])
testpoint.id = idcount
idcount += 1
initializedMarkerArray.markers.append(testpoint)
def get_markers(self):
"""
Return a ROS marker array message structure with an sphere marker to
represent the plume source, the bounding box and an arrow marker to
show the direction of the current velocity if it's norm is greater
than zero.
"""
if self._pnts is None:
return None
marker_array = MarkerArray()
# Generate marker for the source
source_marker = Marker()
source_marker.header.stamp = rospy.Time.now()
source_marker.header.frame_id = 'world'
source_marker.ns = 'plume'
source_marker.id = 0
source_marker.type = Marker.SPHERE
source_marker.action = Marker.ADD
source_marker.color.r = 0.35
source_marker.color.g = 0.45
source_marker.color.b = 0.89
source_marker.color.a = 1.0
source_marker.scale.x = 1.0
source_marker.scale.y = 1.0
source_marker.scale.z = 1.0
source_marker.pose.position.x = self._source_pos[0]
source_marker.pose.position.y = self._source_pos[1]
source_marker.pose.position.z = self._source_pos[2]
source_marker.pose.orientation.x = 0
source_marker.pose.orientation.y = 0
source_marker.pose.orientation.z = 0
source_marker.pose.orientation.w = 1
marker_array.markers.append(source_marker)
# Creating the marker for the bounding box limits where the plume
# particles are generated
limits_marker = Marker()
limits_marker.header.stamp = rospy.Time.now()
limits_marker.header.frame_id = 'world'
limits_marker.ns = 'plume'
limits_marker.id = 1
limits_marker.type = Marker.CUBE
limits_marker.action = Marker.ADD
limits_marker.color.r = 0.1
limits_marker.color.g = 0.1
limits_marker.color.b = 0.1
limits_marker.color.a = 0.3
limits_marker.scale.x = self._x_lim[1] - self._x_lim[0]
limits_marker.scale.y = self._y_lim[1] - self._y_lim[0]
limits_marker.scale.z = self._z_lim[1] - self._z_lim[0]
limits_marker.pose.position.x = self._x_lim[0] + (self._x_lim[1] - self._x_lim[0]) / 2
limits_marker.pose.position.y = self._y_lim[0] + (self._y_lim[1] - self._y_lim[0]) / 2
limits_marker.pose.position.z = self._z_lim[0] + (self._z_lim[1] - self._z_lim[0]) / 2
limits_marker.pose.orientation.x = 0
limits_marker.pose.orientation.y = 0
limits_marker.pose.orientation.z = 0
limits_marker.pose.orientation.w = 0
marker_array.markers.append(limits_marker)
# Creating marker for the current velocity vector
cur_vel_marker = Marker()
cur_vel_marker.header.stamp = rospy.Time.now()
cur_vel_marker.header.frame_id = 'world'
cur_vel_marker.id = 1
cur_vel_marker.type = Marker.ARROW
if np.linalg.norm(self._current_vel) > 0:
cur_vel_marker.action = Marker.ADD
# Calculating the pitch and yaw angles for the current velocity
# vector
yaw = np.arctan2(self._current_vel[1], self._current_vel[0])
pitch = np.arctan2(self._current_vel[2],
np.sqrt(self._current_vel[0]**2 + self._current_vel[1]**2))
qt = quaternion_from_euler(0, -pitch, yaw)
cur_vel_marker.pose.position.x = self._source_pos[0]
cur_vel_marker.pose.position.y = self._source_pos[1]
cur_vel_marker.pose.position.z = self._source_pos[2] - 0.8
cur_vel_marker.pose.orientation.x = qt[0]
cur_vel_marker.pose.orientation.y = qt[1]
cur_vel_marker.pose.orientation.z = qt[2]
cur_vel_marker.pose.orientation.w = qt[3]
cur_vel_marker.scale.x = 1.0
cur_vel_marker.scale.y = 0.1
cur_vel_marker.scale.z = 0.1
cur_vel_marker.color.a = 1.0
cur_vel_marker.color.r = 0.0
cur_vel_marker.color.g = 0.0
cur_vel_marker.color.b = 1.0
else:
cur_vel_marker.action = Marker.DELETE
marker_array.markers.append(cur_vel_marker)
return marker_array
def talker():
rospy.init_node('referee', anonymous=True)
listener = tf.TransformListener()
broadcaster = tf.TransformBroadcaster()
global rate
rate = rospy.Rate(2) # 10hz
rate.sleep()
hunting_distance = rospy.get_param('/hunting_distance')
global teamA
teamA = rospy.get_param('/red')
global teamB
teamB = rospy.get_param('/green')
global teamC
teamC = rospy.get_param('/blue')
rospy.Timer(rospy.Duration(0.1), timerCallback, oneshot=False)
rospy.Timer(rospy.Duration(game_duration), gameEndCallback, oneshot=True)
#rospy.Timer(rospy.Duration(5), gameQueryCallback, oneshot=False)
game_start = rospy.get_time()
while not rospy.is_shutdown():
#print killed
tic = rospy.Time.now()
for i in killed:
d = tic - i[1]
if d.to_sec() > 10:
rospy.logwarn("Ressuscitating %s", i[0])
killed.remove(i)
broadcaster.sendTransform(
(random.random() * 10 - 5, random.random() * 10 - 5, 0),
tf.transformations.quaternion_from_euler(0, 0, 0), tic,
i[0], "/map")
#print killed
#rospy.loginfo("Checking ...")
to_be_killed_A = []
to_be_killed_B = []
to_be_killed_C = []
out_of_arena_A = []
out_of_arena_B = []
out_of_arena_C = []
max_distance_from_center_of_arena = 8
#Checking if players don't stray from the arena
for player in teamA:
#check if hunter is killed
result = [item for item in killed if item[0] == player]
if not result:
try:
(trans,
rot) = listener.lookupTransform("/map", player,
rospy.Time(0))
distance = math.sqrt(trans[0] * trans[0] +
trans[1] * trans[1])
#rospy.loginfo("D from %s to %s is %f", hunter, prey, distance)
if distance > max_distance_from_center_of_arena:
out_of_arena_A.append(player)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
t = 1
#rospy.loginfo("Referee: tf error")
for player in teamB:
#check if hunter is killed
result = [item for item in killed if item[0] == player]
if not result:
try:
(trans,
rot) = listener.lookupTransform("/map", player,
rospy.Time(0))
distance = math.sqrt(trans[0] * trans[0] +
trans[1] * trans[1])
#rospy.loginfo("D from %s to %s is %f", hunter, prey, distance)
if distance > max_distance_from_center_of_arena:
out_of_arena_B.append(player)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
t = 1
#rospy.loginfo("Referee: tf error")
for player in teamC:
#check if hunter is killed
result = [item for item in killed if item[0] == player]
if not result:
try:
(trans,
rot) = listener.lookupTransform("/map", player,
rospy.Time(0))
distance = math.sqrt(trans[0] * trans[0] +
trans[1] * trans[1])
#rospy.loginfo("D from %s to %s is %f", hunter, prey, distance)
if distance > max_distance_from_center_of_arena:
out_of_arena_C.append(player)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
t = 1
#rospy.loginfo("Referee: tf error")
rospy.loginfo("to_be_killed is %s", str(to_be_killed_B))
#Check if anyone is hunted
#Team A hunting team B
for hunter in teamA:
#check if hunter is killed
result = [item for item in killed if item[0] == hunter]
if not result:
for prey in teamB:
try:
(trans, rot) = listener.lookupTransform(
hunter, prey, rospy.Time(0))
distance = math.sqrt(trans[0] * trans[0] +
trans[1] * trans[1])
#rospy.loginfo("D from %s to %s is %f", hunter, prey, distance)
if distance < hunting_distance:
to_be_killed_A.append(prey)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
t = 1
#rospy.loginfo("Referee: tf error")
#Team A hunting team B
for hunter in teamB:
#check if hunter is killed
result = [item for item in killed if item[0] == hunter]
if not result:
for prey in teamC:
try:
(trans, rot) = listener.lookupTransform(
hunter, prey, rospy.Time(0))
distance = math.sqrt(trans[0] * trans[0] +
trans[1] * trans[1])
#rospy.loginfo("D from %s to %s is %f", hunter, prey, distance)
if distance < hunting_distance:
to_be_killed_B.append(prey)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
t = 1
#rospy.loginfo("Referee: tf error")
#Team C hunting team A
for hunter in teamC:
#check if hunter is killed
result = [item for item in killed if item[0] == hunter]
if not result:
for prey in teamA:
try:
(trans, rot) = listener.lookupTransform(
hunter, prey, rospy.Time(0))
distance = math.sqrt(trans[0] * trans[0] +
trans[1] * trans[1])
#rospy.loginfo("D from %s to %s is %f", hunter, prey, distance)
if distance < hunting_distance:
to_be_killed_C.append(prey)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
t = 1
#rospy.loginfo("Referee: tf error")
ma_killed = MarkerArray()
kill_time = rospy.Time.now()
for tbk in to_be_killed_A:
#rospy.logwarn("%s is to be killed by Team A", tbk)
#Check if tbk is in the list of killed players
found = False
for item in killed:
if item[0] == tbk:
found = True
if found == False:
killed.append((tbk, kill_time))
rospy.logwarn("Red hunted %s", tbk)
score[0] = score[0] + positive_score
score[1] = score[1] + negative_score
s = String()
s.data = tbk
#pub_killer.publish(s)
mk1 = Marker()
mk1.header.frame_id = "/map"
(trans,
rot) = listener.lookupTransform("/map", tbk, rospy.Time(0))
mk1.pose.position.x = trans[0]
mk1.pose.position.y = trans[1]
mk1.type = mk1.TEXT_VIEW_FACING
mk1.action = mk1.ADD
mk1.id = 0
mk1.ns = tbk
mk1.scale.z = 0.4
mk1.color.a = 1.0
mk1.text = tbk
mk1.lifetime = rospy.Duration.from_sec(5)
mk1.frame_locked = 0
ma_killed.markers.append(mk1)
broadcaster.sendTransform(
(-100, -100, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
kill_time, tbk, "/map")
else:
t = 1
#rospy.logwarn("%s was already hunted", tbk)
for tbk in to_be_killed_B:
#rospy.logwarn("%s is to be killed by Team B", tbk)
#Check if tbk is in the list of killed players
found = False
for item in killed:
if item[0] == tbk:
found = True
if found == False:
killed.append((tbk, kill_time))
rospy.logwarn("Green hunted %s", tbk)
score[1] = score[1] + positive_score
score[2] = score[2] + negative_score
s = String()
s.data = tbk
#pub_killer.publish(s)
mk1 = Marker()
mk1.header.frame_id = "/map"
(trans,
rot) = listener.lookupTransform("/map", tbk, rospy.Time(0))
mk1.pose.position.x = trans[0]
mk1.pose.position.y = trans[1]
mk1.type = mk1.TEXT_VIEW_FACING
mk1.action = mk1.ADD
mk1.id = 0
mk1.ns = tbk
mk1.scale.z = 0.4
mk1.color.a = 1.0
mk1.text = tbk
mk1.lifetime = rospy.Duration.from_sec(5)
mk1.frame_locked = 0
ma_killed.markers.append(mk1)
broadcaster.sendTransform(
(100, -100, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
kill_time, tbk, "/map")
else:
t = 1
#rospy.logwarn("%s was already hunted", tbk)
for tbk in to_be_killed_C:
#rospy.logwarn("%s is to be killed by Team C", tbk)
#Check if tbk is in the list of killed players
found = False
for item in killed:
if item[0] == tbk:
found = True
if found == False:
killed.append((tbk, kill_time))
rospy.logwarn("Blue hunted %s", tbk)
score[2] = score[2] + positive_score
score[0] = score[0] + negative_score
s = String()
s.data = tbk
#pub_killer.publish(s)
mk1 = Marker()
mk1.header.frame_id = "/map"
(trans,
rot) = listener.lookupTransform("/map", tbk, rospy.Time(0))
mk1.pose.position.x = trans[0]
mk1.pose.position.y = trans[1]
mk1.type = mk1.TEXT_VIEW_FACING
mk1.action = mk1.ADD
mk1.id = 0
mk1.ns = tbk
mk1.scale.z = 0.4
mk1.color.a = 1.0
mk1.text = tbk
mk1.lifetime = rospy.Duration.from_sec(5)
mk1.frame_locked = 0
ma_killed.markers.append(mk1)
broadcaster.sendTransform(
(100, 100, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
kill_time, tbk, "/map")
else:
t = 1
#rospy.logwarn("%s was already hunted", tbk)
#--------------------------------
#Killing because of stray from arena
#--------------------------------
for tbk in out_of_arena_A:
#rospy.logwarn("%s is to be killed by Team A", tbk)
#Check if tbk is in the list of killed players
found = False
for item in killed:
if item[0] == tbk:
found = True
if found == False:
killed.append((tbk, kill_time))
rospy.logwarn("Red hunted %s", tbk)
score[0] = score[0] + negative_score
s = String()
s.data = tbk
#pub_killer.publish(s)
mk1 = Marker()
mk1.header.frame_id = "/map"
(trans,
rot) = listener.lookupTransform("/map", tbk, rospy.Time(0))
mk1.pose.position.x = trans[0]
mk1.pose.position.y = trans[1]
mk1.type = mk1.TEXT_VIEW_FACING
mk1.action = mk1.ADD
mk1.id = 0
mk1.ns = tbk
mk1.scale.z = 0.4
mk1.color.a = 1.0
mk1.text = "Queres fugir " + tbk + "?"
mk1.lifetime = rospy.Duration.from_sec(5)
mk1.frame_locked = 0
ma_killed.markers.append(mk1)
broadcaster.sendTransform(
(-100, -100, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
kill_time, tbk, "/map")
else:
t = 1
#rospy.logwarn("%s was already hunted", tbk)
for tbk in out_of_arena_B:
#rospy.logwarn("%s is to be killed by Team B", tbk)
#Check if tbk is in the list of killed players
found = False
for item in killed:
if item[0] == tbk:
found = True
if found == False:
killed.append((tbk, kill_time))
rospy.logwarn("Green hunted %s", tbk)
score[1] = score[1] + negative_score
s = String()
s.data = tbk
#pub_killer.publish(s)
mk1 = Marker()
mk1.header.frame_id = "/map"
(trans,
rot) = listener.lookupTransform("/map", tbk, rospy.Time(0))
mk1.pose.position.x = trans[0]
mk1.pose.position.y = trans[1]
mk1.type = mk1.TEXT_VIEW_FACING
mk1.action = mk1.ADD
mk1.id = 0
mk1.ns = tbk
mk1.scale.z = 0.4
mk1.color.a = 1.0
mk1.text = "Queres fugir " + tbk + "?"
mk1.lifetime = rospy.Duration.from_sec(5)
mk1.frame_locked = 0
ma_killed.markers.append(mk1)
broadcaster.sendTransform(
(100, -100, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
kill_time, tbk, "/map")
else:
t = 1
#rospy.logwarn("%s was already hunted", tbk)
for tbk in out_of_arena_C:
#rospy.logwarn("%s is to be killed by Team C", tbk)
#Check if tbk is in the list of killed players
found = False
for item in killed:
if item[0] == tbk:
found = True
if found == False:
killed.append((tbk, kill_time))
rospy.logwarn("Blue hunted %s", tbk)
score[2] = score[2] + negative_score
s = String()
s.data = tbk
#pub_killer.publish(s)
mk1 = Marker()
mk1.header.frame_id = "/map"
(trans,
rot) = listener.lookupTransform("/map", tbk, rospy.Time(0))
mk1.pose.position.x = trans[0]
mk1.pose.position.y = trans[1]
mk1.type = mk1.TEXT_VIEW_FACING
mk1.action = mk1.ADD
mk1.id = 0
mk1.ns = tbk
mk1.scale.z = 0.4
mk1.color.a = 1.0
mk1.text = "Queres fugir " + tbk + "?"
mk1.lifetime = rospy.Duration.from_sec(5)
mk1.frame_locked = 0
ma_killed.markers.append(mk1)
broadcaster.sendTransform(
(100, 100, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
kill_time, tbk, "/map")
else:
t = 1
#rospy.logwarn("%s was already hunted", tbk)
if ma_killed.markers:
pub_rip.publish(ma_killed)
#print killed
#rospy.logwarn("Team A: %s hunted %s", hunter, prey)
##os.system('rosnode kill ' + prey)
#score[0] = score[0] + positive_score
#score[1] = score[1] + negative_score
#s = String()
#s.data = prey
#pub_killer.publish(s)
ma = MarkerArray()
m1 = Marker()
m1.header.frame_id = "/map"
m1.type = m1.TEXT_VIEW_FACING
m1.action = m1.ADD
m1.id = 0
m1.scale.x = 0.2
m1.scale.y = 0.2
m1.scale.z = 0.6
m1.color.a = 1.0
m1.color.r = 1.0
m1.color.g = 0.0
m1.color.b = 0.0
m1.text = "R=" + str(score[0])
m1.pose.position.x = -5.0
m1.pose.position.y = 5.2
m1.pose.position.z = 0
ma.markers.append(m1)
m2 = Marker()
m2.header.frame_id = "/map"
m2.type = m2.TEXT_VIEW_FACING
m2.action = m2.ADD
m2.id = 1
m2.scale.x = 0.2
m2.scale.y = 0.2
m2.scale.z = 0.6
m2.color.a = 1.0
m2.color.r = 0.0
m2.color.g = 1.0
m2.color.b = 0.0
m2.text = "G=" + str(score[1])
m2.pose.position.x = -3.0
m2.pose.position.y = 5.2
m2.pose.position.z = 0
ma.markers.append(m2)
m3 = Marker()
m3.header.frame_id = "/map"
m3.type = m3.TEXT_VIEW_FACING
m3.action = m3.ADD
m3.id = 2
m3.scale.x = 0.2
m3.scale.y = 0.2
m3.scale.z = 0.6
m3.color.a = 1.0
m3.color.r = 0.0
m3.color.g = 0.0
m3.color.b = 1.0
m3.text = "B=" + str(score[2])
m3.pose.position.x = -1.0
m3.pose.position.y = 5.2
m3.pose.position.z = 0
ma.markers.append(m3)
m4 = Marker()
m4.header.frame_id = "/map"
m4.type = m4.TEXT_VIEW_FACING
m4.action = m4.ADD
m4.id = 3
m4.scale.x = 0.2
m4.scale.y = 0.2
m4.scale.z = 0.6
m4.color.a = 1.0
m4.color.r = 0.0
m4.color.g = 0.0
m4.color.b = 1.0
#,c rospy.Time(
time_now = rospy.get_time()
m4.text = "Time " + str(format(time_now - game_start,
'.2f')) + " of " + str(game_duration)
m4.pose.position.x = 3.5
m4.pose.position.y = 5.2
m4.pose.position.z = 0
ma.markers.append(m4)
pub_score.publish(ma)
rate.sleep()
def publishState(self):
msg = Odometry()
if self.time is None:
return
# Construct header
msg.header.seq = self.seq # sequence ID
self.seq += 1 # increment sequence ID for next frame
msg.header.stamp = self.time # time stamp
msg.header.frame_id = 'odom' # reference frame
msg.child_frame_id = 'base_link'
# Construct content
current_pose = np.array(
self.KF.getStateMean()[0:dimR_]) # latest (x, y, yaw)
rospy.loginfo("Current pose {}".format(current_pose))
rospy.loginfo("Current pose z {}".format(current_pose[2]))
msg.pose.pose.position.x = current_pose[0]
msg.pose.pose.position.y = current_pose[1]
msg.pose.pose.position.z = 0.0
# quat = transformations.quaternion_from_euler(0.0, 0.0, current_pose[2])
r = R.from_euler('xyz', [0, 0, current_pose[2]])
quat = r.as_quat()
msg.pose.pose.orientation.x = quat[0]
msg.pose.pose.orientation.y = quat[1]
msg.pose.pose.orientation.z = quat[2]
msg.pose.pose.orientation.w = quat[3]
current_covariance = np.array(
self.KF.getStateCovariance()[0:dimR_,
0:dimR_]) # latest 3x3 covariance
cov = np.zeros((36, 1), dtype=np.float)
cov[0] = current_covariance[0, 0]
cov[1] = current_covariance[0, 1]
cov[5] = current_covariance[0, 2]
cov[6] = current_covariance[1, 0]
cov[7] = current_covariance[1, 1]
cov[11] = current_covariance[1, 2]
cov[30] = current_covariance[2, 0]
cov[31] = current_covariance[2, 1]
cov[35] = current_covariance[2, 2]
msg.pose.covariance = np.ravel(cov)
self.pub_odom.publish(msg)
rospy.loginfo("State mean {}".format(self.KF.getStateMean()))
rospy.loginfo("seen landmarks {}".format(seenLandmarks_))
# state_mean = np.array()
Landmark_poses = self.KF.getStateMean()[dimR_:]
print('landmark poses ', Landmark_poses)
marker_array_msg = MarkerArray()
for i in range(int(len(Landmark_poses) / 2)):
marker = Marker()
marker.header.frame_id = 'odom'
marker.id = seenLandmarks_[i]
marker.type = 3
marker.action = 0
marker.pose.position.x = Landmark_poses[2 * i]
marker.pose.position.y = Landmark_poses[2 * i + 1]
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.frame_locked = False
marker.lifetime = rospy.Duration(0.1)
marker_array_msg.markers.append(marker)
self.pub_landmarks.publish(marker_array_msg)
def publishMarkers(self, objects, pruned_objects, bin_pose, camera_to_bin,
bin_dimensions, header):
# construct centroids marker array
marker_array = MarkerArray()
# clear old markers
marker = Marker()
marker.action = 3 # secret action to delete all markers
marker_array.markers.append(marker)
self.marker_publisher.publish(marker_array)
marker_array.markers = []
# add centroid markers
for index, object in enumerate(objects):
marker = Marker()
marker.header = header
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.02
marker.scale.y = 0.02
marker.scale.z = 0.02
marker.color.g = 1.0
marker.color.a = 0.5
marker.pose.position = object.centroid
marker.ns = apc16delft_msgs.objects.objectTypeToString[
object.object.type]
marker.id = len(marker_array.markers)
marker_array.markers.append(marker)
# add pruned centroid markers
for index, object in enumerate(pruned_objects):
marker = Marker()
marker.header = header
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.02
marker.scale.y = 0.02
marker.scale.z = 0.02
marker.color.r = 1.0
marker.color.a = 0.5
marker.pose.position = object.centroid
marker.ns = apc16delft_msgs.objects.objectTypeToString[
object.object.type]
marker.id = len(marker_array.markers)
marker_array.markers.append(marker)
# add bin marker
marker = Marker()
marker.header = header
marker.type = marker.CUBE
marker.action = marker.ADD
marker.scale.x = bin_dimensions[0] + self.bin_margin[0] * 2
marker.scale.y = bin_dimensions[1] + self.bin_margin[1] * 2
marker.scale.z = bin_dimensions[2] + self.bin_margin[2] * 2
marker.color.b = 1.0
marker.color.a = 0.2
marker.ns = "bin_marker"
marker.id = len(marker_array.markers)
# transform so that the marker is originating from the bin_pose
position = np.append(np.array(bin_dimensions) * 0.5, 1)
position = transformations.inverse_matrix(camera_to_bin).dot(position)
marker.pose.position.x = position[0]
marker.pose.position.y = position[1]
marker.pose.position.z = position[2]
marker.pose.orientation = bin_pose.orientation
marker_array.markers.append(marker)
self.marker_publisher.publish(marker_array)
def callback(self, cluster):
human_cluster = np.zeros((len(cluster.candidates), 3))
human_cluster = map(
lambda info: np.array(
[info.centroid.x, info.centroid.y, info.centroid.z]),
cluster.info)
human_prob = map(lambda prob: prob.all, cluster.prob)
thresh = 0.4
if len(cluster.candidates) > 0:
try:
clusters = hcluster.fclusterdata(human_cluster,
thresh,
criterion="distance")
except ValueError:
self.pos.x = 0.0
self.pos.y = 0.0
self.pos.z = 0.0
self.pos_pub.publish(self.pos)
cluster_point = np.asarray(
[0.0 for i in range(len(cluster.candidates))])
candidate_cluster = []
for i in range(len(clusters)):
if human_prob[i] > 0.85:
cluster_point[clusters[i] - 1] += human_prob[i]
candidate_cluster.append(clusters[i])
centroids = [
np.zeros((1, 3)) for i in range(len(candidate_cluster))
]
nums = [0 for i in range(len(candidate_cluster))]
for i in range(len(candidate_cluster)):
for j in range(len(clusters)):
if clusters[j] == candidate_cluster[i]:
centroids[i] += human_cluster[j]
nums[i] += 1
if len(nums) > 0:
rospy.loginfo("human candidates : %d", len(nums))
centroids = [
centroids[i] / nums[i]
for i in range(len(candidate_cluster))
]
markerArray = MarkerArray()
people = Person()
for i in range(len(centroids)):
marker = Marker()
marker.header.frame_id = "realsense_frame"
marker.action = marker.ADD
marker.type = marker.MESH_RESOURCE
marker.mesh_resource = "package://object_recognizer/meshes/lovelive/" + self.name[
i] + ".dae"
marker.mesh_use_embedded_materials = True
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.scale.x = 0.08
marker.scale.y = 0.08
marker.scale.z = 0.08
marker.pose.position.x = centroids[i][0][0]
marker.pose.position.y = centroids[i][0][1] + 0.8
marker.pose.position.z = centroids[i][0][2]
q = tf.transformations.quaternion_from_euler(math.pi, 0, 0)
marker.pose.orientation.x = q[0]
marker.pose.orientation.y = q[1]
marker.pose.orientation.z = q[2]
marker.pose.orientation.w = q[3]
markerArray.markers.append(marker)
centroid = Vector3()
centroid.x = centroids[i][0][0]
centroid.y = centroids[i][0][1]
centroid.z = centroids[i][0][2]
people.centroids.append(centroid)
for i in range(len(markerArray.markers)):
markerArray.markers[i].id = i
markerArray.markers[i].lifetime = rospy.Duration(1.0)
#self.marker_pub.publish(markerArray)
self.pos_pub.publish(people)
else:
rospy.loginfo("no cluster found")
people = Person()
centroid = Vector3()
centroid.x = 0.0
centroid.y = 0.0
centroid.z = 0.0
people.centroids.append(centroid)
self.pos_pub.publish(people)
else:
people = Person()
centroid = Vector3()
centroid.x = 0.0
centroid.y = 0.0
centroid.z = 0.0
people.centroids.append(centroid)
self.pos_pub.publish(people)
def pubTF(self, timer):
br = tf.TransformBroadcaster()
marker_tmp = Marker()
marker_tmp.header.frame_id = "world"
marker_tmp.type = marker_tmp.CUBE_LIST
marker_tmp.action = marker_tmp.ADD
marker_static = copy.deepcopy(marker_tmp)
marker_dynamic = copy.deepcopy(marker_tmp)
marker_dynamic.color = color_dynamic
# marker_dynamic.scale.x=self.world.bbox_dynamic[0]
# marker_dynamic.scale.y=self.world.bbox_dynamic[1]
# marker_dynamic.scale.z=self.world.bbox_dynamic[2]
marker_static.color = color_static
###################3
marker_array_static_mesh = MarkerArray()
marker_array_dynamic_mesh = MarkerArray()
for i in range(self.world.num_of_dyn_objects +
self.world.num_of_stat_objects):
t_ros = rospy.Time.now()
t = rospy.get_time()
#Same as before, but it's float
dynamic_trajectory_msg = DynTraj()
bbox_i = self.bboxes[i]
s = self.world.scale
if (self.type[i] == "dynamic"):
[x_string, y_string,
z_string] = self.trefoil(self.x_all[i], self.y_all[i],
self.z_all[i], s, s, s,
self.offset_all[i], self.slower[i])
# print("self.bboxes[i]= ", self.bboxes[i])
dynamic_trajectory_msg.bbox = bbox_i
marker_dynamic.scale.x = bbox_i[0]
marker_dynamic.scale.y = bbox_i[1]
marker_dynamic.scale.z = bbox_i[2]
else:
# [x_string, y_string, z_string] = self.static(self.x_all[i], self.y_all[i], self.z_all[i]);
dynamic_trajectory_msg.bbox = bbox_i
marker_static.scale.x = bbox_i[0]
marker_static.scale.y = bbox_i[1]
marker_static.scale.z = bbox_i[2]
if (self.type[i] == "static_vert"):
[x_string, y_string,
z_string] = self.wave_in_z(self.x_all[i], self.y_all[i],
self.z_all[i], s,
self.offset_all[i], 1.0)
else:
[x_string, y_string,
z_string] = self.wave_in_z(self.x_all[i], self.y_all[i],
self.z_all[i], s,
self.offset_all[i], 1.0)
x = eval(x_string)
y = eval(y_string)
z = eval(z_string)
dynamic_trajectory_msg.is_agent = False
dynamic_trajectory_msg.header.stamp = t_ros
dynamic_trajectory_msg.function = [x_string, y_string, z_string]
dynamic_trajectory_msg.pos.x = x #Current position
dynamic_trajectory_msg.pos.y = y #Current position
dynamic_trajectory_msg.pos.z = z #Current position
dynamic_trajectory_msg.id = 4000 + i #Current id 4000 to avoid interference with ids from agents #TODO
self.pubTraj.publish(dynamic_trajectory_msg)
br.sendTransform((x, y, z), (0, 0, 0, 1), t_ros,
self.name + str(dynamic_trajectory_msg.id),
"world")
#If you want to move the objets in gazebo
# gazebo_state = ModelState()
# gazebo_state.model_name = str(i)
# gazebo_state.pose.position.x = x
# gazebo_state.pose.position.y = y
# gazebo_state.pose.position.z = z
# gazebo_state.reference_frame = "world"
# self.pubGazeboState.publish(gazebo_state)
#If you want to see the objects in rviz
point = Point()
point.x = x
point.y = y
point.z = z
##########################
marker = Marker()
marker.id = i
marker.ns = "mesh"
marker.header.frame_id = "world"
marker.type = marker.MESH_RESOURCE
marker.action = marker.ADD
marker.pose.position.x = x
marker.pose.position.y = y
marker.pose.position.z = z
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.lifetime = rospy.Duration.from_sec(0.0)
marker.mesh_use_embedded_materials = True
marker.mesh_resource = self.meshes[i]
if (self.type[i] == "dynamic"):
marker_dynamic.points.append(point)
marker.scale.x = bbox_i[0]
marker.scale.y = bbox_i[1]
marker.scale.z = bbox_i[2]
marker_array_dynamic_mesh.markers.append(marker)
if (self.type[i] == "static_vert"
or self.type[i] == "static_horiz"):
marker.scale.x = bbox_i[0]
marker.scale.y = bbox_i[1]
marker.scale.z = bbox_i[2]
marker_array_static_mesh.markers.append(marker)
##########################
marker_static.points.append(point)
self.pubShapes_dynamic_mesh.publish(marker_array_dynamic_mesh)
self.pubShapes_dynamic.publish(marker_dynamic)
# if(self.already_published_static_shapes==False):
self.pubShapes_static_mesh.publish(marker_array_static_mesh)
self.pubShapes_static.publish(marker_static)
