def add_marker(self, array, track, color, tid):
m1 = Marker()
m1.header.frame_id = "camera_rgb_optical_frame"
m1.ns = "line"
m1.id = tid
m1.type = 4 #lines
m1.action = 0
m1.scale.x = .002
m1.color.a = 1.
m1.color.r = color[0]/255.
m1.color.g = color[1]/255.
m1.color.b = color[2]/255.
m1.points = track
array.append(m1)
m2 = Marker()
m2.header.frame_id = "camera_rgb_optical_frame"
m2.ns = "point"
m2.id = tid
m2.type = 8 #points
m2.action = 0
m2.scale.x = .008
m2.scale.y = .008
m2.color.a = 1.
m2.color.r = color[0]/255.
m2.color.g = color[1]/255.
m2.color.b = color[2]/255.
m2.points = [ track[-1] ]
array.append(m2)
def node():
pub = rospy.Publisher('shapes', Marker, queue_size=10)
rospy.init_node('plotter', anonymous=False)
rate = rospy.Rate(1)
points=Marker()
line=Marker()
#Set the frame ID and timestamp. See the TF tutorials for information on these.
points.header.frame_id=line.header.frame_id="/map"
points.header.stamp=line.header.stamp=rospy.Time.now()
points.ns=line.ns = "markers"
points.id = 0
line.id =2
points.type = Marker.POINTS
line.type=Marker.LINE_STRIP
#Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
points.action = line.action = Marker.ADD;
points.pose.orientation.w = line.pose.orientation.w = 1.0;
line.scale.x = 0.02;
points.scale.x=0.03;
line.scale.y= 0.02;
points.scale.y=0.03;
line.color.r = 1.0;
points.color.g = 1.0;
points.color.a=line.color.a = 1.0;
points.lifetime =line.lifetime = rospy.Duration();
p=Point()
p.x = 1;
p.y = 1;
p.z = 1;
pp=[]
pp.append(copy(p))
while not rospy.is_shutdown():
p.x+=0.1
pp.append(copy(p))
points.points=pp
#print points.points,'\n','----------------','\n'
line.points=pp
pub.publish(points)
pub.publish(line)
rate.sleep()
def display_init_config(points, old=False, clear=False):
"""
Displays config on screen.
Assumes markers already in the right place for frame camera1_rgb_optical_frame.
"""
global rviz_marker, rviz_pub
if rviz_pub is None:
rviz_pub = rospy.Publisher('init_config', Marker)
if rviz_marker is None:
rviz_marker = Marker()
rviz_marker.type = Marker.POINTS
rviz_marker.action = Marker.ADD
if clear:
rviz_marker.points = []
rviz_pub.publish(rviz_marker)
return
if old and points is not None:
rviz_pub.publish(rviz_marker)
return
if points is None:
return
# rviz_marker.type = Marker.SPHERE
color = ColorRGBA(0.5,0,1,1)
rviz_marker.scale.x = 0.01
rviz_marker.scale.y = 0.01
rviz_marker.scale.z = 0.01
rviz_marker.pose.position.x = 0.0
rviz_marker.pose.position.y = 0.0
rviz_marker.pose.position.z = 0.0
rviz_marker.pose.orientation.w = 1.0
rviz_marker.pose.orientation.x = 0.0
rviz_marker.pose.orientation.y = 0.0
rviz_marker.pose.orientation.z = 0.0
rviz_marker.color = color
rviz_marker.points = []
for point in points:
rviz_marker.points.append(Point(*point))
rviz_marker.colors.append(color)
rviz_marker.header.frame_id = 'camera1_rgb_optical_frame'
rviz_marker.header.stamp = rospy.Time.now()
rviz_pub.publish(rviz_marker)
def visualize_traj(points):
traj = Marker()
traj.header.frame_id = FRAME
traj.header.stamp = rospy.get_rostime()
traj.ns = "love_letter"
traj.action = Marker.ADD
traj.pose.orientation.w = 1.0
traj.type = Marker.LINE_STRIP
traj.scale.x = 0.001 # line width
traj.color.r = 1.0
traj.color.b = 0.0
traj.color.a = 1.0
if(WRITE_MULTIPLE_SHAPES):
traj.id = shapeCount;
else:
traj.id = 0; #overwrite any existing shapes
traj.lifetime.secs = 1; #timeout for display
traj.points = list(points)
# use interactive marker from place_paper instead
#pub_markers.publish(a4_sheet())
pub_markers.publish(traj)
def makeMarker(self, pos, vec, idNum=1, color=(1,0,0)):
m = Marker()
m.id = idNum
m.ns = "test"
m.header.frame_id = "/camera_rgb_optical_frame"
m.type = Marker.ARROW
m.action = Marker.ADD
markerPt1 = Point()
markerPt2 = Point()
markerPt1.x = pos[0];
markerPt1.y = pos[1];
markerPt1.z = pos[2];
markerPt2.x = markerPt1.x + vec[0];
markerPt2.y = markerPt1.y + vec[1];
markerPt2.z = markerPt1.z + vec[2];
m.points = [markerPt1, markerPt2]
m.scale.x = .1;
m.scale.y = .1;
m.scale.z = .1;
m.color.a = 1;
m.color.r = color[0];
m.color.g = color[1];
m.color.b = color[2];
#m.lifetime = rospy.Duration()
return m
def marker(self, position=None, orientation=None, points=None, colors=None, scale=1.0):
mark = Marker()
mark.header.stamp = rospy.Time.now()
mark.header.frame_id = self.frame_id
mark.ns = self.ns
mark.type = self.type
mark.id = self.counter
mark.action = self.action
mark.scale.x = self.scale[0]
mark.scale.y = self.scale[1]
mark.scale.z = self.scale[2]
mark.color.r = self.color[0]
mark.color.g = self.color[1]
mark.color.b = self.color[2]
mark.color.a = self.color[3]
mark.lifetime = rospy.Duration(self.lifetime)
if points is not None:
mark.points = []
for point in points:
mark.points.append(get_point(point, scale))
if colors is not None:
mark.colors = colors
if position is not None or orientation is not None:
mark.pose.position = get_point(position, scale)
mark.pose.orientation = get_quat(orientation)
self.counter+=1
return mark
def addPolygonFilled(self, points):
oid = self.oid+1
name = "/polygonFilled"+str(self.oid)
marker = Marker()
marker.id = self.oid
marker.ns = "/polygonFilled"
marker.header.frame_id = name
marker.type = marker.TRIANGLE_LIST
marker.action = marker.ADD
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.points = []
for i in range(0,len(points)-2,1):
pt = Point(points[0][0], points[0][1], points[0][2])
marker.points.append(pt)
pt = Point(points[i+1][0], points[i+1][1], points[i+1][2])
marker.points.append(pt)
pt = Point(points[i+2][0], points[i+2][1], points[i+2][2])
marker.points.append(pt)
self.markerArray.markers.append(marker)
def line(self, frame_id, p, r, t=[0.0, 0.0], key=None):
"""
line: t r + p
This will be drawn for t[0] .. t[1]
"""
r = np.array(r)
p = np.array(p)
m = Marker()
m.header.frame_id = frame_id
m.ns = key or 'lines'
m.id = 0 if key else len(self.lines)
m.type = Marker.LINE_STRIP
m.action = Marker.MODIFY
m.pose = Pose(Point(0,0,0), Quaternion(0,0,0,1))
m.scale = Vector3(0.005, 0.005, 0.005)
m.color = ColorRGBA(0,0.8,0.8,1)
m.points = [Point(*(p+r*t)) for t in np.linspace(t[0], t[1], 10)]
m.colors = [m.color] * 10
key = key or element_key(m)
with self.mod_lock:
self.lines[key] = m
return key
def input_handler(space):
global marker_color
global marker_pub
global matches
for str in matches:
if space.aux_payload.find(str) == -1:
# print "FAIL: str='%s' aux_payload='%s'" % (str, space.aux_payload)
return
# print space
# print
marker = Marker()
marker.header.frame_id = "/map"
marker.header.stamp = rospy.Time()
marker.ns = ""
marker.id = 0
marker.type = Marker.POINTS
marker.action = Marker.ADD
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 0
marker.pose.orientation.w = 0
marker.scale.x = .1
marker.scale.y = .1
marker.scale.z = .1
marker.color = marker_color
marker.points = []
# this line cuts off anything before the string "convex_set: " in the aux_payload
convex_set = space.aux_payload[ space.aux_payload.find("convex_set: ") :]
# this line cuts off that string at the next newline character
convex_set = convex_set.split('\n')[0]
# this line splits the string into pieces delineated by spaces
convex_set = convex_set.split(' ')
# print convex_set
for candidate in convex_set:
if len(candidate) == 0 or candidate[0] != '(':
continue
coords = candidate[1:-1].split(',')
# print coords
marker.points.append(Point(float(coords[0]), float(coords[1]), float(coords[2])))
# print marker.points[-1]
# print
# print marker.points
# print
marker_pub.publish(marker)
def arrow_marker(position, direction, shaft_radius=0.02, head_radius=0.04, rgba=[1.0, 1.0, 1.0, 1.0],
ns="", id=0, frame_id="", lifetime=None, stamp=None):
marker = Marker()
if stamp is None:
marker.header.stamp = rospy.Time.now()
else:
marker.header.stamp = stamp
marker.header.frame_id = frame_id
marker.ns = ns
marker.id = id
marker.type = Marker.ARROW
marker.action = Marker.ADD
#marker.pose.position.x = position[0]
#marker.pose.position.y = position[1]
#marker.pose.position.z = position[2]
marker.points = [Point(*tuple(position)), Point(*tuple(position+direction*1.0))]
marker.scale.x = shaft_radius
marker.scale.y = head_radius
marker.scale.z = 1.0
marker.color.r = rgba[0]
marker.color.g = rgba[1]
marker.color.b = rgba[2]
marker.color.a = rgba[3]
if lifetime is None:
marker.lifetime = rospy.Duration()
else:
marker.lifetime = rospy.Duration(lifetime)
return marker
def create_marker(self, markerArray, view, pose, view_values):
marker1 = Marker()
marker1.id = self.marker_id
self.marker_id += 1
marker1.header.frame_id = "/map"
marker1.type = marker1.TRIANGLE_LIST
marker1.action = marker1.ADD
marker1.scale.x = 1
marker1.scale.y = 1
marker1.scale.z = 2
marker1.color.a = 0.25
vals = view_values.values()
max_val = max(vals)
non_zero_vals = filter(lambda a: a != 0, vals)
min_val = min(non_zero_vals)
print min_val, max_val, view_values[view.ID]
marker1.color.r = r_func( float((view_values[view.ID] - min_val)) / float((max_val - min_val + 1)))
marker1.color.g = g_func( float((view_values[view.ID] - min_val)) / float((max_val - min_val + 1)))
marker1.color.b = b_func( float((view_values[view.ID] - min_val)) / float((max_val - min_val + 1)))
marker1.pose.orientation = pose.orientation
marker1.pose.position = pose.position
marker1.points = [Point(0,0,0.01),Point(2.5,-1.39,0.01),Point(2.5,1.39,0.01)]
markerArray.markers.append(marker1)
def draw_curve(
self,
pose_array,
id=None,
rgba=(0, 1, 0, 1),
width=.01,
ns='default_ns',
duration=0,
type=Marker.LINE_STRIP,
):
if id is None:
id = self.get_unused_id()
marker = Marker(type=type, action=Marker.ADD)
marker.header = pose_array.header
marker.points = [pose.position for pose in pose_array.poses]
marker.lifetime = rospy.Duration(0)
if isinstance(rgba, list):
assert len(rgba) == len(pose_array.poses)
marker.colors = [stdm.ColorRGBA(*c) for c in rgba]
else:
marker.color = stdm.ColorRGBA(*rgba)
marker.scale = gm.Vector3(width, width, width)
marker.id = id
marker.ns = ns
self.pub.publish(marker)
self.ids.add(id)
return MarkerHandle(marker, self.pub)
def get_visualization_markers(self, type='hough_lines'):
"""
Returns a visualization_msgs.Marker Message
with the lines from the Hough Transform in the frame of the scanner.
"""
if type == 'hough_lines':
marker = Marker()
marker.header.frame_id = self.scan.frame_id
marker.header.stamp = self.scan.timestamp
marker.ns = "hough_lines"
marker.id = 0
marker.scale.x = 0.01
marker.scale.y = 0.01
marker.scale.z = 0.01
marker.type = marker.LINE_LIST
marker.action = marker.ADD
marker.lifetime = rospy.Duration()
marker.points = []
for (pt1, pt2) in self.get_lines_meters():
marker.points.append(Point(pt1[1], pt1[0], 0))
marker.points.append(Point(pt2[1], pt2[0], 0))
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0
return marker
def otld_callback(self, data):
print "Heard: " + str(data)
# get the depth image
s = rospy.Subscriber("/camera/depth_registered/points", PointCloud2, self.p2callback)
while not self.got_pointcloud:
# do nothing
print "Waiting for pointcloud"
rospy.sleep(0.1)
s.unregister()
self.got_pointcloud = False
# get the depth of the tracked area
# Header header
# int32 x
# int32 y
# int32 width
# int32 height
# float32 confidence
# probably first get the centroid, we should do a median here or something
centerx = data.x + data.width / 2
centery = data.y + data.height / 2
#print "self.pointcloud.row_step: " + str(self.pointcloud.row_step) + " self.pointcloud.point_step: " + str(self.pointcloud.point_step)
print "centerx: " + str(centerx) + " centery: " + str(centery)
#pointdepth = self.pointcloud.data[self.pointcloud.row_step * centery : self.pointcloud.row_step * centery + self.pointcloud.point_step]
#point = read_points(self.pointcloud, field_names=None, skip_nans=False, uvs=[centerx, centery])
point = read_points(self.pointcloud, field_names=None, skip_nans=False, uvs=[[centerx, centery]])
print "point is: " + str(point)
for item in point:
print "item is: " + str(item)
print "x: "+ str(item[0]) + " y: " + str(item[1]) + ", z: " + str(item[2])
# create PoseStamped
pose = PoseStamped()
pose.header = std_msgs.msg.Header()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "camera_link"
pose.pose = Pose()
pose.pose.position.x = item[2] # kinect Z value, [2], is X in TF of camera_link
pose.pose.position.y = - item[0] # kinect X value, [0], is -Y in TF of camera_link
pose.pose.position.z = - item[1] # kinect Y value, [1], is -Z in TF of camera_link
pose.pose.orientation.w = 1
# send PoseStamped
self.pub.publish(pose)
arrow = Marker()
arrow.header.frame_id = "camera_link"
arrow.header.stamp = rospy.Time.now()
arrow.type = Marker.ARROW
arrow.points = [Point(0,0,0), Point(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)]
#arrow.points = [Point(0,0,0), Point(0,0,1)]
arrow.scale.x = 0.1 # anchura del palo
arrow.scale.y = 0.15 # anchura de la punta
# arrow.scale.z = 0.1
arrow.color.a = 1.0
arrow.color.r = 1.0
arrow.color.g = 1.0
arrow.color.b = 1.0
self.marker_publisher.publish(arrow)
def animate_path(circle_length, rate_animate):
animate_pub = rospy.Publisher("marker_visualization", Marker, queue_size=10)
rate = rospy.Rate(rate_animate)
marker = Marker()
marker.header.frame_id = "base" # marker is referenced to base frame
marker.header.stamp = rospy.Time.now()
marker.type = marker.LINE_STRIP
marker.action = marker.ADD
marker.scale.x = 0.01 # define the size of marker
marker.color.a = 1.0
marker.color.g = 1.0
while not rospy.is_shutdown():
try:
(trans, rot) = listener.lookupTransform("base", "end", rospy.Time())
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
p = Point() # Define a point array that takes in x, y and z values
p.x = trans[0]
p.y = trans[1]
p.z = trans[2]
marker.points = deque(
marker.points, circle_length
) # Use deque to prevent list to increasing indefinitely; with this, after a certain circle_length, the list starts building from the beginning while replacing earlier elements
marker.points.append(p)
animate_pub.publish(marker)
rate.sleep()
def draw_risk_grid(self, risk_grid):
if not rospy.is_shutdown():
p_list = list()
c_list = list()
x_gen = xrange(0, self.x_dim, 1)
y_gen = xrange(0, self.y_dim, 1)
for i in x_gen:
for j in y_gen:
risk = risk_grid[i, j]
pnt = Point(i, j, 0)
r, g, b, a = self.sm.to_rgba(risk)
clr = ColorRGBA(r, g, b, a)
p_list.append(pnt)
c_list.append(clr)
marker = Marker()
marker.header.frame_id = "/my_frame"
marker.lifetime = rospy.Duration(10000000)
marker.type = marker.POINTS
marker.scale.x = 1
marker.scale.y = 1
marker.action = marker.ADD
marker.points = p_list
marker.colors = c_list
marker.id = -1
self.pub.publish(marker)
return self
def _create_add_line(self, points, edges, point_group_name, id, red, green, blue):
all_points = []
for i, end_points in enumerate(edges):
for endpoint_index in end_points:
pt1, pt2 = Point(), Point()
pt1.x, pt1.y, pt1.z = points[i][0], points[i][1], points[i][2]
all_points.append(pt1)
pt2.x, pt2.y, pt2.z = points[endpoint_index][0], points[endpoint_index][1], points[endpoint_index][2]
all_points.append(pt2)
marker = Marker()
marker.header.frame_id = "odom_combined"
marker.header.stamp = rospy.get_rostime()
marker.ns = point_group_name
marker.id = id
marker.type = Marker.LINE_STRIP
marker.action = Marker.ADD
marker.points = all_points
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.003
marker.color.r = red
marker.color.g = green
marker.color.b = blue
marker.color.a = 1.0
marker.lifetime = rospy.Duration()
return marker
def get_marker(self, line_seg, frame_id, scale=0.3):
p0, p1 = line_seg
v = np.asarray([p1.position.x - p0.position.x,
p1.position.y - p0.position.y,
p1.position.z - p0.position.z])
vnorm = np.linalg.norm(v)
if vnorm > 0.0:
v /= np.linalg.norm(v)
p2 = copy.deepcopy(p0)
p2.position.x += scale * v[0]
p2.position.y += scale * v[1]
p2.position.z += scale * v[2]
m = Marker()
m.header.stamp = rospy.Time.now()
m.header.frame_id = frame_id
m.ns = "beam"
m.type = Marker.ARROW
m.action = Marker.MODIFY
m.lifetime = rospy.Duration(1)
m.points = [p0.position, p1.position]
m.scale.x = 0.02
m.scale.y = 0.04
m.color.r = 1.0
m.color.a = 1.0
return m
def visualize_poop(x,y,z,color,frame,ns):
msg = Marker()
msg.header = Header(stamp=Time.now(), frame_id=frame)
#msg.scale = Vector3(x=0.02, y=0.02, z=0.02) # for sphere
msg.scale = Vector3(x=0.005, y=0.04, z=0.0) # for arrow
#msg.pose.position = Point(x=x, y=y, z=z)
#msg.pose.position = Point(x=x, y=y, z=z+0.15) # arrow
#msg.pose.orientation = Quaternion(x=0, y=0, z=0, w=1) # arrow
#msg.pose.orientation = Quaternion(x=0.707, y=0, z=0, w=0.707)
msg.points = [Point(x=x, y=y,z=z+0.15), Point(x=x,y=y,z=z)]
msg.ns = ns
msg.id = 0
msg.action = 0 # add
#msg.type = 2 # sphere
msg.type = 0 # arrow
msg.color = ColorRGBA(r=0, g=0, b=0, a=1)
if color == 0:
msg.color.g = 1;
elif color == 1:
msg.color.b = 1;
elif color == 2:
msg.color.r = 1;
msg.color.g = 1;
elif color == 3:
msg.color.g = 1;
msg.color.b = 1;
#loginfo("Publishing %s marker at %0.3f %0.3f %0.3f",ns,x,y,z)
viz_pub.publish(msg)
def create_line_marker(p1, p2, frame_id):
h = Header()
h.frame_id = frame_id #tie marker visualization to laser it comes from
h.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work
#create marker:person_marker, modify a red cylinder, last indefinitely
mark = Marker()
mark.header = h
mark.ns = "unit_vector"
mark.id = 0
mark.type = Marker.LINE_STRIP
mark.action = 0
mark.scale.x = 0.2
mark.color = color = ColorRGBA(0.2, 0.5, 0.7, 1.0)
mark.text = "unit_vector"
points = []
pt1 = Point(p1[0], p1[1], p1[2])
pt2 = Point(p2[0], p2[1], p2[2])
# print "Pt 1 ", pt1
# print "Pt 2 ", pt2
points.append(pt1)
points.append(pt2)
mark.points = points
return mark
def publish_cluster(publisher, points, frame_id, namespace, cluster_id):
"""Publishes a marker representing a cluster.
The x and y arguments specify the center of the target.
Args:
publisher: A visualization_msgs/Marker publisher
points: A list of geometry_msgs/Point
frame_id: The coordinate frame in which to interpret the points.
namespace: string, a unique name for a group of clusters.
cluster_id: int, a unique number for this cluster in the given namespace.
"""
marker = Marker()
# TODO(jstn): Once the point clouds have the correct frame_id,
# use them here.
marker.header.frame_id = frame_id
marker.header.stamp = rospy.Time().now()
marker.ns = namespace
marker.id = 2 * cluster_id
marker.type = Marker.POINTS
marker.action = Marker.ADD
marker.color.r = random.random()
marker.color.g = random.random()
marker.color.b = random.random()
marker.color.a = 0.5
marker.points = points
marker.scale.x = 0.002
marker.scale.y = 0.002
marker.lifetime = rospy.Duration()
center = [0, 0, 0]
for point in points:
center[0] += point.x
center[1] += point.y
center[2] += point.z
center[0] /= len(points)
center[1] /= len(points)
center[2] /= len(points)
text_marker = Marker()
text_marker.header.frame_id = frame_id
text_marker.header.stamp = rospy.Time().now()
text_marker.ns = namespace
text_marker.id = 2 * cluster_id + 1
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.action = Marker.ADD
text_marker.pose.position.x = center[0] - 0.1
text_marker.pose.position.y = center[1]
text_marker.pose.position.z = center[2]
text_marker.color.r = 1
text_marker.color.g = 1
text_marker.color.b = 1
text_marker.color.a = 1
text_marker.scale.z = 0.05
text_marker.text = '{}'.format(cluster_id)
text_marker.lifetime = rospy.Duration()
_publish(publisher, marker, "cluster")
_publish(publisher, text_marker, "text_marker")
return marker
def draw_grasps(self, grasps, frame, ns = 'grasps', pause = 0, frame_locked = False):
marker = Marker()
marker.header.frame_id = frame
marker.header.stamp = rospy.Time.now()
marker.ns = ns
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.color.a = 1.0
marker.lifetime = rospy.Duration(0)
marker.frame_locked = frame_locked
for (grasp_num, grasp) in enumerate(grasps):
if grasp_num == 0:
marker.scale.x = 0.015
marker.scale.y = 0.025
length_fact = 1.5
else:
marker.scale.x = 0.01
marker.scale.y = 0.015
length_fact = 1.0
orientation = grasp.orientation
quat = [orientation.x, orientation.y, orientation.z, orientation.w]
mat = tf.transformations.quaternion_matrix(quat)
start = [grasp.position.x, grasp.position.y, grasp.position.z]
x_end = list(mat[:,0][0:3]*.05*length_fact + scipy.array(start))
y_end = list(mat[:,1][0:3]*.02*length_fact + scipy.array(start))
marker.id = grasp_num*3
marker.points = [Point(*start), Point(*x_end)]
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
self.marker_pub.publish(marker)
marker.id = grasp_num*3+1
marker.points = [Point(*start), Point(*y_end)]
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
self.marker_pub.publish(marker)
marker.id = grasp_num*3+2
if pause:
print "press enter to continue"
raw_input()
time.sleep(.5)
def visual_test(self,data,Type,color,scale):#data=[point1,point2,point3...]###################visual_test
#plot POINTS
#print len(data),data[0],data[1]
if Type==Marker.POINTS:
#print 'pub POINTS Marker'
point_marker=Marker()
point_marker.header.frame_id='/map'
point_marker.header.stamp=rospy.Time.now()
point_marker.ns='detector_visual_test'
point_marker.action=Marker.ADD
point_marker.id=0
point_marker.type=Type
point_marker.scale.x=scale.x#0.1
point_marker.scale.y=scale.y#0.1
point_marker.points=data
for i in data:
point_marker.colors.append(color)
point_marker.lifetime=rospy.Duration(0.2)
return point_marker
#plot LINE_LIST
if Type==Marker.LINE_LIST:
#print 'pub LINE_LIST Marker'
line_marker=Marker()
line_marker.header.frame_id='/map'
line_marker.header.stamp=rospy.Time.now()
line_marker.ns='detector_visual_test'
line_marker.action=Marker.ADD
line_marker.id=1
line_marker.type=Type
line_marker.scale.x=scale.x#0.05
line_marker.scale.y=scale.y#0.05
line_marker.points=data
for i in data:
line_marker.colors.append(color)
line_marker.lifetime=rospy.Duration(0.5)
return line_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 poseupdate():
#set up marker
marker_scale = 0.4
marker_lifetime = 0
marker_ns = 'waypoint'
marker_id = 0
marker_color= {'r':1.0, 'g':0.0, 'b':0.0, 'a':1.0}
marker_pub = rospy.Publisher('waypoint_marker', Marker)
marker = Marker()
marker.ns = marker_ns
marker.id = marker_id
marker.type = Marker.CUBE_LIST
marker.action = Marker.ADD
marker.text = "marker"
marker.lifetime = rospy.Duration(marker_lifetime)
marker.scale.x = marker_scale
marker.scale.y = marker_scale
marker.scale.z = marker_scale
marker.color.r = marker_color['r']
marker.color.g = marker_color['g']
marker.color.b = marker_color['b']
marker.color.a = marker_color['a']
marker.header.frame_id = '/map'
marker.header.stamp = rospy.Time.now()
marker.points = list()
# for waypoint in waypoints:
# p = Point()
# p = waypoint.position
# marker.points.append(p)
# i = 0
# while i < 4 and not rospy.is_shutdown():
# marker_pub.publish(marker)
# i += 1
p1 = Point(0.502, 11.492, 0.000) #room1 Position(0.718, -0.571, 0.000),Orientation(0.000, 0.000, -0.008, 1.000) = Angle: -0.015
p2 = Point(-3.66308784485,13.2560949326, 0.000) #room2 Orientation(0.000, 0.000, -0.741, 0.672) = Angle: -1.668
p3 = Point(-0.00961661338806, 9.34885692596, 0.000) #room3 Position(0.782, -2.700, 0.000), Orientation(0.000, 0.000, -0.052, 0.999) = Angle: -0.105
p4 = Point(-4.05501270294,10.2383213043, 0.000) #room4 Position(-0.343, -6.387, 0.000), Orientation(0.000, 0.000, -0.667, 0.745) = Angle: -1.459
p5 = Point(-3.27898073196, 6.35970401764, 0.000) #door1 , Position(-1.134, -1.524, 0.000), Orientation(0.000, 0.000, -0.704, 0.710) = Angle: -1.562
p6 = Point( -1.94060504436, 0.0072660446167, 0.000) #door2 Position(-0.203, -3.766, 0.000), Orientation(0.000, 0.000, -0.717, 0.697) = Angle: -1.599
p7 = Point(1.34751307964, 0.948370933533, 0.000) #desk Position(-1.579, -3.107, 0.000), Orientation(0.000, 0.000, 0.012, 1.000) = Angle: 0.023
p8 = Point(-1.64714360237,4.02495145798, 0.000) #cooridor Position(-0.175, -4.631, 0.000), Orientation(0.000, 0.000, -0.717, 0.697) = Angle: -1.599
p9 = Point(-3.74548912048,3.87098288536,0.000)
marker.points.append(p1)
marker.points.append(p2)
marker.points.append(p3)
marker.points.append(p4)
marker.points.append(p5)
marker.points.append(p6)
marker.points.append(p7)
marker.points.append(p8)
marker.points.append(p9)
while not rospy.is_shutdown():
marker_pub.publish(marker)
def visualize_cluster(self, cluster, label=None):
points = pc2.read_points(cluster.pointcloud, skip_nans=True)
point_list = [Point(x=x, y=y-0.3, z=z) for x, y, z, rgb in points]
if len(point_list) == 0:
rospy.logwarn('Point list was of size 0, skipping.')
return
marker_id = len(self._current_markers)
marker = Marker()
marker.header.frame_id = 'map'
marker.header.stamp = rospy.Time().now()
marker.ns = 'clusters'
marker.id = marker_id
marker.type = Marker.POINTS
marker.action = Marker.ADD
marker.color.r = random.random()
marker.color.g = random.random()
marker.color.b = random.random()
marker.color.a = 0.5 + random.random()
marker.points = point_list
marker.scale.x = 0.002
marker.scale.y = 0.002
marker.lifetime = rospy.Duration()
self.visualize_marker(marker)
if label is not None:
center = [0, 0, 0]
for point in point_list:
center[0] += point.x
center[1] += point.y
center[2] += point.z
center[0] /= len(point_list)
center[1] /= len(point_list)
center[2] /= len(point_list)
text_marker = Marker()
text_marker.header.frame_id = 'map'
text_marker.header.stamp = rospy.Time().now()
text_marker.ns = 'labels'
text_marker.id = marker_id + 1
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.action = Marker.ADD
text_marker.pose.position.x = center[1] - 0.05
text_marker.pose.position.y = center[1]
text_marker.pose.position.z = center[2]
text_marker.color.r = 1
text_marker.color.g = 1
text_marker.color.b = 1
text_marker.color.a = 1
text_marker.scale.z = 0.05
text_marker.text = label
text_marker.lifetime = rospy.Duration()
self.visualize_marker(text_marker)
def create_trajectory_marker(self, traj):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.name = traj.uuid
int_marker.description = traj.uuid
pose = Pose()
pose.position.x = traj.pose[0]['position']['x']
pose.position.y = traj.pose[0]['position']['y']
int_marker.pose = pose
line_marker = Marker()
line_marker.type = Marker.LINE_STRIP
line_marker.scale.x = 0.05
# random.seed(traj.uuid)
# val = random.random()
# line_marker.color.r = r_func(val)
# line_marker.color.g = g_func(val)
# line_marker.color.b = b_func(val)
# line_marker.color.a = 1.0
line_marker.points = []
MOD = 1
for i, point in enumerate(traj.pose):
if i % MOD == 0:
x = point['position']['x']
y = point['position']['y']
p = Point()
p.x = x - int_marker.pose.position.x
p.y = y - int_marker.pose.position.y
line_marker.points.append(p)
line_marker.colors = []
for i, vel in enumerate(traj.vel):
if i % MOD == 0:
color = ColorRGBA()
val = vel / traj.max_vel
color.r = r_func(val)
color.g = g_func(val)
color.b = b_func(val)
color.a = 1.0
line_marker.colors.append(color)
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.markers.append(line_marker)
int_marker.controls.append(control)
return int_marker
def draw_pose(self, pose_stamped, id):
marker = Marker()
marker.header = pose_stamped.header
marker.ns = "basic_shapes"
marker.type = 0 #arrow
marker.action = 0 #add
marker.scale.x = 0.01
marker.scale.y = 0.02
marker.color.a = 1.0
marker.lifetime = rospy.Duration(30.0)
orientation = pose_stamped.pose.orientation
quat = [orientation.x, orientation.y, orientation.z, orientation.w]
mat = tf.transformations.quaternion_matrix(quat)
start = [pose_stamped.pose.position.x, pose_stamped.pose.position.y, pose_stamped.pose.position.z]
x_end = list(mat[:,0][0:3]*.05 + numpy.array(start))
y_end = list(mat[:,1][0:3]*.05 + numpy.array(start))
z_end = list(mat[:,2][0:3]*.05 + numpy.array(start))
#print "start: %5.3f, %5.3f, %5.3f"%(pose_stamped.pose.position.x, pose_stamped.pose.position.y, pose_stamped.pose.position.z)
#print "x_end:", pplist(x_end)
#print "y_end:", pplist(y_end)
#print "z_end:", pplist(z_end)
marker.id = id
marker.points = [pose_stamped.pose.position, Point(*x_end)]
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
self.marker_pub.publish(marker)
marker.id = id+1
marker.points = [pose_stamped.pose.position, Point(*y_end)]
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
self.marker_pub.publish(marker)
marker.id = id+2
marker.points = [pose_stamped.pose.position, Point(*z_end)]
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
self.marker_pub.publish(marker)
def create_plan_marker(self, plan, plan_values):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.name = plan.ID
int_marker.description = plan.ID
pose = Pose()
#pose.position.x = traj.pose[0]['position']['x']
#pose.position.y = traj.pose[0]['position']['y']
int_marker.pose = pose
line_marker = Marker()
line_marker.type = Marker.LINE_STRIP
line_marker.scale.x = 0.1
# random.seed(float(plan.ID))
# val = random.random()
# line_marker.color.r = r_func(val)
# line_marker.color.g = g_func(val)
# line_marker.color.b = b_func(val)
# line_marker.color.a = 1.0
line_marker.points = []
for view in plan.views:
x = view.get_ptu_pose().position.x
y = view.get_ptu_pose().position.y
z = 0.0 # float(plan.ID) / 10
p = Point()
p.x = x - int_marker.pose.position.x
p.y = y - int_marker.pose.position.y
p.z = z - int_marker.pose.position.z
line_marker.points.append(p)
line_marker.colors = []
for i, view in enumerate(plan.views):
color = ColorRGBA()
val = float(i) / len(plan.views)
color.r = r_func(val)
color.g = g_func(val)
color.b = b_func(val)
color.a = 1.0
line_marker.colors.append(color)
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.markers.append(line_marker)
int_marker.controls.append(control)
return int_marker
def transformStampedArrayToLabeledLineStripMarker(cls, tsdata_lst, fmt="%Y-%m-%d %H:%M:%S", zoffset=0.05, label_downsample=1, discrete=False):
"[[transformStamped, meta],...] -> LineStrip / String"
res = []
# make line strip
points = []
colors = []
t_first = tsdata_lst[0][0]
prev_t = t_first.transform.translation
for ts, _ in tsdata_lst:
t = ts.transform.translation
dist = distanceOfVector3(prev_t, t) * 0.65
rgb = colorsys.hsv_to_rgb(dist, 1.0, 1.0)
points.append(Point(x=t.x, y=t.y, z=t.z))
colors.append(ColorRGBA(rgb[0],rgb[1],rgb[2],1.0))
prev_t = t
h = Header()
h.stamp = rospy.Time(0) #rospy.Time.now()
h.frame_id = "eng2" #t_first.child_frame_id
if discrete:
m_type = Marker.POINTS
else:
m_type = Marker.LINE_STRIP
m = Marker(type=m_type,
action=Marker.ADD,
header=h,
id=cls.marker_id)
cls.marker_id += 1
m.scale.x = 0.1
m.scale.y = 0.1
m.points = points
m.colors = colors
m.ns = "labeled_line"
m.lifetime = rospy.Time(3000)
res.append(m)
for t, t_meta in tsdata_lst[::label_downsample]:
text = Marker(type=Marker.TEXT_VIEW_FACING,
action=Marker.ADD,
header=h,
id=cls.marker_id)
cls.marker_id += 1
text.scale.z = 0.1
text.pose = T.poseFromTransform(t.transform)
text.pose.position.z += zoffset
text.color = ColorRGBA(0.0,0.0,1.0,1.0)
text.text = t_meta["inserted_at"].strftime(fmt)
text.ns = "labeled_line_text"
text.lifetime = rospy.Time(3000)
res.append(text)
return res
def make_arrow_points_marker(scale, tail, tip, idnum):
# make a visualization marker array for the occupancy grid
m = Marker()
m.action = Marker.ADD
m.header.frame_id = 'base_link'
m.header.stamp = rospy.Time.now()
m.ns = 'points_arrows'
m.id = idnum
m.type = Marker.ARROW
m.pose.orientation.y = 0
m.pose.orientation.w = 1
m.scale = scale
m.color.r = 0.2
m.color.g = 0.5
m.color.b = 1.0
m.color.a = 0.3
m.points = [tail, tip]
return m
def points_to_msg(points, id_gen):
marker = Marker()
marker.header.frame_id = 'map'
marker.ns = 'collision_points'
marker.id = next(id_gen)
marker.type = Marker.SPHERE_LIST
marker.action = Marker.ADD
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
marker.color.a = 0.5
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.points = []
for p in points:
marker.points.append(Point(x=p.x, y=p.y, z=p.z))
return marker
def show_arrow(self, tail, tip, color=ColorRGBA(1, 0, 0, 1)):
self.marker_num += 1
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = '/map'
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.frame_locked = False
marker.id = self.marker_num
marker.scale = Vector3(0.05, 0.05, 0.05)
marker.color = color
marker.ns = 'points_arrows'
marker.pose.orientation.y = 0
marker.pose.orientation.w = 1
marker.points = [tail.position, tip.position]
self.marker_array.markers.append(marker)
self.markers_pub.publish(self.marker_array)
def publish_3dbox(box_pub, coners_3d_velos, object_types):
rospy.loginfo("bbox published")
marker_array = MarkerArray()
i = 0
for coners_3d_velo in coners_3d_velos:
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()
marker.id = i
marker.action = Marker.ADD
marker.lifetime = rospy.Duration(LIFETIME_pred)
marker.type = Marker.LINE_LIST
r, g, b = Detection_color_dict[object_types[i]]
marker.color.r = r/255.0
marker.color.g = g/255.0
marker.color.b = b/255.0
marker.color.a = 1.0
marker.scale.x = 0.1
marker.points = []
for l in LINES:
p1 = coners_3d_velo[l[0]]
marker.points.append(Point(p1[0], p1[1], p1[2]))
p2 = coners_3d_velo[l[1]]
marker.points.append(Point(p2[0], p2[1], p2[2]))
marker_array.markers.append(marker)
i=i+1
box_pub.publish(marker_array)
def publish_gt_3dbox(gt_box3d_pub ,gt_coners_3d_velos):
rospy.loginfo("gt_bbox published")
marker_array_2 = MarkerArray()
i = 100
for gt_coners_3d_velo in gt_coners_3d_velos:
marker_2 = Marker()
marker_2.header.frame_id = FRAME_ID
marker_2.header.stamp = rospy.Time.now()
marker_2.id = i+1
marker_2.action = Marker.ADD
marker_2.lifetime = rospy.Duration(LIFETIME_gt)
marker_2.type = Marker.LINE_LIST
marker_2.color.r = 0
marker_2.color.g = 0
marker_2.color.b = 1
marker_2.color.a = 1.0
marker_2.scale.x = 0.1
marker_2.points = []
for l in LINES:
p1 = gt_coners_3d_velo[l[0]]
marker_2.points.append(Point(p1[0], p1[1], p1[2]))
p2 = gt_coners_3d_velo[l[1]]
marker_2.points.append(Point(p2[0], p2[1], p2[2]))
marker_array_2.markers.append(marker_2)
i=i+1
gt_box3d_pub.publish(marker_array_2)
def callback(self, state):
# Checking to make sure list is even, forcing if not
# if len(xyz_tup) % 2 != 0:
# xyz_tup = xyz_tup[:-1]
dist, theta, x_fit, y_fit, d_vec = self.get_controller_values(state)
self.e_diff = (dist - self._DESIRED_DISTANCE) - self.error
#self.error_int += self.error
self.error = dist - self._DESIRED_DISTANCE
delta = self._SIDE * (self._Kp * self.error +
self._Kd * self.error_diff) + self._K_theta * (
theta - (np.pi * self._SIDE) / 2)
segments = [(x_fit[0], y_fit[0], 0), (x_fit[-1], y_fit[-1], 0)]
marker = Marker()
marker.header.frame_id = "laser"
marker.type = Marker.LINE_LIST
marker.scale.x = 0.1
marker.id = 0
marker.color.r = 0
marker.color.g = 0.5
marker.color.b = 0
marker.color.a = 1
marker.points = [Point(x, y, z) for (x, y, z) in segments]
self.pub_marker.publish(marker)
# Initializes the message to be published
drive_state = AckermannDriveStamped()
# Sets drive states
drive_state.drive.steering_angle = delta
drive_state.drive.steering_angle_velocity = 0
drive_state.drive.speed = 2 * self._VELOCITY
drive_state.drive.acceleration = 0
drive_state.drive.jerk = 0
# Publishes drive states
self.pub.publish(drive_state)
def get_arc_marker(self, x, y):
m = Marker()
m.header.stamp = rospy.Time.now()
m.header.frame_id = "base_link"
# assume pose initialized to zero
m.type = m.LINE_STRIP
m.points = []
for i in range(x.size):
p = Point()
p.x = x[i]
p.y = y[i]
p.z = 0.2
m.points.append(p)
m.scale.x = 0.2
m.color.a = 1.0
m.color.r = 0.0
m.color.g = 1.0
m.color.b = 0.0
return m
def get_2d_laser_points_marker(self, timestamp, frame_id, pts_in_map,
marker_id, rgba):
msg = Marker()
msg.header.stamp = timestamp
msg.header.frame_id = frame_id
msg.ns = 'laser_points'
msg.id = marker_id
msg.type = 6
msg.action = 0
msg.points = [Point(pt[0], pt[1], pt[2]) for pt in pts_in_map]
msg.colors = [rgba for pt in pts_in_map]
for pt in pts_in_map:
assert ((not np.isnan(pt).any()) and np.isfinite(pt).all())
msg.scale.x = 0.1
msg.scale.y = 0.1
msg.scale.z = 0.1
return msg
def publishObstacles(self):
obstacles_msg = Marker()
obstacles_msg.header.stamp = self.stamp
obstacles_msg.header.frame_id = self.p_frame_id
obstacles_msg.ns = "lines"
obstacles_msg.id = 0
obstacles_msg.type = Marker.LINE_LIST
obstacles_msg.action = Marker.ADD
obstacles_msg.scale.x = 0.1
obstacles_msg.color.a = 1.0
obstacles_msg.color.r = 0.0
obstacles_msg.color.g = 1.0
obstacles_msg.color.b = 0.0
obstacles_msg.points = self.point_segments
self.obstacles_pub.publish(obstacles_msg)
def publish_rviz_path(self, waypoints):
path = Marker()
path.header.frame_id = '/world'
path.header.stamp = rospy.Time.now()
path.ns = "path"
path.id = 0
path.action = Marker.ADD
path.type = Marker.LINE_LIST
path.scale.x = 0.1
path.color.r = 1.0
path.color.a = 1.0
path.points = []
for wp in waypoints:
pos = copy.deepcopy(wp.pose.pose.position)
path.points.append(pos)
pos = copy.deepcopy(pos)
pos.z = wp.twist.twist.linear.x
path.points.append(pos)
self.pub_path.publish(path)
def transformStampedArrayToLabeledArrayMarker(cls, tsdata_lst, fmt="%Y-%m-%d %H:%M:%S", zoffset=0.05, label_downsample=1, discrete=False):
"[[transformStamped, meta],...] -> LineStrip / String"
h = Header()
h.stamp = rospy.Time(0) #rospy.Time.now()
h.frame_id = "eng2" #t_first.child_frame_id
res = []
t_first = tsdata_lst[0][0]
prev_t = t_first.transform.translation
for ts, _ in tsdata_lst:
m = Marker(type=Marker.ARROW,
action=Marker.ADD,
header=h,
id=cls.marker_id)
cls.marker_id += 1
t = ts.transform.translation
dist = distanceOfVector3(prev_t, t) * 0.65
rgb = colorsys.hsv_to_rgb(dist, 1.0, 1.0)
m.points = [Point(x=prev_t.x,y=prev_t.y,z=prev_t.z),
Point(x=(prev_t.x + t.x) / 2.,y=(prev_t.y + t.y) /2.,z=(prev_t.z + t.z) /2.)]
m.ns = "labeled_line"
m.lifetime = rospy.Time(3000)
m.scale.x, m.scale.y, m.scale.z = 0.02, 0.06, 0.1
m.color = ColorRGBA(rgb[0],rgb[1],rgb[2],1.0)
if dist < 0.65:
res.append(m)
prev_t = t
for t, t_meta in tsdata_lst[::label_downsample]:
text = Marker(type=Marker.TEXT_VIEW_FACING,
action=Marker.ADD,
header=h,
id=cls.marker_id)
cls.marker_id += 1
text.scale.z = 0.1
text.pose = T.poseFromTransform(t.transform)
text.pose.position.z += zoffset
text.color = ColorRGBA(0.0,0.0,1.0,1.0)
text.text = t_meta["inserted_at"].strftime(fmt)
text.ns = "labeled_line_text"
text.lifetime = rospy.Time(3000)
res.append(text)
return res
def publish_grid(self, points):
marker = Marker()
marker.header.frame_id = 'pegasus_map'
marker.ns = 'grids'
marker.id = 0
marker.type = marker.POINTS
marker.action = marker.ADD
marker.pose.orientation.w = 1
marker.points = points
marker.lifetime = rospy.Duration()
marker.scale.x = 3
marker.scale.y = 3
marker.scale.z = 3
marker.color.g = 1
marker.color.r = 1
marker.color.b = 0
marker.color.a = 1
self.publisher_marker.publish(marker)
def publish_new_path(global_path):
marker = Marker(type=Marker.LINE_LIST, action=Marker.ADD)
marker.header.stamp = stamp if stamp is not None else rospy.Time.now()
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.color.b = 1.0
marker.color.a = 1.0
marker.pose.position = Point(0, 0, 0)
marker.pose.orientation = Quaternion(0, 0, 0, 1)
marker.points = []
for j in (range(len(global_path)) - 1):
marker.points.extend([
Point(global_path[j, 0], global_path[j, 1], 0),
Point(global_path[j + 1, 0], global_path[j + 1, 1], 0)
])
marker.lifetime = rospy.Time(5) # 5 sec
path_pub_.publish(marker)
def create_simple_marker(self):
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.LINE_STRIP
marker.action = marker.ADD
# marker scale
marker.scale.x = 0.5
marker.scale.y = 0.5
marker.scale.z = 0.5
# marker color
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
# marker orientaiton
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 position
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
marker.points = []
line_point = Point()
line_point.x = 1
line_point.y = 1
line_point.z = 0.0
marker.points.append(line_point)
line_point2 = Point()
line_point2.x = 2
line_point2.y = 2
line_point2.z = 0.0
marker.points.append(line_point2)
return marker
def getMarkerArrow(self, points, k, n_s, r, g, b):
"""
returns an arrow marker
"""
marker = Marker()
color = ColorRGBA(*[r, g, b, 1])
marker.header.frame_id = "map"
marker.header.stamp = rospy.get_rostime()
marker.ns = n_s
marker.id = k
marker.type = 0
marker.action = 0
marker.points = points
marker.pose.orientation.w = 1.0
marker.scale.x = 0.05
marker.scale.y = 0.1
marker.scale.z = 0
marker.color = color
return marker
def PublishPathArray(self, path):
markarray = MarkerArray()
for i in range(1,len(path)):
#print(path)
x_index = path[i][0]
y_index = path[i][1]
parent_x = path[i-1][0]
parent_y = path[i-1][1]
node_x = self.occ_grid[x_index][y_index][1]
node_y = self.occ_grid[x_index][y_index][2]
pnode_x = self.occ_grid[parent_x][parent_y][1]
pnode_y = self.occ_grid[parent_x][parent_y][2]
z = 0.05
points = self.ConstructPointsArray(node_x, node_y, pnode_x, pnode_y, z)
marker = Marker()
marker.ns = "waypoints"
marker.action = marker.MODIFY
marker.header.frame_id = "/map"
marker.type = marker.LINE_STRIP
marker.points = points
marker.scale.x = 0.05
marker.color.b = 1.0
marker.color.a = 1.0
marker.lifetime = rospy.Duration(2)
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
markarray.markers.append(marker)
id = 0
for m in markarray.markers:
m.id = id
id += 1
self.path_pub.publish(markarray)
def visualize_action(self):
robot_pos = Point(x=self.px, y=self.py, z=0)
next_theta = self.theta + self.cmd_vel.angular.z
next_vx = self.cmd_vel.linear.x * np.cos(next_theta)
next_vy = self.cmd_vel.linear.x * np.sin(next_theta)
action = Vector3(x=next_vx, y=next_vy, z=0)
# green arrow for action (command velocity)
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = "/map"
marker.ns = "action"
marker.id = 0
marker.type = marker.ARROW
marker.action = marker.ADD
marker.points = [robot_pos, add(robot_pos, action)]
marker.scale = Vector3(x=0.1, y=0.3, z=0)
marker.color = ColorRGBA(g=1.0, a=1.0)
marker.lifetime = rospy.Duration(0.5)
self.action_marker_pub.publish(marker)
def set_target_action_marker(action_list, target=False):
goal_marker = Marker()
goal_marker.action = goal_marker.ADD
goal_marker.header.frame_id = "table_gym"
# goal_marker.header.stamp = rospy.get_rostime()
# goal_marker.ns = "goal_marker"
goal_marker.lifetime = rospy.Duration(60)
# goal_marker.id = 0
goal_marker.type = goal_marker.LINE_STRIP
goal_marker.pose.position.x = 0.0
goal_marker.pose.position.y = 0.0
goal_marker.pose.position.z = 0.00
goal_marker.pose.orientation.x = 0.0
goal_marker.pose.orientation.y = 0.0
goal_marker.pose.orientation.z = 0.0
goal_marker.pose.orientation.w = 1.0
goal_marker.scale.x = 0.001
goal_marker.color.r = 1.0
goal_marker.color.g = 0.0
goal_marker.color.b = 0.0
goal_marker.color.a = 1.0
#### if this marker is target action
if target is True:
# goal_marker.id = 100000
goal_marker.color.r = 0.0
goal_marker.color.g = 0.0
goal_marker.color.b = 1.0
goal_marker.scale.x = 0.005
goal_marker.pose.position.z = 0.02
goal_marker.points = []
n = len(action_list)
for i in range(n):
point = geometry_msgs.msg.Point()
point.x = action_list[i][0]
point.y = action_list[i][1]
point.z = 0.0475
goal_marker.points.append(point)
return goal_marker
def PublishTreeArray(self, tree):
markarray = MarkerArray()
for i in range(1,len(tree)):
x_index = tree[i].x
y_index = tree[i].y
parent_index = tree[i].parent
parent_x = tree[parent_index].x
parent_y = tree[parent_index].y
node_x = self.occ_grid[x_index][y_index][1]
node_y = self.occ_grid[x_index][y_index][2]
pnode_x = self.occ_grid[parent_x][parent_y][1]
pnode_y = self.occ_grid[parent_x][parent_y][2]
z = 0.025
points = self.ConstructPointsArray(node_x, node_y, pnode_x, pnode_y, z)
marker = Marker()
marker.ns = "tree"
marker.action = marker.MODIFY
marker.header.frame_id = "/map"
marker.type = marker.LINE_STRIP
marker.points = points
marker.scale.x = 0.05
marker.color.g = 1.0
marker.color.a = 1.0
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.lifetime = rospy.Duration(0.1)
markarray.markers.append(marker)
id = 0
for m in markarray.markers:
m.id = id
id += 1
self.tree_pub.publish(markarray)
def draw_reach_values(self):
if self.rel_z is not None:
# phi_r_ind = int((self.hj_grid['shape'][2]-1) * (self.rel_z[2] - self.hj_grid['llims'][2])/(self.hj_grid['ulims'][2] - self.hj_grid['llims'][2]))
phi_r_ind = int((self.hj_grid['shape'][2] - 1) / 2)
vra_slice_short = self.reachavoid_value_fnc[0][:, :, phi_r_ind]
slice_ext_short = (np.amin(vra_slice_short),
np.amax(vra_slice_short))
vra_slice_mid = self.reachavoid_value_fnc[1][:, :, phi_r_ind]
slice_ext_mid = (np.amin(vra_slice_mid), np.amax(vra_slice_mid))
vra_slice_long = self.reachavoid_value_fnc[2][:, :, phi_r_ind]
slice_ext_long = (np.amin(vra_slice_long), np.amax(vra_slice_long))
print('Min reach value: ' + format(slice_ext_mid[0], '05f') +
' Max reach value: ' + format(slice_ext_mid[1], '05f')),
print(chr(13)),
m = Marker()
m.header.frame_id = "map"
m.ns = "reach_val"
m.type = Marker.POINTS
m.scale.x = 0.02
m.scale.y = 0.02
m.points = []
m.colors = []
m.frame_locked = True
for i in range(self.hj_grid['shape'][0]):
for j in range(self.hj_grid['shape'][1]):
th_ij = self.human_z[2] + self.hj_grid['phi_h'][i, j,
phi_r_ind]
x_ij = self.human_z[0] + np.cos(
th_ij) * self.hj_grid['rho'][i, j, phi_r_ind]
y_ij = self.human_z[1] + np.sin(
th_ij) * self.hj_grid['rho'][i, j, phi_r_ind]
r_ij = (vra_slice_short[i, j] - slice_ext_short[0]) / (
slice_ext_short[1] - slice_ext_short[0])
g_ij = (vra_slice_mid[i, j] - slice_ext_mid[0]) / (
slice_ext_mid[1] - slice_ext_mid[0])
b_ij = (vra_slice_long[i, j] - slice_ext_long[0]) / (
slice_ext_long[1] - slice_ext_long[0])
m.points.append(Point(x_ij, y_ij, 0.))
m.colors.append(ColorRGBA(r_ij, g_ij, b_ij, 1.0))
self.vis_pub.publish(m)
def pub_tracker():
marker_array.markers = []
for i in range(0, len(track_ar_ids_param)):
marker = Marker()
marker.header.frame_id = output_frame
marker.header.stamp = rospy.Time(0)
marker.id = i
marker.type = Marker().LINE_STRIP
marker.action = Marker().ADD
marker.scale.x = float(line_scale)
marker.color = ar_color_rgba[i]
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.points = (ar_pose_matrix[i][0:matrix_position[i]])
marker_array.markers.append(marker)
pub_rviz_markerArray.publish(marker_array)
def draw_shortest_path(marker_publisher, points):
marker = Marker()
marker.type = Marker.LINE_LIST
marker.header.frame_id = 'odom'
marker.action = Marker.ADD
marker.ns = 'shortest_path'
marker.id = 1
marker.scale.x = 0.03
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
shortest_path = []
for i in range(len(points) - 1):
shortest_path.append(points[i])
shortest_path.append(points[i+1])
points = [Point(point.x/100.0, point.y/100.0, 1e-3) for point in shortest_path]
marker.points = points
marker_publisher.publish(marker)
def cb_pose(self, msg):
self.set_inferred_pose(self.dtype(utils.rospose_to_posetup(msg.pose)))
if self.cur_rollout is not None and self.cur_rollout_ip is not None:
m = Marker()
m.header.frame_id = "map"
m.type = m.LINE_STRIP
m.action = m.ADD
with self.traj_pub_lock:
pts = (self.cur_rollout[:, :2] -
self.cur_rollout_ip[:2]) + self.inferred_pose()[:2]
m.points = map(lambda xy: Point(x=xy[0], y=xy[1]), pts)
r, g, b = 0x36, 0xCD, 0xC4
m.colors = [
ColorRGBA(r=r / 255.0, g=g / 255.0, b=b / 255.0, a=0.7)
] * len(m.points)
m.scale.x = 0.05
self.traj_chosen_pub.publish(m)
def plot_odom(self):
"""
Continously plot the robot's x and y pose on a 2D graph.
"""
while not rospy.is_shutdown():
marker = Marker()
marker.header.frame_id = "base_link"
marker.header.stamp = rospy.Time()
marker.type = marker.POINTS
marker.action = marker.ADD
marker.points = self.poses
marker.lifetime = rospy.Duration()
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.r = 1.0
marker.color.a = 1.0
self.marker_pub.publish(marker)
self.rate.sleep()
def visualize(self, x, y, x2, y2, header=None):
marker = Marker()
if header:
marker.header = header
else:
marker.header.frame_id = "/map"
marker.header.stamp = rospy.Time()
marker.type = marker.LINE_STRIP
marker.action = marker.ADD
marker.id = 0
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.b = 1.0
marker.color.r = 1.0
marker.points = [Point(x2, y2, 0), Point(x, y, 0)]
return marker
def talker():
rospy.init_node('traj_commander', anonymous=True)
pub = rospy.Publisher('/carla/ego_vehicle/desired_waypoints',
MultiDOFJointTrajectory,
queue_size=10)
viz_pub = rospy.Publisher('viz/desired_waypoints', Marker, queue_size=10)
rate = rospy.Rate(20)
while not rospy.is_shutdown():
msg = MultiDOFJointTrajectory()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '/map'
viz_msg = Marker()
viz_msg.header.stamp = rospy.Time.now()
viz_msg.header.frame_id = '/map'
viz_msg.type = Marker.CUBE_LIST
viz_msg.scale.x = 1
viz_msg.scale.y = 1
viz_msg.scale.z = 1
viz_msg.color.a = 1
viz_msg.points = []
pts = []
num_pt = 30
radius = 19.5
ang_inc = 2.0 * math.pi / num_pt
for i in range(num_pt):
ang = ang_inc * i
pt = MultiDOFJointTrajectoryPoint()
pt.transforms = [Transform(Vector3(radius * math.cos(ang) - 0.5, radius * math.sin(ang) - 0.3, 0), \
Quaternion(0,0,0,1))]
pt.velocities = [Twist(Vector3(6, 0, 0), Vector3(0, 0, 0))]
viz_msg.points.append(
Point(radius * math.cos(ang), radius * math.sin(ang), 0))
pts.append(pt)
msg.points = pts
pub.publish(msg)
viz_pub.publish(viz_msg)
rate.sleep()
def createPathMarker(self):
marker = Marker()
marker.header.frame_id = "map"
marker.type = marker.LINE_STRIP
marker.action = marker.ADD
# marker scale
marker.scale.x = 0.3
marker.scale.y = 0.3
marker.scale.z = 0.3
# marker color
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
# marker orientaiton
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 position
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
# marker line points
marker.points = []
for i,point in enumerate(self.path):
print("waypoint: ", i)
print(point[0])
print(point[1])
line_point = Point()
line_point.x = point[0]
line_point.y = point[1]
line_point.z = 0.0
marker.points.append(line_point)
self.path_visu_publisher.publish(marker)
def create_polygon_list(self, header, objects, idx):
marker = Marker()
marker.header.frame_id = header.frame_id
marker.header.stamp = header.stamp
marker.ns = self.inputTopic
marker.action = Marker.ADD
marker.pose.orientation.w = 1.0
marker.id = idx
marker.type = Marker.LINE_LIST
marker.scale.x = 0.05 # 0.1
marker.lifetime = rospy.Duration(1.0)
marker.color.r = self.c_red
marker.color.g = self.c_green
marker.color.b = self.c_blue
marker.color.a = 1.0
marker.points = []
for _i in range(len(objects)):
cPoint = objects[_i].cPoint
num_points = len(cPoint.lowerAreaPoints)
if num_points > 0:
objectHigh = cPoint.objectHigh
# Bottom points
marker.points += [ cPoint.lowerAreaPoints[(i-1)//2] for i in range( num_points*2 ) ]
#
# _point_pre = cPoint.lowerAreaPoints[-1]
# for i in range(len(cPoint.lowerAreaPoints)):
# marker.points.append(_point_pre)
# marker.points.append(cPoint.lowerAreaPoints[i])
# _point_pre = cPoint.lowerAreaPoints[i]
# Top points
topAreaPoints = self.get_top_area_point_list(cPoint)
marker.points += [ topAreaPoints[(i-1)//2] for i in range( num_points*2 ) ]
# Edge points
marker.points += [cPoint.lowerAreaPoints[i//2] if i%2==0 else topAreaPoints[i//2] for i in range( num_points*2 ) ]
return marker
def rviz_debug(self):
self.waypoint_list = WaypointList()
self.waypoint_list.header.frame_id = self.frame_id
self.waypoint_size = 0
for i in self.wp_list:
self.waypoint_size = self.waypoint_size + 1
waypoint = Waypoint()
waypoint.x = i[0]
waypoint.y = i[1]
waypoint.z = 0
#print waypoint.x, waypoint.y
self.waypoint_list.list.append(waypoint)
self.waypoint_list.size = self.waypoint_size
marker = Marker()
marker.header.frame_id = self.frame_id
marker.type = marker.LINE_STRIP
marker.action = marker.ADD
marker.scale.x = 0.03
marker.scale.y = 0.03
marker.scale.z = 0.03
marker.color.a = 1.0
marker.color.r = 0
marker.color.g = 1.0
marker.color.b = 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.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
marker.points = []
for i in range(self.waypoint_size):
p = Point()
#print i
#print self.waypoint_size, i, self.waypoint_list.list[i].x
p.x = self.waypoint_list.list[i].x
p.y = self.waypoint_list.list[i].y
p.z = self.waypoint_list.list[i].z
marker.points.append(p)
self.pub_rviz.publish(marker)
def create_scanpoints_marker(self, lines):
marker_points = Marker()
# Initialize basic fields of the marker
marker_points.header.frame_id = lines.header.frame_id
marker_points.header.stamp = rospy.Time()
marker_points.id = 0
marker_points.type = Marker.POINTS
marker_points.action = Marker.ADD
# The marker's pose is at (0,0,0) with no rotation.
marker_points.pose.orientation.w = 1
# Set point width
marker_points.scale.x = 0.10
marker_points.scale.y = 0.10
# Add the scan points
marker_points.points = []
n = len(lines.lines)
for i in range(n):
marker_points.points.append(lines.lines[i].firstScanPoint)
marker_points.points.append(lines.lines[i].lastScanPoint)
if n == 1:
# There is only one line. Just set the color field.
marker_points.color.r = 1.0
marker_points.color.a = 1.0
if n > 1:
# Set per-vertex colours
for i in range(n):
color = ColorRGBA()
# A very simplistic colour spectrum.
color.r = 1.0 - i / float(n - 1)
color.g = 1.0
color.b = i / float(n - 1)
color.a = 1.0
marker_points.colors.append(color)
marker_points.colors.append(color)
black_board.scanpoints_publisher.publish(marker_points)
