int main() {
Hero hercules("Hercules", 0, 100);
hercules.print();
std::vector<Hero> heroes;
heroes.push_back(Hero("Achilles" , 1, 58));
heroes.push_back(Hero("Bellerophon",513,327));
heroes.push_back(Hero("Cadmus" , 27,815));
heroes.push_back(Hero("Cecrops" ,234, 3));
heroes.push_back(Hero("Pollux" ,242,507));
heroes.push_back(Hero("Hektor" , 59, 22));
heroes.push_back(Hero("Diomedes" , 62, 7));
heroes.push_back(Hero("Herakles" ,270, 31));
heroes.push_back(Hero("Odysseus" , 84,910));
heroes.push_back(Hero("Palamedes" ,192, 73));
heroes.push_back(Hero("Perseus" ,128,636));
heroes.push_back(Hero("Phoroneus" , 35, 42));
heroes.push_back(Hero("Prometheus" , 27, 99));
heroes.push_back(Hero("Theseus" , 4,201));
heroes.push_back(Hero("Triptolemos", 28, 90));
std::cout << "\nThe Heroes:" << std::endl;
for(std::vector<Hero>::iterator h = heroes.begin(); h != heroes.end(); ++h)
h->print();
std::sort(heroes.begin(), heroes.end(), has_more_gold);
std::cout << "\nThe Heroes (sorted by wealth):" << std::endl;
for(std::vector<Hero>::iterator h = heroes.begin(); h != heroes.end(); ++h)
h->print();
std::sort(heroes.begin(), heroes.end(), has_more_honor);
std::cout << "\nThe Heroes (sorted by honor):" << std::endl;
for(std::vector<Hero>::iterator h = heroes.begin(); h != heroes.end(); ++h)
h->print();
std::vector<Hero> heroes_copy1 = heroes;
std::vector<Hero>::iterator end = std::remove_if(heroes_copy1.begin(), heroes_copy1.end(), HasLessGoldThan(100));
heroes_copy1.erase(end, heroes_copy1.end()); // this will delete the remaining elements
std::cout << "\nRich Heroes:" << std::endl;
for(std::vector<Hero>::iterator h = heroes_copy1.begin(); h != heroes_copy1.end(); ++h)
h->print();
std::vector<Hero> heroes_copy2 = heroes;
end = std::remove_if(heroes_copy2.begin(), heroes_copy2.end(), HasMoreHonorThan(50));
heroes_copy2.erase(end, heroes_copy2.end()); // this will delete the remaining elements
std::cout << "\nHeroes with shit for honor:" << std::endl;
for(std::vector<Hero>::iterator h = heroes_copy2.begin(); h != heroes_copy2.end(); ++h)
h->print();
}
int main(int argc, char *argv[])
{
ros::init(argc, argv, "hercules_base");
ros::NodeHandle nh, private_nh("~");
double control_frequency, diagnostic_frequency;
private_nh.param<double>("control_frequency", control_frequency, 10.0);
private_nh.param<double>("diagnostic_frequency", diagnostic_frequency, 1.0);
// Initialize robot hardware and link to controller manager
hercules_base::HerculesHardware hercules(nh, private_nh, control_frequency);
controller_manager::ControllerManager cm(&hercules, nh);
// Setup separate queue and single-threaded spinner to process timer callbacks
// that interface with Hercules hardware - libhorizon_legacy not threadsafe. This
// avoids having to lock around hardware access, but precludes realtime safety
// in the control loop.
ros::CallbackQueue hercules_queue;
ros::AsyncSpinner hercules_spinner(1, &hercules_queue);
time_source::time_point last_time = time_source::now();
ros::TimerOptions control_timer(
ros::Duration(1 / control_frequency),
boost::bind(controlLoop, boost::ref(hercules), boost::ref(cm), boost::ref(last_time)),
&hercules_queue);
ros::Timer control_loop = nh.createTimer(control_timer);
// Temporarily disable diagnostic
/*
ros::TimerOptions diagnostic_timer(
ros::Duration(1 / diagnostic_frequency),
boost::bind(diagnosticLoop, boost::ref(hercules)),
&hercules_queue);
ros::Timer diagnostic_loop = nh.createTimer(diagnostic_timer);
*/
hercules_spinner.start();
// Process remainder of ROS callbacks separately, mainly ControlManager related
ros::spin();
return 0;
}
