/**
* This method builds a ModificationRequest srv
* For stop operator, the path can be retreived from srv
* */
void Modifier::buildModificationRequest(const Population& pop, bool imminent_collision, ramp_msgs::ModificationRequest& result)
{
// Push the target paths onto the modification request
std::vector<int> targets;
if(!imminent_collision || (move_dist_ > 100 && move_dir_ > 100))
{
////ROS_INFO("Modifier: No IC");
result.request.op = getOperator();
targets = getTargets(result.request.op, pop);
}
else
{
////ROS_INFO("Modifier: Calling Move because IC exists");
result.request.op = "move";
targets.push_back(pop.calcBestIndex());
}
//////ROS_INFO("targets.size(): %i", (int)targets.size());
for(unsigned int i=0;i<targets.size();i++)
{
//std::cout<<"\nTarget index: "<<targets.at(i);
//std::cout<<"\nPath "<<targets.at(i)<<" size: "<<pop.paths_.at(targets.at(i)).size()<<"\n";
result.request.paths.push_back(
pop.paths_.at(targets.at(i)).buildPathMsg());
}
result.request.move_dir = move_dir_;
result.request.move_dist = move_dist_;
} // End buildModificationRequest
