def __init__(self): # Set parameters self.flag = 1 self.gap = .7 #space needed to pass through self.agent_id = 1 #Number for Robot $$change self.stage = 0 #initial target self.substage = 0 #initial task 0 = drive to objective self.topicConfig() #configure simulation namspaces or actual huskies self.connection = gpsLocalization(self.namespace, self.gpsTopic, self.imuTopic, self.xOffset, self.yOffset) #Connection which will give current position self.path_planner = GapFinder(self.gap) #Finds gaps that the robot can enter self.actuation = ROS2DimActuate(self.controlTopic) #Controls the motion of the robot #Create a tracker which knows how to move the robot and get it's position self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates) #Tell the tracker which robot to command self.tracker.setID(self.agent_id) #Countdown to start sleep_time = 3 while sleep_time > 0: print "Mission starts in:", sleep_time sleep_time -= 1 sleep(1) self.distance = [] self.prev_closest_reading = 0.0 self.prev_time = time() self.crash_avert_velocity = 0.0 print 'Starting the Navigation' self.subscriber = rospy.Subscriber(self.lidarTopic, LaserScan, self.move, queue_size=1) #Call move for each laser scan rospy.spin()
def __init__(self): #set parameters self.gap = .7 #space needed to pass through self.agent_id = 0 self.topicConfig() self.connection = gpsLocalization(argv[3],self.gpsTopic,self.imuTopic) #Connection which will give current position self.path_planner = GapFinder(self.gap) #Finds gaps that the robot can enter self.actuation = ROS2DimActuate(self.controlTopic) #Controls the motion of the robot self.actuation.setAngularVelocityLimit(.5) #Sets the maximum velocity #Create a tracker which knows how to move the robot and get it's position self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates) #Tell the tracker which robot to command self.tracker.setID(self.agent_id) self.distance = [] self.prev_closest_reading = 0.0 self.prev_time = time() self.crash_avert_velocity = 0.0 print 'Starting the Navigation' sleep(2) self.subscriber = rospy.Subscriber(self.lidarTopic, LaserScan, self.move, queue_size=1) #Call move for each laser scan rospy.spin()
def __init__(self): self.path_planner = GapFinder(.6) self.logger = [] self.connection = gpsLocalization('husky1test','husky1/odometry/gps','husky1/imu/data',0,0) self.subscriber = rospy.Subscriber('husky1/scan', LaserScan, self.readDistance) self.rate = rospy.Rate(10) # set rate to 10 Hz while not rospy.is_shutdown() and len(self.logger) < 1: self.rate.sleep()