def straighline_planner(self, obs=None):
waypoints = linear_interp_traj([[self.start_x, self.start_y], [self.goal_x, self.goal_y]], 0.1)
waypoints = np.array(waypoints)
lin_x, lin_y, angu_z, ind, obs_lin_x, obs_lin_y = self.holonomic_controller.control(waypoints, self.start_x, self.start_y, self.start_yaw, self.goal_yaw, self.vel_rob[0], self.vel_rob[1], obs)
self.vel_msg.linear.x = lin_x
self.vel_msg.linear.y = lin_y
self.vel_msg.angular.z = 0.0
# self.vel_msg.angular.z = angu_z
# self.velocity_pub.publish(self.vel_msg)
robot_marker = vis_msg.Marker(type=vis_msg.Marker.SPHERE_LIST, action=vis_msg.Marker.ADD)
robot_marker.header.frame_id = 'world'
robot_marker.header.stamp = rospy.Time.now()
robot_marker.scale.x = 0.5
robot_marker.scale.y = 0.5
robot_marker.points = [geom_msg.Point(self.start_x, self.start_y, 0.0)]
robot_marker.colors = [std_msg.ColorRGBA(1.0, 0.0, 0.0, 1.0)]
self.robot_marker_pub.publish(robot_marker)
goal_marker = vis_msg.Marker(type=vis_msg.Marker.SPHERE_LIST, action=vis_msg.Marker.ADD)
goal_marker.header.frame_id = 'world'
goal_marker.header.stamp = rospy.Time.now()
goal_marker.scale.x = 0.5
goal_marker.scale.y = 0.5
goal_marker.points = [geom_msg.Point(self.goal_x, self.goal_y, 0.0)]
goal_marker.colors = [std_msg.ColorRGBA(0.0, 0.0, 1.0, 1.0)]
self.goal_marker_pub.publish(goal_marker)
return waypoints, lin_x, lin_y, ind, obs_lin_x, obs_lin_y
def publishTagMarkers(tags, publisher):
"""Republishes the tag_markers topic"""
SHIFT = 0.05
h = std_msgs.Header(stamp=rospy.Time.now(), frame_id='map')
ca = std_msgs.ColorRGBA(r=1, a=1)
cb = std_msgs.ColorRGBA(g=1, a=1)
ct = std_msgs.ColorRGBA(b=1, a=1)
sa = geometry_msgs.Vector3(x=0.1, y=0.5, z=0.5)
sb = geometry_msgs.Vector3(x=0.1, y=0.25, z=0.25)
st = geometry_msgs.Vector3(x=1, y=1, z=1)
ma = visualization_msgs.MarkerArray()
ma.markers.append(
visualization_msgs.Marker(
header=h, id=-1, action=visualization_msgs.Marker.DELETEALL))
for i, t in enumerate(tags):
th = t['th_deg'] * math.pi / 180.
q = tf.transformations.quaternion_from_euler(0, 0, th)
q = geometry_msgs.Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
ma.markers.append(
visualization_msgs.Marker(
header=h,
id=i * 3,
type=visualization_msgs.Marker.CUBE,
action=visualization_msgs.Marker.ADD,
pose=geometry_msgs.Pose(
position=geometry_msgs.Point(x=t['x'], y=t['y']),
orientation=q),
scale=sa,
color=ca))
ma.markers.append(
visualization_msgs.Marker(
header=h,
id=i * 3 + 1,
type=visualization_msgs.Marker.CUBE,
action=visualization_msgs.Marker.ADD,
pose=geometry_msgs.Pose(
position=geometry_msgs.Point(
x=t['x'] + SHIFT * math.cos(th),
y=t['y'] + SHIFT * math.sin(th)),
orientation=q),
scale=sb,
color=cb))
ma.markers.append(
visualization_msgs.Marker(
header=h,
id=i * 3 + 2,
type=visualization_msgs.Marker.TEXT_VIEW_FACING,
action=visualization_msgs.Marker.ADD,
pose=geometry_msgs.Pose(
position=geometry_msgs.Point(x=t[1], y=t[2], z=0.5),
orientation=q),
scale=st,
color=ct,
text=str(t['id'])))
publisher.publish(ma)
def Lanecb(msg):
global pub
rate = rospy.Rate(10)
MarkerArr = viz.MarkerArray()
for i in range(len(msg.waypoints)):
Marker = viz.Marker()
Marker.header.frame_id = "map"
Marker.header.stamp = rospy.Time.now()
Marker.type = 0
Marker.action = 0
Marker.scale.x = 0.8
Marker.scale.y = 0.15
Marker.scale.z = 0.15
Marker.color.r = 0.1
Marker.color.g = 1.0
Marker.color.b = 0.0
Marker.color.a = 1.0
Marker.pose = msg.waypoints[i].pose.pose
Marker.id = i
MarkerArr.markers.append(Marker)
if pub is not None:
pub.publish(MarkerArr)
rate.sleep()
def make_cylinder_marker(self,
base,
length,
color,
frame='/base_link',
up_vector=np.array([0.0, 0.0, 1.0]),
**kwargs):
'''Handle the frustration that Rviz cylinders are designated by their center, not base'''
center = base + (up_vector * (length / 2))
pose = Pose(position=mil_ros_tools.numpy_to_point(center),
orientation=mil_ros_tools.numpy_to_quaternion(
[0.0, 0.0, 0.0, 1.0]))
marker = visualization_msgs.Marker(
ns='sub',
header=mil_ros_tools.make_header(frame=frame),
type=visualization_msgs.Marker.CYLINDER,
action=visualization_msgs.Marker.ADD,
pose=pose,
color=ColorRGBA(*color),
scale=Vector3(0.2, 0.2, length),
lifetime=rospy.Duration(),
**kwargs)
return marker
def circle_marker(center,
radius,
scale,
color,
mframe,
z=.03,
duration=10.0,
m_id=0,
resolution=1.):
angles = np.matrix(np.arange(0, 360., resolution)).T
xs = center[0, 0] + np.cos(angles) * radius
ys = center[1, 0] + np.sin(angles) * radius
z = .03
m = vm.Marker()
m.header.frame_id = mframe
m.id = m_id
m.type = create_mdict()['line_strip']
m.action = vm.Marker.ADD
m.points = [
gm.Point(xs[i, 0], ys[i, 0], z) for i in range(angles.shape[0])
]
m.colors = [
sdm.ColorRGBA(color[0, 0], color[1, 0], color[2, 0], color[3, 0])
for i in range(angles.shape[0])
]
m.scale.x = scale
m.lifetime = rospy.Duration(duration)
return m
def list_marker(points, colors, scale, mtype, mframe, duration=10.0, m_id=0):
m = vm.Marker()
m.header.frame_id = mframe
m.id = m_id
m.type = create_mdict()[mtype]
m.action = vm.Marker.ADD
m.points = [
gm.Point(points[0, i], points[1, i], points[2, i])
for i in range(points.shape[1])
]
#pdb.set_trace()
m.colors = [
sdm.ColorRGBA(colors[0, i], colors[1, i], colors[2, i], colors[3, i])
for i in range(colors.shape[1])
]
m.color.r = 1.
m.color.g = 0.
m.color.b = 0.
m.color.a = 1.
m.scale.x = scale[0]
m.scale.y = scale[1]
m.scale.z = scale[2]
m.lifetime = rospy.Duration(duration)
return m
def single_marker(point,
orientation,
mtype,
mframe,
scale=[.2, .2, .2],
color=[1.0, 0, 0, .5],
duration=10.0,
m_id=0):
m = vm.Marker()
m.header.frame_id = mframe
m.id = m_id
m.type = create_mdict()[mtype]
m.action = vm.Marker.ADD
m.pose.position.x = point[0, 0]
m.pose.position.y = point[1, 0]
m.pose.position.z = point[2, 0]
m.pose.orientation.x = orientation[0, 0]
m.pose.orientation.y = orientation[1, 0]
m.pose.orientation.z = orientation[2, 0]
m.pose.orientation.w = orientation[3, 0]
m.scale.x = scale[0]
m.scale.y = scale[1]
m.scale.z = scale[2]
m.color.r = color[0]
m.color.g = color[1]
m.color.b = color[2]
m.color.a = color[3]
m.lifetime = rospy.Duration(duration)
return m
def publish_octomap(pub_ns, arr, carr, size=0.1, stamp=None, frame_id='map', flip_rb=False):
"""
Publish cubes list:
"""
marker = vis_msg.Marker(type=vis_msg.Marker.CUBE_LIST, ns=pub_ns, action=vis_msg.Marker.ADD)
marker.header.frame_id = frame_id
marker.header.stamp = stamp if stamp is not None else rospy.Time.now()
# Point width, and height
marker.scale.x = size
marker.scale.y = size
marker.scale.z = size
N, D = arr.shape
# XYZ
inds, = np.where(~np.isnan(arr).any(axis=1))
marker.points = [geom_msg.Point(arr[j,0], arr[j,1], arr[j,2]) for j in inds]
# RGB (optionally alpha)
rax, bax = 0, 2
# carr = carr.astype(np.float32) * 1.0 / 255
if flip_rb: rax, bax = 2, 0
if D == 3:
marker.colors = [std_msg.ColorRGBA(carr[j,rax], carr[j,1], carr[j,bax], 1.0)
for j in inds]
elif D == 4:
marker.colors = [std_msg.ColorRGBA(carr[j,rax], carr[j,1], carr[j,bax], carr[j,3])
for j in inds]
marker.lifetime = rospy.Duration()
_publish_marker(marker)
def line_marker(id, stamp, path, color, ns):
msg = visualization_msgs.Marker()
msg.header.frame_id = "my_frame"
msg.header.stamp = stamp
msg.ns = ns
msg.id = id
msg.type = 4
msg.action = 0
msg.points = []
for point in path:
position = geometry_msgs.Point()
position.x = point[0]
position.y = point[1]
position.z = point[2] + 0.05
msg.points.append(position)
#msg.points = path
msg.scale.x = 0.05
msg.scale.y = 0.05
msg.scale.z = 0.05
msg.color.r = color[0]
msg.color.g = color[1]
msg.color.b = color[2]
msg.color.a = 1.0
msg.frame_locked = True
return msg
def drawPlan(self, plan):
m = vm.Marker()
m.type = 7
m.ns = 'lfd_info'
m.id = 6
m.header.frame_id = 'torso_lift_link'
npts = len(plan.points)
for i in xrange(npts):
p = geometry_msgs.msg.Point()
c = std_msgs.msg.ColorRGBA()
p.x = plan.points[i].positions[0]
p.y = plan.points[i].positions[1]
p.z = plan.points[i].positions[2]
c.r = float(2 * i) / float(npts)
if c.r > 1.0:
c.r = 1.0
c.g = 1 - c.r
c.b = 0
c.a = 1
m.points.append(p)
m.colors.append(c)
m.scale.x = 0.01
m.scale.y = 0.01
m.scale.z = 0.01
m.color.r = 1.0
m.color.g = 0.5
m.color.b = 0.5
m.color.a = 1.0
m.lifetime = rospy.Duration(0.0)
self.rviz_pub.publish(m)
def execute_control(self, u):
if u is not None:
twist_msg = geometry_msgs.Twist(
linear=geometry_msgs.Point(u[0], u[1], 0.),
angular=geometry_msgs.Point(0., 0., 0.)
)
self.cmd_vel_pub.publish(twist_msg)
### publish marker for debugging
marker = visualization_msgs.Marker()
marker.header.frame_id = '/map'
marker.type = marker.ARROW
marker.action = marker.ADD
marker.points = [
geometry_msgs.Point(0, 0, 0),
geometry_msgs.Point(u[0], u[1], 0)
]
marker.lifetime = rospy.Duration()
marker.scale.x = 0.05
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 1.0
self.cmd_vel_debug_pub.publish(marker)
else:
self.cmd_vel_pub.publish(None)
def drawLegScrew(self, which_screw):
m = vm.Marker()
m.type = 0
m.ns = 'lfd_info'
if (which_screw == 0):
m.header.frame_id = 'ar_marker_0'
m.color.r = 1.0
m.color.g = 0.0
m.color.b = 1.0
m.color.a = 1.0
m.id = 3
else:
m.header.frame_id = 'control_marker_goal'
m.color.r = 0.0
m.color.g = 1.0
m.color.b = 1.0
m.color.a = 1.0
m.id = 4
m.pose.position.x = -0.045
m.pose.position.y = 0.0
m.pose.position.z = -0.025
m.pose.orientation.x = 0.0
m.pose.orientation.y = 0.0
m.pose.orientation.z = 1.0
m.pose.orientation.w = 0.0
m.scale.x = 0.1
m.scale.y = 0.1
m.scale.z = 0.03
m.lifetime = rospy.Duration(0.0)
self.rviz_pub.publish(m)
def publish_cloud_markers(pub_ns, _arr, c='b', stamp=None, flip_rb=False, frame_id='map'):
"""
Publish point cloud on:
pub_ns: Namespace on which the cloud will be published
arr: numpy array (N x 3) for point cloud data
c: Option 1: 'c', 'b', 'r' etc colors accepted by matplotlibs color
Option 2: float ranging from 0 to 1 via matplotlib's jet colormap
Option 3: numpy array (N x 3) with r,g,b vals ranging from 0 to 1
s: supported only by matplotlib plotting
alpha: supported only by matplotlib plotting
"""
arr, carr = copy_pointcloud_data(_arr, c, flip_rb=flip_rb)
marker = vis_msg.Marker(type=vis_msg.Marker.SPHERE_LIST, ns=pub_ns, action=vis_msg.Marker.ADD)
marker.header.frame_id = frame_id
marker.header.stamp = stamp if stamp is not None else rospy.Time.now()
marker.scale.x = 0.01
marker.scale.y = 0.01
# XYZ
inds, = np.where(~np.isnan(arr).any(axis=1))
marker.points = [geom_msg.Point(arr[j,0], arr[j,1], arr[j,2]) for j in inds]
# RGB (optionally alpha)
N, D = arr.shape[:2]
if D == 3:
marker.colors = [std_msg.ColorRGBA(carr[j,0], carr[j,1], carr[j,2], 1.0)
for j in inds]
elif D == 4:
marker.colors = [std_msg.ColorRGBA(carr[j,0], carr[j,1], carr[j,2], carr[j,3])
for j in inds]
marker.lifetime = rospy.Duration()
_publish_marker(marker)
print 'Publishing marker', N
def drawPath(self, path):
m = vm.Marker()
m.type = 7
m.ns = 'lfd_info'
m.id = 5
m.header.frame_id = 'torso_lift_link'
npts = len(path)
i = 0
for element in path:
p = geometry_msgs.msg.Point()
p.x = element[0]
p.y = element[1]
p.z = element[2]
m.points.append(p)
c = std_msgs.msg.ColorRGBA()
c.r = float(2 * i) / float(npts)
if c.r > 1.0:
c.r = 1.0
c.g = 1 - c.r
c.b = 0
c.a = 1
m.colors.append(c)
i += 1
m.scale.x = 0.01
m.scale.y = 0.01
m.scale.z = 0.01
m.color.r = 1.0
m.color.g = 0.5
m.color.b = 0.5
m.color.a = 1.0
m.lifetime = rospy.Duration(0.0)
self.rviz_pub.publish(m)
def talker():
pub = rospy.Publisher('visualization_marker', vm.Marker, queue_size=10)
rospy.init_node('waypoint_course')
frame = rospy.get_param('~frame', 'odom')
dist = rospy.get_param('~size', 2)
XX = [0, dist, -dist, -dist, dist]
YY = [0, dist, dist, -dist, -dist]
m = vm.Marker()
m.type = m.SPHERE_LIST
m.action = m.ADD
s = 0.2
m.scale.x = s
m.scale.y = s
m.scale.z = s
m.color.r = 1.0
m.color.g = 0
m.color.a = 1.0
m.pose.orientation.w = 1.0
m.header.stamp = rospy.Time.now()
m.header.frame_id = frame
m.id = 1
for x, y, mid in zip(XX, YY, range(len(XX))):
p = gm.Point()
p.x = x
p.y = y
m.points.append(p)
cnt = 0
while ((cnt < 25) and (not rospy.is_shutdown())):
print("Publishing Sphere List")
pub.publish(m)
rospy.sleep(0.5)
cnt += 1
def publish_fixtures_shape(self):
v = self.domain.dynamics.manipuland_fixture.shape.vertices
points = planar_shape_to_point3(self.scale_vertices(v))
marker = viz_msg.Marker()
marker.header.frame_id = "object"
marker.header.stamp = self.get_domain_sim_time()
marker.ns = "object_fixture"
marker.id = 0
marker.type = viz_msg.Marker.LINE_STRIP
marker.action = viz_msg.Marker.ADD
marker.pose.orientation.w = 1.0 #identity transform
marker.scale.x = 0.005
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.points = points
marker.lifetime = rospy.Duration(0.1)
self.viz_markers_publisher.publish(marker)
for i, fixture in enumerate(self.domain.dynamics.ground_body.fixtures):
v = fixture.shape.vertices
points = planar_shape_to_point3(self.scale_vertices(v))
marker = viz_msg.Marker()
marker.header.frame_id = "jig"
marker.header.stamp = self.get_domain_sim_time()
marker.ns = "jig_fixture"
marker.id = i
marker.type = viz_msg.Marker.LINE_STRIP
marker.action = viz_msg.Marker.ADD
marker.pose.orientation.w = 1.0 #identity transform
marker.scale.x = 0.005
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
marker.points = points
marker.lifetime = rospy.Duration(0.1)
self.viz_markers_publisher.publish(marker)
def linepub():
rospy.init_node("rviz_marker", anonymous=True)
sc = ScanSubscriber()
rospy.Subscriber("/scan", senmsg.LaserScan, sc.scanCallBack)
pub_free = rospy.Publisher("free_space_polygon",
vismsg.Marker,
queue_size=1)
rate = rospy.Rate(25)
# mark_f - free space marker
mark_f = vismsg.Marker()
mark_f.header.frame_id = "/laser"
mark_f.type = mark_f.LINE_STRIP
mark_f.action = mark_f.ADD
mark_f.scale.x = 0.2
mark_f.color.r = 0.1
mark_f.color.g = 0.4
mark_f.color.b = 0.9
mark_f.color.a = 0.9 # 90% visibility
mark_f.pose.orientation.x = mark_f.pose.orientation.y = mark_f.pose.orientation.z = 0.0
mark_f.pose.orientation.w = 1.0
mark_f.pose.position.x = mark_f.pose.position.y = mark_f.pose.position.z = 0.0
while not rospy.is_shutdown():
if free_space is not None:
# marker line points
mark_f.points = []
pl1 = geomsg.Point()
pl1.x = -0.2
pl1.y = -1.0
pl1.z = 0.0 # x an y mismatch?
pl2 = geomsg.Point()
pl2.x = -0.2
pl2.y = 0.6
pl2.z = 0.0
mark_f.points.append(pl1)
mark_f.points.append(pl2)
try:
if type(free_space) is sg.polygon.Polygon:
for l in free_space.exterior.coords[
1:]: # the last point is the same as the first
p = geomsg.Point()
p.x = l[0]
p.y = l[1]
p.z = 0.0
mark_f.points.append(p)
except:
rospy.logwarn("exception")
mark_f.points.append(pl1)
# Publish the Markers
pub_free.publish(mark_f)
rate.sleep()
def clear():
pub = rospy.Publisher('visualization_marker',
vm.Marker,
latch=True,
queue_size=10)
if not rospy.is_shutdown():
m = vm.Marker()
m.type = m.SPHERE_LIST
m.action = m.DELETE
pub.publish(m)
def publish_line_segments(pub_ns,
_arr1,
_arr2,
c='b',
stamp=None,
frame_id='camera',
flip_rb=False,
size=0.002):
"""
Publish point cloud on:
pub_ns: Namespace on which the cloud will be published
arr: numpy array (N x 3) for point cloud data
c: Option 1: 'c', 'b', 'r' etc colors accepted by matplotlibs color
Option 2: float ranging from 0 to 1 via matplotlib's jet colormap
Option 3: numpy array (N x 3) with r,g,b vals ranging from 0 to 1
s: supported only by matplotlib plotting
alpha: supported only by matplotlib plotting
"""
arr1, carr = copy_pointcloud_data(_arr1, c, flip_rb=flip_rb)
arr2 = (deepcopy(_arr2)).reshape(-1, 3)
marker = vis_msg.Marker(type=vis_msg.Marker.LINE_LIST,
ns=pub_ns,
action=vis_msg.Marker.ADD)
if not arr1.shape == arr2.shape: raise AssertionError
marker.header.frame_id = frame_id
marker.header.stamp = stamp if stamp is not None else rospy.Time.now()
marker.scale.x = size
marker.scale.y = size
marker.color.b = 1.0
marker.color.a = 1.0
marker.pose.position = geom_msg.Point(0, 0, 0)
marker.pose.orientation = geom_msg.Quaternion(0, 0, 0, 1)
# Handle 3D data: [ndarray or list of ndarrays]
arr12 = np.hstack([arr1, arr2])
inds, = np.where(~np.isnan(arr12).any(axis=1))
marker.points = []
for j in inds:
marker.points.extend([
geom_msg.Point(arr1[j, 0], arr1[j, 1], arr1[j, 2]),
geom_msg.Point(arr2[j, 0], arr2[j, 1], arr2[j, 2])
])
# RGB (optionally alpha)
marker.colors = [
std_msg.ColorRGBA(carr[j, 0], carr[j, 1], carr[j, 2], 1.0)
for j in inds
]
marker.lifetime = rospy.Duration()
_publish_marker(marker)
def __init__(self, f1, f2, listener, color='black'):
self.marker = VM.Marker()
self.marker.type = VM.Marker.LINE_LIST
self.marker.header.frame_id = "world"
self.marker.id = hash(f1+f2)%(2**16)
self.marker.scale = GM.Vector3(*(0.01,0.01,0.01))
self.set_color(color)
self.listener = listener
self.f1 = f1
self.f2 = f2
self.update_points()
def __init__(self, m):
self.marker = VM.Marker()
self.marker.type = VM.Marker.LINE_STRIP
scale = m.scale.x/10.0
self.marker.scale = GM.Vector3(*(scale, scale, scale))
self.marker.header.frame_id = m.header.frame_id
self.marker.id = hash(m.id + self.marker.type)%(2**16)
self.marker.color = m.color
self.marker.color.a = 1.0
self.point_list = []
self.max_size = rospy.get_param('path_len', NUMBER_MESSAGES)
self.update_path(m)
def arrow(id, stamp, start_point, direction, color):
msg = visualization_msgs.Marker()
msg.header.frame_id = "my_frame"
msg.header.stamp = stamp
msg.id = id
msg.type = 0
msg.action = 0
msg.pose = geometry_msgs.Pose()
msg.pose.position.x = start_point[0]
msg.pose.position.y = start_point[1]
msg.pose.position.z = start_point[2]
#calculating the half-way vector.
u = [1, 0, 0]
norm = np.linalg.norm(direction)
v = np.asarray(direction) / norm
if (np.array_equal(u, v)):
msg.pose.orientation.w = 1
msg.pose.orientation.x = 0
msg.pose.orientation.y = 0
msg.pose.orientation.z = 0
elif (np.array_equal(u, np.negative(v))):
msg.pose.orientation.w = 0
msg.pose.orientation.x = 0
msg.pose.orientation.y = 0
msg.pose.orientation.z = 1
else:
half = [u[0] + v[0], u[1] + v[1], u[2] + v[2]]
msg.pose.orientation.w = np.dot(u, half)
temp = np.cross(u, half)
msg.pose.orientation.x = temp[0]
msg.pose.orientation.y = temp[1]
msg.pose.orientation.z = temp[2]
norm = np.math.sqrt(msg.pose.orientation.x * msg.pose.orientation.x +
msg.pose.orientation.y * msg.pose.orientation.y +
msg.pose.orientation.z * msg.pose.orientation.z +
msg.pose.orientation.w * msg.pose.orientation.w)
if norm == 0:
norm = 1
msg.pose.orientation.x /= norm
msg.pose.orientation.y /= norm
msg.pose.orientation.z /= norm
msg.pose.orientation.w /= norm
msg.scale.x = 1.0
msg.scale.y = 0.1
msg.scale.z = 0.1
msg.color.r = color[0]
msg.color.g = color[1]
msg.color.b = color[2]
msg.color.a = 1.0
msg.frame_locked = True
return msg
def create_marker(w):
global id_count
m = viz_msgs.Marker()
m.header.frame_id = w['frame_id']
m.ns = w['name']
m.id = id_count
m.action = viz_msgs.Marker.ADD
m.pose = create_geo_pose(w)
m.scale = geometry_msgs.Vector3(1.0,0.3,0.3)
m.color = std_msgs.ColorRGBA(0.0,1.0,0.0,1.0)
id_count = id_count + 1
return m
def publish_pose_list(pub_ns, _poses, texts=[], stamp=None, size=0.05, frame_id='camera', seq=1):
"""
Publish Pose List on:
pub_channel: Channel on which the cloud will be published
"""
poses = deepcopy(_poses)
if not len(poses): return
arrs = np.vstack([pose.matrix[:3,:3].T.reshape((1,9)) for pose in poses]) * size
arrX = np.vstack([pose.tvec.reshape((1,3)) for pose in poses])
arrx, arry, arrz = arrs[:,0:3], arrs[:,3:6], arrs[:,6:9]
# Point width, and height
N = len(poses)
markers = vis_msg.Marker(type=vis_msg.Marker.LINE_LIST, ns=pub_ns, action=vis_msg.Marker.ADD)
markers.header.frame_id = frame_id
markers.header.stamp = stamp if stamp is not None else rospy.Time.now()
markers.header.seq = seq
markers.scale.x = size/20 # 0.01
markers.scale.y = size/20 # 0.01
markers.color.a = 1.0
markers.pose.position = geom_msg.Point(0,0,0)
markers.pose.orientation = geom_msg.Quaternion(0,0,0,1)
markers.points = []
markers.lifetime = rospy.Duration()
for j in range(N):
markers.points.extend([geom_msg.Point(arrX[j,0], arrX[j,1], arrX[j,2]),
geom_msg.Point(arrX[j,0] + arrx[j,0],
arrX[j,1] + arrx[j,1],
arrX[j,2] + arrx[j,2])])
markers.colors.extend([std_msg.ColorRGBA(1.0, 0.0, 0.0, 1.0), std_msg.ColorRGBA(1.0, 0.0, 0.0, 1.0)])
markers.points.extend([geom_msg.Point(arrX[j,0], arrX[j,1], arrX[j,2]),
geom_msg.Point(arrX[j,0] + arry[j,0],
arrX[j,1] + arry[j,1],
arrX[j,2] + arry[j,2])])
markers.colors.extend([std_msg.ColorRGBA(0.0, 1.0, 0.0, 1.0), std_msg.ColorRGBA(0.0, 1.0, 0.0, 1.0)])
markers.points.extend([geom_msg.Point(arrX[j,0], arrX[j,1], arrX[j,2]),
geom_msg.Point(arrX[j,0] + arrz[j,0],
arrX[j,1] + arrz[j,1],
arrX[j,2] + arrz[j,2])])
markers.colors.extend([std_msg.ColorRGBA(0.0, 0.0, 1.0, 1.0), std_msg.ColorRGBA(0.0, 0.0, 1.0, 1.0)])
_publish_marker(markers)
def visualize(self, samples, costs):
marker_array = vm.MarkerArray()
for i, (sample, cost) in enumerate(zip(samples, costs)):
if sample is None:
continue
origin = np.array([0., 0., 0.])
angle = 0.
for t in xrange(len(sample.get_U())):
marker = vm.Marker()
marker.id = i * len(sample.get_U()) + t
marker.header.frame_id = '/map'
marker.type = marker.ARROW
marker.action = marker.ADD
speed = sample.get_U(t=t, sub_control='cmd_vel')[0] / 5.
steer = sample.get_U(t=t, sub_control='cmd_steer')[0]
angle += (steer - 50.) / 100. * (np.pi / 2)
new_origin = origin + [
speed * np.cos(angle), speed * np.sin(angle), 0.
]
marker.points = [
gm.Point(*origin.tolist()),
gm.Point(*new_origin.tolist())
]
origin = new_origin
marker.lifetime = rospy.Duration()
marker.scale.x = 0.05
marker.scale.y = 0.1
marker.scale.z = 0.1
if cost == min(costs):
rgba = (0., 1., 0., 1.)
marker.scale.x *= 2
marker.scale.y *= 2
marker.scale.z *= 2
else:
rgba = list(cm.hot(1 - cost))
rgba[-1] = 0.5
marker.color.r, marker.color.g, marker.color.b, marker.color.a = rgba
marker_array.markers.append(marker)
# for id in xrange(marker.id+1, int(1e4)):
# marker_array.markers.append(vm.Marker(id=id, action=marker.DELETE))
self.debug_cost_probcoll_pub.publish(marker_array)
def text_marker(text, center, color, scale, mframe, duration=10.0, m_id=0):
m = vm.Marker()
m.header.frame_id = mframe
m.id = m_id
m.type = create_mdict()['text_view_facing']
m.action = vm.Marker.ADD
m.text = text
m.scale.z = scale
m.pose.position.x = center[0, 0]
m.pose.position.y = center[1, 0]
m.pose.position.z = center[2, 0]
m.lifetime = rospy.Duration(duration)
return m
def make_ray(base, direction, length, color, frame='/base_link', m_id=0, **kwargs):
'''Handle the frustration that Rviz cylinders are designated by their center, not base'''
marker = visualization_msgs.Marker(
ns='wamv',
id=m_id,
header=mil_ros_tools.make_header(frame=frame),
type=visualization_msgs.Marker.LINE_STRIP,
action=visualization_msgs.Marker.ADD,
color=ColorRGBA(*color),
scale=Vector3(0.05, 0.05, 0.05),
points=map(lambda o: Point(*o), [base, direction * length]),
lifetime=rospy.Duration(),
**kwargs
)
return marker
def __init__(self, part_name,
x=0, y=0, z=0, # initial origin
roll=0, pitch=0, yaw=0, # initial rpy
rate=50, mesh="", # Rate and optional mesh
red=0.53, green=0.53, blue=0.53, alpha=1): # meshcol
BasePublisher.__init__(self, prefix=part_name, rate=rate)
part_frame = geometry_msgs.msg.TransformStamped()
part_frame.header.frame_id = "world"
part_frame.child_frame_id = self.prefix + "_part_frame"
part_frame.transform.translation.x = x
part_frame.transform.translation.y = y
part_frame.transform.translation.z = z
q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
part_frame.transform.rotation.x = q[0]
part_frame.transform.rotation.y = q[1]
part_frame.transform.rotation.z = q[2]
part_frame.transform.rotation.w = q[3]
self.addFrame(part_frame)
if not mesh == "":
self.mesh_available = True
self.pub_mesh = rospy.Publisher("visualization_marker",
viz_msg.Marker,
queue_size=1)
self.mesh_marker = viz_msg.Marker()
self.mesh_marker.header.frame_id = self.prefix + "_part_frame"
self.mesh_marker.ns = self.prefix
self.mesh_marker.id = 0
self.mesh_marker.type = viz_msg.Marker.MESH_RESOURCE
self.mesh_marker.action = 0
self.mesh_marker.pose.position.x = 0
self.mesh_marker.pose.position.y = 0
self.mesh_marker.pose.position.z = 0
self.mesh_marker.pose.orientation.x = 0
self.mesh_marker.pose.orientation.y = 0
self.mesh_marker.pose.orientation.z = 0
self.mesh_marker.pose.orientation.w = 1
self.mesh_marker.scale.x = 1e-3
self.mesh_marker.scale.y = 1e-3
self.mesh_marker.scale.z = 1e-3
self.mesh_marker.color.r = red
self.mesh_marker.color.g = green
self.mesh_marker.color.b = blue
self.mesh_marker.color.a = alpha
self.mesh_marker.lifetime = rospy.Time.from_sec(1.0/rate)
self.mesh_marker.mesh_resource = mesh
else:
self.mesh_available = False
def drawMarkerGoal(self, goal):
m = vm.Marker()
m.type = 1
m.ns = 'lfd_info'
m.id = 1
m.header.frame_id = 'torso_lift_link'
m.pose = self.gen_utils.vecToRosPose(goal)
m.scale.x = 0.045
m.scale.y = 0.045
m.scale.z = 0.008
m.color.r = 0.0
m.color.g = 1.0
m.color.b = 1.0
m.color.a = 1.0
m.lifetime = rospy.Duration(0.0)
self.rviz_pub.publish(m)
def drawArrow(self, pos, s, c):
m = vm.Marker()
m.type = 0
m.ns = 'lfd_info'
m.id = 0
m.header.frame_id = 'torso_lift_link'
m.pose = self.gen_utils.vecToRosPose(pos)
m.scale.x = 0.20
m.scale.y = 0.05
m.scale.z = 0.01
m.color.r = 0.0
m.color.g = 0.0
m.color.b = 1.0
m.color.a = 1.0
m.lifetime = rospy.Duration(0.0)
self.rviz_pub.publish(m)
