def _updateLocation(self): print "Updating location" try: self._map_list.waitForTransform('map', 'base_footprint', rospy.Time(0), rospy.Duration(0.5)) except tf.Exception: print "Waiting for transform failed" self.current_x = tools.gmapifyValue(0) self.current_y = tools.gmapifyValue(0) self.current_z = tools.gmapifyValue(0) return False (p,q) = self._map_list.lookupTransform("map","base_footprint",rospy.Time(0)) x,y,z = p self.current_theta = tools.normalizeTheta(q) self.current_x = tools.gmapifyValue(x) self.current_y = tools.gmapifyValue(y) self.current_z = tools.gmapifyValue(z) self._currentSet = True print "The values are: ",self.current_x, self.current_y # self.current_x = tools.gmapifyValue(self._newX) # self.current_y = tools.gmapifyValue(self._newY) self._currentSet = True # print "The values are: ",self.current_x, self.current_y return True
def rotate(self, angle, speed = 0.05): """ :param angle: <int or double> the angle the robot should turn in degrees :param speed: <int or double> The speed at which the robot should rotate :return: None """ ## Error Checking Angle: if angle == 0: return else: done = False angle_to_travel_rad = math.radians(angle) start_theta_rad = tools.normalizeTheta(self._quat) print angle_to_travel_rad, start_theta_rad print ("angle to travel", "start theta", "Current theta", "d_theta") while not done and (not rospy.is_shutdown()): current_theta_rad = tools.normalizeTheta(self._quat) # Calculates the difference in the thetas # This keeps track of debug_state = None if start_theta_rad > current_theta_rad and start_theta_rad > math.pi*3/2 and current_theta_rad < math.pi/2: d_theta = ((2*math.pi) - start_theta_rad) + current_theta_rad debug_state = 1 elif start_theta_rad < current_theta_rad and start_theta_rad < math.pi/2 and current_theta_rad > math.pi*3/2: d_theta = ((2*math.pi) - current_theta_rad) + start_theta_rad debug_state = 2 else: d_theta = start_theta_rad - current_theta_rad debug_state = 3 print angle_to_travel_rad, start_theta_rad, current_theta_rad, d_theta, debug_state ## If you're within 0.1 Radian stop if abs(angle_to_travel_rad) - abs(d_theta) < 0.05: done = True self._stopRobot() else: if (angle > 0): self._spinWheels(speed,-speed,.1) else: self._spinWheels(-speed,speed,.1)
def _updateLocation(self): try: self._map_list.waitForTransform('map', 'base_footprint', rospy.Time.now(), rospy.Duration(0.5)) except tf.Exception: print "TF library chunkin out broski" return (p,q) = self._map_list.lookupTransform("map","base_footprint",rospy.Time.now()) self.current_x,self.current_y, self.current_z = p self.current_theta = tools.normalizeTheta(q) self.current_x = tools.mapifyValue(self.current_x) - 1 self.current_y = tools.mapifyValue(self.current_y)
def storeGoal(self,msg): # goalX = tools.mapifyValue(msg.pose.position.x) -1 # goalY = tools.mapifyValue(msg.pose.position.y) goalX = tools.gmapifyValue(msg.pose.position.x) goalY = tools.gmapifyValue(msg.pose.position.y) goaltheta= tools.normalizeTheta((msg.pose.orientation.x,msg.pose.orientation.y,msg.pose.orientation.z,msg.pose.orientation.w)) self._goal_,self._current_ = self.map.storeGoal(goalX,goalY,goaltheta) try: path = self.map.getNextWaypoint() print path except RuntimeError,e: print e
def odomCallback(self, msg): """ This will handle the odom frame location and information :param msg: this is a message form the odom subscriber :return: None """ if msg is None: raise RuntimeError("Message is None") if self._x_offset is None or self._y_offset is None: # print "Offsets are none" return pos,quat = self._quatFromMsg(msg) # print "HERE THEY ARE: ",pos, quat, self._x, self._y, self._x_offset, self._y_offset self._x, self._y, self._z = pos self._x = self._x + self._x_offset self._y = self._y + self._y_offset self._quatx, self._quaty, self._quatz, self._quatw = quat self._quat = (self._quatx, self._quaty, self._quatz, self._quatw) self._theta = tools.normalizeTheta(self._quat)