int main( int argc, char* argv[] )
{
struct timespec t_initial, t_final, t_loc_estimation1, t_loc_estimation2;
clock_gettime(CLOCK_MONOTONIC, &t_initial);
//Initialize the ROS node for bubble space
ros::init( argc, argv, "bubble_space_node" );
ros::NodeHandle n;
ros::NodeHandle nh("~");
bool normalize_invariants;
bool exclude_depth;
bool omni_directional, online_localization;
int rotation_count=0;
cv::Mat invariantMatrix;
cv::Mat omni_invariants;
cv::Mat invariants;
cv::Mat bubble_location_matrix;
double angular_velocity;
double bubble_update_period;
cv::Mat location_estimation;
DatabaseManager current_place_db_manager;
DatabaseManager learned_place_db_manager;
DatabaseManager bubble_database_manager;
localizationStruct callback_struct;
//Read the parameters
std::string bubble_database_path, filters_dir_path, positions_dir_path, learned_place_dir_path, current_place_dir_path;
int base_point_number, orientation_number, filter_no, noHarmonics;
nh.param("normalize_invariants", normalize_invariants, true);
nh.param("bubble_update_period", bubble_update_period, 25.0);
nh.param("angular_velocity", angular_velocity, 2/(25.0-5)*9);
nh.param("exclude_depth", exclude_depth, false);
nh.param("harmonics_number", noHarmonics, 10);
nh.getParam("bubble_database_path", bubble_database_path);
nh.getParam("filters_dir_path", filters_dir_path);
nh.getParam("base_point_number", base_point_number);
nh.getParam("orientation_number", orientation_number);
nh.getParam("filter_no", filter_no);
nh.getParam("positions_dir_path", positions_dir_path);
nh.getParam("omni_directional",omni_directional);
nh.getParam("online_localization",online_localization);
if(!online_localization){
nh.getParam("learned_place_dir_path",learned_place_dir_path);
nh.getParam("current_place_dir_path",current_place_dir_path);
std::string learned_place_db_path = learned_place_dir_path.append("/detected_places.db");
std::string current_place_db_path = current_place_dir_path.append("/detected_places.db");
learned_place_db_manager.openDB(learned_place_db_path.c_str());
current_place_db_manager.openDB(current_place_db_path.c_str());
}
if(online_localization){
//Check, whether the database file path is good
std::ifstream database_file(bubble_database_path.c_str());
if (!database_file.good()){
ROS_FATAL("Database file cannot be found!");
return 0;
}
//Check minimum bubble update period and calculate the angular velocity according to bubble update period
if(bubble_update_period < 5){
ROS_WARN("Minimum allowed update period is 5 seconds! Setting the update period to 2 seconds now.");
bubble_update_period = 5;
angular_velocity = 0;
}
else{
angular_velocity = 2/(bubble_update_period-5)*9; //2 radians
}
//cv::Mat location_matrix(base_point_number, 2, CV_32F);
//Open database
bubble_database_manager.openDB(bubble_database_path.c_str());
//Read the database and create the invariant matrix
//invariantMatrix = DatabaseManager::createInvariantMatrix(normalize_invariants, noHarmonics);
invariantMatrix=bubble_database_manager.createInvariantMatrix(normalize_invariants, noHarmonics, orientation_number, base_point_number, filter_no);
ROS_INFO("Invariant database matrix is created successfully!");
//read the locations of the learned base points from the database
bubble_location_matrix=bubble_database_manager.createLocationMatrix(base_point_number);
//Calculate pan and tilt angles for bubble space
bubbleProcess::calculateImagePanAngles(525,640,480);
bubbleProcess::calculateImageTiltAngles(525,640,480);
// definition of filters used
int filter_numbers[5] = {14,16,20,21,43};
//read the filters
for(int i=0; i<filter_no; i++){
callback_struct.filters[i]=localization::selectReadFilter(filter_numbers[i],filters_dir_path);
}
}
callback_struct.bubble_update_period=bubble_update_period;
callback_struct.exclude_depth= exclude_depth;
callback_struct.normalize_invariants=normalize_invariants;
callback_struct.angular_velocity=angular_velocity;
//Subscriber for /camera/depth_registered/points topic
ros::Subscriber sub_pclReg = nh.subscribe<sensor_msgs::PointCloud2> ("/camera/depth_registered/points", 1, boost::bind(&localizationCallback, _1, &callback_struct ) );
//Publisher for cmd_vel topic
velocityCommandPublisher = n.advertise<geometry_msgs::Twist>("cmd_vel",1);
//Set the bubble update period
ros::Rate r(10);
// turn the robot, get the Kinect data and calculate invariants 8 times
if (online_localization && !omni_directional){
while (ros::ok() && rotation_count < (orientation_number+1))
{
//Step 1: Analyze Kinect data until finding a localization in maximally three iterations
//Get Kinect data and apply bubble space algorithm
ros::spinOnce();
std::cout << is_callback_called << std::endl;
if(is_callback_called){
invariants = callback_struct.invariants;
// connecting the invariants as the robot turns and gets new Kinect data
if (rotation_count == 1){
omni_invariants = invariants;
}
else if (rotation_count != 0){
cv::vconcat(omni_invariants, invariants, omni_invariants);
}
rotation_count++;
std::cout << rotation_count << std::endl;
is_callback_called=false;
}
r.sleep();
}
// estimate the location of the robot based on the database invariant matrix and the current invariants
location_estimation=localization::onlineLocationEstimation(invariantMatrix,omni_invariants, bubble_location_matrix, base_point_number, orientation_number);
std::cout << "Estimated location is:" << location_estimation << std::endl;
clock_gettime(CLOCK_MONOTONIC, &t_final);
double elapsed_time = (t_final.tv_sec - t_initial.tv_sec) + (double) (t_final.tv_nsec - t_initial.tv_nsec) * 1e-9;
qDebug() << "Processed in " << elapsed_time << " seconds.";
}
// if the offline localization (by images from datasets) and omnidirectional images are used:
if (!online_localization && omni_directional){
while(ros::ok())
{
ros::spinOnce();
std::cout << "I am here!" << std::endl;
}
}
ROS_INFO("ROS EXIT");
return 0;
}
bool retro_load_game(const struct retro_game_info *info)
{
enum retro_pixel_format fmt = RETRO_PIXEL_FORMAT_XRGB8888;
if (!environ_cb(RETRO_ENVIRONMENT_SET_PIXEL_FORMAT, &fmt))
{
if (log_cb)
log_cb(RETRO_LOG_INFO, "[ProSystem]: XRGB8888 is not supported.\n");
return false;
}
struct retro_input_descriptor desc[] = {
{ 0, RETRO_DEVICE_JOYPAD, 0, RETRO_DEVICE_ID_JOYPAD_LEFT, "Left" },
{ 0, RETRO_DEVICE_JOYPAD, 0, RETRO_DEVICE_ID_JOYPAD_UP, "Up" },
{ 0, RETRO_DEVICE_JOYPAD, 0, RETRO_DEVICE_ID_JOYPAD_DOWN, "Down" },
{ 0, RETRO_DEVICE_JOYPAD, 0, RETRO_DEVICE_ID_JOYPAD_RIGHT, "Right" },
{ 0, RETRO_DEVICE_JOYPAD, 0, RETRO_DEVICE_ID_JOYPAD_B, "1" },
{ 0, RETRO_DEVICE_JOYPAD, 0, RETRO_DEVICE_ID_JOYPAD_A, "2" },
{ 0, RETRO_DEVICE_JOYPAD, 0, RETRO_DEVICE_ID_JOYPAD_X, "Console Reset" },
{ 0, RETRO_DEVICE_JOYPAD, 0, RETRO_DEVICE_ID_JOYPAD_SELECT, "Console Select" },
{ 0, RETRO_DEVICE_JOYPAD, 0, RETRO_DEVICE_ID_JOYPAD_START, "Console Pause" },
{ 0, RETRO_DEVICE_JOYPAD, 0, RETRO_DEVICE_ID_JOYPAD_L, "Left Difficulty" },
{ 0, RETRO_DEVICE_JOYPAD, 0, RETRO_DEVICE_ID_JOYPAD_R, "Right Difficulty" },
{ 1, RETRO_DEVICE_JOYPAD, 0, RETRO_DEVICE_ID_JOYPAD_LEFT, "Left" },
{ 1, RETRO_DEVICE_JOYPAD, 0, RETRO_DEVICE_ID_JOYPAD_UP, "Up" },
{ 1, RETRO_DEVICE_JOYPAD, 0, RETRO_DEVICE_ID_JOYPAD_DOWN, "Down" },
{ 1, RETRO_DEVICE_JOYPAD, 0, RETRO_DEVICE_ID_JOYPAD_B, "1" },
{ 1, RETRO_DEVICE_JOYPAD, 0, RETRO_DEVICE_ID_JOYPAD_A, "2" },
{ 0 },
};
environ_cb(RETRO_ENVIRONMENT_SET_INPUT_DESCRIPTORS, desc);
memset(keyboard_data, 0, sizeof(keyboard_data));
// Difficulty switches: Left position = (B)eginner, Right position = (A)dvanced
// Left difficulty switch defaults to left position, "(B)eginner"
keyboard_data[15] = 1;
// Right difficulty switch defaults to right position, "(A)dvanced", which fixes Tower Toppler
keyboard_data[16] = 0;
const char *system_directory_c = NULL;
if (cartridge_Load((const uint8_t*)info->data, info->size))
{
environ_cb(RETRO_ENVIRONMENT_GET_SYSTEM_DIRECTORY, &system_directory_c);
if (!system_directory_c)
{
if (log_cb)
log_cb(RETRO_LOG_WARN, "[ProSystem]: no system directory defined, unable to look for ProSystem.dat\n");
database_enabled = false;
}
else
{
std::string system_directory(system_directory_c);
std::string database_file_path = system_directory + "/ProSystem.dat";
std::ifstream database_file(database_file_path.c_str());
if (!database_file.is_open())
{
if (log_cb)
log_cb(RETRO_LOG_WARN, "[ProSystem]: ProSystem.dat not found, cannot load internal ROM database\n");
database_enabled = false;
}
else
{
database_filename = database_file_path;
database_enabled = true;
}
}
// BIOS is optional
std::string system_directory(system_directory_c);
std::string bios_file_path = system_directory + "/7800 BIOS (U).rom";
if (bios_Load(bios_file_path.c_str()))
bios_enabled = true;
database_Load(cartridge_digest);
prosystem_Reset();
display_ResetPalette32();
return true;
}
return false;
}
