def plot_3d_vel(self, p_arr, v_arr, v_scale=1.0):
marker_array = MarkerArray()
for i in xrange(len(p_arr)):
p = p_arr[i]
v = vx,vy,vz = v_arr[i]
marker = Marker()
marker.header.frame_id = "/openni_rgb_optical_frame"
marker.type = marker.ARROW
marker.action = marker.ADD
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.points.append(Point(p[0],p[1],p[2]))
marker.points.append(Point(p[0]+vx*v_scale,p[1]+vy*v_scale,p[2]+vz*v_scale))
marker.scale.x=0.05
marker.scale.y=0.1
marker.id = i
marker_array.markers.append(marker)
for i in xrange(len(p_arr),self.max_features):
marker = Marker()
marker.action = marker.DELETE
marker.id = i
marker_array.markers.append(marker)
self.marker_pub.publish(marker_array)
def publish_debug_info(self, footprint_point, distance, scan_point):
array = MarkerArray()
fp = Marker()
fp.id = 1
fp.type = fp.SPHERE
scale = 0.1
fp.pose.position.x,fp.pose.position.y, fp.pose.position.z = footprint_point[0], footprint_point[1], 0
fp.scale.x, fp.scale.y, fp.scale.z = scale, scale, scale
fp.color.r, fp.color.g, fp.color.b, fp.color.a = (0, 0, 1, 1)
fp.header.frame_id = "{}/base_link".format(self.robot.robot_name)
fp.frame_locked = True
fp.action = fp.ADD
fp.ns = "door_opening"
array.markers += [fp]
sp = Marker()
sp.id = 2
sp.type = sp.SPHERE
scale = 0.1
sp.pose.position.x,sp.pose.position.y, sp.pose.position.z = scan_point[0], scan_point[1], 0
sp.scale.x, sp.scale.y, sp.scale.z = scale, scale, scale
sp.color.r, sp.color.g, sp.color.b, sp.color.a = (1, 0, 0, 1)
sp.header.frame_id = "{}/base_link".format(self.robot.robot_name)
sp.frame_locked = False
sp.action = sp.ADD
sp.ns = "door_opening"
array.markers += [sp]
self.debug_vizualizer.publish(array)
def draw_rod(self, rod): marker = Marker() marker.header.frame_id = "/pl_base" marker.ns = "basic_shapes" marker.id = 10 + rod marker.type = marker.CYLINDER marker.action = marker.ADD marker.pose.position.x = self.rod_position[rod][0] marker.pose.position.y = self.rod_position[rod][1] marker.pose.position.z = self.position_z marker.pose.orientation.w = 1.0 marker.scale.x = ROD_SCALE_XYZ[0] marker.scale.y = ROD_SCALE_XYZ[1] marker.scale.z = ROD_SCALE_XYZ[2] marker.color.r = ROD_COLOR_RGB[0] marker.color.g = ROD_COLOR_RGB[1] marker.color.b = ROD_COLOR_RGB[2] marker.color.a = 1.0 self.rviz_pub.publish(marker) # pink top of the rod marker.id = 30 + rod marker.type = marker.SPHERE marker.pose.position.z = marker.pose.position.z + 0.0475 marker.scale.z = TOP_SCALE_Z marker.color.r = TOP_COLOR_RGB[0] marker.color.g = TOP_COLOR_RGB[1] marker.color.b = TOP_COLOR_RGB[2] self.rviz_pub.publish(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 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 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 make_marker(self, robot_id, x, y, ns):
""" Create a Marker message with the given x,y coordinates """
m = Marker()
m.header.stamp = rospy.Time.now()
m.header.frame_id = 'map'
m.ns = ns
m.action = m.ADD
m.pose.position.x = x
m.pose.position.y = y
if robot_id == 0:
m.color.r = 255
m.color.g = 0
m.color.b = 0
else:
m.color.r = 0
m.color.g = 0
m.color.b = 255
m.color.a = 1.0
if ns == 'mess':
m.type = m.CUBE
m.id = len(self.messes)
m.scale.x = m.scale.y = m.scale.z = .15
self.messes.append(m)
else:
m.type = m.SPHERE
m.id = robot_id
m.scale.x = m.scale.y = m.scale.z = .35
return m
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 publishMap(self):
markerArray = MarkerArray()
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.SPHERE_LIST
marker.action = marker.ADD
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 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.id = 0
for p in self.mapPoints:
if p["eaten"]:
continue
if p["powerup"]:
continue
point = Point()
point.x = p["x"]
point.y = p["y"]
point.z = 0.15
marker.points.append(point)
markerArray.markers.append(marker)
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.SPHERE_LIST
marker.action = marker.ADD
marker.scale.x = 0.3
marker.scale.y = 0.3
marker.scale.z = 0.3
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 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.id = 1
for p in self.mapPoints:
if p["eaten"]:
continue
if not p["powerup"]:
continue
point = Point()
point.x = p["x"]
point.y = p["y"]
point.z = 0.2
marker.points.append(point)
markerArray.markers.append(marker)
self.pubMap.publish(markerArray)
def create_force_marker(self, force, axis, frame_id):
m = Marker()
m.header.frame_id = frame_id
m.type = m.CYLINDER
m.scale.x = 0.02 # diameter
m.scale.y = 0.02 # also diameter
m.scale.z = abs(force) / self.force_to_length # length
# MarkerArray visualizer in Rviz complains if a scale is 0
if m.scale.z == 0.0:
m.scale.z = 0.0001
# namespaced so we can disable axis
m.ns = axis
if axis == 'x':
m.id = 555
m.color.r = 1.0
m.color.g = 0.0
m.color.b = 0.0
m.pose.position.x = m.scale.z / 2.0
# Aligned with the X axis of the frame
if force > 0:
m.pose.orientation = Quaternion(
*quaternion_from_euler(0, radians(90), 0))
else: # if the force is negative the cylinder goes to the other side
m.pose.orientation = Quaternion(
*quaternion_from_euler(0, radians(-90), 0))
m.pose.position.x *= -1.0
elif axis == 'y':
m.id = 666
m.color.r = 0.0
m.color.g = 1.0
m.color.b = 0.0
m.pose.position.y = m.scale.z / 2.0
# Aligned with the Y axis of the frame
if force > 0:
m.pose.orientation = Quaternion(
*quaternion_from_euler(radians(90), 0, 0))
else:
m.pose.orientation = Quaternion(
*quaternion_from_euler(radians(-90), 0, 0))
m.pose.position.y *= -1.0
elif axis == 'z':
m.id = 777
m.color.r = 0.0
m.color.g = 0.0
m.color.b = 1.0
m.pose.position.z = m.scale.z / 2.0
# Aligned with the Z axis of the frame
if force > 0:
m.pose.orientation = Quaternion(
*quaternion_from_euler(0, 0, 0))
else:
m.pose.orientation = Quaternion(
*quaternion_from_euler(radians(180), 0, 0))
m.pose.position.z *= -1.0
m.color.a = 1.0
return m
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 publish_waypoint_markers(self):
pub_waypoint_markers = rospy.Publisher('waypoint_markers', MarkerArray)
marker_array = MarkerArray()
for i in range(len(self.waypoints)):
waypoint = self.waypoints[i]
waypoint_marker = Marker()
waypoint_marker.id = i
waypoint_marker.header.frame_id = "/map"
waypoint_marker.header.stamp = rospy.Time.now()
if (waypoint.type == 'P'):
waypoint_marker.type = 5 # Line List
waypoint_marker.text = 'waypoint_%s_point' % i
waypoint_marker.color.r = 176.0
waypoint_marker.color.g = 224.0
waypoint_marker.color.b = 230.0
waypoint_marker.color.a = 0.5
waypoint_marker.scale.x = 0.5
c = waypoint.coordinate
waypoint_marker.points.append(Point(c.x, c.y, c.z))
waypoint_marker.points.append(Point(c.x, c.y, c.z + 3.0))
else:
waypoint_marker.type = 3 # Cylinder
waypoint_marker.text = 'waypoint_%s_cone' % i
waypoint_marker.color.r = 255.0
waypoint_marker.color.g = 69.0
waypoint_marker.color.b = 0.0
waypoint_marker.color.a = 1.0
waypoint_marker.scale.x = 0.3
waypoint_marker.scale.y = 0.3
waypoint_marker.scale.z = 0.5
waypoint_marker.pose.position = waypoint.coordinate
marker_array.markers.append(waypoint_marker)
if self.current_waypoint_idx != len(self.waypoints):
current_waypoint_marker = Marker()
current_waypoint_marker.id = 999
current_waypoint_marker.header.frame_id = "/map"
current_waypoint_marker.header.stamp = rospy.Time.now()
current_waypoint_marker.type = 2 # Sphere
current_waypoint_marker.text = 'current_waypoint'
current_waypoint_marker.color.r = 255.0
current_waypoint_marker.color.g = 0.0
current_waypoint_marker.color.b = 0.0
current_waypoint_marker.color.a = 1.0
current_waypoint_marker.scale.x = 0.3
current_waypoint_marker.scale.y = 0.3
current_waypoint_marker.scale.z = 0.3
current_waypoint = self.waypoints[self.current_waypoint_idx]
current_waypoint_marker.pose.position.x = current_waypoint.coordinate.x
current_waypoint_marker.pose.position.y = current_waypoint.coordinate.y
current_waypoint_marker.pose.position.z = 1.0
marker_array.markers.append(current_waypoint_marker)
pub_waypoint_markers.publish(marker_array)
def publish(self, target_frame, timestamp):
ma = MarkerArray()
for id in self.marker_list:
marker = Marker()
marker.header.stamp = timestamp
marker.header.frame_id = target_frame
marker.ns = "landmark_kf"
marker.id = id
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
Lkf = self.marker_list[id]
marker.pose.position.x = Lkf.L[0,0]
marker.pose.position.y = Lkf.L[1,0]
marker.pose.position.z = 0
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 1
marker.pose.orientation.w = 0
marker.scale.x = max(3*sqrt(Lkf.P[0,0]),0.05)
marker.scale.y = max(3*sqrt(Lkf.P[1,1]),0.05)
marker.scale.z = 0.5;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.lifetime.secs=3.0;
ma.markers.append(marker)
marker = Marker()
marker.header.stamp = timestamp
marker.header.frame_id = target_frame
marker.ns = "landmark_kf"
marker.id = 1000+id
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.ADD
Lkf = self.marker_list[id]
marker.pose.position.x = Lkf.L[0,0]
marker.pose.position.y = Lkf.L[1,0]
marker.pose.position.z = 1.0
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 1
marker.pose.orientation.w = 0
marker.text = str(id)
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 0.2
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 1.0;
marker.lifetime.secs=3.0;
ma.markers.append(marker)
self.marker_pub.publish(ma)
def pubViz(self, ast, bst):
rate = rospy.Rate(10)
for i in range(self.winSize):
msgs = MarkerArray()
#use1について
msg = Marker()
#markerのプロパティ
msg.header.frame_id = 'camera_link'
msg.header.stamp = rospy.Time.now()
msg.ns = 'j1'
msg.action = 0
msg.id = 1
msg.type = 8
msg.scale.x = 0.1
msg.scale.y = 0.1
msg.color = self.carray[2]
#ジョイントポイントを入れる処理
for j1 in range(self.jointSize):
point = Point()
point.x = self.pdata[0][ast+i][j1][0]
point.y = self.pdata[0][ast+i][j1][1]
point.z = self.pdata[0][ast+i][j1][2]
msg.points.append(point)
msg.pose.orientation.w = 1.0
msgs.markers.append(msg)
msg = Marker()
msg.header.frame_id = 'camera_link'
msg.header.stamp = rospy.Time.now()
msg.ns = 'j2'
msg.action = 0
msg.id = 2
msg.type = 8
msg.scale.x = 0.1
msg.scale.y = 0.1
msg.color = self.carray[1]
for j2 in range(self.jointSize):
point = Point()
point.x = self.pdata[1][bst+i][j2][0]
point.y = self.pdata[1][bst+i][j2][1]
point.z = self.pdata[1][bst+i][j2][2]
msg.points.append(point)
msg.pose.orientation.w = 1.0
msgs.markers.append(msg)
self.mpub.publish(msgs)
rate.sleep()
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 add_rviz_rod(self, rod): marker = Marker() actual = self.irpos.get_cartesian_pose() marker.header.frame_id = "/pl_base" marker.ns = "basic_shapes" marker.id = 10 + rod marker.type = marker.CYLINDER marker.action = marker.ADD marker.pose.position.x = actual.position.x marker.pose.position.y = actual.position.y marker.pose.position.z = actual.position.z - 0.108 + 0.009 marker.pose.orientation.w = 1.0 marker.scale.x = 0.01 marker.scale.y = 0.01 marker.scale.z = 0.095 marker.color.r = 0.75 marker.color.g = 0.70 marker.color.b = 0.5 marker.color.a = 1.0 self.rviz_pub.publish(marker) # pink top of the rod marker.id = 30 + rod marker.type = marker.SPHERE marker.pose.position.z = marker.pose.position.z + 0.0475 marker.scale.z = 0.01 marker.color.r = 1. marker.color.g = 0.4 marker.color.b = 0.4 self.rviz_pub.publish(marker) if(rod == 1): marker.type = marker.CUBE marker.pose.position.z = actual.position.z - 0.143 marker.scale.x = 0.079 marker.scale.y = 0.23 marker.scale.z = 0.014 marker.color.r = 0.75 marker.color.g = 0.70 marker.color.b = 0.5 marker.id = 20 self.rviz_pub.publish(marker) marker.id = 30 + rod marker.type = marker.SPHERE marker.pose.position.z
def pubRviz(self, pos, joints):
msgs = MarkerArray()
for p in range(len(pos)):
msg = Marker()
msg.header.frame_id = 'camera_link'
msg.header.stamp = rospy.Time.now()
msg.ns = 'marker'
msg.action = 0
msg.id = p
msg.type = 4
msg.scale.x = 0.1
msg.scale.y = 0.1
msg.color = self.carray[2]
for i in range(len(pos[p])):
point = Point()
point.x = pos[p][i][0]
point.y = pos[p][i][1]
point.z = pos[p][i][2]
msg.points.append(point)
msg.pose.orientation.w = 1.0
msgs.markers.append(msg)
for j in range(len(joints)):
msg = Marker()
msg.header.frame_id = 'camera_link'
msg.header.stamp = rospy.Time.now()
msg.ns = 'joints'
msg.action = 0
msg.id = j
msg.type = 8
msg.scale.x = 0.1
msg.scale.y = 0.1
msg.color = self.carray[j]
#print "joints len:"+str(len(joints[j]))
for i in range(len(joints[j])):
point = Point()
point.x = joints[j][i][0]
point.y = joints[j][i][1]
point.z = joints[j][i][2]
msg.points.append(point)
msg.pose.orientation.w = 1.0
msgs.markers.append(msg)
self.mpub.publish(msgs)
def publish_cartesian_markers(arm, time_stamp, cep, rot, c1, c2, marker_id):
marker = Marker()
marker.header.frame_id = ar.link_tf_name(arm, 0)
marker.header.stamp = time_stamp
marker.ns = arm
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.pose.position.x = cep[0,0]
marker.pose.position.y = cep[1,0]
marker.pose.position.z = cep[2,0]
marker.scale.x = 0.1
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.lifetime = rospy.Duration()
marker.id = marker_id*100 + 0
#rot1 = tr.Ry(math.radians(90.)) * rot.T
rot1 = rot * tr.rotY(math.pi/2)
quat = tr.matrix_to_quaternion(rot1)
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.color.r = c1[0]
marker.color.g = c1[1]
marker.color.b = c1[2]
marker.color.a = 1.
marker_pub.publish(marker)
marker.id = marker_id*100 + 1
if arm == 'left_arm':
#rot2 = tr.Rz(math.radians(90.)) * rot.T
rot2 = rot * tr.rotZ(-math.pi/2)
else:
#rot2 = tr.Rz(math.radians(-90.)) * rot.T
rot2 = rot * tr.rotZ(math.pi/2)
quat = tr.matrix_to_quaternion(rot2)
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.color.r = c2[0]
marker.color.g = c2[1]
marker.color.b = c2[2]
marker.color.a = 1.
marker_pub.publish(marker)
def publishObstacles(self):
"""
Publishes the obstacles as markers
"""
mk = Marker()
mk.header.stamp = rospy.get_rostime()
mk.header.frame_id = '/base_link'
mk.ns='basic_shapes'
mk.id = 0
mk.type = Marker.POINTS
mk.scale.x = 0.3
mk.scale.y = 0.3
mk.scale.z = 0.3
mk.color.r = 1.0
mk.color.a = 1.0
for value in self.obstacle_map.obstacles_in_memory:
p = Point()
p.x = value[0]
p.y = value[1]
mk.points.append(p)
self.obs_pub.publish(mk)
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 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 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 new_marker(self, ns="/debug", frame="enu", time=None, type = Marker.CUBE , position=(0,0,0), orientation=(0,0,0,1), color=(1,0,0)):
marker = Marker()
marker.ns = ns
if time != None:
marker.header.stamp = time
marker.header.frame_id = frame
marker.type = type
marker.action = marker.ADD
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.color.a = 1.0
marker.id = self.last_id
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
marker.pose.position.x = position[0]
marker.pose.position.y = position[1]
marker.pose.position.z = position[2]
self.last_id += 1
self.markers.markers.append(marker)
def __publish_marker_for_object(self, object, txt):
marker = Marker()
marker.header.frame_id = object.object.header.frame_id
marker.header.stamp = rospy.Time(0)
marker.ns = "scene_classifier_marker"
self.marker_id = self.marker_id + 1
marker.id = self.marker_id
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.ADD
marker.pose.position.x = object.c_centroid.x
marker.pose.position.y = object.c_centroid.y
marker.pose.position.z = object.c_centroid.z
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.5
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.text = txt
marker.lifetime = rospy.Duration.from_sec(10)
self.marker_publisher.publish(marker)
def publish(anns, data):
wall_list = WallList()
marker_list = MarkerArray()
marker_id = 1
for a, d in zip(anns, data):
# Walls
object = deserialize_msg(d.data, Wall)
wall_list.obstacles.append(object)
# Markers
marker = Marker()
marker.id = marker_id
marker.header = a.pose.header
marker.type = a.shape
marker.ns = "wall_obstacles"
marker.action = Marker.ADD
marker.lifetime = rospy.Duration.from_sec(0)
marker.pose = copy.deepcopy(a.pose.pose.pose)
marker.scale = a.size
marker.color = a.color
marker_list.markers.append(marker)
marker_id = marker_id + 1
marker_pub = rospy.Publisher('wall_marker', MarkerArray, latch=True, queue_size=1)
wall_pub = rospy.Publisher('wall_pose_list', WallList, latch=True, queue_size=1)
wall_pub.publish(wall_list)
marker_pub.publish(marker_list)
return
def to_marker(self):
"""
:return: a marker representing the map.
:type: Marker
"""
marker = Marker()
marker.type = Marker.CUBE_LIST
for x in range(0, len(self.field)):
for y in range(0, len(self.field[0])):
marker.header.frame_id = '/odom_combined'
marker.ns = 'suturo_planning/map'
marker.id = 23
marker.action = Marker.ADD
(x_i, y_i) = self.index_to_coordinates(x, y)
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.pose.orientation.w = 1
marker.scale.x = self.cell_size
marker.scale.y = self.cell_size
marker.scale.z = 0.01
p = Point()
p.x = x_i
p.y = y_i
marker.points.append(p)
c = self.get_cell_by_index(x, y)
marker.colors.append(self.__cell_to_color(c))
marker.lifetime = rospy.Time(0)
return marker
def publish_prob2(self, waypoints, objects, probs):
prob_msg = MarkerArray()
i = 0
idx = 0
n_waypoints = len(waypoints)
n_objects = len(objects)
scaling_factor = max(probs)
current_probs = [0 for foo in objects]
for node in self._topo_map:
if node.name in waypoints:
for j in range(0, n_objects):
marker = Marker()
marker.header.frame_id = 'map'
marker.id = idx
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.pose = node.pose
prob = probs[n_objects*i + j]
prob = prob/(scaling_factor)
print "AHAHHAHBHBHBHBHBHB", prob
marker.pose.position.z = marker.pose.position.z + current_probs[j]
marker.scale.x = 1*prob
marker.scale.y = 1*prob
marker.scale.z = 1*prob
current_probs[j] = current_probs[j] + prob + 0.1
marker.color.a = 1.0
marker.color.r = 1.0*prob
marker.color.g = 1.0*prob
marker.color.b = 1.0*prob
prob_msg.markers.append(marker)
idx = idx + 1
i = i + 1
self._prob_marker_pub.publish(prob_msg)
def publish_prob(self, waypoints, objects, probs):
prob_msg = MarkerArray()
i = 0
n_waypoints = len(waypoints)
n_objects = len(objects)
scaling_factor = max(probs)
for node in self._topo_map:
if node.name in waypoints:
marker = Marker()
marker.header.frame_id = 'map'
marker.id = i
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.pose = node.pose
prob = 1
for j in range(0, n_objects):
prob = prob*probs[n_objects*i + j]
prob = prob/(scaling_factor**2)
print prob
marker.scale.x = 1*prob
marker.scale.y = 1*prob
marker.scale.z = 1
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
prob_msg.markers.append(marker)
i = i + 1
self._prob_marker_pub.publish(prob_msg)
def publish_markerArray(publisher, points, rgba=(1, 0, 0, 1), shape=Marker.CUBE, duration=rospy.Duration(360), ns='ns'):
#points is expected to be a list of tuples (x,y)
#It's recommended that the publisher is created with latch=True
_id = 0
ma = MarkerArray()
for p in points:
m = Marker()
m.header.frame_id = '/map'
m.header.stamp = rospy.get_rostime()
m.ns = ns
m.id = _id
m.type = shape
m.action = m.ADD
m.pose.position.x = p[0]
m.pose.position.y = p[1]
m.pose.orientation.w = 1
m.scale.x = 0.5
m.scale.y = 0.5
m.scale.z = 0.5
m.color.r = rgba[0]
m.color.g = rgba[1]
m.color.b = rgba[2]
m.color.a = rgba[3]
m.lifetime = duration
ma.markers.append(m)
_id += 1
publisher.publish(ma)
def pose_cb(self, pose):
pose = self.listener.transformPose(self.reference_frame,pose)
q = pose.pose.orientation
qvec = [q.x,q.y,q.z,q.w]
euler = euler_from_quaternion(qvec)
self.goal_x = pose.pose.position.x
self.goal_y = pose.pose.position.y
self.goal_theta = euler[2]
(ex,ey,etheta) = self.compute_error()
if ex < -self.dist_threshold:
self.goal_theta = norm_angle(etheta + pi)
print "New goal: %.2f %.2f %.2f" % (self.goal_x, self.goal_y, self.goal_theta)
marker = Marker()
marker.header = pose.header
marker.ns = "goal_marker"
marker.id = 10
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.pose = pose.pose
marker.scale.x = 0.5
marker.scale.y = 0.5
marker.scale.z = 1.0;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.lifetime.secs=-1.0
self.marker_pub.publish(marker)
def publish_tracked_people(self, now):
"""
Publish markers of tracked people to Rviz and to <people_tracked> topic
"""
people_tracked_msg = PersonArray()
people_tracked_msg.header.stamp = now
people_tracked_msg.header.frame_id = self.publish_people_frame
marker_id = 0
# Make sure we can get the required transform first:
if self.use_scan_header_stamp_for_tfs:
tf_time = now
try:
self.listener.waitForTransform(self.publish_people_frame, self.fixed_frame, tf_time, rospy.Duration(1.0))
transform_available = True
except:
transform_available = False
else:
tf_time = rospy.Time(0)
transform_available = self.listener.canTransform(self.publish_people_frame, self.fixed_frame, tf_time)
marker_id = 0
if not transform_available:
rospy.loginfo("Person tracker: tf not avaiable. Not publishing people")
else:
for person in self.objects_tracked:
if self.publish_occluded or person.seen_in_current_scan: # Only publish people who have been seen in current scan, unless we want to publish occluded people
# Get position in the <self.publish_people_frame> frame
ps = PointStamped()
ps.header.frame_id = self.fixed_frame
ps.header.stamp = tf_time
ps.point.x = person.pos_x
ps.point.y = person.pos_y
try:
ps = self.listener.transformPoint(self.publish_people_frame, ps)
except:
rospy.logerr("Not publishing people due to no transform from fixed_frame-->publish_people_frame")
continue
# publish to people_tracked topic
new_person = Person()
new_person.pose.position.x = ps.point.x
new_person.pose.position.y = ps.point.y
yaw = math.atan2(person.vel_y, person.vel_x)
quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
new_person.pose.orientation.x = quaternion[0]
new_person.pose.orientation.y = quaternion[1]
new_person.pose.orientation.z = quaternion[2]
new_person.pose.orientation.w = quaternion[3]
new_person.id = person.id_num
new_person.speed = np.sqrt(person.vel_x**2 + person.vel_y**2)
people_tracked_msg.people.append(copy.deepcopy(new_person))
# publish rviz markers
# Cylinder for body
marker = Marker()
marker.header.frame_id = self.publish_people_frame
marker.header.stamp = now
marker.ns = "People_tracked"
marker.color.r = person.colour[0]
marker.color.g = person.colour[1]
marker.color.b = person.colour[2]
marker.color.a = (rospy.Duration(3) - (rospy.get_rostime() - person.last_seen)).to_sec()/rospy.Duration(3).to_sec() + 0.1
marker.pose.position.x = ps.point.x
marker.pose.position.y = ps.point.y
marker.id = marker_id
marker_id += 1
marker.type = Marker.CYLINDER
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 1.2
marker.pose.position.z = 0.8
self.marker_pub.publish(marker)
# Sphere for head shape
marker.type = Marker.SPHERE
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.pose.position.z = 1.5
marker.id = marker_id
marker_id += 1
self.marker_pub.publish(marker)
# Text showing person's ID number
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
marker.id = marker_id
marker_id += 1
marker.type = Marker.TEXT_VIEW_FACING
marker.text = "C: {:.2f}".format(person.confidence) #str(person.id_num)
marker.scale.z = 0.2
marker.pose.position.z = 1.7
self.marker_pub.publish(marker)
# Arrow pointing in direction they're facing with magnitude proportional to speed
marker.color.r = person.colour[0]
marker.color.g = person.colour[1]
marker.color.b = person.colour[2]
marker.color.a = (rospy.Duration(3) - (rospy.get_rostime() - person.last_seen)).to_sec()/rospy.Duration(3).to_sec() + 0.1
start_point = Point()
end_point = Point()
start_point.x = marker.pose.position.x
start_point.y = marker.pose.position.y
end_point.x = start_point.x + 0.5*person.vel_x
end_point.y = start_point.y + 0.5*person.vel_y
marker.pose.position.x = 0.
marker.pose.position.y = 0.
marker.pose.position.z = 0.1
marker.id = marker_id
marker_id += 1
marker.type = Marker.ARROW
marker.points.append(start_point)
marker.points.append(end_point)
marker.scale.x = 0.05
marker.scale.y = 0.1
marker.scale.z = 0.2
self.marker_pub.publish(marker)
# <self.confidence_percentile>% confidence bounds of person's position as an ellipse:
cov = person.filtered_state_covariances[0][0] + person.var_obs # cov_xx == cov_yy == cov
std = cov**(1./2.)
gate_dist_euclid = scipy.stats.norm.ppf(1.0 - (1.0-self.confidence_percentile)/2., 0, std)
marker.pose.position.x = ps.point.x
marker.pose.position.y = ps.point.y
marker.type = Marker.SPHERE
marker.scale.x = 2*gate_dist_euclid
marker.scale.y = 2*gate_dist_euclid
marker.scale.z = 0.01
marker.color.r = person.colour[0]
marker.color.g = person.colour[1]
marker.color.b = person.colour[2]
marker.color.a = 0.1
marker.pose.position.z = 0.0
marker.id = marker_id
marker_id += 1
self.marker_pub.publish(marker)
# Clear previously published people markers
for m_id in xrange(marker_id, self.prev_person_marker_id):
marker = Marker()
marker.header.stamp = now
marker.header.frame_id = self.publish_people_frame
marker.ns = "People_tracked"
marker.id = m_id
marker.action = marker.DELETE
self.marker_pub.publish(marker)
self.prev_person_marker_id = marker_id
# Publish people tracked message
self.people_tracked_pub.publish(people_tracked_msg)
self.polar_grid.publish_fov(now)
def pubTF(self, timer):
# rospy.loginfo("[CallbackPy]***************In PUBTF")
br = tf.TransformBroadcaster()
marker_tmp = Marker()
marker_tmp.header.frame_id = "world"
marker_tmp.type = marker_tmp.CUBE_LIST
marker_tmp.action = marker_tmp.ADD
marker_static = copy.deepcopy(marker_tmp)
marker_dynamic = copy.deepcopy(marker_tmp)
marker_dynamic.color = color_dynamic
# marker_dynamic.scale.x=self.world.bbox_dynamic[0]
# marker_dynamic.scale.y=self.world.bbox_dynamic[1]
# marker_dynamic.scale.z=self.world.bbox_dynamic[2]
marker_static.color = color_static
###################3
marker_array_static_mesh = MarkerArray()
marker_array_dynamic_mesh = MarkerArray()
for i in range(self.world.num_of_dyn_objects +
self.world.num_of_stat_objects):
t_ros = rospy.Time.now()
t = rospy.get_time()
#Same as before, but it's float
dynamic_trajectory_msg = DynTraj()
bbox_i = self.bboxes[i]
s = self.world.scale
if (self.type[i] == "dynamic"):
[x_string, y_string,
z_string] = self.trefoil(self.x_all[i], self.y_all[i],
self.z_all[i], s, s, s,
self.offset_all[i], self.slower[i])
# print("self.bboxes[i]= ", self.bboxes[i])
dynamic_trajectory_msg.bbox = bbox_i
marker_dynamic.scale.x = bbox_i[0]
marker_dynamic.scale.y = bbox_i[1]
marker_dynamic.scale.z = bbox_i[2]
else:
# [x_string, y_string, z_string] = self.static(self.x_all[i], self.y_all[i], self.z_all[i]);
dynamic_trajectory_msg.bbox = bbox_i
marker_static.scale.x = bbox_i[0]
marker_static.scale.y = bbox_i[1]
marker_static.scale.z = bbox_i[2]
if (self.type[i] == "static_vert"):
[x_string, y_string,
z_string] = self.static(self.x_all[i], self.y_all[i],
self.z_all[i])
else:
[x_string, y_string,
z_string] = self.wave_in_z(self.x_all[i], self.y_all[i],
self.z_all[i], 2.0,
self.offset_all[i], 1.0)
x = eval(x_string)
y = eval(y_string)
z = eval(z_string)
dynamic_trajectory_msg.is_agent = False
dynamic_trajectory_msg.header.stamp = t_ros
dynamic_trajectory_msg.function = [x_string, y_string, z_string]
dynamic_trajectory_msg.pos.x = x #Current position
dynamic_trajectory_msg.pos.y = y #Current position
dynamic_trajectory_msg.pos.z = z #Current position
dynamic_trajectory_msg.id = 4000 + i #Current id 4000 to avoid interference with ids from agents #TODO
self.pubTraj.publish(dynamic_trajectory_msg)
br.sendTransform((x, y, z), (0, 0, 0, 1), t_ros,
self.name + str(dynamic_trajectory_msg.id),
"world")
#If you want to move the objets in gazebo
# gazebo_state = ModelState()
# gazebo_state.model_name = str(i)
# gazebo_state.pose.position.x = x
# gazebo_state.pose.position.y = y
# gazebo_state.pose.position.z = z
# gazebo_state.reference_frame = "world"
# self.pubGazeboState.publish(gazebo_state)
#If you want to see the objects in rviz
point = Point()
point.x = x
point.y = y
point.z = z
##########################
marker = Marker()
marker.id = i
marker.ns = "mesh"
marker.header.frame_id = "world"
marker.type = marker.MESH_RESOURCE
marker.action = marker.ADD
marker.pose.position.x = x
marker.pose.position.y = y
marker.pose.position.z = z
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.lifetime = rospy.Duration.from_sec(0.0)
marker.mesh_use_embedded_materials = True
marker.mesh_resource = self.meshes[i]
if (self.type[i] == "dynamic"):
marker_dynamic.points.append(point)
marker.scale.x = bbox_i[0]
marker.scale.y = bbox_i[1]
marker.scale.z = bbox_i[2]
marker_array_dynamic_mesh.markers.append(marker)
if (self.type[i] == "static_vert"
or self.type[i] == "static_horiz"):
marker.scale.x = bbox_i[0]
marker.scale.y = bbox_i[1]
marker.scale.z = bbox_i[2]
marker_array_static_mesh.markers.append(marker)
##########################
marker_static.points.append(point)
self.pubShapes_dynamic_mesh.publish(marker_array_dynamic_mesh)
self.pubShapes_dynamic.publish(marker_dynamic)
# if(self.already_published_static_shapes==False):
self.pubShapes_static_mesh.publish(marker_array_static_mesh)
self.pubShapes_static.publish(marker_static)
def find_affordance(self, data):
aff_list = list()
if self.affordance[0] == 'pcb':
try:
tmp_img = self.bridge.imgmsg_to_cv2(data.assos_img, 'rgb8')
tmp_img2 = ImageEnhance.Color(
Image.fromarray(tmp_img)).enhance(2)
self.curr_img = image.img_to_array(tmp_img2)
except CvBridgeError as e:
print(e)
for part in data.part_array:
partname = part.part_id[:-1]
if partname == 'pcb':
pcb = part
aff = Affordance()
aff.object_name = pcb.part_id
pcb_bounding_values = self.returnPcbBoundingValues(pcb)
rospy.set_param('pcb_bounding_values', pcb_bounding_values)
img_w = self.curr_img.shape[0]
img_h = self.curr_img.shape[1]
aff_mask = self.model.predict(self.curr_img)
leverup_points, confidences = self.sample_leverup_points(aff_mask)
if len(leverup_points) == 0:
return aff_list
aff.effect_name = 'levered'
aff.affordance_name = 'leverable'
leverup_points_converted = ConvertPixel2WorldRequest()
for i in range(len(leverup_points)):
lever_up_point = geometry_msgs.msg.Point()
lever_up_point.y = leverup_points[i][0] * img_w / 256.0
lever_up_point.x = leverup_points[i][1] * img_h / 256.0
lever_up_point.z = 0
leverup_points_converted.pixels.append(lever_up_point)
resp = self.pixel_world_srv(leverup_points_converted)
affordance_vis_image = cv2.resize(tmp_img, (256, 256))
tmp_img = self.bridge.imgmsg_to_cv2(
pcb.part_outline.part_mask,
pcb.part_outline.part_mask.encoding)
tmp_img = cv2.resize(tmp_img, (256, 256))
markerArray = MarkerArray()
for i in range(len(leverup_points)):
marker = Marker()
ap = ActionParameters()
ap.confidence = confidences[i]
lever_up_point = AscPair()
lever_up_point.key = 'start_pose'
lever_up_point.value_type = 2
lever_up_point.value_pose.position.x = resp.points[i].x
lever_up_point.value_pose.position.y = resp.points[i].y
lever_up_point.value_pose.position.z = resp.points[i].z
x = leverup_points[i][0]
y = leverup_points[i][1]
w_size = 9
temp = tmp_img[max(0, x - w_size):min(256, x + w_size),
max(0, y - w_size):min(256, y + w_size)]
x1, y1 = np.where(temp == 255)
x2, y2 = np.where(temp == 0)
direction = math.pi + math.atan2(
np.mean(y2) - np.mean(y1),
np.mean(x2) - np.mean(x1))
if len(y2) == 0 or len(y1) == 0 or len(x2) == 0 or len(
x1) == 0 or np.isnan(direction) or np.isinf(direction):
continue
import tf
point_inverted = np.array(leverup_points[i][::-1])
_ = cv2.arrowedLine(
affordance_vis_image,
tuple(point_inverted.astype(np.int16)),
tuple((point_inverted +
[np.sin(direction) * 15,
np.cos(direction) * 15]).astype(np.int16)),
(0, 0, 255),
1,
tipLength=0.5)
_ = cv2.arrowedLine(
affordanceWrapper.affordance_vis_image,
tuple(point_inverted.astype(np.int16)),
tuple((point_inverted +
[np.sin(direction) * 15,
np.cos(direction) * 15]).astype(np.int16)),
(0, 0, 255),
1,
tipLength=0.5)
quaternion = tf.transformations.quaternion_from_euler(
0, 0, direction)
lever_up_point.value_pose.orientation.x = quaternion[0]
lever_up_point.value_pose.orientation.y = quaternion[1]
lever_up_point.value_pose.orientation.z = quaternion[2]
lever_up_point.value_pose.orientation.w = quaternion[3]
ap.parameters.append(lever_up_point)
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
h.frame_id = resp.header.frame_id
marker.header = h
marker.ns = "lever_up_points"
marker.id = i
marker.type = 0 # arrow
marker.action = 0
marker.scale.x = 0.01
marker.scale.y = 0.001
marker.scale.z = 0.002
marker.lifetime = rospy.Duration(150)
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.pose = lever_up_point.value_pose
markerArray.markers.append(marker)
aff.action_parameters_array.append(ap)
aff_list.append(aff)
rate = rospy.Rate(10)
comp_img = self.bridge.cv2_to_compressed_imgmsg(
affordance_vis_image)
for _ in range(5):
self.lever_up_rviz.publish(markerArray)
self.lever_up_image_viz.publish(comp_img)
rate.sleep()
elif self.affordance[0] == 'magnet':
for part in data.part_array:
partname = part.part_id[:-1]
if partname == 'magnet':
aff = Affordance()
aff.object_name = part.part_id
aff.effect_name = 'levered'
aff.affordance_name = 'leverable'
ap = ActionParameters()
ap.confidence = 0.8 # Dummy Value
asc_pair = AscPair()
asc_pair.key = 'start_pose'
asc_pair.value_type = 2
asc_pair.value_pose = part.pose
ap.parameters.append(asc_pair)
aff.action_parameters_array.append(ap)
aff_list.append(aff)
return aff_list
def publish_3dbox(box3d_pub, corners_3d_velos, track_ids, types=None, publish_id=True):
"""
Publish 3d boxes in velodyne coordinate, with color specified by object_types
If object_types is None, set all color to cyan
corners_3d_velos : list of (8, 4) 3d corners
"""
marker_array = MarkerArray()
for i, corners_3d_velo in enumerate(corners_3d_velos):
# 3d box
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)
marker.type = Marker.LINE_LIST
b, g, r = DETECTION_COLOR_MAP[types[i]]
if types is None:
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 1.0
else:
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 = corners_3d_velo[l[0]]
marker.points.append(Point(p1[0], p1[1], p1[2]))
p2 = corners_3d_velo[l[1]]
marker.points.append(Point(p2[0], p2[1], p2[2]))
marker_array.markers.append(marker)
# add track id
if publish_id:
track_id = track_ids[i]
text_marker = Marker()
text_marker.header.frame_id = FRAME_ID
text_marker.header.stamp = rospy.Time.now()
text_marker.id = track_id + 1000
text_marker.action = Marker.ADD
text_marker.lifetime = rospy.Duration(LIFETIME)
text_marker.type = Marker.TEXT_VIEW_FACING
p4 = corners_3d_velo[4] # upper front left corner
text_marker.pose.position.x = p4[0]
text_marker.pose.position.y = p4[1]
text_marker.pose.position.z = p4[2] + 0.5
text_marker.text = str(track_id)
text_marker.scale.x = 1
text_marker.scale.y = 1
text_marker.scale.z = 1
if types is None:
text_marker.color.r = 0.0
text_marker.color.g = 1.0
text_marker.color.b = 1.0
else:
b, g, r = DETECTION_COLOR_MAP[types[i]]
text_marker.color.r = r/255.0
text_marker.color.g = g/255.0
text_marker.color.b = b/255.0
text_marker.color.a = 1.0
marker_array.markers.append(text_marker)
box3d_pub.publish(marker_array)
r = rospy.Rate(30)
f = float(0)
while not rospy.is_shutdown():
points, line_strip, line_list = Marker(), Marker(), Marker()
points.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = "/my_frame"
points.header.stamp = line_strip.header.stamp = line_list.header.stamp = rospy.Time.now(
)
points.ns = line_strip.ns = line_list.ns = "points_and_lines"
points.action = line_list.action = line_list.action = Marker.ADD
points.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0
points.id = 0
line_strip.id = 1
line_list.id = 2
points.type = Marker.POINTS
line_strip.type = Marker.LINE_STRIP
line_list.type = Marker.LINE_LIST
# POINTS markers use x and y scale for width/height respectively
points.scale.x = 0.2
points.scale.y = 0.2
# LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
line_strip.scale.x = 0.1
line_list.scale.x = 0.1
for j in range(N_MARKER):
for k in range(N_MARKER):
marker = Marker()
marker.header.frame_id = "/world"
marker.type = marker.CUBE
marker.action = marker.ADD
marker.scale.x = cube_scale
marker.scale.y = cube_scale
marker.scale.z = cube_scale
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.pose.orientation.w = 1.0
marker.id = idx
marker.pose.position.x = l_w / N_MARKER * i + r_min #math.cos(count / 50.0)
marker.pose.position.y = l_w / N_MARKER * j + r_min #math.cos(count / 40.0)
marker.pose.position.z = l_w / N_MARKER * k + r_min #math.cos(count / 30.0)
markerArray.markers.append(marker)
idx += 1
while not rospy.is_shutdown():
# We add the new marker to the MarkerArray, removing the oldest
# marker from it when necessary
# if(count > MARKERS_MAX):
# markerArray.markers.pop(0)
# Renumber the marker IDs
# id = 0
def approach(event):
global trajs
global records
global cur_msg
# print(len(records[0,:]))
print records
for prs in range(0, len(records[0, :])):
# if prs == processed_prs:
# continue
# global buffer_full
#print('records:')
#print(records)
#print((prs,records[0,prs]))
if records[0, prs] >= 7:
if len(cur_msg.poses) > prs:
j = cur_msg.poses[prs]
xpos = j.position.x
ypos = j.position.y
global xpos_array
global ypos_array
xpos_array = trajs[prs][0]
ypos_array = trajs[prs][1]
# print('xpos_array:')
# print(xpos_array)
# print('ypos_array:')
# print(ypos_array)
xpos_std = statistics.stdev(xpos_array)
ypos_std = statistics.stdev(ypos_array)
xpos_array = []
ypos_array = []
#records[0,prs] = 0
# print('records:')
# print(records)
stopped = False
if xpos_std < 0.005 and ypos_std < 0.005:
print('%d.%d - Person %d stopped!' %
(cur_msg.header.stamp.secs,
cur_msg.header.stamp.nsecs / 10000000, prs))
stopped = True
else:
print('%d.%d - Person %d moving...' %
(cur_msg.header.stamp.secs,
cur_msg.header.stamp.nsecs / 1000000, prs))
stopped = False
#print((xpos_std,ypos_std))
if stopped:
global processed_prs
processed_prs = prs
quaternion = (j.orientation.x, j.orientation.y,
j.orientation.z, j.orientation.w)
euler = tf.transformations.euler_from_quaternion(
quaternion)
roll = euler[0]
pitch = euler[1]
yaw = euler[2]
#angle = math.pi-yaw
xdif = math.fabs(xpos - cur_robposx)
ydif = math.fabs(ypos - cur_robposy)
if xpos > cur_robposx:
if ypos > cur_robposy:
angle = math.atan(ydif / xdif)
else:
angle = (2 * math.pi) - math.atan(ydif / xdif)
else:
if ypos > cur_robposy:
angle = math.pi - math.atan(ydif / xdif)
else:
angle = math.pi + math.atan(ydif / xdif)
#print((xpos,ypos,cur_robposx,cur_robposy,(angle/math.pi)*180))
print((prs, xpos, ypos))
xd = math.fabs(math.cos(angle) * 0.65)
yd = math.fabs(math.sin(angle) * 0.65)
#print(((angle/math.pi)*180, xpos, ypos, xd, yd))
if angle >= 0 and angle < math.pi:
yrob = ypos - yd
if angle <= math.pi / 2:
xrob = xpos - xd
else:
xrob = xpos + xd
else:
yrob = ypos + yd
if angle <= (3 * math.pi / 2):
xrob = xpos + xd
else:
xrob = xpos - xd
marker = Marker()
marker.header = cur_msg.header
marker.ns = 'ROBOT'
marker.id = 0
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.lifetime.secs = 0
marker.lifetime.nsecs = 500000000
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.pose.orientation.w = 1.0
marker.pose.position.x = xrob
marker.pose.position.y = yrob
marker.pose.position.z = 0
publisher.publish(marker)
new_angle = angle #-math.pi
#print((angle/math.pi)*180, xpos, ypos, xrob, yrob,(new_angle/math.pi)*180)
marker2 = Marker()
marker2.header = cur_msg.header
marker2.ns = 'ROBOT'
marker2.id = 0
marker2.type = marker2.ARROW
marker2.action = marker2.ADD
marker2.lifetime.secs = 0
marker2.lifetime.nsecs = 500000000
marker2.scale.x = 1.0
marker2.scale.y = 0.1
marker2.scale.z = 0.1
marker2.color.a = 1.0
marker2.color.b = 0.5
marker2.color.r = 1.0
new_quaternion = tf.transformations.quaternion_from_euler(
0, 0, new_angle)
marker2.pose.orientation.x = new_quaternion[0]
marker2.pose.orientation.y = new_quaternion[1]
marker2.pose.orientation.z = new_quaternion[2]
marker2.pose.orientation.w = new_quaternion[3]
marker2.pose.position.x = xrob
marker2.pose.position.y = yrob
marker2.pose.position.z = 0
publisher2.publish(marker2)
#print(rospy.Duration())
client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "odom_combined"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = xrob
goal.target_pose.pose.position.y = yrob
goal.target_pose.pose.orientation.x = new_quaternion[0]
goal.target_pose.pose.orientation.y = new_quaternion[1]
goal.target_pose.pose.orientation.z = new_quaternion[2]
goal.target_pose.pose.orientation.w = new_quaternion[3]
client.send_goal(goal)
client.wait_for_result(rospy.Duration.from_sec(5.0))
# #rospy.sleep(3)
idx = random.randint(0, len(speechs) - 1)
#soundhandle.say(speechs[idx])
#rospy.sleep(3)
# speechClient.wait_for_server()
#speechGoal = maryttsActionGoal()
#speechGoal.header.frame_id = ''
#speechGoal.header.stamp = rospy.Time.now()
# sGoal = maryttsGoal()
# sGoal.text = speechs[idx]
#print(sGoal)
#speechGoal.goal = sGoal
# speechClient.send_goal(sGoal)
# speechClient.wait_for_result(rospy.Duration.from_sec(3.0))
speech_msg = String()
speech_msg.data = speechs[idx]
pub3.publish(speech_msg)
rospy.sleep(3)
do_randomwalk()
def observation_callback(self, tracks_msg):
markers_msg = MarkerArray()
self.last_marker_id = 0
for track in tracks_msg.changes.nodes:
if track.has_shape is True:
for shape_idx, shape in enumerate(track.shapes):
marker = Marker()
self.last_marker_id += 1
marker.id = self.last_marker_id
marker.ns = track.id
marker.action = Marker.MODIFY
marker.header = tracks_msg.header
position = [
track.pose_stamped.pose.pose.position.x,
track.pose_stamped.pose.pose.position.y,
track.pose_stamped.pose.pose.position.z
]
orientation = [
track.pose_stamped.pose.pose.orientation.x,
track.pose_stamped.pose.pose.orientation.y,
track.pose_stamped.pose.pose.orientation.z,
track.pose_stamped.pose.pose.orientation.w
]
world_transform = np.dot(translation_matrix(position),
quaternion_matrix(orientation))
position = [
shape.pose.position.x, shape.pose.position.y,
shape.pose.position.z
]
orientation = [
shape.pose.orientation.x, shape.pose.orientation.y,
shape.pose.orientation.z, shape.pose.orientation.w
]
shape_transform = np.dot(translation_matrix(position),
quaternion_matrix(orientation))
shape_world_transform = np.dot(world_transform,
shape_transform)
position = translation_from_matrix(shape_world_transform)
orientation = quaternion_from_matrix(shape_world_transform)
marker.pose.position.x = position[0]
marker.pose.position.y = position[1]
marker.pose.position.z = position[2]
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
if shape.type == PrimitiveShape.CYLINDER:
marker.type = Marker.CYLINDER
marker.scale = Vector3(x=shape.dimensions[0],
y=shape.dimensions[0],
z=shape.dimensions[1])
elif shape.type == PrimitiveShape.SPHERE:
marker.type = Marker.SPHERE
marker.scale = Vector3(x=shape.dimensions[0],
y=shape.dimensions[0],
z=shape.dimensions[0])
elif shape.type == PrimitiveShape.BOX:
marker.type = Marker.CUBE
marker.scale = Vector3(x=shape.dimensions[0],
y=shape.dimensions[1],
z=shape.dimensions[2])
elif shape.type == PrimitiveShape.MESH:
if shape.mesh_resource != "":
marker.type = Marker.MESH_RESOURCE
marker.mesh_resource = shape.mesh_resource
marker.mesh_use_embedded_materials = True
else:
marker.type = Marker.TRIANGLE_LIST
marker.points = shape.vertices
marker.scale = Vector3(x=1.0, y=1.0, z=1.0)
else:
raise NotImplementedError("Shape not implemented")
marker.color = shape.color
marker.color.a = 0.75
marker.lifetime = rospy.Duration(0.25)
markers_msg.markers.append(marker)
self.markers_publisher.publish(markers_msg)
def publishMarkers(self):
drone_marker = Marker()
drone_marker.header.frame_id = "/map"
drone_marker.header.stamp = rospy.Time.now()
drone_marker.id = ord(self._id[-1])
drone_marker.ns = "uavs"
drone_marker.type = Marker.MESH_RESOURCE
drone_marker.mesh_resource = "package://robots_description/models/mbzirc/meshes/multirotor.dae"
drone_marker.color.a = 1
drone_marker.action = Marker.ADD
drone_marker.pose = self._pose.pose
drone_marker.scale.x = 0.001
drone_marker.scale.y = 0.001
drone_marker.scale.z = 0.001
drone_marker.mesh_use_embedded_materials = True
drone_marker.color.r = 0.0
drone_marker.color.g = 0.0
drone_marker.color.b = 0.0
# Drone identifier
id_marker = Marker()
id_marker.header.frame_id = "/map"
id_marker.header.stamp = rospy.Time.now()
id_marker.id = ord(self._id[-1])
id_marker.ns = "uavs_state"
id_marker.type = Marker.TEXT_VIEW_FACING
id_marker.text = self._id
id_marker.pose.position.z = self._pose.pose.position.z + 2.0
id_marker.pose.position.y = self._pose.pose.position.y
id_marker.pose.position.x = self._pose.pose.position.x
id_marker.color.a = 1
id_marker.color.r = 1.0
id_marker.color.g = 1.0
id_marker.color.b = 1.0
id_marker.scale.x = 1
id_marker.scale.y = 1
id_marker.scale.z = 1
id_marker.mesh_use_embedded_materials = True
# Drone goal
goal_marker = Marker()
goal_marker.header.frame_id = "/map"
goal_marker.header.stamp = rospy.Time.now()
goal_marker.id = ord(self._id[-1])
goal_marker.ns = "uavs_state"
goal_marker.type = Marker.SPHERE
goal_marker.text = self._id
goal_marker.pose.position.z = self._goal[2]
goal_marker.pose.position.y = self._goal[1]
goal_marker.pose.position.x = self._goal[0]
goal_marker.color.a = 1
goal_marker.color.r = 0.0
goal_marker.color.g = 1.0
goal_marker.color.b = 0.0
goal_marker.scale.x = 1
goal_marker.scale.y = 1
goal_marker.scale.z = 1
goal_marker.mesh_use_embedded_materials = True
# Drone cylinder
uav_cylinder = Marker()
uav_cylinder.header.frame_id = "/map"
uav_cylinder.header.stamp = rospy.Time.now()
uav_cylinder.id = ord(self._id[-1])
uav_cylinder.ns = "uavs"
uav_cylinder.type = Marker.CYLINDER
uav_cylinder.color.a = 0.5
uav_cylinder.color.r = 1.0
uav_cylinder.color.g = 0.647
uav_cylinder.color.b = 0
uav_cylinder.action = Marker.ADD
uav_cylinder.pose.position = self._pose.pose.position
uav_cylinder.scale.x = 2 * self._radius # Diameter
uav_cylinder.scale.y = 2 * self._radius
uav_cylinder.scale.z = 5.0
uav_cylinder.mesh_use_embedded_materials = True
# Publish arrow for velocity
arrow = Marker()
arrow.header.frame_id = "/map"
arrow.header.stamp = rospy.Time.now()
arrow.id = ord(self._id[-1])
arrow.ns = "uavs"
arrow.type = Marker.ARROW
arrow.lifetime = rospy.Duration(0.15)
arrow.color.a = 1
arrow.color.r = 1.0
arrow.color.g = 0.0
arrow.color.b = 0.0
arrow.action = Marker.ADD
arrow.points = []
arrow.points.append(
Point(self._pose.pose.position.x, self._pose.pose.position.y,
self._pose.pose.position.z))
arrow.points.append(
Point(self._pose.pose.position.x + self._vel.twist.linear.x * 2,
self._pose.pose.position.y + self._vel.twist.linear.y * 2,
self._pose.pose.position.z))
arrow.scale.x = 0.1
arrow.scale.y = 0.2
self._vel_pub.publish(arrow)
self._pose_pub.publish(drone_marker)
self._cylinder_pub.publish(uav_cylinder)
self._id_pub.publish(id_marker)
self._goal_pub.publish(goal_marker)
def planPath(self, x0, y0, yaw0, signID):
self.firstNode = True
self.hitta = False
self.signID = signID
self.openSet = []
self.closedSet = []
#Target position
self.grid = [[None] * self.cell_x
] * self.cell_y # Creating the matrix: [[empty]*x]*y
targetSign = next(
(x for x in self.mapInfo["roadsigns"] if x["sign"] == self.signID),
None)
[xt0, yt0] = targetSign["pose"]["position"][:2]
self.yaw_t = math.radians(
targetSign["pose"]["orientation"][2]) + math.pi / 2
[self.xt, self.yt] = self.mapToGrid([
xt0 + math.cos(self.yaw_t) * self.target_threshold,
yt0 + math.sin(self.yaw_t) * self.target_threshold
])
self.Start_node = AstarNode(self, x0, y0, yaw0)
self.openSet.append(self.Start_node)
self.node = self.Start_node
#Starting position
[self.Start_node.x, self.Start_node.y] = self.mapToGrid([x0, y0])
#print([self.Start_node.x, self.Start_node.y])
self.grid[int(self.Start_node.y)][int(
self.Start_node.x)] = self.openSet[0]
tm_array = MarkerArray()
#Creating marker for displaying start location
tm = Marker()
tm.header.stamp = rospy.Time.now()
tm.header.frame_id = "map"
tm.id = 91
tm.type = tm.CYLINDER
tm.action = tm.ADD
tm.pose.position.x = x0
tm.pose.position.y = y0
tm.pose.position.z = 0.4
(tm.pose.orientation.x, tm.pose.orientation.y, tm.pose.orientation.z,
tm.pose.orientation.w) = quaternion_from_euler(0, 0, 0)
tm.scale.x = 0.05
tm.scale.y = 0.05
tm.scale.z = 0.8
tm.color = ColorRGBA(0.2, 1, 1, 1)
tm_array.markers.append(tm)
#Creating marker for displaying target location
tm = Marker()
tm.header.stamp = rospy.Time.now()
tm.header.frame_id = "map"
tm.id = 92
tm.type = tm.CYLINDER
tm.action = tm.ADD
tm.pose.position.x = self.gridToMap([self.xt, self.yt])[0]
tm.pose.position.y = self.gridToMap([self.xt, self.yt])[1]
tm.pose.position.z = 0.4
(tm.pose.orientation.x, tm.pose.orientation.y, tm.pose.orientation.z,
tm.pose.orientation.w) = quaternion_from_euler(0, 0, 0)
tm.scale.x = 0.05
tm.scale.y = 0.05
tm.scale.z = 0.8
tm.color = ColorRGBA(1, 1, 0, 1)
tm_array.markers.append(tm)
#Creating marker for displaying target roadsign
tm = Marker()
tm.header.stamp = rospy.Time.now()
tm.header.frame_id = "map"
tm.id = 93
tm.type = tm.CUBE
tm.action = tm.ADD
tm.pose.position.x = [
xt0 + math.cos(self.yaw_t) * 0.02,
yt0 + math.sin(self.yaw_t) * 0.02
][0]
tm.pose.position.y = [
xt0 + math.cos(self.yaw_t) * 0.02,
yt0 + math.sin(self.yaw_t) * 0.02
][1]
tm.pose.position.z = 0.4
(tm.pose.orientation.x, tm.pose.orientation.y, tm.pose.orientation.z,
tm.pose.orientation.w) = quaternion_from_euler(
0, 0, self.yaw_t - math.pi / 2)
tm.scale.x = 0.2
tm.scale.y = 0.01
tm.scale.z = 0.2
tm.color = ColorRGBA(1, 0, 0, 1)
tm_array.markers.append(tm)
rospy.sleep(0.1)
self.markerPub.publish(tm_array)
self.Create_Node()
result = self.PathPlanning()
# print(result)
return result
def map_to_pathPlanner(self):
#Read map info from json file
mapFilePath = "/home/alsarmi/Dropbox/KTH_Classes/Project_Course_Drone/dd2419_ws/src/course_packages/dd2419_resources/worlds_json/awesome.world.json"
mapString = ""
with open(mapFilePath, "r") as file:
for line in file: #for each row
l = line.strip().replace(" ", "") #remove all blankspace
mapString += l
self.mapInfo = ast.literal_eval(
mapString
) #convert string representation of read file into dictionary through some kind of black magic
# Upper bounds
self.x_ub = self.mapInfo["airspace"]["max"][
0] + 6 * self.threshold # From json
self.y_ub = self.mapInfo["airspace"]["max"][
1] + 6 * self.threshold # from json
# Lower bounds
self.x_lb = self.mapInfo["airspace"]["min"][
0] - 6 * self.threshold # From json
self.y_lb = self.mapInfo["airspace"]["min"][
1] - 6 * self.threshold # from json
# Making the grid
self.cell_x = int((self.x_ub - self.x_lb) /
self.cell_constant) # Number of cells in x
self.cell_y = int((self.y_ub - self.y_lb) /
self.cell_constant) # Number of cells in y
lines = []
for wall in self.mapInfo["walls"]:
wallStart = [
wall["plane"]["start"][0], wall["plane"]["start"][1],
wall["plane"]["start"][2]
]
wallStop = [
wall["plane"]["stop"][0], wall["plane"]["stop"][1],
wall["plane"]["start"][2]
]
lines.append(
LineString([(wallStart[0], wallStart[1]),
(wallStop[0], wallStop[1])]))
for line in lines:
self.obstacles.append(line.buffer(self.obstacleInflation))
self.walls.append(line.buffer(self.wall_inflation))
tm_array = MarkerArray()
#Creating marker for displaying airspace boundaries
airspaceStart = self.mapInfo["airspace"]["min"]
airspaceStop = self.mapInfo["airspace"]["max"]
tm = Marker()
tm.header.stamp = rospy.Time.now()
tm.header.frame_id = "map"
tm.id = 90
tm.type = tm.CUBE
tm.action = tm.ADD
tm.pose.position.x = (airspaceStart[0] + airspaceStop[0]) / 2
tm.pose.position.y = (airspaceStart[1] + airspaceStop[1]) / 2
tm.pose.position.z = 0.01
(tm.pose.orientation.x, tm.pose.orientation.y, tm.pose.orientation.z,
tm.pose.orientation.w) = quaternion_from_euler(0, 0, 0)
tm.scale.x = abs(airspaceStart[0]) + abs(airspaceStop[0])
tm.scale.y = abs(airspaceStart[1]) + abs(airspaceStop[1])
tm.scale.z = 0.02
tm.color = ColorRGBA(1, 1, 1, 0.3)
tm_array.markers.append(tm)
for i, ii in enumerate(lines):
#Creating marker for displaying wall
tm = Marker()
wall_angle = math.atan2(
(list(ii.coords)[0][1] - list(ii.coords)[1][1]),
(list(ii.coords)[0][0] - list(ii.coords)[1][0]))
tm.header.stamp = rospy.Time.now()
tm.header.frame_id = "map"
tm.id = 100 + i * 5
tm.type = tm.CUBE
tm.action = tm.ADD
tm.pose.position.x = (list(ii.coords)[0][0] +
list(ii.coords)[1][0]) / 2
tm.pose.position.y = (list(ii.coords)[0][1] +
list(ii.coords)[1][1]) / 2
tm.pose.position.z = 1.5
(tm.pose.orientation.x, tm.pose.orientation.y,
tm.pose.orientation.z,
tm.pose.orientation.w) = quaternion_from_euler(0, 0, wall_angle)
tm.scale.x = math.sqrt(
(list(ii.coords)[0][0] - list(ii.coords)[1][0])**2 +
(list(ii.coords)[0][1] - list(ii.coords)[1][1])**2)
tm.scale.y = 0.02
tm.scale.z = 3
tm.color = ColorRGBA(1, .5, .1, 1)
tm_array.markers.append(tm)
#Creating markers for displaying inflated wall
tm = Marker()
wall_angle = math.atan2(
(list(ii.coords)[0][1] - list(ii.coords)[1][1]),
(list(ii.coords)[0][0] - list(ii.coords)[1][0]))
tm.header.stamp = rospy.Time.now()
tm.header.frame_id = "map"
tm.id = 101 + i * 5
tm.type = tm.CUBE
tm.action = tm.ADD
tm.pose.position.x = (list(ii.coords)[0][0] +
list(ii.coords)[1][0]) / 2
tm.pose.position.y = (list(ii.coords)[0][1] +
list(ii.coords)[1][1]) / 2
tm.pose.position.z = 1.5
(tm.pose.orientation.x, tm.pose.orientation.y,
tm.pose.orientation.z,
tm.pose.orientation.w) = quaternion_from_euler(0, 0, wall_angle)
tm.scale.x = math.sqrt(
(list(ii.coords)[0][0] - list(ii.coords)[1][0])**2 +
(list(ii.coords)[0][1] - list(ii.coords)[1][1])**2)
tm.scale.y = self.obstacleInflation * 2
tm.scale.z = 3
tm.color = ColorRGBA(1, .5, .1, 0.2)
tm_array.markers.append(tm)
tm = Marker()
wall_angle = math.atan2(
(list(ii.coords)[0][1] - list(ii.coords)[1][1]),
(list(ii.coords)[0][0] - list(ii.coords)[1][0]))
tm.header.stamp = rospy.Time.now()
tm.header.frame_id = "map"
tm.id = 102 + i * 5
tm.type = tm.CYLINDER
tm.action = tm.ADD
tm.pose.position.x = list(ii.coords)[0][0]
tm.pose.position.y = list(ii.coords)[0][1]
tm.pose.position.z = 1.5
(tm.pose.orientation.x, tm.pose.orientation.y,
tm.pose.orientation.z,
tm.pose.orientation.w) = quaternion_from_euler(0, 0, wall_angle)
tm.scale.x = self.obstacleInflation * 2
tm.scale.y = self.obstacleInflation * 2
tm.scale.z = 3
tm.color = ColorRGBA(1, .5, .1, 0.2)
tm_array.markers.append(tm)
tm = Marker()
wall_angle = math.atan2(
(list(ii.coords)[0][1] - list(ii.coords)[1][1]),
(list(ii.coords)[0][0] - list(ii.coords)[1][0]))
tm.header.stamp = rospy.Time.now()
tm.header.frame_id = "map"
tm.id = 103 + i * 5
tm.type = tm.CYLINDER
tm.action = tm.ADD
tm.pose.position.x = list(ii.coords)[1][0]
tm.pose.position.y = list(ii.coords)[1][1]
tm.pose.position.z = 1.5
(tm.pose.orientation.x, tm.pose.orientation.y,
tm.pose.orientation.z,
tm.pose.orientation.w) = quaternion_from_euler(0, 0, wall_angle)
tm.scale.x = self.obstacleInflation * 2
tm.scale.y = self.obstacleInflation * 2
tm.scale.z = 3
tm.color = ColorRGBA(1, .5, .1, 0.2)
tm_array.markers.append(tm)
rospy.sleep(.5)
self.markerPub.publish(tm_array)
## Creating Arena with obstacles
boundary = Marker()
boundary.header.frame_id = "map" # This is to pair the frames with Rviz
boundary.type = Marker.LINE_STRIP
boundary.action = Marker.ADD
boundary.scale.x = 0.2
boundary.color.a = 1.0
boundary.color.r = 1.0
boundary.color.g = 1.0
boundary.color.b = 0.0
boundary.ns = "Obstacles"
boundary.id = 0
#boundary.points = []
boundary.points.append(Point(-width / 2, -width / 2, 0))
boundary.points.append(Point(width / 2, -width / 2, 0))
boundary.points.append(Point(width / 2, width / 2, 0))
boundary.points.append(Point(-width / 2, width / 2, 0))
boundary.points.append(Point(-width / 2, -width / 2, 0))
markerArray.markers.append(boundary)
###################################################
## Creating Obstacles
for i in range(4):
obst = Marker()
obst.type = Marker.CUBE
listener = tf.TransformListener()
mpub = rospy.Publisher("/graph/viz", MarkerArray, latch=True, queue_size=1)
rospy.sleep(1.0)
ma = MarkerArray()
now = rospy.Time.now()
print get_pkg_dir('floor_graph') + "/graph2d.picklez"
g = ig.Graph.Read_Picklez(get_pkg_dir('floor_graph') + "/graph2d.picklez.gz")
id = 0
for v in g.vs:
marker = Marker()
marker.header.stamp = now
marker.header.frame_id = "/world"
marker.ns = "graph_nodes"
marker.id = id
id += 1
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.pose.position.x = v["x"]
marker.pose.position.y = v["y"]
marker.pose.position.z = -0.05
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.01
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
ma.markers.append(marker)
for e in g.es:
2.53591989399e-05, 0.707060385227, 0.707153172752
]
manipPose1 = [
0.441024004555, -0.0157218288562, 0.402612554019, 0.527145535299,
0.477387854476, 0.486045011226, 0.50791600494
]
manipPose2 = [0.01, -0.3, 0.81, 0, 0, 0, 0.707]
pub_line_min_dist = rospy.Publisher('visualization_marker',
Marker,
queue_size=1)
marker = Marker()
marker.header.frame_id = "/chassis"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.id = 0
# marker scale
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
# marker color
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 1.0
# marker orientaitn
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
def main():
# Load File
folder_path = '/home/dank-engine/3d_ws/json2_merger/'
path = list(pathlib.Path(folder_path).glob('*.json'))
id = 0
k = 0
num = 20
arr_rx = np.zeros(num)
arr_ry = np.zeros(num)
arr_lx = np.zeros(num)
arr_ly = np.zeros(num)
arr_cx = np.zeros(num)
arr_cy = np.zeros(num)
for i in range(100, 552):
filename = folder_path + '{i:0{width}}'.format(i=i + 1,
width=4) + '.json'
with open(filename, 'r') as f:
print(filename)
data = json.load(f)
# GPS Topic
gps_coor = GeoPointStamped()
gps_map = NavSatFix()
gps_pub = rospy.Publisher('/gps', GeoPointStamped, queue_size=10)
gps_map_pub = rospy.Publisher('/gps/fix', NavSatFix, queue_size=10)
#IMU Topic
imu_coor = Imu()
imu_pub = rospy.Publisher('/imu', Imu, queue_size=10)
#PointCloud Topic
cloud = PointCloud2()
pc_data = rospy.Publisher('/velodyne_points',
PointCloud2,
queue_size=32)
rospy.init_node('converter')
# Publish GPS Data in ROS
gps_coor.header.frame_id = "gps"
gps_coor.position.latitude = data["INS_SWIFT"]["Latitude"]
gps_coor.position.longitude = data["INS_SWIFT"]["Longitude"]
gps_coor.position.altitude = float('nan')
gps_map.header.frame_id = "base_link"
gps_map.latitude = data["INS_SWIFT"]["Latitude"]
gps_map.longitude = data["INS_SWIFT"]["Longitude"]
gps_map.altitude = data["INS_SWIFT"]["Altitude"]
gps_pub.publish(gps_coor)
gps_map_pub.publish(gps_map)
# Publish IMU Data in ROS
roll = data["INS_SWIFT"]["Roll"]
pitch = data["INS_SWIFT"]["Pitch"]
yaw = data["INS_SWIFT"]["Yaw"]
quaternion = tf.transformations.quaternion_from_euler(
roll, pitch, yaw)
imu_coor.header.frame_id = "imu"
imu_coor.header.stamp = rospy.get_rostime()
imu_coor.orientation.x = quaternion[0]
imu_coor.orientation.y = quaternion[1]
imu_coor.orientation.z = quaternion[2]
imu_coor.orientation.w = quaternion[3]
imu_coor.orientation_covariance = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
imu_pub.publish(imu_coor)
# Publish Point Cloud Data
point = []
scan = data["PointCloud"]
for i in range(len(scan)):
if (scan[i]["x"] != None):
xyzi = (scan[i]["x"], scan[i]["y"], scan[i]["z"],
scan[i]["intensity"])
point.append(xyzi)
else:
xyzi = (np.nan, np.nan, np.nan, 0)
point.append(xyzi)
cloud = xyzi_array_to_pointcloud2(point, rospy.get_rostime(),
"velodyne")
pc_data.publish(cloud)
lane_mark = rospy.Publisher('/marker_lane', Marker, queue_size=100)
marker_l = Marker()
marker_l.header.frame_id = "base_link"
marker_l.type = marker_l.TEXT_VIEW_FACING
marker_l.action = marker_l.ADD
marker_l.scale.x = 5
marker_l.scale.y = 5
marker_l.scale.z = 5
marker_l.color.a = 1.0
marker_l.color.r = 1.0
marker_l.color.g = 1.0
marker_l.color.b = 0.0
marker_l.pose.position.x = 0
marker_l.pose.position.y = 30
marker_l.pose.position.z = 1
marker_l.pose.orientation.w = 1.0
marker_l.text = "Lanes: " + str(
data["Lanes"]) + '\n' + "Width: " + str(
data["Width"]) + '\n' + "Slope_Hoizontal: " + str(
data["Slope"]["Slope_Hoizontal"])
marker_l.id = 0
lane_mark.publish(marker_l)
try:
r = rospy.Rate(1) # 10hz
lane_r = data["Right_Marker"]
lane_l = data["Left_Marker"]
lane_c = data["Center_Marker"]
publisher = rospy.Publisher('/drive_region',
MarkerArray,
queue_size=100)
arr_rx[k % num] = lane_r["x"]
arr_ry[k % num] = lane_r["y"]
arr_lx[k % num] = lane_l["x"]
arr_ly[k % num] = lane_l["y"]
arr_cx[k % num] = lane_c["x"]
arr_cy[k % num] = lane_c["y"]
# if k == 10:
markerArray = MarkerArray()
marker = Marker()
marker.header.frame_id = "base_link"
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 2
marker.scale.y = 2
marker.scale.z = 2
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = np.average(arr_rx)
marker.pose.position.y = np.average(arr_ry)
marker.pose.position.z = 0
marker1 = Marker()
marker1.header.frame_id = "base_link"
marker1.type = marker.SPHERE
marker1.action = marker.ADD
marker1.scale.x = 2
marker1.scale.y = 2
marker1.scale.z = 2
marker1.color.a = 1.0
marker1.color.r = 1.0
marker1.color.g = 1.0
marker1.color.b = 0.0
marker1.pose.orientation.w = 1.0
marker1.pose.position.x = np.average(arr_lx)
marker1.pose.position.y = np.average(arr_ly)
marker1.pose.position.z = 0
marker2 = Marker()
marker2.header.frame_id = "base_link"
marker2.type = marker.SPHERE
marker2.action = marker.ADD
marker2.scale.x = 2
marker2.scale.y = 2
marker2.scale.z = 2
marker2.color.a = 1.0
marker2.color.r = 0.0
marker2.color.g = 0.0
marker2.color.b = 1.0
marker2.pose.orientation.w = 1.0
marker2.pose.position.x = np.average(arr_cx)
marker2.pose.position.y = np.average(arr_cy)
marker2.pose.position.z = 0
markerArray.markers.append(marker)
markerArray.markers.append(marker1)
markerArray.markers.append(marker2)
# Renumber the marker IDs
for m in markerArray.markers:
m.id = id
id += 1
# Publish the MarkerArray
publisher.publish(markerArray)
# k = 0
k = k + 1
r.sleep()
except KeyError:
r = rospy.Rate(1) # 10hz
r.sleep()
def marker_calc(copterPos, force, namespace):
markerArray = MarkerArray()
#marker for copter
marker = Marker()
marker.header.frame_id = "/neck"
marker.ns = namespace
marker.type = marker.SPHERE
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.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = copterPos.x
marker.pose.position.y = copterPos.y
marker.pose.position.z = 0.0
markerArray.markers.append(marker)
#marker for force
markerArrow = Marker()
markerArrow.header.frame_id = "/neck"
markerArrow.ns = namespace
markerArrow.type = marker.ARROW
markerArrow.action = marker.ADD
markerArrow.id = 1
#marker.scale.x = 1
#Scaling the force magnitude to the width of the arrow by scaling in y
#This can be used if the arrow has no start and end point
F = math.sqrt((force[0] * force[0]) + (force[1] * force[1]))
markerArrow.scale.x = 0.1 #* abs (F) #length
markerArrow.scale.y = 0.2 # * abs (F) #width
markerArrow.scale.z = 0.2 #height
markerArrow.color.a = 1.0
markerArrow.color.r = 1.0
markerArrow.color.g = 1.0
markerArrow.color.b = 0.0
#Setting marker position
#markerArrow.pose.orientation.w = - 0.5
#markerArrow.pose.orientation.x = 0.0
#markerArrow.pose.orientation.y = 0.0
#markerArrow.pose.orientation.z = 0.0
#markerArrow.pose.position.x = copterPos.x
#markerArrow.pose.position.y = copterPos.y
#markerArrow.pose.position.z = 0.1
start = geometry_msgs.msg.Point()
start.x = copterPos.x
start.y = copterPos.y
start.z = 0.0
end = geometry_msgs.msg.Point()
end.x = copterPos.x + (force[0])
end.y = copterPos.y + (force[1])
end.z = 0.0
markerArrow.points.append(start)
markerArrow.points.append(end)
markerArray.markers.append(markerArrow)
publisher.publish(markerArray)
def node():
r1=1
r2=1
global mapData
rospy.Subscriber("/map", OccupancyGrid, mapCallBack)
pub = rospy.Publisher('shapes', Marker, queue_size=10)
rospy.init_node('RRTexplorer', anonymous=False)
#Actionlib client
client1 = actionlib.SimpleActionClient('/move_base', MoveBaseAction)
client1.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.stamp=rospy.Time.now()
goal.target_pose.header.frame_id="map"
goal.target_pose.pose.position.x=1
goal.target_pose.pose.position.y=0
goal.target_pose.pose.position.z=0
goal.target_pose.pose.orientation.w = 1.0
client1.send_goal(goal)
h=client1.get_state()
print h,'\n ------',rospy.Time.now(),'------ \n'
client1.wait_for_result()
client1.get_result()
rate = rospy.Rate(50)
listener = tf.TransformListener()
listener.waitForTransform('map', 'base_link', rospy.Time(0),rospy.Duration(10.0))
try:
(trans,rot) = listener.lookupTransform('map', 'base_link', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
trans=[0,0]
xinx=trans[0]
xiny=trans[1]
x_init=array([xinx,xiny])
V=array([x_init])
i=1.0
E=concatenate((x_init,x_init))
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 =1
points.type = Marker.POINTS
line.type=Marker.LINE_LIST
#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.05;
line.scale.y= 0.02;
points.scale.y=0.05;
line.color.r =9.0/255.0
line.color.g= 91.0/255.0
line.color.b =236.0/255.0
points.color.r = 255.0/255.0
points.color.g = 244.0/255.0
points.color.b = 0.0/255.0
points.color.a=1;
line.color.a = 0.6;
points.lifetime =line.lifetime = rospy.Duration();
p=Point()
p.x = x_init[0] ;
p.y = x_init[0] ;
p.z = 0;
pp=[]
pl=[]
pp.append(copy(p))
xdim=mapData.info.width
ydim=mapData.info.height
resolution=mapData.info.resolution
Xstartx=mapData.info.origin.position.x
Xstarty=mapData.info.origin.position.y
#raw_input('Press Enter to start exploration')
#-------------------------------RRT------------------------------------------
while not rospy.is_shutdown():
# Sample free
#indxRand= floor( len(mapData.data)*random())
#yr=ceil(indxRand/xdim)
#xr=indxRand-(floor(indxRand/xdim))*xdim
#xr=xr*resolution+Xstartx
#yr=yr*resolution+Xstarty
xr=(random()*20.0)-10.0
yr=(random()*20.0)-10.0
x_rand = array([xr,yr])
# Nearest
x_nearest=V[Nearest(V,x_rand),:]
# Steer
x_new=Steer(x_nearest,x_rand,param.eta)
goal.target_pose.pose.position.x=x_new[0]
goal.target_pose.pose.position.y=x_new[1]
goal.target_pose.pose.orientation.w = 1.0
# unKnow discovery
if gridValue(mapData,x_new)==-1:
assigner1(goal,x_new,client1,listener)
# ObstacleFree
if ObstacleFree(x_nearest,x_new,mapData):
# Near function
X_near=Near(V,x_new,param.rneighb)
s_Xnear=X_near.shape[0]
if All(X_near==array([0])):
s_Xnear=-1
V=vstack((V,x_new))
xmin=x_nearest
cmin=Cost(E,x_nearest)+LA.norm(x_new-x_nearest)
ii=0
for ii in range(0,s_Xnear):
xnear=copy(X_near[ii,:])
if ObstacleFree(xnear,x_new,mapData) and ( Cost(E,xnear)+LA.norm(xnear-x_new) )<cmin:
xmin=copy(xnear)
cmin=Cost(E,xnear)+LA.norm(xnear-x_new)
temp=concatenate((xmin,x_new))
E=vstack((E,temp))
iii=0
for iii in range(0,s_Xnear):
xnear=copy(X_near[iii,:])
if ObstacleFree(xnear,x_new,mapData) and ( Cost(E,x_new)+LA.norm(xnear-x_new) )<Cost(E,xnear):
row=Find(E,xnear)
E=delete(E, (row), axis=0)
temp=concatenate((x_new,xnear))
E=vstack((E,temp))
#Plotting
pl=prepEdges(E)
p.x=x_new[0]
p.y=x_new[1]
pp.append(copy(p))
points.points=pp
line.points=pl
pub.publish(points)
pub.publish(line)
rate.sleep()
def _make_lane_markings(clockwise=True, lane_index=-1):
_lane_marking = Marker()
_lane_marking.header.frame_id = "/map"
_lane_marking.ns = 'lane'
if lane_index == -1:
_lane_marking.id = 0 # center lane
_deviation = 0
elif clockwise:
_lane_marking.id = 1 + lane_index
_deviation = -_lane_marking.id * self._lane_width
else:
_lane_marking.id = 1 + self._lane_num_clockwise + lane_index
_deviation = (1 + lane_index) * self._lane_width
_lane_marking.type = Marker.LINE_STRIP
_lane_marking.action = Marker.ADD
_lane_marking.pose = convert_to_message(
tf.transformations.translation_matrix((0, 0, 0)))
_lane_marking.scale.x = 0.3 # lane marking width
_lane_marking.color.r = 1.0
_lane_marking.color.g = 1.0
if lane_index == -1:
_lane_marking.color.b = 0.0
else:
_lane_marking.color.b = 1.0
_lane_marking.color.a = 1.0
lm_init_position = (0, _deviation, 0)
lm_init_point = geometry_msgs.msg.Point(*lm_init_position)
curve_radius = self._radius_curve_track + _deviation
lm_length = 2 * self._length_straight_track + 2 * math.pi * curve_radius
lm_discrete_num = int(lm_length / s) + 1
print(lm_discrete_num)
lm_landmark1 = int(self._length_straight_track / s)
lm_landmark2 = int(
(self._length_straight_track + math.pi * curve_radius) / s)
lm_landmark3 = int(
(2 * self._length_straight_track + math.pi * curve_radius) / s)
theta = s / curve_radius
for i in range(lm_discrete_num - 1):
if i < lm_landmark1:
pos = (i * s, _deviation, 0)
elif i < lm_landmark2:
pos = (self._length_straight_track + math.sin((i - lm_landmark1) * theta) \
* curve_radius, _deviation + math.cos((i - lm_landmark1) * theta) * curve_radius - curve_radius, 0)
elif i < lm_landmark3:
pos = (self._length_straight_track -
(i - lm_landmark2) * s,
_deviation - 2 * curve_radius, 0)
else:
pos = (-math.sin((i - lm_landmark3) * theta) \
* curve_radius, _deviation - math.cos((i - lm_landmark3) * theta) * curve_radius - curve_radius, 0)
_lane_marking.points.append(geometry_msgs.msg.Point(*pos))
_lane_marking.points.append(lm_init_point)
return _lane_marking
def publish_marker_callback(self, _):
# publish checkpoints lines marker
checkpoints_marker = Marker()
checkpoints_marker.header.frame_id = "map"
checkpoints_marker.header.stamp = rospy.Time.now()
checkpoints_marker.ns = "checkpoints"
checkpoints_marker.type = Marker.LINE_LIST
checkpoints_marker.action = Marker.ADD
checkpoints_marker.pose.orientation.w = 1
checkpoints_marker.scale.x = 0.05
checkpoints_marker.color.r = 0.85
checkpoints_marker.color.g = 0.85
checkpoints_marker.color.b = 1.0
checkpoints_marker.color.a = 1.0
next_checkpoint_marker = deepcopy(checkpoints_marker)
next_checkpoint_marker.id = 1
next_checkpoint_marker.color.r = 0.0
next_checkpoint_marker.color.g = 0.0
next_checkpoint_marker.color.b = 1.0
reached_checkpoints_marker = deepcopy(checkpoints_marker)
reached_checkpoints_marker.id = 2
reached_checkpoints_marker.color.r = 0.0
reached_checkpoints_marker.color.g = 1.0
reached_checkpoints_marker.color.b = 0.0
marker_array = MarkerArray()
for checkpoint_index, checkpoint_segment in enumerate(
self.checkpoints):
text_checkpoints_marker = Marker()
text_checkpoints_marker.header.frame_id = "map"
text_checkpoints_marker.header.stamp = rospy.Time.now()
text_checkpoints_marker.ns = "checkpoints"
text_checkpoints_marker.action = Marker.ADD
text_checkpoints_marker.id = checkpoint_index + 3
text_checkpoints_marker.type = Marker.TEXT_VIEW_FACING
text_checkpoints_marker.color.r = 1.0
text_checkpoints_marker.color.g = 1.0
text_checkpoints_marker.color.b = 1.0
text_checkpoints_marker.color.a = 1.0
text_checkpoints_marker.text = str(checkpoint_index + 1)
text_checkpoints_marker.scale.z = 0.3
text_checkpoints_marker.pose.position.x = (
checkpoint_segment[0][0] + checkpoint_segment[1][0]) / 2
text_checkpoints_marker.pose.position.y = (
checkpoint_segment[0][1] + checkpoint_segment[1][1]) / 2
text_checkpoints_marker.pose.orientation.w = 1
marker_array.markers.append(text_checkpoints_marker)
if checkpoint_index == self.next_checkpoint_index:
for point in checkpoint_segment:
next_checkpoint_marker.points.append(
Point(x=point[0], y=point[1]))
if checkpoint_index < self.next_checkpoint_index:
for point in checkpoint_segment:
reached_checkpoints_marker.points.append(
Point(x=point[0], y=point[1]))
if checkpoint_index > self.next_checkpoint_index:
for point in checkpoint_segment:
checkpoints_marker.points.append(
Point(x=point[0], y=point[1]))
if len(checkpoints_marker.points):
marker_array.markers.append(checkpoints_marker)
if len(next_checkpoint_marker.points):
marker_array.markers.append(next_checkpoint_marker)
if len(reached_checkpoints_marker.points):
marker_array.markers.append(reached_checkpoints_marker)
self.vis_pub.publish(marker_array)
current_pose = PoseStamped()
def pose_local_cb(pose):
global current_pose
current_pose = pose
points = Marker()
line_strip = Marker()
landmark = Marker()
points.header.frame_id = line_strip.header.frame_id = landmark.header.frame_id = "map"
#points.header.stamp = line_strip.header.stamp = landmark.header.stamp = rospy.Time.now()
points.ns = line_strip.ns = landmark.ns = "points_and_lines"
points.action = line_strip.action = landmark.action = Marker.ADD
points.id = 0
line_strip.id = 1
landmark.id = 2
points.type = Marker.SPHERE_LIST
line_strip.type = Marker.LINE_STRIP
landmark.type = Marker.POINTS
points.pose.orientation.w = line_strip.pose.orientation.w = landmark.pose.orientation.w = 1.0
# POINTS markers use x and y scale for width/height respectively
points.scale.x = 0.05 # 0.05, 0.1
points.scale.y = 0.05
points.scale.z = 0.05
line_strip.scale.x = 0.03
line_strip.scale.x = 0.03
line_strip.scale.x = 0.03
def on_new_point_cloud(data):
pc = pointcloud2_to_array(data)
points = np.zeros((pc.shape[0], 4), dtype=np.float32)
for i in range(pc.shape[0]):
points[i][0] = pc[i][0]
points[i][1] = pc[i][1]
points[i][2] = pc[i][2] - offset
points[i][3] = pc[i][3] / 255
#points[i][3] = random.random()
box_preds_lidar, labels, scores = pointp_based_detection(
'/home/these/second.pytorch/path/for_ped/model_dir', params.net,
params.input_cfg, params.model_cfg, params.train_cfg,
params.class_names, params.voxel_generator, params.target_assigner,
points, None, True, None, None, True)
# box_preds_lidar,labels,scores =pointp_based_detection('/home/these/catkin_ws/src/pointpillars/src/path/to/model_dir',
# params.net,params.input_cfg,params.model_cfg,params.train_cfg,params.class_names,params.voxel_generator,params.target_assigner,
# points,
# None,
# True,
# None,
# None,
# True
# )
#visualization####
topic = '/pointpillars/visualization/marker_box'
marker_publisher = rospy.Publisher(topic, MarkerArray)
markerArray = MarkerArray()
del_marker = Marker()
del_marker.action = del_marker.DELETEALL
del_marker.header.frame_id = "visualization"
del_marker.id = 1
markerArray.markers.append(del_marker)
for i in range(len(box_preds_lidar)):
print(scores[i])
if scores[i] > 0.3:
# result_string = str(labels[i])+','+str(scores[i])+','+str(box_preds_lidar[i][0])+ ','+ str(box_preds_lidar[i][1]) + ','+ str(math.sqrt(box_preds_lidar[i][1]*box_preds_lidar[i][1]+ box_preds_lidar[i][0]*box_preds_lidar[i][0])) +','+ str(box_preds_lidar[i][6])+'\n'
# with open('20191121result_file.csv',mode ='a') as result_file:
# print(result_string)
# result_file.write(result_string)
marker = Marker()
marker.header.frame_id = "visualization"
marker.type = marker.CUBE
marker.action = marker.ADD
marker.scale.x = box_preds_lidar[i][3]
marker.scale.y = box_preds_lidar[i][4]
marker.scale.z = box_preds_lidar[i][5]
marker.color.a = 0.3
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = math.sin(-box_preds_lidar[i][6] / 2)
marker.pose.orientation.w = math.cos(-box_preds_lidar[i][6] / 2)
marker.pose.position.x = box_preds_lidar[i][0]
marker.pose.position.y = box_preds_lidar[i][1]
marker.pose.position.z = box_preds_lidar[i][
2] + box_preds_lidar[i][5] / 2.0 + offset
marker.id = 10 + i
marker.text = 'Car'
marker.lifetime = rospy.Duration()
markerArray.markers.append(marker)
marker_publisher.publish(markerArray)
topic = '/pointpillars/visualization/points'
data.header.frame_id = "visualization"
points_publisher = rospy.Publisher(topic, PointCloud2)
points_publisher.publish(data)
boundary = Marker()
boundary.header.frame_id = "visualization"
boundary.type = boundary.LINE_STRIP
boundary.action = boundary.ADD
boundary.scale.x = 0.5
boundary.color.a = 1.0
boundary.color.r = 1.0
boundary.color.g = 0.0
boundary.color.b = 0.0
boundary.pose.orientation.x = 0
boundary.pose.orientation.y = 0
boundary.pose.orientation.z = 0
boundary.pose.orientation.w = 1
corners = [0, -19.84, -2.5, 47.36, 19.84, 0.5]
p1 = geometry_msgs.msg.Point(corners[0], corners[1], 0.)
boundary.points.append(p1)
p2 = geometry_msgs.msg.Point(corners[0], corners[4], 0.)
boundary.points.append(p2)
p3 = geometry_msgs.msg.Point(corners[3], corners[4], 0.)
boundary.points.append(p3)
p4 = geometry_msgs.msg.Point(corners[3], corners[1], 0.)
boundary.points.append(p4)
boundary.points.append(p1)
boundary.id = 0
boundary.lifetime = rospy.Duration()
topic = '/pointpillars/visualization/boundary'
boundary_publisher = rospy.Publisher(topic, Marker)
boundary_publisher.publish(boundary)
def publish_markers(self):
offset_main_psi = -np.pi*0.70
offset_steer_psi = -np.pi*0.70
# create main frame marker
marker_main_frame = Marker()
marker_main_frame.header.frame_id = self.tf_steering_frame_name
marker_main_frame.header.stamp = rospy.Time.now()
marker_main_frame.id = 1
marker_main_frame.action = Marker.ADD
marker_main_frame.type = Marker.MESH_RESOURCE
marker_main_frame.mesh_resource = self.model_main_frame
marker_main_frame.pose.position.x = 0.0
marker_main_frame.pose.position.y = 0.0
marker_main_frame.pose.position.z = 0.0
roll = 0.0
pitch = -self.bicycle_state.lean_phi
yaw = self.bicycle_state.heading_psi + offset_main_psi
quaternion = quaternion_from_euler(roll, pitch, yaw)
marker_main_frame.pose.orientation.x = quaternion[0]
marker_main_frame.pose.orientation.y = quaternion[1]
marker_main_frame.pose.orientation.z = quaternion[2]
marker_main_frame.pose.orientation.w = quaternion[3]
marker_main_frame.scale.x = 1.0
marker_main_frame.scale.y = 1.0
marker_main_frame.scale.z = 1.0
marker_main_frame.color.a = self.alpha
marker_main_frame.color.r = 1.0
marker_main_frame.color.g = 0.5
marker_main_frame.color.b = 0.0
self.pub_marker_main.publish(marker_main_frame)
# create steering frame marker
maker_steering_frame = Marker()
maker_steering_frame.header.frame_id = self.tf_steering_frame_name
maker_steering_frame.header.stamp = rospy.Time.now()
maker_steering_frame.id = 1
maker_steering_frame.action = Marker.ADD
maker_steering_frame.type = Marker.MESH_RESOURCE
maker_steering_frame.mesh_resource = self.model_steer_frame
maker_steering_frame.scale.x = 1.0
maker_steering_frame.scale.y = 1.0
maker_steering_frame.scale.z = 1.0
maker_steering_frame.pose.position.x = 0.0
maker_steering_frame.pose.position.y = 0.0
maker_steering_frame.pose.position.z = 0.0
roll = 0.0
pitch = -self.bicycle_state.lean_phi
yaw = offset_steer_psi + self.bicycle_state.heading_psi + self.bicycle_state.steering_delta
quaternion = quaternion_from_euler(roll, pitch, yaw)
maker_steering_frame.pose.orientation.x = quaternion[0]
maker_steering_frame.pose.orientation.y = quaternion[1]
maker_steering_frame.pose.orientation.z = quaternion[2]
maker_steering_frame.pose.orientation.w = quaternion[3]
maker_steering_frame.color.a = self.alpha
maker_steering_frame.color.r = 0.8
maker_steering_frame.color.g = 0.4
maker_steering_frame.color.b = 0.0
self.pub_marker_steering.publish(maker_steering_frame)
def publish_tracked_people(self, now):
"""
Publish markers of tracked people to Rviz and to <people_tracked> topic
"""
people_tracked_msg = PersonArray()
people_tracked_msg.header.stamp = now
people_tracked_msg.header.frame_id = self.publish_people_frame
marker_id = 0
# Make sure we can get the required transform first:
if self.use_scan_header_stamp_for_tfs:
tf_time = now
try:
self.listener.waitForTransform(self.publish_people_frame,
self.fixed_frame, tf_time,
rospy.Duration(1.0))
transform_available = True
except:
transform_available = False
else:
tf_time = rospy.Time(0)
transform_available = self.listener.canTransform(
self.publish_people_frame, self.fixed_frame, tf_time)
marker_id = 0
if not transform_available:
rospy.loginfo(
"Person tracker: tf not avaiable. Not publishing people")
else:
# Publish tracked people to /people_tracked topic and to rviz
for person in self.people_tracked:
leg_1 = person.leg_1
leg_2 = person.leg_2
if self.publish_occluded or leg_1.seen_in_current_scan or leg_2.seen_in_current_scan:
# Get person's position in the <self.publish_people_frame> frame
ps = PointStamped()
ps.header.frame_id = self.fixed_frame
ps.header.stamp = tf_time
ps.point.x = (leg_1.pos_x + leg_2.pos_x) / 2.
ps.point.y = (leg_1.pos_y + leg_2.pos_y) / 2.
try:
ps = self.listener.transformPoint(
self.publish_people_frame, ps)
except:
rospy.logerr(
"Not publishing people due to no transform from fixed_frame-->publish_people_frame"
)
continue
# publish to people_tracked topic
new_person = Person()
new_person.pose.position.x = ps.point.x
new_person.pose.position.y = ps.point.y
new_person.id = person.id_num
people_tracked_msg.people.append(new_person)
# publish rviz markers
marker = Marker()
marker.header.frame_id = self.publish_people_frame
marker.header.stamp = now
marker.ns = "People_tracked"
marker.color.r = person.colour[0]
marker.color.g = person.colour[1]
marker.color.b = person.colour[2]
marker.color.a = (
rospy.Duration(3) -
(rospy.get_rostime() - leg_1.last_seen)
).to_sec() / rospy.Duration(3).to_sec() + 0.1
marker.pose.position.x = ps.point.x
marker.pose.position.y = ps.point.y
for i in xrange(
2
): # publish two markers per person: one for body and one for head
marker.id = marker_id #person.id_num + 20000*i
marker_id += 1
if i == 0: # cylinder for body shape
marker.type = Marker.CYLINDER
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 1.2
marker.pose.position.z = 0.8
else: # sphere for head shape
marker.type = Marker.SPHERE
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.pose.position.z = 1.5
self.marker_pub.publish(marker)
# Text showing person's ID number
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
marker.id = marker_id
marker_id += 1
marker.type = Marker.TEXT_VIEW_FACING
marker.text = str(person.id_num)
marker.scale.z = 0.2
marker.pose.position.z = 1.7
self.marker_pub.publish(marker)
# Clear previously published people markers
for m_id in xrange(marker_id, self.prev_person_marker_id):
marker = Marker()
marker.header.stamp = now
marker.header.frame_id = self.publish_people_frame
marker.ns = "People_tracked"
marker.id = m_id
marker.action = marker.DELETE
self.marker_pub.publish(marker)
self.prev_person_marker_id = marker_id
# Publish people tracked message
self.people_tracked_pub.publish(people_tracked_msg)
def handle_periodic_publish(self, event):
req = ReadDataRequest()
req.perception_name = 'objects'
req.query = '{}'
req.data = ['~']
result = self.rd_srv(req)
if not result.result:
return
vis_msg = MarkerArray()
ret_data = json.loads(result.data)
for obj in ret_data:
br = tf2_ros.TransformBroadcaster()
transformed_msg = TransformStamped()
transformed_msg.header.stamp = rospy.Time.now()
transformed_msg.header.frame_id = obj['frame_id']
transformed_msg.child_frame_id = obj['name']
transformed_msg.transform.translation.x = obj['xyz'][0]
transformed_msg.transform.translation.y = obj['xyz'][1]
transformed_msg.transform.translation.z = obj['xyz'][2]
q = tf.transformations.quaternion_from_euler(
obj['rpy'][0], obj['rpy'][1], obj['rpy'][2])
transformed_msg.transform.rotation.x = q[0]
transformed_msg.transform.rotation.y = q[1]
transformed_msg.transform.rotation.z = q[2]
transformed_msg.transform.rotation.w = q[3]
br.sendTransform(transformed_msg)
marker = Marker()
marker.header.frame_id = obj['frame_id']
marker.header.stamp = rospy.Time.now()
marker.ns = obj['name']
marker.id = 0
obj_type = obj['geometry'].keys()[0]
if obj_type == 'box':
marker.type = Marker.CUBE
elif obj_type == 'cylinder':
marker.type = Marker.CYLINDER
elif obj_type == 'sphere':
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose.position.x = obj['xyz'][0]
marker.pose.position.y = obj['xyz'][1]
marker.pose.position.z = obj['xyz'][2]
orientation = tf.transformations.quaternion_from_euler(
obj['rpy'][0], obj['rpy'][1], obj['rpy'][2])
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
marker.scale.x = obj['geometry'][obj_type][0]
marker.scale.y = obj['geometry'][obj_type][1]
marker.scale.z = obj['geometry'][obj_type][2]
marker.color.r = obj['material']['color'][0]
marker.color.g = obj['material']['color'][1]
marker.color.b = obj['material']['color'][2]
marker.color.a = obj['material']['color'][3]
vis_msg.markers.append(marker)
self.pub_vis_msg.publish(vis_msg)
def create_delta_simulation():
print "Creating simulation"
markerArray = MarkerArray()
width = 0.005
# Point of first dynamixel
base1 = Marker()
base1.header.frame_id = "/map"
base1.type = base1.SPHERE
base1.action = base1.ADD
base1.scale.x = width
base1.scale.y = width
base1.scale.z = width
base1.color.a = 1.0
base1.color.r = 1.0
base1.color.g = 0.0
base1.color.b = 0.0
base1.pose.orientation.w = 1.0
base1.pose.position.x = 0
base1.pose.position.y = - 0.06 * sqrt(2)
base1.pose.position.z = 0
base1.id = 1
# Point of second dynamixel
base2 = Marker()
base2.header.frame_id = "/map"
base2.type = base2.SPHERE
base2.action = base2.ADD
base2.scale.x = width
base2.scale.y = width
base2.scale.z = width
base2.color.a = 1.0
base2.color.r = 0.0
base2.color.g = 1.0
base2.color.b = 0.0
base2.pose.orientation.w = 1.0
base2.pose.position.x = 0.06
base2.pose.position.y = 0.06
base2.pose.position.z = 0
base2.id = 2
# Point of third dynamixel
base3 = Marker()
base3.header.frame_id = "/map"
base3.type = base3.SPHERE
base3.action = base3.ADD
base3.scale.x = width
base3.scale.y = width
base3.scale.z = width
base3.color.a = 1.0
base3.color.r = 0.0
base3.color.g = 0.0
base3.color.b = 1.0
base3.pose.orientation.w = 1.0
base3.pose.position.x = - 0.06
base3.pose.position.y = 0.06
base3.pose.position.z = 0
base3.id = 3
# Arrow from first dynamixel to elbow
arrow1 = Marker()
arrow1.header.frame_id = "/map"
arrow1.type = arrow1.ARROW
arrow1.action = arrow1.ADD
arrow1.scale.x = width
arrow1.scale.y = width
arrow1.color.a = 1.0
arrow1.color.r = 1.0
arrow1.color.g = 0.0
arrow1.color.b = 0.0
arrow1.points = [None]*2
arrow1.points[0] = Point(base1.pose.position.x, base1.pose.position.y, 0.0)
arrow1.points[1] = Point(base1.pose.position.x, base1.pose.position.y, 0.08)
arrow1.pose.orientation.w = 1.0
arrow1.id = 4
# Arrow from second dynamixel to elbow
arrow2 = Marker()
arrow2.header.frame_id = "/map"
arrow2.type = arrow2.ARROW
arrow2.action = arrow2.ADD
arrow2.scale.x = width
arrow2.scale.y = width
arrow2.color.a = 1.0
arrow2.color.r = 0.0
arrow2.color.g = 1.0
arrow2.color.b = 0.0
arrow2.points = [None]*2
arrow2.points[0] = Point(base2.pose.position.x, base2.pose.position.y, 0.0)
arrow2.points[1] = Point(base2.pose.position.x, base2.pose.position.y, 0.08)
arrow2.pose.orientation.w = 1.0
arrow2.id = 5
# Arrow from third dynamixel to elbow
arrow3 = Marker()
arrow3.header.frame_id = "/map"
arrow3.type = arrow3.ARROW
arrow3.action = arrow3.ADD
arrow3.scale.x = width
arrow3.scale.y = width
arrow3.color.a = 1.0
arrow3.color.r = 0.0
arrow3.color.g = 0.0
arrow3.color.b = 1.0
arrow3.points = [None]*2
arrow3.points[0] = Point(base3.pose.position.x, base3.pose.position.y, 0.0)
arrow3.points[1] = Point(base3.pose.position.x, base3.pose.position.y, 0.08)
arrow3.pose.orientation.w = 1.0
arrow3.id = 6
# Arrow from elbow to final for first dynamixel
arrow4 = Marker()
arrow4.header.frame_id = "/map"
arrow4.type = arrow4.ARROW
arrow4.action = arrow4.ADD
arrow4.scale.x = width
arrow4.scale.y = width
arrow4.color.a = 1.0
arrow4.color.r = 1.0
arrow4.color.g = 0.0
arrow4.color.b = 0.0
arrow4.points = [None]*2
arrow4.points[0] = arrow1.points[1]
arrow4.points[1] = arrow1.points[1]
arrow4.points[1].z = arrow4.points[1].z + 0.2 # end to elbow dist
arrow4.pose.orientation.w = 1.0
arrow4.id = 7
# Arrow from second dynamixel to elbow
arrow5 = Marker()
arrow5.header.frame_id = "/map"
arrow5.type = arrow5.ARROW
arrow5.action = arrow5.ADD
arrow5.scale.x = width
arrow5.scale.y = width
arrow5.color.a = 1.0
arrow5.color.r = 0.0
arrow5.color.g = 1.0
arrow5.color.b = 0.0
arrow5.points = [None]*2
arrow5.points[0] = arrow2.points[1]
arrow5.points[1] = arrow2.points[1]
arrow5.points[1].z = arrow2.points[1].z + 0.2
arrow5.pose.orientation.w = 1.0
arrow5.id = 8
# Arrow from third dynamixel to elbow
arrow6 = Marker()
arrow6.header.frame_id = "/map"
arrow6.type = arrow6.ARROW
arrow6.action = arrow6.ADD
arrow6.scale.x = width
arrow6.scale.y = width
arrow6.color.a = 1.0
arrow6.color.r = 0.0
arrow6.color.g = 0.0
arrow6.color.b = 1.0
arrow6.points = [None]*2
arrow6.points[0] = arrow3.points[1]
arrow6.points[1] = arrow3.points[1]
arrow6.points[1].z = arrow3.points[1].z + 0.2
arrow6.pose.orientation.w = 1.0
arrow6.id = 9
floor = Marker()
floor.header.frame_id = "/map"
floor.type = floor.CUBE
floor.action = floor.ADD
floor.scale.x = .2
floor.scale.y = .2
floor.scale.z = width
floor.color.a = 1.0
# brown: 205-133-63
floor.color.r = 205.0 / 255.0
floor.color.g = 133.0 / 255.0
floor.color.b = 63.0 / 255.0
floor.pose.orientation.w = 1
floor.pose.position.x = 0
floor.pose.position.y = 0
floor.pose.position.z = -width
floor.id = 10
# Append marker
markerArray.markers.append(base1)
markerArray.markers.append(base2)
markerArray.markers.append(base3)
markerArray.markers.append(arrow1)
markerArray.markers.append(arrow2)
markerArray.markers.append(arrow3)
markerArray.markers.append(arrow4)
markerArray.markers.append(arrow5)
markerArray.markers.append(arrow6)
markerArray.markers.append(floor)
return markerArray
def publish_tracked_objects(self, now):
"""
Publish markers of tracked objects to Rviz
"""
# Make sure we can get the required transform first:
if self.use_scan_header_stamp_for_tfs:
tf_time = now
try:
self.listener.waitForTransform(self.publish_people_frame,
self.fixed_frame, tf_time,
rospy.Duration(1.0))
transform_available = True
except:
transform_available = False
else:
tf_time = rospy.Time(0)
transform_available = self.listener.canTransform(
self.publish_people_frame, self.fixed_frame, tf_time)
marker_id = 0
if not transform_available:
rospy.loginfo(
"Person tracker: tf not avaiable. Not publishing people")
else:
for track in self.objects_tracked:
if self.publish_occluded or track.seen_in_current_scan: # Only publish people who have been seen in current scan, unless we want to publish occluded people
# Get the track position in the <self.publish_people_frame> frame
ps = PointStamped()
ps.header.frame_id = self.fixed_frame
ps.header.stamp = tf_time
ps.point.x = track.pos_x
ps.point.y = track.pos_y
try:
ps = self.listener.transformPoint(
self.publish_people_frame, ps)
except:
continue
# publish rviz markers
marker = Marker()
marker.header.frame_id = self.publish_people_frame
marker.header.stamp = now
marker.ns = "objects_tracked"
marker.color.r = track.colour[0]
marker.color.g = track.colour[1]
marker.color.b = track.colour[2]
marker.color.a = 1
marker.pose.position.x = ps.point.x
marker.pose.position.y = ps.point.y
marker.id = marker_id
marker_id += 1
marker.type = Marker.CYLINDER
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.2
marker.pose.position.z = 0.15
self.marker_pub.publish(marker)
# # Publish a marker showing distance travelled:
# if track.dist_travelled > 1:
# marker.color.r = 1.0
# marker.color.g = 1.0
# marker.color.b = 1.0
# marker.color.a = 1.0
# marker.id = marker_id
# marker_id += 1
# marker.type = Marker.TEXT_VIEW_FACING
# marker.text = str(round(track.dist_travelled,1))
# marker.scale.z = 0.1
# marker.pose.position.z = 0.6
# self.marker_pub.publish(marker)
# # Publish <self.confidence_percentile>% confidence bounds of person as an ellipse:
# cov = track.filtered_state_covariances[0][0] + track.var_obs # cov_xx == cov_yy == cov
# std = cov**(1./2.)
# gate_dist_euclid = scipy.stats.norm.ppf(1.0 - (1.0-self.confidence_percentile)/2., 0, std)
# marker.type = Marker.SPHERE
# marker.scale.x = 2*gate_dist_euclid
# marker.scale.y = 2*gate_dist_euclid
# marker.scale.z = 0.01
# marker.color.r = track.colour[0]
# marker.color.g = track.colour[1]
# marker.color.b = track.colour[2]
# marker.color.a = 0.1
# marker.pose.position.z = 0.0
# marker.id = marker_id
# marker_id += 1
# self.marker_pub.publish(marker)
# Clear previously published track markers
for m_id in xrange(marker_id, self.prev_track_marker_id):
marker = Marker()
marker.header.stamp = now
marker.header.frame_id = self.publish_people_frame
marker.ns = "objects_tracked"
marker.id = m_id
marker.action = marker.DELETE
self.marker_pub.publish(marker)
self.prev_track_marker_id = marker_id
def setupVisualization(dataset, args, selected_collection_key):
"""
Creates the necessary variables in a dictionary "dataset_graphics", which will be passed onto the visualization
function
"""
# Create a python dictionary that will contain all the visualization related information
graphics = {
'collections': {},
'sensors': {},
'pattern': {},
'ros': {},
'args': args
}
# Initialize ROS stuff
rospy.init_node("calibrate")
graphics['ros']['tf_broadcaster'] = tf.TransformBroadcaster()
graphics['ros']['publisher_models'] = rospy.Publisher('~robot_meshes',
MarkerArray,
queue_size=0,
latch=True)
now = rospy.Time.now()
# Parse xacro description file
description_file, _, _ = uriReader(
dataset['calibration_config']['description_file'])
rospy.loginfo('Reading description file ' + description_file + '...')
xml_robot = readXacroFile(description_file)
pattern = dataset['calibration_config']['calibration_pattern']
# Create colormaps to be used for coloring the elements. Each collection contains a color, each sensor likewise.
graphics['pattern']['colormap'] = cm.gist_rainbow(
np.linspace(0, 1,
pattern['dimension']['x'] * pattern['dimension']['y']))
graphics['collections']['colormap'] = cm.tab20b(
np.linspace(0, 1, len(dataset['collections'].keys())))
for idx, collection_key in enumerate(sorted(
dataset['collections'].keys())):
graphics['collections'][str(collection_key)] = {
'color': graphics['collections']['colormap'][idx, :]
}
# color_map_sensors = cm.gist_rainbow(np.linspace(0, 1, len(dataset['sensors'].keys())))
# for idx, sensor_key in enumerate(sorted(dataset['sensors'].keys())):
# dataset['sensors'][str(sensor_key)]['color'] = color_map_sensors[idx, :]
# Create image publishers ----------------------------------------------------------
# We need to republish a new image at every visualization
for collection_key, collection in dataset['collections'].items():
for sensor_key, sensor in dataset['sensors'].items():
if not collection['labels'][str(sensor_key)][
'detected']: # not detected by sensor in collection
continue
if sensor['msg_type'] == 'Image':
msg_type = sensor_msgs.msg.Image
topic = dataset['calibration_config']['sensors'][sensor_key][
'topic_name']
topic_name = '~c' + str(collection_key) + topic + '/labeled'
graphics['collections'][collection_key][str(sensor_key)] = {
'publisher':
rospy.Publisher(topic_name,
msg_type,
queue_size=0,
latch=True)
}
msg_type = sensor_msgs.msg.CameraInfo
topic_name = '~c' + str(collection_key) + '/' + str(
sensor_key) + '/camera_info'
graphics['collections'][collection_key][str(sensor_key)]['publisher_camera_info'] = \
rospy.Publisher(topic_name, msg_type, queue_size=0, latch=True)
# Create Labeled Data publishers ----------------------------------------------------------
markers = MarkerArray()
for collection_key, collection in dataset['collections'].items():
for sensor_key, sensor in dataset['sensors'].items():
if not collection['labels'][str(sensor_key)][
'detected']: # not detected by sensor in collection
continue
if sensor[
'msg_type'] == 'LaserScan': # -------- Publish the laser scan data ------------------------------
frame_id = genCollectionPrefix(
collection_key,
collection['data'][sensor_key]['header']['frame_id'])
marker = Marker(
header=Header(frame_id=frame_id, stamp=now),
ns=str(collection_key) + '-' + str(sensor_key),
id=0,
frame_locked=True,
type=Marker.POINTS,
action=Marker.ADD,
lifetime=rospy.Duration(0),
pose=Pose(position=Point(x=0, y=0, z=0),
orientation=Quaternion(x=0, y=0, z=0, w=1)),
scale=Vector3(x=0.03, y=0.03, z=0),
color=ColorRGBA(
r=graphics['collections'][collection_key]['color'][0],
g=graphics['collections'][collection_key]['color'][1],
b=graphics['collections'][collection_key]['color'][2],
a=1.0))
# Get laser points that belong to the chessboard (labelled)
idxs = collection['labels'][sensor_key]['idxs']
rhos = [
collection['data'][sensor_key]['ranges'][idx]
for idx in idxs
]
thetas = [
collection['data'][sensor_key]['angle_min'] +
collection['data'][sensor_key]['angle_increment'] * idx
for idx in idxs
]
for idx, (rho, theta) in enumerate(zip(rhos, thetas)):
marker.points.append(
Point(x=rho * math.cos(theta),
y=rho * math.sin(theta),
z=0))
markers.markers.append(copy.deepcopy(marker))
# Draw extrema points
marker.ns = str(collection_key) + '-' + str(sensor_key)
marker.type = Marker.SPHERE_LIST
marker.id = 1
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 0.5
marker.points = [marker.points[0], marker.points[-1]]
markers.markers.append(copy.deepcopy(marker))
# Draw detected edges
marker.ns = str(collection_key) + '-' + str(sensor_key)
marker.type = Marker.CUBE_LIST
marker.id = 2
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.a = 0.5
marker.points = [] # Reset the list of marker points
for edge_idx in collection['labels'][sensor_key][
'edge_idxs']: # add edge points
p = Point()
p.x = rhos[edge_idx] * math.cos(thetas[edge_idx])
p.y = rhos[edge_idx] * math.sin(thetas[edge_idx])
p.z = 0
marker.points.append(p)
markers.markers.append(copy.deepcopy(marker))
if sensor[
'msg_type'] == 'PointCloud2': # -------- Publish the velodyne data ------------------------------
# Convert velodyne data on .json dictionary to ROS message type
cloud_msg = message_converter.convert_dictionary_to_ros_message(
"sensor_msgs/PointCloud2", collection['data'][sensor_key])
# Get LiDAR points that belong to the pattern
idxs = collection['labels'][sensor_key]['idxs']
pc = ros_numpy.numpify(cloud_msg)
points = np.zeros((pc.shape[0], 3))
points[:, 0] = pc['x']
points[:, 1] = pc['y']
points[:, 2] = pc['z']
frame_id = genCollectionPrefix(
collection_key,
collection['data'][sensor_key]['header']['frame_id'])
marker = Marker(
header=Header(frame_id=frame_id, stamp=now),
ns=str(collection_key) + '-' + str(sensor_key),
id=0,
frame_locked=True,
type=Marker.SPHERE_LIST,
action=Marker.ADD,
lifetime=rospy.Duration(0),
pose=Pose(position=Point(x=0, y=0, z=0),
orientation=Quaternion(x=0, y=0, z=0, w=1)),
scale=Vector3(x=0.02, y=0.02, z=0.02),
color=ColorRGBA(
r=graphics['collections'][collection_key]['color'][0],
g=graphics['collections'][collection_key]['color'][1],
b=graphics['collections'][collection_key]['color'][2],
a=0.4))
for idx in idxs:
marker.points.append(
Point(x=points[idx, 0],
y=points[idx, 1],
z=points[idx, 2]))
markers.markers.append(copy.deepcopy(marker))
# Visualize LiDAR corner points
marker = Marker(
header=Header(frame_id=frame_id, stamp=now),
ns=str(collection_key) + '-' + str(sensor_key) +
'-limit_points',
id=0,
frame_locked=True,
type=Marker.SPHERE_LIST,
action=Marker.ADD,
lifetime=rospy.Duration(0),
pose=Pose(position=Point(x=0, y=0, z=0),
orientation=Quaternion(x=0, y=0, z=0, w=1)),
scale=Vector3(x=0.07, y=0.07, z=0.07),
color=ColorRGBA(
r=graphics['collections'][collection_key]['color'][0],
g=graphics['collections'][collection_key]['color'][1],
b=graphics['collections'][collection_key]['color'][2],
a=0.8))
for pt in collection['labels'][sensor_key]['limit_points']:
marker.points.append(Point(x=pt['x'], y=pt['y'],
z=pt['z']))
markers.markers.append(copy.deepcopy(marker))
graphics['ros']['MarkersLabeled'] = markers
graphics['ros']['PubLabeled'] = rospy.Publisher('~labeled_data',
MarkerArray,
queue_size=0,
latch=True)
# -----------------------------------------------------------------------------------------------------
# -------- Robot meshes
# -----------------------------------------------------------------------------------------------------
# Check whether the robot is static, in the sense that all of its joints are fixed. If so, for efficiency purposes,
# only one robot mesh (from the selected collection) is published.
all_joints_fixed = True
for joint in xml_robot.joints:
if not joint.type == 'fixed':
print('Robot has at least joint ' + joint.name +
' non fixed. Will render all collections')
all_joints_fixed = False
break
markers = MarkerArray()
if all_joints_fixed: # render a single robot mesh
print('Robot has all joints fixed. Will render only collection ' +
selected_collection_key)
rgba = [.5, .5, .5, 1] # best color we could find
m = urdfToMarkerArray(xml_robot,
frame_id_prefix=genCollectionPrefix(
selected_collection_key, ''),
namespace=selected_collection_key,
rgba=rgba)
markers.markers.extend(m.markers)
else: # render robot meshes for all collections
for collection_key, collection in dataset['collections'].items():
# rgba = graphics['collections'][collection_key]['color']
# rgba[3] = 0.4 # change the alpha
rgba = [.5, .5, .5, 0.7] # best color we could find
m = urdfToMarkerArray(xml_robot,
frame_id_prefix=genCollectionPrefix(
collection_key, ''),
namespace=collection_key,
rgba=rgba)
markers.markers.extend(m.markers)
graphics['ros']['robot_mesh_markers'] = markers
# -----------------------------------------------------------------------------------------------------
# -------- Publish the pattern data
# -----------------------------------------------------------------------------------------------------
if dataset['calibration_config']['calibration_pattern'][
'fixed']: # Draw single pattern for selected collection key
frame_id = generateName(
dataset['calibration_config']['calibration_pattern']['link'],
prefix='c' + selected_collection_key)
ns = str(selected_collection_key)
markers = createPatternMarkers(frame_id, ns, selected_collection_key,
now, dataset, graphics)
else: # Draw a pattern per collection
markers = MarkerArray()
for idx, (collection_key,
collection) in enumerate(dataset['collections'].items()):
frame_id = generateName(
dataset['calibration_config']['calibration_pattern']['link'],
prefix='c' + collection_key)
ns = str(collection_key)
collection_markers = createPatternMarkers(frame_id, ns,
collection_key, now,
dataset, graphics)
markers.markers.extend(collection_markers.markers)
graphics['ros']['MarkersPattern'] = markers
graphics['ros']['PubPattern'] = rospy.Publisher('~patterns',
MarkerArray,
queue_size=0,
latch=True)
# Create LaserBeams Publisher -----------------------------------------------------------
# This one is recomputed every time in the objective function, so just create the generic properties.
markers = MarkerArray()
for collection_key, collection in dataset['collections'].items():
for sensor_key, sensor in dataset['sensors'].items():
if not collection['labels'][sensor_key][
'detected']: # chess not detected by sensor in collection
continue
if sensor['msg_type'] == 'LaserScan' or sensor[
'msg_type'] == 'PointCloud2':
frame_id = genCollectionPrefix(
collection_key,
collection['data'][sensor_key]['header']['frame_id'])
marker = Marker(
header=Header(frame_id=frame_id, stamp=rospy.Time.now()),
ns=str(collection_key) + '-' + str(sensor_key),
id=0,
frame_locked=True,
type=Marker.LINE_LIST,
action=Marker.ADD,
lifetime=rospy.Duration(0),
pose=Pose(position=Point(x=0, y=0, z=0),
orientation=Quaternion(x=0, y=0, z=0, w=1)),
scale=Vector3(x=0.001, y=0, z=0),
color=ColorRGBA(
r=graphics['collections'][collection_key]['color'][0],
g=graphics['collections'][collection_key]['color'][1],
b=graphics['collections'][collection_key]['color'][2],
a=1.0))
markers.markers.append(marker)
graphics['ros']['MarkersLaserBeams'] = markers
graphics['ros']['PubLaserBeams'] = rospy.Publisher('~LaserBeams',
MarkerArray,
queue_size=0,
latch=True)
# Create Miscellaneous MarkerArray -----------------------------------------------------------
markers = MarkerArray()
# Text signaling the anchored sensor
for _sensor_key, sensor in dataset['sensors'].items():
if _sensor_key == dataset['calibration_config']['anchored_sensor']:
marker = Marker(header=Header(frame_id=str(_sensor_key),
stamp=now),
ns=str(_sensor_key),
id=0,
frame_locked=True,
type=Marker.TEXT_VIEW_FACING,
action=Marker.ADD,
lifetime=rospy.Duration(0),
pose=Pose(position=Point(x=0, y=0, z=0.2),
orientation=Quaternion(x=0,
y=0,
z=0,
w=1)),
scale=Vector3(x=0.0, y=0.0, z=0.1),
color=ColorRGBA(r=0.6, g=0.6, b=0.6, a=1.0),
text='Anchored')
markers.markers.append(marker)
graphics['ros']['MarkersMiscellaneous'] = markers
graphics['ros']['PubMiscellaneous'] = rospy.Publisher('~Miscellaneous',
MarkerArray,
queue_size=0,
latch=True)
# Publish only once in latched mode
graphics['ros']['PubMiscellaneous'].publish(
graphics['ros']['MarkersMiscellaneous'])
return graphics
def __init__(self):
rospy.init_node('revisualizer')
self.rviz_pub = rospy.Publisher("visualization/state", visualization_msgs.Marker, queue_size=2)
self.rviz_pub_t = rospy.Publisher("visualization/state_t", visualization_msgs.Marker, queue_size=2)
self.rviz_pub_utils = rospy.Publisher("visualization/bus_voltage", visualization_msgs.Marker, queue_size=2)
self.kill_server = InteractiveMarkerServer("interactive_kill")
# text marker
# TODO: Clean this up, there should be a way to set all of this inline
self.surface_marker = visualization_msgs.Marker()
self.surface_marker.type = self.surface_marker.TEXT_VIEW_FACING
self.surface_marker.color = ColorRGBA(1, 1, 1, 1)
self.surface_marker.scale.z = 0.1
self.depth_marker = visualization_msgs.Marker()
self.depth_marker.type = self.depth_marker.TEXT_VIEW_FACING
self.depth_marker.color = ColorRGBA(1.0, 1.0, 1.0, 1.0)
self.depth_marker.scale.z = 0.1
# create marker for displaying current battery voltage
self.low_battery_threshold = rospy.get_param('/battery/kill_voltage', 44.0)
self.warn_battery_threshold = rospy.get_param('/battery/warn_voltage', 44.5)
self.voltage_marker = visualization_msgs.Marker()
self.voltage_marker.header.frame_id = "base_link"
self.voltage_marker.lifetime = rospy.Duration(5)
self.voltage_marker.ns = 'sub'
self.voltage_marker.id = 22
self.voltage_marker.pose.position.x = -2.0
self.voltage_marker.scale.z = 0.2
self.voltage_marker.color.a = 1
self.voltage_marker.type = visualization_msgs.Marker.TEXT_VIEW_FACING
# create an interactive marker to display kill status and change it
self.need_kill_update = True
self.kill_marker = InteractiveMarker()
self.kill_marker.header.frame_id = "base_link"
self.kill_marker.pose.position.x = -2.3
self.kill_marker.name = "kill button"
kill_status_marker = Marker()
kill_status_marker.type = Marker.TEXT_VIEW_FACING
kill_status_marker.text = "UNKILLED"
kill_status_marker.id = 55
kill_status_marker.scale.z = 0.2
kill_status_marker.color.a = 1.0
kill_button_control = InteractiveMarkerControl()
kill_button_control.name = "kill button"
kill_button_control.interaction_mode = InteractiveMarkerControl.BUTTON
kill_button_control.markers.append(kill_status_marker)
self.kill_server.insert(self.kill_marker, self.kill_buttton_callback)
self.kill_marker.controls.append(kill_button_control)
self.killed = False
# connect kill marker to kill alarm
self.kill_listener = AlarmListener("kill")
self.kill_listener.add_callback(self.kill_alarm_callback)
self.kill_alarm = AlarmBroadcaster("kill")
# distance to bottom
self.range_sub = rospy.Subscriber("dvl/range", RangeStamped, self.range_callback)
# distance to surface
self.depth_sub = rospy.Subscriber("depth", DepthStamped, self.depth_callback)
# battery voltage
self.voltage_sub = rospy.Subscriber("/bus_voltage", Float64, self.voltage_callback)
def publish(self, timestamp):
pose = PoseStamped()
pose.header.frame_id = self.target_frame
pose.header.stamp = timestamp
pose.pose.position.x = self.X[0, 0]
pose.pose.position.y = self.X[1, 0]
pose.pose.position.z = 0.0
Q = quaternion_from_euler(0, 0, self.X[2, 0])
pose.pose.orientation.x = Q[0]
pose.pose.orientation.y = Q[1]
pose.pose.orientation.z = Q[2]
pose.pose.orientation.w = Q[3]
self.pose_pub.publish(pose)
ma = MarkerArray()
marker = Marker()
marker.header = pose.header
marker.ns = "kf_uncertainty"
marker.id = 5000
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.pose = pose.pose
print 'diag(P): ' + str(numpy.diag(self.P))
marker.pose.position.z = -0.1
marker.scale.x = 3 * sqrt(self.P[0, 0])
marker.scale.y = 3 * sqrt(self.P[1, 1])
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 1.0
ma.markers.append(marker)
for id in self.idx.iterkeys():
marker = Marker()
marker.header.stamp = timestamp
marker.header.frame_id = self.target_frame
marker.ns = "landmark_kf"
marker.id = id
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
l = self.idx[id]
marker.pose.position.x = self.X[l, 0]
marker.pose.position.y = self.X[l + 1, 0]
marker.pose.position.z = -0.1
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 1
marker.pose.orientation.w = 0
marker.scale.x = 0.2
#3*sqrt(self.P[l,l])
marker.scale.y = 0.2
#3*sqrt(self.P[l+1,l+1]);
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.lifetime.secs = 3.0
ma.markers.append(marker)
marker = Marker()
marker.header.stamp = timestamp
marker.header.frame_id = self.target_frame
marker.ns = "landmark_kf"
marker.id = 1000 + id
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.ADD
marker.pose.position.x = self.X[l + 0, 0]
marker.pose.position.y = self.X[l + 1, 0]
marker.pose.position.z = 1.0
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 1
marker.pose.orientation.w = 0
marker.text = str(id)
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.lifetime.secs = 3.0
ma.markers.append(marker)
self.marker_pub.publish(ma)
if __name__ == '__main__':
# Initialize the node, as usual
rospy.init_node('markers', argv=sys.argv)
# Create a marker. Markers of all shapes share a common type.
sphere = Marker()
# Set the frame ID and type. The frame ID is the frame in which the position of the marker
# is specified. The type is the shape of the marker, detailed on the wiki page.
sphere.header.frame_id = '/base_link'
sphere.type = sphere.SPHERE
# Each marker has a unique ID number. If you have more than one marker that you want displayed at a
# given time, then each needs to have a unique ID number. If you publish a new marker with the same
# ID number and an existing marker, it will replace the existing marker with that ID number.
sphere.id = 0
# Set the action. We can add, delete, or modify markers.
sphere.action = sphere.ADD
# These are the size parameters for the marker. The effect of these on the marker will vary by shape,
# but, basically, they specify how big the marker along each of the axes of the coordinate frame named
# in frame_id.
sphere.scale.x = 0.5
sphere.scale.y = 0.5
sphere.scale.z = 0.5
# Color, as an RGB triple, from 0 to 1.
sphere.color.r = 1.0
sphere.color.g = 0.0
sphere.color.b = 0.0
