def execute(self, userdata):
"""DriveBackward state
Driving backwards for a given time.
"""
return_state = "locate"
Utils.drive(Twist(linear=Vector3(x = -userdata.in_drive_speed, y = 0.0 , z = 0.0)))
rospy.sleep(userdata.in_drive_time)
Utils.drive(Utils.STOP)
return return_state
def execute(self, userdata):
"""TurnRight state
Turning to the right for a given time.
"""
return_state = "locate"
Utils.drive(Twist(angular=Vector3(x = 0.0, y = 0.0 , z = -userdata.in_turn_speed)))
print "###Driving for:", userdata.in_drive_time
print "###Driving with:", userdata.in_turn_speed
rospy.sleep(userdata.in_drive_time)
Utils.drive(Utils.STOP)
return return_state
def execute(self, userdata):
"""DriveLeft state
Driving to the left for a given time.
"""
return_state = "locate"
Utils.drive(Twist(linear=Vector3(x = 0.0, y = userdata.in_drive_speed , z = 0.0)))
print "###Driving for:", userdata.in_drive_time
print "###Driving with:", userdata.in_turn_speed
rospy.sleep(userdata.in_drive_time)
Utils.drive(Utils.STOP)
return return_state
def get_turn_params(self):
"""Returns a tupel of: (rotation_state, rotation_speed, driving_time)
"""
angle = Utils.get_wall_angle(SmachGlobalData.msg_line)
#angle = math.copysign(angle, self.get_x_dif_fl_fr())
(return_speed, return_time) = self.get_speed_and_time(angle, ROTATION_MAX)
return_state = self.get_return_state(angle, "turnLeft", "turnRight")
return (return_state, return_speed, return_time)
def get_front_average_point(self, left_border, right_border):
array = Utils.get_carts_in_range(left_border, right_border, self.get_msg_laser(), Utils.FRONT)
return Utils.array_average_point(array)
def get_side_average_point(self, back_border, front_border):
array = Utils.get_carts_in_range(back_border, front_border, self.get_msg_laser(), Utils.SIDE)
if DEBUG:
#rospy.logdebug("side_array: =%s", array)
pass
return Utils.array_average_point(array)
def get_msg_laser(self):
if self.msg_laser == None:
self.wait_for_laser_msgs(Utils.BUFFERSIZE)
self.msg_laser = Utils.laser_average()
return self.msg_laser
def is_not_collimated(self):
angle = Utils.get_wall_angle(SmachGlobalData.msg_line)
print "wall_angle", angle
return abs(angle) > GOAL_ANGLE_TOLERANCE
