def responder(name): global path global start global goal global grid global need_next last = None current = None # initalize the publishers directions = rospy.Publisher('/'+ zumy_name + "/next_cell", startGoal, queue_size=10) next_dest = rospy.Publisher('/'+ zumy_name + "/nav_complete", doneWith, queue_size=10) rospy.init_node('navigator', anonymous=True) r = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): #publish on all of my publishing topics if not stop: if not updated or dirty: # start a new path path = pth.gen_path(start, goal, grid) print path, start, goal last = 0 current = 0 elif need_next: # send the next location in the path last = current current += 1 need_next = False # keep resending the old directions if current == len(path): next_dest.publish(doneWith(True, goal[0], goal[1])) else: print path[current] directions.publish(path[last][0], path[last][1], path[current][0], path[current][1]) r.sleep() rospy.signal_shutdown("should be dead")
plt.show(block=False) if reached and points[idx, 0] == desti[0] and points[idx, 1] == desti[1]: # print desti idx += 1 reached = False if idx < len(points): # print 'we reached this shit' pub_nav.publish(startGoalCentroid(zumy_pos[0],zumy_pos[1], points[idx,0], points[idx,1], cent[0], cent[1])) else: pub_nav.publish(startGoalCentroid(zumy_pos[0],zumy_pos[1], points[idx,0], points[idx,1], cent[0], cent[1])) elif explore > 0: # print done print "explore done: " + str(target) done = True # print 'we did not reach this shit' pub_fin.publish(doneWith(True, target[0], target[1])) else: # print done print "explore done: " + str(target) done = True # print 'we did not reach this shit' pub_fin.publish(doneWith(True)) # stateUpdate() except Exception as e: print 'insufficient data: ' + str(e) rate.sleep() rospy.signal_shutdown("should be dead")
homogeneous = eqf.compute_gab(rb_cur,rb_des) print 'h**o'+str(homogeneous) trans = homogeneous[0:3,3] # (omega,theta) = eqf.find_omega_theta(homogeneous[0:2,0:2]) # try: # (trans, rot) = listener.lookupTransform(rw_des, rw_cur, rospy.Time(0)) # except: # print "not enough" # continue twist = [[0,0,0], [0,0,0]] unfound = True print 'trans'+str(trans) if trans[0]**2+ trans[1]**2+ trans[2]**2 < .01: pub.publish(Twist(Vector3(0,0,0), Vector3(0,0,0))) # print 'twist'+str(twist[0][0]**2+ twist[0][1]**2+ twist[0][2]**2) pub_state.publish(doneWith(True, rw_des.translation.x, rw_des.translation.y)) unfound = False elif trans[0] < 0: if abs(trans[1]) > .3: if trans[1] < 0: twist[1] = [0,0,-.2] else: twist[1] = [0,0,.2] else: if abs(trans[0]) > .3: if trans[0] < 0: twist[1] = [0,0,.2] else: twist[1] = [0,0,-.2] else: twist[0] = [.2,0,0]
def responder(name): global path, start, goal, grid, need_next, updated, current, stop current += 1 last = None # initalize the publishers directions = rospy.Publisher('/'+ zumy_name + "/next_cell", startGoal, queue_size=10) next_dest = rospy.Publisher('/'+ zumy_name + "/nav_complete", doneWith, queue_size=10) rospy.init_node('navigator', anonymous=True) r = rospy.Rate(3) # 10hz while not rospy.is_shutdown(): #publish on all of my publishing topics # print updated # print grid # if zumy is not None: # print "grid zumy: " + str((zumy[0], zumy[1])) if grid is not None and updated: # start a new path path = pth.gen_path(start, goal, grid) print 'path'+str(path) # print 'path, start, goal from explorer: ' + str((path, start, goal)) # print path, start, goal last = 0 current = 0 updated = False stop = False elif not stop: if need_next: # send the next location in the path # print current last = current current += 1 need_next = False # keep resending the old directions if current >= len(path): # print "current" # print current if current == len(path) and cent is not None: print "turn_directive" # plt.imshow(grid, interpolation='nearest') # plt.pause(.001) # plt.show(block=False) directions.publish(startGoal(-2, -2, cent[0], cent[1])) else: print "done" directions.publish(startGoal(-3, -3, 3, 3)) next_dest.publish(doneWith(True, goal[0], goal[1])) else: print "current: " + str(path[current]) gzum = grid.astype(float) for pnt in path[current:]: gzum[pnt] = .75 gzum[zumy] = .5 plt.imshow(gzum, interpolation='nearest', origin ='lower') plt.pause(.001) plt.show(block=False) # print 'path[current], current dest' # print "path current: " + str(path[current]) # directions.publish(path[last][0], path[last][1], 3, 3) directions.publish(path[last][0], path[last][1], path[current][0], path[current][1]) # directions.publish(3,3,1,1) r.sleep() rospy.signal_shutdown("should be dead")
ang = np.arctan(trans[1]/trans[0]) if trans[1] < 0 and trans[0] < 0: ang = -np.pi + ang elif trans[1] > 0 and trans[0] < 0: ang = np.pi + ang # print 'ang angnew ' + str(ang) + str(np.arctan(trans[1]/trans[0])) ang_buff.append(ang) ang = np.median(np.asarray(list(ang_buff))) print "angle: " + str(ang) + " x: " + str(trans[0]) + "y: " + str(trans[1]) # print homogeneous, rb_cur, rb_des if trans[0]**2+ trans[1]**2 < .0006 or done: print done pub.publish(Twist(Vector3(0,0,0), Vector3(0,0,0))) # print 'twist'+str(twist[0][0]**2+ twist[0][1]**2+ twist[0][2]**2) pub_state.publish(doneWith(True, round((rw_des.translation.x - length/2)/length), round((rw_des.translation.y - length/2)/length))) unfound = False forward_mode = False print doneWith(True, round((rw_des.translation.x - length/2)/length), round((rw_des.translation.y - length/2)/length)) # print 'hella small' # print unfound # print unfound else: if abs(ang) < .01 or forward_mode: # forward_mode = True if turn: pub_state.publish(doneWith(True, -2, -2)) else: if trans[0] < 0: twist[0] = [-1*trans[0]*.6,0,0]