Sheep Movement - TomHulme/306-Swarm-Robotics-Project GitHub Wiki

Overview

Employs a Finite State Machine representing the movement of the sheep. There are 3 states: FSM_MOVE_FORWARD, FSM_ROTATE and FSM_GOTO_GOAL, which are self-explanatory as to what each state is. Use of a FSM enables for an easier way to control how the robots/sheep move. Future states will be added as the sheep become more complex (such as moving to a destination, stop moving when eating grass etc.)

MOVE_FORWARD

In the MOVE_FORWARD state, the robots simply move forward at a certain velocity while scanning ahead for any walls within its field of view. The node subscribes to the <sensor_msgs::LaserScan> topic to enable the use of scanning ahead to help avoid walls. For each angle in the robots sensor field of view, it scans through the ranges array in the topic message msg->ranges[minIndex]. As it scans through, it records the range between itself and a wall. If any of the ranges in the array is less than the specified distance that the sheep must be from a wall, then the FSM changes state to ROTATE.

ROTATE

In the ROTATE state, the robot spins around for a certain length of time to face towards an open space and changes back to the MOVE_FORWARD state.

GOTO_GOAL

In the GOTO_GOAL state, it triggers a variable which makes the sheep follow a goal finding algorithm. This goal algorithm consists of getting the current position of the sheep, and the goal/destination position and then using Pythagoras to determine which direction the sheep should be facing in order to move forward to its goal. Once the correct heading has been established, the sheep rotates until it is facing in the right direction. After that, the sheep moves forward until it reaches its goal. Once it reaches its goal, depending on why the sheep needed to go towards the goal (either by going towards grass to eat it, or to reach a position away from the sheepdog); it either stops in its place or it resumes with the MOVE_FORWARD state.

Origin problems

At the start of the project, there was a problem of the sheep positions being relative to where they started. This was noticed during the implementation of the sheep goal. It was decided that rather than do mapping of the robot local references, it would be easier to have all positions relative to origin. This was made possible by adding localization_origin [0 0 0 0] to each definition of a robot in the world file.