bclab-unm / cpfa-ros Goto Github PK

I merged the SwarmBaseCode into the CPFA. I found one of the rovers did not move when the simulation started.

The locations of site fidelity should be checked.
The site fidelity only last in current foraging round. It should be used only once.

Rovers pickup a tag and drive away to somewhere directly and then they could not find the nest.

When a rover switches to SearchLocationType PHEROMONE, it is expected that it will select one of the pheromones from the pheromone list. However, instead the rovers are failing to select any pheromone. Because of this, while users may see that the rostopic echo stream states that the rover is using PHEROMONES, in actuality it is using its old target location.

This error is obvious when a rover leaves the nest in PHEROMONE mode but fails to go to any actual cluster location. The robot that lays the pheromone appears to be using the correct pheromone location but is actually using its last target location.

This can be verified using a print statement after the for loop in the setPheromones function in CPFASearchController.

After observing the rover in a travel state using a random heading, it reached the edge of the arena. We had programmed it to iterating through the CPFA state machine call through the CPFA search controller, which would eventually trigger the rover to switch to an uninformed search. I then realized that this means that the rover would inevitably switch to an uninformed search, it was just a matter of time before it switches while running into the wall. We might as well just have the rover switch to an uninformed search as soon as it hits the edge of the wall because in a sense, we are just delaying this inevitability.

After observing the rover in an uninformed search state at the edge of the arena, I noticed that it kept trying to travel into the wall. This logically means that it set a goal location outside of the wall.

Solution: As soon as the rover's current location + the search step size is equivalent to the edge of the arena, then we can just switch to uninformed search state.

Once rovers detect cubes on the way to search location, they switch to search automatically. No need to sample the random value for switching to search probability

The rover cannot pick up a target. It moves backward when it is close to targets

As the rover transitions from a travel state and starts to search near the collection zone and the blocks in the collection zone are visible to the rover and it will start to count them.

We want to avoid the rover counting the blocks in the collection zone because it will set the blocks in the center as a target location that will trigger site fidelity or even insert it as a pheromone.

The solution will be to ignore targets (april tags with a 0 ID) as resources while it's in a search state near the collection zone.

A rover is close to the nest. It gets stuck when it detects the tags on the

collection disk.

At the beginning of the simulation when all the rovers enter a travel state, if the direction that the rovers travel at leads to a collision due a vector intersection, then the rovers will trigger the obstacle avoidance handler in mobility.

If the rover happens to be in the collection zone during this case, then as soon as the rover exits continue interrupted search then it will stay stuck in the nest trying to avoid the nest.

We can solve this by having the rovers avoid traveling in a way where their velocity vectors will intersect as well as having the rovers not calculate a random heading that leads to them driving into the nest.

A rover turns to left and right repeatedly and does not move away from current location. After a while, it stops, then it moves normally.

While in a travel state the rover will ignore tags, meaning that if the rover is traveling and happens to be in the collection zone it can transition into a search state inside the collection zone. If the rover is a in a search state, then it stops ignoring tags , meaning that it will start to push out blocks in the collection zone and get stuck.

We need to calculate all travel paths to avoid the collection zone.

Rovers make turns but not drive when avoiding obstacles. This does not help them to avoid the collision. They just turn to right and them left. The situation comes back to the beginning. We need a random function to update the avoidance strategy.