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