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 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 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 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 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 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 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 _make_mesh_marker(self):
'''Creates a mesh marker'''
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
alpha = 0.6
if self.is_dimmed:
alpha = 0.1
if self.is_fake:
alpha = 0.4
mesh.color = ColorRGBA(0.3, 0.3, 0.3, alpha)
return mesh
if self._is_reachable():
# Original: some kinda orange
# r,g,b = 1.0, 0.5, 0.0
# New: rainbow! See method comment for details.
r,g,b = self.get_marker_color()
mesh.color = ColorRGBA(r, g, b, alpha)
else:
mesh.color = ColorRGBA(0.5, 0.5, 0.5, alpha)
return mesh
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 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 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 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_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 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 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 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 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 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 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 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 print_points(self):
rate = rospy.Rate(10)
# iterations = 0
while not rospy.is_shutdown():
marker = Marker()
marker.header.frame_id = "/base"
marker.header.stamp = rospy.Time()
marker.ns = "primitive_surfaces"
marker.id = 10
marker.mesh_use_embedded_materials = True
marker.type = marker.MESH_RESOURCE
marker.action = marker.ADD
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.mesh_resource = "file:///home/pratique/drone_course_data/CC_tag_detection/Submission/cctag.dae"
self.tag_pub.publish(marker)
rate.sleep()
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 getMarker(self, source, markerID):
mType = source.get('marker_type', Marker.SPHERE)
mType = mType if not isinstance(mType, str) else getattr(
Marker, mType.upper())
mAction = source.get('marker_action', Marker.ADD)
mAction = mAction if not isinstance(mType, str) else getattr(
Marker, mType.upper())
marker = Marker()
marker.header.stamp = rospy.Time(0)
marker.header.frame_id = source.get('marker_frame', 'map')
marker.ns = source.get('marker_ns', '')
marker.id = int(source.get('markerID', markerID))
marker.type = mType
marker.action = mAction
marker.pose.position.x = source.get('marker_position',
{}).get('x', 0.0)
marker.pose.position.y = source.get('marker_position',
{}).get('y', 0.0)
marker.pose.position.z = source.get('marker_position',
{}).get('z', 0.0)
marker.pose.orientation = self._getOrientation(
source.get('marker_orientation', None))
marker.scale.x = source.get('marker_scale', {}).get('x', 0.1)
marker.scale.y = source.get('marker_scale', {}).get('y', 0.1)
marker.scale.z = source.get('marker_scale', {}).get('z', 0.1)
marker.color.r = source.get('marker_color', {}).get('r', 1.0)
marker.color.g = source.get('marker_color', {}).get('g', 1.0)
marker.color.b = source.get('marker_color', {}).get('b', 1.0)
marker.color.a = source.get('marker_color', {}).get('a', 1.0)
marker.lifetime = rospy.Duration(source.get('marker_lifetime', 0))
marker.frame_locked = bool(source.get('marker_framelocked', False))
# TODO: points
# TODO: colors
marker.text = source.get('marker_text', '')
marker.mesh_resource = source.get('marker_mesh', '')
marker.mesh_use_embedded_materials = bool(
source.get('marker_mesh_resources', True))
# Second (default hidden) marker as a label
label = copy.deepcopy(marker)
label.action = Marker.DELETE
label.ns += ('/' if label.ns else '') + 'labels'
label.scale.z = 0.1
label.pose.position.z += marker.scale.z + label.scale.z * 0.5
label.type = Marker.TEXT_VIEW_FACING
label.color.r = source.get('label_color', {}).get('r', 1.0)
label.color.g = source.get('label_color', {}).get('g', 1.0)
label.color.b = source.get('label_color', {}).get('b', 1.0)
label.color.a = source.get('label_color', {}).get('a', 1.0)
return marker, label
def load_markers():
publisher = rospy.Publisher('~marker', MarkerArray, queue_size=1, latch=True)
markerArray = MarkerArray();
file_path = rospy.get_param('~world_file')
if not file_path:
rospy.loginfo('World description file path is empty.')
return
if not isfile(file_path):
rospy.logwarn('World description file with path "' + file_path + '" does not exists.')
return
parameters = load_file(file_path)
if not parameters:
return
for model in parameters[0][0]['models']:
marker = Marker();
if 'box' in model:
marker.type = Marker.CUBE;
model = model['box']
marker.color.a = 1.0;
marker.color.r = 0.5;
marker.color.g = 0.5;
marker.color.b = 0.5;
elif 'cylinder' in model:
marker.type = Marker.CYLINDER;
model = model['cylinder']
marker.color.a = 1.0;
marker.color.r = 0.5;
marker.color.g = 0.5;
marker.color.b = 0.5;
elif 'mesh' in model:
marker.type = Marker.MESH_RESOURCE;
model = model['mesh']
marker.mesh_resource = model['mesh_resource']
marker.mesh_use_embedded_materials = True
else:
continue
marker.ns = model['name']
marker.header.frame_id = model['frame_id']
# marker.id = i # TODO Required?
marker.action = Marker.ADD
marker.pose.position.x = model['position']['x']
marker.pose.position.y = model['position']['y']
marker.pose.position.z = model['position']['z']
marker.pose.orientation.x = model['orientation']['x']
marker.pose.orientation.y = model['orientation']['y']
marker.pose.orientation.z = model['orientation']['z']
marker.pose.orientation.w = model['orientation']['w']
marker.scale.x = model['scale']['x']
marker.scale.y = model['scale']['y']
marker.scale.z = model['scale']['z']
markerArray.markers.append(marker)
rospy.loginfo("World `%s` is published as marker.", marker.ns)
publisher.publish(markerArray)
rospy.spin()
def makeTable(id, frame_id, scale, pose, file_name):
marker = Marker(type=Marker.MESH_RESOURCE,
id=id,
header=Header(frame_id=frame_id))
marker.scale = scale
marker.pose = pose
marker.mesh_resource = file_name
marker.mesh_use_embedded_materials = True
return marker
def interactive_gripper_marker(pose_stamped, finger_distance):
# gripper marker
gripper_marker = Marker()
gripper_marker.pose.position.x = 0.166 # wrist_roll_link/gripper_link Translation: [0.166, 0.000, 0.000]
gripper_marker.pose.orientation.w = 1 # wrist_roll_link/gripper_link Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
gripper_marker.type = Marker.MESH_RESOURCE
gripper_marker.mesh_resource = GRIPPER_MESH
gripper_marker.mesh_use_embedded_materials = True
finger_distance = max(CLOSE_FINGER_POS,
min(finger_distance, OPEN_FINGER_POS))
# left finger marker
l_marker = Marker()
l_marker.pose.position.x = 0.166
l_marker.pose.position.y = -finger_distance / 2.0
l_marker.pose.orientation.w = 1
l_marker.type = Marker.MESH_RESOURCE
l_marker.mesh_resource = L_FINGER_MESH
l_marker.mesh_use_embedded_materials = True
# right finger marker
r_marker = Marker()
r_marker.pose.position.x = 0.166
r_marker.pose.position.y = finger_distance / 2.0
r_marker.pose.orientation.w = 1
r_marker.type = Marker.MESH_RESOURCE
r_marker.mesh_resource = R_FINGER_MESH
r_marker.mesh_use_embedded_materials = True
control = InteractiveMarkerControl()
control.orientation.w = 1
control.interaction_mode = InteractiveMarkerControl.NONE
control.always_visible = True
control.markers.append(gripper_marker)
control.markers.append(l_marker)
control.markers.append(r_marker)
interactive_marker = InteractiveMarker()
interactive_marker.header = pose_stamped.header
interactive_marker.pose = pose_stamped.pose
interactive_marker.controls.append(control)
interactive_marker.scale = 0.3
return interactive_marker
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 gripper_interactive_marker(pose_stamped, finger_distance):
gripper_marker = Marker()
gripper_marker.pose.position.x = 0.166
gripper_marker.pose.orientation.w = 1
gripper_marker.type = Marker.MESH_RESOURCE
gripper_marker.mesh_resource = GRIPPER_MESH
gripper_marker.mesh_use_embedded_materials = True
finger_distance = max(0, min(finger_distance, 0.1))
l_marker = Marker()
l_marker.pose.position.x = 0.166
l_marker.pose.position.y = finger_distance / 2.0 - 0.1
l_marker.pose.orientation.w = 1
l_marker.type = Marker.MESH_RESOURCE
l_marker.mesh_resource = L_FINGER_MESH
l_marker.mesh_use_embedded_materials = True
r_marker = Marker()
r_marker.pose.position.x = 0.166
r_marker.pose.position.y = -finger_distance / 2.0 + 0.1
r_marker.pose.orientation.w = 1
r_marker.type = Marker.MESH_RESOURCE
r_marker.mesh_resource = R_FINGER_MESH
r_marker.mesh_use_embedded_materials = True
control = InteractiveMarkerControl()
control.orientation.w = 1
control.interaction_mode = InteractiveMarkerControl.NONE
control.always_visible = True
control.markers.append(gripper_marker)
control.markers.append(l_marker)
control.markers.append(r_marker)
interactive_marker = InteractiveMarker()
interactive_marker.header = pose_stamped.header
interactive_marker.pose = pose_stamped.pose
interactive_marker.controls.append(control)
interactive_marker.scale = 0.25
return interactive_marker
def link_as_marker(self, link_name):
if link_name not in self._link_to_marker:
marker = Marker()
geometry = self.get_urdf_link(link_name).visual.geometry
if isinstance(geometry, up.Mesh):
marker.type = Marker.MESH_RESOURCE
marker.mesh_resource = geometry.filename
if geometry.scale is None:
marker.scale.x = 1.0
marker.scale.z = 1.0
marker.scale.y = 1.0
else:
marker.scale.x = geometry.scale[0]
marker.scale.z = geometry.scale[1]
marker.scale.y = geometry.scale[2]
marker.mesh_use_embedded_materials = True
elif isinstance(geometry, up.Box):
marker.type = Marker.CUBE
marker.scale.x = geometry.size[0]
marker.scale.y = geometry.size[1]
marker.scale.z = geometry.size[2]
elif isinstance(geometry, up.Cylinder):
marker.type = Marker.CYLINDER
marker.scale.x = geometry.radius * 2
marker.scale.y = geometry.radius * 2
marker.scale.z = geometry.length
elif isinstance(geometry, up.Sphere):
marker.type = Marker.SPHERE
marker.scale.x = geometry.radius * 2
marker.scale.y = geometry.radius * 2
marker.scale.z = geometry.radius * 2
else:
return None
marker.header.frame_id = self.get_root()
marker.action = Marker.ADD
marker.color.a = 0.5
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.scale.x *= 0.99
marker.scale.y *= 0.99
marker.scale.z *= 0.99
marker.pose = Pose()
if self.has_non_identity_visual_offset(link_name):
marker.pose = self.get_visual_pose(link_name)
self._link_to_marker[link_name] = marker
return deepcopy(self._link_to_marker[link_name])
def _make_mesh_marker(self):
'''Creates a mesh marker'''
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
if self._is_reachable():
mesh.color = ColorRGBA(1.0, 0.5, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.5, 0.5, 0.5, 0.6)
return mesh
def _make_mesh_marker(self):
'''Creates a mesh marker'''
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
if self._is_reachable():
mesh.color = ColorRGBA(1.0, 0.5, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.5, 0.5, 0.5, 0.6)
return mesh
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 to_ros_markers(shape, stamp, frame_name, X_FG, color_rgba):
assert isinstance(shape, Shape), shape
assert isinstance(frame_name, str), frame_name
assert isinstance(X_FG, RigidTransform), X_FG
marker = Marker()
marker.header.stamp = stamp
marker.header.frame_id = frame_name
marker.pose = to_ros_pose(X_FG)
marker.action = Marker.ADD
marker.lifetime = Duration(0.)
marker.frame_locked = True
def use_color():
nonlocal color_rgba
if color_rgba is None:
color_rgba = DEFAULT_RGBA
(marker.color.r, marker.color.g, marker.color.b,
marker.color.a) = (color_rgba.r(), color_rgba.g(), color_rgba.b(),
color_rgba.a())
if type(shape) == Box:
marker.type = Marker.CUBE
marker.scale.x, marker.scale.y, marker.scale.z = shape.size()
use_color()
return [marker]
if type(shape) == Sphere:
marker.type = Marker.SPHERE
marker.scale.x = shape.radius()
marker.scale.y = shape.radius()
marker.scale.z = shape.radius()
use_color()
return [marker]
elif type(shape) == Cylinder:
marker.type = Marker.CYLINDER
marker.scale.x = shape.radius()
marker.scale.y = shape.radius()
marker.scale.z = shape.length()
use_color()
return [marker]
elif type(shape) in (Mesh, Convex):
marker.type = Marker.MESH_RESOURCE
marker.mesh_resource = f"file://{shape.filename()}"
# TODO(eric.cousineau): Is this the correct way to handle color vs.
# texture?
use_color()
marker.mesh_use_embedded_materials = True
marker.scale.x, marker.scale.y, marker.scale.z = 3 * [shape.scale()]
return [marker]
else:
# TODO(eric): Support Capsule, Ellipse, all dat jazz.
assert False, f"Unsupported type: {shape}"
def _make_mesh_marker(self):
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6)
ik_solution = self.ik.get_ik_for_ee(self.ee_pose)
if (ik_solution == None):
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def get_mesh_marker():
marker = Marker()
marker.type = marker.MESH_RESOURCE
marker.pose = Pose()
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.pose.orientation.w = 1.
marker.mesh_resource = "package://point_cloud_tools/data/mesh.stl"
marker.mesh_use_embedded_materials = True
marker.header.frame_id = "world"
return marker
def _make_mesh_marker(self):
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6)
ik_solution = self.ik.get_ik_for_ee(self.ee_pose)
if (ik_solution == None):
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def _make_mesh_marker(self):
'''Creates and returns a mesh marker.
Returns:
Marker
'''
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
mesh.color = self._get_mesh_marker_color()
return mesh
def callback_tag(self, data):
marker_tag = Marker()
marker_tag.header.frame_id = self.frame_id
marker_tag.header.stamp = rospy.Time.now()
marker_tag.id = 0
marker_tag.action = Marker().ADD
if self.isNinebot:
marker_tag.type = Marker().MESH_RESOURCE
marker_tag.mesh_resource = "package://tms_ss_ninebot_pozyx/meshes/ninebot_v2.dae"
marker_tag.mesh_use_embedded_materials = True
marker_tag.scale.x = 0.01
marker_tag.scale.y = 0.01
marker_tag.scale.z = 0.01
elif self.isHuman:
marker_tag.type = Marker().MESH_RESOURCE
marker_tag.mesh_resource = "package://tms_ss_ninebot_pozyx/meshes/WalkingMan4.dae"
marker_tag.mesh_use_embedded_materials = True
marker_tag.scale.x = 0.025
marker_tag.scale.y = 0.025
marker_tag.scale.z = 0.025
else:
marker_tag.type = Marker().CYLINDER
marker_tag.scale.x = 0.2
marker_tag.scale.y = 0.2
marker_tag.scale.z = 1.0
marker_tag.color.r = 1.0
marker_tag.color.g = 0.0
marker_tag.color.b = 0.0
marker_tag.color.a = 1.0
marker_tag.pose = data.pose
marker_tag.lifetime = rospy.Duration()
self.publish_anchors()
self.tag_pub.publish(marker_tag)
def _make_mesh_marker(self):
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_sol = self._ik_service.get_ik_for_ee(target_pose)
if ik_sol == None:
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def getDroneMarker(self, pose):
marker = Marker()
marker.id = 1
marker.ns = "mesh_" + self.name
marker.header.frame_id = "world"
marker.type = marker.MESH_RESOURCE
marker.action = marker.ADD
marker.pose = pose
marker.lifetime = rospy.Duration.from_sec(0.0)
marker.mesh_use_embedded_materials = True
marker.mesh_resource = "package://acl_sim/meshes/quadrotor/quadrotor.dae"
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
return marker
def _make_mesh_marker(color):
'''Creates and returns a mesh marker.
Args:
color (ColorRGBA)
Returns:
Marker
'''
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
mesh.color = color
return mesh
def publish_rviz_markers(frames, frame_id, handarm_params):
timestamp = rospy.Time.now()
global markers_rviz
global frames_rviz
markers_rviz = MarkerArray()
for i, f in enumerate(frames):
msg = Marker()
msg.header.stamp = timestamp
msg.header.frame_id = frame_id
msg.frame_locked = True # False
msg.id = i
msg.type = Marker.MESH_RESOURCE
msg.action = Marker.ADD
msg.lifetime = rospy.Duration()
msg.color.r = msg.color.g = msg.color.b = msg.color.a = 0
msg.mesh_use_embedded_materials = True
msg.mesh_resource = handarm_params["mesh_file"]
msg.scale.x = msg.scale.y = msg.scale.z = handarm_params[
"mesh_file_scale"]
#msg.mesh_resource = mesh_resource
msg.pose = homogenous_tf_to_pose_msg(f)
markers_rviz.markers.append(msg)
for f1, f2 in zip(frames, frames[1:]):
msg = Marker()
msg.header.stamp = timestamp
msg.header.frame_id = frame_id
msg.frame_locked = True # False
msg.id = markers_rviz.markers[-1].id + 1
msg.action = Marker.ADD
msg.lifetime = rospy.Duration()
msg.type = Marker.ARROW
msg.color.g = msg.color.b = 0
msg.color.r = msg.color.a = 1
msg.scale.x = 0.01 # shaft diameter
msg.scale.y = 0.03 # head diameter
msg.points.append(homogenous_tf_to_pose_msg(f1).position)
msg.points.append(homogenous_tf_to_pose_msg(f2).position)
markers_rviz.markers.append(msg)
frames_rviz = frames
def make_model_marker(model):
marker = Marker()
marker.type = Marker.MESH_RESOURCE
marker.scale.x = 0.01
marker.scale.y = 0.01
marker.scale.z = 0.01
model_type = '_'.join(
model.split('_')[:-1]) # remove instance index to get the model type
marker.mesh_resource = "package://thorp_simulation/worlds/gazebo/models/" + model_type + "/meshes/Cat_v1_l3.obj"
marker.mesh_use_embedded_materials = True
# TODO: get mesh path and mesh pose from the model SDF file to make this generic and avoid the hack below
if model_type == 'cat_black':
marker.pose.position.x = -0.09
marker.pose.orientation = quaternion_msg_from_yaw(math.pi / 2.0)
if model_type == 'cat_orange':
marker.pose.orientation = quaternion_msg_from_yaw(math.pi / 2.0)
return marker
def _make_mesh_marker(self):
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
target_pose = GripperMarkers._offset_pose(self.marker_pose, GripperMarkers._offset)
print target_pose
ik_solution = self.ik.get_ik_for_ee(target_pose)
if (ik_solution == None):
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
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 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)
for x in xrange(0,num_obs): kind=int(data[3*x]) px=float(data[3*x+1]) py=float(data[3*x+2]) filename="package://dynamic_3d_view/dynamic_model/"+switch[kind] # print filename marker = Marker() marker.id = x marker.header.frame_id = "/map" marker.ns='put_obstacles' marker.type=10 marker.scale.x=1 marker.scale.y=1 marker.scale.z=1 # marker.header.stamp =rospy.Time.now() marker.mesh_resource=filename marker.mesh_use_embedded_materials=1 marker.action=0 # marker.lifetime = rospy.Duration(0) marker.pose.position.x = px-6.5 marker.pose.position.y = py-6.5 marker.pose.position.z = 0 markerArray.markers.append(marker) while not rospy.is_shutdown(): # Publish the MarkerArray publisher.publish(markerArray) rospy.sleep(1)
def publish_object_poses(input_markers):
output_markers = MarkerArray()
tag_detections = {}
for marker in input_markers.markers:
tag_id = marker.id
position_data = marker.pose.position
orientation_data = marker.pose.orientation
tag_pose = numpy.matrix(quaternion_matrix([orientation_data.x,
orientation_data.y,
orientation_data.z,
orientation_data.w]))
tag_pose[0,3] = position_data.x
tag_pose[1,3] = position_data.y
tag_pose[2,3] = position_data.z
tag_detections[tag_id] = [tag_pose]
obj_tag_poses = {}
for tag in tag_detections:
if tag not in tag_to_obj: #Not a noted marker
continue
#Get the object for that tag
tag_obj = tag_to_obj[tag]
# Multiply apriltags value with pre-stored value
tag_to_cam = tag_detections[tag]*obj_to_tags[tag_obj][tag]
if tag_obj not in obj_tag_poses:
#Only append
obj_tag_poses[tag_obj] = {}
obj_tag_poses[tag_obj][tag] = tag_to_cam
print(obj_tag_poses)
for obj in obj_tag_poses:
for tag in obj_tag_poses[obj]:
obj_pose_transform = obj_tag_poses[obj][tag]
output_marker = Marker()
output_marker.header.stamp = rospy.Time.now()
output_marker.header.frame_id = input_markers.markers[0].header.frame_id
output_marker.ns = obj
output_marker.id = tag
output_marker.type = Marker.CUBE
output_marker.action = output_marker.ADD
output_marker.pose.position.x = obj_pose_transform[0,3]
output_marker.pose.position.y = obj_pose_transform[1,3]
output_marker.pose.position.z = obj_pose_transform[2,3]
output_marker.scale.x = 0.01
output_marker.scale.y = 0.01
output_marker.scale.z = 0.01
output_marker.color.r = 1.0
output_marker.color.a = 1.0
output_quat = quaternion_from_matrix(obj_pose_transform)
output_marker.pose.orientation.x = output_quat[0]
output_marker.pose.orientation.y = output_quat[1]
output_marker.pose.orientation.z = output_quat[2]
output_marker.pose.orientation.w = output_quat[3]
output_marker.mesh_use_embedded_materials = False
output_markers.markers.append(output_marker)
obj_pose_pub.publish(output_markers)
from __future__ import print_function
import copy
import rospy
from rospkg import RosPack, ResourceNotFound
from visualization_msgs.msg import Marker
import pysdf
import os.path
protoMarkerMsg = Marker()
protoMarkerMsg.frame_locked = True
protoMarkerMsg.id = 0
protoMarkerMsg.action = Marker.ADD
protoMarkerMsg.mesh_use_embedded_materials = True
protoMarkerMsg.color.a = 0.0
protoMarkerMsg.color.r = 0.0
protoMarkerMsg.color.g = 0.0
protoMarkerMsg.color.b = 0.0
supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box']
gazebo_rospack = RosPack()
def link2marker_msg(link, full_linkname, use_collision = False, lifetime = rospy.Duration(0)):
marker_msg = None
linkpart = None
if use_collision:
linkparts = getattr(link, 'collisions')
else: # visual
def publish_sim_path_as_markers(self, sim_path, frame_id):
"""Publishes the given sim apth as a set of ROS markers"""
msg = MarkerArray()
now = rospy.Time.now()
if not sim_path or len(sim_path) < self.last_marker_length:
diff = self.last_marker_length
offset = 0
if sim_path:
offset = len(sim_path)
diff -= offset
for index in range(diff+1):
marker = Marker()
marker.header.frame_id = frame_id
marker.ns = 'sim_path'
marker.id = offset + index - 1
marker.action = Marker.DELETE
if sim_path:
for index, waypoint in enumerate(sim_path):
marker = Marker()
marker.header.frame_id = frame_id
marker.ns = 'sim_path'
marker.id = index
marker.type = Marker.ARROW
marker.action = Marker.MODIFY
marker.pose.position.x = waypoint[1]
marker.pose.position.y = waypoint[2]
quat = qfe(0, 0, waypoint[3])
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.scale.x = 0.25
marker.scale.y = 0.25
marker.scale.z = 0.25
marker.color.r = 0.25
marker.color.g = 0.25
marker.color.b = 0.25
marker.color.a = 0.25
msg.markers.append(marker)
waypoint = sim_path[-1]
marker = Marker()
marker.header.frame_id = frame_id
marker.ns = 'prediction'
marker.id = 0
marker.type = Marker.MESH_RESOURCE
marker.action = Marker.MODIFY
marker.pose.position.x = waypoint[1]
marker.pose.position.y = waypoint[2]
marker.pose.position.z = 0.13
quat = qfe(0, 0, waypoint[3])
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.color.r = 1
marker.color.g = 0
marker.color.b = 0
marker.color.a = 1
marker.mesh_resource = 'package://gavlab_atrv_description/meshes/atrv_chassis.dae'
marker.mesh_use_embedded_materials = True
msg.markers.append(marker)
self.marker_array_pub.publish(msg)
def makeGripperMarker(angle=0.541, color=None, scale=1.0):
"""
Creates an InteractiveMarkerControl with the PR2 gripper shape.
Parameters:
angle: the aperture angle of the gripper (default=0.541)
color: (r,g,b,a) tuple or None (default) if using the material colors
scale: the scale of the gripper, default is 1.0
Returns:
The new gripper InteractiveMarkerControl
"""
T1 = euclid.Matrix4()
T2 = euclid.Matrix4()
T1.translate(0.07691, 0.01, 0.)
T1.rotate_axis(angle, euclid.Vector3(0,0,1))
T2.translate(0.09137, 0.00495, 0.)
T1.rotate_axis(-angle, euclid.Vector3(0,0,1))
T_proximal = T1.copy()
T_distal = T1 * T2
control = InteractiveMarkerControl()
mesh = Marker()
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = scale
mesh.scale.y = scale
mesh.scale.z = scale
if color is not None:
mesh.color.r = color[0]
mesh.color.g = color[1]
mesh.color.b = color[2]
mesh.color.a = color[3]
mesh.mesh_use_embedded_materials = False
else:
mesh.mesh_use_embedded_materials = True
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae"
mesh.pose.orientation.w = 1
control.markers.append(copy.deepcopy(mesh))
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
mesh.pose = matrix4ToPose(T_proximal)
control.markers.append(copy.deepcopy(mesh))
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
mesh.pose = matrix4ToPose(T_distal)
control.markers.append(copy.deepcopy(mesh))
T1 = euclid.Matrix4()
T2 = euclid.Matrix4()
T1.translate(0.07691, -0.01, 0.)
T1.rotate_axis(numpy.pi, euclid.Vector3(1,0,0))
T1.rotate_axis(angle, euclid.Vector3(0,0,1))
T2.translate(0.09137, 0.00495, 0.)
T1.rotate_axis(-angle, euclid.Vector3(0,0,1))
T_proximal = T1.copy()
T_distal = T1 * T2
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
mesh.pose = matrix4ToPose(T_proximal)
control.markers.append(copy.deepcopy(mesh))
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
mesh.pose = matrix4ToPose(T_distal)
control.markers.append(copy.deepcopy(mesh))
control.interaction_mode = InteractiveMarkerControl.BUTTON
return control
def create_map(self):
"""
This function collects the survey data and puts it into marker arrays ready to be published in rviz
"""
stripes, centers = self.survey()
self.map_stripe_array = MarkerArray()
self.map_lane_array = MarkerArray()
NCAT_id = 0
### Stripes ################################################################
for pt in stripes:
lat = float(pt[0])
lon = float(pt[1])
(east, nrth, _), info = self.gps.lla2utm((lat, lon, 0)) # convert to UTM
marker = Marker()
marker.header.frame_id = 'odom'
marker.id = NCAT_id # enumerate subsequent markers here
marker.action = Marker.ADD # can be ADD, REMOVE, or MODIFY
marker.lifetime = rospy.Duration() # will last forever unless modified
marker.ns = "stripes"
marker.type = Marker.CUBE
marker.pose.position.x = east - float(self.UTMdatum['E'])
marker.pose.position.y = nrth - float(self.UTMdatum['N'])
marker.pose.position.z = 0 # zero is a novatel mount level
marker.color.r = 255
marker.color.g = 255
marker.color.b = 0
marker.color.a = 1.0
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.4
marker.mesh_use_embedded_materials = False
self.map_stripe_array.markers.append(marker)
NCAT_id += 1
self.map_stripe_publisher.publish(self.map_stripe_array) # publish stripes as markers
print('mapBridge: Stripe Markers have been printed')
del marker
### Lane Centers ###########################################################
for pt in centers:
lat = float(pt[0])
lon = float(pt[1])
(east, nrth, _), _ = self.gps.lla2utm((lat, lon, 0)) # convert to UTM
marker = Marker()
marker.header.frame_id = 'odom'
marker.id = NCAT_id # enumerate subsequent markers here
marker.action = Marker.ADD # can be ADD, REMOVE, or MODIFY
marker.lifetime = rospy.Duration() # will last forever unless modified
marker.ns = "lane_centers"
marker.type = Marker.SPHERE
marker.pose.position.x = east - float(self.UTMdatum['E'])
marker.pose.position.y = nrth - float(self.UTMdatum['N'])
marker.pose.position.z = 0 # zero is a novatel mount level
marker.color.r = 255
marker.color.g = 255
marker.color.b = 255
marker.color.a = 0.75
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.mesh_use_embedded_materials = False
self.map_lane_array.markers.append(marker)
NCAT_id += 1
self.map_lane_publisher.publish(self.map_lane_array) # publish lane centers as markers
print('mapBridge: Lane Center Markers have been printed')
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 publish_gripper(server, pose_stamped, name):
"""Publishes a marker representing a gripper.
Code taken from action_step_marker.py in PR2/PbD.
Args:
server: An InteractiveMarkerServer
pose_stamped: A PoseStamped giving the wrist_roll_link pose.
name: string, a unique name for this gripper.
"""
# Set angle of meshes based on gripper open vs closed.
angle = 28 * math.pi / 180.0 # Fully open.
STR_MESH_GRIPPER_FOLDER = 'package://pr2_description/meshes/gripper_v0/'
STR_GRIPPER_PALM_FILE = STR_MESH_GRIPPER_FOLDER + 'gripper_palm.dae'
STR_GRIPPER_FINGER_FILE = STR_MESH_GRIPPER_FOLDER + 'l_finger.dae'
STR_GRIPPER_FINGERTIP_FILE = STR_MESH_GRIPPER_FOLDER + 'l_finger_tip.dae'
# Make transforms in preparation for meshes 1, 2, and 3.
# NOTE(mbforbes): There are some magic numbers in here that are
# used a couple times. Seems like a good candidate for
# refactoring to constants, but I think they're more clear if
# left in here as (a) they likely won't be changed, and (b) it's
# easier to understand the computations with them here.
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1, transform2)
# Create mesh 1 (palm).
mesh1 = Marker()
mesh1.header.frame_id = pose_stamped.header.frame_id
mesh1.mesh_use_embedded_materials = True
mesh1.type = Marker.MESH_RESOURCE
mesh1.scale.x = 1.0
mesh1.scale.y = 1.0
mesh1.scale.z = 1.0
mesh1.mesh_resource = STR_GRIPPER_PALM_FILE
mesh1.pose = pose_stamped.pose
# Create mesh 2 (finger).
mesh2 = Marker()
mesh2.mesh_use_embedded_materials = True
mesh2.type = Marker.MESH_RESOURCE
mesh2.scale.x = 1.0
mesh2.scale.y = 1.0
mesh2.scale.z = 1.0
mesh2.mesh_resource = STR_GRIPPER_FINGER_FILE
mesh2.pose = _get_pose_from_transform(t_proximal)
# Create mesh 3 (fingertip).
mesh3 = Marker()
mesh3.mesh_use_embedded_materials = True
mesh3.type = Marker.MESH_RESOURCE
mesh3.scale.x = 1.0
mesh3.scale.y = 1.0
mesh3.scale.z = 1.0
mesh3.mesh_resource = STR_GRIPPER_FINGERTIP_FILE
mesh3.pose = _get_pose_from_transform(t_distal)
# Make transforms in preparation for meshes 4 and 5.
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(math.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1, transform2)
# Create mesh 4 (other finger).
mesh4 = Marker()
mesh4.mesh_use_embedded_materials = True
mesh4.type = Marker.MESH_RESOURCE
mesh4.scale.x = 1.0
mesh4.scale.y = 1.0
mesh4.scale.z = 1.0
mesh4.mesh_resource = STR_GRIPPER_FINGER_FILE
mesh4.pose = _get_pose_from_transform(t_proximal)
# Create mesh 5 (other fingertip).
mesh5 = Marker()
mesh5.mesh_use_embedded_materials = True
mesh5.type = Marker.MESH_RESOURCE
mesh5.scale.x = 1.0
mesh5.scale.y = 1.0
mesh5.scale.z = 1.0
mesh5.mesh_resource = STR_GRIPPER_FINGERTIP_FILE
mesh5.pose = _get_pose_from_transform(t_distal)
# Append all meshes we made.
control = InteractiveMarkerControl()
control.markers = [mesh1, mesh2, mesh3, mesh4, mesh5]
control.interaction_mode = InteractiveMarkerControl.NONE
interactive_marker = InteractiveMarker()
interactive_marker.controls = [control]
interactive_marker.header.frame_id = pose_stamped.header.frame_id
interactive_marker.pose = pose_stamped.pose
interactive_marker.name = name
server.insert(interactive_marker)
server.applyChanges()
def simulate_quad(self):
# simulator node
rospy.init_node('simulate_quad', anonymous=True)
#-----------------------------------------------------------------------#
#-----------------------------------------------------------------------#
# Simulator subscribes to command inputs, published by a controller
rospy.Subscriber("quad_cmd", quad_cmd, self.get_input)
#-----------------------------------------------------------------------#
#-----------------------------------------------------------------------#
# this node is a simulator, thus it will publish the state of the quad
# it uses the commands -- that it is subscribed to -- to solve differential equations
pub = rospy.Publisher('quad_state', quad_state, queue_size=10)
#-----------------------------------------------------------------------#
#-----------------------------------------------------------------------#
# TO SAVE FLAG
# by default, no dynamics: everything stopped
self.StartFlag = False
# Service is created, so that simulation can be started or stopped
Start_service = rospy.Service('StartSimulator', StartSim, self.handle_Start_service)
# Service is created, so that simulation can be reseted
Reset_service = rospy.Service('ResetSimulator', StartSim, self.handle_Reset_service)
# NOTICE THAT SERVICE TYPE IS THE SAME FOR BOTH SERVICES ABOVE
#-----------------------------------------------------------------------#
#-----------------------------------------------------------------------#
#-----------------------------------------------------------------------#
#-----------------------------------------------------------------------#
# Service is created, so that user can change simulator on GUI
Chg_Simulator = rospy.Service('ServiceChangeSimulator', SrvCreateJsonableObjectByStr, self.__handle_simulator_change_service)
# solve differential equations at frequency
rate = rospy.Rate(self.frequency)
pub_rviz = rospy.Publisher("visualization_marker",Marker, queue_size=10);
marker = Marker()
marker.header.frame_id = "map";
marker.header.stamp = rospy.Time();
marker.ns = "my_namespace";
marker.id = 0;
# marker.type = Marker.SPHERE;
# marker.action = Marker.ADD;
marker.pose.position.y = 0.1;
marker.pose.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 = 1;
# marker.scale.y = 0.1;
# marker.scale.z = 0.1;
# marker.color.a = 1.0; #Don't forget to set the alpha!
# marker.color.r = 0.0;
# marker.color.g = 1.0;
# marker.color.b = 0.0;
# //only if using a MESH_RESOURCE marker type:
# marker.color.r = 1.0;
# marker.color.g = 1.0;
# marker.color.b = 1.0;
marker.scale.x = 1;
marker.scale.y = 1;
marker.scale.z = 1;
marker.type = Marker.MESH_RESOURCE
marker.mesh_use_embedded_materials = True
marker.mesh_resource = "package://rotors_description/meshes/firefly.dae"
# marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
while not rospy.is_shutdown():
# WARNING: IT IS VERY IMPORTANT THAT U0, U1, U2 AND U3 ARE PROVIDED THIS WAY
# I CANNOT PROVIDE SELF.U TO THE INTEGRATION BECAUSE IT IS CHANGING
# AND IT MESSES UP THE INTEGRATION!!!
# thid DOES NOT work: r.set_f_params(deepcopy(self.U),parameters)
# we delay system the initialization of the system by a TimeDelay
simtime = self.sim.get_time()
simstate = self.sim.get_state()
if (simtime >= self.TimeDelay and self.StartFlag):
self.sim.run(1.0/self.frequency)
# set dynamics vector accordinf to current input vector
# input vector is assumed constant during integration
#self.r.set_f_params(Input(self.U))
# integrate equation for period of loop
#self.r.integrate(self.r.t + 1.0/self.frequency);
# reset initial state and initial time
#self.r.set_initial_value(self.r.y, self.r.t)
else:
# need to update initial state and time
#self.r.set_initial_value(self.r.y, self.r.t + 1.0/self.frequency)
self.sim.reset(
initial_time=simtime+1.0/self.frequency,
initial_state=simstate
)
# create a message of type quad_state with current state
state = self.write_state()
# publish current state
# rospy.logwarn(state)
# rospy.logwarn('aaaaaaaaaaaaaaaaaaaaaaaaa')
pub.publish(state)
# marker.pose.position.x = 0.1 + numpy.cos(2*3.14/5*rospy.get_time());
marker.pose.position.x = simstate[0]
marker.pose.position.y = simstate[1]
marker.pose.position.z = simstate[2]
# quaternion
# marker.pose.orientation.x = simstate[0]
# marker.pose.orientation.y = simstate[1]
# marker.pose.orientation.z = simstate[2]
# marker.pose.orientation.w = simstate[2]
pub_rviz.publish( marker )
# rospy.logwarn(marker)
# let node sleep
rate.sleep()
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
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)
def makeGripperMarker(pose, angle=0.0, color=None, scale=1.0):
"""
Creates an MarkerArray with the PR2 gripper shape.
Parameters:
pose: the position of the marker, a Pose instance
angle: the aperture angle of the gripper (default=0.541)
color: (r,g,b,a) tuple or None (default) if using the material colors
scale: the scale of the gripper, default is 1.0
Returns:
The new gripper MarkerArray
"""
assert isinstance(pose, PoseStamped)
pose_T = poseTomatrix(pose.pose)
gripper_marker = MarkerArray()
T1 = transformations.translation_matrix((0.07691, 0.01, 0.))
T1 = T1.dot(transformations.rotation_matrix(angle, (0, 0, 1)))
T2 = transformations.translation_matrix((0.09137, 0.00495, 0.))
T2 = T2.dot(transformations.rotation_matrix(angle, (0, 0, 1)))
#T1 = euclid.Matrix4()
#T2 = euclid.Matrix4()
#T1.translate(0.07691, 0.01, 0.)
#T1.rotate_axis(angle, euclid.Vector3(0,0,1))
#T2.translate(0.09137, 0.00495, 0.)
#T1.rotate_axis(-angle, euclid.Vector3(0,0,1))
T_proximal = T1.copy()
T_distal = T1.dot(T2)
mesh = Marker()
mesh.header.frame_id = pose.header.frame_id
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = scale
mesh.scale.y = scale
mesh.scale.z = scale
if color is not None:
mesh.color.r = color[0]
mesh.color.g = color[1]
mesh.color.b = color[2]
mesh.color.a = color[3]
mesh.mesh_use_embedded_materials = False
else:
mesh.mesh_use_embedded_materials = True
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae"
mesh.pose = matrixToPose(pose_T)
gripper_marker.markers.append(copy.deepcopy(mesh))
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
mesh.pose = matrixToPose(pose_T.dot(T_proximal))
gripper_marker.markers.append(copy.deepcopy(mesh))
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
mesh.pose = matrixToPose(pose_T.dot(T_distal))
gripper_marker.markers.append(copy.deepcopy(mesh))
#T1 = euclid.Matrix4()
#T2 = euclid.Matrix4()
#T1.translate(0.07691, -0.01, 0.)
#T1.rotate_axis(np.pi, euclid.Vector3(1,0,0))
#T1.rotate_axis(angle, euclid.Vector3(0,0,1))
#T2.translate(0.09137, 0.00495, 0.)
#T1.rotate_axis(-angle, euclid.Vector3(0,0,1))
T1 = transformations.translation_matrix((0.07691, -0.01, 0.))
T1 = T1.dot(transformations.rotation_matrix(np.pi, (1, 0, 0)))
T1 = T1.dot(transformations.rotation_matrix(angle, (0, 0, 1)))
T2 = transformations.translation_matrix((0.09137, 0.00495, 0.))
T2 = T2.dot(transformations.rotation_matrix(-angle, (0, 0, 1)))
T_proximal = T1.copy()
T_distal = T1.dot(T2)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
mesh.pose = matrixToPose(pose_T.dot(T_proximal))
gripper_marker.markers.append(copy.deepcopy(mesh))
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
mesh.pose = matrixToPose(pose_T .dot(T_distal))
gripper_marker.markers.append(copy.deepcopy(mesh))
for i, marker in enumerate(gripper_marker.markers):
marker.id = i
return gripper_marker
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()