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 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 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 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 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 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 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 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 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 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 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 __init__(self):
self.layout = rospy.get_param('/thruster_layout/thrusters') # Add thruster layout from ROS param set by mapper
assert self.layout is not None, 'Could not load thruster layout from ROS param'
'''
Create MarkerArray with an arrow marker for each thruster at index node_id.
The position of the marker is as specified in the layout, but the length of the arrow
will be set at each thrust callback.
'''
self.markers = MarkerArray()
for i in xrange(len(self.layout)):
# Initialize each marker (color, scale, namespace, frame)
m = Marker()
m.header.frame_id = '/base_link'
m.type = Marker.ARROW
m.ns = 'thrusters'
m.color.a = 0.8
m.scale.x = 0.01 # Shaft diameter
m.scale.y = 0.05 # Head diameter
m.action = Marker.DELETE
m.lifetime = rospy.Duration(0.1)
self.markers.markers.append(m)
for key, layout in self.layout.iteritems():
# Set position and id of marker from thruster layout
idx = layout['node_id']
pt = numpy_to_point(layout['position'])
self.markers.markers[idx].points.append(pt)
self.markers.markers[idx].points.append(pt)
self.markers.markers[idx].id = idx
# Create publisher for marker and subscribe to thrust
self.pub = rospy.Publisher('/thrusters/markers', MarkerArray, queue_size=5)
self.thrust_sub = rospy.Subscriber('/thrusters/thrust', Thrust, self.thrust_cb, queue_size=5)
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 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 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 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)
self.marker_pub.publish(marker_array)
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 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 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 drawlandmark(pub):
global pt_count
points_tag = Marker()
points_tag.header.frame_id = "/base_link"
points_tag.action = Marker.ADD
points_tag.header.stamp = rospy.Time.now()
points_tag.lifetime = rospy.Time(0)
points_tag.scale.x = 0.1;
points_tag.scale.y = 0.1;
points_tag.scale.z = 0.1;
points_tag.color.a = 1.0;
points_tag.color.r = 1.0;
points_tag.color.g = 0.0;
points_tag.color.b = 0.0;
points_tag.ns = "pts_line"
pt_count+=1
points_tag.id = pt_count
points_tag.type = Marker.POINTS
for x in range(6):
p1 = Point()
p1.x = tagnum_x[x]/100
p1.y = tagnum_y[x]/100
p1.z = 0
points_tag.points.append(p1)
pub.publish(points_tag)
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_marker(self): # create marker marker = Marker() marker.header.frame_id = "/base_link" marker.header.stamp = rospy.Time.now() marker.ns = "color" marker.id = 0 marker.type = 2 # SPHERE marker.action = 0 # ADD marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 1.5 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.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = self.color.a #Transparency marker.color.r = self.color.r marker.color.g = self.color.g marker.color.b = self.color.b # publish marker self.pub_marker.publish(marker)
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 handle_velodyne_msg(msg, arg=None):
t_start = time.time()
global tf_segmenter_graph
global last_known_position, last_known_box_size, last_known_yaw
if segmenter_model is None:
init_segmenter(args.segmenter_model)
if localizer_model is None:
init_localizer(args.localizer_model)
assert msg._type == 'sensor_msgs/PointCloud2'
# PointCloud2 reference http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html
if verbose: print 'stamp: %s' % msg.header.stamp
#print 'number of points: %i' % msg.width * msg.height
# PERFORMANCE WARNING START
# this preparation code is super slow b/c it uses generator, ideally the code should receive 3 arrays:
# lidar_d lidar_h lidar_i already preprocessed in C++
# points = 0
# for x, y, z, intensity, ring in pc2.read_points(msg):
# cloud[points] = x, y, z, intensity, ring
# points += 1
cloud = pcl.msg2cloud(msg)
lidar, lidar_int, angle_at_edge = DidiTracklet.filter_lidar_rings(
#cloud[:points],
cloud,
rings,
points_per_ring,
clip=CLIP_DIST,
clip_h=CLIP_HEIGHT,
return_lidar_interpolated=True,
return_angle_at_edges=True)
points_per_sector = points_per_ring // sectors
_sectors = 2 * sectors if segmenter_phased else sectors
lidar_d = np.empty((_sectors, points_per_sector, len(rings)),
dtype=np.float32)
lidar_h = np.empty((_sectors, points_per_sector, len(rings)),
dtype=np.float32)
lidar_i = np.empty((_sectors, points_per_sector, len(rings)),
dtype=np.float32)
s_start = 0
for sector in range(sectors):
s_end = s_start + points_per_sector
for ring in range(len(rings)):
lidar_d[sector, :, ring] = lidar[ring, s_start:s_end, 0]
lidar_h[sector, :, ring] = lidar[ring, s_start:s_end, 1]
lidar_i[sector, :, ring] = lidar[ring, s_start:s_end, 2]
s_start = s_end
if segmenter_phased:
s_start = points_per_sector // 2
for sector in range(sectors - 1):
_sector = sectors + sector
s_end = s_start + points_per_sector
for ring in range(len(rings)):
lidar_d[_sector, :, ring] = lidar[ring, s_start:s_end, 0]
lidar_h[_sector, :, ring] = lidar[ring, s_start:s_end, 1]
lidar_i[_sector, :, ring] = lidar[ring, s_start:s_end, 2]
s_start = s_end
for ring in range(len(rings)):
lidar_d[_sectors - 1, :points_per_sector // 2,
ring] = lidar[ring, :points_per_sector // 2, 0]
lidar_d[_sectors - 1, points_per_sector // 2:,
ring] = lidar[ring,
points_per_ring - points_per_sector // 2:, 0]
lidar_h[_sectors - 1, :points_per_sector // 2,
ring] = lidar[ring, :points_per_sector // 2, 1]
lidar_h[_sectors - 1, points_per_sector // 2:,
ring] = lidar[ring,
points_per_ring - points_per_sector // 2:, 1]
lidar_i[_sectors - 1, :points_per_sector // 2,
ring] = lidar[ring, :points_per_sector // 2, 2]
lidar_i[_sectors - 1, points_per_sector // 2:,
ring] = lidar[ring,
points_per_ring - points_per_sector // 2:, 2]
# PERFORMANCE WARNING END
if K._backend == 'tensorflow':
with tf_segmenter_graph.as_default():
time_seg_infe_start = time.time()
class_predictions_by_angle = segmenter_model.predict(
[lidar_d, lidar_h, lidar_i], batch_size=_sectors)
time_seg_infe_end = time.time()
else:
time_seg_infe_start = time.time()
class_predictions_by_angle = segmenter_model.predict(
[lidar_d, lidar_h, lidar_i], batch_size=_sectors)
time_seg_infe_end = time.time()
if verbose:
print ' Seg inference: %0.3f ms' % (
(time_seg_infe_end - time_seg_infe_start) * 1000.0)
if segmenter_phased:
_class_predictions_by_angle = class_predictions_by_angle.reshape(
(-1, points_per_ring, len(rings)))
class_predictions_by_angle = np.copy(_class_predictions_by_angle[0, :])
class_predictions_by_angle[points_per_sector // 2 : points_per_ring - (points_per_sector//2)] += \
_class_predictions_by_angle[1 , : points_per_ring - points_per_sector]
class_predictions_by_angle[0 : points_per_sector // 2 ] += \
_class_predictions_by_angle[1 , points_per_ring - points_per_sector : points_per_ring - (points_per_sector // 2)]
class_predictions_by_angle[points_per_ring - (points_per_sector // 2) : ] += \
_class_predictions_by_angle[1 , points_per_ring - (points_per_sector // 2): ]
class_predictions_by_angle_idx = np.argwhere(
class_predictions_by_angle >= (2 * segmenter_threshold))
else:
class_predictions_by_angle = np.squeeze(
class_predictions_by_angle.reshape(
(-1, points_per_ring, len(rings))),
axis=0)
if segmenter_threshold == 0.:
# dynamic thresholding
number_of_segmented_points = 0
dyn_threshold = 0.7
while (number_of_segmented_points < 100) and dyn_threshold >= 0.2:
class_predictions_by_angle_thresholded = (
class_predictions_by_angle >= dyn_threshold)
number_of_segmented_points = np.sum(
class_predictions_by_angle_thresholded)
dyn_threshold -= 0.1
class_predictions_by_angle_idx = np.argwhere(
class_predictions_by_angle_thresholded)
# print(dyn_threshold + 0.1, number_of_segmented_points)
else:
#for prob in [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8]:
# print(prob, np.sum(class_predictions_by_angle >= prob))
class_predictions_by_angle_idx = np.argwhere(
class_predictions_by_angle >= segmenter_threshold)
filtered_points_xyz = np.empty((0, 3))
if (class_predictions_by_angle_idx.shape[0] > 0):
# the idea of de-interpolation is to remove artifacts created by same-neighbor interpolation
# by checking repeated values (which are going to be nearest-neighbor interpolated values with high prob)
# for code convenience, we'e just taking the unique indexes as returned by np.unique but we
# could further improve this by calculating the center of mass on the X axis of the prediction
# vector (with the unique elements only), and take the index closest to the center for each duplicated stride.
if deinterpolate:
#((a.shape[0] - 1 - i_l ) + (i_f)) // 2
deinterpolated_class_predictions_by_angle_idx = np.empty((0, 2))
lidar_d_interpolated = lidar_d.reshape(
(-1, points_per_ring, len(rings)))[0]
for ring in range(len(rings)):
predictions_idx_in_ring = class_predictions_by_angle_idx[
class_predictions_by_angle_idx[:, 1] == ring]
if predictions_idx_in_ring.shape[0] > 1:
lidar_d_predictions_in_ring = lidar_d_interpolated[
predictions_idx_in_ring[:, 0], ring]
_, lidar_d_predictions_in_ring_unique_idx_first = np.unique(
lidar_d_predictions_in_ring, return_index=True)
_, lidar_d_predictions_in_ring_unique_idx_last = np.unique(
lidar_d_predictions_in_ring[::-1], return_index=True)
lidar_d_predictions_in_ring_unique_idx = \
(lidar_d_predictions_in_ring.shape[0] - 1 - lidar_d_predictions_in_ring_unique_idx_last + lidar_d_predictions_in_ring_unique_idx_first ) // 2
deinterpolated_class_predictions_by_angle_idx_this_ring = \
predictions_idx_in_ring[lidar_d_predictions_in_ring_unique_idx]
deinterpolated_class_predictions_by_angle_idx = np.concatenate(
(deinterpolated_class_predictions_by_angle_idx,
deinterpolated_class_predictions_by_angle_idx_this_ring
))
class_predictions_by_angle_idx = deinterpolated_class_predictions_by_angle_idx.astype(
int)
segmented_points = lidar_int[class_predictions_by_angle_idx[:, 0] +
points_per_ring *
class_predictions_by_angle_idx[:, 1]]
# remove capture vehicle, helps in ford01.bag (and doesn't hurt)
segmented_points = DidiTracklet._remove_capture_vehicle(
segmented_points)
# TODO: use PREDICTED position instead of last known for false positive rejection
if reject_false_positives and last_known_position is not None and segmented_points.shape[
0] > 2:
original_number_of_points = segmented_points.shape[0]
time_start = time.time()
rfp_implementation = 2
if rfp_implementation == 1:
import hdbscan
clusterer = hdbscan.HDBSCAN(allow_single_cluster=True,
metric='l2',
min_cluster_size=50)
cluster_labels = clusterer.fit_predict(segmented_points[:, :2])
number_of_clusters = len(
set(cluster_labels)) - (1 if -1 in cluster_labels else 0)
if verbose:
print("Clusters " + str(number_of_clusters) + ' from ' +
str(segmented_points.shape[0]) + ' points')
if number_of_clusters > 1:
unique_clusters = set(cluster_labels)
closest_cluster_center_of_mass = np.array([1e20, 1e20])
best_cluster = None
for cluster in unique_clusters:
points_in_cluster = segmented_points[cluster_labels ==
cluster]
points_in_cluster_center_of_mass = np.mean(
points_in_cluster[:, :2], axis=0)
if verbose:
print(cluster, points_in_cluster.shape,
points_in_cluster_center_of_mass)
if cluster != -1:
if np.linalg.norm(closest_cluster_center_of_mass - last_known_position[:2]) >\
np.linalg.norm(points_in_cluster_center_of_mass - last_known_position[:2]):
closest_cluster_center_of_mass = points_in_cluster_center_of_mass
best_cluster = cluster
#if verbose: print("best cluster", best_cluster, 'last_known_position at ', last_known_position)
selected_clusters = [best_cluster]
for cluster in unique_clusters:
if (cluster != -1) and (cluster != best_cluster):
points_in_cluster = segmented_points[cluster_labels
== cluster]
distances_from_last_known_position = np.linalg.norm(
points_in_cluster[:, :2] -
closest_cluster_center_of_mass,
axis=1)
if np.all(distances_from_last_known_position < 2.
): # TODO ADJUST
selected_clusters.append(cluster)
filtered_points_xyz = segmented_points[np.in1d(
cluster_labels, selected_clusters, invert=True), :3]
segmented_points = segmented_points[np.in1d(
cluster_labels, selected_clusters)]
if verbose:
print('selected_clusters: ' + str(selected_clusters) +
' with points ' +
str(segmented_points.shape[0]) + '/' +
str(original_number_of_points))
else:
within_tolerance_idx = (
((segmented_points[:, 0] - last_known_position[0])**2 +
(segmented_points[:, 0] - last_known_position[0])**2) <=
(5**2))
filtered_points_xyz = segmented_points[
~within_tolerance_idx, :3]
if filtered_points_xyz.shape[0] < original_number_of_points:
segmented_points = segmented_points[within_tolerance_idx]
if verbose:
print 'clustering filter: {:.2f}ms'.format(
1e3 * (time.time() - time_start))
else:
segmented_points = np.empty((0, 3))
detection = 0
pose = np.zeros((3))
box_size = np.zeros((3))
yaw = np.zeros((1))
if segmented_points.shape[0] >= localizer_points_threshold:
detection = 1
# filter outlier points
if True:
time_start = time.time()
cloud_orig = pcl.PointCloud(segmented_points[:, :3].astype(
np.float32))
fil = cloud_orig.make_statistical_outlier_filter()
fil.set_mean_k(50)
fil.set_std_dev_mul_thresh(1.0)
inlier_inds = fil.filter_ind()
segmented_points = segmented_points[inlier_inds, :]
if verbose:
print 'point cloud filter: {:.2f}ms'.format(
1e3 * (time.time() - time_start))
#filtered_points_xyz = segmented_points[:,:3]
segmented_points_mean = np.mean(segmented_points[:, :3], axis=0)
angle = np.arctan2(segmented_points_mean[1], segmented_points_mean[0])
aligned_points = point_utils.rotZ(segmented_points, angle)
segmented_and_aligned_points_mean = np.mean(aligned_points[:, :3],
axis=0)
aligned_points[:, :3] -= segmented_and_aligned_points_mean
distance_to_segmented_and_aligned_points = np.linalg.norm(
segmented_and_aligned_points_mean[:2])
aligned_points_resampled = DidiTracklet.resample_lidar(
aligned_points[:, :4], pointnet_points)
aligned_points_resampled = np.expand_dims(aligned_points_resampled,
axis=0)
distance_to_segmented_and_aligned_points = np.expand_dims(
distance_to_segmented_and_aligned_points, axis=0)
if K._backend == 'tensorflow':
with tf_localizer_graph.as_default():
time_loc_infe_start = time.time()
pose, box_size, yaw = localizer_model.predict_on_batch([
aligned_points_resampled,
distance_to_segmented_and_aligned_points
])
time_loc_infe_end = time.time()
else:
time_loc_infe_start = time.time()
pose, box_size, yaw = localizer_model.predict_on_batch([
aligned_points_resampled,
distance_to_segmented_and_aligned_points
])
time_loc_infe_end = time.time()
pose = np.squeeze(pose, axis=0)
box_size = np.squeeze(box_size, axis=0)
yaw = np.squeeze(yaw, axis=0)
if verbose:
print ' Loc inference: %0.3f ms' % (
(time_loc_infe_end - time_loc_infe_start) * 1000.0)
pose += segmented_and_aligned_points_mean
pose = point_utils.rotZ(pose, -angle)
yaw = point_utils.remove_orientation(yaw + angle)
pose_angle = np.arctan2(pose[1], pose[0])
angle_diff = angle_at_edge - pose_angle
if angle_diff < 0.:
angle_diff += 2 * np.pi
# ALI => delta_time is the time difference in milliseconds (0-100) from the start of the lidar
# scan to the time the object was detected, i don' know if the lidar msg is referenced to be
# beginning of the scan or the end... so basically adjust the lidar observation for cases which
# we need to test:
# observation_time = msg.header.stamp.to_sec() + delta_time
# observation_time = msg.header.stamp.to_sec() - delta_time
# observation_time = msg.header.stamp.to_sec() + 100 msecs - delta_time
delta_time = 0.1 * angle_diff / (2 * np.pi)
if verbose: print(angle_at_edge, pose_angle, angle_diff)
if verbose: print(pose, box_size, yaw, delta_time)
if delta_time < 0:
print(angle_at_edge, pose_angle, angle_diff, delta_time)
# fix lidar static tilt
Rx = rotXMat(np.deg2rad(g_roll_correction))
Ry = rotYMat(np.deg2rad(g_pitch_correction))
Rz = rotZMat(np.deg2rad(g_yaw_correction))
pose = Rz.dot(Ry.dot(Rx.dot([pose[0], pose[1], pose[2]])))
pose[2] += g_z_correction
# scale bbox size
box_size[2] = g_bbox_scale_l * box_size[2]
box_size[1] = g_bbox_scale_w * box_size[1]
box_size[0] = g_bbox_scale_h * box_size[0]
last_known_position = pose
last_known_box_size = box_size
# FUSION
with g_fusion_lock:
observation = LidarObservation(msg.header.stamp.to_sec(), pose[0],
pose[1], pose[2], yaw)
g_fusion.filter(observation)
segmented_points_cloud_msg = pc2.create_cloud_xyz32(
msg.header, segmented_points[:, :3])
# publish car prediction data as separate regular ROS messages just for vizualization (dunno how to visualize custom messages in rviz)
publish_rviz_topics = False
if publish_rviz_topics and detection > 0:
# point cloud
seg_pnt_pub = rospy.Publisher(name='segmented_obj',
data_class=PointCloud2,
queue_size=1)
seg_msg = PointCloud2()
seg_pnt_pub.publish(segmented_points_cloud_msg)
# car pose frame
yaw_q = ros_tf.transformations.quaternion_from_euler(0, 0, yaw)
br = ros_tf.TransformBroadcaster()
br.sendTransform(tuple(pose), tuple(yaw_q), rospy.Time.now(),
'obj_lidar_centroid', 'velodyne')
# give bbox different color, depending on the predicted object class
if detection == 1: # car
bbox_color = ColorRGBA(r=1., g=1., b=0., a=0.5)
else: # ped
bbox_color = ColorRGBA(r=0., g=0., b=1., a=0.5)
# bounding box
marker = Marker()
marker.header.frame_id = "obj_lidar_centroid"
marker.header.stamp = rospy.Time.now()
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.scale.x = box_size[2]
marker.scale.y = box_size[1]
marker.scale.z = box_size[0]
marker.color = bbox_color
marker.lifetime = rospy.Duration()
pub = rospy.Publisher("obj_lidar_bbox", Marker, queue_size=10)
pub.publish(marker)
# filtered point cloud
fil_points_msg = pc2.create_cloud_xyz32(msg.header,
filtered_points_xyz)
rospy.Publisher(name='segmented_filt_obj',
data_class=PointCloud2,
queue_size=1).publish(fil_points_msg)
print "TIME: ", time.time() - t_start
return {
'detection': detection,
'x': pose[0],
'y': pose[1],
'z': pose[2],
'l': box_size[2],
'w': box_size[1],
'h': box_size[0],
'yaw': np.squeeze(yaw)
}
def node():
global frontiers, mapData, global1, global2, global3, globalmaps, litraIndx, n_robots, namespace_init_count
rospy.init_node('filter', anonymous=False)
#rospy.loginfo('filter and send: I am here')
# fetching all parameters
map_topic = rospy.get_param('~map_topic', '/map')
threshold = rospy.get_param('~costmap_clearing_threshold', 70)
# this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate
info_radius = rospy.get_param('~info_radius', 1.0)
goals_topic = rospy.get_param('~goals_topic', '/detected_points')
n_robots = rospy.get_param('~n_robots', 1)
namespace = rospy.get_param('~namespace', '')
namespace_init_count = rospy.get_param('namespace_init_count', 1)
rateHz = rospy.get_param('~rate', 100)
global_costmap_topic = rospy.get_param(
'~global_costmap_topic', '/move_base/global_costmap/costmap'
) #Initially /move_base_node/global_costmap/costmap
robot_frame = rospy.get_param('~robot_frame', 'base_link')
litraIndx = len(namespace)
rate = rospy.Rate(rateHz)
# -------------------------------------------
rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack)
# ---------------------------------------------------------------------------------------------------------------
for i in range(0, n_robots):
globalmaps.append(OccupancyGrid())
if len(namespace) > 0:
for i in range(0, n_robots):
rospy.Subscriber(
namespace + str(i + namespace_init_count) +
global_costmap_topic, OccupancyGrid, globalMap)
elif len(namespace) == 0:
rospy.Subscriber(global_costmap_topic, OccupancyGrid, globalMap)
# wait if map is not received yet
while (len(mapData.data) < 1):
rospy.loginfo('Waiting for the map')
rospy.sleep(0.1)
pass
# wait if any of robots' global costmap map is not received yet
for i in range(0, n_robots):
while (len(globalmaps[i].data) < 1):
rospy.loginfo('Waiting for the global costmap')
rospy.sleep(0.1)
pass
global_frame = "/" + mapData.header.frame_id
tfLisn = tf.TransformListener()
if len(namespace) > 0:
for i in range(0, n_robots):
tfLisn.waitForTransform(
global_frame[1:],
namespace + str(i + namespace_init_count) + '/' + robot_frame,
rospy.Time(0), rospy.Duration(10.0))
elif len(namespace) == 0:
tfLisn.waitForTransform(global_frame[1:], '/' + robot_frame,
rospy.Time(0), rospy.Duration(10.0))
rospy.Subscriber(goals_topic,
PointStamped,
callback=callBack,
callback_args=[tfLisn, global_frame[1:]])
pub = rospy.Publisher('frontiers', Marker, queue_size=10)
pub2 = rospy.Publisher('centroids', Marker, queue_size=10)
filterpub = rospy.Publisher('filtered_points', PointArray, queue_size=10)
rospy.loginfo("the map and global costmaps are received")
# wait if no frontier is received yet
while len(frontiers) < 1:
pass
#rospy.loginfo('filter and send: I am here')
points = Marker()
points_clust = Marker()
# Set the frame ID and timestamp. See the TF tutorials for information on these.
points.header.frame_id = mapData.header.frame_id
points.header.stamp = rospy.Time.now()
points.ns = "markers2"
points.id = 0
points.type = Marker.POINTS
# Set the marker action for latched frontiers. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
points.action = Marker.ADD
points.pose.orientation.w = 1.0
points.scale.x = 0.2
points.scale.y = 0.2
points.color.r = 255.0 / 255.0
points.color.g = 255.0 / 255.0
points.color.b = 0.0 / 255.0
points.color.a = 1
points.lifetime = rospy.Duration()
p = Point()
p.z = 0
pp = []
pl = []
points_clust.header.frame_id = mapData.header.frame_id
points_clust.header.stamp = rospy.Time.now()
points_clust.ns = "markers3"
points_clust.id = 4
points_clust.type = Marker.POINTS
# Set the marker action for centroids. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
points_clust.action = Marker.ADD
points_clust.pose.orientation.w = 1.0
points_clust.scale.x = 0.2
points_clust.scale.y = 0.2
points_clust.color.r = 0.0 / 255.0
points_clust.color.g = 255.0 / 255.0
points_clust.color.b = 0.0 / 255.0
points_clust.color.a = 1
points_clust.lifetime = rospy.Duration()
temppoint = PointStamped()
temppoint.header.frame_id = mapData.header.frame_id
temppoint.header.stamp = rospy.Time(0)
temppoint.point.z = 0.0
arraypoints = PointArray()
tempPoint = Point()
tempPoint.z = 0.0
# -------------------------------------------------------------------------
# --------------------- Main Loop -------------------------------
# -------------------------------------------------------------------------
while not rospy.is_shutdown():
# -------------------------------------------------------------------------
# Clustering frontier points
centroids = []
front = copy(frontiers)
if len(front) > 1:
ms = MeanShift(bandwidth=0.3)
ms.fit(front)
centroids = ms.cluster_centers_ # centroids array is the centers of each cluster
# if there is only one frontier no need for clustering, i.e. centroids=frontiers
if len(front) == 1:
centroids = front
# -------------------------------------------------------------------------
# clearing old frontiers
z = 0
while z < len(centroids):
cond = False
temppoint.point.x = centroids[z][0]
temppoint.point.y = centroids[z][1]
for i in range(0, n_robots):
transformedPoint = tfLisn.transformPoint(
globalmaps[i].header.frame_id, temppoint)
x = array([transformedPoint.point.x, transformedPoint.point.y])
cond = (gridValue(globalmaps[i], x) > threshold) or cond
if (cond or
(informationGain(mapData, [centroids[z][0], centroids[z][1]],
info_radius * 0.5)) < 0.2):
centroids = delete(centroids, (z), axis=0)
z = z - 1
z += 1
frontiers = copy(centroids) #Change this to after the cleannup
# -------------------------------------------------------------------------
# publishing
arraypoints.points = []
for i in centroids:
tempPoint.x = i[0]
tempPoint.y = i[1]
arraypoints.points.append(copy(tempPoint))
filterpub.publish(arraypoints)
pp = []
for q in range(0, len(frontiers)):
p.x = frontiers[q][0]
p.y = frontiers[q][1]
pp.append(copy(p))
points.points = pp
pp = []
for q in range(0, len(centroids)):
p.x = centroids[q][0]
p.y = centroids[q][1]
pp.append(copy(p))
points_clust.points = pp
pub.publish(points)
pub2.publish(points_clust)
rate.sleep()
def plotPolygons(self):
# https://answers.ros.org/question/203782/rviz-marker-line_strip-is-not-displayed/
marker = Marker()
marker.header.frame_id = "yumi_base_link"
marker.type = marker.LINE_LIST
marker.action = marker.ADD
# marker scale
marker.scale.x = 0.002
marker.scale.y = 0.002
marker.scale.z = 0.002
# marker color
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
# marker orientaiton
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
# marker position
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
objectList = self.objects
# marker line points
marker.points = []
for j in range(len(objectList)):
for ii in range(objectList[j].lines.shape[0]):
lineIndex = objectList[j].lines[ii]
lineVerticeis = objectList[j].verticesTransformed[lineIndex]
height = objectList[j].heightTransformed
x_data = lineVerticeis[:, 0].tolist()
y_data = lineVerticeis[:, 1].tolist()
point1 = Point()
point1.x = x_data[0]
point1.y = y_data[0]
point1.z = height[0]
marker.points.append(point1)
point2 = Point()
point2.x = x_data[1]
point2.y = y_data[1]
point2.z = height[0]
marker.points.append(point2)
point3 = Point()
point3.x = x_data[0]
point3.y = y_data[0]
point3.z = height[1]
marker.points.append(point1)
marker.points.append(point3)
point4 = Point()
point4.x = x_data[1]
point4.y = y_data[1]
point4.z = height[1]
marker.points.append(point2)
marker.points.append(point4)
point5 = Point()
point5.x = x_data[0]
point5.y = y_data[0]
point5.z = height[1]
marker.points.append(point5)
point6 = Point()
point6.x = x_data[1]
point6.y = y_data[1]
point6.z = height[1]
marker.points.append(point6)
self.linePub.publish(marker)
def forward_kinematics(data):
if not correct(data):
rospy.logerr('Incorrect position! ' + str(data))
return
a, d, al, th = params['i1']
al, a, d, th = float(al), float(a), float(d), float(th)
tz = translation_matrix((0, 0, d))
rz = rotation_matrix(data.position[0], zaxis)
tx = translation_matrix((a, 0, 0))
rx = rotation_matrix(al, xaxis)
T1 = concatenate_matrices(rx, tx, rz, tz)
a, d, al, th = params['i2']
al, a, d, th = float(al), float(a), float(d), float(th)
tz = translation_matrix((0, 0, d))
rz = rotation_matrix(data.position[1], zaxis)
tx = translation_matrix((a, 0, 0))
rx = rotation_matrix(al, xaxis)
T2 = concatenate_matrices(rx, tx, rz, tz)
a, d, al, th = params['i3']
al, a, d, th = float(al), float(a), float(d), float(th)
tz = translation_matrix((0, 0, data.position[2]))
rz = rotation_matrix(th, zaxis)
tx = translation_matrix((a, 0, 0))
rx = rotation_matrix(al, xaxis)
T3 = concatenate_matrices(rx, tx, rz, tz)
Tk = concatenate_matrices(T1, T2, T3)
x, y, z = translation_from_matrix(Tk)
qx, qy, qz, qw = quaternion_from_matrix(Tk)
pose = PoseStamped()
pose.header.frame_id = 'base_link'
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
pose.pose.orientation.x = qx
pose.pose.orientation.y = qy
pose.pose.orientation.z = qz
pose.pose.orientation.w = qw
pub.publish(pose)
marker = Marker()
marker.header.frame_id = 'base_link'
marker.type = marker.CUBE
marker.action = marker.ADD
marker.pose.orientation.w = 1
time = rospy.Duration()
marker.lifetime = time
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.pose.position.x = x
marker.pose.position.y = y
marker.pose.position.z = z
marker.pose.orientation.x = qx
marker.pose.orientation.y = qy
marker.pose.orientation.z = qz
marker.pose.orientation.w = qw
marker.color.a = 0.7
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
marker_pub.publish(marker)
def publish(self, target_frame, timestamp):
pose = PoseStamped()
pose.header.frame_id = 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 = tf.transformations.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
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 = 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 = 3 * sqrt(self.P[l, l])
marker.scale.y = 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 = 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)
def gnd_marker_pub(gnd_label, marker_pub, cfg, color = "red"):
length = int(cfg.grid_range[2] - cfg.grid_range[0]) # x direction
width = int(cfg.grid_range[3] - cfg.grid_range[1]) # y direction
gnd_marker = Marker()
gnd_marker.header.frame_id = "/kitti/base_link"
gnd_marker.header.stamp = rospy.Time.now()
gnd_marker.type = gnd_marker.LINE_LIST
gnd_marker.action = gnd_marker.ADD
gnd_marker.scale.x = 0.05
gnd_marker.scale.y = 0.05
gnd_marker.scale.z = 0.05
if(color == "red"):
gnd_marker.color.a = 1.0
gnd_marker.color.r = 1.0
gnd_marker.color.g = 0.0
gnd_marker.color.b = 0.0
if(color == "green"):
gnd_marker.color.a = 1.0
gnd_marker.color.r = 0.0
gnd_marker.color.g = 1.0
gnd_marker.color.b = 0.0
gnd_marker.points = []
# gnd_labels are arranged in reverse order
for j in range(gnd_label.shape[0]):
for i in range(gnd_label.shape[1]):
pt1 = Point()
pt1.x = i + cfg.grid_range[0]
pt1.y = j + cfg.grid_range[1]
pt1.z = gnd_label[j,i]
if j>0 :
pt2 = Point()
pt2.x = i + cfg.grid_range[0]
pt2.y = j-1 +cfg.grid_range[1]
pt2.z = gnd_label[j-1, i]
gnd_marker.points.append(pt1)
gnd_marker.points.append(pt2)
if i>0 :
pt2 = Point()
pt2.x = i -1 + cfg.grid_range[0]
pt2.y = j + cfg.grid_range[1]
pt2.z = gnd_label[j, i-1]
gnd_marker.points.append(pt1)
gnd_marker.points.append(pt2)
if j < width-1 :
pt2 = Point()
pt2.x = i + cfg.grid_range[0]
pt2.y = j + 1 + cfg.grid_range[1]
pt2.z = gnd_label[j+1, i]
gnd_marker.points.append(pt1)
gnd_marker.points.append(pt2)
if i < length-1 :
pt2 = Point()
pt2.x = i + 1 + cfg.grid_range[0]
pt2.y = j + cfg.grid_range[1]
pt2.z = gnd_label[j, i+1]
gnd_marker.points.append(pt1)
gnd_marker.points.append(pt2)
marker_pub.publish(gnd_marker)
def collision_state():
global ms, co_svc, root_link_name, root_link_offset, last_collision_status
diagnostic = DiagnosticArray()
now = rospy.Time.now()
diagnostic.header.stamp = now
if not co.isActive():
rospy.loginfo("co is not activated")
return
collision_status = co_svc.getCollisionStatus()
if (now - last_collision_status > rospy.Duration(5.0)):
num_collision_pairs = len(collision_status[1].lines)
rospy.loginfo(
"check %d collision status, %f Hz", num_collision_pairs,
num_collision_pairs / (now - last_collision_status).to_sec())
last_collision_status = now
# check if ther are collision
status = DiagnosticStatus(name='CollisionDetector',
level=DiagnosticStatus.OK,
message="Ok")
#if any(a): # this calls omniORB.any
for collide in collision_status[1].collide:
if collide:
status.level = DiagnosticStatus.ERROR
status.message = "Robots is in collision mode"
status.values.append(
KeyValue(key="Time", value=str(collision_status[1].time)))
status.values.append(
KeyValue(key="Computation Time",
value=str(collision_status[1].computation_time)))
status.values.append(
KeyValue(key="Safe Posture",
value=str(collision_status[1].safe_posture)))
status.values.append(
KeyValue(key="Recover Time",
value=str(collision_status[1].recover_time)))
status.values.append(
KeyValue(key="Loop for check",
value=str(collision_status[1].loop_for_check)))
frame_id = root_link_name # root id
markerArray = MarkerArray()
for line in collision_status[1].lines:
p1 = Point(*(numpy.dot(root_link_offset[3, 3], line[0]) +
root_link_offset[0:3, 3]))
p2 = Point(*(numpy.dot(root_link_offset[3, 3], line[1]) +
root_link_offset[0:3, 3]))
sphere_color = ColorRGBA(0, 1, 0, 1)
line_width = 0.01
line_length = numpy.linalg.norm(
numpy.array((p1.x, p1.y, p1.z)) - numpy.array((p2.x, p2.y, p2.z)))
sphere_scale = 0.02
# color changes between 0.145(green) -> 0.02(red), under 0.02, it always red
if (line_length <= 0.145):
sphere_scale = 0.02
if (line_length <= 0.0002):
sphere_scale = 0.045
sphere_color = ColorRGBA(1, 0, 1,
1) ## color is purple, if collide
elif (line_length < 0.02):
sphere_scale = 0.04
sphere_color = ColorRGBA(1, 0, 0, 1) ## color is red
else:
ratio = 1.0 - (line_length -
0.02) * 8 # 0.0 (0.145) -> 1.0 ( 0.02)
sphere_scale = 0.02 + ratio * 0.02 # 0.02 -> 0.04
sphere_color = ColorRGBA(ratio, 1 - ratio, 0,
1) # green -> red
marker = Marker()
marker.header.frame_id = frame_id
marker.type = marker.LINE_LIST
marker.action = marker.ADD
marker.color = sphere_color
marker.points = [p1, p2]
marker.scale.x = line_width
markerArray.markers.append(marker)
sphere_scale = Vector3(sphere_scale, sphere_scale, sphere_scale)
marker = Marker()
marker.header.frame_id = frame_id
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale = sphere_scale
marker.color = sphere_color
marker.pose.orientation.w = 1.0
marker.pose.position = p1
markerArray.markers.append(marker)
marker = Marker()
marker.header.frame_id = frame_id
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale = sphere_scale
marker.color = sphere_color
marker.pose.orientation.w = 1.0
marker.pose.position = p2
markerArray.markers.append(marker)
id = 0
for m in markerArray.markers:
m.lifetime = rospy.Duration(1.0)
m.id = id
id += 1
pub_collision.publish(markerArray)
diagnostic.status.append(status)
pub_diagnostics.publish(diagnostic)
punkte = []
for h in np.arange(0.1, 12.7, 0.2):
hilfspunkt = Point()
hilfspunkt.x = f(h)
hilfspunkt.y = f2(h)
hilfspunkt.z = 0.0
punkte.append(hilfspunkt)
punkte.append(punkte[0])
marker = Marker()
marker.header.frame_id = "map"
marker.ns = "road"
marker.id = 0
marker.type = Marker.LINE_STRIP
marker.action = Marker.ADD
marker.scale.x = 0.3
marker.scale.y = 0.3
marker.scale.z = 0.3
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.y = 0.0
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.points = punkte
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
def yolo_darknet(self, img, depth_image, imgstamp):
cv2.imwrite('/home/ubuntu/SSD-Tensorflow/test.jpg', img)
yolo_results = {}
yolo_results[0] = "name"
yolo_results[1] = "" #"x"
yolo_results[2] = "" #"y"
yolo_results[3] = "" #"w"
yolo_results[4] = "" #"h"
name = ""
#global frame
global flag_find
#frame += 1
#if frame%10 == 0:
yolo_process = Popen(['python', 'darknet_test.py'],
stdout=PIPE,
stderr=STDOUT)
results = ""
for line in yolo_process.stdout:
results += line.decode('utf-8', "replace")
i = 0
'''with open('/home/ubuntu/SSD-Tensorflow/result_yolo.txt', mode = 'rt') as f:
txtdata = list(f)
print txtdata[0].strip('\n')
print int(txtdata[4])
name = txtdata[0].strip('\n')
x = int(txtdata[1])
y = int(txtdata[2])
w = int(txtdata[3])
h = int(txtdata[4])'''
name, x, y, w, h = read_file()
if name != " ":
flag_find = 1
else:
flag_find = 0
rx = 0.0
ry = 0.0
rw = 0.0
rh = 0.0
if flag_find == 1:
name, x, y, w, h = read_file()
rx = (x - w / 2) / 320
ry = (y - h / 2) / 240
rw = (x + w / 2) / 320
rh = (y + h / 2) / 240
x = int(x)
y = int(y)
w = int(w)
h = int(h)
cv2.rectangle(img, (int(x - w / 2), int(y - h / 2)),
(int(x + w / 2), int(y + h / 2)), (255, 0, 0), 2)
roi_color = img[y:y + h, x:x + w]
cv2.putText(img, name, (int(x - w / 2), int(y - h / 2)),
cv2.FONT_HERSHEY_DUPLEX, 0.4, (255, 0, 0), 1)
print(rx, ry, rw, rh)
self.distance = []
distance_count = []
distance_r = 0.10
self.center = 0.0
x1 = x - w / 2
y1 = y - h / 2
x2 = x + w / 2
y2 = y + h / 2
if (x1 < 0):
x1 = 0
if (y1 < 0):
y1 = 0
if (x2 > 320):
x2 = 319
if (y2 > 240):
y2 = 239
img.flags.writeable = True
for i in range(y1, y2 + 1):
for j in range(x1, x2 + 1):
if depth_image.item(i, j) == depth_image.item(i, j):
self.distance.append((depth_image.item(i, j), 0))
img.itemset((i, j, 0), 0)
img.itemset((i, j, 1), 0)
print(len(self.distance))
if (len(self.distance) == 0):
return name, rx, ry, rw, rh
if (len(self.distance) != 0):
#self.center = median(self.distance)
distance_count = [0] * len(self.distance)
distance_kd_tree = ss.KDTree(self.distance, leafsize=10)
distance_res = list(distance_kd_tree.query_pairs(distance_r))
for j in range(len(distance_res)):
for k in range(2):
distance_count[distance_res[j][k]] += 1
max_index = distance_count.index(max(distance_count))
self.center = self.distance[max_index][0] - 0.15
print("%f [m]" % self.center)
tmp = math.radians((((x2 + x1) / 2.0) - (320.0 / 2.0)) *
(self.depth_horizon / 320.0))
self.robot_x = abs(self.center * math.cos(tmp))
if (x2 + x1 / 2.0) < 160:
self.robot_y = abs(self.center * math.sin(tmp))
else:
self.robot_y = -abs(self.center * math.sin(-tmp))
print(self.robot_x, self.robot_y)
#TF transformation
point_result = PointStamped()
point_result.header.frame_id = "base_footprint"
#point_result.header.stamp = rospy.Time(0)
point_result.header.stamp = imgstamp
self._latest_point = point_result
point_result.point.x = self.robot_x
point_result.point.y = self.robot_y
point_result.point.z = 0.0
print("###transformation")
try:
self._latest_point = self._tf_listener.transformPoint(
"/map", point_result)
#self._latest_point = self._tf_listener.transformPoint("/map",rospy.Time.now(),imgstamp,point_result,"/map")
#self._latest_point = self._tf_listener.transformPoint("/map",rospy.Time(0),imgstamp,point_result,"/map")
except (rostf.LookupException, rostf.ConnectivityException,
rostf.ExtrapolationException) as e:
rospy.loginfo("Exception: {}".format(e))
print(self._latest_point.point.x, self._latest_point.point.y)
marker_data = Marker()
marker_data.header.frame_id = "map"
marker_data.header.stamp = rospy.Time.now()
marker_data.ns = "text"
marker_data.id = 0
marker_data.action = Marker.ADD
marker_data.type = 9
marker_data.text = "object"
marker_data.pose.position.x = self._latest_point.point.x
marker_data.pose.position.y = self._latest_point.point.y
marker_data.pose.position.z = 0.0
marker_data.pose.orientation.x = 0.0
marker_data.pose.orientation.y = 0.0
marker_data.pose.orientation.z = 0.0
marker_data.pose.orientation.w = 0.0
marker_data.color.r = 1.0
marker_data.color.g = 0.0
marker_data.color.b = 0.0
marker_data.color.a = 1.0
marker_data.scale.x = 1.0
marker_data.scale.y = 0.1
marker_data.scale.z = 0.1
self.marker_pub.publish(marker_data)
#print("#Delay:", (rospy.Time.now()-rgb_data.header.stamp).to_sec())
if (name == "banana"):
self.banana_data.append(
(self._latest_point.point.x, self._latest_point.point.y))
self.data_count = [0] * len(self.banana_data)
kd_tree = ss.KDTree(self.banana_data, leafsize=10)
res = list(kd_tree.query_pairs(self.r))
for j in range(len(res)):
for k in range(2):
self.data_count[res[j][k]] += 1
print("------- self.data_coount -------")
print(self.data_count)
if (max(self.data_count) != 0):
max_index = self.data_count.index(max(self.data_count))
print(self.banana_data[max_index])
print("max_data_count:", max(self.data_count))
else:
max_index = 0
marker_voting = Marker()
marker_voting.header.frame_id = "map"
marker_voting.header.stamp = rospy.Time.now()
marker_voting.ns = "voting_point"
marker_voting.id = 0
marker_voting.action = Marker.ADD
marker_voting.type = 9
marker_voting.text = "BANANA"
marker_voting.pose.position.x = self.banana_data[max_index][0]
marker_voting.pose.position.y = self.banana_data[max_index][1]
marker_voting.pose.position.z = 0.0
marker_voting.pose.orientation.x = 0.0
marker_voting.pose.orientation.y = 0.0
marker_voting.pose.orientation.z = 0.0
marker_voting.pose.orientation.w = 0.0
marker_voting.color.r = 0.0
marker_voting.color.g = 0.0
marker_voting.color.b = 1.0
marker_voting.color.a = 1.0
marker_voting.scale.x = 1.0
marker_voting.scale.y = 0.1
marker_voting.scale.z = 0.1
self.marker_voting_banana_pub.publish(marker_voting)
#cv2.namedWindow("color_image")
#cv2.imshow("color_image", img)
#cv2.waitKey(10)
self._pub_banana_point.publish(
Float32MultiArray(data=[
self.banana_data[max_index][0],
self.banana_data[max_index][1]
]))
#self._pub_banana_point.publish(Float32MultiArray(data=[self.banana_data[max_index][0]-0.2, self.banana_data[max_index][1]-0.2]))
elif (name == "apple"):
self.apple_data.append(
(self._latest_point.point.x, self._latest_point.point.y))
self.data_count = [0] * len(self.apple_data)
kd_tree = ss.KDTree(self.apple_data, leafsize=10)
res = list(kd_tree.query_pairs(self.r))
for j in range(len(res)):
for k in range(2):
self.data_count[res[j][k]] += 1
print("------- self.data_coount -------")
print(self.data_count)
if (max(self.data_count) != 0):
max_index = self.data_count.index(max(self.data_count))
print(self.apple_data[max_index])
print("max_data_count:", max(self.data_count))
else:
max_index = 0
marker_voting = Marker()
marker_voting.header.frame_id = "map"
marker_voting.header.stamp = rospy.Time.now()
marker_voting.ns = "voting_point"
marker_voting.id = 0
marker_voting.action = Marker.ADD
marker_voting.type = 9
marker_voting.text = "APPLE"
marker_voting.pose.position.x = self.apple_data[max_index][0]
marker_voting.pose.position.y = self.apple_data[max_index][1]
marker_voting.pose.position.z = 0.0
marker_voting.pose.orientation.x = 0.0
marker_voting.pose.orientation.y = 0.0
marker_voting.pose.orientation.z = 0.0
marker_voting.pose.orientation.w = 0.0
marker_voting.color.r = 0.0
marker_voting.color.g = 0.0
marker_voting.color.b = 1.0
marker_voting.color.a = 1.0
marker_voting.scale.x = 1.0
marker_voting.scale.y = 0.1
marker_voting.scale.z = 0.1
self.marker_voting_apple_pub.publish(marker_voting)
#cv2.namedWindow("color_image")
#cv2.imshow("color_image", img)
#cv2.waitKey(10)
self._pub_apple_point.publish(
Float32MultiArray(data=[
self.apple_data[max_index][0],
self.apple_data[max_index][1]
]))
#self._pub_apple_point.publish(Float32MultiArray(data=[self.apple_data[max_index][0]-0.2, self.apple_data[max_index][1]-0.2]))
elif (name == "cable"):
self.cable_data.append(
(self._latest_point.point.x, self._latest_point.point.y))
self.data_count = [0] * len(self.cable_data)
kd_tree = ss.KDTree(self.cable_data, leafsize=10)
res = list(kd_tree.query_pairs(self.r))
for j in range(len(res)):
for k in range(2):
self.data_count[res[j][k]] += 1
print("------- self.data_coount -------")
print(self.data_count)
if (max(self.data_count) != 0):
max_index = self.data_count.index(max(self.data_count))
print(self.cable_data[max_index])
print("max_data_count:", max(self.data_count))
else:
max_index = 0
marker_voting = Marker()
marker_voting.header.frame_id = "map"
marker_voting.header.stamp = rospy.Time.now()
marker_voting.ns = "voting_point"
marker_voting.id = 0
marker_voting.action = Marker.ADD
marker_voting.type = 9
marker_voting.text = "CABLE"
marker_voting.pose.position.x = self.cable_data[max_index][0]
marker_voting.pose.position.y = self.cable_data[max_index][1]
marker_voting.pose.position.z = 0.0
marker_voting.pose.orientation.x = 0.0
marker_voting.pose.orientation.y = 0.0
marker_voting.pose.orientation.z = 0.0
marker_voting.pose.orientation.w = 0.0
marker_voting.color.r = 0.0
marker_voting.color.g = 0.0
marker_voting.color.b = 1.0
marker_voting.color.a = 1.0
marker_voting.scale.x = 1.0
marker_voting.scale.y = 0.1
marker_voting.scale.z = 0.1
self.marker_voting_cable_pub.publish(marker_voting)
#cv2.namedWindow("color_image")
#cv2.imshow("color_image", img)
#cv2.waitKey(10)
#self._pub_cable_point.publish(Float32MultiArray(data=[self.cable_data[max_index][0]-0.2, self.cable_data[max_index][1]-0.2]))
#self._pub_cable_point.publish(Float32MultiArray(data=[self.cable_data[max_index][0], self.cable_data[max_index][1]]))
self.latest_cable_x = self.cable_data[max_index][0]
self.latest_cable_y = self.cable_data[max_index][1]
return name, rx, ry, rw, rh
import numpy as np
rospy.init_node("marker_node", anonymous=True)
pub = rospy.Publisher("marker_test", Marker, queue_size=10)
rate = rospy.Rate(25)
while not rospy.is_shutdown():
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time.now()
marker.id = 0
marker.action = Marker.ADD
marker.lifetime = rospy.Duration()
marker.type = Marker.MESH_RESOURCE
marker.mesh_resource = "package://kitti/bmw_x5/BMW X5 4.dae"
# marker.mesh_use_embedded_materials = True
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = -1.73
q = tf.transformations.quaternion_from_euler(np.pi / 2, 0, np.pi)
marker.pose.orientation.x = q[0]
marker.pose.orientation.y = q[1]
marker.pose.orientation.z = q[2]
marker.pose.orientation.w = q[3]
marker.color.r = 1.0
marker.color.g = 1.0
def callback(msg, tmp_list):
""""""
global old_yaw
[
particle_filter, publisher_position, publisher_mavros,
publisher_particles, broadcaster, publisher_marker
] = tmp_list
# particle filter algorithm
particle_filter.predict() # move particles
# get length of message
num_meas = len(msg.detections)
orientation_yaw_pitch_roll = np.zeros((num_meas, 3))
# if new measurement: update particles
if num_meas >= 1:
measurements = np.zeros((num_meas, 1 + PART_DIM))
# get data from topic /tag_detection
if rviz:
markerArray = MarkerArray()
for i, tag in enumerate(msg.detections):
tag_id = int(tag.id[0])
distance = np.array(([
tag.pose.pose.pose.position.x, tag.pose.pose.pose.position.y,
tag.pose.pose.pose.position.z
]))
measurements[i, 0] = np.linalg.norm(distance)
tmpquat = Quaternion(w=tag.pose.pose.pose.orientation.w,
x=tag.pose.pose.pose.orientation.x,
y=tag.pose.pose.pose.orientation.y,
z=tag.pose.pose.pose.orientation.z)
orientation_yaw_pitch_roll[i, :] = tmpquat.inverse.yaw_pitch_roll
index = np.where(tags[:, 0] == tag_id)
measurements[i, 1:4] = tags[index, 1:4]
# print(measurements[i, 1:4])
if rviz:
marker = Marker()
marker.header.frame_id = "global_tank"
marker.id = i
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = measurements[
i, 0] * 2 # r*2 of distance to camera from tag_14
marker.scale.y = measurements[i, 0] * 2
marker.scale.z = measurements[i, 0] * 2
marker.color.g = 1
marker.color.a = 0.1 # transparency
marker.pose.orientation.w = 1.0
marker.pose.position.x = tags[index, 1][0] # x
marker.pose.position.y = tags[index, 2][0] # y
marker.pose.position.z = tags[index, 3][0] # z
markerArray.markers.append(marker)
if rviz:
# print(len(markerArray.markers))
publisher_marker.publish(markerArray)
# print(index)
particle_filter.update(measurements)
yaw = np.mean(orientation_yaw_pitch_roll[:, 0])
pitch = np.mean(orientation_yaw_pitch_roll[:, 1])
roll = np.mean(orientation_yaw_pitch_roll[:, 2])
else:
yaw = old_yaw
old_yaw = yaw
# print "reale messungen: " + str(measurements)
print("Angle yaw:", yaw * 180 / np.pi)
estimated_orientation = yaw_pitch_roll_to_quat(-(yaw - np.pi / 2), 0, 0)
# estimated_orientation = yaw_pitch_roll_to_quat(yaw, 0, 0)#evtl wrong
# calculate position as mean of particle positions
estimated_position = particle_filter.get_position_estimate()
# [mm]
x_mean_ned = estimated_position[
0] * 1000 # global Tank Coordinate System(NED)
y_mean_ned = estimated_position[1] * 1000
z_mean_ned = estimated_position[2] * 1000
# publish estimated_pose [m] in mavros to /mavros/vision_pose/pose
# this pose needs to be in ENU
mavros_position = PoseStamped()
mavros_position.header.stamp = rospy.Time.now()
mavros_position.header.frame_id = "map"
mavros_position.pose.position.x = y_mean_ned / 1000 # NED Coordinate to ENU(ROS)
mavros_position.pose.position.y = x_mean_ned / 1000
mavros_position.pose.position.z = -z_mean_ned / 1000
mavros_position.pose.orientation.w = estimated_orientation.w
mavros_position.pose.orientation.x = estimated_orientation.x
mavros_position.pose.orientation.y = estimated_orientation.y
mavros_position.pose.orientation.z = estimated_orientation.z
# publish estimated_pose [m]
position = PoseStamped()
position.header.stamp = rospy.Time.now()
position.header.frame_id = "global_tank" # ned
position.pose.position.x = x_mean_ned / 1000
position.pose.position.y = y_mean_ned / 1000
position.pose.position.z = z_mean_ned / 1000
estimated_orientation = yaw_pitch_roll_to_quat(yaw, 0, 0)
position.pose.orientation.w = estimated_orientation.w
position.pose.orientation.x = estimated_orientation.x
position.pose.orientation.y = estimated_orientation.y
position.pose.orientation.z = estimated_orientation.z
publisher_position.publish(position)
# yaw = 0 / 180.0 * np.pi
# tmp = yaw_pitch_roll_to_quat(-(yaw-np.pi/2), 0, 0)
# print(tmp)
# mavros_position.pose.orientation.w = tmp.w
# mavros_position.pose.orientation.x = tmp.x
# mavros_position.pose.orientation.y = tmp.y
# mavros_position.pose.orientation.z = tmp.z
publisher_mavros.publish(mavros_position)
# For Debugging
# mavros_position = PoseStamped()
# mavros_position.header.stamp = rospy.Time.now()
# mavros_position.header.frame_id = "map"
# mavros_position.pose.position.x = 1.0 + np.random.normal(0, 0.01)
# mavros_position.pose.position.y = 2.0 + np.random.normal(0, 0.01)
# mavros_position.pose.position.z = 3.0 + np.random.normal(0, 0.01)
#
# mavros_position.pose.orientation.w = 1.0
# mavros_position.pose.orientation.x = 2.0
# mavros_position.pose.orientation.y = 3.0
# mavros_position.pose.orientation.z = 4.0
# publisher_mavros.publish(mavros_position)
"""
# publish transform
broadcaster.sendTransform((estimated_position[0], estimated_position[1], estimated_position[2]),
(1.0, 0, 0, 0),
rospy.Time.now(),
"TestPose",
"world")
"""
if rviz:
# publish particles as PoseArray
pose_array = PoseArray()
pose_array.header.stamp = rospy.Time.now()
pose_array.header.frame_id = "global_tank"
# for i in range(num_meas):
# print(orientation_yaw_pitch_roll[i, 0] * 180 / np.pi, orientation_yaw_pitch_roll[i, 1] * 180 / np.pi,
# orientation_yaw_pitch_roll[i, 2] * 180 / np.pi)
print("done")
for i in range(particle_filter.NUM_P):
pose = Pose()
pose.position.x = particle_filter.particles[i, 0]
pose.position.y = particle_filter.particles[i, 1]
pose.position.z = particle_filter.particles[i, 2]
# pose.orientation.x =
# pose.orientation.y =
# pose.orientation.z =
# pose.orientation.w =
pose_array.poses.append(pose)
publisher_particles.publish(pose_array)
def callback(data):
xaxis = (1,0,0)
zaxis = (0,0,1)
#ARM 1
a, d, al, th = dh['i1']
a, d, al, th = float(a), float(d), float(al), float(th)
tz = translation_matrix((0, 0, d))
rz = rotation_matrix(data.position[0], zaxis)
tx = translation_matrix((a, 0, 0))
rx = rotation_matrix(al, xaxis)
T1 = concatenate_matrices(rx, tx, rz, tz)
#ARM 2
a, d, al, th = dh['i2']
a, d, al, th = float(a), float(d), float(al), float(th)
tz = translation_matrix((0, 0, d))
rz = rotation_matrix(data.position[1], zaxis)
tx = translation_matrix((a, 0, 0))
rx = rotation_matrix(al, xaxis)
T2 = concatenate_matrices(rx, tx, rz, tz)
#ARM 3
a, d, al, th = dh['i3']
a, d, al, th = float(a), float(d), float(al), float(th)
tz = translation_matrix((0, 0, data.position[2]-d))
rz = rotation_matrix(th, zaxis)
tx = translation_matrix((a, 0, 0))
rx = rotation_matrix(al, xaxis)
T3 = concatenate_matrices(rx, tx, rz, tz)
#FINAL
final_matrix = concatenate_matrices(T1, T2, T3)
x, y, z = translation_from_matrix(final_matrix)
qx, qy, qz, qw = quaternion_from_matrix(final_matrix)
pose = PoseStamped()
pose.header.frame_id = 'base_link'
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z+baseHeight
pose.pose.orientation.x = qx
pose.pose.orientation.y = qy
pose.pose.orientation.z = qz
pose.pose.orientation.w = qw
pubP.publish(pose)
marker = Marker()
marker.header.frame_id = 'base_link'
path.header.frame_id = 'base_link'
marker.header.stamp = rospy.Time.now()
path.header.stamp = rospy.Time.now()
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.pose.orientation.w = 1
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.pose.position.x = x
marker.pose.position.y = y
marker.pose.position.z = z+baseHeight
marker.pose.orientation.x = qx
marker.pose.orientation.y = qy
marker.pose.orientation.z = qz
marker.pose.orientation.w = qw
marker.color.a = 0.5
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b = 0
path.poses.append(pose)
pubM.publish(marker)
pubH.publish(path)
#SHOW MATRIX
print("T1:")
print(T1)
print("T2:")
print(T2)
print("T3:")
print(T3)
print("FINAL:")
print(final_matrix)
def interactive_callback(self, data):
if (data.markers!=[]):
eps=1
n = 0
self.markerArray.markers = []
self.obstacle_matrix = np.zeros( (self.map_size[0]/self.resolution+10,self.map_size[1]/self.resolution+10,2),dtype='f' )
self.obstacle_matrix.fill(-100)
for index in range(0,2):
self.obstacle=data.markers[index].pose
x1,y1=[data.markers[index].pose.position.x*100,data.markers[index].pose.position.y*100]
x_index=np.int(round((x1+self.map_origin[0])/self.resolution))
y_index=np.int(round((y1+self.map_origin[1])/self.resolution))
if ((x_index>-1)and(x_index<(self.map_size[0]/self.resolution-1))and(y_index>-1)and(y_index<(self.map_size[1]/self.resolution-1))):
for i in range(-10,10):
for j in range(-10,10):
x_index_=x_index+i
if (x_index_<0):
x_index_=0
if (x_index_>((self.map_size[0]/self.resolution)-1)):
x_index_=(self.map_size[0]/self.resolution-1)
y_index_=y_index+j
if (y_index_<0):
y_index_=0
if (y_index_>((self.map_size[1]/self.resolution)-1)):
y_index_=self.map_size[1]/self.resolution-1
theta=np.arctan2(self.matrix[x_index,y_index,1],self.matrix[x_index,y_index,0])
if ((((np.cos(theta)*i+np.sin(theta)*j)**2)/100+((np.sin(theta)*i-np.cos(theta)*j)**2)/10)<1.0):
Lg= np.sqrt(self.matrix[x_index_,y_index_,0]**2+self.matrix[x_index_,y_index_,1]**2)
Lo= np.sqrt(i**2+j**2)
if (Lo!=0):
if (self.obstacle_matrix[x_index_,y_index_,0]!=-100):
self.obstacle_matrix[x_index_,y_index_,0]=self.obstacle_matrix[x_index_,y_index_,0]+self.matrix[x_index_,y_index_,0]+i*(Lg/Lo)*max(1.0,(4.0/(abs(Lo)+0.1)))
self.obstacle_matrix[x_index_,y_index_,1]=self.obstacle_matrix[x_index_,y_index_,1]+self.matrix[x_index_,y_index_,1]+j*(Lg/Lo)*max(1.0,(4.0/(abs(Lo)+0.1)))
else:
self.obstacle_matrix[x_index_,y_index_,0]=self.matrix[x_index_,y_index_,0]+i*(Lg/Lo)*max(1.0,(3.0/(abs(Lo)+0.1)))
self.obstacle_matrix[x_index_,y_index_,1]=self.matrix[x_index_,y_index_,1]+j*(Lg/Lo)*max(1.0,(3.0/(abs(Lo)+0.1)))
quaternion = quaternion_from_euler(0, 0, np.arctan2(self.obstacle_matrix[x_index+i,y_index+j,1],2.5*self.obstacle_matrix[x_index+i,y_index+j,0]))
marker = Marker()
marker.ns = 'obstacles3'
marker.id = n
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.lifetime = rospy.Duration(0)
marker.scale.x = 0.12
marker.scale.y = 0.03
marker.scale.z = 0.04
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0
pose = Pose()
pose.position.x = ((x_index+i)*self.resolution-self.map_origin[0])/100.0;
pose.position.y = ((y_index+j)*self.resolution-self.map_origin[1])/100.0;
pose.position.z = 0;
pose.orientation.x = quaternion[0]
pose.orientation.y = quaternion[1]
pose.orientation.z = quaternion[2]
pose.orientation.w = quaternion[3]
marker.header.frame_id = 'map'
marker.header.stamp = rospy.Time.now()
marker.pose=pose
self.markerArray.markers.append(marker)
n = n+1
self.marker_pub3.publish(self.markerArray)
def run(self):
thrust = 0
print("jello")
while not rospy.is_shutdown():
if self.state == Controller.TakeOff:
t = self.listener.getLatestCommonTime("/mocap",
"/Nano_Mark_Gon4")
if self.listener.canTransform("/mocap", "/Nano_Mark_Uni2", t):
position, quaternion = self.listener.lookupTransform(
"/mocap", "/Nano_Mark_Gon4", t)
print(position[0], position[1], position[2])
#if position[2] > 2.0 or thrust > 54000:
if thrust > 55000:
self.pidReset()
self.pidZ.integral = thrust / self.pidZ.ki
#self.targetZ = 0.5
self.state = Controller.Automatic
thrust = 0
else:
thrust += 500
#self.power = thrust
msg = self.cmd_vel_telop
msg.linear.z = thrust
self.pubNav.publish(msg)
if self.state == Controller.Land:
t = self.listener.getLatestCommonTime("/mocap",
"/Nano_Mark_Gon4")
if self.listener.canTransform("/mocap", "/Nano_Mark_Uni2", t):
position, quaternion = self.listener.lookupTransform(
"/mocap", "/Nano_Mark_Gon4", t)
if position[2] > 0.05:
msg_land = self.cmd_vel_telop
self.power -= 100
msg_land.linear.z = self.power
self.pubNav.publish(msg_land)
else:
msg_land = self.cmd_vel_telop
msg_land.linear.z = 0
self.pubNav.publish(msg_land)
if self.state == Controller.Automatic:
# transform target world coordinates into local coordinates
t = self.listener.getLatestCommonTime("/mocap",
"/Nano_Mark_Gon4")
print(t)
if self.listener.canTransform("/mocap", "/Nano_Mark_Uni2", t):
position, quaternion = self.listener.lookupTransform(
"/mocap", "/Nano_Mark_Gon4", t)
#print(position[0],position[1],position[2])
euler = tf.transformations.euler_from_quaternion(
quaternion)
print(euler[2] * (180 / math.pi))
msg = self.cmd_vel_telop
#print(self.power)
#Descompostion of the x and y contributions following the Z-Rotation
x_prim = self.pidX.update(0.0, self.targetX - position[0])
y_prim = self.pidY.update(0.0, self.targetY - position[1])
msg.linear.x = x_prim * math.cos(
euler[2]) - y_prim * math.sin(euler[2])
msg.linear.y = x_prim * math.sin(
euler[2]) + y_prim * math.cos(euler[2])
#---old stuff---
#msg.linear.x = self.pidX.update(0.0, 0.0-position[0])
#msg.linear.y = self.pidY.update(0.0,-1.0-position[1])
#msg.linear.z = self.pidZ.update(position[2],1.0)
#z_prim = self.pidZ.update(position[2],self.targetZ)
#print(z_prim)
#if z_prim < self.power:
# msg.linear.z = self.power
#else:
# msg.linear.z = z_prim
#msg.linear.z = self.power
#print(self.power)
msg.linear.z = self.pidZ.update(
0.0, self.targetZ - position[2]
) #self.pidZ.update(position[2], self.targetZ)
msg.angular.z = self.pidYaw.update(
0.0,
self.des_angle * (math.pi / 180) +
euler[2]) #*(180/math.pi))
#msg.angular.z = self.pidYaw.update(0.0,self.des_angle - euler[2])#*(180/math.pi))
print(msg.linear.x, msg.linear.y, msg.linear.z,
msg.angular.z)
#print(euler[2])
#print(msg.angular.z)
self.pubNav.publish(msg)
#Publish Real and Target position
self.pubtarX.publish(self.targetX)
self.pubtarY.publish(self.targetY)
self.pubtarZ.publish(self.targetZ)
self.pubrealX.publish(position[0])
self.pubrealY.publish(position[1])
self.pubrealZ.publish(position[2])
#change square point
if abs(self.targetX-position[0])<0.08 and \
abs(self.targetY-position[1])<0.08 and \
abs(self.targetZ-position[2])<0.08 and \
self.square_start == True:
self.square_go()
#Landing
if abs(self.targetX-position[0])<0.1 and \
abs(self.targetY-position[1])<0.1 and \
abs(self.targetZ-position[2])<0.1 and \
self.land_flag == True:
self.state = Controller.Land
self.power = msg.linear.z
#Publish Path
#point = Marker()
#line = Marker()
#point.header.frame_id = line.header.frame_id = 'mocap'
#POINTS
#point.action = point.ADD
#point.pose.orientation.w = 1.0
#point.id = 0
#point.type = point.POINTS
#point.scale.x = 0.01
#point.scale.y = 0.01
#point.color.g = 1.0
#point.color.a = 1.0
#LINE
#line.action = line.ADD
#line.pose.orientation.w = 1.0
#line.id = 1
#line.type = line.LINE_STRIP
#line.scale.x = 0.01
#line.color.g = 1.0
#line.color.a = 1.0
#p = Point()
#p.x = position[0]
#p.y = position[1]
#p.z = position[2]
#point.points.append(p)
# line.points.append(p)
#self.path.markers.append(p)
#id = 0
#for m in self.path.markers:
# m.id = id
# id += 1
#self.pubPath.publish(self.path)
#self.pubPath.publish(point)
#self.pubPath.publish(line)
point = Marker()
point.header.frame_id = 'mocap'
point.type = point.SPHERE
#points.header.stamp = rospy.Time.now()
point.ns = 'cf_Uni_path'
point.action = point.ADD
#points.id = 0;
point.scale.x = 0.005
point.scale.y = 0.005
point.scale.z = 0.005
point.color.a = 1.0
point.color.r = 1.0
point.color.g = 1.0
point.color.b = 0.0
point.pose.orientation.w = 1.0
point.pose.position.x = position[0]
point.pose.position.y = position[1]
point.pose.position.z = position[2]
self.path.markers.append(point)
id = 0
for m in self.path.markers:
m.id = id
id += 1
self.pubPath.publish(self.path)
#point = Point()
#point.x = position[0]
#point.y = position[1]
#point.z = position[2]
#points.points.append(point)
#self.p.append(pos2path)
#self.path.header.stamp = rospy.Time.now()
#self.path.header.frame_id = 'mocap'
#self.path.poses = self.p
#self.pubPath.publish(points)
rospy.sleep(0.01)
def scan_callback(self, msg):
atic = timer()
# edge case: first callback
if self.msg_prev is None:
self.msg_prev = msg
return
if self.odom is None:
print("odom not received yet")
return
if self.tf_rob_in_fix is None:
print("tf_rob_in_fix not found yet")
return
if self.tf_goal_in_fix is None:
self.tf_goal_in_fix = self.tf_rob_in_fix
print("goal set")
# TODO check that odom and tf are not old
# measure rotation TODO
s = np.array(msg.ranges)
# prediction
dt = (msg.header.stamp - self.msg_prev.header.stamp).to_sec()
s_prev = np.array(self.msg_prev.ranges)
ds = (s - s_prev)
max_ds = self.kMaxObstacleVel_ms * dt
ds_capped = ds
ds_capped[np.abs(ds) > max_ds] = 0
s_next = np.maximum(0, s + ds_capped).astype(np.float32)
# cluster
EUCLIDEAN_CLUSTERING_THRESH_M = 0.05
angles = np.linspace(0,
2 * np.pi,
s_next.shape[0] + 1,
dtype=np.float32)[:-1]
clusters, x, y = clustering.euclidean_clustering(
s_next, angles, EUCLIDEAN_CLUSTERING_THRESH_M)
cluster_sizes = clustering.cluster_sizes(len(s_next), clusters)
s_next[cluster_sizes <= 3] = 25
# dwa
# Get state
# goal in robot frame
goal_in_robot_frame = apply_tf_to_pose(
Pose2D(self.tf_goal_in_fix),
inverse_pose2d(Pose2D(self.tf_rob_in_fix)))
gx = goal_in_robot_frame[0]
gy = goal_in_robot_frame[1]
# robot speed in robot frame
u = self.odom.twist.twist.linear.x
v = self.odom.twist.twist.linear.y
w = self.odom.twist.twist.angular.z
DWA_DT = 0.5
COMFORT_RADIUS_M = self.kRobotComfortRadius_m
MAX_XY_VEL = 0.5
tic = timer()
best_u, best_v, best_score = dynamic_window.linear_dwa(
s_next,
angles,
u,
v,
gx,
gy,
DWA_DT,
DV=0.05,
UMIN=0 if self.args.forward_only else -MAX_XY_VEL,
UMAX=MAX_XY_VEL,
VMIN=-MAX_XY_VEL,
VMAX=MAX_XY_VEL,
AMAX=10.,
COMFORT_RADIUS_M=COMFORT_RADIUS_M,
)
toc = timer()
# print(best_u * DWA_DT, best_v * DWA_DT, best_score)
# print("DWA: {:.2f} Hz".format(1/(toc-tic)))
# Slow turn towards goal
# TODO
best_w = 0
WMAX = 0.5
angle_to_goal = np.arctan2(gy, gx) # [-pi, pi]
if np.sqrt(gx * gx + gy * gy) > 0.5: # turn only if goal is far away
if np.abs(angle_to_goal) > (np.pi / 4 / 10): # deadzone
best_w = np.clip(angle_to_goal, -WMAX, WMAX) # linear ramp
if not self.STOP:
# publish cmd_vel
cmd_vel_msg = Twist()
SIDEWAYS_FACTOR = 0.3 if self.args.forward_only else 1.
cmd_vel_msg.linear.x = np.clip(best_u * 0.5, -0.3, 0.3)
cmd_vel_msg.linear.y = np.clip(best_v * 0.5 * SIDEWAYS_FACTOR,
-0.3, 0.3)
cmd_vel_msg.angular.z = best_w
self.cmd_vel_pub.publish(cmd_vel_msg)
# publish cmd_vel vis
pub = rospy.Publisher("/dwa_cmd_vel", Marker, queue_size=1)
mk = Marker()
mk.header.frame_id = self.kRobotFrame
mk.ns = "arrows"
mk.id = 0
mk.type = 0
mk.action = 0
mk.scale.x = 0.02
mk.scale.y = 0.02
mk.color.b = 1
mk.color.a = 1
mk.frame_locked = True
pt = Point()
pt.x = 0
pt.y = 0
pt.z = 0.03
mk.points.append(pt)
pt = Point()
pt.x = 0 + best_u * DWA_DT
pt.y = 0 + best_v * DWA_DT
pt.z = 0.03
mk.points.append(pt)
pub.publish(mk)
pub = rospy.Publisher("/dwa_goal", Marker, queue_size=1)
mk = Marker()
mk.header.frame_id = self.kRobotFrame
mk.ns = "arrows"
mk.id = 0
mk.type = 0
mk.action = 0
mk.scale.x = 0.02
mk.scale.y = 0.02
mk.color.g = 1
mk.color.a = 1
mk.frame_locked = True
pt = Point()
pt.x = 0
pt.y = 0
pt.z = 0.03
mk.points.append(pt)
pt = Point()
pt.x = 0 + gx
pt.y = 0 + gy
pt.z = 0.03
mk.points.append(pt)
pub.publish(mk)
pub = rospy.Publisher("/dwa_radius", Marker, queue_size=1)
mk = Marker()
mk.header.frame_id = self.kRobotFrame
mk.ns = "radius"
mk.id = 0
mk.type = 3
mk.action = 0
mk.pose.position.z = -0.1
mk.scale.x = COMFORT_RADIUS_M * 2
mk.scale.y = COMFORT_RADIUS_M * 2
mk.scale.z = 0.01
mk.color.b = 1
mk.color.g = 1
mk.color.r = 1
mk.color.a = 1
mk.frame_locked = True
pub.publish(mk)
# publish scan prediction
msg_next = deepcopy(msg)
msg_next.ranges = s_next
# for pretty colors
cluster_ids = clustering.cluster_ids(len(x), clusters)
random_map = np.arange(len(cluster_ids))
np.random.shuffle(random_map)
cluster_ids = random_map[cluster_ids]
msg_next.intensities = cluster_ids
self.pubs[0].publish(msg_next)
# publish past
msg_prev = deepcopy(msg)
msg_prev.ranges = self.msg_prev.ranges
self.pubs[1].publish(msg_prev)
# ...
# finally, set up for next callback
self.msg_prev = msg
atoc = timer()
if self.args.hz:
print("DWA callback: {:.2f} Hz".format(1 / (atoc - atic)))
def publish_FOV(self, event):
try:
self.dist_FOV = 4.0
self.angle = 45
self.delta_point_FOV = 22.5
A_local = np.array([0,0])
B_local = np.array([self.dist_FOV * np.cos(self.angle * np.pi / 180) , self.dist_FOV * np.sin(self.angle * np.pi / 180) ])
C_local = np.array([self.dist_FOV * np.cos((self.angle-self.delta_point_FOV) * np.pi / 180) , self.dist_FOV * np.sin((self.angle-self.delta_point_FOV) * np.pi / 180) ])
D_local = np.array([self.dist_FOV * np.cos((self.angle-2*self.delta_point_FOV) * np.pi / 180) , self.dist_FOV * np.sin((self.angle-2*self.delta_point_FOV) * np.pi / 180) ])
E_local = np.array([self.dist_FOV * np.cos((self.angle-3*self.delta_point_FOV) * np.pi / 180) , self.dist_FOV * np.sin((self.angle-3*self.delta_point_FOV) * np.pi / 180) ])
F_local = np.array([self.dist_FOV * np.cos((self.angle-4*self.delta_point_FOV) * np.pi / 180) , self.dist_FOV * np.sin((self.angle-4*self.delta_point_FOV) * np.pi / 180) ])
A_map = A_local
B_map = B_local
C_map = C_local
D_map = D_local
E_map = E_local
F_map = F_local
# A_map = A_local + self.T
A_map = np.matmul(A_map , self.R_minus)
A_map = A_map + self.T
# B_map = B_local + self.T
B_map = np.matmul(B_map , self.R_minus)
B_map = B_map + self.T
# C_map = C_local + self.T
C_map = np.matmul(C_map , self.R_minus)
C_map = C_map + self.T
D_map = np.matmul(D_map , self.R_minus)
D_map = D_map + self.T
E_map = np.matmul(E_map , self.R_minus)
E_map = E_map + self.T
F_map = np.matmul(F_map , self.R_minus)
F_map = F_map + self.T
A_map = A_map.flatten()
B_map = B_map.flatten()
C_map = C_map.flatten()
D_map = D_map.flatten()
E_map = E_map.flatten()
F_map = F_map.flatten()
marker_publisher = rospy.Publisher('/FOV_marker', Marker, queue_size=10)
marker = Marker()
marker.id = 1500
marker.ns = "FOV"
marker.header.frame_id = "/map"
marker.type = marker.LINE_LIST
marker.action = marker.ADD
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.a = 0.5
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
pA = Point() ; pA.x = A_map[0] ; pA.y = A_map[1] ; pA.z = 0
pB = Point() ; pB.x = B_map[0] ; pB.y = B_map[1] ; pB.z = 0
pC = Point() ; pC.x = C_map[0] ; pC.y = C_map[1] ; pC.z = 0
pD = Point() ; pD.x = D_map[0] ; pD.y = D_map[1] ; pD.z = 0
pE = Point() ; pE.x = E_map[0] ; pE.y = E_map[1] ; pE.z = 0
pF = Point() ; pF.x = F_map[0] ; pF.y = F_map[1] ; pF.z = 0
p_list = [pA, pB, pB, pC, pC, pD, pD, pE, pE, pF, pF, pA]
marker.points = p_list
marker_publisher.publish(marker)
rospy.Rate(100).sleep()
except:
return
def tf_callback(self, msg):
for transform in msg.transforms:
#Check if in all transform head is published
if transform.child_frame_id == 'head': #If head tf was publish update the marker
marker = Marker()
marker.id = 0
marker.ns = 'head'
marker.header.frame_id = 'head'
marker.type = Marker.MESH_RESOURCE
marker.mesh_resource = "package://blindbuy_oakd/meshes/head.dae"
marker.action = Marker.ADD
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
#https://wiki.ogre3d.org/tiki-index.php?page=Quaternion%20and%20Rotation%20Primer#Some_useful_normalized_quaternions
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.07
marker.scale.y = 0.07
marker.scale.z = 0.07
#Skin color
# marker.color.r=0.98824
# marker.color.g=0.81569
# marker.color.b=0.70588
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
self.publisher_.publish(marker)
self.update_head = False
if transform.child_frame_id == 'palm': #If head tf was publish update the marker
marker = Marker()
marker.id = 0
marker.ns = 'palm'
marker.header.frame_id = 'palm'
marker.type = Marker.MESH_RESOURCE
marker.mesh_resource = "package://blindbuy_oakd/meshes/hand.dae"
marker.action = Marker.ADD
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
#https://wiki.ogre3d.org/tiki-index.php?page=Quaternion%20and%20Rotation%20Primer#Some_useful_normalized_quaternions
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.01
marker.scale.y = 0.01
marker.scale.z = 0.01
#Skin color
# marker.color.r=0.98824
# marker.color.g=0.81569
# marker.color.b=0.70588
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
self.publisher_.publish(marker)
self.update_head = False
def publish_3dbox(box3d_pub, corners_3d_velos, types, track_ids): marker_array = MarkerArray() for track_id, corners_3d_velo in enumerate(corners_3d_velos): marker = Marker() marker.header.frame_id = FRAME_ID marker.header.stamp = rospy.Time.now() marker.id = track_id marker.action = Marker.ADD marker.lifetime = rospy.Duration(LIFETIME) marker.type = Marker.LINE_LIST b, g, r = DETECTION_COLOR_MAP[types[track_id]] 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) text_marker = Marker() text_marker.header.frame_id = FRAME_ID text_marker.header.stamp = rospy.Time.now() text_marker.id = track_id + 1000 #avoid rpeat same id 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_ids[track_id]) text_marker.scale.x = 1 text_marker.scale.y = 1 text_marker.scale.z = 1 b, g, r = DETECTION_COLOR_MAP[types[track_id]] 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) marker = Marker() marker.header.frame_id = FRAME_ID marker.header.stamp = rospy.Time.now() marker.id = -1 marker.action = Marker.ADD marker.lifetime = rospy.Duration(LIFETIME) marker.type = Marker.LINE_LIST marker.color.r = 0 marker.color.g = 1 marker.color.b = 1 marker.color.a = 1.0 marker.scale.x = 0.1 marker.points = [] for l in LINES: print(l) p1 = EGOCAR[l[0]] print(p1) marker.points.append(Point(p1[0], p1[1], p1[2])) p2 = EGOCAR[l[1]] print(p2) marker.points.append(Point(p2[0], p2[1], p2[2])) marker_array.markers.append(marker) text_marker = Marker() text_marker.header.frame_id = FRAME_ID text_marker.header.stamp = rospy.Time.now() text_marker.id = -2 text_marker.action = Marker.ADD text_marker.lifetime = rospy.Duration(LIFETIME) text_marker.type = Marker.TEXT_VIEW_FACING p4 = EGOCAR[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 = 'EGOCAR' text_marker.scale.x = 1 text_marker.scale.y = 1 text_marker.scale.z = 1 b, g, r = DETECTION_COLOR_MAP['Car'] 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)
def _create_visualization_marker(self, obj, obj_type):
# Text marker to display object name
text_marker = Marker()
text_marker.header.frame_id = "map"
text_marker.header.stamp = rospy.Time() # Time zero, always show
text_marker.ns = obj
text_marker.id = 0
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.action = Marker.ADD
text_marker.text = obj
text_marker.pose.orientation.x = 0.0
text_marker.pose.orientation.y = 0.0
text_marker.pose.orientation.z = 0.0
text_marker.pose.orientation.w = 1.0
text_marker.color.a = 1.0
text_marker.color.r = 1.0
text_marker.color.g = 1.0
text_marker.color.b = 1.0
text_marker.scale.x = 0.2
text_marker.scale.y = 0.2
text_marker.scale.z = 0.1
text_marker.lifetime = rospy.Time(3)
#Set position, depends on type
if obj_type in ["drone", "turtlebot"]:
text_marker.header.frame_id = "/%s/base_link" % obj if obj_type == "drone" else "/base_link"
text_marker.frame_locked = True
text_marker.pose.position.x = 0.0
text_marker.pose.position.y = 0.0
text_marker.pose.position.z = 0.5*text_marker.scale.z
elif obj_type == "person":
wp_pos = self.waypoint_positions[self.obj2loc[obj]]
text_marker.pose.position.x = wp_pos['x']
text_marker.pose.position.y = wp_pos['y']
text_marker.pose.position.z = 0.5*text_marker.scale.z
elif obj_type == "waypoint":
wp_pos = self.waypoint_positions[obj]
text_marker.pose.position.x = wp_pos['x']
text_marker.pose.position.y = wp_pos['y']
text_marker.pose.position.z = 0.5*text_marker.scale.z
text_marker.color.a = 0.2
#Early exit for types that only needs text
if obj_type == "turtlebot":
self.vis_markers[obj] = [text_marker]
return
# Colored marker
marker = deepcopy(text_marker)
marker.id = 1
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.1
marker.color.a = 0.8
if obj_type == "drone":
marker.type = Marker.SPHERE
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
elif obj_type == "box":
marker.type = Marker.CUBE
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
elif obj_type == "waypoint":
marker.type = Marker.CYLINDER
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
marker.scale.x = 0.4
marker.scale.y = 0.4
marker.scale.z = 0.01
marker.color.a = 0.2
elif obj_type == "person":
marker.type = Marker.CYLINDER
#Set color in update step
self.vis_markers[obj] = [text_marker,marker]
wpoints = []
while waypoints is None or start_flag == 0:
if waypoints is None:
print "no waypoints"
lock_aux_pointx = 0
waypoints = new_waypoints
for i in range(waypoints.shape[0]):
p = Point()
p.x = waypoints[i][0]
p.y = waypoints[i][1]
p.z = 0
wpoints.append(p)
marker = Marker()
marker.header.frame_id = "/odom"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.pose.orientation.w = 1
marker.points = wpoints
t = rospy.Duration()
marker.lifetime = t
marker.scale.x = 0.4
marker.scale.y = 0.4
marker.scale.z = 0.4
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
pub_marker.publish(marker)
def callback(bbox_data, laserScan_data):
nameArray = []
xMinArray = []
yMinArray = []
xMaxArray = []
yMaxArray = []
detected_Objects = [[]]
global id_stock
global markerArray
global subArray
i = 0
#Bounding box verisini kontrol el
if (bbox_data is not None):
for i in range(len(bbox_data.bounding_boxes)):
try:
tmp = bbox_data.bounding_boxes[i].Class
nameArray.append(bbox_data.bounding_boxes[i].Class)
xMinArray.append(bbox_data.bounding_boxes[i].xmin)
yMinArray.append(bbox_data.bounding_boxes[i].ymin)
xMaxArray.append(bbox_data.bounding_boxes[i].xmax)
yMaxArray.append(bbox_data.bounding_boxes[i].ymax)
i += 1
except:
print("hata")
objCount = i
counter = 0
#Laser scan verisinden obje uzakligini bul
for counter in range(objCount):
rangeMin = 380 + ((640 - xMaxArray[counter]) / 2)
rangeMax = 700 - (xMinArray[counter] / 2)
sum = 0
k = 0
'''
for j in range (int(round(rangeMax-rangeMin))):
sum += laserScan_data.ranges[rangeMin+j]
mean = sum/(rangeMax-rangeMin)
'''
midPoint = int(round((rangeMin + rangeMax) / 2))
mean = laserScan_data.ranges[midPoint]
detected_Objects.append([nameArray[counter], mean, midPoint])
i = 1
print("Bulunan obje sayisi: " + str(len(detected_Objects)))
print(detected_Objects)
print(" | Class " + " | Dist | " + "pos |")
while (i < len(detected_Objects)): #Bulunan objeleri yazdir
text = " "
text += str(i) + ".| "
text += str(detected_Objects[i][0])
text += " | "
text += "{:.2f}".format(detected_Objects[i][1])
text += "m | "
text += str(detected_Objects[i][2])
text += " | "
print(text)
#Marker olustur
marker = Marker()
marker.header.frame_id = "/robot/odom"
marker.header.stamp = rospy.get_rostime()
marker.id = id_stock[0]
id_stock.pop(0)
marker.ns = str(detected_Objects[i][0])
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 0.1
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.pose.orientation.w = 1.0
#Robotun global koordinatlarini bul
(trans, rot) = listener.lookupTransform('/robot/odom',
'/robot/base_link',
rospy.Time(0))
tX = trans[0]
tY = trans[1]
tZ = trans[2]
rX = rot[0]
rY = rot[1]
rZ = rot[2]
rW = rot[3]
quaternion = (rX, rY, rZ, rW)
euler = tf.transformations.euler_from_quaternion(quaternion)
print("Trans: " + "{:.2f}".format(tX) + " {:.2f}".format(tY) +
" {:.2f}".format(tZ))
print("Roll: " + "{:.2f}".format(degrees(euler[0])) + " Pitch: " +
"{:.2f}".format(degrees(euler[1])) + " Yaw: " +
"{:.2f}".format(degrees(euler[2])))
#Kameranin merkezine gore obje acisini bul
if (detected_Objects[i][2] > 539):
degree = (float(detected_Objects[i][2]) -
539) * laserScan_data.angle_increment
else:
degree = (539 - float(
detected_Objects[i][2])) * laserScan_data.angle_increment
degree = degrees(degree)
#Global aciyi bul
if (detected_Objects[i][2] < 540):
degree = degree * (-1)
yaw = round(degrees(euler[2]), 2)
globalYaw = yaw + degree
print("ObjeDegree: " + str(degree) + " Yaw: " + str(yaw) +
" Global Yaw: " + str(globalYaw))
xAdd = detected_Objects[i][1] * cos(radians(globalYaw))
yAdd = detected_Objects[i][1] * sin(radians(globalYaw))
marker.pose.position.x = trans[0]
marker.pose.position.y = trans[1]
marker.pose.position.z = 0.2
marker.pose.position.x += xAdd
marker.pose.position.y += yAdd
j = 0
different = True
sameID = -1
pointIncRate = 0.1
pointDecRate = 0.03
#Bulunan obje daha once isaretlenmis mi diye kontrol et
while (j < (len(subArray.markers))):
if (subArray.markers[j].ns == str(detected_Objects[i][0])):
x1 = subArray.markers[j].pose.position.x
x2 = marker.pose.position.x
y1 = subArray.markers[j].pose.position.y
y2 = marker.pose.position.y
distanceBetween = sqrt(pow((x1 - x2), 2) + pow((y1 - y2), 2))
print("Distance between:" + str(distanceBetween))
if (distanceBetween != float("inf")):
if (subArray.markers[j].color.a < 1.0):
if (distanceBetween < 0.1):
print("Color.a " +
str(subArray.markers[j].color.a))
different = False
sameID = j
else:
if (distanceBetween < 1):
print("Color.a2 " +
str(subArray.markers[j].color.a))
different = False
sameID = j
j += 1
if (different):
#marker.id = id_stock[len(id_stock)-1]
#id_stock.pop(len(id_stock)-1)
subArray.markers.append(marker)
else:
id_stock.append(marker.id)
if (subArray.markers[sameID].color.a < 1.0):
subArray.markers[sameID].color.a += pointIncRate
if (subArray.markers[sameID].color.a >= 1.0):
#subArray.markers[sameID].text= str(detected_Objects[i][0])
tmpMarker = Marker()
tmpMarker.header.frame_id = "/robot/odom"
tmpMarker.header.stamp = rospy.get_rostime()
tmpMarker.id = subArray.markers[sameID].id
tmpMarker.ns = "textmarker"
tmpMarker.type = marker.TEXT_VIEW_FACING
tmpMarker.action = marker.ADD
tmpMarker.scale.x = 0.2
tmpMarker.scale.y = 0.2
tmpMarker.scale.z = 0.2
tmpMarker.color.a = 1.0
tmpMarker.color.r = 0.0
tmpMarker.color.g = 0.0
tmpMarker.color.b = 1.0
#print("MarkerPoseZ: " + str(subArray.markers[sameID].pose.position.z) + " TextPoseZ: " + str(tmpMarker.pose.position.z))
tmpMarker.pose.orientation.w = 1.0
tmpMarker.pose.position.x = subArray.markers[
sameID].pose.position.x
tmpMarker.pose.position.y = subArray.markers[
sameID].pose.position.y
tmpMarker.pose.position.z = subArray.markers[
sameID].pose.position.z
tmpMarker.pose.position.z += 0.2
if (str(subArray.markers[sameID].ns) == "QR"):
tmpMarker.text = "QR_"
print("i: " + str(i))
tmp_xMin = xMinArray[i - 1]
tmp_yMin = yMinArray[i - 1]
tmp_xMax = xMaxArray[i - 1]
tmp_yMax = yMaxArray[i - 1]
img = rospy.wait_for_message(
"/robot/camera/rgb/image_raw", Image)
cv2_img = bridge.imgmsg_to_cv2(img, "bgr8")
#cv2.imshow("asf",cv2_img)
#time.sleep(1)
#time.sleep(10)
#print("Xmin: " +str(tmp_xMin)+ " yMin: " +str(tmp_yMin)+ " tmp_xMax: " +str(tmp_xMax)+ " tmp_yMax: "+str(tmp_yMax))
if (tmp_xMin > 20):
tmp_xMin -= 20
if (tmp_yMin > 20):
tmp_yMin -= 20
if (tmp_xMax < 620):
tmp_xMax += 20
if (tmp_yMax < 460):
tmp_yMax += 20
#print("Xmin: " +str(tmp_xMin)+ " yMin: " +str(tmp_yMin)+ " tmp_xMax: " +str(tmp_xMax)+ " tmp_yMax: "+str(tmp_yMax))
#print("tmp_yMin: "+ str(tmp_yMin) + " tmp_yMax-tmp_yMin: "+ str(tmp_yMax-tmp_yMin) + "tmp_xMin: "+ str(tmp_xMin) + " tmp_xMax-tmp_xMin" + str(tmp_xMax-tmp_xMin))
cv2_img = cv2_img[tmp_yMin:tmp_yMax, tmp_xMin:tmp_xMax]
#qr = qrtools.QR()
#qr.decode(cv2_img)
qrData = pyzbar.decode(cv2_img)
#print("QR data:" + str(qrData[0][0]))
#time.sleep(10)
#print(cv2_img)
#cv2.imshow("QRimg",cv2_img)
#cv2.waitKey(1)
#img = rospy.Subscriber("/robot/camera/rgb/image_raw", Image, img_callback)
#print("QR hata")
#cv2.imshow(img)
#cv2.waitKey(1)
#time.sleep(10)
if (qrData):
subArray.markers[sameID].color.a = 1.0
subArray.markers[sameID].color.r = 0.0
subArray.markers[sameID].color.g = 1.0
tmpMarker.text += str(qrData[0][0])
subArray.markers.append(tmpMarker)
#markerArray.markers.append(tmpMarker)
print("qr okundu")
else:
subArray.markers[sameID].color.a = 0.2
print("qr okunamadi")
#time.sleep(10)
#time.sleep(10)
else:
subArray.markers[sameID].color.a = 1.0
subArray.markers[sameID].color.r = 0.0
subArray.markers[sameID].color.g = 1.0
tmpMarker.text = subArray.markers[sameID].ns
subArray.markers.append(tmpMarker)
#markerArray.markers.append(tmpMarker)
#print("MarkerPoseZ: " + str(subArray.markers[sameID].pose.position.z) + " TextPoseZ: " + str(tmpMarker.pose.position.z))
#time.sleep(10)
#markerArray.markers.append(subArray.markers[sameID])
print(subArray.markers[sameID])
j = 0
while (j < (len(subArray.markers))):
if (subArray.markers[j].ns == str(
detected_Objects[i][0])):
x1 = subArray.markers[j].pose.position.x
x2 = marker.pose.position.x
y1 = subArray.markers[j].pose.position.y
y2 = marker.pose.position.y
distanceBetween = sqrt(
pow((x1 - x2), 2) + pow((y1 - y2), 2))
if (distanceBetween != float("inf")):
if ((subArray.markers[j].color.a < 1.0)
and (distanceBetween < 1)):
subArray.markers[j].color.a = 0.0
j += 1
i += 1
print("\n")
j = 0
deleteArray = []
marker_pub.publish(subArray)
while j < (len(subArray.markers)):
if ((subArray.markers[j].ns == "QR")
and (subArray.markers[j].color.a >= 1.0)):
print("lensub" + str(len(subArray.markers)) + " id: " +
str(subArray.markers[j].id) + " NS: " +
str(subArray.markers[j].ns) + " text:" +
str(subArray.markers[j].text) + " color.a: " +
str(subArray.markers[j].color.a))
if (subArray.markers[j].color.a < 1.0):
print("dec")
print("lensub" + str(len(subArray.markers)) + " id: " +
str(subArray.markers[j].id) + " NS: " +
str(subArray.markers[j].ns) + " text:" +
str(subArray.markers[j].text) + " color.a: " +
str(subArray.markers[j].color.a))
if (subArray.markers[j] > 0.0):
subArray.markers[j].color.a -= pointDecRate
if (subArray.markers[j].color.a <= 0.0):
deleteArray.append(subArray.markers[j])
j += 1
if (deleteArray is not None):
deleteArray.sort(reverse=True)
for j in range(len(deleteArray)):
id_stock.append(deleteArray[j].id)
subArray.markers.remove(deleteArray[j])
#marker_pub.publish(subArray)
#marker_pub.publish(markerArray)
print("Marker sayisi: " + str(len(markerArray.markers)))
print("id_stock: " + str(len(id_stock)))
def update(self, x_pos, y_pos, yaw):
global waypoints, waypoint_index, station_keep_flag, stable, lock, lock_aux_pointx, lock_aux_pointy
self.pos_SetPointx = waypoints[waypoint_index][0]
self.pos_SetPointy = waypoints[waypoint_index][1]
# get waypoint index
last_waypoint_index = waypoint_index
if np.sqrt((waypoints[waypoint_index][0] - x_pos) *
(waypoints[waypoint_index][0] - x_pos) +
(waypoints[waypoint_index][1] - y_pos) *
(waypoints[waypoint_index][1] - y_pos)) < 5:
if waypoint_index == waypoints.shape[0] - 1:
waypoint_index = waypoint_index
else:
waypoint_index = waypoint_index + 1
if waypoint_index == waypoints.shape[0] - 1 and np.sqrt(
(waypoints[waypoint_index][0] - x_pos) *
(waypoints[waypoint_index][0] - x_pos) +
(waypoints[waypoint_index][1] - y_pos) *
(waypoints[waypoint_index][1] - y_pos)) < 5:
station_keep_flag = 1
else:
station_keep_flag = 0
if time.time() - self.wait_start > 10:
self.wait_flag = 0
#print "delta t", time.time() - self.wait_start
pos_error = np.sqrt((self.pos_SetPointx - x_pos) *
(self.pos_SetPointx - x_pos) +
(self.pos_SetPointy - y_pos) *
(self.pos_SetPointy - y_pos))
waypoint_error = np.sqrt((waypoints[waypoint_index][0] - x_pos) *
(waypoints[waypoint_index][0] - x_pos) +
(waypoints[waypoint_index][1] - y_pos) *
(waypoints[waypoint_index][1] - y_pos))
if np.abs(
waypoint_error) < 5 and station_keep_flag == 1 and stable == 1:
print "turn to desired yaw"
# lock yaw
if waypoints[waypoint_index][2] != -10:
error = waypoints[waypoint_index][2] - yaw
else:
error = np.arctan2((waypoints[waypoint_index][1] -
waypoints[waypoint_index - 1][1]),
(waypoints[waypoint_index][0] -
waypoints[waypoint_index - 1][0])) - yaw
else:
error = np.arctan2((self.SetPointy - y_pos),
(self.SetPointx - x_pos)) - yaw
if waypoint_index >= 1:
last_waypointx = waypoints[waypoint_index - 1][0]
last_waypointy = waypoints[waypoint_index - 1][1]
else:
if lock_aux_pointx == 0:
lock_aux_pointx = x_pos
lock_aux_pointy = y_pos
last_waypointx = lock_aux_pointx
last_waypointy = lock_aux_pointy
if np.abs(waypoints[waypoint_index][0] - last_waypointx) < 1:
dist_to_line = np.abs(x_pos - last_waypointx)
tmp_x = last_waypointx
tmp_y = y_pos
dir_vectorx = 0
dir_vectory = 1
else:
line_slope = (waypoints[waypoint_index][1] - last_waypointy) / (
waypoints[waypoint_index][0] - last_waypointx)
line_intercept = waypoints[waypoint_index][
1] - line_slope * waypoints[waypoint_index][0]
dist_to_line = np.abs(line_slope * x_pos + line_intercept -
y_pos) / np.sqrt(line_slope * line_slope + 1)
tmp_x = x_pos + (line_slope / np.sqrt(line_slope * line_slope + 1)
) * dist_to_line
tmp_y = y_pos - dist_to_line
#print line_slope, line_intercept, dist_to_line
if np.abs(line_slope * tmp_x + line_intercept - tmp_y) / np.sqrt(
line_slope * line_slope + 1) > dist_to_line:
tmp_x = x_pos - (line_slope / np.sqrt(line_slope * line_slope +
1)) * dist_to_line
tmp_y = y_pos + dist_to_line
dir_vectorx = 1 / np.sqrt(line_slope * line_slope + 1)
dir_vectory = line_slope / np.sqrt(line_slope * line_slope + 1)
#print dir_vectorx, dir_vectory
#print dist_to_line
proc_dist = np.sqrt(5 * 5 - dist_to_line * dist_to_line)
aux_pointx = tmp_x + dir_vectorx * proc_dist
aux_point3x = tmp_x + 3 * dir_vectorx * proc_dist
aux_pointy = tmp_y + dir_vectory * proc_dist
aux_point3y = tmp_y + 3 * dir_vectory * proc_dist
if np.abs((waypoints[waypoint_index][0] - tmp_x) *
(waypoints[waypoint_index][0] - tmp_x) +
(waypoints[waypoint_index][1] - tmp_y) *
(waypoints[waypoint_index][1] - tmp_y)) < np.abs(
(waypoints[waypoint_index][0] - aux_pointx) *
(waypoints[waypoint_index][0] - aux_pointx) +
(waypoints[waypoint_index][1] - aux_pointy) *
(waypoints[waypoint_index][1] - aux_pointy)):
aux_pointx = tmp_x - dir_vectorx * proc_dist
aux_point3x = tmp_x - 3 * dir_vectorx * proc_dist
aux_pointy = tmp_y - dir_vectory * proc_dist
aux_point3y = tmp_y - 3 * dir_vectory * proc_dist
#print np.sqrt((tmp_x - x_pos)*(tmp_x - x_pos)+(tmp_y - y_pos)*(tmp_y - y_pos))
if np.sqrt((tmp_x - x_pos) * (tmp_x - x_pos) + (tmp_y - y_pos) *
(tmp_y - y_pos)) < 5:
if station_keep_flag == 1 and pos_error < 5:
if lock == 0:
self.SetPointx = aux_point3x
self.SetPointy = aux_point3y
aux_pointx = aux_point3x
aux_pointy = aux_point3y
lock = 1
aux_pointx = self.SetPointx
aux_pointy = self.SetPointy
else:
self.SetPointx = aux_pointx
self.SetPointy = aux_pointy
lock = 0
#print "aux", aux_pointx, aux_pointy
wpoints = []
for i in range(1):
p = Point()
p.x = aux_pointx
p.y = aux_pointy
p.z = 0
wpoints.append(p)
marker = Marker()
marker.header.frame_id = "/odom"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.pose.orientation.w = 1
marker.points = wpoints
t = rospy.Duration()
marker.lifetime = t
marker.scale.x = 0.4
marker.scale.y = 0.4
marker.scale.z = 0.4
marker.color.a = 1.0
marker.color.r = 0
marker.color.g = 0
marker.color.b = 1.0
self.pub_aux.publish(marker)
else:
lock = 0
if station_keep_flag == 1:
self.SetPointx = waypoints[waypoint_index][0]
self.SetPointy = waypoints[waypoint_index][1]
tmp_x = waypoints[waypoint_index][0]
tmp_y = waypoints[waypoint_index][1]
else:
if (last_waypointx - x_pos) * (
waypoints[waypoint_index][0] -
last_waypointx) + (last_waypointy - y_pos) * (
waypoints[waypoint_index][1] - last_waypointy) > 0:
self.SetPointx = tmp_x
self.SetPointy = tmp_y
else:
self.SetPointx = waypoints[waypoint_index][0]
self.SetPointy = waypoints[waypoint_index][1]
print "tmp", tmp_x, tmp_y
wpoints = []
for i in range(1):
p = Point()
p.x = tmp_x
p.y = tmp_y
p.z = 0
wpoints.append(p)
marker = Marker()
marker.header.frame_id = "/odom"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.pose.orientation.w = 1
marker.points = wpoints
t = rospy.Duration()
marker.lifetime = t
marker.scale.x = 0.4
marker.scale.y = 0.4
marker.scale.z = 0.4
marker.color.a = 1.0
marker.color.r = 0
marker.color.g = 1.0
marker.color.b = 0
self.pub_aux.publish(marker)
wpoints = []
for i in range(1):
p = Point()
p.x = waypoints[waypoint_index][0]
p.y = waypoints[waypoint_index][1]
p.z = 0
wpoints.append(p)
marker2 = Marker()
marker2.header.frame_id = "/odom"
marker2.type = marker.POINTS
marker2.action = marker.ADD
marker2.pose.orientation.w = 1
marker2.points = wpoints
t = rospy.Duration()
marker2.lifetime = t
marker2.scale.x = 0.4
marker2.scale.y = 0.4
marker2.scale.z = 0.4
marker2.color.a = 1.0
marker2.color.r = 0.5
marker2.color.g = 0
marker2.color.b = 0.5
self.pub_tgwp.publish(marker2)
wpoints = []
for i in range(1):
p = Point()
p.x = last_waypointx
p.y = last_waypointy
p.z = 0
wpoints.append(p)
marker3 = Marker()
marker3.header.frame_id = "/odom"
marker3.type = marker.POINTS
marker3.action = marker.ADD
marker3.pose.orientation.w = 1
marker3.points = wpoints
t = rospy.Duration()
marker3.lifetime = t
marker3.scale.x = 0.4
marker3.scale.y = 0.4
marker3.scale.z = 0.4
marker3.color.a = 1.0
marker3.color.r = 0
marker3.color.g = 0.5
marker3.color.b = 0.5
self.pub_lwp.publish(marker3)
'''
#print np.abs(waypoint_error)
if np.abs(waypoint_error) < 5 and waypoints is not None:
if waypoint_index < waypoints.shape[0]-1:
if self.wait_flag == 0 and station_keep_flag == 0:
waypoint_index = waypoint_index + 1
self.wait_flag = 1
self.wait_start = time.time()
print "target waypoint", waypoint_index
#print waypoint_index, waypoints.shape[0]
else:
station_keep_flag = 1
print "station keep in last waypoint"
'''
#print "error", np.arctan((self.pos_SetPointy - y_pos)/(self.pos_SetPointx - x_pos)) - yaw
if np.abs(error) > np.pi:
if error > 0:
error = error - 2 * np.pi
else:
error = error + 2 * np.pi
if station_keep_flag == 1 and np.abs(waypoint_error) > 5:
if np.abs(error) > np.pi / 2:
if error < 0:
error = error + np.pi
else:
error = error - np.pi
#print "error", error
#print "yaw", yaw
#print "target", np.arctan2((self.pos_SetPointy - y_pos), (self.pos_SetPointx - x_pos))
self.current_time = time.time()
delta_time = self.current_time - self.last_time
delta_error = error - self.last_error
if (delta_time >= self.sample_time):
self.PTerm = self.Kp * error
self.ITerm += error * delta_time
if (self.ITerm < -self.windup_guard):
self.ITerm = -self.windup_guard
elif (self.ITerm > self.windup_guard):
self.ITerm = self.windup_guard
self.DTerm = 0.0
if delta_time > 0:
self.DTerm = delta_error / delta_time
self.last_time = self.current_time
self.last_error = error
self.output = self.PTerm + (self.Ki * self.ITerm) + (self.Kd *
self.DTerm)
pic[i] = cv2.resize(pic[i], (pic_resize, pic_resize),
interpolation=cv2.INTER_AREA)
kp[i], des[i] = sift.detectAndCompute(pic[i], None)
#kp_f[i], des_f[i] = sift.detectAndCompute(pic_f[i],None)
voting.append(0)
voting2.append(0)
published.append(False)
# BFMatcher with default params
bf = cv2.BFMatcher()
pub = rospy.Publisher('visualization_marker', Marker, queue_size=100)
marker_arr = []
for i in range(0, pic_len):
marker = Marker()
marker.header.frame_id = "camera_link"
marker.type = marker.TEXT_VIEW_FACING
marker.action = marker.ADD
marker.scale.x = 0.5
marker.scale.y = 0.5
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.pose.orientation.w = 1.0
marker.text = name[i]
marker.id = i
marker_arr.append(marker)
def pub_marking(img, name_id): #Camera resectioning & make marker
from sailboat_message.msg import WTST_msg
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped, Point
from visualization_msgs.msg import Marker
## fleet race
# target_x = [0, 58.7091157621, 64.8362530808, 250.098605093, 213.669988671]
# target_y = [0, 115.264402922, 91.6514720561, 112.199262859, 207.218604858], e5: 366.770497586
## station_keeping
target_x = [0, -151.061785525]
target_y = [0, 366.770497586]
path = Path()
marker = Marker()
marker.header.frame_id = 'WTST'
marker.type = marker.POINTS
marker.action = marker.ADD
marker.pose.orientation.w = 1
marker.scale.x = 5.0
marker.scale.y = 5.0
marker.scale.z = 5.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0
marker.color.a = 0.5
for xx, yy in zip(target_x, target_y):
p = Point()
p.x = xx
p.y = yy
marker.points.append(p)
def plan_path(self, start_point, end_point, map_obj):
"""
RRT*. Tries to find a collision free path from start to goal.
"""
# STUFF FOR TESTING
self.trajectory.clear()
if self.enable_vis:
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.scale.x = 0.1
marker.scale.y = 0.1
self.vis_pub.publish(marker)
exploration_bias = 1.0 - self.goal_bias
final_node = None
num_existing_path_points_added = 0
self.rrt_star = RRTStar(Node(start_point))
self.max_iterations = self.rrt_star.max_size
while self.rrt_star.size <= self.max_iterations:
p = np.random.uniform()
if p < exploration_bias:
x_rand = self.map.sample_free_space()
else:
if final_node is None:
x_rand = end_point
else:
x_rand = self.branched_from_existing_path(
final_node,
depth_underestimate=num_existing_path_points_added)
num_existing_path_points_added += 1
x_nearest = self.rrt_star.nearest(
x_rand) # Find the nearest node to x_rand
path = self.map.generate_line_path(x_nearest.value,
x_rand,
eta=self.eta)
if path is not None: # no obstacles between x_nearest and x_rand
x_new = path[-1]
X_nearby_connectable = self.find_nearby_connectable(
x_nearest, x_new)
cost_min, node_min = self.find_best_parent(
X_nearby_connectable, x_new)
X_nearby_connectable.remove(
node_min
) # Remove x_new's parent node from the list of nearby nodes so it is not considered for rewiring
# Create the new node at x_new!
node_new = self.rrt_star.add_config(node_min, x_new)
if self.enable_vis:
# FOR TESTING ONLY #
# Code to publish marker for new node
###########################################################################################
TEMP = Point()
TEMP.x = x_new[0]
TEMP.y = x_new[1]
TEMP.z = .05
marker.points.append(TEMP)
TEMP = ColorRGBA()
TEMP.r = 1
TEMP.g = 0
TEMP.b = 0
TEMP.a = 1
marker.colors.append(TEMP)
self.vis_pub.publish(marker)
###########################################################################################
self.rewire(cost_min, node_new, X_nearby_connectable)
if np.allclose(node_new.value, end_point, .05, 0) and (
final_node is
None): #np.array_equal(node_new.value, end_point):
final_node = node_new
# reduce exploration bias so that we reinforce the existing path
exploration_bias = .5
if VERBOSE:
print("Path found!!!!")
print(final_node.cost)
if rospy.get_time() - self.start_time > self.time_thresh:
if VERBOSE:
print(self.rrt_star.size)
break
if final_node is not None:
if self.enable_vis:
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.points = []
marker.colors = []
def recur(node):
if self.enable_vis:
TEMP = Point()
TEMP.x = node.value[0]
TEMP.y = node.value[1]
TEMP.z = .05
marker.points.append(TEMP)
TEMP = ColorRGBA()
TEMP.r = 1
TEMP.g = 0
TEMP.b = 0
TEMP.a = 1
marker.colors.append(TEMP)
self.trajectory.points.append([node.value[0], node.value[1]])
parent = node.parent
if parent is not None:
recur(parent)
recur(final_node)
self.trajectory.points.reverse()
if self.enable_vis:
self.vis_pub.publish(marker)
if VERBOSE:
print(final_node.depth)
else:
# TODO:give best guess- find closest node to goal
if VERBOSE:
print("No path found! Please try again.")
counter = 0
for irow in blue:
icone = Cone()
icone.posx = irow[0]
icone.posy = irow[1]
icone.color = 0
conesmsg.cones.append(icone)
imarker = Marker()
imarker.pose.position.x = irow[0]
imarker.pose.position.y = irow[1]
imarker.header.frame_id = 'map'
imarker.id = counter
imarker.scale.x = .5
imarker.scale.y = .5
imarker.scale.z = .5
imarker.type = 3
imarker.color.a = 1.0
imarker.color.b = 1.0
imarker.lifetime.secs = 10
markersmsg.markers.append(imarker)
counter = counter + 1
# at this point, have all blue markers /cones ready to publish
for irow in yellow:
icone = Cone()
icone.posx = irow[0]
icone.posy = irow[1]
icone.color = 1
conesmsg.cones.append(icone)
imarker = Marker()
imarker.pose.position.x = irow[0]
def callback(msg):
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
qx = msg.pose.pose.orientation.x
qy = msg.pose.pose.orientation.y
qz = msg.pose.pose.orientation.z
qw = msg.pose.pose.orientation.w
pose_quaternion = np.array([qx, qy, qz, qw])
yaw = tf.transformations.euler_from_quaternion(pose_quaternion)[2]
# 2. Find the path point closest to the vehicle that is >= 1 lookahead distance from vehicle's current location.
global LOOKAHEAD_DISTANCE
goal = find_goal_point(x, y, yaw, LOOKAHEAD_DISTANCE)
goal_point = path_points[goal]
previous_goal = goal
# # 3. Transform the goal point to vehicle coordinates.
goal_pose_msg, goal_pose = transform_point(goal_point)
# 4. Calculate the curvature = 1/r = 2x/l^2
# The curvature is transformed into steering wheel angle and published to the 'drive_param' topic.
abs_y = abs(goal_pose.pose.position.y)
curvature = ((2.0 * abs_y) / (LOOKAHEAD_DISTANCE**2))
angle = np.arctan(LOOKAHEAD_DISTANCE * curvature)
if (goal_pose.pose.position.y < 0):
angle = -angle #Right Steering
else:
angle = angle #Left Steering
angle = np.clip(
angle, -0.4189, 0.4189
) # 0.4189 radians = 24 degrees because car can only turn 24 degrees max
# clipping speeds and Lookahead
degree_angle = math.degrees(angle)
if abs(degree_angle) < ANGLE_LEVEL_1:
vel = SPEED_LEVEL_1
LOOKAHEAD_DISTANCE = 2
elif ANGLE_LEVEL_1 <= abs(degree_angle) and abs(
degree_angle) < ANGLE_LEVEL_2:
vel = SPEED_LEVEL_2
LOOKAHEAD_DISTANCE = 1.5
else:
vel = SPEED_LEVEL_3
LOOKAHEAD_DISTANCE = 1.0
msg = drive_param()
msg.velocity = vel
msg.angle = angle
pub.publish(msg)
goal_pub.publish(goal_pose_msg)
lookahead_marker = Marker()
lookahead_marker.type = Marker.TEXT_VIEW_FACING
lookahead_marker.header.frame_id = '/map'
lookahead_marker.scale.z = 0.5
lookahead_marker.color.a = 1
lookahead_marker.color.r = 1.0
lookahead_marker.color.g = 0.0
lookahead_marker.color.b = 0.0
lookahead_marker.pose.position.x = 0.0
lookahead_marker.pose.position.y = 4.0
rospy.set_param('LOOKAHEAD_DISTANCE', LOOKAHEAD_DISTANCE)
multiline_str = 'LOOKAHEAD_DISTANCE: %s' % str(
LOOKAHEAD_DISTANCE) + ' \n' + 'VELOCITY: %s' % str(vel)
lookahead_marker.text = multiline_str
marker_pub.publish(lookahead_marker)
def initPathMarkers():
# cannot write in one equation!!!
sourcePoint = Marker()
goalPoint = Marker()
neighbourPoint = Marker()
finalPath = Marker()
sourcePoint.header.frame_id = 'path_planner'
goalPoint.header.frame_id = 'path_planner'
neighbourPoint.header.frame_id = 'path_planner'
finalPath.header.frame_id = 'path_planner'
sourcePoint.header.stamp = rospy.get_rostime()
goalPoint.header.stamp = rospy.get_rostime()
neighbourPoint.header.stamp = rospy.get_rostime()
finalPath.header.stamp = rospy.get_rostime()
sourcePoint.ns = "path_planner"
goalPoint.ns = "path_planner"
neighbourPoint.ns = "path_planner"
finalPath.ns = "path_planner"
sourcePoint.action = 0 # add/modify an object
goalPoint.action = 0
neighbourPoint.action = 0
finalPath.action = 0
sourcePoint.id = 0
goalPoint.id = 1
neighbourPoint.id = 2
finalPath.id = 3
# sourcePoint.text = 'sourcePoint'
# goalPoint.text = 'goalPoint'
# neighbourPoint.text = 'neighbourPoint'
# finalPath.text = 'finalPath'
sourcePoint.type = 2 # Sphere
goalPoint.type = 2
neighbourPoint.type = 8 # Points
finalPath.type = 4 # Line Strip
sourcePoint.pose.orientation.w = 1.0
goalPoint.pose.orientation.w = 1.0
neighbourPoint.pose.orientation.w = 1.0
finalPath.pose.orientation.w = 1.0
sourcePoint.pose.position.x = 0.0
sourcePoint.pose.position.y = 0.0
sourcePoint.pose.position.z = 0.0
goalPoint.pose.position.x = 10.0
goalPoint.pose.position.y = 10.0
goalPoint.pose.position.z = 0.0
neighbourPoint.pose.position.x = 0.0
neighbourPoint.pose.position.y = 0.0
neighbourPoint.pose.position.z = 0.0
sourcePoint.scale.x = sourcePoint.scale.y = sourcePoint.scale.z = 1.0
goalPoint.scale.x = goalPoint.scale.y = goalPoint.scale.z = 1.0
neighbourPoint.scale.x = neighbourPoint.scale.y = neighbourPoint.scale.z = 0.1
finalPath.scale.x = 0.5 # scale.x controls the width of the line segments
sourcePoint.color.g = 1.0
goalPoint.color.r = 1.0
neighbourPoint.color.r = 0.8
neighbourPoint.color.g = 0.4
finalPath.color.r = 0.2
finalPath.color.g = 0.2
finalPath.color.b = 1.0
sourcePoint.color.a = 1.0
goalPoint.color.a = 1.0
neighbourPoint.color.a = 0.5
finalPath.color.a = 1.0
return (sourcePoint, goalPoint, neighbourPoint, finalPath)
def compute_optimal_diffdrive_action(self,curr_state, currently_executing_action, step):
x_index = self.x_index
y_index = self.y_index
yaw_index = self.yaw_index
move_to_next= 0
curr_seg = int (np.copy(self.curr_line_segment))
moved_to_next = 0
#array of "the point"... for each sim
pt = np.copy(curr_state) # N x state
#arrays of line segment points... for each sim
curr_start = self.desired_states[curr_seg]
curr_end = self.desired_states[curr_seg+1]
next_start = self.desired_states[curr_seg+1]
next_end = self.desired_states[curr_seg+2]
############ closest distance from point to curr line segment
#vars
a = pt[x_index]- curr_start[0]
b = pt[y_index]- curr_start[1]
c = curr_end[0]- curr_start[0]
d = curr_end[1]- curr_start[1]
#project point onto line segment
which_line_section = np.divide((np.multiply(a,c) + np.multiply(b,d)), (np.multiply(c,c) + np.multiply(d,d)))
#point on line segment that's closest to the pt
closest_pt_x = np.copy(which_line_section)
closest_pt_y = np.copy(which_line_section)
if(which_line_section<0):
closest_pt_x=curr_start[0]
closest_pt_y=curr_start[1]
elif(which_line_section>1):
closest_pt_x=curr_end[0]
closest_pt_y=curr_end[1]
else:
closest_pt_x= curr_start[0] + np.multiply(which_line_section,c)
closest_pt_y= curr_start[1] + np.multiply(which_line_section,d)
#min dist from pt to that closest point (ie closest dist from pt to line segment)
min_perp_dist = np.sqrt((pt[x_index]-closest_pt_x)*(pt[x_index]-closest_pt_x) + (pt[y_index]-closest_pt_y)*(pt[y_index]-closest_pt_y))
#"forward-ness" of the pt... for each sim
curr_forward = which_line_section
############ closest distance from point to next line segment
#vars
a = pt[x_index]- next_start[0]
b = pt[y_index]- next_start[1]
c = next_end[0]- next_start[0]
d = next_end[1]- next_start[1]
#project point onto line segment
which_line_section = np.divide((np.multiply(a,c) + np.multiply(b,d)), (np.multiply(c,c) + np.multiply(d,d)))
#point on line segment that's closest to the pt
closest_pt_x = np.copy(which_line_section)
closest_pt_y = np.copy(which_line_section)
if(which_line_section<0):
closest_pt_x=next_start[0]
closest_pt_y=next_start[1]
elif(which_line_section>1):
closest_pt_x=next_end[0]
closest_pt_y=next_end[1]
else:
closest_pt_x= next_start[0] + np.multiply(which_line_section,c)
closest_pt_y= next_start[1] + np.multiply(which_line_section,d)
#min dist from pt to that closest point (ie closes dist from pt to line segment)
dist = np.sqrt((pt[x_index]-closest_pt_x)*(pt[x_index]-closest_pt_x) + (pt[y_index]-closest_pt_y)*(pt[y_index]-closest_pt_y))
#pick which line segment it's closest to, and update vars accordingly
if(dist<=min_perp_dist):
curr_seg += 1
moved_to_next = 1
curr_forward = which_line_section
min_perp_dist=dist
################## publish the desired traj
markerArray2 = MarkerArray()
marker_id=0
for des_pt_num in range(8): #5 for all, 8 for circle, 4 for zigzag
marker = Marker()
marker.id=marker_id
marker.header.frame_id = "/world"
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.15
marker.scale.y = 0.15
marker.scale.z = 0.15
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
marker.pose.position.x = self.desired_states[des_pt_num,0]
marker.pose.position.y = self.desired_states[des_pt_num,1]
marker.pose.position.z = 0
markerArray2.markers.append(marker)
marker_id+=1
self.publish_markers_desired.publish(markerArray2)
################## advance which line segment we are on
if(moved_to_next==1):
self.curr_line_segment+=1
print("**************************** MOVED ONTO NEXT LINE SEG")
################## given the current state, the perpendicular distance, and desired heading, calculate the velocities to output for each motor
#what side of the line segment is the point on
angle_to_point = np.arctan2(pt[y_index]-curr_start[1], pt[x_index]-curr_start[0])
angle_of_line = np.arctan2(curr_end[1]-curr_start[1], curr_end[0]-curr_start[0])
if(moving_distance(angle_of_line, angle_to_point)>0):
on_right = True
else:
on_right = False
#angle for moving robot toward desired heading
heading_turn_amount = moving_distance(angle_of_line, pt[yaw_index])
if(on_right):
heading_turn_amount += self.horiz*min_perp_dist
else:
heading_turn_amount -= self.horiz*min_perp_dist
#angle --> velocities
# + heading_turn_amount means ccw (toward left, so L slower and R faster)
#left_vel = self.nominal - heading_turn_amount*self.heading
#right_vel = self.nominal + heading_turn_amount*self.heading
angular_vel = heading_turn_amount*self.heading
#clip
'''if(left_vel>self.max_motor_gain):
left_vel=self.max_motor_gain
if(left_vel<self.min_motor_gain):
left_vel=self.min_motor_gain
if(right_vel>self.max_motor_gain):
right_vel=self.max_motor_gain
if(right_vel<self.min_motor_gain):
right_vel=self.min_motor_gain'''
if(angular_vel<self.min_motor_gain):
angular_vel=self.min_motor_gain
if(angular_vel>self.max_motor_gain):
angular_vel=self.max_motor_gain
action_to_take = angular_vel #########[left_vel*math.pow(2,16)*0.001, right_vel*math.pow(2,16)*0.001]
save_perp_dist= min_perp_dist
save_forward_dist= curr_forward
save_moved_to_next= moved_to_next
save_desired_heading= angle_of_line
save_curr_heading= pt[yaw_index]
return action_to_take, save_perp_dist, save_forward_dist, save_moved_to_next, save_desired_heading, save_curr_heading
def publish_rviz_marker(self,
stamp,
ros_publisher,
classes,
positions,
pos_covariances=None,
track_ids=None):
#remove all rviz markers before we publish the new ones
self.delete_all_markers(ros_publisher)
markers = MarkerArray()
for i in range(len(classes)):
cla = classes[i]
#setup marker
marker = Marker()
marker.header.stamp = stamp
marker.header.frame_id = positions[i]["frame_id"]
if track_ids is not None:
marker.id = track_ids[i]
else:
marker.id = i
marker.ns = "mobility_aids"
marker.type = Marker.SPHERE
marker.action = Marker.MODIFY
marker.lifetime = rospy.Duration()
#maker position
marker.pose.position.x = positions[i]["x"]
marker.pose.position.y = positions[i]["y"]
marker.pose.position.z = positions[i]["z"]
#marker color
color_box = self.viz_helper.colors_box[cla]
marker.color.b = float(color_box[2])
marker.color.g = float(color_box[1])
marker.color.r = float(color_box[0])
marker.color.a = 1.0
#get error ellipse
width, height, scale, angle = 0.5, 0.5, 0.5, 0.0
#if a pose covariance is given, like for tracking, plot ellipse marker
if pos_covariances is not None:
width, height, angle = Visualizer.get_error_ellipse(
pos_covariances[i])
angle = angle + np.pi / 2
scale = 0.1
quat = tf.transformations.quaternion_from_euler(0, 0, angle)
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.scale.x = height
marker.scale.y = width
marker.scale.z = scale
markers.markers.append(marker)
ros_publisher.publish(markers)
