Esempio n. 1
0
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");
}
Esempio n. 4
0
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");
    }

}