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)
