void SimpleDeploymentDummy::positionsCallback(const turtlebot_deployment::PoseWithName::ConstPtr& posePtr)
{
ROS_DEBUG("SimpleDeployment: Positions received, finding robot in database");
// Search for agent in the catalog using functor that compares the name to an input string
std::vector<Agent>::iterator it = std::find_if(agent_catalog_.begin(), agent_catalog_.end(), MatchString(posePtr->name) );
// If the agent is already in the catalog, update the position and recalculate the distance.
if ( it != agent_catalog_.end() ) {
ROS_DEBUG("SimpleDeployment: Robot found, updating pose and distance");
it->setPose(posePtr->pose);
it->setDistance(distance(this_agent_.getPose(),posePtr->pose));
}
// else (the agent is not yet in the catalog), create an Agent object and push it into the catalog vector
else {
ROS_DEBUG("SimpleDeployment: Robot not found in database");
if ( posePtr->name != this_agent_.getName() ) {
ROS_DEBUG("SimpleDeployment: Robot is not me. Adding it to database");
// This initializes an object called "agent" with id = 1, the pose of the incoming message, and the distance from this agent to the agent that published the message
Agent agent( 1, *posePtr, distance(this_agent_.getPose(), posePtr->pose) );
agent_catalog_.push_back( agent );
}
else {
ROS_ERROR("SimpleDeployment: Robot is me! Updating position and adding to database");
this_agent_.setPose(posePtr->pose);
got_me_ = true;
// This initializes an object called "agent" with id = 0, the pose of the incoming message, and a distance of 0.0;
Agent agent( 0, *posePtr, 0.0 );
agent_catalog_.push_back( agent );
}
}
// Sort agent list on distance (using functor)
std::sort( agent_catalog_.begin(), agent_catalog_.end(), SortAgentsOnDistance() );
ROS_DEBUG("SimpleDeployment: Positions processed");
}
