def publishPositions(self):
if not self.initialized:
return
nr = 0
markerArray = MarkerArray()
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.MESH_RESOURCE
marker.action = marker.ADD
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.mesh_use_embedded_materials = True
marker.mesh_resource = "package://pacman_controller/meshes/pacman.dae"
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.pose.orientation = self.pacman['orientation']
marker.pose.position.x = self.pacman['x']
marker.pose.position.y = self.pacman['y']
marker.pose.position.z = 0.0
marker.id = nr
markerArray.markers.append(marker)
for ghost in self.ghosts:
curGhost = self.ghosts[ghost]
if not curGhost["initialized"]:
continue
nr += 1
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.MESH_RESOURCE
marker.action = marker.ADD
marker.scale.x = 0.3
marker.scale.y = 0.3
marker.scale.z = 0.3
marker.mesh_use_embedded_materials = True
if curGhost["eaten"]:
marker.mesh_resource = "package://pacman_controller/meshes/dead.dae"
elif self.state == State.FLEEING:
marker.mesh_resource = "package://pacman_controller/meshes/ghost_catchable.dae"
else:
marker.mesh_resource = "package://pacman_controller/meshes/%s.dae" % ghost
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.pose.orientation = curGhost['orientation']
marker.pose.position.x = curGhost['x']
marker.pose.position.y = curGhost['y']
marker.pose.position.z = 0.0
marker.id = nr
markerArray.markers.append(marker)
self.pubPositions.publish(markerArray)
return
def publishPositions(self):
if not self.initialized:
return
nr = 0
markerArray = MarkerArray()
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.MESH_RESOURCE
marker.action = marker.ADD
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.mesh_use_embedded_materials = True
marker.mesh_resource = "package://pacman_controller/meshes/pacman.dae"
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.pose.orientation = self.pacman["orientation"]
marker.pose.position.x = self.pacman["x"]
marker.pose.position.y = self.pacman["y"]
marker.pose.position.z = 0.0
marker.id = nr
markerArray.markers.append(marker)
for ghost in self.ghosts:
curGhost = self.ghosts[ghost]
if not curGhost["initialized"]:
continue
nr += 1
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.MESH_RESOURCE
marker.action = marker.ADD
marker.scale.x = 0.3
marker.scale.y = 0.3
marker.scale.z = 0.3
marker.mesh_use_embedded_materials = True
if curGhost["eaten"]:
marker.mesh_resource = "package://pacman_controller/meshes/dead.dae"
elif self.state == State.FLEEING:
marker.mesh_resource = "package://pacman_controller/meshes/ghost_catchable.dae"
else:
marker.mesh_resource = "package://pacman_controller/meshes/%s.dae" % ghost
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.pose.orientation = curGhost["orientation"]
marker.pose.position.x = curGhost["x"]
marker.pose.position.y = curGhost["y"]
marker.pose.position.z = 0.0
marker.id = nr
markerArray.markers.append(marker)
self.pubPositions.publish(markerArray)
return
def createMarker(x_pos, z_pos):
# gen marker
gripper_center = Marker()
gripper_left = Marker()
gripper_right = Marker()
#mesh resource
gripper_center.type = Marker.MESH_RESOURCE
gripper_left.type = Marker.MESH_RESOURCE
gripper_right.type = Marker.MESH_RESOURCE
# add resource
gripper_center.mesh_resource = GRIPPER_MESH
gripper_left.mesh_resource = L_FINGER_MESH
gripper_right.mesh_resource = R_FINGER_MESH
# add color to gripper
gripper_center.color.r = 1.0
gripper_left.color.r = 1.0
gripper_right.color.r = 1.0
gripper_center.color.a = 1.0
gripper_left.color.a = 1.0
gripper_right.color.a = 1.0
gripper_center.scale.x = 1.0
gripper_center.scale.y = 1.0
gripper_center.scale.z = 1.0
gripper_center.pose.position.x += GRIPPER_OFFSET + x_pos
gripper_left.pose.position.x += GRIPPER_OFFSET + x_pos
gripper_right.pose.position.x += GRIPPER_OFFSET + x_pos
gripper_center.pose.position.z += z_pos
gripper_left.pose.position.z += z_pos
gripper_right.pose.position.z += z_pos
gripper_left.pose.position.y = gripper_center.pose.position.y - 0.05
gripper_right.pose.position.y = gripper_center.pose.position.y + 0.05
return [gripper_center, gripper_left, gripper_right]
def createMarker():
gripper_center = Marker()
gripper_left = Marker()
gripper_right = Marker()
gripper_center.type = Marker.MESH_RESOURCE
gripper_left.type = Marker.MESH_RESOURCE
gripper_right.type = Marker.MESH_RESOURCE
gripper_center.mesh_resource = GRIPPER_MESH
gripper_left.mesh_resource = L_FINGER_MESH
gripper_right.mesh_resource = R_FINGER_MESH
return [gripper_center, gripper_left, gripper_right]
def init_markers(xOffset, yOffset, zOffset):
"""
Returns either 1 InteractiveMarker or list of 3 Markers
tbd
"""
gripper = Marker()
l_finger = Marker()
r_finger = Marker()
# setup meshes
gripper.type = Marker.MESH_RESOURCE
gripper.mesh_resource = GRIPPER_MESH
l_finger.type = Marker.MESH_RESOURCE
l_finger.mesh_resource = L_FINGER_MESH
r_finger.type = Marker.MESH_RESOURCE
r_finger.mesh_resource = R_FINGER_MESH
# # gripper
gripper.pose.position.x += xOffset
gripper.pose.position.y += yOffset
gripper.pose.position.z += zOffset
gripper.pose.orientation.w = 1.0
gripper.color.r = 1
gripper.color.g = 0.0
gripper.color.b = 0.0
gripper.color.a = 0.7
# # l finger stuff
l_finger.pose.position.x += xOffset
l_finger.pose.position.y += -0.05 + yOffset
l_finger.pose.position.z += zOffset
l_finger.pose.orientation.w = 1.0
l_finger.color.r = 1
l_finger.color.g = 0.0
l_finger.color.b = 0.0
l_finger.color.a = 0.7
# # r finger stuff
r_finger.pose.position.x += xOffset
r_finger.pose.position.y += 0.05 + yOffset
r_finger.pose.position.z += zOffset
r_finger.pose.orientation.w = 1.0
r_finger.color.r = 1
r_finger.color.g = 0.0
r_finger.color.b = 0.0
r_finger.color.a = 0.7
markers = [gripper, l_finger, r_finger]
return markers
def createMarkers(ps):
name1 = "left"
marker1 = Marker()
# marker1.header.frame_id = "base_link"
marker1.type = Marker.MESH_RESOURCE
marker1.mesh_resource = L_FINGER_MESH
marker1.pose = copy.deepcopy(ps.pose)
marker1.pose.position.x += 0.166
marker1.pose.position.y -= 0.05
# marker1.pose.orientation.w = 1
marker1.scale.x = 1
marker1.scale.y = 1
marker1.scale.z = 1
marker1.color.r = 0.0
marker1.color.g = 0.5
marker1.color.b = 0.5
marker1.color.a = 1.0
#marker1.markers.append(create_box_marker())
name2 = "right"
marker2 = Marker()
# marker2.header.frame_id = "base_link"
marker2.type = Marker.MESH_RESOURCE
marker2.mesh_resource = R_FINGER_MESH
marker2.pose = copy.deepcopy(ps.pose)
marker2.pose.position.x += 0.166
marker2.pose.position.y += 0.05
# marker2.pose.orientation.w = 1
marker2.scale.x = 1
marker2.scale.y = 1
marker2.scale.z = 1
marker2.color.r = 0.0
marker2.color.g = 0.5
marker2.color.b = 0.5
marker2.color.a = 1.0
name3 = "gripperer"
marker3 = Marker()
# marker3.header.frame_id = "base_link"
marker3.type = Marker.MESH_RESOURCE
marker3.mesh_resource = GRIPPER_MESH
marker3.pose = copy.deepcopy(ps.pose)
# marker3.pose.position.x += 0.166
marker3.scale.x = 1
marker3.scale.y = 1
marker3.scale.z = 1
marker3.color.r = 0.0
marker3.color.g = 0.5
marker3.color.b = 0.5
marker3.color.a = 1.0
return [marker1, marker2, marker3]
def draw_quad_base(self, quad):
quad_id = self.hash32(quad)
if not rospy.is_shutdown():
marker = Marker()
marker.header.frame_id = "/my_frame"
marker.lifetime = rospy.Duration(self.duration)
marker.type = marker.MESH_RESOURCE
marker.mesh_resource = "package://hector_quadrotor_description/"\
+ "meshes/quadrotor/quadrotor_base.dae"
marker.action = marker.ADD
marker.scale.x = 80
marker.scale.y = 80
marker.scale.z = 80
marker.mesh_use_embedded_materials = True
marker.pose.orientation.w = math.cos(
math.radians(quad.get_orientation() / 2)
)
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = math.sin(
math.radians(quad.get_orientation() / 2)
)
marker.pose.position.x = quad.get_x()
marker.pose.position.y = quad.get_y()
marker.pose.position.z = quad.get_z()
marker.id = quad_id
self.markers.append(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 get_trafficsign_marker(marker_id, x, y, q, marker_type):
if marker_type == "T-intersection":
marker_type = "T-intersect"
marker = Marker()
marker.header.frame_id = "/map"
marker.id = marker_id
marker.ns = "traffic_signs"
marker.type = marker.MESH_RESOURCE
marker.action = marker.ADD
marker.mesh_resource = "package://duckietown_visualization/meshes/traffic-signs/" + \
"sign_" + marker_type.replace('-','_') + ".dae"
marker.mesh_use_embedded_materials = True
marker.pose.position.x = x
marker.pose.position.y = y
marker.pose.position.z = -0.04
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
(_,_,yaw) = tf.transformations.euler_from_quaternion(q)
q = tf.transformations.quaternion_from_euler(0, 0, yaw-math.pi/2)
marker.pose.orientation.x = q[0]
marker.pose.orientation.y = q[1]
marker.pose.orientation.z = q[2]
marker.pose.orientation.w = q[3]
return marker
def get_apriltag_marker(x, y, angle, marker_type, marker_id, marker_scale):
marker = Marker()
marker.header.frame_id = "/map"
marker.id = marker_id
marker.ns = 'apriltags'
marker.type = marker.MESH_RESOURCE
marker.action = marker.ADD
marker.pose.position.x = x
marker.pose.position.y = y
marker.pose.position.z = -0.04
marker.scale.x = 0.065
marker.scale.y = 0.065
marker.scale.z = 1
marker.mesh_resource = "package://duckietown_visualization/meshes/apriltags/" + \
marker_type + ".dae"
marker.mesh_use_embedded_materials = True
q = quaternion_from_euler(0, 0, angle)
marker.pose.orientation.x = q[0]
marker.pose.orientation.y = q[1]
marker.pose.orientation.z = q[2]
marker.pose.orientation.w = q[3]
return marker
def main():
# Initialize ROS stuff
rospy.init_node("optimization_node")
pub = rospy.Publisher('robot_meshes',
MarkerArray,
queue_size=0,
latch=True)
# Draw the chessboard
markers = MarkerArray()
m = Marker(
header=Header(frame_id='base_link', stamp=rospy.Time.now()),
ns='charuco',
id=0,
frame_locked=True,
type=Marker.MESH_RESOURCE,
action=Marker.ADD,
lifetime=rospy.Duration(0),
pose=Pose(
position=Point(x=-0.9, y=-0.176 * 2, z=0.5),
orientation=Quaternion(
*trf.quaternion_from_euler(math.pi *
0.5, 0, math.atan2(-0.176, -1)) -
math.pi * 0.1)),
scale=Vector3(x=1.5, y=1.5, z=1.5),
color=ColorRGBA(r=.5, g=.5, b=.5, a=1))
m.mesh_resource = 'package://ur_e_description/meshes/objects/charuco_5x5.dae'
m.mesh_use_embedded_materials = True
markers.markers.append(m)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
pub.publish(markers)
rate.sleep()
def get_marker(marker_id, x, y, q):
marker = Marker()
marker.header.frame_id = "/duckiebot_link"
marker.id = marker_id
marker.ns = "duckiebots"
marker.type = marker.MESH_RESOURCE
marker.action = marker.ADD
marker.mesh_resource = "package://duckietown_visualization/meshes/duckiebot/duckiebot.dae"
marker.mesh_use_embedded_materials = True
marker.pose.position.x = x
marker.pose.position.y = y
marker.pose.position.z = 0
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.pose.orientation.x = q[0]
marker.pose.orientation.y = q[1]
marker.pose.orientation.z = q[2]
marker.pose.orientation.w = q[3]
return marker
def visualize_object(
pose,
filepath="package://config/descriptions/meshes/objects/object.stl",
name="/object",
color=(0., 0., 1., 1.),
frame_id="/yumi_body",
scale=(1., 1., 1.)):
marker_pub = rospy.Publisher(name, Marker, queue_size=1)
marker_type = Marker.MESH_RESOURCE
marker = Marker()
marker.header.frame_id = frame_id
marker.ns = name
marker.header.stamp = rospy.Time(0)
marker.action = Marker.ADD
marker.pose.orientation.w = 1.0
marker.type = marker_type
marker.scale.x = scale[0]
marker.scale.y = scale[1]
marker.scale.z = scale[2]
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.color.a = color[3]
marker.lifetime.secs = 1
marker.pose = pose.pose
marker.mesh_resource = filepath
marker.lifetime = rospy.Duration(10000)
for i in range(10):
marker_pub.publish(marker)
def getMarker(self):
marker = Marker()
ns = rospy.get_namespace()
marker.header.frame_id = rospy.get_param('~grid_frame')
marker.header.stamp = rospy.Time.now()
marker.ns = "shape_namespace"
marker.id = self.idnum
marker.type = 1
marker.action = 0
marker.pose.position.x = self.position[0]
marker.pose.position.y = self.position[1]
marker.pose.position.z = self.position[2]
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 0
marker.pose.orientation.w = 1
marker.scale.x = self.size[0]
marker.scale.y = self.size[1]
marker.scale.z = self.size[2]
marker.color.a = 0.9
marker.color.r = 1
marker.color.g = 0
marker.color.b = 0
marker.lifetime = rospy.Duration(0.6)
marker.mesh_resource = ""
return marker
def init(self):
for poseview,mesh_path,mesh_scale,mesh_rgba in zip(self.ros_pose_viewers,
self.mesh_paths,
self.mesh_scales,
self.mesh_rgbas):
marker = Marker()
marker.ns = self.namespace
marker.id = self.last_used_id+1
self.last_used_id += 1
marker.action = 0
# 10 for mesh
marker.type = 10
marker.pose = poseview.pose
x,y,z = mesh_scale
marker.scale.x = x
marker.scale.y = y
marker.scale.z = z
r,g,b,a = mesh_rgba
marker.color.a = a
marker.color.r = r
marker.color.g = g
marker.color.b = b
marker.mesh_resource = 'file://'+mesh_path
marker.header.frame_id = '/world'
self.marker_array.markers.append(marker)
def publish_shelf(publisher, pose_stamped):
"""Publishes a shelf marker at a given pose.
The pose is assumed to represent the bottom center of the shelf, with the
+x direction pointing along the depth axis of the bins and +z pointing up.
Args:
publisher: A visualization_msgs/Marker publisher
pose_stamped: A PoseStamped message with the location, orientation, and
reference frame of the shelf.
"""
marker = Marker()
marker.header.frame_id = pose_stamped.header.frame_id
marker.header.stamp = rospy.Time().now()
marker.ns = 'shelf'
marker.id = 0
marker.type = Marker.MESH_RESOURCE
marker.mesh_resource = 'package://pr2_pick_perception/models/shelf/shelf.ply'
marker.mesh_use_embedded_materials = True
marker.action = Marker.ADD
marker.pose = pose_stamped.pose
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.lifetime = rospy.Duration()
_publish(publisher, marker, "shelf")
def rosbagVisualizationMessage(bag, posX, posY, posZ, oriW, oriX, oriY, oriZ,
objType, mid, mesh):
# build object ...
msg = Marker()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '/PTU'
msg.ns = 'textured'
msg.id = mid
msg.type = 10
msg.action = 0
msg.pose.position.x = posX
msg.pose.position.y = posY
msg.pose.position.z = posZ
msg.pose.orientation.w = oriW
msg.pose.orientation.x = oriX
msg.pose.orientation.y = oriY
msg.pose.orientation.z = oriZ
msg.scale.x = 0.001
msg.scale.y = 0.001
msg.scale.z = 0.001
msg.mesh_resource = mesh
msg.mesh_use_embedded_materials = True
# ... and write it into bag file
bag.write('/stereo/visualization_marker', msg)
def publishVisualizationMessage(pub, posX, posY, posZ, oriW, oriX, oriY, oriZ,
objType, mid, mesh):
# build object ...
msg = Marker()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '/PTU'
msg.ns = 'textured'
msg.id = mid
msg.type = 10
msg.action = 0
msg.pose.position.x = posX
msg.pose.position.y = posY
msg.pose.position.z = posZ
msg.pose.orientation.w = oriW
msg.pose.orientation.x = oriX
msg.pose.orientation.y = oriY
msg.pose.orientation.z = oriZ
msg.scale.x = 0.001
msg.scale.y = 0.001
msg.scale.z = 0.001
msg.mesh_resource = mesh
msg.mesh_use_embedded_materials = True
# ... and publish it
pub.publish(msg)
def get_current_position_marker(self,
link,
offset=None,
root="",
scale=1,
color=(0, 1, 0, 1),
idx=0):
(mesh, pose) = self.get_link_data(link)
marker = Marker()
if offset == None:
marker.pose = pose
else:
marker.pose = toMsg(fromMsg(offset) * fromMsg(pose))
marker.header.frame_id = root
marker.header.stamp = rospy.get_rostime()
marker.ns = self.robot_name
marker.mesh_resource = mesh
marker.type = Marker.MESH_RESOURCE
marker.action = Marker.MODIFY
marker.scale.x = scale
marker.scale.y = scale
marker.scale.z = scale
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.color.a = color[3]
marker.text = link
marker.id = idx
marker.mesh_use_embedded_materials = True
return marker
def get_watchtower_marker(marker_id, x, y, q):
marker = Marker()
marker.header.frame_id = "/map"
marker.id = marker_id
marker.ns = "watchtowers"
marker.type = marker.MESH_RESOURCE
marker.action = marker.ADD
marker.mesh_resource = "package://duckietown_visualization/meshes/watchtower/watchtower.dae"
marker.mesh_use_embedded_materials = True
marker.pose.position.x = x
marker.pose.position.y = y
marker.pose.position.z = 0
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
(_,_,yaw) = tf.transformations.euler_from_quaternion(q)
q = tf.transformations.quaternion_from_euler(0, 0, yaw)
marker.pose.orientation.x = q[0]
marker.pose.orientation.y = q[1]
marker.pose.orientation.z = q[2]
marker.pose.orientation.w = q[3]
return marker
def publish_car_model(model_pub):
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()
marker.id = 100
marker.action = Marker.ADD
marker.lifetime = rospy.Duration()
marker.type = Marker.MESH_RESOURCE
marker.mesh_resource = "package://kitti_test/car_model/car_model.dae"
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = -1.73
q = tf.transformations.quaternion_from_euler(0, 0, -np.pi)
marker.pose.orientation.x = q[0]
marker.pose.orientation.y = q[1]
marker.pose.orientation.z = q[2]
marker.pose.orientation.w = q[3]
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
model_pub.publish(marker)
def get_apriltag_marker(marker_id, trans, q):
marker = Marker()
marker.header.frame_id = "/map"
marker.header.stamp = rospy.Time.now()
marker.id = marker_id
marker.ns = "ground_tags"
marker.type = marker.MESH_RESOURCE
marker.action = marker.ADD
marker.mesh_resource = "package://duckietown_visualization/meshes/apriltags/apriltag.dae"
marker.mesh_use_embedded_materials = True
marker.pose.position.x = trans[0]
marker.pose.position.y = trans[1]
marker.pose.position.z = trans[2]
marker.scale.x = 0.064
marker.scale.y = 0.064
marker.scale.z = 1
# (_,_,yaw) = tf.transformations.euler_from_quaternion(q)
# q = tf.transformations.quaternion_from_euler(0, 0, yaw)
marker.pose.orientation.x = q[0]
marker.pose.orientation.y = q[1]
marker.pose.orientation.z = q[2]
marker.pose.orientation.w = q[3]
return marker
def add_text(frame_id='map', id_=0, x=0, y=0, r=0, g=0, b=0, scale=0.1):
marker = Marker()
marker.header.frame_id = frame_id
marker.header.stamp = rospy.get_rostime()
marker.ns = "my_namespace"
marker.id = id_
marker.type = marker.TEXT_VIEW_FACING
marker.action = marker.ADD
marker.pose.position.x = x + 0.1
marker.pose.position.y = y + 0.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.text = "chair" + str(int(id_ / 2))
marker.scale.x = scale
marker.scale.y = scale
marker.scale.z = scale
marker.color.a = 1.0
marker.color.r = r
marker.color.g = g
marker.color.b = b
marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae"
return marker
def world_body_to_marker_msg(world_body, id=1, ns=u''):
"""
:type world_body: WorldBody
:rtype: Marker
"""
m = Marker()
m.ns = u'{}/{}'.format(ns, world_body.name)
m.id = id
if world_body.type == WorldBody.URDF_BODY:
raise Exception(
u'can\'t convert urdf body world object to marker array')
elif world_body.type == WorldBody.PRIMITIVE_BODY:
if world_body.shape.type == SolidPrimitive.BOX:
m.type = Marker.CUBE
m.scale = Vector3(*world_body.shape.dimensions)
elif world_body.shape.type == SolidPrimitive.SPHERE:
m.type = Marker.SPHERE
m.scale = Vector3(world_body.shape.dimensions[0],
world_body.shape.dimensions[0],
world_body.shape.dimensions[0])
elif world_body.shape.type == SolidPrimitive.CYLINDER:
m.type = Marker.CYLINDER
m.scale = Vector3(
world_body.shape.dimensions[SolidPrimitive.CYLINDER_RADIUS],
world_body.shape.dimensions[SolidPrimitive.CYLINDER_RADIUS],
world_body.shape.dimensions[SolidPrimitive.CYLINDER_HEIGHT])
else:
raise Exception(
u'world body type {} can\'t be converted to marker'.format(
world_body.shape.type))
elif world_body.type == WorldBody.MESH_BODY:
m.type = Marker.MESH_RESOURCE
m.mesh_resource = world_body.mesh
m.color = ColorRGBA(0, 1, 0, 0.8)
return m
def publish_ego_car(ego_car_pub): """ Publish lef/right 45 degree FOV line use Marker http://wiki.ros.org/rviz/DisplayTypes/Marker """ ##camera FOV maker_array = MarkerArray() marker = Marker() marker.header.frame_id = FRAME_ID marker.header.stamp = rospy.Time.now() marker.id = 0 #must be different marker.action = Marker.ADD marker.lifetime = rospy.Duration() #forever show in screen marker.type = Marker.LINE_STRIP #you can change line style in here marker.color.r = 0.0 # 010 = green marker.color.g = 1.0 # marker.color.b = 0.0 # marker.color.a = 1.0 # Transparent 0 ~ 1 opaque marker.scale.x = 0.2 # Thickness marker.points = [] #Connection point coordinates marker.points.append(Point(10, -10, 0)) marker.points.append(Point(0, 0, 0)) marker.points.append(Point(10, 10, 0)) maker_array.markers.append(marker) ##car model mesh_marker = Marker() mesh_marker.header.frame_id = FRAME_ID mesh_marker.header.stamp = rospy.Time.now() mesh_marker.id = -1 mesh_marker.lifetime = rospy.Duration() #forever show in screen mesh_marker.type = Marker.MESH_RESOURCE #you can change line style in here mesh_marker.mesh_resource = "package://agv_object_dection/Land Rover Range Rover Sport HSE 2010.dae" mesh_marker.pose.position.x = 0.0 # set position mesh_marker.pose.position.y = 0.0 mesh_marker.pose.position.z = -1.73 #lidar = 0 0 0 car = 0 0 -1.73 q = tf.transformations.quaternion_from_euler(np.pi/2, 0, np.pi) #ratation value mesh_marker.pose.orientation.x = q[0] mesh_marker.pose.orientation.y = q[1] mesh_marker.pose.orientation.z = q[2] mesh_marker.pose.orientation.w = q[3] mesh_marker.color.r = 1.0 # mesh_marker.color.g = 1.0 # mesh_marker.color.b = 1.0 # mesh_marker.color.a = 1.0 # Transparent 0 ~ 1 opaque mesh_marker.scale.x = 0.9 # Thickness mesh_marker.scale.y = 0.9 # Thickness mesh_marker.scale.z = 0.9 # Thickness maker_array.markers.append(mesh_marker) ego_car_pub.publish(maker_array)
def createMeshMarker(resource, offset=(0,0,0), rgba=(1,0,0,1), orientation=(0,0,0,1), scale=1, scales=(1,1,1), frame_id="/map"):
marker = Marker()
marker.mesh_resource = resource;
marker.header.frame_id = frame_id
marker.type = marker.MESH_RESOURCE
marker.scale.x = scale*scales[0]
marker.scale.y = scale*scales[1]
marker.scale.z = scale*scales[2]
marker.color.a = rgba[3]
marker.color.r = rgba[0]
marker.color.g = rgba[1]
marker.color.b = rgba[2]
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
marker.pose.position.x = offset[0]
marker.pose.position.y = offset[1]
marker.pose.position.z = offset[2]
obj_control = InteractiveMarkerControl()
obj_control.always_visible = True
obj_control.markers.append( marker )
return obj_control
def publish_people_marker(people_pos, nav_map, timestamp, publisher):
"""
Get rviz visualization marker for people poses.
:param people_pos: cell coordinates of people
:param nav_map: map of the environment
:param timestamp: ros timestamp
:param publisher: ros publisher
"""
people_markers = MarkerArray()
person_marker = Marker()
person_marker.header.frame_id = nav_map.frame_id
person_marker.header.stamp = timestamp
person_marker.type = Marker.MESH_RESOURCE
person_marker.mesh_resource = "package://learning-nav-irl/meshes/human/meshes/standing.dae"
person_marker.mesh_use_embedded_materials = True
person_marker.scale.x = 1.0
person_marker.scale.y = 1.0
person_marker.scale.z = 1.0
for i, person_pos in enumerate(people_pos):
person_marker.id = i
person_x, person_y = nav_map.cell2world(person_pos[0], person_pos[1])
person_marker.pose.position.x = person_x
person_marker.pose.position.y = person_y
# person orientation hard-coded for visualization
person_marker.pose.orientation.z = 0.7071068
person_marker.pose.orientation.w = 0.7071068
people_markers.markers.append(copy.deepcopy(person_marker))
publisher.publish(people_markers)
def copy_marker_attributes(sample):
marker = Marker()
marker.header.frame_id = sample.header.frame_id
marker.header.stamp = sample.header.stamp
marker.id = sample.id
marker.type = sample.type
marker.action = sample.action
marker.scale.x = sample.scale.x
marker.scale.y = sample.scale.y
marker.scale.z = sample.scale.z
marker.pose.position.x = sample.pose.position.x
marker.pose.position.y = sample.pose.position.y
marker.pose.position.z = sample.pose.position.z
marker.pose.orientation.x = sample.pose.orientation.x
marker.pose.orientation.y = sample.pose.orientation.y
marker.pose.orientation.z = sample.pose.orientation.z
marker.pose.orientation.w = sample.pose.orientation.w
marker.color.a = sample.color.a
marker.color.r = sample.color.r
marker.color.g = sample.color.g
marker.color.b = sample.color.b
marker.text = sample.text
marker.mesh_resource = sample.mesh_resource
marker.mesh_use_embedded_materials = sample.mesh_use_embedded_materials
marker.points = sample.points
marker.colors = sample.colors
return marker
def publish_car_model(model_pub):
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time.now()
marker.id = 0
marker.lifetime = rospy.Duration()
marker.type = Marker.MESH_RESOURCE
marker.mesh_resource = "package://kitti/bmw_x5/BMW X5 4.dae"
# marker.mesh_use_embedded_materials = True
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = -1.73
q = tf.transformations.quaternion_from_euler(np.pi / 2, 0, np.pi)
marker.pose.orientation.x = q[0]
marker.pose.orientation.y = q[1]
marker.pose.orientation.z = q[2]
marker.pose.orientation.w = q[3]
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 0.8
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
model_pub.publish(marker)
rospy.loginfo("marker published")
def callback(msg):
""""""
global rate
marker = Marker()
marker.header.frame_id = "global_tank"
marker.id = 0
marker.type = marker.MESH_RESOURCE
marker.action = marker.ADD
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.color.r = 1.0
marker.color.g = 0
marker.color.b = 0
marker.color.a = 1 # transparency
marker.pose.orientation.w = msg.pose.orientation.w
marker.pose.orientation.x = msg.pose.orientation.x
marker.pose.orientation.y = msg.pose.orientation.y
marker.pose.orientation.z = msg.pose.orientation.z
marker.pose.position.x = msg.pose.position.x # x
marker.pose.position.y = msg.pose.position.y # y
marker.pose.position.z = msg.pose.position.z # z
# marker.mesh_resource = "package://hippocampus_test/models/uuv_hippocampus.stl"
marker.mesh_resource = "package://hippocampus_common/models/uuv_hippocampus.stl"
publisher_marker.publish(marker)
def draw_grasps(grasps):
"""
Draws grasps in RVIZ
Parameters:
grasps: a list of grasps
"""
# TODO: add fingers to gripper
publisher = rospy.Publisher("visualization_marker", Marker)
for i, g in enumerate(grasps):
marker = Marker()
marker.header.frame_id = "base_link"
marker.header.stamp = rospy.Time.now()
marker.ns = "grasps"
marker.type = Marker.MESH_RESOURCE
marker.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae"
marker.action = marker.ADD
marker.id = i
marker.pose.position = g.grasp_pose.position
marker.pose.orientation = g.grasp_pose.orientation
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
marker.mesh_use_embedded_materials = True
publisher.publish(marker)
def pub_at_position(self, odom_msg):
"""
Handles necessary information for displaying things at
ACCEPTED (NOVATEL) POSITION SOLUTION:
-vehicle mesh
!!!this should only be invoked when the accepted (novatel) position
is updated
"""
### G35 Mesh #############################################################
marker = Marker()
pub = self.curpos_publisher
marker.header = odom_msg.header
marker.id = 0 # enumerate subsequent markers here
marker.action = Marker.MODIFY # can be ADD, REMOVE, or MODIFY
marker.pose = odom_msg.pose.pose
marker.pose.position.z = 1.55
marker.lifetime = rospy.Duration() # will last forever unless modified
marker.ns = "vehicle_model"
marker.type = Marker.MESH_RESOURCE
marker.scale.x = 0.0254 # artifact of sketchup export
marker.scale.y = 0.0254 # artifact of sketchup export
marker.scale.z = 0.0254 # artifact of sketchup export
marker.color.r = .05
marker.color.g = .05
marker.color.b = .05
marker.color.a = .2
marker.mesh_resource = self.veh_mesh_resource
marker.mesh_use_embedded_materials = False
pub.publish(marker)
def createMeshMarker(resource,
offset=(0, 0, 0),
rgba=(1, 0, 0, 1),
orientation=(0, 0, 0, 1),
scale=1,
scales=(1, 1, 1),
frame_id="/map",
marker_id=0):
marker = Marker()
marker.mesh_resource = resource
marker.header.frame_id = frame_id
marker.id = marker_id
marker.type = marker.MESH_RESOURCE
marker.scale.x = scale * scales[0]
marker.scale.y = scale * scales[1]
marker.scale.z = scale * scales[2]
marker.color.a = rgba[3]
marker.color.r = rgba[0]
marker.color.g = rgba[1]
marker.color.b = rgba[2]
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
marker.pose.position.x = offset[0]
marker.pose.position.y = offset[1]
marker.pose.position.z = offset[2]
return marker
def create_object_marker(self, soma_obj, soma_type, pose):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.name = soma_obj
int_marker.description = "id" + soma_obj
int_marker.pose = pose
mesh_marker = Marker()
mesh_marker.type = Marker.MESH_RESOURCE
mesh_marker.scale.x = 1
mesh_marker.scale.y = 1
mesh_marker.scale.z = 1
random.seed(soma_type)
val = random.random()
mesh_marker.color.r = r_func(val)
mesh_marker.color.g = g_func(val)
mesh_marker.color.b = b_func(val)
mesh_marker.color.a = 1.0
#mesh_marker.pose = pose
mesh_marker.mesh_resource = self.mesh[soma_type]
control = InteractiveMarkerControl()
control.markers.append(mesh_marker)
int_marker.controls.append(control)
return int_marker
def mesh_marker(position, orientation=None, mesh_resource="", rgba=[1.0, 1.0, 1.0, 1.0],
ns="", id=0, frame_id="", lifetime=None, stamp=None):
marker = Marker()
if stamp is None:
marker.header.stamp = rospy.Time.now()
else:
marker.header.stamp = stamp
marker.header.frame_id = frame_id
marker.ns = ns
marker.id = id
marker.type = Marker.MESH_RESOURCE
marker.mesh_resource = mesh_resource
marker.action = Marker.ADD
marker.pose.position.x = position[0]
marker.pose.position.y = position[1]
marker.pose.position.z = position[2]
if orientation is not None:
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
#marker.points = [Point(*tuple(position)), Point(*tuple(position+direction*1.0))]
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
marker.color.r = rgba[0]
marker.color.g = rgba[1]
marker.color.b = rgba[2]
marker.color.a = rgba[3]
if lifetime is None:
marker.lifetime = rospy.Duration()
else:
marker.lifetime = rospy.Duration(lifetime)
return marker
def publishMarker(self, position, visual):
markertypes = {
"cube": Marker.CUBE,
"sphere": Marker.SPHERE,
"mesh": Marker.MESH_RESOURCE
}
marker = Marker()
marker.header.frame_id = position.get("frame_id")
marker.type = markertypes[visual.get("type")]
marker.ns = visual.get("ns")
marker.id = visual.get("id")
marker.action = Marker.ADD
marker.scale.x = visual.get("scale")[0]
marker.scale.z = visual.get("scale")[1]
marker.scale.y = visual.get("scale")[2]
marker.color.r = visual.get("color")[0]
marker.color.g = visual.get("color")[1]
marker.color.b = visual.get("color")[2]
marker.color.a = visual.get("color")[3]
marker.pose.position.x = position.get("x")
marker.pose.position.y = position.get("y")
marker.pose.position.z = visual.get("z")
orientation = self.eulerToQuaternion(visual.get("orientation"))
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
marker.mesh_resource = visual.get("mesh_path") if marker.type == Marker.MESH_RESOURCE else ''
self.MarkersPUBL.publish(marker)
def createMesh(self,
name,
frame_id='world',
mesh_path='',
transform=PyKDL.Frame(),
color=Color(),
scale=np.array([1, 1, 1])):
""" Creates a Mesh Marker """
marker = Marker()
marker.header.frame_id = frame_id
marker.type = Marker.MESH_RESOURCE
marker.ns = name
marker.action = marker.ADD
marker.mesh_resource = mesh_path
marker.color.r = color.r
marker.color.g = color.g
marker.color.b = color.b
marker.color.a = color.a
marker.scale.x = scale[0]
marker.scale.y = scale[1]
marker.scale.z = scale[2]
marker.pose.position.x = transform.p.x()
marker.pose.position.y = transform.p.y()
marker.pose.position.z = transform.p.z()
quat = transform.M.GetQuaternion()
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
self.objects_map[name] = marker
return marker
def createMeshMarker(resource, offset=(0,0,0), rgba=(1,0,0,1), orientation=(0,0,0,1), scale=1, scales=(1,1,1), frame_id="/map"):
marker = Marker()
marker.mesh_resource = resource;
marker.header.frame_id = frame_id
marker.type = marker.MESH_RESOURCE
marker.scale.x = scale*scales[0]
marker.scale.y = scale*scales[1]
marker.scale.z = scale*scales[2]
marker.color.a = rgba[3]
marker.color.r = rgba[0]
marker.color.g = rgba[1]
marker.color.b = rgba[2]
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
marker.pose.position.x = offset[0]
marker.pose.position.y = offset[1]
marker.pose.position.z = offset[2]
obj_control = InteractiveMarkerControl()
obj_control.always_visible = True
obj_control.markers.append( marker )
return obj_control
def get_current_position_marker(self, link, offset=None, root="", scale=1, color=(0,1,0,1), idx=0):
(mesh, pose) = self.get_link_data(link)
marker = Marker()
if offset==None :
marker.pose = pose
else :
marker.pose = toMsg(fromMsg(offset)*fromMsg(pose))
marker.header.frame_id = root
marker.header.stamp = rospy.get_rostime()
marker.ns = self.robot_name
marker.mesh_resource = mesh
marker.type = Marker.MESH_RESOURCE
marker.action = Marker.MODIFY
marker.scale.x = scale
marker.scale.y = scale
marker.scale.z = scale
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.color.a = color[3]
marker.text = link
marker.id = idx
marker.mesh_use_embedded_materials = True
return 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 make_kin_tree_from_link(ps,linkname, ns='default_ns',valuedict=None):
markers = []
U = get_pr2_urdf()
link = U.links[linkname]
if link.visual and link.visual.geometry and isinstance(link.visual.geometry,urdf.Mesh):
rospy.logdebug("%s is a mesh. drawing it.", linkname)
marker = Marker(type = Marker.MESH_RESOURCE, action = Marker.ADD)
marker.ns = ns
marker.header = ps.header
linkFromGeom = conversions.trans_rot_to_hmat(link.visual.origin.position, transformations.quaternion_from_euler(*link.visual.origin.rotation))
origFromLink = conversions.pose_to_hmat(ps.pose)
origFromGeom = np.dot(origFromLink, linkFromGeom)
marker.pose = conversions.hmat_to_pose(origFromGeom)
marker.scale = gm.Vector3(1,1,1)
marker.color = stdm.ColorRGBA(1,1,0,.5)
marker.id = 0
marker.lifetime = rospy.Duration()
marker.mesh_resource = str(link.visual.geometry.filename)
marker.mesh_use_embedded_materials = False
markers.append(marker)
else:
rospy.logdebug("%s is not a mesh", linkname)
if U.child_map.has_key(linkname):
for (cjoint,clink) in U.child_map[linkname]:
markers.extend(make_kin_tree_from_joint(ps,cjoint,ns=ns,valuedict=valuedict))
return markers
def publish_sub_marker(self, pos, ori):
marker = Marker()
#marker.header.frame_id = "/base_footprint"
marker.header.frame_id = "/base_link"
marker.header.stamp = rospy.Time()
marker.id = 0
marker.type = Marker.MESH_RESOURCE
marker.action = Marker.ADD
marker.pose.position.x = pos[0]
marker.pose.position.y = pos[1]
marker.pose.position.z = pos[2]
marker.pose.orientation.x = ori[0]
marker.pose.orientation.y = ori[1]
marker.pose.orientation.z = ori[2]
marker.pose.orientation.w = ori[3]
marker.color.a = 1.
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
if self.model == 'chair':
name = 'subject_model'
marker.mesh_resource = "package://hrl_base_selection/models/wheelchair_and_body_assembly_rviz.STL"
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
elif self.model == 'bed':
name = 'subject_model'
marker.mesh_resource = "package://hrl_base_selection/models/head_bed.dae"
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
elif self.model == 'autobed':
name = 'subject_model'
marker.mesh_resource = "package://hrl_base_selection/models/bed_and_body_v3_rviz.dae"
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
else:
print 'I got a bad model. What is going on???'
return None
vis_pub = rospy.Publisher(''.join(['~',name]), Marker, queue_size=1, latch=True)
marker.ns = ''.join(['base_service_',name])
vis_pub.publish(marker)
print 'Published a model of the subject to rviz'
def publish_subject_marker(self, pos, ori):
marker = Marker()
marker.header.frame_id = "/base_link"
marker.header.stamp = rospy.Time()
marker.ns = "base_service_subject_model"
marker.id = 0
marker.type = Marker.MESH_RESOURCE;
marker.action = Marker.ADD
marker.pose.position.x = pos[0]
marker.pose.position.y = pos[1]
marker.pose.position.z = 0
marker.pose.orientation.x = ori[0]
marker.pose.orientation.y = ori[1]
marker.pose.orientation.z = ori[2]
marker.pose.orientation.w = ori[3]
marker.scale.x = .0254
marker.scale.y = .0254
marker.scale.z = .0254
marker.color.a = 1.
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
if self.model=='chair':
name = 'wc_model'
marker.mesh_resource = "package://hrl_base_selection/models/ADA_Wheelchair.dae"
marker.scale.x = .0254
marker.scale.y = .0254
marker.scale.z = .0254
elif self.model=='bed':
name = 'bed_model'
marker.mesh_resource = "package://hrl_base_selection/models/head_bed.dae"
marker.scale.x = 1.0
marker.scale.y = 1.
marker.scale.z = 1.0
elif self.model=='autobed':
name = 'autobed_model'
marker.mesh_resource = "package://hrl_base_selection/models/bed_and_body_v3_rviz.dae"
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
else:
print 'I got a bad model. What is going on???'
return None
self.vis_pub.publish(marker)
def create_object_marker(self, soma_obj, soma_type, pose):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = soma_obj
int_marker.description = "id" + soma_obj
int_marker.pose = pose
int_marker.pose.position.z = 0.01
mesh_marker = Marker()
mesh_marker.type = Marker.MESH_RESOURCE
mesh_marker.scale.x = 1
mesh_marker.scale.y = 1
mesh_marker.scale.z = 1
random.seed(soma_type)
val = random.random()
mesh_marker.color.r = r_func(val)
mesh_marker.color.g = g_func(val)
mesh_marker.color.b = b_func(val)
mesh_marker.color.a = 1.0
#mesh_marker.pose = pose
mesh_marker.mesh_resource = self.mesh[soma_type]
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
if self._interactive:
int_marker.controls.append(copy.deepcopy(control))
# add the control to the interactive marker
if self.marker[soma_type] == '3D':
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
# add menu control
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
menu_control.markers.append( mesh_marker) #makeBox(int_marker) )
int_marker.controls.append(menu_control)
return int_marker
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 draw_object(object_name, object):
marker = Marker()
marker.header.frame_id = "/BASE"
marker.header.stamp = rospy.Time.now()
marker.ns = object_name
marker.id = 0
marker.type = Marker.MESH_RESOURCE
marker.action = Marker.ADD
marker.mesh_resource = "package://dec_object_models/objects/%s/%s.obj" % (object_name, object.name)
marker.pose = object.pose.pose.pose
marker.scale.x, marker.scale.y, marker.scale.z = 1.0, 1.0, 1.0
marker.color.r, marker.color.g, marker.color.b, marker.color.a = (1.0, 0.0, 0.0, 0.5)
marker.lifetime = rospy.Duration()
mesh_pub.publish(marker)
def mesh_resource(frame, ns, idnum, color, mesh_uri, pose):
m = Marker()
m.header.frame_id = frame
m.header.stamp = rospy.Time.now()
m.ns = ns
m.id = idnum
m.type = Marker.MESH_RESOURCE
m.action = Marker.ADD
m.mesh_resource = mesh_uri
m.pose = pose
m.scale.x, m.scale.y, m.scale.z = 1.0, 1.0, 1.0
m.color.r, m.color.g, m.color.b, m.color.a = color
m.lifetime = rospy.Duration()
return m
def make_darci_ee_marker(ps, color, orientation = None,
marker_array=None, control=None,
mesh_id_start = 0, ns = "darci_ee",
offset=-0.14):
mesh_id = mesh_id_start
# this is the palm piece
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1.,1.,1.)
mesh.color = ColorRGBA(*color)
# stub_end_effector.dae
# stub_end_effector_mini45.dae
# tube_with_ati_collisions.dae
# wedge_blender.dae
# mesh.mesh_resource = "package://hrl_dynamic_mpc/meshes/tube_with_ati_collisions.dae"
mesh.mesh_resource = "package://hrl_dynamic_mpc/meshes/Darci_Flipper.stl"
mesh.type = Marker.MESH_RESOURCE
#rot_default = tr.Ry(np.radians(-90))*tr.Rz(np.radians(90))
rot_default = tr.Ry(np.radians(0))*tr.Rz(np.radians(90))
if orientation == None:
orientation = [0, 0, 0, 1]
rot_buf = tr.quaternion_to_matrix(orientation)
orientation = tr.matrix_to_quaternion(rot_buf*rot_default)
mesh.pose.orientation = Quaternion(*orientation)
mesh.pose.position.x = offset
if control != None:
control.markers.append( mesh )
elif marker_array != None:
mesh.pose.position = ps.pose.position
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
marker_array.markers.append(mesh)
if control != None:
control.interaction_mode = InteractiveMarkerControl.BUTTON
return control
elif marker_array != None:
return marker_array
def create_marker_for_link(self, link, T, scale=1, color=(0,1,0,1), idx=0) :
marker = Marker()
if not link in self.urdf.link_map :
print "EndEffectorHelper::create_marker_for_link() -- link: ", link, " not found in URDF model"
return marker
model_link = self.urdf.link_map[link]
if model_link :
if model_link.visual :
if model_link.visual.geometry :
if model_link.visual.geometry.filename :
mesh = model_link.visual.geometry.filename
p = geometry_msgs.msg.Pose()
q = (kdl.Rotation.RPY(model_link.visual.origin.rpy[0],model_link.visual.origin.rpy[1],model_link.visual.origin.rpy[2])).GetQuaternion()
p.position.x = model_link.visual.origin.xyz[0]
p.position.y = model_link.visual.origin.xyz[1]
p.position.z = model_link.visual.origin.xyz[2]
p.orientation.x = q[0]
p.orientation.y = q[1]
p.orientation.z = q[2]
p.orientation.w = q[3]
marker.pose = toMsg(T*fromMsg(p))
marker.header.frame_id = self.root_frame
marker.ns = self.robot_name
marker.mesh_resource = mesh
marker.type = Marker.MESH_RESOURCE
marker.action = Marker.MODIFY
marker.scale.x = scale
marker.scale.y = scale
marker.scale.z = scale
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.color.a = color[3]
marker.text = link
marker.id = idx
marker.mesh_use_embedded_materials = True
return marker
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 createMeshMarker(resource, offset=(0,0,0), rgba=(1,0,0,1), orientation=(0,0,0,1), scale=1, scales=(1,1,1), frame_id="/map", marker_id = 0):
marker = Marker()
marker.mesh_resource = resource;
marker.header.frame_id = frame_id
marker.id = marker_id
marker.type = marker.MESH_RESOURCE
marker.scale.x = scale*scales[0]
marker.scale.y = scale*scales[1]
marker.scale.z = scale*scales[2]
marker.color.a = rgba[3]
marker.color.r = rgba[0]
marker.color.g = rgba[1]
marker.color.b = rgba[2]
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
marker.pose.position.x = offset[0]
marker.pose.position.y = offset[1]
marker.pose.position.z = offset[2]
return marker
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 get_gripper_marker(self, pose_stamped, quality):
m = Marker()
m.header = pose_stamped.header
m.ns = "gripper"
m.id = self.marker_id
self.marker_id += 1
m.type = m.MESH_RESOURCE
m.action = m.ADD
m.pose = pose_stamped.pose
m.scale.x = 1.0
m.scale.y = 1.0
m.scale.z = 1.0
if quality < 0.5:
m.color.r = quality * 2
m.color.g = 0
else:
m.color.r = 0
m.color.g = (quality - 0.5) * 2
m.color.b = 0
m.color.a = 0.8
m.mesh_resource = "package://ubr1_description/meshes/gripper_link.STL"
return 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 publish_wc_marker(self, pos, ori):
marker = Marker()
marker.header.frame_id = "/base_footprint"
marker.header.stamp = rospy.Time()
marker.ns = "base_service_wc_model"
marker.id = 0
marker.type = Marker.MESH_RESOURCE;
marker.action = Marker.ADD
marker.pose.position.x = pos[0]
marker.pose.position.y = pos[1]
marker.pose.position.z = 0
marker.pose.orientation.x = ori[0]
marker.pose.orientation.y = ori[1]
marker.pose.orientation.z = ori[2]
marker.pose.orientation.w = ori[3]
marker.scale.x = .0254
marker.scale.y = .0254
marker.scale.z = .0254
marker.color.a = 1.
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.mesh_resource = "package://hrl_base_selection/models/ADA_Wheelchair.dae"
self.vis_pub.publish(marker)
import math
topic = "visualization_marker"
publisher = rospy.Publisher(topic, Marker)
rospy.init_node("register")
markerArray = MarkerArray()
count = 0
MARKERS_MAX = 100
while not rospy.is_shutdown():
marker = Marker()
marker.mesh_resource = "package://apc_config/models/object_meshes/expo_dry_erase_board_eraser.stl"
marker.header.frame_id = "/world"
marker.type = marker.MESH_RESOURCE
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 1
publisher.publish(marker)
def create_map_mesh(self):
"""Puts a blender mesh of the paement and stripes (continuous) into rviz"""
marker_array = MarkerArray()
### Track Pavement Publisher ###########################################
if self.track_mesh_resource:
marker = Marker()
marker.header.frame_id = 'odom' # publish in static frame
marker.id = 0
marker.action = Marker.ADD
marker.lifetime = rospy.Duration() # immortal unless changed
marker.ns = "track_mesh_pavement"
marker.type = Marker.MESH_RESOURCE
marker.mesh_use_embedded_materials = False
marker.mesh_resource = '//'.join(['file:', self.track_mesh_resource])
marker.pose.position.x = 0# - float(self.UTMdatum['E'])
marker.pose.position.y = 0# - float(self.UTMdatum['N'])
marker.pose.position.z = 0
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.color.r = 0.3
marker.color.g = 0.3
marker.color.b = 0.3
marker.color.a = 1.0
marker_array.markers.append(marker)
### Lane Marking Publisher ##############################################
if self.marking_mesh_resource:
marker = Marker()
marker.header.frame_id = 'odom' # publish in static frame
marker.id = 1
marker.action = Marker.ADD
marker.lifetime = rospy.Duration() # immortal unless changed
marker.ns = "track_mesh_lane_markings"
marker.type = Marker.MESH_RESOURCE
marker.mesh_use_embedded_materials = False
marker.mesh_resource = self.marking_mesh_resource
marker.pose.position.x = 0# - float(self.UTMdatum['E'])
marker.pose.position.y = 0# - float(self.UTMdatum['N'])
marker.pose.position.z = 0
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.color.r = 255
marker.color.g = 255
marker.color.b = 0
marker.color.a = 1.0
marker_array.markers.append(marker)
if self.sign_mesh_resource:
marker = Marker()
marker.header.frame_id = 'odom' # publish in static frame
marker.id = 1
marker.action = Marker.ADD
marker.lifetime = rospy.Duration() # immortal unless changed
marker.ns = "track_mesh_section_signs"
marker.type = Marker.MESH_RESOURCE
marker.mesh_use_embedded_materials = False
marker.mesh_resource = '//'.join(['file:',self.sign_mesh_resource])
marker.pose.position.x = 0# - float(self.UTMdatum['E'])
marker.pose.position.y = 0# - float(self.UTMdatum['N'])
marker.pose.position.z = 0
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.color.r = 0.1
marker.color.g = 0.6
marker.color.b = 0.1
marker.color.a = 1.0
marker_array.markers.append(marker)
if self.stop_mesh_resource:
marker = Marker()
marker.header.frame_id = 'odom' # publish in static frame
marker.id = 1
marker.action = Marker.ADD
marker.lifetime = rospy.Duration() # immortal unless changed
marker.ns = "track_mesh_stop_signs"
marker.type = Marker.MESH_RESOURCE
marker.mesh_use_embedded_materials = False
marker.mesh_resource = '//'.join(['file:',self.stop_mesh_resource])
marker.pose.position.x = 0# - float(self.UTMdatum['E'])
marker.pose.position.y = 0# - float(self.UTMdatum['N'])
marker.pose.position.z = 0
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.color.r = 0.7
marker.color.g = 0.1
marker.color.b = 0.1
marker.color.a = 1.0
marker_array.markers.append(marker)
self.track_mesh_publisher.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)
from std_msgs.msg import *
from geometry_msgs.msg import Pose, Vector3, Point, Quaternion
from visualization_msgs.msg import Marker
MESH_FILENAME = 'package://grasper_plan/data/dae/'+sys.argv[1]
if __name__ == '__main__':
rospy.init_node('grasp_marker_pub')
pub = rospy.Publisher('grasper_ctrl/marker_pub', Marker, queue_size=3)
m = Marker()
m.header.frame_id = 'graspable_object'
m.header.stamp = rospy.Time()
m.ns = '.'.join(sys.argv[1].split('.')[:-1]) # removes file extension from argv[1]
m.id = 0
m.type = Marker.MESH_RESOURCE
m.action = Marker.ADD
m.pose = Pose(Point(0,0,0), Quaternion(0,0,0,1))
m.scale = Vector3(1,1,1)
m.color = ColorRGBA(0.5,0.5,0.5,1)
m.lifetime = rospy.Duration(3, 0)
m.mesh_resource = MESH_FILENAME
r = rospy.Rate(1)
while not rospy.is_shutdown():
pub.publish(m)
m.id += 1
# print m.id
r.sleep()
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()