def publish_markers(frontiers, centroids, unique_labels, grid_info):
global last_marker_ids
delete_old_markers()
# used for creating equidistant HSV colors
deg_interval = 1.0 / len(unique_labels)
#rospy.loginfo("deg_interval {}".format(deg_interval))
marker_array = MarkerArray()
hsv_colors = [(deg_interval * i, 1, 1) for i in range(len(unique_labels))]
#rospy.loginfo("colors {}".format(hsv_colors))
# build markers for frontier points
# marker_array.markers = [make_marker(i, grid_to_world(positions[i], grid_info), hsv_colors[unique_labels.index(labels[i])], 0.05, 0) for i in range(len(labels))]
marker_array.markers = []
for i, frontier in enumerate(frontiers):
marker_array.markers += [make_marker(random.randint(0, 2147000000), grid_to_world(position, grid_info), hsv_colors[i], 0.05, 0) for position in list(frontier)]
# append centroid markers to original markers
marker_array.markers = marker_array.markers + [make_marker(random.randint(0, 2147000000), centroid, hsv_colors[i], 0.25, 0) for i, centroid in enumerate(centroids)]
# extract marker ids for reference
#marker_ids = [marker.id for marker in marker_array.markers]
#last_marker_ids = marker_ids
# publish markers
marker_publisher.publish(marker_array)
rospy.loginfo("published {} markers".format(len(marker_array.markers)))
def run(self):
while not rospy.is_shutdown():
rospy.loginfo("$gt: %d" % (rospy.Time.now().secs - self.duration * 60 * 60 * 24))
if self.use_ros_time or self.use_sim_time:
rospy.loginfo("fetching data")
trans = self.msg_store.query(type=TransformStamped._type,
message_query={"header.stamp.secs": {
"$lte": rospy.Time.now().secs,
"$gt": rospy.Time.now().secs - self.duration * 60 * 60 * 24
}},
sort_query=[("$natural", 1)],
limit=self.limit)
else:
trans = self.msg_store.query(type=TransformStamped._type,
meta_query={"inserted_at": {
"$gt": datetime.utcnow() - timedelta(days=self.duration)
}},
sort_query=[("$natural", 1)])
rospy.loginfo("found %d msgs" % len(trans))
m_arr = MarkerArray()
m_arr.markers = V.transformStampedArrayToLabeledLineStripMarker(trans[::self.downsample], label_downsample=self.label_downsample, discrete=True)
m_arr.markers = V.transformStampedArrayToLabeledArrayMarker(trans[::self.downsample], label_downsample=self.label_downsample, discrete=True)
self.pub.publish(m_arr)
rospy.sleep(1.0)
rospy.loginfo("publishing move_base_marker_array")
def update_drones(self):
"""
Updates drone markers and past path on rviz.
"""
# Updating drone poses
for drone in self.__drones.values():
marker = self.__drone_markers[drone.id]
marker.action = Marker.MODIFY
marker.pose = drone.pose
m = MarkerArray()
m.markers = list(self.__drone_markers.values())
self.__drone_publisher.publish(m)
# Updating past path
for drone in self.__drones.values():
marker = self.__past_path_markers[drone.id]
marker.action = Marker.MODIFY
marker.id += 1
if len(marker.points) == 2:
marker.points[0] = marker.points[1]
marker.points[1] = Point(drone.pose.position.x,
drone.pose.position.y,
drone.pose.position.z)
else:
marker.points.append(
Point(drone.pose.position.x, drone.pose.position.y,
drone.pose.position.z))
m.markers = list(self.__past_path_markers.values())
self.__past_path_publisher.publish(m)
def people_locations_cb(self, data):
## Handles an array of personlocation2d
## creates a marker for each and publishes an
## array of the markers
## holds the objects representing people
marker_array = MarkerArray()
marker_array.markers = []
## contains labels for people
marker_label_array = MarkerArray()
marker_label_array.markers = []
for person in data.people_location:
## in future have a heat map color marker
# color = self.contamination_color(person.contamination)
color = ColorRGBA(0.2, 0.5, 0.7, 1.0)
marker = self.create_cylindrical_person_marker(person, color)
marker_label = self.create_person_label_marker(person, color)
marker_array.markers.append(marker)
marker_label_array.markers.append(marker_label)
self.people_marker_pub.publish(marker_array)
self.people_label_marker_pub.publish(marker_label_array)
def publish_lines_waypts(self, waypoints): markers_array_msg = MarkerArray() markers_array = [] marker = Marker() marker.ns = "waypoints_line" marker.id = 1 marker.header.frame_id = "odom" marker.type = marker.LINE_STRIP marker.action = marker.ADD marker.scale.x = 0.06 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.color.a = 0.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 for i,waypoint in enumerate(waypoints): marker.points.append(Point(waypoint[0], waypoint[1], 0)) marker.lifetime = rospy.Duration(0) markers_array.append(marker) markers_array_msg.markers = markers_array self.pub_lines_waypts.publish(markers_array_msg)
def publish_marker_lookahead_circle(self, robot_pose, lookahead): markers_array_msg = MarkerArray() markers_array = [] marker = Marker() marker.ns = "lookahead_circle" marker.id = 1 marker.header.frame_id = "odom" marker.type = marker.CYLINDER marker.action = marker.ADD marker.scale.x = lookahead*2 marker.scale.y = lookahead*2 marker.scale.z = 0.05 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.color.a = 0.4 marker.pose = robot_pose marker.lifetime = rospy.Duration(0) markers_array.append(marker) markers_array_msg.markers = markers_array self.pub_marker_lookahead_circle.publish(markers_array_msg)
def publish_marker_robot_pose(self, robot_pose): markers_array_msg = MarkerArray() markers_array = [] marker = Marker() marker.ns = "robot_pose" marker.id = 1 marker.header.frame_id = "odom" marker.type = marker.ARROW marker.action = marker.ADD marker.scale.x = 1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.color.a = 1 marker.pose = robot_pose marker.lifetime = rospy.Duration(0) markers_array.append(marker) markers_array_msg.markers = markers_array self.pub_marker_robot_pose.publish(markers_array_msg)
def _makeAndSendCutRegions(self):
_regionMarkers = MarkerArray()
_markerCutPlaneMinZ = self._makePlaneMarker(
(0, 0, PCloudFilterParams.PASSTHROUGH_LIMITS_Z[0]),
color=(1, 0, 0))
_markerCutPlaneMaxZ = self._makePlaneMarker(
(0, 0, PCloudFilterParams.PASSTHROUGH_LIMITS_Z[1]),
color=(0, 1, 0))
_markerCutPlaneMinX = self._makePlaneMarker(
(0, PCloudFilterParams.PASSTHROUGH_LIMITS_Y[0], 1),
color=(1, 1, 0),
axis='y')
_markerCutPlaneMaxX = self._makePlaneMarker(
(0, PCloudFilterParams.PASSTHROUGH_LIMITS_Y[1], 1),
color=(0, 1, 1),
axis='y')
_regionMarkers.markers = [
_markerCutPlaneMinZ, _markerCutPlaneMaxZ, _markerCutPlaneMinX,
_markerCutPlaneMaxX
]
_id = 0
for m in _regionMarkers.markers:
m.id = _id
_id += 1
self.m_pubCutRegions.publish(_regionMarkers)
def update_goal(self, setpoint_msg):
array_msg = MarkerArray()
array_msg.markers = []
marker_msg = Marker()
marker_msg.header = setpoint_msg.header
marker_msg.ns = "guidance/current"
marker_msg.id = 0
marker_msg.type = 2
marker_msg.action = 0
marker_msg.pose = setpoint_msg.pose
marker_msg.scale = Vector3(2 * self.distance_error_acceptance,
2 * self.distance_error_acceptance,
2 * self.distance_error_acceptance)
marker_msg.color = ColorRGBA(0.77432878, 0.31884126, 0.54658502,
1.0) #HOT PINK
array_msg.markers.append(marker_msg)
marker_msg = Marker()
marker_msg.header = setpoint_msg.header
marker_msg.ns = "guidance/current"
marker_msg.id = 1
marker_msg.type = 0
marker_msg.action = 0
marker_msg.pose = setpoint_msg.pose
marker_msg.scale = Vector3(20, 2, 2)
marker_msg.color = ColorRGBA(0.77432878, 0.31884126, 0.54658502,
1.0) #HOT PINK
array_msg.markers.append(marker_msg)
self.marker_pub.publish(array_msg)
def output_rviz_message(self):
markers = MarkerArray()
spheres = Marker()
lines = Marker()
spheres.id = 1
spheres.header.frame_id = self.frame_id
spheres.type = Marker.SPHERE_LIST
spheres.action = 0
spheres.scale = Vector3(0.005, 0.005, 0.005)
spheres.color.b = 1.0
spheres.color.a = 1.0
for node in self.graph.nodes:
spheres.points.append(Point(*node))
lines.id = 2
lines.header.frame_id = self.frame_id
lines.type = Marker.LINE_LIST
lines.action = 0
lines.scale.x = 0.0025
lines.color.g = 1.0
lines.color.a = 1.0
for node_1, node_2 in self.graph.edges:
lines.points.extend([Point(*node_1), Point(*node_2)])
markers.markers = [spheres, lines]
return markers
def publish_marker_goal(self, pg):
markers_array_msg = MarkerArray()
markers_array = []
marker = Marker()
marker.ns = "goal_pt"
marker.id = 1
marker.header.frame_id = "odom"
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.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.pose.position.x = pg[0]
marker.pose.position.y = pg[1]
marker.pose.position.z = 0
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(0)
markers_array.append(marker)
markers_array_msg.markers = markers_array
self.pub_marker_goal.publish(markers_array_msg)
def dark_callback(self, data):
if self.pointcloud is None:
return
marker_array = MarkerArray()
marker_list = []
index = 0
for dark_box in data.dark_array:
u = int(dark_box.xmin + ((dark_box.xmax - dark_box.xmin) / 2.0))
v = int(dark_box.ymin + ((dark_box.ymax - dark_box.ymin) / 2.0))
xyz_point_from_sensor, nan_flg = self.pixelTo3DPoint(
u, v, self.pointcloud)
if nan_flg is True:
return
xyz_point_from_world = self.transform_3D_point_to_world_frame(
xyz_point_from_sensor, self.pointcloud.header.frame_id)
marker_data = self.generate_marker(xyz_point_from_world, index)
marker_list.append(marker_data)
index += 1
marker_array.markers = marker_list
self.marker_pub.publish(marker_array)
def visualize_state(self):
# List of active objects, used to remove old ones
active_objs = set()
#Go through locations
for obj_type, objs in self.objects.iteritems():
# Skip waypoints
if obj_type in ['airwaypoint']:
continue
for obj in objs:
active_objs.add(obj)
#Add object if it does not exist
if obj not in self.vis_markers:
self._create_visualization_marker(obj, obj_type)
#Update object
self._update_visualization_marker(obj, obj_type)
#Check for old objects, need to notify rviz to remove them
old_objects = set(self.vis_markers).difference(active_objs)
for old_obj in old_objects:
self._remove_visualization_marker(old_obj)
#Create array message
marray = MarkerArray()
marray.markers = []
for obj, markers in self.vis_markers.iteritems():
marray.markers += markers
# Publish!
self.marker_pub.publish(marray)
# Remove old objects from dict
for old_obj in old_objects:
del self.vis_markers[old_obj]
def nodePublish_worker(self, points):
pub = rospy.Publisher('Marker', MarkerArray, queue_size=1000)
# rospy.init_node('pose_publisher', anonymous=True)
rate = rospy.Rate(2) # Hz
poseArray = PoseArray()
poseArray.header.frame_id = 'base'
markerArray = MarkerArray()
# this is Array of arrow
# p1 = create_arrow([0.6155, -0.4195, -0.2159], [0, 0, 0, 1])
# p2 = create_arrow([0.6155, -0.4195, 0.2159], [0, 0, 0, 1])
# poseArray.poses = [p1, p2]
markerArray.markers = []
print 'start adding markers'
id = 0
for point in points:
# marker = self.create_marker(point.pos)
markerArray.markers.append(self.create_marker(point.pos))
print id
id += 1
id = 0
for m in markerArray.markers:
m.id = id
id += 1
pub.publish(markerArray)
rate.sleep()
def processtag_callback(self, rgb_data, depth_data, tag_data):
try:
self.rgb_image = self.bridge.imgmsg_to_cv2(rgb_data, "bgr8")
self.depth_image = self.bridge.imgmsg_to_cv2(depth_data, "16UC1")
except CvBridgeError as e:
print(e)
all_detections = tag_data.detections
detections_transformed = []
marker_transformed = []
if (self.rgb_image != None and self.depth_image != None):
for current_detection in all_detections:
current_fused, current_marker = fuser.fuse_transform(
current_detection, self.rgb_image, self.depth_image,
self.camera_intrinsics, tag_data.header.frame_id)
current_marker.ns = 'tag' + str(current_detection.id)
current_marker.id = current_detection.id
detections_transformed.append(current_fused)
marker_transformed.append(current_marker)
detection_msg = AprilTagDetections()
detection_msg.detections = detections_transformed
marker_msg = MarkerArray()
marker_msg.markers = marker_transformed
self.detection_publish.publish(detection_msg)
self.marker_publish.publish(marker_msg)
def show_text_in_rviz_mullti_cube(marker_publisher, text):
markers_my = MarkerArray()
markers_my.markers = []
for i in range(5):
print(time.time())
###################################################333333
marker = Marker(
type=Marker.CUBE,
lifetime=rospy.Duration(5),
pose=Pose(Point(0.5 + (i * 20), 0.5 + (i * 20), 1.45), Quaternion(i, i, i, 1)),
scale=Vector3(0.6, 0.6, 0.6),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1, 0.0, .2))
marker.action = Marker.ADD
marker.ns = "est_pose_" + str(i)
marker.id = 42 + i
marker.header.stamp = marker.header.stamp
marker.pose.orientation.w = 1.0
marker.pose.position.x = (0.5 + (i * 2))
marker.pose.position.y = (0.5 + (i * 2))
marker.pose.position.z = 1.45
print(i)
markers_my.markers.append(marker)
# rospy.sleep(1)
marker_publisher.publish(markers_my)
def makeViz(self, reachSets):
# if self.skipfreq == self.skipcount:
# self.skipcount = 0
# self.skipcount += 1
pointSets = [[tuple(p.p) for p in rs.set] for rs in reachSets.sets]
#triangleSets = [tripy.earclip(ps) for ps in pointSets]
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = reachSets.header.frame_id
origin = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1))
lineMarkers = MarkerArray()
lineMarkerArray = []
for ii in range(len(pointSets)):
m = Marker()
m.header = header
m.id = self.outline_marker_id + ii
m.action = 0
m.pose = origin
m.type = 4 #LINE_STRIP
m.color = self.colors[ii % len(self.colors)]
m.points = [Point(p[0], p[1], 0) for p in pointSets[ii]]
m.scale = Vector3(.05, 0, 0)
lineMarkerArray.append(m)
lineMarkers.markers = lineMarkerArray
self.outlinePub.publish(lineMarkers)
rospy.logdebug("Published Visualization")
def clearMarkers(self, maps=False, vector=False):
ma = MarkerArray()
ma.markers = [self.stamp(Marker())]
ma.markers[0].action = 3
ma.markers[0].ns = "maps"
if maps: self.map_pub.publish(ma)
if vector: self.vf_pub.publish(ma)
def publish_glob(self, glob):
marker_publisher = rospy.Publisher('/glob_map_rviz',
MarkerArray,
queue_size=10)
msg = MarkerArray()
markers = []
i = 0
for cone in glob:
marker = Marker()
# marker.id = np.random.randint(0,1e6)
marker.id = int(cone[6])
marker.ns = "glob"
marker.header.frame_id = "/map"
marker.type = marker.CYLINDER
if int(cone[4]) > -4:
marker.action = marker.ADD
else:
marker.action = marker.DELETE
marker.scale.x = cone[3]
marker.scale.y = cone[3]
marker.scale.z = 0.01
if cone[5] == 1:
marker.color.a = 1
else:
marker.color.a = 0.4
if int(cone[2]) == 0.0: #BLUE
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
elif int(cone[2]) == 1.0: #YELLOW
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
elif int(cone[2]) == 2.0: #ORANGE
marker.color.r = 1.0
marker.color.g = 0.5
marker.color.b = 0.0
elif int(cone[2]) == 3.0: #ORANGE
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b = 0.5
marker.pose.position.x = cone[0]
marker.pose.position.y = cone[1]
marker.pose.position.z = 0.01
marker.lifetime = rospy.Duration(0.5)
markers.append(marker)
msg.markers = markers
marker_publisher.publish(msg)
rospy.Rate(100).sleep()
i = i + 1
def publishLines(self, markerLines, lines, stamp):
markerArray = MarkerArray()
markerArray.markers = markerLines
self.pubMarker.publish(markerArray);
linesMsg = visionTests.msg.Lines()
linesMsg.header.stamp = stamp
linesMsg.lines = lines
self.pubLines.publish(linesMsg)
def drawMap(self):
self.clearMarkers(maps=True)
ma = MarkerArray()
ma.markers = [self.drawRect('ground', 0, 0, 0, *self.GROUND)]
for i, o in enumerate(self.OBSTACLES):
ma.markers.append(self.drawRect('obs', i, *o))
for i, (x, y, th) in enumerate(self.AR_TAGS):
ma.markers.append(self.drawRect('ar', i, x, y, .1, .1))
self.map_pub.publish(ma)
def calcAffordances(self, lines):
# Calculate affordances by intersecting agains one triangle per affordance
# make initial triangle
o = Point(0, 0, 0)
# The triangle is a bit behind
v1 = Point(cos(self.aperture / 2) * self.lenght, sin(self.aperture / 2) * self.lenght, 0)
v2 = Point(cos(self.aperture / 2) * self.lenght, -sin(self.aperture / 2) * self.lenght, 0)
t = Triangle([o, v1, v2])
# for each angle: transform the triangle and intersect it with all lines
lBase = Line(o, Point(1, 0, 0));
affordances = []
markers = []
i = 0
for angle in self.possibleAngles:
# reference transformation
lTransform = Line(o, Point(cos(angle), sin(angle), 0))
mov = Movement(lBase, lTransform)
tMoved = t.moved(mov);
#canMove = True
# Empty lines means no affordance (too close to the wall to see a line
canMove = len(lines) > 0
j = 0
while j < len(lines) and canMove:
l = lines[j]
p1 = l.points[0]
p2 = l.points[1]
canMove = canMove and not tMoved.containedOrIntersect(Point(p1.x, p1.y, p1.z), Point(p2.x, p2.y, p2.z))
# if not canMove:
# print "cant move to", angle
# print tMoved.vertices
# print p1
# print p2
j += 1
affordances.append(canMove)
if canMove:
color = 'b'
else:
color = 'r'
geomPoints = [geometry_msgs.msg.Point(p.r[0], p.r[1], p.r[2]) for p in tMoved.vertices]
geomPoints.append(geomPoints[0])
markers.append(self.getLineStrip(geomPoints, i, rospy.Time.now(), color))
i += 1
for l in lines:
# print "line", a
markers.append(self.getLineStrip([l.points[0], l.points[1]], i, rospy.Time.now(), 'g'))
i = i + 1
mArray = MarkerArray()
mArray.markers = markers
self.pubMarker.publish(mArray)
return affordances
def table_marker_publisher():
rospy.init_node('table_marker_publisher', anonymous=True)
pub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=1)
rospy.sleep(.3)
if not rospy.is_shutdown():
msg = MarkerArray()
msg.markers = make_table_markers()
msg.markers.append(make_mesh_marker("maker_frame", len(msg.markers) + 1, Pose(Point(z=0.04), Quaternion(z=1.0)), Vector3(1, 1, 1), opaque(), "package://giskard_examples/models/electrical-devices/pancake-maker.dae"))
msg.markers.append(make_cylinder_marker("cup_bottom_frame", len(msg.markers) +1, Pose(position=Point(z=0.07)), Vector3(0.085, 0.085, 0.14), transparent_red(), True))
pub.publish(msg)
def run(self):
while not rospy.is_shutdown():
rospy.loginfo(
"$gt: %d" %
(rospy.Time.now().secs - self.duration * 60 * 60 * 24))
if self.use_ros_time or self.use_sim_time:
rospy.loginfo("fetching data")
trans = self.msg_store.query(
type=TransformStamped._type,
message_query={
"header.stamp.secs": {
"$lte":
rospy.Time.now().secs,
"$gt":
rospy.Time.now().secs -
self.duration * 60 * 60 * 24
}
},
sort_query=[("$natural", 1)],
limit=self.limit)
else:
trans = self.msg_store.query(
type=TransformStamped._type,
meta_query={
"inserted_at": {
"$gt":
datetime.utcnow() - timedelta(days=self.duration)
}
},
sort_query=[("$natural", 1)])
rospy.loginfo("found %d msgs" % len(trans))
m_arr = MarkerArray()
m_arr.markers = V.transformStampedArrayToLabeledLineStripMarker(
trans[::self.downsample],
label_downsample=self.label_downsample,
discrete=True)
m_arr.markers = V.transformStampedArrayToLabeledArrayMarker(
trans[::self.downsample],
label_downsample=self.label_downsample,
discrete=True)
self.pub.publish(m_arr)
rospy.sleep(1.0)
rospy.loginfo("publishing move_base_marker_array")
def skeletons_to_marker_array(skeletons, veh_name='default'):
markers = []
for index, skeleton in enumerate(skeletons.skeletons):
marker = skeleton_to_marker(skeleton, index, veh_name=veh_name)
markers.append(marker)
marker_array = MarkerArray()
marker_array.markers = markers
return marker_array
def get_markers(self):
marker_array = MarkerArray()
marker_array.markers = []
for obj in self._tracked_objects:
markers = obj.get_markers().markers
for marker in markers:
marker_array.markers.append(marker)
return marker_array
def _add_markers(self, point, text1, point_2, text2):
# add marker for start and goal pose of EE
marker_array = MarkerArray()
marker_1 = Marker()
marker_1.header.frame_id = "world"
marker_1.header.stamp = rospy.Time.now()
marker_1.type = Marker.SPHERE
marker_1.scale.x = 0.04
marker_1.scale.y = 0.04
marker_1.scale.z = 0.04
marker_1.lifetime = rospy.Duration()
marker_2 = deepcopy(marker_1)
marker_1.color.g = 0.5
marker_1.color.a = 1.0
marker_1.id = 0
marker_1.pose.position.x = point[0]
marker_1.pose.position.y = point[1]
marker_1.pose.position.z = point[2]
marker_2.color.r = 0.5
marker_2.color.a = 1.0
marker_2.id = 1
marker_2.pose.position.x = point_2[0]
marker_2.pose.position.y = point_2[1]
marker_2.pose.position.z = point_2[2]
marker_3 = Marker()
marker_3.header.frame_id = "world"
marker_3.header.stamp = rospy.Time.now()
marker_3.type = Marker.TEXT_VIEW_FACING
marker_3.scale.z = 0.10
marker_3.lifetime = rospy.Duration()
marker_4 = deepcopy(marker_3)
marker_3.text = text1
marker_3.id = 2
marker_3.color.g = 0.5
marker_3.color.a = 1.0
marker_3.pose.position.x = point[0]
marker_3.pose.position.y = point[1]
marker_3.pose.position.z = point[2] + 0.15
marker_4.text = text2
marker_4.id = 3
marker_4.color.r = 0.5
marker_4.color.a = 1.0
marker_4.pose.position.x = point_2[0]
marker_4.pose.position.y = point_2[1]
marker_4.pose.position.z = point_2[2] + 0.15
marker_array.markers = [marker_1, marker_2, marker_3, marker_4]
self._marker_pub.publish(marker_array)
rospy.sleep(1)
def countGoodGaps(self, gaps):
scores = np.array(gaps.scores)
scores = np.sqrt(scores)
mean = np.mean(scores)
std = np.std(scores)
thresh = mean + (std * self.goodGapThresholdFactor)
print(scores)
print("AVG: {} STD: {} THRESH: {}".format(mean, std, thresh))
criticalGapIndices = np.argwhere(scores > thresh)
if DO_VISUALIZATION:
goodColor = [.1, 1.0, .1]
badColor = [1.0, 0, 0]
gapMarkers = MarkerArray()
gapMarkers.markers = []
self.maxMarkers = max(len(gaps.gaps), self.maxMarkers)
for gdx in xrange(self.maxMarkers):
marker = Marker()
if gdx >= len(gaps.gaps):
marker.id = 100 + gdx
marker.action = marker.DELETE
gapMarkers.markers.append(marker)
continue
gap = gaps.gaps[gdx]
goodGap = gdx in criticalGapIndices
color = goodColor if goodGap else badColor
# Specify the frame in which to interpret the x,y,z coordinates. It is the laser frame.
marker.id = 100 + gdx
marker.header.frame_id = "/laser"
marker.pose.position.x = gap.points[1].range * np.cos(
gap.points[1].angle - np.deg2rad(135))
marker.pose.position.y = gap.points[1].range * np.sin(
gap.points[1].angle - np.deg2rad(135))
marker.pose.position.z = 0 # or set this to 0
marker.type = marker.SPHERE
marker.action = marker.MODIFY
marker.lifetime = rospy.rostime.Duration(secs=.2)
marker.scale.x = 1.0 - (not goodGap) * .5
marker.scale.y = 1.0 - (not goodGap) * .5
marker.scale.z = 1.0 - (not goodGap) * .5
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.color.a = 1.0
gapMarkers.markers.append(marker)
rospy.loginfo("Published good gap markers")
self.goodGapVisualizationPub.publish(gapMarkers)
return len(criticalGapIndices)
def beziers_to_marker_array(beziers, veh_name='default'):
markers = []
for index, bezier in enumerate(beziers.beziers):
marker = bezier_to_marker(bezier, index, veh_name=veh_name)
marker.id = index
markers.append(marker)
marker_array = MarkerArray()
marker_array.markers = markers
return marker_array
def clear_markers(self):
ma = MarkerArray()
ma.markers = [Marker() for tid in self.tracks3d.keys()]
for tid, i in zip(self.tracks3d.keys(), range(len(self.tracks3d))):
ma.markers[i].header.frame_id = "camera_rgb_optical_frame"
ma.markers[i].ns = "line"
ma.markers[i].id = tid
ma.markers[i].action = 2
self.pub_marker.publish(ma)
for i in range(len(self.tracks3d)):
ma.markers[i].ns = "point"
self.pub_marker.publish(ma)
def clear_markers(self):
ma = MarkerArray()
ma.markers = [ Marker() for tid in self.tracks3d.keys() ]
for tid,i in zip(self.tracks3d.keys(), range(len(self.tracks3d))):
ma.markers[i].header.frame_id = "camera_rgb_optical_frame"
ma.markers[i].ns = "line"
ma.markers[i].id = tid
ma.markers[i].action = 2
self.pub_marker.publish(ma)
for i in range(len(self.tracks3d)):
ma.markers[i].ns = "point"
self.pub_marker.publish(ma)
def makeViz(self, reachSets):
pointSets = [[tuple(p.p) for p in rs.set] for rs in reachSets.sets]
triangleSets = [tripy.earclip(ps) for ps in pointSets]
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = reachSets.header.frame_id
origin = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1))
lineMarkers = MarkerArray()
lineMarkerArray = []
for ii in range(len(pointSets)):
m = Marker()
m.header = header
m.id = self.outline_marker_id + ii
m.action = 0
m.pose = origin
m.type = 4 #LINE_STRIP
m.color = self.colors[ii % len(self.colors)]
m.points = [Point(p[0], p[1], 0) for p in pointSets[ii]]
m.scale = Vector3(.05, 0, 0)
lineMarkerArray.append(m)
lineMarkers.markers = lineMarkerArray
self.outlinePub.publish(lineMarkers)
triPoints = [xy for tri in triangleSets for xy in tri]
triMarker = Marker()
triMarker.header = header
triMarker.id = self.tri_marker_id
triMarker.type = 11 #TRIANGLE_LIST
triMarker.action = 0
triMarker.pose = origin
triMarker.color = ColorRGBA(1, 1, 1, 1)
triMarker.scale = Vector3(1, 1, 1)
triMarker.points = [
Point(p[0], p[1], 0) for tri in triPoints for p in tri
]
#expand color array to cover all verts for all tris in each set with same color
triFrequency = [len(ps) for ps in pointSets]
triColors = [
self.colors[ii % len(self.colors)] for ii in range(len(pointSets))
]
triMarker.colors = [
c for cidx in range(len(triColors))
for c in repeat(triColors[cidx], triFrequency[cidx])
]
self.trisPub.publish(triMarker)
def publish_check_poses(self, pose_cam_1_data, pose_cam_2_data,
pose_cam_4_data):
ma_1 = MarkerArray()
ma_1.markers = pose_cam_1_data.markers
ma_1.markers = self.set_check_poses_markers(ma_1.markers, 0.5, 0.61,
0.9)
if SHOW_CONFIDENCES:
ma_1 = self.add_confidence_markers(ma_1,
pose_cam_1_data.confidences)
elif SHOW_COVARIANCES:
ma_1 = self.add_covariance_markers(ma_1, 1)
self.check_cam_1_pub.publish(ma_1)
ma_2 = MarkerArray()
ma_2.markers = pose_cam_2_data.markers
ma_2.markers = self.set_check_poses_markers(ma_2.markers, 160 / 255.0,
1.0, 121 / 255.0)
if SHOW_CONFIDENCES:
ma_2 = self.add_confidence_markers(ma_2,
pose_cam_2_data.confidences)
elif SHOW_COVARIANCES:
ma_2 = self.add_covariance_markers(ma_2, 2)
self.check_cam_2_pub.publish(ma_2)
ma_4 = MarkerArray()
ma_4.markers = pose_cam_4_data.markers
ma_4.markers = self.set_check_poses_markers(ma_4.markers, 0.5, 0.82,
0.9)
if SHOW_CONFIDENCES:
ma_4 = self.add_confidence_markers(ma_4,
pose_cam_4_data.confidences)
elif SHOW_COVARIANCES:
ma_4 = self.add_covariance_markers(ma_4, 4)
self.check_cam_4_pub.publish(ma_4)
def callback_skeleton(self, data):
mk = Marker()
mk.header.frame_id = '/map'
mk.ns = 'skeletro'
mk.id = self.m_id
mk.type = Marker.SPHERE_LIST
mk.points = []
for j in data.joints:
if self.point_is_valid(j):
mk.points.append(j)
mk.scale = geometry_msgs.msg.Vector3(3, 3, 3)
mk.pose.position = geometry_msgs.msg.Point(0, 0, 0)
mk.pose.orientation = geometry_msgs.msg.Quaternion(0.0, 0.0, 0.0, 1.0)
# mk.lifetime = genpy.Duration(0.1)
mk.color = std_msgs.msg.ColorRGBA(1.0, 0.0, 0.0, 1.0)
linek = Marker()
linek.header.frame_id = '/map'
linek.ns = 'lines'
linek.id = self.m_id
linek.type = Marker.LINE_LIST
linek.scale = geometry_msgs.msg.Vector3(0.3, 0.3, 0.3)
linek.pose.position = geometry_msgs.msg.Point(0, 0, 0)
linek.pose.orientation = geometry_msgs.msg.Quaternion(
0.0, 0.0, 0.0, 1.0)
linek.points = []
self.vis_add_line(linek, data.joints, 0, 1)
self.vis_add_line(linek, data.joints, 1, 2)
self.vis_add_line(linek, data.joints, 3, 4)
self.vis_add_line(linek, data.joints, 4, 5)
self.vis_add_line(linek, data.joints, 6, 7)
self.vis_add_line(linek, data.joints, 7, 8)
self.vis_add_line(linek, data.joints, 9, 10)
self.vis_add_line(linek, data.joints, 10, 11)
self.vis_add_line(linek, data.joints, 12, 13)
self.vis_add_line(linek, data.joints, 13, 14)
# linek.lifetime = genpy.Duration(0.1)
linek.color = std_msgs.msg.ColorRGBA(0.0, 1.0, 0.0, 1.0)
msg = MarkerArray()
msg.markers = []
msg.markers.append(mk)
msg.markers.append(linek)
self.visualization_pub.publish(msg)
def place_kin_tree_from_link(self, ps, linkname, valuedict=None, ns = "default_ns"):
markers = make_kin_tree_from_link(ps, linkname, valuedict=valuedict)
marker_array = MarkerArray()
marker_array.markers = markers
handles = []
for marker in markers:
id = self.get_unused_id()
self.ids.add(id)
marker.id = id
marker.ns = ns
handles.append(MarkerHandle(marker, self.pub))
self.pub.publish(marker)
return handles
def removeObject(self, name):
if name in self.objects:
self.intManager.removeObject(name)
del self.objects[name]
m = Marker()
m.ns = self.ns
m.header.stamp = rospy.get_rostime()
m.action = Marker.DELETE
ma = MarkerArray()
ma.markers = [m]
self.visPub.publish(ma)
def table_marker_publisher():
rospy.init_node('table_marker_publisher')
cup_on_table = rospy.get_param("~cup_on_table")
pub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=1)
rospy.sleep(.3)
if not rospy.is_shutdown():
msg = MarkerArray()
msg.markers = make_table_markers()
if cup_on_table:
msg.markers.append(make_mesh_marker("base_link", len(msg.markers) + 1, Pose(Point(x=0.4, z=0.71), Quaternion(z=1.0)), Vector3(1, 1, 1), white(), "package://giskard_ros/object_meshes/cup_eco_orange.dae", frame_locked=True))
else:
msg.markers.append(make_mesh_marker("l_gripper_tool_frame", len(msg.markers) + 1, Pose(Point(x=-0.01, z=-0.02), Quaternion(z=1.0)), Vector3(1, 1, 1), white(), "package://giskard_ros/object_meshes/cup_eco_orange.dae", frame_locked=True))
msg.markers.append(make_mesh_marker("r_gripper_tool_frame", len(msg.markers) + 1, Pose(Point(x=-0.01, z=0.02), Quaternion(z=1.0)), Vector3(1, 1, 1), white(), "package://giskard_ros/object_meshes/sigg_bottle.dae", frame_locked=True))
# msg.markers.append(make_cylinder_marker("cup_bottom_frame", len(msg.markers) +1, Pose(position=Point(z=0.07)), Vector3(0.085, 0.085, 0.14), transparent_red(), True))
pub.publish(msg)
def get_many_vectors(self):
arrows = MarkerArray()
coords = []
i = 0
color = ColorRGBA(0.0, 1.0, 0.0, 1.0)
for lat in np.linspace(0, np.pi, 10):
color.g += 0.1
color.b = 0
for lon in np.linspace(0, 2 * np.pi, 10):
color.b += 0.1
coords.append((lat, lon, 1, i, copy.copy(color)))
i += 1
arrows.markers = [
create_arrow_marker(self.ell_space.ellipsoidal_to_pose(lat, lon, height), i, clr)
for lat, lon, height, i, clr in coords
]
return arrows
def callback(data):
global marker_ests
#Publish it as a marker in rviz
marker_ests = MarkerArray()
marker_ests.markers = []
print len(data.poses)
i = 0
for pose in data.poses:
marker_est = Marker()
marker_est.header.frame_id = "base"
marker_est.ns = "est_pose_"+str(i)
marker_est.id = 42+i
marker_est.type = Marker.CUBE
marker_est.action = Marker.ADD
marker_est.pose = pose
marker_est.color.r, marker_est.color.g, marker_est.color.b = (0, 255, 0)
marker_est.color.a = 0.5
marker_est.scale.x, marker_est.scale.y, marker_est.scale.z = (0.06, 0.06, 0.06)
marker_ests.markers.append(marker_est)
i+=1
def scan_callback(data):
global angles, current_state, min_index, max_index, mid_poses, distance_thresh, pub_marker, min_opening_width, door_poses
# print "scan"
my_angles = angles[min_index:max_index]
ranges = list(data.ranges[min_index:max_index])
# ranges = map(lambda y: y>distance_thresh and distance_thresh or y, ranges)
ranges = map(lambda y: y > min_dist and distance_thresh or y, ranges)
cart_points = map(polar2cart, ranges, my_angles)
# print cart_points
# opening = [x > distance_thresh for x in ranges]
start_border_index = []
stop_border_index = []
opening_IDs = []
started = False
for i in range(1, len(ranges)): # also supposed to work for the start and stop ends
if math.fabs(ranges[i] - ranges[i - 1]) > delta_dist:
opening_IDs.append(i)
# print opening_IDs
# print start_border_index
# langles = [None]
# for i in range(1, len(cart_points)-2):
# b = math.sqrt( (cart_points[i+1][0] - cart_points[i-1][0])**2 + (cart_points[i+1][1] - cart_points[i-1][1])**2 )
# c = math.sqrt( (cart_points[i][0] - cart_points[i-1][0])**2 + (cart_points[i][1] - cart_points[i-1][1])**2 )
# a = math.sqrt( (cart_points[i+1][0] - cart_points[i][0])**2 + (cart_points[i+1][1] - cart_points[i][1])**2 )
# langle = math.acos( (a**2 + c**2 - b**2)/(2*a*c) )
# langles.append(langle)
# langles.append(None)
# print langles
# print cart_points
mm = MarkerArray()
mm.markers = []
# marker = Marker() # circle of maximal distance (green)
# marker.header.frame_id = "/base_laser_rear_link"
# marker.header.stamp = rospy.Time.now()
# marker.lifetime = rospy.Duration(1.5)
# marker.ns = "edges"
# marker.id = 11000
# marker.type = 2 # SPHERE
# marker.action = 0 # ADD
# marker.pose.position.x = 0.
# marker.pose.position.y = 0.
# marker.pose.position.z = 0.
# marker.pose.orientation.x = 0.0
# marker.pose.orientation.y = 0.0
# marker.pose.orientation.z = 1.0
# marker.pose.orientation.w = 1.0
# marker.scale.x = 2*distance_thresh
# marker.scale.y = 2*distance_thresh
# marker.scale.z = 0.25
# marker.color.a = 0.3
# marker.color.r = 0. # self.color.r
# marker.color.g = 1. # self.color.g
# marker.color.b = 0. # self.color.b
# mm.markers.append(marker)
# pub_marker.publish(mm)
marker = Marker() # circle of minimal distance (yellow)
marker.header.frame_id = "/base_laser_rear_link"
marker.header.stamp = rospy.Time.now()
# marker.lifetime = rospy.Duration(1.5)
marker.ns = "edges"
marker.id = 8746
marker.type = 2 # SPHERE
marker.action = 0 # ADD
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 1.0
marker.pose.orientation.w = 1.0
marker.scale.x = 2 * min_dist
marker.scale.y = 2 * min_dist
marker.scale.z = 0.30
marker.color.a = 0.7
marker.color.r = 1.0 # self.color.r
marker.color.g = 1.0 # self.color.g
marker.color.b = 0.0 # self.color.b
mm.markers.append(marker)
## special cases: open to right or left
iStart = []
iStop = []
lengths = []
if ranges[0] >= min_dist:
ranges[0] = min_dist
cart_points[0] = polar2cart(ranges[0], my_angles[0])
if ranges[len(ranges) - 1] >= min_dist:
ranges[len(ranges) - 1] = min_dist
cart_points[len(ranges) - 1] = polar2cart(ranges[len(ranges) - 1], my_angles[len(ranges) - 1])
started = False
for i in range(1, len(ranges)):
if not started and ranges[i] >= min_dist:
iStart.append(i - 1)
started = True
elif started and ranges[i] <= min_dist:
iStop.append(i)
started = False
for i in range(0, len(ranges)): # all ranges (blue):
marker = Marker()
marker.lifetime = rospy.Duration(1.5)
marker.header.frame_id = "/base_laser_rear_link"
marker.header.stamp = rospy.Time.now()
marker.ns = "edges"
marker.id = 3000 + i
marker.type = 2 # SPHERE
marker.action = 0 # ADD
marker.pose.position.x = cart_points[i][0]
marker.pose.position.y = cart_points[i][1]
marker.pose.position.z = 0.5
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 1.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.04
marker.scale.y = 0.04
marker.scale.z = 0.25
marker.color.a = 0.7
marker.color.r = 0.6 # self.color.r
marker.color.g = 0.0 # self.color.g
marker.color.b = 0.8 # self.color.b
if i == 0:
marker.color.g = 0.0
marker.color.a = 1.0
marker.color.b = 1.0
marker.scale.x = 0.18
marker.scale.y = 0.28
marker.scale.z = 0.3
mm.markers.append(marker)
for i in range(0, len(iStart)):
marker = Marker()
marker.lifetime = rospy.Duration(1.5)
marker.header.frame_id = "/base_laser_rear_link"
marker.header.stamp = rospy.Time.now()
marker.ns = "edges"
marker.id = 17101 + 10 * i
marker.type = 2 # SPHERE
marker.action = 0 # ADD
marker.pose.position.x = cart_points[iStart[i]][0]
marker.pose.position.y = cart_points[iStart[i]][1]
marker.pose.position.z = 0.25
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 1.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 = 0.7
marker.color.r = 1.0 # self.color.r
marker.color.g = 0.0 # self.color.g
marker.color.b = 0.0 # self.color.g
mm.markers.append(marker)
for i in range(0, len(iStop)):
marker = Marker()
marker.lifetime = rospy.Duration(1.5)
marker.header.frame_id = "/base_laser_rear_link"
marker.header.stamp = rospy.Time.now()
marker.ns = "edges"
marker.id = 17102 + 10 * i
marker.type = 2 # SPHERE
marker.action = 0 # ADD
marker.pose.position.x = cart_points[iStop[i]][0]
marker.pose.position.y = cart_points[iStop[i]][1]
marker.pose.position.z = 0.25
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 1.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 = 0.7
marker.color.r = 1.0 # self.color.r
marker.color.g = 0.0 # self.color.g
marker.color.b = 1.0 # self.color.g
mm.markers.append(marker)
pub_marker.publish(mm)
print "iStart:"
print iStart
print "iStop:"
print iStop
for i in range(0, len(iStart)):
lengths.append(
math.sqrt(
(cart_points[iStart[i]][0] - cart_points[iStop[i]][0]) ** 2
+ (cart_points[iStart[i]][1] - cart_points[iStop[i]][1]) ** 2
)
)
toKeep = []
for i in range(0, len(iStart)):
if lengths[i] > min_opening_width:
toKeep.append(i)
iStart = [iStart[i] for i in toKeep]
iStop = [iStop[i] for i in toKeep]
lengths = [lengths[i] for i in toKeep]
centerpoints = []
tangents = []
normals = []
phis = []
for i in range(0, len(iStart)):
centerpoints.append(
[
(cart_points[iStart[i]][0] + cart_points[iStop[i]][0]) * 0.5,
(cart_points[iStart[i]][1] + cart_points[iStop[i]][1]) * 0.5,
]
)
tangents.append(
[cart_points[iStart[i]][0] - cart_points[iStop[i]][0], cart_points[iStart[i]][1] - cart_points[iStop[i]][1]]
)
normals.append([-tangents[len(tangents) - 1][1], tangents[len(tangents) - 1][0]])
phis.append(math.atan(normals[len(tangents) - 1][1] / normals[len(tangents) - 1][0]))
door_poses = []
for i in range(0, len(iStart)):
p = Pose2D()
p.x = centerpoints[i][0]
p.y = centerpoints[i][1]
p.theta = phis[i]
door_poses.append(p)
print len(iStart)
print door_poses
for i in range(0, len(iStart)):
marker = Marker()
marker.lifetime = rospy.Duration(1.5)
marker.header.frame_id = "/base_laser_rear_link"
marker.header.stamp = rospy.Time.now()
marker.ns = "edges"
marker.id = 17103 + 10 * i
marker.type = 2 # SPHERE
marker.action = 0 # ADD
marker.pose.position.x = centerpoints[i][0]
marker.pose.position.y = centerpoints[i][1]
marker.pose.position.z = 0.25
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 1.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 0.7
marker.color.r = 1.0 # self.color.r
marker.color.g = 1.0 # self.color.g
marker.color.b = 0.0 # self.color.g
mm.markers.append(marker)
marker = Marker()
marker.lifetime = rospy.Duration(1.5)
marker.header.frame_id = "/base_laser_rear_link"
marker.header.stamp = rospy.Time.now()
marker.ns = "edges"
marker.id = 17104 + 10 * i
marker.type = 2 # SPHERE
marker.action = 0 # ADD
marker.pose.position.x = centerpoints[i][0] + normals[i][0]
marker.pose.position.y = centerpoints[i][1] + normals[i][1]
marker.pose.position.z = 0.25
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 1.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.25
marker.scale.y = 0.25
marker.scale.z = 0.25
marker.color.a = 0.7
marker.color.r = 1.0 # self.color.r
marker.color.g = 0.0 # self.color.g
marker.color.b = 1.0 # self.color.g
mm.markers.append(marker)
pub_marker.publish(mm)
x = random.randint(1, 10)
if x < 5:
current_state = False
else:
current_state = True
def publishLines(self, markerLines):
markerArray = MarkerArray()
markerArray.markers = markerLines
self.pubMarker.publish(markerArray);
