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 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 SendFaceMarkers(self, frame_id, ts, cface_id, ns, position):
# the face
marker = Marker()
marker.header.frame_id = frame_id
marker.header.stamp = ts
marker.id = cface_id
marker.ns = ns
marker.type = Marker.SPHERE
marker.action = Marker.MODIFY
marker.pose.position.x = position.x
marker.pose.position.y = position.y
marker.pose.position.z = position.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.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 = 0.5
marker.lifetime = rospy.Duration(2, 0)
marker.frame_locked = False
self.face_rviz_pub.publish(marker)
# label under the face
marker = Marker()
marker.header.frame_id = frame_id
marker.header.stamp = ts
marker.id = cface_id + 1
marker.ns = ns
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.MODIFY
marker.pose.position.x = position.x
marker.pose.position.y = position.y
marker.pose.position.z = position.z - 0.1
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.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.r = 0.7
marker.color.g = 0.7
marker.color.b = 0.7
marker.color.a = 0.5
if self.cfaces[cface_id].gender == 2:
gender = "female"
else:
gender = "male"
marker.text = self.name + " candidate {} ({} {})".format(
cface_id, gender, int(self.cfaces[cface_id].age))
marker.lifetime = rospy.Duration(2, 0)
marker.frame_locked = False
self.face_rviz_pub.publish(marker)
def publish_artifacts(self, event):
VISUALIZATION = {
# TYPE: (label, rgb, shape, size)
'TYPE_BACKPACK': ("Backpack", (1.0, 0.0, 0.0), Marker.SPHERE, (0.4, 0.4, 0.5)),
'TYPE_CUBE': ("Cube", (0.0, 1.0, 1.0), Marker.CUBE, (0.3, 0.3, 0.3)),
'TYPE_DRILL': ("Drill", (1.0, 0.5, 1.0), Marker.CYLINDER, (0.2, 0.2, 0.3)),
'TYPE_EXTINGUISHER': ("Extinguisher", (1.0, 0.0, 0.0), Marker.CYLINDER, (0.3, 0.3, 0.5)),
'TYPE_GAS': ("Gas", (1.0, 1.0, 1.0), Marker.SPHERE, (1.0, 1.0, 1.0)),
'TYPE_HELMET': ("Helmet", (0.7, 0.7, 0.7), Marker.SPHERE, (0.3, 0.3, 0.3)),
'TYPE_PHONE': ("Phone", (0.0, 1.0, 0.5), Marker.CUBE, (0.15, 0.15, 0.1)),
'TYPE_RESCUE_RANDY': ("Survivor", (0.8, 1.0, 0.7), Marker.CYLINDER, (0.8, 0.8, 1.4)),
'TYPE_ROPE': ("Rope", (0.0, 0.0, 1.0), Marker.CYLINDER, (0.5, 0.5, 0.2)),
'TYPE_VENT': ("Vent", (1.0, 1.0, 1.0), Marker.CUBE, (0.8, 0.8, 0.8)),
}
msg = MarkerArray()
now = rospy.Time.now()
with self.lock:
for artf_idx, (artf_type, artf_xyz) in enumerate(self.artifacts):
artf_name, rgb, shape, size = VISUALIZATION[artf_type]
marker = Marker()
marker.header.frame_id = 'global'
marker.header.stamp = now
marker.ns = 'artifact:object'
marker.id = artf_idx
marker.action = Marker.MODIFY
marker.pose.position.x, marker.pose.position.y, marker.pose.position.z = [v / 1000.0 for v in artf_xyz]
marker.pose.orientation.x, marker.pose.orientation.y, marker.pose.orientation.z, marker.pose.orientation.w = 0, 0, 0, 1
marker.lifetime = rospy.Duration(0) # FOREVER
marker.frame_locked = False
marker.type = shape
marker.scale.x, marker.scale.y, marker.scale.z = size
marker.color.r, marker.color.g, marker.color.b = rgb
marker.color.a = 0.3
msg.markers.append(marker)
label = Marker()
label.header.frame_id = 'global'
label.header.stamp = now
label.ns = 'artifact:name'
label.id = artf_idx
label.action = Marker.MODIFY
label.text = artf_name
label.pose.position.x, label.pose.position.y, label.pose.position.z = [v / 1000.0 for v in artf_xyz]
label.pose.position.z += 1.2 # Showing the label above the object.
label.pose.orientation.x, label.pose.orientation.y, label.pose.orientation.z, label.pose.orientation.w = 0, 0, 0, 1
label.lifetime = rospy.Duration(0) # FOREVER
label.frame_locked = False
label.type = Marker.TEXT_VIEW_FACING
label.scale.x, label.scale.y, label.scale.z = 1, 1, 1
label.color.r, label.color.g, label.color.b = rgb
label.color.a = 1.0
msg.markers.append(label)
self.artf_publisher.publish(msg)
def update_marker(
self, room_id, scene_id,
belief): #function used to update a specif place's marker
Markers_up = MarkerArray()
SMarker = Marker()
Text_Marker = Marker()
SMarker.header.frame_id = "map"
SMarker.type = 1
SMarker.pose.orientation.w = 1.0
SMarker.lifetime = rospy.Duration(0)
SMarker.action = SMarker.ADD
SMarker.pose.position.z = 0
SMarker.color.r = float(self.scenes_markers[scene_id]['r'])
SMarker.color.g = float(self.scenes_markers[scene_id]['g'])
SMarker.color.b = float(self.scenes_markers[scene_id]['b'])
SMarker.color.a = 0.65
SMarker.frame_locked = True
SMarker.id = 1
SMarker.header.stamp = rospy.Time.now()
SMarker.ns = '{}-{}'.format(self.rooms[room_id].xcenter,
self.rooms[room_id].ycenter)
SMarker.pose.position.x = self.rooms[room_id].xcenter
SMarker.pose.position.y = self.rooms[room_id].ycenter
SMarker.scale.x = self.rooms[room_id].xmax - self.rooms[room_id].xmin
SMarker.scale.y = self.rooms[room_id].ymax - self.rooms[room_id].ymin
SMarker.scale.z = 0.1
Markers_up.markers.append(SMarker)
Text_Marker.header.frame_id = "map"
Text_Marker.id = 1
Text_Marker.type = Text_Marker.TEXT_VIEW_FACING
Text_Marker.action = 0
Text_Marker.pose.orientation.w = 1.0
Text_Marker.scale.z = 0.65
Text_Marker.color.r = 0
Text_Marker.color.g = 0
Text_Marker.color.b = 0
Text_Marker.color.a = 1
Text_Marker.lifetime = rospy.Duration(0)
Text_Marker.frame_locked = True
Text_Marker.pose.position.z = 0.2
Text_Marker.header.stamp = rospy.Time.now()
Text_Marker.ns = 'txt{}-{}'.format(self.rooms[room_id].xcenter,
self.rooms[room_id].ycenter)
Text_Marker.pose.position.x = self.rooms[room_id].xcenter
Text_Marker.pose.position.y = self.rooms[room_id].ycenter
Text_Marker.text = '{}:{:.2f}%'.format(
self.scenes_markers[scene_id]['label'], belief * 100)
#Text_Marker.text = 'Plc:{} ({})'.format(room_id,self.scenes_markers[scene_id]['label'])
Text_Marker.header.stamp = rospy.Time.now()
Markers_up.markers.append(Text_Marker)
self.marker_pub.publish(Markers_up)
def SendHandMarkers(self, frame_id, ts, chand_id, ns, position, gestures):
# the hand
marker = Marker()
marker.header.frame_id = frame_id
marker.header.stamp = ts
marker.id = chand_id
marker.ns = ns
marker.type = Marker.SPHERE
marker.action = Marker.MODIFY
marker.pose.position.x = position.x
marker.pose.position.y = position.y
marker.pose.position.z = position.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.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 0.5
marker.lifetime = rospy.Duration(2, 0)
marker.frame_locked = False
self.hand_rviz_pub.publish(marker)
# label under the hand
marker = Marker()
marker.header.frame_id = frame_id
marker.header.stamp = ts
marker.id = chand_id + 1
marker.ns = ns
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.MODIFY
marker.pose.position.x = position.x
marker.pose.position.y = position.y
marker.pose.position.z = position.z - 0.1
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.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.r = 0.7
marker.color.g = 0.7
marker.color.b = 0.7
marker.color.a = 0.5
marker.text = self.name + " hand: "
for gesture in gestures:
marker.text += " " + gesture
marker.lifetime = rospy.Duration(2, 0)
marker.frame_locked = False
self.hand_rviz_pub.publish(marker)
def add_rviz_disks(self, disk, rod, act, move): marker = Marker() if(move == True): marker.header.frame_id = "/pl_6" marker.frame_locked = True marker.pose.position.x = 0. marker.pose.position.y = 0. marker.pose.position.z = 0.27965 else: marker.header.frame_id = "/pl_base" marker.frame_locked = False marker.pose.position.x = act.position.x marker.pose.position.y = act.position.y marker.pose.position.z = act.position.z - 0.143 + 0.012 + len(self.rod_content[rod]) * 0.01 if(self.carrying_disk): marker.pose.position.z = marker.pose.position.z + 0.03 marker.ns = "basic_shapes" marker.id = disk marker.type = marker.CYLINDER marker.action = marker.ADD marker.pose.orientation.w = 1.0 marker.scale.z = 0.01 if(disk == 1): marker.color.r = 1.0 marker.color.a = 1.0 marker.scale.x = 0.027 elif(disk == 2): marker.color.r = 1.0 marker.color.g = 1.0 marker.color.a = 1.0 marker.scale.x = 0.031 elif(disk == 3): marker.color.g = 1.0 marker.color.a = 1.0 marker.scale.x = 0.036 elif(disk == 4): marker.color.b = 1.0 marker.color.a = 1.0 marker.scale.x = 0.04 elif(disk == 5): marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.color.a = 1.0 marker.scale.x = 0.027 marker.scale.y = marker.scale.x self.rviz_pub.publish(marker)
def __init__(self):
rospack = rospkg.RosPack()
pose_csv_path = rospack.get_path('robotx_tools')+'/data/gps-pose.csv'
markers = MarkerArray()
pub = rospy.Publisher('gps_pose_marker', MarkerArray, queue_size=1, latch=True)
with open(pose_csv_path, 'r') as f:
reader = csv.reader(f)
i = 0
for row in reader:
marker = Marker()
label_marker = Marker()
marker.header.frame_id = "world"
label_marker.header.frame_id = "world"
marker.ns = "pose"
label_marker.ns = "description"
label_marker.id = i
marker.id = i
label_marker.type = label_marker.TEXT_VIEW_FACING
label_marker.scale.x = 10
label_marker.scale.y = 10
label_marker.scale.z = 10
marker.type = marker.ARROW
marker.action = marker.ADD
marker.scale.x = 10
marker.scale.y = 5
marker.scale.z = 10
latlon = self.convert_to_latlon(row[1],row[2],row[3],row[4],row[5],row[6],row[7],row[8])
utm_point = utm.from_latlon(latlon[0],latlon[1])
label_marker.text = row[0]
label_marker.pose.position.x = utm_point[0]
label_marker.pose.position.y = utm_point[1]
label_marker.pose.position.z = 10
label_marker.color.r = 1
label_marker.color.g = 1
label_marker.color.b = 1
label_marker.color.a = 1
marker.pose.position.x = utm_point[0]
marker.pose.position.y = utm_point[1]
marker.color.b = 1
marker.color.a = 1
label_marker.frame_locked = True
marker.frame_locked = True
label_marker.pose.orientation = self.euler_to_quaternion(Vector3(0.0, 0.0, float(row[9])/180*math.pi))
marker.pose.orientation = self.euler_to_quaternion(Vector3(0.0, 0.0, float(row[9])/180*math.pi))
markers.markers.append(marker)
markers.markers.append(label_marker)
i = i + 1
pub.publish(markers)
def callback(ultra):
point = Marker()
point.header.stamp = rospy.Time.now()
point.header.frame_id = "imu_1"
point.ns = "points"
point.id = random.randint(0, 1000000)
point.type = Marker.CUBE
point.action = Marker.ADD
point.pose.position.x = ultra.seq
point.pose.position.y = 0
point.pose.position.z = 0
point.pose.orientation.w = 1
point.scale.x = .5
point.scale.y = .5
point.scale.z = .5
point.color.g = 1
point.color.a = .7
point.lifetime = rospy.Duration(30)
point.frame_locked = False
pub.publish(point)
def make_node_marker(self):
self.node_marker = MarkerArray()
time = rospy.get_rostime()
for node in self.map_data['NODE']:
n = Marker()
n.ns = "node"
n.header.frame_id = self.map_data['MAP_FRAME']
n.header.stamp = time
n.id = node['id']
n.action = Marker().ADD
n.type = Marker().CUBE
n.lifetime = rospy.Duration()
n.frame_locked = True
self.set_marker_scale(n, 1.5, 1.5, 1.5)
self.set_marker_rgb(n, 0.0, 1.0, 0.0)
self.set_marker_position(n, node['point']['x'], node['point']['y'],
0)
self.set_marker_orientation(n, 0, 0, 0)
self.node_marker.markers.append(n)
for n_id in self.deleted_id_list:
n = Marker()
n.ns = "node"
n.header.frame_id = self.map_data['MAP_FRAME']
n.header.stamp = time
n.id = n_id
n.action = Marker().DELETE
n.type = Marker().CUBE
n.lifetime = rospy.Duration()
self.set_marker_orientation(n, 0, 0, 0)
self.node_marker.markers.append(n)
def get_marker(self, ns, id, fix, r, g, b):
pt = GeoPoint()
pt.latitude = fix[0]
pt.longitude = fix[1]
pt.altitude = fix[2]
resp = self.from_ll(pt)
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time.now()
marker.ns = ns
marker.id = id
marker.type = marker.SPHERE
marker.pose.position.x = resp.map_point.x
marker.pose.position.y = resp.map_point.y
marker.pose.position.z = resp.map_point.z
marker.pose.orientation.w = 1.0
marker.scale.x = 0.5
marker.scale.y = 0.5
marker.scale.z = 0.5
marker.color.r = r
marker.color.g = g
marker.color.b = b
marker.color.a = 1.0
marker.frame_locked = True
return marker
def callback(msg):
marker = Marker()
if not msg.data:
marker.header.frame_id = 'map'
else:
marker.header = msg.data[0].header
marker.ns = 'roomba_observations'
marker.id = 0
marker.type = Marker.SPHERE_LIST
marker.action = Marker.MODIFY
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.r = 0.0
marker.color.g = 0.5
marker.color.b = 0.8
marker.color.a = 1.0
marker.lifetime = rospy.Duration(0.5)
marker.frame_locked = False
for odom in msg.data:
marker.points.append(odom.pose.pose.position)
pub.publish(marker)
def show_marker(self, dist):
'''
Records all the mappings of the brick every made when to brick is detected.
Places a marker at all of the mappings.
'''
marker = Marker()
marker = Marker(type=Marker.POINTS,
ns='brick_detection',
action=Marker.ADD)
marker.header.frame_id = "camera_rgb_frame"
marker.header.stamp = rospy.Time.now()
points = []
point = Point()
point.x = dist
point.y = 0
point.z = 0
points.append(point)
marker.points = points
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 2.0
marker.color.r = 1.0
marker.action = marker.ADD
marker.pose.orientation.w = 1
marker.lifetime = rospy.Duration(0)
marker.frame_locked = False
self.pub_marker.publish(marker)
def draw_footprint(self, points):
# print 'PRINT ARIS FOOTPRINT'
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = "/girona500"
marker.ns = "aris_foot_print"
marker.id = 1
marker.type = 4 # SPHERE
marker.action = 0 # Add/Modify an object
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.r = 0.0
for i in points:
p = Point()
p.x = i[0]
p.y = i[1]
p.z = i[2]
color = ColorRGBA()
color.r = 0.0
color.g = 0.5
color.b = 0.5
color.a = 0.7
marker.points.append(p)
marker.colors.append(color)
marker.lifetime = rospy.Duration(0.5)
marker.frame_locked = False
self.pub_aris_footprint.publish(marker)
def add_marker(self, mesh_frame, marker_name, mesh_file, use_materials=False, color=None):
marker_msg = Marker()
marker_msg.frame_locked = True
marker_msg.id = 0
marker_msg.action = Marker.ADD
marker_msg.mesh_use_embedded_materials = use_materials
if marker_msg.mesh_use_embedded_materials:
marker_msg.color.r = 0
marker_msg.color.g = 0
marker_msg.color.b = 0
marker_msg.color.a = 0
elif color:
marker_msg.color = color
else:
marker_msg.color.r = 0.6
marker_msg.color.g = 0.6
marker_msg.color.b = 0.6
marker_msg.color.a = 1.0
marker_msg.header.stamp = rospy.get_rostime()
#marker_msg.lifetime =
#marker_msg.pose =
marker_msg.type = Marker.MESH_RESOURCE
marker_msg.header.frame_id = mesh_frame
marker_msg.ns = marker_name
marker_msg.mesh_resource = 'file://%s' % (os.path.abspath(mesh_file))
scale = 1.0
marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale, scale, scale
marker_msg.pose.position.x, marker_msg.pose.position.y, marker_msg.pose.position.z = 0, 0, 0
marker_msg.pose.orientation.x, marker_msg.pose.orientation.y, marker_msg.pose.orientation.z, marker_msg.pose.orientation.w = 0, 0, 0, 1
self.pub_marker_sync(marker_msg)
if not self.marker_thread:
self.marker_thread = thread.start_new_thread(MarkerPublisher.publish_markers, (self,))
def gpscallback(msg):
marker = Marker()
marker.header = msg.header
marker.header.frame_id = "map"
marker.ns = 'gps'
marker.id = 0
marker.type = Marker.SPHERE_LIST
marker.action = Marker.MODIFY
marker.scale.x = 0.3
marker.scale.y = 0.3
marker.scale.z = 0.3
marker.color.r = 0
marker.color.g = 0
marker.color.b = 1
marker.color.a = 1
marker.lifetime = rospy.Duration(2000)
marker.frame_locked = False
point = Point()
point.x = msg.pose.pose.position.x
point.y = msg.pose.pose.position.y
point.z = msg.pose.pose.position.z
gpspoints.append(point)
marker.points.extend(gpspoints)
vis_pub.publish(marker)
def draw_chain_links(self, detections):
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = "/world"
marker.ns = "chain_links"
for i in range(len(self.chain_links)):
marker.id = 10 + i
marker.type = 2 # SPHERE
marker.action = 0 # Add/Modify an object
marker.pose.position.x = self.chain_links[i][0]
marker.pose.position.y = self.chain_links[i][1]
marker.pose.position.z = self.chain_links[i][2]
marker.scale.x = 0.7
marker.scale.y = 0.35
marker.scale.z = 0.1
marker.color.r = 1.0
if detections[i]:
marker.color.g = 0.0
marker.color.b = 0.0
else:
marker.color.g = 0.5
marker.color.b = 0.5
marker.color.a = 0.3
marker.lifetime = rospy.Duration(1)
marker.frame_locked = False
self.pub_aris_footprint.publish(marker)
def trueMarkerCallback(msg):
markerArray = MarkerArray()
for detect in msg.tracked_obj_arr:
marker1 = Marker()
marker1.header.stamp = rospy.Time.now()
marker1.header.frame_id = "/world"
marker1.id = detect.object_id
marker1.type = marker1.SPHERE
marker1.action = marker1.ADD
marker1.pose.position.x = detect.point.x
marker1.pose.position.y = detect.point.y
marker1.pose.position.z = detect.point.z
marker1.pose.orientation.w = 1
marker1.scale.x = sphere_rad * 2
marker1.scale.y = sphere_rad * 2
marker1.scale.z = sphere_rad * 2
marker1.color.r = 1.0
marker1.color.g = 0
marker1.color.b = 0
marker1.color.a = 0.7
marker1.lifetime = rospy.Duration.from_sec(0)
marker1.frame_locked = 0
markerArray.markers.append(marker1)
pub.publish(markerArray)
def publish_target_marker(self, target_pose):
# Publish Target RViz Marker
t_marker = Marker()
t_marker.type = 2 #=>SPHERE
t_marker.scale.x = 0.3
t_marker.scale.y = 0.3
t_marker.scale.z = 0.3
t_marker.action = 0
t_marker.frame_locked = 1
t_marker.pose.position.x = target_pose[0]
t_marker.pose.position.y = target_pose[1]
t_marker.pose.position.z = 0.0
rpy_orientation = PyKDL.Rotation.RPY(0.0, 0.0, target_pose[2])
q_orientation = rpy_orientation.GetQuaternion()
t_marker.pose.orientation.x = q_orientation[0]
t_marker.pose.orientation.y = q_orientation[1]
t_marker.pose.orientation.z = q_orientation[2]
t_marker.pose.orientation.w = q_orientation[3]
t_marker.id = 0
t_marker.header.stamp = rospy.Time.now()
t_marker.header.frame_id = self.path_frame
t_marker.color.a = 1.0
t_marker.color.r = 0.0 # red
t_marker.color.g = 1.0
t_marker.color.b = 0.0
self.target_pub.publish(t_marker)
def add_marker(self, mesh_frame, marker_name, mesh_file):
marker_msg = Marker()
marker_msg.frame_locked = True
marker_msg.id = 0
marker_msg.action = Marker.ADD
marker_msg.mesh_use_embedded_materials = False
marker_msg.color.a = 1.0
marker_msg.color.r = 0.6
marker_msg.color.g = 0.6
marker_msg.color.b = 0.6
marker_msg.header.stamp = rospy.get_rostime()
#marker_msg.lifetime =
#marker_msg.pose =
marker_msg.type = Marker.MESH_RESOURCE
marker_msg.header.frame_id = mesh_frame
marker_msg.ns = marker_name
marker_msg.mesh_resource = 'file://%s' % (os.path.abspath(mesh_file))
scale = 1.0
marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale, scale, scale
marker_msg.pose.position.x, marker_msg.pose.position.y, marker_msg.pose.position.z = 0, 0, 0
marker_msg.pose.orientation.x, marker_msg.pose.orientation.y, marker_msg.pose.orientation.z, marker_msg.pose.orientation.w = 0, 0, 0, 1
self.pub_marker_sync(marker_msg)
if not self.marker_thread:
self.marker_thread = thread.start_new_thread(
MarkerPublisher.publish_markers, (self, ))
def SendSaliencyMarker(self, frame_id, ts, csaliency_id, ns, direction):
# the arrow
marker = Marker()
marker.header.frame_id = frame_id
marker.header.stamp = ts
marker.id = csaliency_id
marker.ns = ns
marker.type = Marker.ARROW
marker.action = Marker.MODIFY
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 = 0.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.01
marker.scale.y = 0.03
marker.scale.z = 0.08
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
marker.color.a = 0.5
points = []
points.append(Point(0.0, 0.0, 0.0))
points.append(Point(direction.x, direction.y, direction.z))
marker.points = points
marker.lifetime = rospy.Duration.from_sec(0.1)
marker.frame_locked = False
self.saliency_rviz_pub.publish(marker)
def callback(msg):
marker = Marker()
marker.header = msg.header
marker.header.frame_id = "map"
marker.ns = 'obstacles'
marker.id = 0
marker.type = Marker.SPHERE_LIST
marker.action = Marker.MODIFY
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.r = 0
marker.color.g = 1
marker.color.b = 0
marker.color.a = 1
marker.lifetime = rospy.Duration(2)
marker.frame_locked = False
for obstacle in msg.obstacles:
point = Point()
point.x = obstacle.odom.pose.pose.position.x
point.y = obstacle.odom.pose.pose.position.y
point.z = 0
marker.points.append(point)
vis_pub.publish(marker)
def create_marker(self, type, dims, frame, ns, id, duration = 60., color = [1,0,0], opaque = 0.5, pos = [0.,0.,0.], quat = [0.,0.,0.,1.], frame_locked = False):
marker = Marker()
marker.header.frame_id = frame
marker.header.stamp = rospy.Time.now()
marker.ns = ns
marker.type = type
marker.action = Marker.ADD
marker.scale.x = dims[0]
marker.scale.y = dims[1]
marker.scale.z = dims[2]
marker.color.a = opaque
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.lifetime = rospy.Duration(duration)
marker.id = id
marker.pose.position.x = pos[0]
marker.pose.position.y = pos[1]
marker.pose.position.z = pos[2]
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.frame_locked = frame_locked
return marker
def get_all_markers(self, frame_id="map"):
if len(self.points) > 0:
wx, wy, wz = getDims(self.points[0], self.points[1])
else:
wx, wy, wz = 0, 0, 0
marker = Marker()
clr_ = create_color(1.0, 0, 0, 1.0)
pts = [
center(mn, mx)
for mn, mx in zip(self.points[0::2], self.points[1::2])
]
colors = [clr_ for x in pts]
marker.type = marker.CUBE_LIST
marker.action = marker.ADD
marker.scale.x = abs(wx)
marker.scale.y = abs(wy)
marker.scale.z = abs(wz)
marker.color.a = 1.0
marker.color.g = 1.0
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.pose.orientation.w = 1.0
marker.points = pts
marker.colors = colors
marker.frame_locked = True
marker.header.frame_id = frame_id
return [marker]
def make_sphere_marker(self, xy, radius, color, namespace):
# type: ([int,int], float, ColorRGBA, str) -> Marker
""" Function to generate a spherical marker
ARGUMENTS:
x, y: coordinates of point in grid space
radius: float
color: ColorRGBA
mark_id: unique id
off_x, off_y: Origin of map coordinates in world frame """
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = 'map'
marker.ns = namespace
marker.id = self.mark_id
self.mark_id += 1
marker.type = 2
marker.action = 0
marker.pose = geometry_msgs.msg.Pose()
marker.pose.orientation.w = 1
marker.pose.position.x = xy[0]
marker.pose.position.y = xy[1]
marker.scale = geometry_msgs.msg.Vector3(radius, radius, radius)
marker.color = color
marker.lifetime = rospy.rostime.Duration(0)
marker.frame_locked = True
return marker
def on_feet_msg(self, feet_msg):
marker = Marker()
marker.header.frame_id = "base_link"
marker.header.stamp = rospy.Time()
marker.type = Marker.SPHERE_LIST
marker.action = Marker.ADD
marker.pose.orientation.w = 1.
marker.scale = Vector3(0.02, 0.02, 0.02)
marker.lifetime = rospy.Duration(0)
marker.frame_locked = True
def vector_to_point(vector):
return Point(vector.x, vector.y, vector.z)
def lookup_for_foot(link_name, sensor_triggered):
position = Point()
try:
position = self.tf_buffer.lookup_transform(
marker.header.frame_id, link_name,
rospy.Time()).transform.translation
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
pass
marker.points.append(vector_to_point(position))
marker.colors.append(
ColorRGBA(0, 1, 0, 1
) if sensor_triggered else ColorRGBA(1, 0, 0, 1))
lookup_for_foot("left_front_button", feet_msg.left_front)
lookup_for_foot("left_middle_button", feet_msg.left_middle)
lookup_for_foot("left_rear_button", feet_msg.left_rear)
lookup_for_foot("right_front_button", feet_msg.right_front)
lookup_for_foot("right_middle_button", feet_msg.right_middle)
lookup_for_foot("right_rear_button", feet_msg.right_rear)
self.marker_publisher.publish(marker)
def get_centerline_markers(self, scene, nusc_map, stamp):
pose_lists = nusc_map.discretize_centerlines(1)
bbox = self.scene_bounding_box(scene, nusc_map)
contained_pose_lists = []
for pose_list in pose_lists:
new_pose_list = []
for pose in pose_list:
if self.rectContains(bbox, pose):
new_pose_list.append(pose)
if len(new_pose_list) > 0:
contained_pose_lists.append(new_pose_list)
msg = MarkerArray()
for i, pose_list in enumerate(contained_pose_lists):
marker = Marker()
marker.header.frame_id = 'map'
marker.header.stamp = stamp
marker.ns = 'centerline'
marker.id = i
marker.type = Marker.LINE_STRIP
marker.action = Marker.ADD
marker.frame_locked = True
marker.scale.x = 0.1
marker.color.r = 51.0 / 255.0
marker.color.g = 160.0 / 255.0
marker.color.b = 44.0 / 255.0
marker.color.a = 1.0
marker.pose.orientation.w = 1.0
for pose in pose_list:
marker.points.append(Point(pose[0], pose[1], 0))
msg.markers.append(marker)
return msg
def publish_target_marker(self, target_pose):
t_marker = Marker()
t_marker.type = 1 # =>CUBE
t_marker.action = 0
t_marker.frame_locked = 1
t_marker.pose.position.x = target_pose[0]
t_marker.pose.position.y = target_pose[1]
t_marker.pose.position.z = target_pose[2]
rpy_orientation = PyKDL.Rotation.RPY(target_pose[3], target_pose[4],
target_pose[5])
q_orientation = rpy_orientation.GetQuaternion()
t_marker.pose.orientation.x = q_orientation[0]
t_marker.pose.orientation.y = q_orientation[1]
t_marker.pose.orientation.z = q_orientation[2]
t_marker.pose.orientation.w = q_orientation[3]
t_marker.scale.x = 0.1
t_marker.scale.y = 0.1
t_marker.scale.z = 0.1
t_marker.id = 0
t_marker.header.stamp = rospy.Time.now()
t_marker.header.frame_id = self.reference_frame
t_marker.color.a = 0.7
t_marker.color.r = 1.0 # red
t_marker.color.g = 0.0
t_marker.color.b = 0.0
self.target_pub.publish(t_marker)
def make_id_marker(self):
self.id_marker = MarkerArray()
time = rospy.get_rostime()
for node in self.map_data['NODE']:
m = Marker()
m.ns = "id"
m.header.frame_id = self.map_data['MAP_FRAME']
m.header.stamp = time
m.id = node['id']
m.action = Marker().ADD
m.type = Marker().TEXT_VIEW_FACING
m.lifetime = rospy.Duration()
m.scale.z = 1.0
m.frame_locked = True
self.set_marker_position(m, node['point']['x'], node['point']['y'],
1)
self.set_marker_orientation(m, 0, 0, 0)
self.set_marker_rgb(m, 1.0, 1.0, 1.0)
m.text = str(node['id'])
self.id_marker.markers.append(m)
for n_id in self.deleted_id_list:
m = Marker()
m.ns = "id"
m.header.frame_id = self.map_data['MAP_FRAME']
m.header.stamp = time
m.id = n_id
m.action = Marker().DELETE
m.type = Marker().TEXT_VIEW_FACING
m.lifetime = rospy.Duration()
self.id_marker.markers.append(m)
def put_disk(self, disk, rod): marker = Marker() marker.header.frame_id = "/pl_base" marker.frame_locked = False marker.pose.position.x = self.rod_position[rod][0] marker.pose.position.y = self.rod_position[rod][1] marker.pose.position.z = self.position_z + DIFF_DISTANCE_BASE + 0.012 + len(self.rod_content[rod]) * 0.01 marker.ns = "basic_shapes" marker.id = disk marker.type = marker.CYLINDER marker.action = marker.ADD marker.pose.orientation.w = 1.0 marker.scale.x = DISK_SIZE[disk - 1] marker.scale.y = DISK_SIZE[disk - 1] marker.scale.z = 0.01 marker.color.r = DISK_COLOR[disk - 1][0] marker.color.g = DISK_COLOR[disk - 1][1] marker.color.b = DISK_COLOR[disk - 1][2] marker.color.a = 1.0 self.carrying_disk = -1 self.rviz_pub.publish(marker)
def showTextRobot(self, text):
marker = Marker()
marker.header.frame_id = "/base_link"
marker.frame_locked = True
marker.header.stamp = rospy.Time.now()
marker.ns = "robot_text"
marker.id = 0
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.ADD
marker.scale.x = marker.scale.y = marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = marker.color.g = 0.9
marker.color.b = 0.6
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.5
marker.text = text
self.pub_rviz.publish(marker)
def publishMarker_(self):
pose = Pose()
marker_msg = Marker()
if (self.angle_measure_ < (math.pi / 2.0)):
angle = (math.pi / 2.0) - self.angle_measure_
else:
angle = (2 * math.pi) - self.angle_measure_
angle = (math.pi / 2.0) + angle
pose.position.x = abs(self.laser_measure_ * math.sin(angle))
pose.position.y = self.laser_measure_ * math.cos(angle)
if (angle > math.pi / 2.0):
pose.position.y = pose.position.y * (-1)
pose.position.z = 0.0
pose.orientation.x = pose.orientation.y = pose.orientation.z = 0.0
pose.orientation.w = 1.0
marker_msg.header.frame_id = "base_scan"
marker_msg.ns = "bump_and_go"
marker_msg.pose = pose
marker_msg.scale.x = marker_msg.scale.y = marker_msg.scale.z = 0.1 # 10cm
marker_msg.color.r = 1.0
marker_msg.color.g = marker_msg.color.b = 0.0
marker_msg.color.a = 1.0
marker_msg.lifetime = rospy.Duration(secs=0.1)
marker_msg.type = marker_msg.SPHERE
marker_msg.action = marker_msg.ADD
marker_msg.frame_locked = False
self.marker_pub_.publish(marker_msg)
def move_disk(self, disk, rod): marker = Marker() marker.header.frame_id = "/pl_6" marker.frame_locked = True marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = 0.27965 marker.ns = "basic_shapes" marker.id = disk marker.type = marker.CYLINDER marker.action = marker.ADD marker.pose.orientation.w = 1.0 marker.scale.x = DISK_SIZE[disk - 1] marker.scale.y = DISK_SIZE[disk - 1] marker.scale.z = 0.01 marker.color.r = DISK_COLOR[disk - 1][0] marker.color.g = DISK_COLOR[disk - 1][1] marker.color.b = DISK_COLOR[disk - 1][2] marker.color.a = 1.0 self.carrying_disk = disk self.rviz_pub.publish(marker)
def markers(self):
'''
This property creates a Marker message with Sphere List for all detected frontiers.
Since this is mostly just for visualization (?), I went with Sphere List instead of a MarkerArray
'''
marker = Marker()
marker.header.frame_id = self.name
marker.header.stamp = self.map_meta.map_load_time
marker.ns = self.name + '_frontiers'
marker.id = 0
marker.frame_locked = True
marker.type = marker.SPHERE_LIST
marker.action = marker.ADD
positions = [
self.cell2pos(f['mean'][0], f['mean'][1]) for f in self.frontiers
]
marker.points = [Point(x=x, y=y) for x, y in positions]
marker.pose.orientation.w = 1.0
marker.scale.x = 0.5
marker.scale.y = 0.5
marker.scale.z = 0.1
marker.color.a = 0.5
marker.color.b = 1.0
marker.lifetime = rospy.Duration()
return marker
def publish_target_markers(self, target_poses):
marker_array = MarkerArray()
for i in range(len(target_poses)):
m = Marker()
m.type = 2 # sphere
m.scale.x = self.room_generator_params['target_size']
m.scale.y = self.room_generator_params['target_size']
m.scale.z = self.room_generator_params['target_size']
m.action = 0
m.frame_locked = 1
m.pose.position.x = target_poses[i][0]
m.pose.position.y = target_poses[i][1]
m.pose.position.z = 0.0
rpy_orientation = PyKDL.Rotation.RPY(0, 0, target_poses[i][2])
q_orientation = rpy_orientation.GetQuaternion()
m.pose.orientation.x = q_orientation[0]
m.pose.orientation.y = q_orientation[1]
m.pose.orientation.z = q_orientation[2]
m.pose.orientation.w = q_orientation[3]
m.id = i
m.header.stamp = rospy.Time.now()
m.header.frame_id = 'map'
m.color.a = 1.0
m.color.r = 0.0
m.color.g = 1.0
m.color.b = 1.0
marker_array.markers.append(m)
self.target_pub.publish(marker_array)
def create_mesh(self, mesh_id, pose):
response = self.get_mesh(mesh_id)
mesh = response.mesh
# build the mesh marker
marker = Marker()
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 0.66
marker.frame_locked = False
marker.type = Marker.TRIANGLE_LIST
# add the mesh
for j in range(len(mesh.triangles)):
marker.points.append(
mesh.vertices[mesh.triangles[j].vertex_indices[0]])
marker.points.append(
mesh.vertices[mesh.triangles[j].vertex_indices[1]])
marker.points.append(
mesh.vertices[mesh.triangles[j].vertex_indices[2]])
# create the interactive marker
name = self.create_name(mesh_id)
self.server.insert(self.create_im(marker, pose, name),
self.process_feedback)
self.server.setCallback(name, self.release,
InteractiveMarkerFeedback.MOUSE_UP)
self.server.applyChanges()
def _default_marker(self, index=0):
m = Marker()
m.ns = self._ns
m.id = self._ids[index]
m.header.frame_id = self._frame_id
m.action = Marker.ADD
m.frame_locked = True
return m
def test():
rospy.init_node('intersect_plane_test')
marker_pub = rospy.Publisher('table_marker', Marker)
pose_pub = rospy.Publisher('pose', PoseStamped)
int_pub = rospy.Publisher('intersected_points', PointCloud2)
tfl = tf.TransformListener()
# Test table
table = Table()
table.pose.header.frame_id = 'base_link'
table.pose.pose.orientation.x, table.pose.pose.orientation.y, table.pose.pose.orientation.z, table.pose.pose.orientation.w = (0,0,0,1)
table.x_min = -0.5
table.x_max = 0.5
table.y_min = -0.5
table.y_max = 0.5
# A marker for that table
marker = Marker()
marker.header.frame_id = table.pose.header.frame_id
marker.id = 1
marker.type = Marker.LINE_STRIP
marker.action = 0
marker.pose = table.pose.pose
marker.scale.x, marker.scale.y, marker.scale.z = (0.005,0.005,0.005)
marker.color.r, marker.color.g, marker.color.b, marker.color.a = (0.0,1.0,1.0,1.0)
marker.frame_locked = False
marker.ns = 'table'
marker.points = [
Point(table.x_min,table.y_min, table.pose.pose.position.z),
Point(table.x_min,table.y_max, table.pose.pose.position.z),
Point(table.x_max,table.y_max, table.pose.pose.position.z),
Point(table.x_max,table.y_min, table.pose.pose.position.z),
Point(table.x_min,table.y_min, table.pose.pose.position.z),
]
marker.colors = []
marker.text = ''
# marker.mesh_resource = ''
marker.mesh_use_embedded_materials = False
marker.header.stamp = rospy.Time.now()
# A test pose
pose = PoseStamped()
pose.header = table.pose.header
pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = (0,0,0.5)
pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quaternion_from_euler(0,-pi/5,pi/5)
intersection = cast_ray(pose, table, tfl)
cloud = xyz_array_to_pointcloud2(np.array([[intersection.point.x, intersection.point.y, intersection.point.z]]))
cloud.header = pose.header
while not rospy.is_shutdown():
marker_pub.publish(marker)
pose_pub.publish(pose)
int_pub.publish(cloud)
rospy.loginfo('published')
rospy.sleep(0.1)
def make_marker(frame_id, id, pose, scale, color, frame_locked):
msg = Marker()
msg.header.frame_id = frame_id
msg.header.stamp = rospy.Time.now()
msg.ns = "pouring_visualization"
msg.id = id
msg.action = Marker.ADD
msg.pose = pose
msg.scale = scale
msg.color = color
msg.frame_locked = frame_locked
return msg
def CreateMeshMarker(pose, mesh_path, alpha = 1, scaleFactor=1, id=randint(0,10000)):
marker = Marker()
marker.ns = "visual"
marker.id = id
marker.scale.x = scaleFactor
marker.scale.y = scaleFactor
marker.scale.z = scaleFactor
marker.pose = pose
marker.type = Marker.MESH_RESOURCE
marker.mesh_use_embedded_materials = True
marker.mesh_resource = mesh_path
marker.frame_locked = True
return marker
def create_status_marker(self, text): marker = Marker() marker.header.frame_id = 'base_footprint' marker.header.stamp = rospy.Time.now() marker.pose.position.z += 1 marker.pose.orientation.w = 1 marker.type = Marker.TEXT_VIEW_FACING marker.text = text marker.frame_locked = True marker.ns = 'status' marker.scale.z = 0.3 marker.color.r = 0.8 marker.color.g = 0.5 marker.color.b = 0.1 marker.color.a = 1.0 return marker
def main():
rospy.init_node('field_marker_publisher_node', anonymous=True)
pub = rospy.Publisher('agent_markers', MarkerArray, queue_size=10)
agent_number = rospy.get_param('~agent_number')
print "agent_number = " + str(agent_number)
height = rospy.get_param('/agent/height')
print "height = " + str(height)
radius = rospy.get_param('/agent/radius')
print "radius = " + str(radius)
ma = MarkerArray()
#Green rectangle
m = Marker()
m.header.stamp = rospy.Time.now()
m.header.frame_id = "base_footprint_agent" + str(agent_number)
m.ns = "agent"
m.id = 0
m.type = 3
m.frame_locked = True
m.pose.orientation.w = 1
m.pose.position.z = 0
m.scale.x = radius
m.scale.y = radius
m.scale.z = height
m.color.r = 0.2
m.color.g = 0.2
m.color.b = 0.2
m.color.a = 1
ma.markers.append(copy.deepcopy(m))
rate = rospy.Rate(1)
while not rospy.is_shutdown():
print "in cycle"
pub.publish(ma)
##rospy.loginfo(t)
rate.sleep()
def draw_grasps(self, grasps, frame, ns = 'grasps', pause = 0, frame_locked = False):
marker = Marker()
marker.header.frame_id = frame
marker.header.stamp = rospy.Time.now()
marker.ns = ns
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.color.a = 1.0
marker.lifetime = rospy.Duration(0)
marker.frame_locked = frame_locked
for (grasp_num, grasp) in enumerate(grasps):
if grasp_num == 0:
marker.scale.x = 0.015
marker.scale.y = 0.025
length_fact = 1.5
else:
marker.scale.x = 0.01
marker.scale.y = 0.015
length_fact = 1.0
orientation = grasp.orientation
quat = [orientation.x, orientation.y, orientation.z, orientation.w]
mat = tf.transformations.quaternion_matrix(quat)
start = [grasp.position.x, grasp.position.y, grasp.position.z]
x_end = list(mat[:,0][0:3]*.05*length_fact + scipy.array(start))
y_end = list(mat[:,1][0:3]*.02*length_fact + scipy.array(start))
marker.id = grasp_num*3
marker.points = [Point(*start), Point(*x_end)]
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
self.marker_pub.publish(marker)
marker.id = grasp_num*3+1
marker.points = [Point(*start), Point(*y_end)]
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
self.marker_pub.publish(marker)
marker.id = grasp_num*3+2
if pause:
print "press enter to continue"
raw_input()
time.sleep(.5)
def _init_markers(self):
self.marker_idx = 0
for i in range(self.max_markers):
marker = Marker()
marker.header.frame_id = self.global_frame
marker.id = self.marker_idx
marker.type = 2
marker.action = 2
marker.pose = Pose()
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 0.0
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.frame_locked = False
marker.ns = "Goal-%u"%i
self.marker_array_msg.markers.append(marker)
def initMarker(baseFrameId="world", stamp=None, markerNamespace=None, markerId=0, markerType=None, markerAction=Marker.ADD, markerDuration=rospy.Duration()):
marker = Marker()
# cframes.header.frame_id = baseFrameId;
marker.header.frame_id = baseFrameId
# marker.header.stamp = timestamp; // TODO: check on this...not sure this is right
if stamp is None:
stamp = rospy.Time.now()
marker.header.stamp = stamp
# marker.ns = "/vertices";
if not markerNamespace is None:
marker.ns = markerNamespace
# marker.id = 0;
marker.id = markerId
# marker.type = visualization_msgs::Marker::LINE_LIST;
if not markerType is None:
marker.type = markerType
# marker.action = visualization_msgs::Marker::ADD;
marker.action = markerAction
# marker.lifetime = ros::Duration();
marker.lifetime = markerDuration
# marker.pose.position.x = marker.pose.position.y = marker.pose.position.z = 0.0;
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
# marker.pose.orientation.x = marker.pose.orientation.y = marker.pose.orientation.z = 0.0;
# marker.pose.orientation.w = 1.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.scale.x = marker.scale.y = marker.scale.z = 0.01;
marker.scale.x = 1.0;
marker.scale.y = 1.0;
marker.scale.z = 1.0;
# marker.color.r = marker.color.g = marker.color.b = marker.color.a = 1.f;
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
# marker.frame_locked = false;
marker.frame_locked = False
return marker
def _append_waypoint_pose(self,pose):
print pose
self.waypoints.append(pose)
marker = Marker()
marker.header.frame_id = self.global_frame
marker.id = self.marker_idx
marker.type = 2
marker.action = 0
marker.pose = pose
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.ns = "Goal-%u"%self.marker_idx
self.marker_array_msg.markers[self.marker_idx] = marker
self.marker_idx+=1
def addText(self, text, pos):
oid = 1
name = "/text"+str(oid)
marker = Marker()
marker.id = oid
marker.ns = "/text"
marker.header.frame_id = "/map"
marker.type = marker.TEXT_VIEW_FACING
marker.action = marker.ADD
marker.scale.z = 0.2
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 = pos[0]
marker.pose.position.y = pos[1]
marker.pose.position.z = pos[2]
marker.text = text
marker.frame_locked=1
self.markerArray.markers.append(marker)
def create_mesh(self, mesh_id, pose):
response = self.get_mesh(mesh_id)
mesh = response.mesh
# build the mesh marker
marker = Marker()
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 0.66
marker.frame_locked = False
marker.type = Marker.TRIANGLE_LIST
# add the mesh
for j in range(len(mesh.triangles)):
marker.points.append(mesh.vertices[mesh.triangles[j].vertex_indices[0]])
marker.points.append(mesh.vertices[mesh.triangles[j].vertex_indices[1]])
marker.points.append(mesh.vertices[mesh.triangles[j].vertex_indices[2]])
# create the interactive marker
name = self.create_name(mesh_id)
self.server.insert(self.create_im(marker, pose, name), self.process_feedback)
self.server.setCallback(name, self.release, InteractiveMarkerFeedback.MOUSE_UP)
self.server.applyChanges()
def Reset(self, event):
#with self.locker:
dur = rospy.Duration(6)
rospy.sleep(dur)
InitData = Path()
InitData.header.seq = self.seq_num
InitData.header.stamp = rospy.Time.now()
InitData.header.frame_id = self.frame_id
self.path.poses=[]
Initmark = Marker()
Initmark.header.frame_id = self.frame_id
Initmark.ns = self.root_topic
Initmark.type = Marker.POINTS
Initmark.action = Marker.ADD
Initmark.lifetime = rospy.Duration(1)
Initmark.frame_locked = 1
self.path_mark.points=[]
self.path_mark.colors=[]
self.path_pub.publish(InitData)
self.path_marker.publish(Initmark)
def publishCylinder(pos, ori, scale, color, id): m = Marker() m.id = id m.ns = frame_name m.action = m.ADD m.type = m.CYLINDER m.header.stamp = rospy.Time.now() m.header.frame_id = frame_name m.lifetime = rospy.Duration(4.0) m.color = color m.pose.position = Point(*pos) m.pose.orientation = Quaternion(*ori) m.scale = Vector3(*scale) m.frame_locked = True pub.publish(m)
def callback(msgs):
"msgs = ContactStatesStamped"
global g_config
if g_config.use_parent_link:
urdf_robot = URDF.from_parameter_server()
marker_array = MarkerArray()
for msg, i in zip(msgs.states, range(len(msgs.states))):
marker = Marker()
link_name = msg.header.frame_id
if g_config.use_parent_link:
# lookup parent link
chain = urdf_robot.get_chain(urdf_robot.get_root(), link_name)
link_name = chain[-3]
mesh_file, offset = find_mesh(link_name)
marker.header.frame_id = link_name
marker.header.stamp = rospy.Time.now()
marker.type = Marker.MESH_RESOURCE
if msg.state.state == ContactState.ON:
marker.color.a = g_config.on_alpha
marker.color.r = g_config.on_red
marker.color.g = g_config.on_green
marker.color.b = g_config.on_blue
elif msg.state.state == ContactState.OFF:
marker.color.a = g_config.off_alpha
marker.color.r = g_config.off_red
marker.color.g = g_config.off_green
marker.color.b = g_config.off_blue
marker.scale.x = g_config.marker_scale
marker.scale.y = g_config.marker_scale
marker.scale.z = g_config.marker_scale
marker.pose = offset
marker.mesh_resource = mesh_file
marker.frame_locked = True
marker.id = i
if msg.state.state == ContactState.OFF:
if not g_config.visualize_off:
marker.action = Marker.DELETE
marker_array.markers.append(marker)
pub.publish(marker_array)
def talker():
pub = rospy.Publisher('marker_chatter', Marker)
rospy.init_node('marker_talker')
while not rospy.is_shutdown():
m = Marker()
m.header.frame_id = '/camera_rgb_optical_frame'
m.type = 10
m.id = 2
m.action = 0
m.scale.x = 1
m.scale.y = 1
m.scale.z = 1
m.color.r = 0.3
m.color.g = 1
m.color.b = 0.2
m.color.a = 0.7
m.lifetime.secs = 1
m.frame_locked = False
m.mesh_resource= 'http://localhost:5984/object_recognition/cd4c27a2dd307cda79b49ee31494ba08/mesh.stl'
m.mesh_use_embedded_materials= False
pub.publish(m)
rospy.sleep(1.0)
def moveit_to_marker_array(msg):
ma = MarkerArray()
for pid, prim in enumerate(msg.primitives):
m = Marker()
m.ns = msg.id
m.id = pid
m.frame_locked = True
m.lifetime = rospy.Duration(2.0)
m.color = ColorRGBA(
21.0/255.0,
150.0/255.0,
118.0/255.0,
1.0)
if prim.type is SolidPrimitive.BOX:
m.type = Marker.CUBE
m.scale.x = prim.dimensions[SolidPrimitive.BOX_X]
m.scale.y = prim.dimensions[SolidPrimitive.BOX_Y]
m.scale.z = prim.dimensions[SolidPrimitive.BOX_Z]
elif prim.type is SolidPrimitive.SPHERE:
m.type = Marker.SPHERE
m.scale.x = 2*prim.dimensions[SolidPrimitive.SPHERE_RADIUS]
m.scale.y = 2*prim.dimensions[SolidPrimitive.SPHERE_RADIUS]
m.scale.z = 2*prim.dimensions[SolidPrimitive.SPHERE_RADIUS]
elif prim.type is SolidPrimitive.CYLINDER:
m.type = Marker.CYLINDER
m.scale.x = 2*prim.dimensions[SolidPrimitive.CYLINDER_RADIUS]
m.scale.y = 2*prim.dimensions[SolidPrimitive.CYLINDER_RADIUS]
m.scale.z = prim.dimensions[SolidPrimitive.CYLINDER_HEIGHT]
else:
rospy.logerr("Can't convert moveit collision object type: {}".format(prim.type))
continue
ma.markers.append(m)
return ma
def publishCube(pos, ori, scale, id):
m = Marker()
m.id = id
m.ns = "spatula"
m.action = m.ADD
m.type = m.CUBE
m.header.stamp = rospy.Time.now()
m.header.frame_id = frame_name
m.lifetime.secs = 0
m.lifetime.nsecs = 0
m.color = ColorRGBA(0.3, 0.3, 0.3, 1)
m.pose.position = Point(*pos)
m.pose.orientation = Quaternion(*ori)
m.scale = Vector3(*scale)
m.frame_locked = True
pub.publish(m)
def callback(data):
global listener
try:
p = listener.transformPose('world', data)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
return
m = Marker()
m.header = data.header
m.header.frame_id = 'world'
# m.header.stamp = rospy.get_time()
m.ns = 'ncvrl'
m.id = 0
m.type = 0
m.pose.position.x = p.pose.position.x
m.pose.position.y = p.pose.position.y
m.pose.position.z = p.pose.position.z + 0.3
m.pose.orientation.x = 0
m.pose.orientation.y = 1.0
m.pose.orientation.z = 0.0
m.pose.orientation.w = 1.0
m.scale.x = 0.2
m.scale.y = 0.1
m.scale.z = 0.1
m.color.a = 0.4
m.color.r = 0.0
m.color.g = 1.0
m.color.b = 0.0
m.frame_locked = 1
pub.publish(m);
def callback(msgs):
"msgs = ContactStatesStamped"
marker_array = MarkerArray()
for msg, i in zip(msgs.states, range(len(msgs.states))):
marker = Marker()
mesh_file, offset = find_mesh(msg.header.frame_id)
marker.header.frame_id = msg.header.frame_id
marker.header.stamp = rospy.Time.now()
marker.type = Marker.MESH_RESOURCE
marker.color.a = alpha
marker.color.r = rgb[0]
marker.color.g = rgb[1]
marker.color.b = rgb[2]
marker.scale.x = scale
marker.scale.y = scale
marker.scale.z = scale
marker.pose = offset
marker.mesh_resource = mesh_file
marker.frame_locked = True
marker.id = i
if msg.state.state == ContactState.OFF:
marker.action = Marker.DELETE
marker_array.markers.append(marker)
pub.publish(marker_array)
def __init__(self, input_ref_frame_id, input_frame_id):
self.input_ref_frame_id = input_ref_frame_id
self.input_frame_id = input_frame_id
# Parameters
self.tip_link = rospy.get_param('~tip_link')
self.cmd_frame_id = rospy.get_param('~cmd_frame', 'wam/cmd')
self.scale = rospy.get_param('~scale', 1.0)
self.use_hand = rospy.get_param('~use_hand', True)
# TF structures
self.last_time_check = rospy.Time.now()
self.listener = tf.TransformListener()
self.broadcaster = tf.TransformBroadcaster()
# State
self.cmd_origin = kdl.Frame()
self.tip_origin = kdl.Frame()
# Command state
self.cmd_frame = None
self.deadman_engaged = False
self.engage_augmenter = False
self.cmd_scaling = 0.0
# Hand structures
if self.use_hand:
# Hand state
self.hand_cmd = oro_barrett_msgs.msg.BHandCmd()
self.last_hand_cmd = rospy.Time.now()
self.hand_position = [0, 0, 0, 0]
self.hand_velocity = [0, 0, 0, 0]
self.move_f = [True, True, True]
self.move_spread = True
self.move_all = True
# Hand joint state and direct command
self.hand_joint_state_sub = rospy.Subscriber(
'hand/joint_states',
sensor_msgs.msg.JointState,
self.hand_state_cb)
self.hand_pub = rospy.Publisher('hand/cmd', oro_barrett_msgs.msg.BHandCmd, queue_size=1000)
# ROS generic telemanip command
self.telemanip_cmd_pub = rospy.Publisher('telemanip_cmd_out', TelemanipCommand, queue_size=1000)
# goal marker
self.marker_pub = rospy.Publisher('master_target_markers', MarkerArray, queue_size=1000)
self.master_target_markers = MarkerArray()
self.color_blue = ColorRGBA(0.0, 0.66, 1.0, 1.0)
self.color_gray = ColorRGBA(0.5, 0.5, 0.5, 1.0)
self.color_orange = ColorRGBA(1.0, 0.5, 0.0, 1.0)
self.color_green = ColorRGBA(0.0, 1.0, 0.0, 1.0)
self.color_red = ColorRGBA(1.0, 0.0, 0.0, 1.0)
m = Marker()
m.header.frame_id = self.cmd_frame_id
m.ns = 'finger_marker'
m.id = 0
m.type = m.MESH_RESOURCE
m.action = 0
p = m.pose.position
o = m.pose.orientation
p.x, p.y, p.z = finger_point(0.0)
o.x, o.y, o.z, o.w = (0, 0, 0, 1.0)
m.color = self.color_gray
m.scale.x = m.scale.y = m.scale.z = 0.02
m.frame_locked = True
m.mesh_resource = 'package://lcsr_barrett/models/teleop_finger_marker.dae'
m.mesh_use_embedded_materials = False
self.master_target_markers.markers.append(m)
m = Marker()
m.header.frame_id = self.cmd_frame_id
m.ns = 'finger_marker'
m.id = 1
m.type = m.MESH_RESOURCE
m.action = 0
p = m.pose.position
o = m.pose.orientation
p.x, p.y, p.z = finger_point(0.0)
o.x, o.y, o.z, o.w = (0, 0, 0, 1.0)
m.color = self.color_gray
m.scale.x = m.scale.y = m.scale.z = 0.02
m.frame_locked = True
m.mesh_resource = 'package://lcsr_barrett/models/teleop_finger_marker.dae'
m.mesh_use_embedded_materials = False
self.master_target_markers.markers.append(m)
m = Marker()
m.header.frame_id = self.cmd_frame_id
m.ns = 'finger_marker'
m.id = 2
m.type = m.MESH_RESOURCE
m.action = 0
p = m.pose.position
o = m.pose.orientation
p.x, p.y, p.z = (0.0, 0.08, -0.08)
o.x, o.y, o.z, o.w = (0, 0, 0, 1.0)
m.color = self.color_gray
m.scale.x = m.scale.y = m.scale.z = 0.02
m.frame_locked = True
m.mesh_resource = 'package://lcsr_barrett/models/teleop_finger_marker.dae'
m.mesh_use_embedded_materials = False
self.master_target_markers.markers.append(m)
m = Marker()
m.header.frame_id = self.cmd_frame_id
m.ns = 'master_target_markers'
m.id = 3
m.type = m.MESH_RESOURCE
m.action = 0
o = m.pose.orientation
o.x, o.y, o.z, o.w = (-0.7071, 0.0, 0.0, 0.7071)
m.color = self.color_gray
m.scale.x = m.scale.y = m.scale.z = 0.02
m.frame_locked = True
m.mesh_resource = 'package://lcsr_barrett/models/teleop_target.dae'
m.mesh_use_embedded_materials = False
self.master_target_markers.markers.append(m)
import sys frame_name = sys.argv[1] rate = rospy.Rate(2) counter_id = 0 while not rospy.is_shutdown(): m = Marker() m.id = counter_id m.ns = frame_name m.action = m.ADD m.type = m.MESH_RESOURCE m.header.stamp = rospy.Time.now() m.header.frame_id = frame_name m.color = ColorRGBA(255.0/255.0, 224.0/255.00, 150.0/255.0, 1) m.lifetime = rospy.Duration(1) m.mesh_resource = "package://pr2_pouring_experiments/models/dinner_plate.dae"; m.scale = Vector3(0.0254,0.0254,0.0254) m.mesh_use_embedded_materials = True m.pose.position = Point(0, 0, 0) m.pose.orientation.w = 1 m.frame_locked = True pub.publish(m) rate.sleep()
# Parse robot_description using minidom directly
# because urdf_parser_py cannot read PR2 urdf
doc = parseString(robot_description)
links = doc.getElementsByTagName('link')
mesh_file = None
for link in links:
if link_name == link.getAttribute('name'):
visual_mesh = link.getElementsByTagName('visual').item(0).getElementsByTagName('mesh').item(0)
mesh_file = visual_mesh.getAttribute('filename')
break
if not mesh_file:
raise Exception("Cannot find link: {0}".format(link_name))
pub = rospy.Publisher('~marker', Marker)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
marker = Marker()
marker.header.frame_id = link_name
marker.header.stamp = rospy.Time.now()
marker.type = Marker.MESH_RESOURCE
marker.color.a = alpha
marker.color.r = rgb[0]
marker.color.g = rgb[1]
marker.color.b = rgb[2]
marker.scale.x = scale
marker.scale.y = scale
marker.scale.z = scale
marker.mesh_resource = mesh_file
marker.frame_locked = True
pub.publish(marker)
rate.sleep()
def load_data(self, path=""):
if path == "":
path = roslib.packages.get_pkg_dir('ollieRosTools')+"/matlab/Data/exported/"
# Load Trajectory
with open(path+'trajectory/TCAM.csv', 'rb') as f:
reader = csv.reader(f)
for row in reader:
self.TCAM.append(row)
self.TCAM = np.array(self.TCAM)
self.TCAM =xyzWXYZ_xyzXYZW(self.TCAM.astype(np.double))
# Load IMU Trajectory
with open(path+'/trajectory/TOBJ.csv', 'rb') as f:
reader = csv.reader(f)
for row in reader:
self.TOBJ.append(row)
self.TOBJ = np.array(self.TOBJ)
self.TOBJ = xyzWXYZ_xyzXYZW(self.TOBJ.astype(np.double))
# Load cloud
with open(path+'points3d/cloud.csv', 'rb') as f:
reader = csv.reader(f)
for row in reader:
self.CLOUD.append(row)
self.CLOUD = np.array(self.CLOUD)
self.CLOUD = self.CLOUD.astype(np.double)
mincol = np.min(self.CLOUD,0)
maxcol = np.max(self.CLOUD,0)
col = self.CLOUD-mincol
col = col/np.abs(mincol - maxcol )
m = MarkerMSG()
m.action = m.ADD
m.frame_locked = True
m.ns = "WorldPointsGT"
m.header.frame_id = "/world"
m.type = m.POINTS
m.pose.orientation.w = 1.0
m.pose.orientation.z = 0
m.pose.orientation.y = 0
m.pose.orientation.x = 0
m.id = 0
m.pose.position.x = 0
m.pose.position.y = 0
m.pose.position.z = 0
#m.color.a = 0.6
#m.color.r = 1.0
#m.color.b = 0.0
#m.color.g = 0.0
m.scale.x = 0.05
m.scale.y = 0.05
m.scale.z = 0.05
for wp,co in zip(self.CLOUD, col):
p = PointMSG()
p.x = wp[1]
p.y = wp[2]
p.z = wp[3]
c = ColorMSG()
c.a = 0.75
c.r = co[1]
c.g = co[2]
c.b = co[3]
m.points.append(p)
m.colors.append(c)
self.CLOUD = m
# Load Camera
with open(path+'meta/cam2img.csv', 'rb') as f:
reader = csv.reader(f)
for row in reader:
self.CAM2IMG.append(row)
self.CAM2IMG = np.array(self.CAM2IMG)
self.CAM2IMG = self.CAM2IMG.astype(np.double)
self.CAM2IMG = np.array(self.CAM2IMG)
self.camInfo = CameraInfoMSG()
self.camInfo.width = self.CAM2IMG[0,2]*2
self.camInfo.height = self.CAM2IMG[1,2]*2
self.camInfo.distortion_model = "plumb_bob"
self.camInfo.P = self.CAM2IMG.flatten()
self.camInfo.K = self.CAM2IMG[0:3,0:3].flatten()
self.camInfo.R = np.eye(3).flatten()
self.camInfo.D = [0.,0.,0.,0.,0.]
self.camInfo.header.frame_id = "/synCamGT"
# Load IMU Cam transform
with open(path+'meta/imu2cam.csv', 'rb') as f:
reader = csv.reader(f)
for row in reader:
self.OBJ2CAM.append(row)
self.OBJ2CAM = np.array(self.OBJ2CAM)
self.OBJ2CAM = xyzWXYZ_xyzXYZW(self.OBJ2CAM.astype(np.double))[0]
self.OBJ2CAMmsg = toTransformMsg(self.OBJ2CAM)
#Load KPS
files = sorted(glob.glob(path+'features/f2d*.csv'))
for file in files:
f2dmsg = []
with open(file, 'rb') as fi:
reader = csv.reader(fi)
f2d = []
for row in reader:
f2d.append(row)
f2d = np.array(f2d)
f2d = f2d.astype(np.double)
for f in f2d:
p= PointMSG()
p.x = f[1]
p.y = f[2]
p.z = f[0] #id
f2dmsg.append(p)
self.KPS.append(f2dmsg)
#Load images
files = sorted(glob.glob(path+'images/img*.png'))
for file in files:
self.IMAGES.append(cv2.imread(file))
