def _get_next_impl(self): """ Move robot randomly. """ vel_msg = move_helper.get_zero_twist() linear_choices = [ 0, self._jmp_and_rndint(-5, 5), self._jmp_and_rndint(-7, 7), self._jmp_and_rndint(-10, 10) ] if self._speedup: linear_choices = [ 0, self._jmp_and_rndint(-20, -10), self._jmp_and_rndint(10, 20), self._jmp_and_rndint(-100, -30), self._jmp_and_rndint(30, 100) ] angular_z_choices = [ 0, self._jmp_and_rndint(-1, 1), self._jmp_and_rndint(-3, 3), self._jmp_and_rndint(-5, 5) ] vel_msg.linear.x = self._rand_gen.choice(linear_choices) self._rand_gen.jumpahead(7) vel_msg.linear.y = self._rand_gen.choice(linear_choices) self._rand_gen.jumpahead(7) vel_msg.angular.z = self._rand_gen.choice(angular_z_choices) return vel_msg
def _stay_impl(self): """ Alternative to get_next with compromised intelligence: Stop moving as soon as the illegal colour is hit. """ # Normal steps if self._last_colour.data != RandomMoveStrategy._ILLEGAL_COLOUR: return self._get_next_impl() return move_helper.get_zero_twist()
def _pipe(self, data):
""" Pipe the given data """
vel_msg = move_helper.get_zero_twist()
# Decide direction or speed?
if self._just_chose_speed:
self._turtle_walks = data.data > 0
self._just_chose_speed = False
return
if self._turtle_walks:
vel_msg.linear.x = data.data
else:
vel_msg.angular.z = data.data
self._velocity_publisher.publish(vel_msg)
def get_next(self):
""" Get next velocity from movement file """
# Used to use os.getcwd()
self._file_path = os.path.join(BASE_PATH, "data", "move")
if not os.path.isfile(self._file_path):
rospy.logwarn("No movement file found in" + self._file_path)
return None
self._current_line = 0
file_contents = []
# Try to read the whole movement file
try:
file_reader = open(self._file_path)
file_contents = file_reader.readlines()
file_reader.close()
except IOError:
rospy.logwarn("Couldn't read movement file")
return None
vel_msg = move_helper.get_zero_twist()
if self._current_line >= len(file_contents):
rospy.loginfo("End of movement file reached")
return None
# Read next movement command
next_line = file_contents[self._current_line]
self._current_line += 1
# Remove trailing new line
newline_index = len(next_line) - 1
assert(next_line[newline_index:] == "\n")
next_line = next_line[:newline_index]
try:
vel_msg = move_helper.get_twist_from_string(next_line)
except ValueError:
rospy.logerr("Invalid value read from line:\n" + next_line)
return None
# Return read velocity
return vel_msg
def _dont_move_impl(self): """ Alternative to get_next that never moves. """ return move_helper.get_zero_twist()
def stop_turtle(self):
""" Convenience method for stopping turtle (by publishing a zero velocity) """
self.publish(move_helper.get_zero_twist())