void justine::robocar::SmartCar::nextEdge(void)
{
if (traffic.hasNode(to_node())) {
if (m_step >= traffic.palist(m_from, m_to)) {
osmium::unsigned_object_id_type next_m_from
= traffic.alist(m_from, m_to);
osmium::unsigned_object_id_type next_m_to
= traffic.naive_node_for_nearest_gangster(m_from, m_to, m_step);
if (traffic.palist(next_m_from, next_m_to) >
traffic.salist(next_m_from, next_m_to)) {
traffic.set_salist(m_from, m_to, traffic.salist(m_from, m_to) - 1);
m_from = next_m_from;
m_to = next_m_to;
m_step = 0;
traffic.set_salist(m_from, m_to, traffic.salist(m_from, m_to) + 1);
}
} else
++m_step;
} else {
// car stopped
}
}
virtual void assign(CarData *car_data, bool full)
{
car_data->set_node_from(m_from);
car_data->set_node_to(to_node());
car_data->set_max_step(get_max_steps());
car_data->set_step(get_step());
car_data->set_type(static_cast<CarData::ProtoCarType>(get_type())); // safe
}
static ssize_t node_read_cpumap(struct device *dev, bool list, char *buf)
{
struct node *node_dev = to_node(dev);
const struct cpumask *mask = cpumask_of_node(node_dev->dev.id);
/* 2008/04/07: buf currently PAGE_SIZE, need 9 chars per 32 bits. */
BUILD_BUG_ON((NR_CPUS/32 * 9) > (PAGE_SIZE-1));
return cpumap_print_to_pagebuf(list, buf, mask);
}
/** Search for a path between two nodes.
* This function executes an A* search to find an (optimal) path
* from node @p from to node @p to.
* @param from name of node to search from
* @param to name of the goal node
* @param estimate_func function to estimate the cost from any node to the goal.
* Note that the estimate function must be admissible for optimal A* search. That
* means that for no query may the calculated estimate be higher than the actual
* cost.
* @param cost_func function to calculate the cost from a node to another adjacent
* node. Note that the cost function is directly related to the estimate function.
* For example, the cost can be calculated in terms of distance between nodes, or in
* time that it takes to travel from one node to the other. The estimate function must
* match the cost function to be admissible.
* @param use_constraints true to respect constraints imposed by the constraint
* repository, false to ignore the repository searching as if there were no
* constraints whatsoever.
* @param compute_constraints if true re-compute constraints, otherwise use constraints
* as-is, for example if they have been computed before to check for changes.
* @return ordered vector of nodes which denote a path from @p from to @p to.
* Note that the vector is empty if no path could be found (i.e. there is non
* or it was prohibited when using constraints.
*/
fawkes::NavGraphPath
NavGraph::search_path(const std::string &from, const std::string &to,
navgraph::EstimateFunction estimate_func,
navgraph::CostFunction cost_func,
bool use_constraints, bool compute_constraints)
{
NavGraphNode from_node(node(from));
NavGraphNode to_node(node(to));
return search_path(from_node, to_node, estimate_func, cost_func,
use_constraints, compute_constraints);
}
virtual void assign(CarData *car_data, bool full)
{
// meghívjuk a protobuf által generált setter függvényeket
// és belehelyezzük a CarData objektumba az autó adatait
car_data->set_node_from(m_from);
car_data->set_node_to(to_node());
car_data->set_max_step(get_max_steps());
car_data->set_step(get_step());
// a cast biztosan lehetséges, így a static_cast is megfelelő lesz
// (nincs runtime ellenőrzés benne)
car_data->set_type(static_cast<CarData::ProtoCarType>(get_type())); // safe
}
virtual void print ( std::ostream & os ) const
{
os << m_from
<< " "
<< to_node()
<< " "
<< get_max_steps()
<< " "
<< get_step()
<< " "
<< static_cast<unsigned int> ( get_type() );
}
static ssize_t node_read_cpumap(struct sys_device * dev, char * buf)
{
struct node *node_dev = to_node(dev);
cpumask_t mask = node_dev->cpumap;
int len;
/* FIXME - someone should pass us a buffer size (count) or
* use seq_file or something to avoid buffer overrun risk. */
len = cpumask_snprintf(buf, 99 /* XXX FIXME */, mask);
len += sprintf(buf + len, "\n");
return len;
}
static ssize_t node_read_cpumap(struct sys_device * dev, char * buf)
{
struct node *node_dev = to_node(dev);
cpumask_t mask = node_to_cpumask(node_dev->sysdev.id);
int len;
/* 2004/06/03: buf currently PAGE_SIZE, need > 1 char per 4 bits. */
BUILD_BUG_ON(MAX_NUMNODES/4 > PAGE_SIZE/2);
len = cpumask_scnprintf(buf, PAGE_SIZE-1, mask);
len += sprintf(buf + len, "\n");
return len;
}
osmium::unsigned_object_id_type justine::robocar::Traffic::naive_nearest_gangster (
osmium::unsigned_object_id_type from,
osmium::unsigned_object_id_type to,
osmium::unsigned_object_id_type step )
{
osmium::unsigned_object_id_type ret = from;
double lon1 {0.0}, lat1 {0.0};
toGPS ( from, to, step, &lon1, &lat1 );
double maxd = std::numeric_limits<double>::max();
double lon2 {0.0}, lat2 {0.0};
/*for ( auto car:m_smart_cars )
{
if ( car->get_type() == CarType::GANGSTER )
{
toGPS ( car->from(), car->to() , car->get_step(), &lon2, &lat2 );
double d = dst ( lon1, lat1, lon2, lat2 );
if ( d < maxd )
{
maxd = d;
ret = car->to_node();
}
}
}*/
//atirva
for ( std::vector<std::shared_ptr<SmartCar>>::iterator it=m_smart_cars.begin() ; it != m_smart_cars.end() ; ++it )
{
auto car = *it;
if ( car->get_type() == CarType::GANGSTER )
{
toGPS ( car->from(), car->to() , car->get_step(), &lon2, &lat2 );
double d = dst ( lon1, lat1, lon2, lat2 );
if ( d < maxd )
{
maxd = d;
ret = car->to_node();
}
}
}
return ret;
}
static ssize_t node_read_cpumap(struct device *dev, int type, char *buf)
{
struct node *node_dev = to_node(dev);
const struct cpumask *mask = cpumask_of_node(node_dev->dev.id);
int len;
/* 2008/04/07: buf currently PAGE_SIZE, need 9 chars per 32 bits. */
BUILD_BUG_ON((NR_CPUS/32 * 9) > (PAGE_SIZE-1));
len = type?
cpulist_scnprintf(buf, PAGE_SIZE-2, mask) :
cpumask_scnprintf(buf, PAGE_SIZE-2, mask);
buf[len++] = '\n';
buf[len] = '\0';
return len;
}
void justine::robocar::Car::step()
{
if ( traffic.hasNode ( to_node() ) )
{
if ( m_step >= traffic.palist ( m_from, m_to ) )
{
nextSmarterEdge();
}
else
++m_step;
}
else
{
// car stopped
}
}
virtual void print(std::ostream &os) const {
os << m_from
<< " "
<< to_node()
<< " "
<< get_max_steps()
<< " "
<< get_step()
<< " "
<< static_cast<unsigned int>(get_type())
<< " "
<< get_num_captured_gangsters()
<< " "
<< m_name;
}
static void node_device_release(struct device *dev)
{
struct node *node = to_node(dev);
#if defined(CONFIG_MEMORY_HOTPLUG_SPARSE) && defined(CONFIG_HUGETLBFS)
/*
* We schedule the work only when a memory section is
* onlined/offlined on this node. When we come here,
* all the memory on this node has been offlined,
* so we won't enqueue new work to this work.
*
* The work is using node->node_work, so we should
* flush work before freeing the memory.
*/
flush_work(&node->node_work);
#endif
kfree(node);
}
virtual void print (std::ostream & os) const
{
os << m_from
<< " "
<< to_node()
<< " "
<< get_max_steps()
<< " "
<< get_step()
<< " "
<< static_cast<unsigned int> (get_type())
<< " "
<< num_gangsters_caught_
<< " "
<< team_name_
<< " "
<< id_;
}
virtual void assign(CarData *car_data, bool full)
{
// annyiban külonbozik a tobbi assign() függvénytől, hogy
// a rendőrautókra jellemző adatokat is átadjuk
// ezek a .proto fájlban "optional" értékek
car_data->set_node_from(m_from);
car_data->set_node_to(to_node());
car_data->set_max_step(get_max_steps());
car_data->set_step(get_step());
car_data->set_type(static_cast<CarData::ProtoCarType>(get_type())); // safe
car_data->set_caught(num_gangsters_caught_);
car_data->set_team(team_name_);
car_data->set_id(id_);
if(full){
car_data->set_size(route.size());
for(auto it = route.begin();it!=route.end();it++)
car_data->add_path(*it);
}else{
car_data->set_size(0);
}
}
void justine::robocar::SmartCar::nextGuidedEdge(void)
{
if (traffic.hasNode(to_node())) {
if (m_step >= traffic.palist(m_from, m_to)) {
std::vector<unsigned int>::iterator i = std::find(route.begin(), route.end(), to_node());
std::vector<unsigned int>::iterator j = std::find(route.begin(), route.end(), m_from);
if (std::distance(route.begin(), j) == route.size() - 2 &&
std::distance(route.begin(), i) == route.size() - 1
) {
return;
}
if (i == route.end()) {
return;
}
osmium::unsigned_object_id_type next_m_to;
osmium::unsigned_object_id_type next_m_from;
if (std::distance(route.begin(), i) == route.size() - 1) {
next_m_to = -1;
next_m_from = to_node();
} else {
osmium::unsigned_object_id_type inv = traffic.alist_inv(to_node(), * (i + 1));
if (inv != -1) {
next_m_to = inv;
} else {
next_m_to = 0;
}
m_step = 0;
next_m_from = to_node();
}
if (traffic.palist(next_m_from, next_m_to) >=
traffic.salist(next_m_from, next_m_to)) {
traffic.set_salist(m_from, m_to, traffic.salist(m_from, m_to) - 1);
unsigned pfrom = m_from;
m_from = next_m_from;
m_to = next_m_to;
m_step = 0;
traffic.set_salist(m_from, m_to, traffic.salist(m_from, m_to) + 1);
} else {
;
}
} else {
++m_step;
}
} else {
;
// car stopped
}
}
void justine::robocar::Traffic::cmd_session ( boost::asio::ip::tcp::socket client_socket )
{
const int network_buffer_size = 524288;
char data[network_buffer_size]; // TODO buffered write...
try
{
for ( ;; )
{
CarLexer cl;
boost::system::error_code err;
size_t length = client_socket.read_some ( boost::asio::buffer ( data ), err );
if ( err == boost::asio::error::eof )
{
break;
}
else if ( err )
{
throw boost::system::system_error ( err );
}
std::istringstream pdata ( data );
cl.switch_streams ( &pdata );
cl.yylex();
length = 0;
int resp_code = cl.get_errnumber();
int num = cl.get_num();
int id {0};
if ( cl.get_cmd() == 0 )
{
for ( ;; )
{
std::vector<std::shared_ptr<Car>> cars_copy;
{
std::lock_guard<std::mutex> lock ( cars_mutex );
cars_copy = cars;
}
std::stringstream ss;
ss <<
m_time <<
" " <<
m_minutes <<
" " <<
cars_copy.size()
<< std::endl;
/*for ( auto car:cars_copy )
{
car->step();
ss << *car
<< " " << std::endl;
}*/
//atirva
for ( std::vector<std::shared_ptr<Car>>::iterator it=cars_copy.begin(); it != cars_copy.end(); ++it )
{
auto car = *it;
car->step();
ss << *car
<< " " << std::endl;
}
boost::asio::write ( client_socket, boost::asio::buffer ( data, length ) );
length = std::sprintf ( data,
"%s", ss.str().c_str() );
boost::asio::write ( client_socket, boost::asio::buffer ( data, length ) );
std::this_thread::sleep_for ( std::chrono::milliseconds ( 200 ) );
}
}
else if ( cl.get_cmd() <100 )
{
std::lock_guard<std::mutex> lock ( cars_mutex );
for ( int i {0}; i<cl.get_num(); ++i )
{
if ( cl.get_role() =='c' )
id = addCop ( cl );
else
id = addGangster ( cl );
if ( !resp_code )
length += std::sprintf ( data+length,
"<OK %d %d/%d %c>", id, num,
i+1, cl.get_role() );
else
length += std::sprintf ( data+length,
"<WARN %d %d/%d %c>", id, num,
i+1, cl.get_role() );
}
} // cmd 100
else if ( cl.get_cmd() == 101 )
{
if ( m_smart_cars_map.find ( cl.get_id() ) != m_smart_cars_map.end() )
{
std::shared_ptr<SmartCar> c = m_smart_cars_map[cl.get_id()];
if ( c->set_route ( cl.get_route() ) )
length += std::sprintf ( data+length, "<OK %d>", cl.get_id() );
else
length += std::sprintf ( data+length, "<ERR bad routing vector>" );
}
else
length += std::sprintf ( data+length, "<ERR unknown car id>" );
}
else if ( cl.get_cmd() == 1001 )
{
if ( m_smart_cars_map.find ( cl.get_id() ) != m_smart_cars_map.end() )
{
std::shared_ptr<SmartCar> c = m_smart_cars_map[cl.get_id()];
length += std::sprintf ( data+length,
"<OK %d %u %u %u>", cl.get_id(), c->from(),
c->to_node(), c->get_step() );
}
else
length += std::sprintf ( data+length, "<ERR unknown car id>" );
}
else if ( cl.get_cmd() == 1002 )
{
std::lock_guard<std::mutex> lock ( cars_mutex );
if ( m_smart_cars_map.find ( cl.get_id() ) != m_smart_cars_map.end() )
{
bool hasGangsters {false};
/*for ( auto c:m_smart_cars )
{
if ( c->get_type() == CarType::GANGSTER )
{
length += std::sprintf ( data+length,
"<OK %d %u %u %u>", cl.get_id(), c->from(),
c->to_node(), c->get_step() );
if ( length > network_buffer_size - 512 )
{
length += std::sprintf ( data+length,
"<WARN too many gangsters to send through this implementation...>" );
break;
}
hasGangsters = true;
}
} */
//atirva
for ( std::vector<std::shared_ptr<SmartCar>>::iterator it = m_smart_cars.begin(); it != m_smart_cars.end(); ++it )
{
auto c = *it;
if ( c->get_type() == CarType::GANGSTER )
{
length += std::sprintf ( data+length,
"<OK %d %u %u %u>", cl.get_id(), c->from(),
c->to_node(), c->get_step() );
if ( length > network_buffer_size - 512 )
{
length += std::sprintf ( data+length,
"<WARN too many gangsters to send through this implementation...>" );
break;
}
hasGangsters = true;
}
}
if ( !hasGangsters )
length += std::sprintf ( data+length, "<WARN there is no gangsters>" );
}
else
length += std::sprintf ( data+length, "<ERR unknown car id>" );
}
else if ( cl.get_cmd() == 1003 )
{
std::lock_guard<std::mutex> lock ( cars_mutex );
if ( m_smart_cars_map.find ( cl.get_id() ) != m_smart_cars_map.end() )
{
bool hasCops {false};
/* for ( auto c:m_cop_cars )
{
length += std::sprintf ( data+length,
"<OK %d %u %u %u %d>", cl.get_id(), c->from(),
c->to_node(), c->get_step(), c->get_num_captured_gangsters() );
if ( length > network_buffer_size - 512 )
{
length += std::sprintf ( data+length,
"<WARN too many cops to send through this implementation...>" );
break;
}
hasCops = true;
}*/
//atirva
for ( std::vector<std::shared_ptr<CopCar>>::iterator it = m_cop_cars.begin(); it != m_cop_cars.end(); ++it )
{
auto c = *it;
length += std::sprintf ( data+length,
"<OK %d %u %u %u %d>", cl.get_id(), c->from(),
c->to_node(), c->get_step(), c->get_num_captured_gangsters() );
if ( length > network_buffer_size - 512 )
{
length += std::sprintf ( data+length,
"<WARN too many cops to send through this implementation...>" );
break;
}
hasCops = true;
}
if ( !hasCops )
length += std::sprintf ( data+length, "<WARN there is no cops>" );
}
else
length += std::sprintf ( data+length, "<ERR unknown car id>" );
}
else if ( cl.get_cmd() == 10001 )
{
if ( m_smart_cars_map.find ( cl.get_id() ) != m_smart_cars_map.end() )
{
std::shared_ptr<SmartCar> c = m_smart_cars_map[cl.get_id()];
if ( c->set_fromto ( cl.get_from(), cl.get_to() ) )
length += std::sprintf ( data+length, "<OK %d>", cl.get_id() );
else
length += std::sprintf ( data+length, "<ERR cannot set>" );
}
else
length += std::sprintf ( data+length, "<ERR unknown car id>" );
}
else
{
length += std::sprintf ( data+length, "<ERR unknown proto command>" );
}
boost::asio::write ( client_socket, boost::asio::buffer ( data, length ) );
}
}
catch ( std::exception& e )
{
std::cerr << "Ooops: " << e.what() << std::endl;
}
}