def publishChecked(grid):
global ckd
print "publishing"
k=0
cells = GridCells()
cells.header.frame_id = 'map'
cells.cell_width = 0.3 # edit for grid size .3 for simple map
cells.cell_height = 0.3 # edit for grid size
for i in range(1,10): #height should be set to hieght of grid
for j in range(1,9): #height should be set to hieght of grid
#print k # used for debugging
if (grid[k] == 0):
point=Point()
point.x=j*.3+.32 # edit for grid size
point.y=i*.3-.15 # edit for grid size
point.z=0
cells.cells.append(point)
k=k+1
k=k+1
if (grid[k] == 0):
point=Point()
point.x=j*.3+.62 # edit for grid size
point.y=i*.3-.15 # edit for grid size
point.z=0
cells.cells.append(point)
#print cells # used for debugging
ckd.publish(cells)
def linkMap(): for i in range(0, height*width): currentPoint = Point() currentPoint.x = getX(i) currentPoint.y = getY(i) #print "I is %i, x is %i, y is %i" % (i, currentPoint.x, currentPoint.y) # try adding north if(isInMap(pointAbove(currentPoint))): myPoint = pointAbove(currentPoint) #print "My Point X: %i Y: %i calc Index: %i" % (myPoint.x, myPoint.y,getIndexFromPoint(myPoint.x,myPoint.y)) G[i].addAdjacent(getIndexFromPoint(myPoint.x,myPoint.y)) currentPoint.x = getX(i) currentPoint.y = getY(i) # try adding east if(isInMap(pointRight(currentPoint))): myPoint = pointRight(currentPoint) #print "My Point X: %i Y: %i calc Index: %i" % (myPoint.x, myPoint.y,getIndexFromPoint(myPoint.x,myPoint.y)) G[i].addAdjacent(getIndexFromPoint(myPoint.x,myPoint.y)) currentPoint.x = getX(i) currentPoint.y = getY(i) # try adding south if(isInMap(pointBelow(currentPoint))): myPoint = pointBelow(currentPoint) #print "My Point X: %i Y: %i calc Index: %i" % (myPoint.x, myPoint.y,getIndexFromPoint(myPoint.x,myPoint.y)) G[i].addAdjacent(getIndexFromPoint(myPoint.x,myPoint.y)) currentPoint.x = getX(i) currentPoint.y = getY(i) # try adding west if(isInMap(pointLeft(currentPoint))): myPoint = pointLeft(currentPoint) #print "My Point X: %i Y: %i calc Index: %i" % (myPoint.x, myPoint.y,getIndexFromPoint(myPoint.x,myPoint.y)) G[i].addAdjacent(getIndexFromPoint(myPoint.x,myPoint.y))
def construct(self, int_marker): x1 = self.node1.x y1 = self.node1.y z1 = self.node1.z x2 = self.node2.x y2 = self.node2.y z2 = self.node2.z color = (80, 80, 80) a_marker = Marker() a_marker.type = Marker.ARROW a_marker.scale.x = .05 a_marker.scale.y = .1 a_marker.scale.z = .1 (a_marker.color.r, a_marker.color.g, a_marker.color.b) = color a_marker.color.a = .5 start = Point() end = Point() start.x = self.node1.x start.y = self.node1.y start.z = self.node1.z end.x = self.node2.x end.y = self.node2.y end.z = self.node2.z a_marker.points.append(start) a_marker.points.append(end) a_control = InteractiveMarkerControl() a_control.always_visible = True a_control.markers.append( a_marker ) int_marker.controls.append(a_control) return(int_marker)
def frontier(fullmapGridExplored): global currentPoint smallestCostCell = Point() smallestCostCell.x = 10000 smallestCostCell.y = 10000 smallestCostCell.z=0 smallestDistance =10000 for i in range(0,rows): for j in range(0,columns): amount =0 if(fullmapGridExplored[i][j] > 0 and fullmapGridExplored[i][j]<9000): if(not i==0): if(fullmapGridExplored[i-1][j]==-1): amount= amount+1 if(not j==0): if(fullmapGridExplored[i][j-1]==-1): amount= amount+1 if(not i==columns): if(fullmapGridExplored[i+1][j]): amount= amount+1 if(not j==rows): if(fullmapGridExplored[i][j+1]): amount= amount+1 if(amount>0): pass #put cells in arrayOfFrontires for element in arrayOfFrontires:#create array of frontiers at some point if(heuristic(currentPoint,element)<smallestDistance): smallestCostCell.x = element.x smallestCostCell.y = element.y smallestDistance = heuristic(currentPoint,element) localNav(smallestCostCell,currentPoint) #publish instead
def pub_arrow(self, pos1, pos2, color, mag_force):
marker = Marker()
marker.header.frame_id = self.frame
marker.header.stamp = rospy.Time.now()
marker.ns = "basic_shapes"
marker.id = 99999
marker.type = Marker.ARROW
marker.action = Marker.ADD
pt1 = Point()
pt2 = Point()
pt1.x = pos1[0,0]
pt1.y = pos1[1,0]
pt1.z = pos1[2,0]
pt2.x = pos2[0,0]
pt2.y = pos2[1,0]
pt2.z = pos2[2,0]
marker.points.append(pt1)
marker.points.append(pt2)
marker.scale.x = 0.05
marker.scale.y = 0.1
#marker.scale.z = scale[2]
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.color.a = color[3]
marker.lifetime = rospy.Duration()
marker.text = mag_force
self.pub.publish(marker)
def callback_Versor(data): #print data m = marker_array.markers[markers_list[id_]] m.header.stamp = rospy.Time.now() m.action = 0 m.type = m.ARROW p0 = Point() p1 = Point() x = 0 y = 0 z = 0 s_x = data.data[0] s_y = data.data[1] s_z = data.data[2] p1.x = (x + 2 * s_x) * 0.2 p1.y = (y + 2 * s_y) * 0.2 p1.z = (z + 2 * s_z) * 0.2 p0.x = x p0.y = y p0.z = z m.points = [] m.points.append(p0) m.points.append(p1) m.scale.x = 0.02 m.scale.y = 0.05 m.scale.z = 0.08 m.color.r = 1 m.color.g = 1 m.color.b = 0 m.color.a = 0.6 m.lifetime = rospy.Duration(0) m.frame_locked = False
def offset_calc(data):
pub = rospy.Publisher('x_y_offseter', Point)
msg = Point()
if data.num_satellites < 3:
return None
global INITIAL_LATITUDE
global INITIAL_LONGITUDE
rospy.loginfo('INIT_LONG: ' + str(INITIAL_LONGITUDE) + ' INIT_LAT: ' + str(INITIAL_LATITUDE))
rospy.loginfo('data_long: ' + str(data.longitude) + ' data_lat: ' + str(data.latitude))
if INITIAL_LONGITUDE == data.longitude and INITIAL_LATITUDE == data.latitude:
msg.x = 0
msg.y = 0
else:
msg.y = haversine_distance(INITIAL_LONGITUDE, INITIAL_LATITUDE, INITIAL_LONGITUDE, data.latitude)
msg.x = haversine_distance(INITIAL_LONGITUDE, INITIAL_LATITUDE, data.longitude , INITIAL_LATITUDE)
if data.longitude < INITIAL_LONGITUDE:
msg.x = -msg.x
if data.latitude < INITIAL_LATITUDE:
msg.y = -msg.y
rospy.loginfo('Publishing to topic gps_data:\n' +
'x: ' + str(msg.x) + '\ny: ' + str(msg.y) + '\n\n')
pub.publish(msg)
def draw_line(self, quad, x, y, z, hash_val):
if not rospy.is_shutdown():
marker = Marker()
marker.header.frame_id = "/my_frame"
marker.lifetime = rospy.Duration(self.duration)
marker.type = marker.LINE_STRIP
marker.action = marker.ADD
marker.scale.x = 4
marker.color.b = 1.0
marker.color.a = 1.0
p1 = Point()
p1.x = quad.get_x()
p1.y = quad.get_y()
p1.z = quad.get_z()
p2 = Point()
p2.x = x
p2.y = y
p2.z = z
marker.points.append(p1)
marker.points.append(p2)
marker.pose.orientation.w = 1.0
marker.id = hash_val
self.markers.append(marker)
return self
def main():
global tfl
rospy.init_node('coordinate_publisher')
tfl = tf.TransformListener()
array_publisher = rospy.Publisher('all_positions', PointArray, queue_size=1)
rate = rospy.Rate(100)
while not rospy.is_shutdown():
if(fill_points(tfl)):
lo = Point()
ro = Point()
lp = Point()
rp = Point()
ho = Point()
rp.x = right_arm_point[0]
rp.y = right_arm_point[1]
rp.z = right_arm_point[2]
lp.x = left_arm_point[0]
lp.y = left_arm_point[1]
lp.z = left_arm_point[2]
ro.x = right_arm_origin[0]
ro.y = right_arm_origin[1]
ro.z = right_arm_origin[2]
lo.x = left_arm_origin[0]
lo.y = left_arm_origin[1]
lo.z = left_arm_origin[2]
ho.x = head_origin[0]
ho.y = head_origin[1]
ho.z = head_origin[2]
array_publisher.publish((lo, ro, lp, rp, ho))
rate.sleep()
def makeMarker(self, pos, vec, idNum=1, color=(1,0,0)):
m = Marker()
m.id = idNum
m.ns = "test"
m.header.frame_id = "/camera_rgb_optical_frame"
m.type = Marker.ARROW
m.action = Marker.ADD
markerPt1 = Point()
markerPt2 = Point()
markerPt1.x = pos[0];
markerPt1.y = pos[1];
markerPt1.z = pos[2];
markerPt2.x = markerPt1.x + vec[0];
markerPt2.y = markerPt1.y + vec[1];
markerPt2.z = markerPt1.z + vec[2];
m.points = [markerPt1, markerPt2]
m.scale.x = .1;
m.scale.y = .1;
m.scale.z = .1;
m.color.a = 1;
m.color.r = color[0];
m.color.g = color[1];
m.color.b = color[2];
#m.lifetime = rospy.Duration()
return m
def get_effective_point(data):
map_matrix=map_matrix_ranger(data)
#print map_matrix
clear_area,block_area=[],[]
width=data.info.width
height=data.info.height
resolution=data.info.resolution
map_origin=data.info.origin
clear=Point()
block=Point()
centremap_cell=map_center_cell(map_origin,resolution)
#print centremap_cell
for y in range(height):
for x in range(width):
if map_matrix[y][x]==0:
clear.x=(x-centremap_cell[0])*resolution
clear.y=(y-centremap_cell[1])*resolution + resolution
clear_area.append(clear)
if map_matrix[y][x]==100:
block.x=(x-centremap_cell[0])*resolution
block.y=(y-centremap_cell[1])*resolution + resolution
block_area.append(block)
#print block,'\n'
#print block_area
clear=Point()
block=Point()
#print 'block_area',len(block_area)
return [clear_area,block_area]
def getWaypoints(path): returnPath = list() point = Point() pointNode = path[0] point.x = getWorldPointFromIndex(pointNode).x point.y = getWorldPointFromIndex(pointNode).y point.z = 0 returnPath.append(point) for i,node in enumerate(path): currPoint = Point() currNode = path[i] currPoint.x = getWorldPointFromIndex(currNode).x currPoint.y = getWorldPointFromIndex(currNode).y currPoint.z = 0 if (i+1 < len(path)): nextPoint = Point() nextNode = path[i+1] nextPoint.x = getWorldPointFromIndex(nextNode).x nextPoint.y = getWorldPointFromIndex(nextNode).y nextPoint.z = 0 if(math.degrees(math.fabs(math.atan2(nextPoint.y-currPoint.y,nextPoint.x-nextPoint.y))) >= 10): returnPath.append(currPoint) else: returnPath.append(currPoint) pass #print "Point in Path: X: %f Y: %f" % (point.x, point.y) return returnPath
def addPolygon(self, points, linewidth=0.02):
self.oid = self.oid+1
self.name = "/polygon"+str(self.oid)
self.marker = Marker()
self.marker.id = self.oid
self.marker.ns = "/polygon"
self.marker.header.frame_id = self.name
self.marker.type = self.marker.LINE_STRIP
self.marker.action = self.marker.ADD
self.marker.scale.x = linewidth
self.marker.scale.y = 1
self.marker.scale.z = 1
self.marker.color.r = 1.0
self.marker.color.g = 0.0
self.marker.color.b = 0.0
self.marker.color.a = 1.0
self.marker.pose.orientation.w = 1.0
self.marker.pose.position.x = 0
self.marker.pose.position.y = 0
self.marker.pose.position.z = 0
self.marker.points = []
for p in points:
pt = Point()
pt.x = p[0]; pt.y = p[1]; pt.z = p[2]
self.marker.points.append(pt)
#connect last marker to first marker
pt = Point()
pt.x = points[0][0]; pt.y = points[0][1]; pt.z = points[0][2]
self.marker.points.append(pt)
self.markerArray.markers.append(self.marker)
def smoothPath(path): #takes the parsed path & tries to remove unecessary zigzags #TODO returnPath = list() for i,node in enumerate(path): averagePoint = Point() if(i+1 < len(path)): nextNode = path[i+1] nextNodeX = getWorldPointFromIndex(nextNode).x nextNodeY = getWorldPointFromIndex(nextNode).y currNode = path[i] currNodeX = getWorldPointFromIndex(currNode).x currNodeY = getWorldPointFromIndex(currNode).y if( not returnPath): averagePoint.x = (currNodeX+nextNodeX)/2 averagePoint.y = (currNodeY+nextNodeY)/2 averagePoint.z = 0#math.atan2(currNodeY-nextNodeY, currNodeX-nextNodeX) else: averagePoint.x = (returnPath[i-1].x+currNodeX)/2 averagePoint.y = (returnPath[i-1].y+currNodeY)/2 averagePoint.z = 0 #math.atan2(currNodeY-returnPath[i-1].y, currNodeX-returnPath[i-1].x) returnPath.append(averagePoint) #print "Average Point in Path: X: %f Y: %f" % (averagePoint.x, averagePoint.y) return returnPath
def publishCells(grid):
global wallpub
print "publishing"
k = 0
cells = GridCells()
cells.header.frame_id = "map"
cells.cell_width = 0.3 # edit for grid size .3 for simple map
cells.cell_height = 0.3 # edit for grid size
for i in range(1, height): # height should be set to hieght of grid
for j in range(1, width): # height should be set to hieght of grid
# print k # used for debugging
if grid[k] == 100:
point = Point()
point.x = j * 0.3 + 0.32 # edit for grid size
point.y = i * 0.3 - 0.15 # edit for grid size
point.z = 0
cells.cells.append(point)
k = k + 1
k = k + 1
if grid[k] == 100:
point = Point()
point.x = j * 0.3 + 0.62 # edit for grid size
point.y = i * 0.3 - 0.15 # edit for grid size
point.z = 0
cells.cells.append(point)
# print cells # used for debugging
wallpub.publish(cells)
def makeVector(end_point, start_point, color, i, frame): vector = Marker() vector.header.stamp = rospy.Time.now() vector.header.frame_id = frame vector.ns = 'vectors' vector.action = Marker.ADD vector.pose.orientation.w = 1.0 vector.type = Marker.ARROW vector.id = i vector.color.r = color[0] vector.color.b = color[1] vector.color.g = color[2] vector.color.a = color[3] vector.scale.x = .1 vector.scale.y = .2 p1 = Point() p1.x, p1.y, p1.z = start_point vector.points.append(p1) p2 = Point() p2.x, p2.y, p2.z = end_point vector.points.append(p2) return vector
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 point_to_msg(v):
p = Point()
if hasattr(v, 'x') and hasattr(v, 'y'):
p.x, p.y = v.x, v.y
elif isinstance(v, tuple) and len(v) == 2:
p.x, p.y = v
else:
raise TypeError('Argument is not a point-like object')
p.z = 0.0
return p
def setupStart(pos):
global startTheta
start = Point()
start.x = pos.pose.pose.position.x
start.y = pos.pose.pose.position.y
start.x = round((int)((start.x/resolution))*resolution,2) #rounds to even multiple of resolution
start.y = round((int)((start.y/resolution))*resolution,2) #rounds to even multiple of resolution
quat = pos.pose.pose.orientation
q = [quat.x, quat.y, quat.z, quat.w]
roll, pitch, yaw = euler_from_quaternion(q)
startTheta = yaw
def __init__(self): position1 = Point() position1.x = 1 position2 = Point() position2.x = 2 a = [position1, position2] position1 = Point() position1.x = 1 b = [position1] print set(a).intersection(set(b))
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 translatePoints(gridMap, start, goal): translatedStart = Point(start.x, start.y, 0) translatedGoal = Point(goal.x, goal.y, 0) translatedStart.x = int(round((translatedStart.x - gridMap.info.origin.position.x) * 10)) translatedStart.y = int(round((translatedStart.y - gridMap.info.origin.position.y) * 10)) translatedGoal.x = int(round((translatedGoal.x - gridMap.info.origin.position.x) * 10)) translatedGoal.y = int(round((translatedGoal.y - gridMap.info.origin.position.y) * 10)) return translatedStart, translatedGoal
def GoTorsoCB(self, data):
RobotPosition = self.motionProxy.getRobotPosition(False)
naoPos = Point()
naoPos.x = RobotPosition[0]
naoPos.y = RobotPosition[1]
naoPos.z = RobotPosition[2]
go = Point()
go.x = (naoPos.x - self.naoCalib.x) + (self.torsoDestination.x - self.torsoCalib.x)
go.y = (naoPos.y - self.naoCalib.y) + (self.torsoDestination.y - self.torsoCalib.y)
go.z = (naoPos.z - self.naoCalib.z) + (self.torsoDestination.z - self.torsoCalib.z)
self.motionProxy.walkTo(go.x, go.y, go.z)
def get_min_max_dist_for_height(self, height, start = 0.2, step = 0.001):
point = Point()
point.x = start
point.y = 0
point.z = height
while self.calc_joints_for_point(point) is not None:
point.x -= step
min_dist = point.x + step
point.x = start
while self.calc_joints_for_point(point) is not None:
point.x += step
max_dist = point.x - step
return min_dist, max_dist
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 getWorldPointFromIndex(index): point=Point() #print "GetX: %i" % getX(index) point.x=(getX(index)*resolution)+offsetX + (.5 * resolution) point.y=(getY(index)*resolution)+offsetY + (.5 * resolution) point.z=0 return point
def clear_octomap(self, center, width, height):
rospy.loginfo("Clearing octomap. The box is {}m (x) by {}m (y) around ({}, {})".format(width,height,self.start_pos[0], self.start_pos[1]))
min = Point()
max = Point()
min.x = center[0] - width/2.0
max.x = center[0] + width/2.0
min.y = center[1] - height/2.0
max.y = center[1] + height/2.0
# tweak Z to clear only certain parts of the map (only obstales, only lines, etc)
min.z = -5.0
max.z = 5.0
self.clear_octomap_service(min, max)
def cb_points3d(self, p):
for i in range(len(p.ids)):
mp = Point()
mp.x = p.x[i]
mp.y = p.y[i]
mp.z = p.z[i]
self.tracks3d[p.ids[i]].append( mp )
def reset_octomap(self):
rospy.loginfo("Resetting by clearing octomap.")
min = Point()
max = Point()
min.x = -100
max.x = 100
min.y = -50
max.y = 50
# tweak Z to clear only certain parts of the map (only obstales, only lines, etc)
min.z = -5.0
max.z = 5.0
self.clear_octomap_service(min, max)
def node():
print('usao u glavni')
global mapData, global1, global2, global3, globalmaps, current_frontier, all_frontiers, litraIndx, n_robots, namespace_init_count
rospy.init_node('filter', anonymous=False)
# fetching all parameters
map_topic = rospy.get_param('~map_topic', '/map')
threshold = rospy.get_param('~costmap_clearing_threshold', 50)
info_radius = rospy.get_param(
'~info_radius', 1
) #this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate
goals_topic = rospy.get_param('~goals_topic', '/detected_points')
transform_topic = rospy.get_param('~transform_topic',
'/transformed_points')
submap_topic = rospy.get_param('~submap_topic', '/used_submaps')
n_robots = rospy.get_param('~n_robots', 1)
namespace_list = rospy.get_param('~namespace_list', ['alpha'])
namespace_init_count = rospy.get_param('namespace_init_count', 1)
rateHz = rospy.get_param('~rate', 100)
litraIndx = len(namespace_list)
rate = rospy.Rate(rateHz)
#-------------------------------------------
rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack)
#---------------------------------------------------------------------------------------------------------------
for i in range(0, n_robots):
globalmaps.append(OccupancyGrid())
if len(namespace_list) > 0:
for i in range(0, len(namespace_list)):
print(namespace_list[i])
rospy.Subscriber(
namespace_list[i] + '/move_base/global_costmap/costmap',
OccupancyGrid, globalMap)
elif len(namespace_list) == 0:
rospy.Subscriber('/move_base/global_costmap/costmap', OccupancyGrid,
globalMap)
print('usao u glavni2')
#wait if map is not received yet
while (len(mapData.data) < 1):
pass
print('usao u glavni3')
#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):
pass
global_frame = "/" + mapData.header.frame_id
print('usao u glavni4')
tfLisn = tf.TransformListener()
rospy.sleep(2)
if len(namespace_list) > 0:
for i in range(0, len(namespace_list)):
tfLisn.waitForTransform(global_frame[1:],
namespace_list[i] + '/base_link',
rospy.Time(0), rospy.Duration(10.0))
elif len(namespace_list) == 0:
tfLisn.waitForTransform(global_frame[1:], '/base_link', rospy.Time(0),
rospy.Duration(10.0))
rospy.Subscriber(submap_topic, SubmapList, submapCallBack)
rospy.Subscriber(transform_topic, FrontierTF, transformCallBack)
#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)
filterpub2 = rospy.Publisher('filtered_struct', FrontierTF, queue_size=10)
rospy.loginfo("the map and global costmaps are received")
# wait if no frontier is received yet
while len(current_frontier) < 1:
pass
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():
#-------------------------------------------------------------------------
#SVE FRONTIERE PONOVNO TRANSFORMIRATI, U SVAKOM PROLAZU
my_frontiers = copy(current_frontier)
transform(my_frontiers)
transform(all_frontiers)
all_frontiers.extend(my_frontiers)
#print ('koje dobivam', all_frontiers)
all_centroids = copy(all_frontiers)
#print('centroidi su:', all_centroids[0])
#-------------------------------------------------------------------------
#clearing old frontiers
z = 0
#print ('duljina frontiera',len(all_frontiers))
for centroid in all_centroids:
cond = False
temppoint.point.x = centroid.projected.x
temppoint.point.y = centroid.projected.y
for i in range(0, len(namespace_list)):
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, [centroid.projected.x, centroid.projected.y],
info_radius * 0.5)) < 0.3):
all_centroids = list(delete(all_centroids, (z), axis=0))
z = z - 1
#print ('deleted centroid')
z += 1
print("resize frontiers from %d to %d" %
(len(all_frontiers), len(all_centroids)))
#-------------------------------------------------------------------------
#------------------------------------------------------------------------
#publishing
arraypoints.points = []
#arraystruct=[]
cnt = 0
for centroid in all_centroids:
tempPoint.x = centroid.projected.x
tempPoint.y = centroid.projected.y
arraypoints.points.append(copy(tempPoint))
#arraystruct.append(copy(centroid))
cnt += 1
filterpub.publish(arraypoints)
#filterpub2.publish(arraystruct)
print("Published transformed centroids:" + str(cnt))
#-------------------------------------------------------------------------
pp = []
for q in all_centroids:
p.x = q.projected.x
p.y = q.projected.y
pp.append(copy(p))
points_clust.points = pp
pub2.publish(points_clust)
print("delete old frontiers")
z = 0
for frontier in all_frontiers:
cond = False
temppoint.point.x = frontier.projected.x
temppoint.point.y = frontier.projected.y
for i in range(0, len(namespace_list)):
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, [frontier.projected.x, frontier.projected.y],
info_radius * 0.5)) < 0.2):
all_frontiers = list(delete(all_frontiers, (z), axis=0))
z = z - 1
z += 1
pp = []
for q in all_frontiers:
p.x = q.projected.x
p.y = q.projected.y
pp.append(copy(p))
points.points = pp
pub.publish(points)
rate.sleep()
from nav_msgs.msg import Odometry
from tf import transformations
from std_srvs.srv import *
import math
active_ = False
# robot state variables
position_ = Point()
yaw_ = 0
# machine state
state_ = 0
# goal
desired_position_ = Point()
desired_position_.x = rospy.get_param('des_pos_x')
desired_position_.y = rospy.get_param('des_pos_y')
desired_position_.z = 0
# parameters
yaw_precision_ = math.pi / 9 # +/- 20 degree allowed
yaw_precision_2_ = math.pi / 90 # +/- 2 degree allowed
dist_precision_ = 0.3
kp_a = 3.0 # In ROS Noetic, it may be necessary to change the sign of this proportional controller
kp_d = 0.2
ub_a = 0.6
lb_a = -0.5
ub_d = 0.6
# publishers
pub = None
att_msg.yaw = -0.0
att_msg.posZ_thrust =current_target_pos.z
att_msg.posY_pitch = current_target_pos.y
att_msg.posX_roll = current_target_pos.x
att_msg.velZ=current_target_vel.z
att_msg.velY=current_target_vel.y
att_msg.velX=current_target_vel.x
return att_msg
if __name__ == "__main__":
current_controller = FollowCtrl10_2_2()
#Grid definition
base = Point()
base.x = X_MINIMUM
base.y = Y_MINIMUM
base.z = Z_LEVEL
maximum = Point()
maximum.x = X_MAXIMUM
maximum.y = Y_MAXIMUM
maximum.z = Z_LEVEL
grid = Grid(X_NUMBER_TILE, Y_NUMBER_TILE,Z_LEVEL, base, maximum)
quad_env1 = Quad("env1")
sys_thread = Sys_node(SYS_NAME)
rospy.init_node('system_node', anonymous=True)
rospy.Subscriber("/vicon"+SYS_NAME+SYS_NAME, TransformStamped, sys_thread.handle_position)
rospy.Subscriber("/vicon"+ENV_NAME+ENV_NAME, TransformStamped, quad_env1.handle_position)
PoseStamped,
queue_size=10)
#Start RVIZ and publish occupancy grid
raw_input(
'Start RVIZ and LAUNCH OCCUPANCY GRID node (Octomap node with the registered map)'
)
####### Initialize grid specifications #################
Z_LEVEL = 0.0
X_NUMBER_TILE = occupancy_grid.info.width # X correspond to the larger size -> width
Y_NUMBER_TILE = occupancy_grid.info.height # Y height correspond to the smaller -> height
base = Point()
base.x = occupancy_grid.info.origin.position.x
base.y = occupancy_grid.info.origin.position.y
base.z = Z_LEVEL
maxi = Point()
maxi.x = occupancy_grid.info.resolution * X_NUMBER_TILE + base.x
maxi.y = occupancy_grid.info.resolution * Y_NUMBER_TILE + base.y
maxi.z = Z_LEVEL
grid = Grid(X_NUMBER_TILE, Y_NUMBER_TILE, base, maxi)
print grid.x, grid.y, grid.base, grid.maximum, grid.blocklengthX, grid.blocklengthY
####### End grid specifications #############################
#unsubscribe and subscribe to occupancy grid until the map is static
def get_image(CompressedImage):
# get_image is the main function to find the circles in the image. Get_image triggers each time a new image arrives.
# All the images used to find the ball are made global so we can display them durring debugging.
global imgBGR
global imgHSV
global imgBLUR
global mask
global imgMorphOps
global imgTrack
# Needed parameters from outside this function (lazy and globaling them).
global p
global update
global start
global morphOpSize
global blurSize
global width
global pt
# The "CompressedImage" is transformed to a color image in BGR space and is store in "imgBGR"
imgBGR = bridge.compressed_imgmsg_to_cv2(CompressedImage, "bgr8")
# height and width of the image to pass along to the PID controller as the reference point.
height, width = imgBGR.shape[:2]
# Image used to draw things on!
imgTrack = imgBGR.copy()
# Blur the image to reduce edges caused by noise or that are useless to us.
imgBlur = cv2.GaussianBlur(imgBGR, (blurSize, blurSize), 0)
# Transform BGR to HSV to avoid lighting issues.
imgHSV = cv2.cvtColor(imgBlur, cv2.COLOR_BGR2HSV)
# Threshold the image using the selected lower and upper bounds of the color of the object.
mask = cv2.inRange(imgHSV, lower, upper)
# To get rid of noise and fill in gaps in our object use open and close.
imgMorphOps = morphOps(mask, morphOpSize)
centers = findObjects(imgMorphOps)
#print ("centers", not centers)
# Not always, the houghCircles function finds circle, so a None inspection is made
if not centers:
#print("hello")
#If no object was found, sends bogus numbers.
pt = Point()
pt.x = 999
pt.y = 999
pt.z = 999
update = True
elif centers is not []:
for i in centers:
# The x position of the center of the object, the width of the object, and the width of the image.
pt = Point(i[0], i[1], width)
# pt = Point()
# pt.x = p.x
# pt.y=0
# pt.z=0
# Bool to indicate the need to publish new information
update = True
# Once the first image has been processed set start to True to display.
start = True
def add_point_marker(self, marker, cx, cy):
point = Point()
point.x = cx
point.y = cy
point.z = 0
marker.points.append(point)
import time
from geometry_msgs.msg import Point
from std_srvs.srv import *
from move_base_msgs.msg import MoveBaseActionGoal
from geometry_msgs.msg import Twist
from tf import transformations
import random
import math
pub = None
yaw_ = 0
yaw_error_allowed_ = 5 * (math.pi / 180) # 5 degrees
position_ = Point()
desired_position_ = Point()
# first_action_check = True
desired_position_.x = 0
desired_position_.y = 0
desired_position_.z = 0
# first target setter
target_pos_lists = [[-4,-3],[-4,2],[-4,7],[5,-7],[5,-3],[5,1]]
# service callback
def clbk_odom(msg):
global position_, yaw_
# position
position_ = msg.linear
yaw_ = msg.angular.z
# def target_maker(msg):
# global first_action_check,pub,yaw_,yaw_error_allowed_ ,position_,desired_position_ ,target_pos_lists
def find_cones(img, depthImg=None):
h, w = img.shape[:2]
image_centerX = w/2
image_centerY = h # y goes down from top
captureFrames(img, depthImg)
# Process orange color and convert to gray image
imgThresh = process_orange_color(img)
#captureFrames(imgThresh, depthImg)
# clone/copy thresh image before smoothing
imgThreshSmoothed = imgThresh.copy()
# open image (erode, then dilate)
kernel = np.ones((3, 3), np.uint8)
imgThreshSmoothed = cv2.erode(imgThresh, kernel, iterations=1)
imgThreshSmoothed = cv2.dilate(imgThreshSmoothed, kernel, iterations=1)
# Gaussian blur
imgThreshSmoothed = cv2.GaussianBlur(imgThreshSmoothed, (5, 5), 0)
#cv2.imshow('imgThreshSmoothed ', imgThreshSmoothed)
# get Canny edges
imgCanny = cv2.Canny(imgThreshSmoothed, 160, 80)
#cv2.imshow('imgCanny ', imgCanny)
if is_cv2():
contours, hierarchy = cv2.findContours(imgCanny,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
else:
image, contours, hierarchy = cv2.findContours(imgCanny,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
listOfHullsAndArea = []
if len(contours) != 0:
for cnt in contours:
epsilon = 0.1 * cv2.arcLength(cnt, True)
# print'epsilon',epsilon
contour = cv2.approxPolyDP(cnt, epsilon, True)
#contour = cv2.approxPolyDP(cnt, 6.7, True)
# Find convex hulls.
hull = cv2.convexHull(contour, returnPoints=True)
# See how the hull looks as a triangle
# tri = cv2.minEnclosingTriangle(hull)
# get the depth for the hull. Is it one value or multiple?
depthRange = getHullDepth(hull)
# We need to sort and store the contours by proximity of their centroids
listOfHullsAndArea.append((hull, cv2.contourArea(hull), depthRange))
listOfCones = []
listOfAreas = []
listOfObstructions = []
pose = Point()
poses = []
loc = location_data()
# Sort the list by decreasing area
listOfHullsAndArea = sorted(listOfHullsAndArea, key=lambda pair: pair[1], reverse=True)
for (hull, area, (dMin, dMax, isReal)) in listOfHullsAndArea:
# print 'convexHull',len(temp)
if (len(hull) >= 3 and convexHullIsPointingUp(hull)):
listOfCones.append(hull)
x, y, w, h = cv2.boundingRect(hull)
pose.x = x + w/2 - image_centerX
# Height is being measured top of screen to down so we need to invert y
pose.y = (image_centerY - (y+h))
# Divide depth by 256 since x isn't really in real units
pose.z = depthRange[0] # But this is the hypotenuse
poses.append(pose)
loc.poses = poses
loc.header.stamp = rospy.Time.now()
imghull = img.copy()
cv2.drawContours(imghull, listOfCones, -1, (0, 255, 0), 3)
if(len(listOfCones)):
pub.publish(loc)
return len(listOfCones), imghull
imu_msg = Imu()
xy_msg = Point()
wind_msg = Point()
rospy.Subscriber("control_send_uq", Point, sub_uq)
rospy.init_node('simu_boat')
rate = rospy.Rate(10) # 10hz
# --- Main ------------- #
u, q = np.array([[0.1], [0.1]]), 1
while not rospy.is_shutdown():
xdot, delta_s = f(x, u)
x = x + dt * xdot
imu_msg.orientation.x = x[2, 0] # heading
xy_msg.x = x[0, 0] # x pos
xy_msg.y = x[1, 0] # y pos
xy_msg.z = delta_s
wind_msg.x = awind # wind force/speed
wind_msg.y = psi # wind direction
pub_send_imu.publish(imu_msg)
pub_send_xy.publish(xy_msg)
pub_send_wind.publish(wind_msg)
rospy.loginfo(xy_msg)
rate.sleep()
except rospy.ROSInterruptException:
pass
#if within search field
if self.Ro <= self.do <= self.So + self.Ro:
delta.x = -beta * (self.So + self.Ro - self.do) * cos(self.tho)
delta.y = -beta * (self.So + self.Ro - self.do) * sin(self.tho)
#if outside search field
if self.do > self.So + self.Ro:
delta.x = delta.y = 0
return delta
#Implementation
current_x = 0.0 # current x co-ord of the robot (global)
current_y = 0.0 # current y co-ord of the robot (global)
current_th = 0.0 # current orientation of the robot (global)
goal = Point() # goal co-ordinates (global)
goal.x = 0 # instanciate x co-ord of goal
goal.y = 0 # instanciate y co-ord of goal
delta = Point() # delta (global)
delta.x = delta.y = 0 # instanciate delta x and y
resetted = True # Has the odometry been reset? Boolean (global)
dist = 10 # instanciate dist (cannot be zero or linear velocity calculation does not function)
# Laser scan callback - steering method
def steering(data):
global delta
global goal
global current_x
global current_y
global resetted
global dist
def display_line_list(points, publisher):
"""
A function that publishes a set of points as marker line list to Rviz.
It will draw a line between each pair of points, so 0-1, 2-3, 4-5, ...
Parameters:
points (list): Each item in the list is a tuple (x, y) representing a point in xy space.
publisher (rospy.Publisher): A publisher object used to pubish the marker
Returns:
None
"""
marker = Marker()
# The coordinate frame in which the marker is publsihed.
# Make sure "Fixed Frame" under "Global Options" in the Display panel
# in rviz is "/map"
marker.header.frame_id = "/map"
# Mark type (http://wiki.ros.org/rviz/DisplayTypes/Marker)
# LINE_LIST: It will draw a line between each pair of points, so 0-1, 2-3, 4-5, ...
marker.type = marker.LINE_LIST
# Marker action (Set this as ADD)
marker.action = marker.ADD
# Marker scale
marker.scale.x = 0.01
marker.scale.y = 0.01
marker.scale.z = 0.01
# Marker color (Make sure a=1.0 which sets the opacity)
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
# Marker orientaiton (Set it as default orientation in quaternion)
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
# The position of the marker. In this case it the COM of all the points
# Set this as 0,0,0
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
# Marker line points
marker.points = []
# zplus = 0.0
# points2 = [points[len(points) - 1],points[len(points) - 2],points[len(points )- 3],points[len(points )- 4]]
for point in points:
global zplus
marker_point = Point() # Create a new Point()
marker_point.x = (point[0] ) * 0.15
marker_point.y = (point[1] ) * 0.15
marker.color.b = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
# zplus = zplus + 0.01
# print(zplus)
marker.points.append(marker_point) # Append the marker_point to the marker.points list
# Publish the Marker using the appropirate publisher
publisher.publish(marker)
from time import time
from sensor_msgs.msg import LaserScan
#initialize values
current_x = 0.0 # current x co-ord of robot
current_y = 0.0 # current y co-ord of robot
current_th = 0.0 # current orientation of the robot
turn = 0.0 # holds co-ords of next maneuver
achieved = True # boolean - has the robot reached it's goal?
resetted = False # boolean - has the odometry been reset?
#Set the goal coordinates
goal = Point() # co-ordinates of the final goal
goal.x = 5
goal.y = 0
sub_goal = Point() # a subgoal 0.5 meters from the robot
sub_goal.x = 0
sub_goal.y = 0
#set the vector increments for a 0.5m radius
def turn_options(index): # This function holds 7 possible turning options (windows)
global current_x # which will be added to the current co-ordinates to d
global current_y # the location of the next subgoal 0 -> 7 , left -> right
th = 0.0
global turn
global current_th
turn = Point()
x = 0
marker.header.frame_id = "/desired_trajec"
#marker.type = marker.SPHERE
marker.type = marker.LINE_LIST
marker.action = marker.ADD
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
# We add the new marker to the MarkerArray, removing the oldest
# marker from it when necessary
for i in range(100):
p1 = Point()
p1.x = traject[i, 0]
p1.y = traject[i, 1]
p1.z = 0
marker.points.append(p1)
while not rospy.is_shutdown():
# Publish the Marker
publisher.publish(marker)
rate.sleep()
def processImage(self, image_msg):
if not self.thread_lock.acquire(False):
return
#image setup
image_cv = self.bridge.imgmsg_to_cv2(image_msg)
width, height, cha = image_cv.shape
lowthresarea = 10000#2500
# Convert BGR to HSV
hsv = cv2.cvtColor(image_cv, cv2.COLOR_BGR2HSV)
Arr = []
BoxArr = []
cent = Point()
for c in colors:
#Create mask
(lower, upper) = color_map[c]
mask = cv2.inRange(hsv, lower, upper)
#Create contours
contours = cv2.findContours(mask, cv2.cv.CV_RETR_EXTERNAL, cv2.cv.CV_CHAIN_APPROX_NONE)[0]
#for each contour of the color
for co in contours:
#find the minimum rectangle
rect = cv2.minAreaRect(co)
#area
(w,h) = rect[1]
area = w*h
if area < lowthresarea: continue
#angle = rect[2]
#if angle < -30 and angle > -60: continue#too angled
#p = max(w/h,h/w)
#if p > 2: continue#the rectangle probably does not enclose the paper
#find the center of the rectangle
center = rect[0]
(y,x) = center
cent.x = x/width
cent.y = y/height
#define the box coordinates
box = cv2.cv.BoxPoints(rect)
box = np.int0(box)
#save the appropriate message/image data
Arr.append((Float64(area),cent,String(c)))
BoxArr.append(box)#order does not matter
if c == "pink":#send image to the correct place
if area < 20000: continue
#cropping picture
corn1 = box[0]
corn2 = box[2]
corn3 = box[1]
corn4 = box[3]
p1 = min(min(corn1[1], corn2[1]),min(corn3[1],corn4[1]))
p2 = max(max(corn1[1], corn2[1]),max(corn3[1],corn4[1]))
p3 = min(min(corn1[0], corn2[0]),min(corn3[0],corn4[0]))
p4 = max(max(corn1[0], corn2[0]),max(corn3[0],corn4[0]))
cropped = image_cv[p1:p2, p3:p4]
#cleaning the image
img2 = cv2.cvtColor(cropped, cv2.COLOR_BGR2GRAY)
if img2 == None: continue
widthc, heightc = img2.shape
## find the keypoints and descriptors with SIFT - image
kp2, des2 = self.surf.detectAndCompute(img2,None)
if des2 == None: continue #just leave, no keypoints
flann = cv2.BFMatcher(cv2.NORM_HAMMING,crossCheck=True)
# Initalize good counts, function
counts = []
for name in self.names:
'''
img1 = misc.imread(name+".jpg","L").astype(np.uint8)
# find the keypoints and descriptors with SIFT - reference
kp1, des1 = self.surf.detectAndCompute(img1,None)
'''
des1 = self.desImages[name]
#record number of matches
matches = flann.match(des1,des2)
bools = map(self.function,matches)
count = sum(bools)
counts.append(count)
#find max count
maxi = max(counts)
#find index of max count
ind = counts.index(maxi)
#get picture name of the ind
chosen = self.names[ind]
spec = image_cv.copy()
#put string on image
cv2.putText(spec, chosen, (100, 100), cv2.FONT_HERSHEY_PLAIN, 2, (0,255,0))
cv2.drawContours(spec,[co],0,(255,255,255),4)
#counter for special images
self.piccount += 1
#save file as special
cv2.imwrite("/home/racecar/challenge_photos/special"+str(self.piccount)+".jpg",spec)
#sort blobs
Arr.sort()
Arr = Arr[::-1]
#create message
self.message.sizes = [o[0] for o in Arr]
self.message.locations = [o[1] for o in Arr]
self.message.colors = [o[2] for o in Arr]
#sending a challenge picture
if len(self.message.colors) > 0:
cv2.drawContours(image_cv,BoxArr,0,(255,255,255),4)
colorname = self.message.colors[0].data
cv2.putText(image_cv, colorname, (100, 100), cv2.FONT_HERSHEY_PLAIN,10, (0,255,0))
try:
#publish
rospy.loginfo("Publishing")
self.pub_image.publish(self.bridge.cv2_to_imgmsg(image_cv, "bgr8"))
self.pub_msg.publish(self.message)
if len(self.message.colors) > 0:
self.pub_chal.publish(self.bridge.cv2_to_imgmsg(image_cv, "bgr8"))
except CvBridgeError as e:
print(e)
self.thread_lock.release()
marker.id = marker_id
marker.type = Marker.POINTS
marker.action = Marker.ADD
marker.pose.orientation.w = 1.0
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.lifetime = rospy.Duration(1.0)
point = Point()
point.x = .5
point.y = .5
marker.points.append(point)
marker_msg.markers.append(marker)
obstacle_publisher.publish(all_ob)
if marker_msg.markers:
marker_pub.publish(marker_msg)
rospy.sleep(0.07)
def follow_path(location):
global path_progress
global iteration
global path_output
new_target = Point()
global path
rospy.logwarn("Self Driving Iteration: " + str(iteration))
if (iteration == 0):
current_radians = get_yaw(location.pose)
rospy.loginfo("\nCurrent Pose \n" + str(location.pose) + "\n")
rospy.loginfo("Current Radians: " + str(current_radians))
new_target.x = path[path_progress][1]
new_target.y = path[path_progress][2]
new_target.z = 0
rospy.loginfo("New Target: \n " + str(new_target))
#new_target = Point(path.poses[path_progress + 3].pose.position.x)
current_location = location.pose.position
rospy.loginfo("Current Location: \n" + str(current_location) + " \n New Target: \n" + str(new_target))
target_radians = math.atan((new_target.y - current_location.y)/( new_target.x - current_location.x))
rospy.loginfo("target_radians = " + str(target_radians))
desired_yaw = target_radians
quaternion = quaternion_from_euler(roll, pitch, desired_yaw)
#type(pose) = geometry_msgs.msg.Pose
location.pose.orientation.x = quaternion[0]
location.pose.orientation.y = quaternion[1]
location.pose.orientation.z = quaternion[2]
location.pose.orientation.w = quaternion[3]
rospy.loginfo("Current Pose: " + str(location.pose))
rospy.loginfo("END ITERATION 0")
iteration += 1
time.sleep(1)
current_radians = get_yaw(location.pose)
rospy.loginfo("Yaw (Current Radians):" + str(current_radians))
x_movement = math.cos(current_radians) * velocity
rospy.loginfo("X Movement: " +str(x_movement))
y_movement = math.sin(current_radians) * velocity
rospy.loginfo("Y Movement: " + str(y_movement))
location.pose.position.x += x_movement
location.pose.position.y += y_movement
rospy.loginfo("Current Position (After Movement): " +str(location.pose))
##############################################################################
#current_radians = get_yaw(location.pose)
#rospy.loginfo("\nCurrent Pose \n" + str(location.pose) + "\n")
#rospy.loginfo("Current Radians: " + str(current_radians))
new_target.x = path[path_progress][1]
new_target.y = path[path_progress][2]
new_target.z = 0
rospy.loginfo("New Target: \n " + str(new_target))
#new_target = Point(path.poses[path_progress + 3].pose.position.x)
current_location = location.pose.position
#rospy.loginfo("Current Location: \n" + str(current_location) + " \n New Target: \n" + str(new_target))
target_radians = math.atan((new_target.y - current_location.y)/( new_target.x - current_location.x))
rospy.loginfo("target_radians = " + str(target_radians))
desired_yaw = target_radians
quaternion = quaternion_from_euler(roll, pitch, desired_yaw)
#type(pose) = geometry_msgs.msg.Pose
location.pose.orientation.x = quaternion[0]
location.pose.orientation.y = quaternion[1]
location.pose.orientation.z = quaternion[2]
location.pose.orientation.w = quaternion[3]
rospy.loginfo("Orientation (Steered): " + str(location.pose.orientation))
rospy.loginfo("END STEERING")
#######################################################################
#quaternion = quaternion_from_euler(roll, pitch, yaw)
#type(pose) = geometry_msgs.msg.Pose
#current_pose.orientation.x = quaternion[0]
#current_pose.orientation.y = quaternion[1]
#current_pose.orientation.z = quaternion[2]
#current_pose.orientation.w = quaternion[3]
#rospy.loginfo("Yaw: " + str(yaw))
#current_radians = get_yaw(location.pose)
#rospy.loginfo("Yaw: " + str(yaw))
#current_degrees = np.degrees(current_radians)
#new_target.x = path.poses[path_progress + 3].pose.position.x
#new_target.y = path.poses[path_progress + 3].pose.position.y
#new_target.z = path.poses[path_progress + 3].pose.position.z
#new_target = Point(path.poses[path_progress + 3].pose.position.x)
#current_location = path.poses[path_progress].pose.position
#target_radians = math.atan((new_target.y - current_location.y)/( new_target.y - current_location.y))
path_progress = path_progress + 1
#figure out current location
#rospy.loginfo("Pose into publisher: " + str(location.pose))
header = Header(stamp = rospy.Time.now(), frame_id = "my_frame")
publish_pose = PoseStamped(header = header, pose = location.pose)
publish_heading_control.publish(publish_pose)
publish_pose.header.frame_id = "my_frame"
publish_pose.header.seq = iteration + 1
#PS = PoseStamped(header=header, pose=publish_pose)
#PS.header.frame_id = "my_frame"
#path_output.header = publish_pose.header
rospy.loginfo("Pose to append: " + str(publish_pose))
path_output.poses.append(publish_pose)
publish_Autonomous_Path.publish(path_output)
iteration += 1
def euler_deg_to_euler_rad(self, point_deg):
point_rad = Point()
point_rad.x = self.deg_to_rad(point_deg.x)
point_rad.y = self.deg_to_rad(point_deg.y)
point_rad.z = self.deg_to_rad(point_deg.z)
return point_rad
def controller(self):
# distance control
dist = self.distanceToMid()
err_dist = dist - self.desired_radius
pid_output = self.PID_dist.regulate(err_dist, rospy.get_time())
vec_to_mid_glob_frame = self.getVectorFromAUVToCentre()
rot_mat = np.array([[np.cos(self.yaw), -np.sin(self.yaw)],[np.sin(self.yaw), np.cos(self.yaw)]])
rot_mat = rot_mat.transpose()
vec_to_mid_auv_frame = np.dot(rot_mat,vec_to_mid_glob_frame)
force_vec = vec_to_mid_auv_frame * pid_output
force_x = force_vec[0]
force_y = force_vec[1]
# angle control
auv_pos = Point()
auv_pos.x = self.x
auv_pos.y = self.y
desired_angle = self.getYawFrom2Pos(auv_pos, self.centre_of_rot)
err_yaw = desired_angle - self.yaw
err_yaw = self.fixHeadingWrapping(err_yaw)
torque_z = self.PID_angle.regulate(err_yaw, rospy.get_time())
# sideways force to rotate around point
# dont go sideways unless facing towards point
max_side_force = 5
sideway_force = max(0, max_side_force - 5*err_yaw)
force_y = force_y + sideway_force
# depth control
tau_depth_hold = self.PID.depthController(self.desired_depth, self.z, rospy.get_time())
# make wrench and publish
wrench = Wrench()
wrench.force.x = force_x
wrench.force.y = force_y
wrench.torque.z = torque_z
wrench.force.z = tau_depth_hold
self.pub_thrust.publish(wrench)
PRINT_INFO = False
if PRINT_INFO:
print("Force_x: ", force_x, " Force_y: ", force_y, " torque_z: ", torque_z)
print("Error distance: ", err_dist, " Error yaw: ", err_yaw)
# Copyright © 2018 Cesar Sinchiguano <*****@*****.**>
#
# Distributed under terms of the BSD license.
import rospy
from geometry_msgs.msg import Twist, Point
from nav_msgs.msg import Odometry
from std_msgs.msg import Empty
from math import radians
import tf
import math
import time
goal_position = Point()
goal_position.x = 5
goal_position.y = 5
goal_position.z = 0
goal_angle = math.atan2(goal_position.y, goal_position.x)
current_position = Point(0.0, 0.0, 0.0)
delta_xy = Point(0.0, 0.0, 0.0)
theta = 0.0
def odometry_callback(msg):
#print('====================')
global current_position
current_position.x = msg.pose.pose.position.x
current_position.y = msg.pose.pose.position.y
def __init__(self):
rospy.init_node('markers_from_tf')
rospy.loginfo("Initializing Skeleton Markers Node...")
rate = rospy.get_param('~rate', 20)
r = rospy.Rate(rate)
tf_prefix = rospy.get_param('~tf_prefix', '')
skeleton_frames = rospy.get_param('~skeleton_frames', '')
self.fixed_frame = rospy.get_param('~fixed_frame', 'openni_depth_frame')
# Normalize the tf_prefix variable with / at the beginning and end
tf_prefix = '/' + tf_prefix + '/'
tf_prefix = re.sub('/+', '/', tf_prefix)
# Normalize the tf_prefix variable with / at the beginning and end
self.fixed_frame = '/' + tf_prefix + '/' + self.fixed_frame
self.fixed_frame = re.sub('/+', '/', self.fixed_frame)
# Initialize tf listener
tf_listener = tf.TransformListener()
# Define a marker publisher
marker_pub = rospy.Publisher('skeleton_markers', Marker)
# Intialize the markers
self.init_markers()
# The openni_tracker identifies each skeleton frame with a user_id beginning
# with 1 and incrementing as new users are tracked.
user_id = None
# Get the list of all skeleton frames from tf
#skeleton_frames = [f for f in tf_listener.getFrameStrings() if f.startswith("/" + self.tf_prefix)]
# Begin the main loop
while not rospy.is_shutdown():
# Get a list of all frames
all_frames = [f for f in tf_listener.getFrameStrings()]
# Test for the user ID to track
try:
test = all_frames.index(tf_prefix + 'torso')
user_id = ""
except:
try:
test = all_frames.index(tf_prefix + 'torso_1')
user_id = "_1"
except:
pass
if user_id is None:
r.sleep
continue
# Get all skeleton frames for the detected user user
user_frames = list()
for frame in skeleton_frames:
qualified_frame = tf_prefix + frame + user_id
try:
all_frames.index(qualified_frame)
user_frames.append(qualified_frame)
except:
pass
#tf_listener.waitForTransform(self.fixed_frame, user_frames[0], rospy.Time(), rospy.Duration(25.0))
# Set the markers header
self.markers.header.stamp = rospy.Time.now()
# Clear the markers point list
self.markers.points = list()
# Loop through the skeleton frames
for frame in user_frames:
if frame == self.fixed_frame:
continue
# Find the position of the frame's origin relative to the fixed frame.
try:
position = Point()
# Get the transformation from the fixed frame to the skeleton frame
(trans, rot) = tf_listener.lookupTransform(self.fixed_frame, frame, rospy.Time(0))
position.x = trans[0]
position.y = trans[1]
position.z = trans[2]
# Set a marker at the origin of this frame
self.markers.points.append(position)
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.logerr("tf error when looking up " + frame + ' and ' + self.fixed_frame)
continue
# Publish the set of markers
marker_pub.publish(self.markers)
r.sleep()
def pub_odom(self):
p = Point()
p.x, p.y = self.robot_pose[:2]
p.z = 0
self.odom.points.append(p)
self.marker_pub.publish(self.odom)
def _is_done(self, observations):
"""
The done can be done due to three reasons:
1) It went outside the workspace
2) It detected something with the sonar that is too close
3) It flipped due to a crash or something
4) It has reached the desired point
"""
episode_done = False
current_position = Point()
current_position.x = observations[0]
current_position.y = observations[1]
current_position.z = observations[2]
current_orientation = Point()
current_orientation.x = observations[3]
current_orientation.y = observations[4]
current_orientation.z = observations[5]
sonar_value = observations[6]
is_inside_workspace_now = self.is_inside_workspace(current_position)
sonar_detected_something_too_close_now = self.sonar_detected_something_too_close(
sonar_value)
drone_flipped = self.drone_has_flipped(current_orientation)
has_reached_des_point = self.is_in_desired_position(
current_position, self.desired_point_epsilon)
rospy.logwarn(">>>>>> DONE RESULTS <<<<<")
if not is_inside_workspace_now:
rospy.logerr("is_inside_workspace_now=" +
str(is_inside_workspace_now))
else:
rospy.logwarn("is_inside_workspace_now=" +
str(is_inside_workspace_now))
if sonar_detected_something_too_close_now:
rospy.logerr("sonar_detected_something_too_close_now=" +
str(sonar_detected_something_too_close_now))
else:
rospy.logwarn("sonar_detected_something_too_close_now=" +
str(sonar_detected_something_too_close_now))
if drone_flipped:
rospy.logerr("drone_flipped=" + str(drone_flipped))
else:
rospy.logwarn("drone_flipped=" + str(drone_flipped))
if has_reached_des_point:
rospy.logerr("has_reached_des_point=" + str(has_reached_des_point))
else:
rospy.logwarn("has_reached_des_point=" +
str(has_reached_des_point))
# We see if we are outside the Learning Space
episode_done = not (
is_inside_workspace_now
) or sonar_detected_something_too_close_now or drone_flipped or has_reached_des_point
if episode_done:
rospy.logerr("episode_done====>" + str(episode_done))
else:
rospy.logwarn("episode_done====>" + str(episode_done))
return episode_done
def __init__(self):
self.x = 0
self.y = 0
self.theta = 0
self.ck = 0
self.yaw = 0
# initiliaze
rospy.init_node('Navigation_Waypoints', anonymous=False)
# tell user how to stop TurtleBot
rospy.loginfo("To stop TurtleBot CTRL + C")
# What function to call when you ctrl + c
rospy.on_shutdown(self.shutdown)
# Create a publisher which can "talk" to TurtleBot and tell it to move
# Tip: You may need to change cmd_vel_mux/input/navi to /cmd_vel if you're not using TurtleBot2
print("pre")
self.sub = rospy.Subscriber("/yellow/odom", Odometry, self.newOdom)
print("after sub")
self.cmd_vel = rospy.Publisher('/yellow/mobile_base/commands/velocity',
Twist,
queue_size=10)
#self.sub_nav = rospy.Subscriber("/green/navigation", Twist, self.callbackNav)
#TurtleBot will stop if we don't keep telling it to move. How often should we tell it to move? 10 HZ
r = rospy.Rate(10)
move_cmd = Twist()
x_arr, y_arr = self.read_input()
print("aaaa")
goalie = [x_arr[-1], y_arr[-1]]
print("a")
target_speed = 10.0 / 33.6 # simulation parameter km/h -> m/s
print(self.yaw)
sp = calc_speed_profile(self.yaw, target_speed)
print(sp)
i = 1
while not rospy.is_shutdown():
# print("b")
goal = Point()
goal.x = x_arr[i]
goal.y = y_arr[i]
inc_x = goal.x - self.x
inc_y = goal.y - self.y
print("c")
angle_to_goal = atan2(inc_y, inc_x)
print('d')
print(inc_x)
print(inc_y)
test = False
if abs(inc_x) < 0.05 and abs(inc_y) < 0.05:
print('e')
i += 1
if i == len(x_arr) - 1:
print('yeet')
self.shutdown()
# if not facing next waypoint, rotate turtlebot
elif abs(angle_to_goal - self.theta) > 0.03:
move_cmd.linear.x = 0.0
if test:
move_cmd.angular.z = 0.4
else:
move_cmd.angular.z = -0.4
print('f')
test = True
else:
# if facing next waypoint, move the turtlebot
move_cmd.linear.x = 0.3
move_cmd.angular.z = 0.0
print('g')
print("inside3")
#t, x, y, yaw, v = do_simulation(x_arr, y_arr, self.yaw, self.ck, sp, goalie)
#print(v)
#print(yaw)
#print("afterwatrds---------------------------------------------------------")
self.cmd_vel.publish(move_cmd)
print(move_cmd.linear.x)
print(move_cmd.angular.z)
rospy.sleep(0.1)
print("inside4")
def display_cube_list(points, publisher):
"""
A function that publishes a set of points as marker cubes in Rviz.
Each point represents the COM of the cube to be displayed.
Parameters:
points (list): Each item in the list is a tuple (x, y) representing a point in xy space
for the COM of the cube.
publisher (rospy.Publisher): A publisher object used to pubish the marker
Returns:
None
"""
marker = Marker()
# The coordinate frame in which the marker is published.
# Make sure "Fixed Frame" under "Global Options" in the Display panel
# in rviz is "/map"
marker.header.frame_id = "/map"
# Mark type (http://wiki.ros.org/rviz/DisplayTypes/Marker)
# CUBE_LIST
marker.type = marker.CUBE_LIST
# Marker action (Set this as ADD)
marker.action = marker.ADD
# Marker scale (Size of the cube)
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
# Marker color (Make sure a=1.0 which sets the opacity)
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
# Marker orientation (Set it as default orientation in quaternion)
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
# The position of the marker. In this case it the COM of all the cubes
# Set this as 0,0,0
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
# Marker line points
marker.points = []
for point in points:
marker_point = Point() # Create a new Point()
marker_point.x = point[0] * 0.15
marker_point.y = point[1] * 0.15
marker_point.z = 0
marker.points.append(marker_point) # Append the marker_point to the marker.points list
# Publish the Marker using the apporopriate publisher
publisher.publish(marker)
line_list.scale.x = 0.1
points.color.g = 1.0
points.color.a = 1.0
line_strip.color.b = 1.0
line_strip.color.a = 1.0
line_list.color.r = 1.0
line_list.color.a = 1.0
f = 0.0
for i in range(100):
y = 5 * math.sin(f + i / 100.0 * 2 * math.pi)
z = 5 * math.cos(f + i / 100.0 * 2 * math.pi)
p = Point()
p.x = i-50
p.y = y
p.z = z
points.points.append(p)
line_strip.points.append(p)
line_list.points.append(p)
p.z = p.z + 1.0
line_list.points.append(p)
pub.publish(points)
pub.publish(line_list)
pub.publish(line_strip)
f = f + 0.4
rate.sleep()
def data_exchange(self):
if self.messenger != None:
if self.live:
try:
self.__send_log("send: Battery state request")
battery_state = SimpleBatteryState()
battery_state.header.stamp = rospy.Time.now()
battery_state.charge = self.messenger.hub['CBoard']['VoltBatt'].read()[0] / 1000.0
self.__send_log(f"response: Battery state - {battery_state.charge}")
self.battery_publisher.publish(battery_state)
except:
pass
try:
self.__send_log("send: Gyro request")
gyro = Point()
gyro.x = self.messenger.hub['SensorMonitor']['gyroX'].read()[0] / 1e3
gyro.y = self.messenger.hub['SensorMonitor']['gyroY'].read()[0] / 1e3
gyro.z = self.messenger.hub['SensorMonitor']['gyroZ'].read()[0] / 1e3
self.__send_log(f"response: Gyro - [{gyro.x}, {gyro.y}, {gyro.z}]")
self.gyro_publisher.publish(gyro)
except:
pass
try:
self.__send_log("send: Accel request")
accel = Point()
accel.x = self.messenger.hub['SensorMonitor']['accelX'].read()[0] / 1e3
accel.y = self.messenger.hub['SensorMonitor']['accelY'].read()[0] / 1e3
accel.z = self.messenger.hub['SensorMonitor']['accelZ'].read()[0] / 1e3
self.__send_log(f"response: Accel - [{accel.x}, {accel.y}, {accel.z}]")
self.accel_publisher.publish(accel)
except:
pass
try:
self.__send_log("send: Orientation request")
orientation = Orientation()
orientation.roll = self.messenger.hub['UavMonitor']['roll'].read()[0] / 1e2
orientation.pitch = self.messenger.hub['UavMonitor']['pitch'].read()[0] / 1e2
orientation.azimuth = self.messenger.hub['UavMonitor']['yaw'].read()[0] / 1e2
self.__send_log(f"response: Orientation - [{orientation.roll}, {orientation.pitch}, {orientation.azimuth}]")
self.orientation_publisher.publish(orientation)
except:
pass
try:
self.__send_log("send: Altitude request")
altitude = self.messenger.hub['UavMonitor']['altitude'].read()[0] / 1e3
self.__send_log(f"response: Altitude - {altitude}")
self.altitude_publisher.publish(altitude)
except:
pass
if self.navSystem == 0:
try:
self.__send_log("send: Global position request")
global_point = PointGPS()
global_point.latitude = self.messenger.hub['Ublox']['latitude'].read()[0] / 1e7
global_point.longitude = self.messenger.hub['Ublox']['longitude'].read()[0] / 1e7
global_point.altitude = self.messenger.hub['Ublox']['altitude'].read()[0] / 1e3
self.__send_log(f"response: Global position - [{global_point.latitude}, {global_point.longitude}, {global_point.altitude}]")
self.global_position_publisher.publish(global_point)
self.__send_log("send: Mag request")
mag = Point()
mag.x = self.messenger.hub['SensorMonitor']['magX'].read()[0] / 1e3
mag.y = self.messenger.hub['SensorMonitor']['magY'].read()[0] / 1e3
mag.z = self.messenger.hub['SensorMonitor']['magZ'].read()[0] / 1e3
self.__send_log(f"response: Mag - [{mag.x}, {mag.y}, {mag.z}]")
self.mag_publisher.publish(mag)
sat = SatellitesGPS()
try:
sat.gps = self.messenger.hub['Ublox']['satGps'].read()[0]
except:
sat.gps = 0
try:
sat.glonass = self.messenger.hub['Ublox']['satGlonass'].read()[0]
except:
sat.glonass = 0
self.satellites_publisher.publish(sat)
self.__send_log("send: Global status")
status = self.messenger.hub['Ublox']['status'].read()[0]
self.__send_log(f"response: Global status - {status}")
self.global_status_publisher.publish(status)
except TypeError as e:
rospy.logerr(str(e))
rospy.loginfo("Restarting to activate Gnns module")
self.restart_board()
except:
self.__navSystem_except("Gnns Module")
elif self.navSystem == 1:
try:
self.__send_log("send: Local position request")
local_point = Point()
local_point.x = self.messenger.hub['USNav_module']['x'].read()[0] * 0.001
local_point.y = self.messenger.hub['USNav_module']['y'].read()[0] * 0.001
local_point.z = self.messenger.hub['USNav_module']['z'].read()[0] * 0.001
self.__send_log(f"response: Local position - [{local_point.x}, {local_point.y}, {local_point.z}]")
self.local_position_publisher.publish(local_point)
self.__send_log("send: LPS yaw request")
yaw = self.messenger.hub['USNav_module']['yaw'].read()[0] * 0.01
self.__send_log(f"response: LPS yaw - {yaw}")
self.local_yaw_publisher.publish(yaw)
self.__send_log("send: LPS velocity request")
lps_vel = Point()
lps_vel.x = self.messenger.hub['USNav_module']['velX'].read()[0]
lps_vel.y = self.messenger.hub['USNav_module']['velY'].read()[0]
lps_vel.z = self.messenger.hub['USNav_module']['velZ'].read()[0]
self.__send_log(f"response: LPS velocity - [{lps_vel.x}, {lps_vel.y}, {lps_vel.z}]")
self.local_velocity_publisher.publish(lps_vel)
except TypeError:
rospy.loginfo("Restarting to activate LPS module")
self.restart_board()
except:
self.navSystem_except("LPS")
elif self.navSystem == 2:
try:
self.__send_log("send: OpticalFlow velocity request")
velocity = OptVelocity()
velocity.x = self.messenger.hub['SensorMonitor']['optFlowX'].read()[0]
velocity.y = self.messenger.hub['SensorMonitor']['optFlowY'].read()[0]
velocity.range = self.messenger.hub['SensorMonitor']['optFlowRange'].read()[0] / 1e3
self.__send_log(f"response: OpticalFlow velocity - [{velocity.x}, {velocity.y}, {velocity.range}]")
self.opt_velocity_publisher.publish(velocity)
except:
self.__navSystem_except("OpticalFlow Module")
else:
self.live = False
self.disconnect()
else:
self.live = False
global max_y
max_x = 0
max_y = 0
for ind in max_positions[0]:
x, y = find_position(ind, m, res)
max_x += x
max_y += y
max_x /= len(max_positions[0])
max_y /= len(max_positions[0])
rospy.init_node("source_from_map")
grid = GridWorld()
m = grid.m
res = grid.res
max_x = 0
max_y = 0
rospy.Subscriber("mapping_viz", OccupancyGrid, callbackfn)
pub = rospy.Publisher("max_probability", Point, queue_size=10)
max_prob = Point()
max_prob.z = 0
r = rospy.Rate(1)
while not rospy.is_shutdown():
max_prob.x = max_x
max_prob.y = max_y
pub.publish(max_prob)
r.sleep()
temp = []
for j in range(grid_sizey):
temp.append(0)
explore.append(temp)
active_ = False
# robot state variables
position_ = Point()
curr_point = Point()
yaw_ = 0
# machine state
state_ = 0
# goal
desired_position_ = Point()
desired_position_.x = rospy.get_param('des_pos_x')
desired_position_.y = rospy.get_param('des_pos_y')
desired_position_.z = 0
curr_point.x = desired_position_.x
curr_point.y = desired_position_.y
prev = Point()
borders = []
borders.append(Point())
borders.append(Point())
borders.append(Point())
borders.append(Point())
# parameters
yaw_precision_ = math.pi / 90 # +/- 2 degree allowed
dist_precision_ = 5
udp.sender()
# while True:
while not rospy.is_shutdown():
udp.receiver()
# set publish data
# odometry = Float32MultiArray()
# odometry.data = (float(udp.view3Recv[1]) / 10, float(udp.view3Recv[2]) / 10, float(udp.view3Recv[3]) / 10)
checkFlag = float(udp.view3Recv[1])
getOdometry = Odometry()
poseWithCovariance = PoseWithCovariance()
point = Point()
quaternion = Quaternion()
pose = Pose()
header = Header()
point.x = float(udp.view3Recv[2]) / 10
point.y = float(udp.view3Recv[3]) / 10
point.z = float(udp.view3Recv[4]) / 10
quaternion.x = 0
quaternion.y = 0
quaternion.z = 0
quaternion.w = 0
pose.position = point
pose.orientation = quaternion
poseWithCovariance.pose = pose
poseWithCovariance.covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
header.seq = 1
header.stamp = rospy.Time.now()
header.frame_id = "odometry"
getOdometry.header = header
getOdometry.pose = poseWithCovariance
def run(args):
c_id = args.id
c_name = 'c' + str(c_id)
rospy.init_node('chaser' + str(c_id))
# Update control every 10 ms.
rate_limiter = rospy.Rate(100)
publisher = rospy.Publisher('/' + c_name + '/cmd_vel', Twist, queue_size=5)
if RVIZ_PUBLISH:
rviz_publisher = rospy.Publisher('/' + c_name + '_position',
PointStamped,
queue_size=1)
rospy.Subscriber('/captured_runners', String, capture_runner)
groundtruth = MultiGroundtruthPose([c_name] + RUNNERS)
path_sub = PathSubscriber(c_name)
rev_frames = 0
had_path = False
frame_id = 0
init_publish = False
while not rospy.is_shutdown():
# Make sure all measurements are ready.
if not groundtruth.ready:
rate_limiter.sleep()
continue
pose = groundtruth.poses[c_name]
# Publish initial position
if not init_publish or not path_sub.ready:
if RVIZ_PUBLISH:
position_msg = PointStamped()
position_msg.header.seq = frame_id
position_msg.header.stamp = rospy.Time.now()
position_msg.header.frame_id = '/odom'
pt = Point()
pt.x = pose[X]
pt.y = pose[Y]
pt.z = .05
position_msg.point = pt
rviz_publisher.publish(position_msg)
init_publish = True
rate_limiter.sleep()
continue
for runner in ['r0', 'r1', 'r2']:
if np.linalg.norm(groundtruth.poses[runner][:2] - pose[:2]) < 0.15:
rev_frames = 50
path = path_sub.path
if path is None or len(path) == 0:
if had_path:
rev_frames = 50
else:
had_path = True
# Calculate and publish control inputs.
v = get_velocity(pose[:2], path, CHASER_SPEED)
for runner in RUNNERS:
runner_pose = groundtruth.poses[runner]
runner_pos = runner_pose[:2]
diff = runner_pos - pose[:2]
dist = np.linalg.norm(diff)
if dist < 1 and in_line_of_sight(runner_pos, pose[:2]):
f_pos = runner_pos + np.array(
[np.cos(runner_pose[2]),
np.sin(runner_pose[2])]) * dist
f_pos_diff = f_pos - pose[:2]
f_pos_dist = np.linalg.norm(f_pos_diff)
v = f_pos_diff / f_pos_dist * CHASER_SPEED
u, w = feedback_linearized(pose, v, 0.1)
if rev_frames > 0:
u = -CHASER_SPEED
w = 0
vel_msg = Twist()
vel_msg.linear.x = u
vel_msg.angular.z = w
publisher.publish(vel_msg)
if RVIZ_PUBLISH:
position_msg = PointStamped()
position_msg.header.seq = frame_id
position_msg.header.stamp = rospy.Time.now()
position_msg.header.frame_id = '/odom'
pt = Point()
pt.x = pose[X]
pt.y = pose[Y]
pt.z = .05
position_msg.point = pt
rviz_publisher.publish(position_msg)
rate_limiter.sleep()
frame_id += 1
if rev_frames > 0:
rev_frames -= 1
