void remove_dead_agents(void) {
for(agent_iter iter=agents.begin(); iter != agents.end() ; iter++) {
if(is_dead(*iter)) {
(*iter)->kill(0);
printf("kill %s\n", (*iter)->getName().c_str());
}
}
int status; pid_t dead_pid;
while ((dead_pid = waitpid(-1, &status, WNOHANG))>0) {
agent_iter iter = find_if(agents.begin(), agents.end(), bind2nd(pid_equal<Agent*>(), dead_pid));
if(iter != agents.end()) {
Agent * deadAgent = *iter;
stringstream msg;
msg<<deadAgent->getName();
switch(deadAgent->attr.status) {
case EATEN: msg<<" has been eaten"; break;
default: msg<<" has died"; break;
}
add_announcement(msg.str().c_str(), deadAgent->get_location(), 25);
delete deadAgent;
agents.erase(iter);
}
}
}
int main(int argc, char **argv)
{
qpid::types::Variant::Map options;
qpid::types::Variant::Map callOptions;
qpid::messaging::Connection connection;
string sessionOptions;
ConsoleEvent event;
Agent agent;
mh_log_init("sysconfig-console", LOG_TRACE, TRUE);
mh_add_option('U', required_argument, "uri", "URI of configuration", &options, NULL);
mh_add_option('d', no_argument, "daemon", "run as a daemon", NULL, mh_should_daemonize);
qpid::types::Variant::Map amqp_options = mh_parse_options("sysconfig-console", argc, argv, options);
callOptions["uri"] = options["uri"];
mh_log_init("sysconfig-console", mh_log_level, mh_log_level > LOG_INFO);
connection = mh_connect(options, amqp_options, TRUE);
connection.open();
ConsoleSession session(connection, sessionOptions);
// Only filter connecting agents under matahariproject.org vendor and Config product
session.setAgentFilter("[and, [eq, _vendor, [quote, 'matahariproject.org']], [eq, _product, [quote, 'Sysconfig']]]");
session.open();
while(session.getAgentCount() == 0) {
g_usleep(1000);
}
while (true) {
if(session.nextEvent(event)) {
switch(event.getType()) {
case CONSOLE_AGENT_ADD:
{
agent = event.getAgent();
DataAddr agent_event_addr("Sysconfig", agent.getName(), 0);
ConsoleEvent result(agent.callMethod("run_uri",
callOptions,
agent_event_addr));
}
break;
default:
break;
}
}
}
return 0;
}
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");
}
void SelectMenu::handleAction(const int actionId, void *ctx, const int modKeys)
{
if (actionId == teamButId_) {
tab_ = TAB_TEAM;
showItemList();
} else if (actionId == modsButId_) {
tab_ = TAB_MODS;
showItemList();
} else if (actionId == equipButId_) {
tab_ = TAB_EQUIPS;
showItemList();
} else if (actionId == pTeamLBox_->getId()) {
// get the selected agent from the team listbox
std::pair<int, void *> * pPair = static_cast<std::pair<int, void *> *> (ctx);
Agent *pNewAgent = static_cast<Agent *> (pPair->second);
bool found = false;
// check if selected agent is already part of the mission squad
for (size_t j = 0; j < AgentManager::kMaxSlot; j++) {
if (g_Session.agents().squadMember(j) == pNewAgent) {
found = true;
break;
}
}
// Agent was not part of the squad
if (!found) {
// adds him to the squad
g_Session.agents().setSquadMember(cur_agent_, pNewAgent);
// Update current agent name
getStatic(txtAgentId_)->setTextFormated("#SELECT_SUBTITLE", pNewAgent->getName());
pTeamLBox_->setSquadLine(cur_agent_, pPair->first);
updateAcceptEnabled();
// redraw agent display
addDirtyRect(158, 110, 340, 260);
// redraw agent buttons
dirtyAgentSelector();
}
} else if (actionId == pModsLBox_->getId()) {
std::pair<int, void *> * pPair = static_cast<std::pair<int, void *> *> (ctx);
pSelectedMod_ = static_cast<Mod *> (pPair->second);
showModWeaponPanel();
} else if (actionId == pWeaponsLBox_->getId()) {
std::pair<int, void *> * pPair = static_cast<std::pair<int, void *> *> (ctx);
pSelectedWeap_ = static_cast<Weapon *> (pPair->second);
showModWeaponPanel();
} else if (actionId == cancelButId_) {
showItemList();
} else if (actionId == reloadButId_) {
Agent *selected = g_Session.agents().squadMember(cur_agent_);
WeaponInstance *wi = selected->weapon(selectedWInstId_ - 1);
int rldCost = (pSelectedWeap_->ammo()
- wi->ammoRemaining()) * pSelectedWeap_->ammoCost();
if (g_Session.getMoney() >= rldCost) {
g_Session.setMoney(g_Session.getMoney() - rldCost);
wi->setAmmoRemaining(pSelectedWeap_->ammo());
getOption(reloadButId_)->setVisible(false);
getStatic(moneyTxtId_)->setTextFormated("%d", g_Session.getMoney());
}
} else if (actionId == purchaseButId_) {
// Buying weapon
if (pSelectedWeap_) {
if (sel_all_) {
for (int n = 0; n < 4; n++) {
Agent *selected = g_Session.agents().squadMember(n);
if (selected && selected->numWeapons() < 8
&& g_Session.getMoney() >= pSelectedWeap_->cost()) {
g_Session.setMoney(g_Session.getMoney() - pSelectedWeap_->cost());
selected->addWeapon(pSelectedWeap_->createInstance());
getStatic(moneyTxtId_)->setTextFormated("%d", g_Session.getMoney());
}
}
} else {
Agent *selected = g_Session.agents().squadMember(cur_agent_);
if (selected && selected->numWeapons() < 8
&& g_Session.getMoney() >= pSelectedWeap_->cost()) {
g_Session.setMoney(g_Session.getMoney() - pSelectedWeap_->cost());
selected->addWeapon(pSelectedWeap_->createInstance());
getStatic(moneyTxtId_)->setTextFormated("%d", g_Session.getMoney());
}
}
needRendering();
} else if (pSelectedMod_) {
if (sel_all_) {
for (int n = 0; n < 4; n++) {
Agent *selected = g_Session.agents().squadMember(n);
if (selected && selected->canHaveMod(pSelectedMod_)
&& g_Session.getMoney() >= pSelectedMod_->cost()) {
selected->addMod(pSelectedMod_);
g_Session.setMoney(g_Session.getMoney() - pSelectedMod_->cost());
getStatic(moneyTxtId_)->setTextFormated("%d", g_Session.getMoney());
}
}
} else {
Agent *selected = g_Session.agents().squadMember(cur_agent_);
if (selected && selected->canHaveMod(pSelectedMod_)
&& g_Session.getMoney() >= pSelectedMod_->cost()) {
selected->addMod(pSelectedMod_);
g_Session.setMoney(g_Session.getMoney() - pSelectedMod_->cost());
getStatic(moneyTxtId_)->setTextFormated("%d", g_Session.getMoney());
}
}
showItemList();
}
} else if (actionId == sellButId_ && selectedWInstId_) {
addDirtyRect(360, 305, 135, 70);
Agent *selected = g_Session.agents().squadMember(cur_agent_);
WeaponInstance *pWi = selected->removeWeapon(selectedWInstId_ - 1);
g_Session.setMoney(g_Session.getMoney() + pWi->getWeaponClass()->cost());
getStatic(moneyTxtId_)->setTextFormated("%d", g_Session.getMoney());
delete pWi;
showItemList();
}
}
bool operator()(const Agent& obj) const
{
return obj.getName() == s_;
}
// In this function, the Voronoi cell is calculated, integrated. A new goal IS NOT published, as this node should only be used with integration of teleop.
void SimpleDeploymentDummy::publish()
{
if ( got_map_ && got_me_ ) {
double factor = map_.info.resolution / resolution_; // zoom factor for openCV visualization
ROS_DEBUG("SimpleDeployment: Map received, determining Voronoi cells");
removeOldAgents();
// Create variables for x-values, y-values and the maximum and minimum of these, needed for VoronoiDiagramGenerator
float xvalues[agent_catalog_.size()];
float yvalues[agent_catalog_.size()];
double xmin = 0.0, xmax = 0.0, ymin = 0.0, ymax = 0.0;
cv::Point seedPt = cv::Point(1,1);
// Create empty image with the size of the map to draw points and voronoi diagram in
cv::Mat vorImg = cv::Mat::zeros(map_.info.height*factor,map_.info.width*factor,CV_8UC1);
for ( uint i = 0; i < agent_catalog_.size(); i++ ) {
geometry_msgs::Pose pose = agent_catalog_[i].getPose();
// Keep track of x and y values
xvalues[i] = pose.position.x;
yvalues[i] = pose.position.y;
// Keep track of min and max x
if ( pose.position.x < xmin ) {
xmin = pose.position.x;
} else if ( pose.position.x > xmax ) {
xmax = pose.position.x;
}
// Keep track of min and max y
if ( pose.position.y < ymin ) {
ymin = pose.position.y;
} else if ( pose.position.y > ymax ) {
ymax = pose.position.y;
}
// Store point as seed point if it represents this agent
if ( agent_catalog_[i].getName() == this_agent_.getName() ){
// Scale positions in metric system column and row numbers in image
int c = ( pose.position.x - map_.info.origin.position.x ) * factor / map_.info.resolution;
int r = map_.info.height * factor - ( pose.position.y - map_.info.origin.position.y ) * factor / map_.info.resolution;
cv::Point pt = cv::Point(c,r);
seedPt = pt;
}
// Draw point on image
// cv::circle( vorImg, pt, 3, WHITE, -1, 8);
}
ROS_DEBUG("SimpleDeployment: creating a VDG object and generating Voronoi diagram");
// Construct a VoronoiDiagramGenerator (VoronoiDiagramGenerator.h)
VoronoiDiagramGenerator VDG;
xmin = map_.info.origin.position.x; xmax = map_.info.width * map_.info.resolution + map_.info.origin.position.x;
ymin = map_.info.origin.position.y; ymax = map_.info.height * map_.info.resolution + map_.info.origin.position.y;
// Generate the Voronoi diagram using the collected x and y values, the number of points, and the min and max x and y values
VDG.generateVoronoi(xvalues,yvalues,agent_catalog_.size(),float(xmin),float(xmax),float(ymin),float(ymax));
float x1,y1,x2,y2;
ROS_DEBUG("SimpleDeployment: collecting line segments from the VDG object");
// Collect the generated line segments from the VDG object
while(VDG.getNext(x1,y1,x2,y2))
{
// Scale the line segment end-point coordinates to column and row numbers in image
int c1 = ( x1 - map_.info.origin.position.x ) * factor / map_.info.resolution;
int r1 = vorImg.rows - ( y1 - map_.info.origin.position.y ) * factor / map_.info.resolution;
int c2 = ( x2 - map_.info.origin.position.x ) * factor / map_.info.resolution;
int r2 = vorImg.rows - ( y2 - map_.info.origin.position.y ) * factor / map_.info.resolution;
// Draw line segment
cv::Point pt1 = cv::Point(c1,r1),
pt2 = cv::Point(c2,r2);
cv::line(vorImg,pt1,pt2,WHITE);
}
ROS_DEBUG("SimpleDeployment: drawing map occupancygrid and resizing to voronoi image size");
// Create cv image from map data and resize it to the same size as voronoi image
cv::Mat mapImg = drawMap();
cv::Mat viewImg(vorImg.size(),CV_8UC1);
cv::resize(mapImg, viewImg, vorImg.size(), 0.0, 0.0, cv::INTER_NEAREST );
// Add images together to make the total image
cv::Mat totalImg(vorImg.size(),CV_8UC1);
cv::bitwise_or(viewImg,vorImg,totalImg);
cv::Mat celImg = cv::Mat::zeros(vorImg.rows+2, vorImg.cols+2, CV_8UC1);
cv::Scalar newVal = cv::Scalar(1), upDiff = cv::Scalar(100), loDiff = cv::Scalar(256);
cv::Rect rect;
cv::floodFill(totalImg,celImg,seedPt,newVal,&rect,loDiff,upDiff,4 + (255 << 8) + cv::FLOODFILL_MASK_ONLY);
// Compute the center of mass of the Voronoi cell
cv::Moments m = moments(celImg, false);
cv::Point centroid(m.m10/m.m00, m.m01/m.m00);
cv::circle( celImg, centroid, 3, BLACK, -1, 8);
// Convert seed point to celImg coordinate system (totalImg(x,y) = celImg(x+1,y+1)
cv::Point onePt(1,1);
centroid = centroid - onePt;
for ( uint i = 0; i < agent_catalog_.size(); i++ ){
int c = ( xvalues[i] - map_.info.origin.position.x ) * factor / map_.info.resolution;
int r = map_.info.height * factor - ( yvalues[i] - map_.info.origin.position.y ) * factor / map_.info.resolution;
cv::Point pt = cv::Point(c,r);
cv::circle( totalImg, pt, 3, GRAY, -1, 8);
}
cv::circle( totalImg, seedPt, 3, WHITE, -1, 8);
cv::circle( totalImg, centroid, 2, WHITE, -1, 8); //where centroid is the goal position
// Due to bandwidth issues, only display this image if requested
if (show_cells_) {
cv::imshow( "Display window", totalImg );
}
cv::waitKey(3);
}
else {
ROS_DEBUG("SimpleDeployment: No map received");
}
}