def main(): global goal_client, exploration_client, rate, status rospy.init_node('homework5_navigator') rate = rospy.Rate(10) rospy.Subscriber("/pose", Odometry, odometryHandler) rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, initialPoseHandler) rospy.Subscriber("/move_base/result", MoveBaseActionResult, moveBaseActionResultHandler) rospy.Subscriber("/explore_server/result", ExploreTaskActionResult, exploreTaskActionResultHandler) rospy.Subscriber("/tag_detections", AprilTagDetectionArray, tagDetectionsHandler) velPub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) motPub = rospy.Publisher("/cmd_motor_state", MotorState, queue_size=10) motor = MotorState() motor.state = 1 print "starting motors..." for i in xrange(20): motPub.publish(motor) rate.sleep() print "\n--------------------------------------" print "set the initial pose in rviz ya fool!!!" print "--------------------------------------\n" while initial_pose == None: rate.sleep() #TODO time = 0 three_minutes = 3 # MAIN LOOP print "exploration!" startExploration()
def main(): global goal_client, exploration_client, rate, initial_time rospy.init_node('fembots_finder') rate = rospy.Rate(10) rospy.Subscriber("/pose", Odometry, odometryHandler) rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, initialPoseHandler) rospy.Subscriber("/tag_detections", AprilTagDetectionArray, tagDetectionsHandler) velPub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) motPub = rospy.Publisher("/cmd_motor_state", MotorState, queue_size=10) motor = MotorState() motor.state = 1 print "starting motors..." for i in xrange(20): motPub.publish(motor) rate.sleep() print "\n--------------------------------------" print "set the initial pose in rviz ya fool!!!" print "--------------------------------------\n" while initial_pose == None: rate.sleep() initial_time = int(round(time.time() * 1000)) #TODO # MAIN LOOP print "exploration!" startExploration() rospy.spin()
def main(): global point global status print 'what... are your ending coordinates?' rospy.init_node('homework4_navigator') rate = rospy.Rate(10) sub = rospy.Subscriber('/clicked_point', PointStamped, subFn) rospy.Subscriber("/move_base/result", MoveBaseActionResult, callback) rospy.Subsciber("/tag_detections_pose", PoseArray, aprilFn) while (point == None): rate.sleep() print "x: ", point.x x = point.x print "y: ", point.y y = point.y velPub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) motPub = rospy.Publisher("/cmd_motor_state", MotorState, queue_size=10) #goalPub = rospy.Publisher("/move_base_simple/goal", PoseStamped, queue_size=10) motor = MotorState() motor.state = 1 print "make sure you start the motors!" #for i in xrange(20): # motPub.publish(motor) # rate.sleep() client = actionlib.SimpleActionClient("move_base", MoveBaseAction) #Waits until the action server has started up and started listening for goals client.wait_for_server() goal = MoveBaseGoal() goal.target_pose.header.frame_id = "map" goal.target_pose.header.stamp = rospy.get_rostime() goal.target_pose.pose.position.x = x goal.target_pose.pose.position.y = y goal.target_pose.pose.orientation.w = 1.0 print "Sending goal!" client.send_goal(goal) while (status != 3): rate.sleep() print "Result:", client.get_result()
def main(args): global goal_client, exploration_client, rate hide_time = 60 # default hide time 60 seconds #hide_time = 30 #hide_time = 120 if (len(args) > 0): hide_time = int(args[0]) print 'set hide time to ', hide_time rospy.init_node('fembots_hider') rate = rospy.Rate(10) rospy.Subscriber("/pose", Odometry, odometryHandler) rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, initialPoseHandler) rospy.Subscriber("/scan", LaserScan, scanHandler) velPub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) motPub = rospy.Publisher("/cmd_motor_state", MotorState, queue_size=10) motor = MotorState() motor.state = 1 print "starting motors..." for i in xrange(20): motPub.publish(motor) rate.sleep() print "\n--------------------------------------" print "set the initial pose in rviz ya fool!!!" print "--------------------------------------\n" while initial_pose == None: rate.sleep() # MAIN LOOP try: t = threading.Thread(target=waitForHideTime, args=(hide_time, )) t.start() except: print 'Error: couldn\'t create thread' print "exploration!" startExploration()
def main(args): global goal_client, exploration_client, rate hide_time = 60 # default hide time 60 seconds #hide_time = 30 #hide_time = 120 if (len(args) > 0): hide_time = int(args[0]) print 'set hide time to ' ,hide_time rospy.init_node('fembots_hider') rate = rospy.Rate(10) rospy.Subscriber("/pose", Odometry, odometryHandler) rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, initialPoseHandler) rospy.Subscriber("/scan", LaserScan, scanHandler) velPub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) motPub = rospy.Publisher("/cmd_motor_state", MotorState, queue_size=10) motor = MotorState() motor.state = 1 print "starting motors..." for i in xrange(20): motPub.publish(motor) rate.sleep() print "\n--------------------------------------" print "set the initial pose in rviz ya fool!!!" print "--------------------------------------\n" while initial_pose == None: rate.sleep() # MAIN LOOP try: t = threading.Thread(target = waitForHideTime,args=(hide_time,)) t.start() except: print 'Error: couldn\'t create thread' print "exploration!" startExploration()
def main(): global goal_client, exploration_client, rate, status rospy.init_node('homework5_navigator') rate = rospy.Rate(10) rospy.Subscriber("/pose", Odometry, odometryHandler) rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, initialPoseHandler) rospy.Subscriber("/move_base/result", MoveBaseActionResult, moveBaseActionResultHandler) rospy.Subscriber("/explore_server/result", ExploreTaskActionResult, exploreTaskActionResultHandler) rospy.Subscriber("/scan", LaserScan, scanHandler) velPub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) motPub = rospy.Publisher("/cmd_motor_state", MotorState, queue_size=10) motor = MotorState() motor.state = 1 print "starting motors..." for i in xrange(20): motPub.publish(motor) rate.sleep() print "\n--------------------------------------" print "set the initial pose in rviz ya fool!!!" print "--------------------------------------\n" while initial_pose == None: rate.sleep() #TODO # after some fixed time kill exploration server # choose a hiding spot and navigate there # see chooseHidingSpot # MAIN LOOP print "exploration!" startExploration()
def processJoystick(self, joyMsg): state = MotorState() state.state = 4 self.motPub.publish(state) joyMsg.axes = numpy.array(joyMsg.axes) left_axis = joyMsg.axes[[0,1]] right_axis = joyMsg.axes[[2,5]] l2_axis = joyMsg.axes[[3]] r2_axis = joyMsg.axes[[4]] square, x, circle, triangle, l1, r1, l2, r2, share, options, l3, r3, ps_button, touch_button = [button == 1 for button in joyMsg.buttons] left_dpad = joyMsg.axes[6] == 1 right_dpad = joyMsg.axes[6] == -1 up_dpad = joyMsg.axes[7] == 1 down_dpad = joyMsg.axes[7] == -1 vel = Twist() vel.linear.x = self.max_lin_vel * left_axis[1] if x: vel.linear.x = vel.linear.x / 2 vel.angular.z = self.max_ang_vel * left_axis[0] self.velPub.publish(vel)
if __name__ == "__main__": ns = "pioneer" sonSub = rospy.Subscriber("/sonar", SonarArray, processSonar) odoSub = rospy.Subscriber("/pose", Odometry, processOdometry) #publishers velPub = rospy.Publisher("/cmd_vel", Twist) motPub = rospy.Publisher("/cmd_motor_state", MotorState) rate = rospy.Rate(1) count = 0 vel = Twist() vel.linear.x = 0.1 motor = MotorState() motor.state = 1 while not rospy.is_shutdown(): motPub.publish(motor) velPub.publish(vel) rate.sleep() if count > 15: vel.linear.x = 0.0 velPub.publish(vel) motor.state = 0 motPub.publish(motor) break count += 1
def callback(self, rgb_data, dist_data, sonar_data): #def callback(self, rgb_data, dist_data): # self.crt = True # print self.crt vel = Twist() state = MotorState() state.state = 4 self.motor_state_pub.publish(state) vel.linear.x = 0.0 vel.angular.z = 0.0 if self.alo == 'right': vel.linear.x = 0.0 vel.angular.z = -0.2 self.cmd_vel_pub.publish(vel) rospy.sleep(4) self.alo = '' self.crt = False elif self.alo == 'left': vel.linear.x = 0.0 vel.angular.z = 0.2 self.cmd_vel_pub.publish(vel) rospy.sleep(2) vel.linear.x = 0.0 vel.angular.z = 0.0 self.cmd_vel_pub.publish(vel) rospy.sleep(2) self.alo = '' self.crt = False self.soundhandle.playWave(self.wavepath + "Final01.wav") rospy.sleep(20) self.soundhandle.playWave(self.wavepath + "Final02.wav") rospy.sleep(22) self.soundhandle.playWave(self.wavepath + "Final03.wav") rospy.sleep(23) self.soundhandle.playWave(self.wavepath + "Final04.wav") rospy.sleep(14) self.soundhandle.playWave(self.wavepath + "Final05.wav") rospy.sleep(14) self.soundhandle.playWave(self.wavepath + "Final06.wav") rospy.sleep(20) self.soundhandle.playWave(self.wavepath + "Final07.wav") rospy.sleep(30) elif self.crt: try: image = self.bridge.imgmsg_to_cv2(rgb_data, "bgr8") except CvBridgeError, e: print e (rows, cols, channels) = image.shape hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) px = cols/2 py = rows/2 h_high = 179 h_low = 160 s_high = 255 s_low = 130 v_high = 255 v_low = 0 lower = np.array([h_low, s_low, v_low]) upper = np.array([h_high, s_high, v_high]) # Threshold the HSV image to get only orange colors mask = cv2.inRange(hsv, lower, upper) #erosion kernel = np.ones((5,5),np.uint8) erosion = cv2.erode(mask,kernel,iterations=3) #dilation dilation = cv2.dilate(erosion,kernel,iterations=3) img = cv2.medianBlur(dilation,21) moment = cv2.moments(img) try: px = int(moment['m10']/moment['m00']) py = int(moment['m01']/moment['m00']) area = moment['m00'] except ZeroDivisionError: pass cv2.circle(img, (px, py), 3, (171,110,0), 2) cv2.circle(img, (px, py), 3, (171,110,0), 2) #cv2.imshow('Color Segmentation', img) sonar_array = list(sonar_data.ranges) # if min(sonar_array[1:3]) < 0.3: # vel.linear.x = 0.0 # vel.angular.z = -0.5 # self.fala = 1 # self.cmd_vel_pub.publish(vel) # elif min(sonar_array[3:5]) < 0.3: # vel.linear.x = 0.0 # vel.angular.z = 0.0 # self.fala = 1 # self.cmd_vel_pub.publish(vel) # elif min(sonar_array[5:7]) < 0.3: # vel.linear.x = 0.0 # vel.angular.z = 0.5 # self.fala = 1 # self.cmd_vel_pub.publish(vel) # elif min(sonar_array[10:14]) < 0.5: # vel.linear.x = 0.0 # vel.angular.z = 0.0 # self.fala = 1 # self.cmd_vel_pub.publish(vel) # else: #print "ok" if dist_data.distance > 0.8 and dist_data.distance < 1.5: self.motor_state_pub.publish(state) #rospy.loginfo('lim-left: {0} < px: {1} < lim-right: {2}'.format((cols/3), px, (cols - (cols/3)))) if ((cols/3) > px and px > 0) or (dist_data.y > 0.2):# or max(sonar_array[0:3]) > 0.4: # Goes to the left # rospy.loginfo('left <<<<<<<<') vel.linear.x = 0.0 vel.angular.z = 0.3 elif ((cols - (cols/3)) < px and px < cols) or (dist_data.y < -0.2):# or max(sonar_array[5:8]) > 0.4: # Goes to the right # rospy.loginfo('right >>>>>>>>') vel.linear.x = 0.0 vel.angular.z = -0.3 # elif max(sonar_array[8:15]) < 0.4: # vel.linear.x = 0.0 # vel.angular.z = 0.0 else: # rospy.loginfo('Distancia: %.4f' % dist_data.distance) # Foward self.fala = 0 vel.linear.x = 0.2 #vel.linear.x = 0.25 * math.sqrt(dist_data.x**2 + dist_data.y**2) vel.angular.z = 0.0 self.cmd_vel_pub.publish(vel) else: if dist_data.distance < 0.8 and self.fala == 0: #self.crt = False self.speech_pub.publish("I am close to you.") self.fala = 1 elif dist_data.distance > 1.5 and self.fala == 0: # self.speech_pub.publish("You are very fast. Please, step back and wait for me.") self.speech_pub.publish("You are very fast. Please, wait for me.") self.fala = 1 else: pass vel.linear.x = 0.0 vel.angular.z = 0.0
if __name__ == "__main__": print 'what... are your ending coordinates?' print "x: " x = float(raw_input()) print "y: " y = float(raw_input()) rospy.init_node('homework4_navigator') rate = rospy.Rate(10) velPub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) motPub = rospy.Publisher("/cmd_motor_state", MotorState, queue_size=10) #goalPub = rospy.Publisher("/move_base_simple/goal", PoseStamped, queue_size=10) motor = MotorState() motor.state = 1 print "make sure you start the motors!" #for i in xrange(20): # motPub.publish(motor) # rate.sleep() client = actionlib.SimpleActionClient("move_base", MoveBaseAction) #Waits until the action server has started up and started listening for goals client.wait_for_server() goal = MoveBaseGoal() goal.target_pose.header.frame_id = "map" goal.target_pose.header.stamp = rospy.get_rostime()
class follow_person: def __init__(self): self.bridge = CvBridge() image_sub = message_filters.Subscriber("camera/rgb/image_color", Image) dist_sub = message_filters.Subscriber("distance_person", Distance) self.ts = message_filters.ApproximateTimeSynchronizer([image_sub, dist_sub], 10, 0.5) self.ts.registerCallback(self.callback) self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) self.motor_state_pub = rospy.Publisher("/cmd_motor_state", MotorState, queue_size=10) self.speech_pub = rospy.Publisher("/speech", String, queue_size=10) self.fala = 1 def callback(self, rgb_data, dist_data): try: image = self.bridge.imgmsg_to_cv2(rgb_data, "bgr8") except CvBridgeError, e: print e (rows, cols, channels) = image.shape hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) px = cols/2 py = rows/2 h_high = 130 h_low = 75 s_high = 255 s_low = 130 v_high = 255 v_low = 0 lower = np.array([h_low,s_low,v_low]) upper = np.array([h_high,s_high,v_high]) # Threshold the HSV image to get only orange colors mask = cv2.inRange(hsv, lower, upper) #erosion kernel = np.ones((5,5),np.uint8) erosion = cv2.erode(mask,kernel,iterations=3) #dilation dilation = cv2.dilate(erosion,kernel,iterations=3) img = cv2.medianBlur(dilation,21) moment = cv2.moments(img) try: px = int(moment['m10']/moment['m00']) py = int(moment['m01']/moment['m00']) area = moment['m00'] except ZeroDivisionError: pass cv2.circle(img, (px, py), 3, (171,110,0), 2) cv2.circle(img, (px, py), 3, (171,110,0), 2) cv2.imshow('Color Segmentation', img) vel = Twist() state = MotorState() state.state = 4 if dist_data.distance > 0.7 and dist_data.distance < 1.7: self.motor_state_pub.publish(state) #rospy.loginfo('lim-left: {0} < px: {1} < lim-right: {2}'.format((cols/3), px, (cols - (cols/3)))) if ((cols/3) > px and px > 0) or (dist_data.y > 0.2): #Goes to the left #rospy.loginfo('left <<<<<<<<') vel.linear.x = 0.0 vel.angular.z = 0.2 elif ((cols - (cols/3)) < px and px < cols) or (dist_data.y < -0.2): #Goes to the right #rospy.loginfo('right >>>>>>>>') vel.linear.x = 0.0 vel.angular.z = -0.2 else: #rospy.loginfo('Distancia: %.4f' % dist_data.distance) #Foward self.fala = 0 vel.linear.x = 0.2 vel.angular.z = 0.0 self.cmd_vel_pub.publish(vel) else: if dist_data.distance < 0.7 and self.fala == 0: self.speech_pub.publish("I am closer to you.") self.fala = 1 elif dist_data.distance > 1.7 and self.fala == 0: self.speech_pub.publish("You are very fast. Please, step back and wait for me.") self.fala = 1 else: pass vel.linear.x = 0.0 vel.angular.z = 0.0 self.cmd_vel_pub.publish(vel) cv2.waitKey(3)
def enable_motors(self): motor_state_msg = MotorState() motor_state_msg.state = 1 self.motor_state_pub.publish(motor_state_msg) rospy.loginfo("Motors Enabled") self.motor_enabled = True
print sonMsg.ranges if __name__ == "__main__": ns = "pioneer" sonSub = rospy.Subscriber("/sonar", SonarArray, processSonar) odoSub = rospy.Subscriber("/pose", Odometry, processOdometry) #publishers velPub = rospy.Publisher("/cmd_vel", Twist) motPub = rospy.Publisher("/cmd_motor_state", MotorState) rate = rospy.Rate(1) count = 0 vel = Twist() vel.linear.x = 0.1 motor = MotorState() motor.state = 1 while not rospy.is_shutdown(): motPub.publish(motor) velPub.publish(vel) rate.sleep() if count > 15: vel.linear.x = 0.0 velPub.publish(vel) motor.state = 0 motPub.publish(motor) break count += 1