/** * Main function * Navigate randomly using distance sensor data and Particle Filter-based * localization. */ int main(int argc, char** argv) { std::string robot_name = "/robot_0"; // Open file to store robot poses outfile.open("Output.txt"); // Write file header outfile << "Estimated and real pose of the robot\n\n" << "[T]: Odometry [X Y Theta] Real [X Y Theta] Particle [X Y Theta]\n\n"; // // Create map related variables // // We will create an image with the map wich contains the original map plus // a border around it, so as to allow estimates outside of the original // map. std::string map_file_path(getenv("HOME")); map_file_path += "/ros/worlds/stage/cave.png"; cv::Mat cave_map = cv::imread(map_file_path, CV_LOAD_IMAGE_COLOR); if( cave_map.data == 0 ) { std::cerr << "Error reading map!" << std::endl; return -1; } // Read original map and resize it to our resolution cv::resize(cave_map, map, map.size(), 0, 0, cv::INTER_AREA ); // Read original map and resize it to our resolution // We need the original map, black and white, so that it is faster to find // occupied cells cv::resize(cv::imread(map_file_path, CV_LOAD_IMAGE_GRAYSCALE), org_map, org_map.size(), 0, 0, cv::INTER_AREA ); // We need a temporary map tmp_map = map.clone(); // Create window for the map cv::namedWindow("Debug", 0); // // Particle filter related variables with their initial values // // This variable will hold our final estimation at each iteration geometry_msgs::Pose2D pose_estimate; // Initialize particles with uniform random distribution across all space cv::randu( particles_x, -MAP_LENGTH/2.0+SAFETY_BORDER, MAP_LENGTH/2.0-SAFETY_BORDER); // X cv::randu( particles_y, -MAP_LENGTH/2.0+SAFETY_BORDER, MAP_LENGTH/2.0-SAFETY_BORDER); // Y cv::randu( particles_theta, -CV_PI, CV_PI ); // Theta // Re-add particles that are outside of the map or inside an obstacle, until // it is in a free space inside the map. for(int n = 0; n < NUM_PARTICLES; n++) { while( ( (particles_x(n,0) > MAP_LENGTH/2.0) || (particles_x(n,0) < -MAP_LENGTH/2.0) || (particles_y(n,0) > MAP_LENGTH/2.0) || (particles_y(n,0) < -MAP_LENGTH/2.0) ) || (org_map.at<uchar>( cvRound(org_map.size().height/2.0 - particles_y(n,0)/MAP_RESOLUTION), cvRound(org_map.size().width/2.0 + particles_x(n,0)/MAP_RESOLUTION)) < 127) ) { particles_x(n,0) = rng.uniform(-MAP_LENGTH/2.0+SAFETY_BORDER, MAP_LENGTH/2.0-SAFETY_BORDER); particles_y(n,0) = rng.uniform(-MAP_LENGTH/2.0+SAFETY_BORDER, MAP_LENGTH/2.0-SAFETY_BORDER); } } // Init ROS ros::init(argc, argv, "tp7"); // ROS variables/objects ros::NodeHandle nh; // Node handle // Get parameters ros::NodeHandle n_private("~"); n_private.param("min_front_dist", min_front_dist, 1.0); n_private.param("stop_front_dist", stop_front_dist, 0.6); std::cout << "Random navigation with obstacle avoidance and particle filter" << " based localization\n---------------------------" << std::endl; /// Setup subscribers // Odometry ros::Subscriber sub_odom = nh.subscribe(robot_name + "/odom", 10, odomCallback); // Real, error-free robot pose (for debug purposes only) ros::Subscriber sub_real_pose = nh.subscribe(robot_name + "/base_pose_ground_truth", 1, realPoseCallback); // Laser scans ros::Subscriber sub_laser = nh.subscribe(robot_name + "/base_scan", 1, laserCallback); // Markers detected ros::Subscriber sub_markers = nh.subscribe(robot_name + "/markers", 1, markersCallback); // Setup publisher for linear and angular velocity vel_pub = nh.advertise<geometry_msgs::Twist>(robot_name + "/cmd_vel", 1); ros::Rate cycle(10.0); // Cycle rate // Wait until we get the robot pose (from odometry) while( odom_updated == false ) { ros::spinOnce(); cycle.sleep(); } // Stop the robot (if not stopped already) vel_cmd.angular.z = 0; vel_cmd.linear.x = 0; vel_pub.publish(vel_cmd); // Infinite loop (will call the callbacks whenever information is available, // until ros::shutdown() is called. ros::spin(); // If we are quitting, stop the robot vel_cmd.angular.z = 0; vel_cmd.linear.x = 0; vel_pub.publish(vel_cmd); // Close file outfile.close(); // Store final map cv::imwrite("mapa.png", map); return 1; }
TP8::TP8(const double map_resolution, const double map_length, const double map_border, const uint delta_save, const double max_lin_vel, const double max_ang_vel, std::string robotname): _MAP_RESOLUTION(map_resolution), _MAP_LENGTH(map_length), _MAP_BORDER(map_border), _DELTA_SAVE(delta_save), _lin_vel(0.0), _ang_vel(0.0), _avoid(false), _rotating(false), _rotate_left(false), _MAX_LIN_VEL(max_lin_vel), _MAX_ANG_VEL(max_ang_vel), _odom_updated(false), _odom_first_update(true), _iteration(0) { // Specify markers positions _markers_wpos[0].x = -8.0; _markers_wpos[0].y = 4.0; _markers_wpos[1].x = -8.0; _markers_wpos[1].y = -4.0; _markers_wpos[2].x = -3.0; _markers_wpos[2].y = -8.0; _markers_wpos[3].x = 3.0; _markers_wpos[3].y = -8.0; _markers_wpos[4].x = 8.0; _markers_wpos[4].y = -4.0; _markers_wpos[5].x = 8.0; _markers_wpos[5].y = 4.0; _markers_wpos[6].x = 3.0; _markers_wpos[6].y = 8.0; _markers_wpos[7].x = -3.0; _markers_wpos[7].y = 8.0; // Open file to store robot poses _outfile.open("Output.txt"); // Write file header _outfile << "Estimated and real pose of the robot\n\n" << "[T]: Odometry [X Y Theta] Real [X Y Theta] Particle [X Y Theta]\n\n"; // // Update/create map related variables // // We will create an image with the map wich contains the original map plus // a border around it, so as to allow estimates outside of the original // map. // Read original map and resize it to our resolution std::string map_file_path(getenv("HOME")); map_file_path += "/ros/worlds/stage/cave.png"; cv::Mat org_full_size_map = cv::imread(map_file_path, CV_LOAD_IMAGE_COLOR); if( org_full_size_map.data == 0 ) std::cerr << "Error reading map, program will shutdown!" << std::endl; _org_map.create(ceil(_MAP_LENGTH/_MAP_RESOLUTION), ceil(_MAP_LENGTH/_MAP_RESOLUTION), CV_8UC3); cv::resize(org_full_size_map, _org_map, cv::Size(ceil(_MAP_LENGTH/_MAP_RESOLUTION), ceil(_MAP_LENGTH/_MAP_RESOLUTION)), 0, 0, cv::INTER_AREA ); // This will be our actual map, with the border around it. _map.create(ceil((_MAP_LENGTH+_MAP_BORDER)/_MAP_RESOLUTION), ceil((_MAP_LENGTH+_MAP_BORDER)/_MAP_RESOLUTION), CV_8UC3); cv::copyMakeBorder(_org_map, _map, floor((_map.size().height-_org_map.size().height)/2), ceil((_map.size().height-_org_map.size().height)/2), floor((_map.size().width-_org_map.size().width)/2), ceil((_map.size().width-_org_map.size().width)/2), cv::BORDER_CONSTANT, cv::Scalar(255,255,255)); // Create window for the map cv::namedWindow("Debug", 0); // Initialize random number generator _rng(time(NULL)); /// Initilize Kalman filter related variables // The state holds X, Y and Theta estimate of the robot, all of type double. _ekf._state.create(3, 1); // Covariance matrix associated with the robot movement _ekf._V = (cv::Mat_<double>(3,3) << 0.10*0.10, 0.00, 0.00, 0.00, 0.03*0.03, 0.00, 0.00, 0.00, 0.20*0.20); // Process dynamics jacobian regarding the state // Only elements (1,3) and (2,3) need to be updated, so we set the correct //values for the other ones now. _ekf._Fx = (cv::Mat_<double>(3,3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); // Process dynamics jacobian regarding the inputs // Only elements (0,0), (0,1), (1,0) and (1,1) need to be updated, so we set // the correct the other ones now. _ekf._Fv = (cv::Mat_<double>(3,3) << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); // Process covariance (confidence associated with current state) // (It is called Σ in the theoretic material) _ekf._P = cv::Mat::zeros(3, 3, CV_64F); // Get parameters ros::NodeHandle n_private("~"); n_private.param("min_front_dist", _min_front_dist, 1.0); n_private.param("stop_front_dist", _stop_front_dist, 0.6); std::cout << "Random navigation with obstacle avoidance and EKF-based" << " localization\n---------------------------" << std::endl; /// Setup subscribers _sub_real_pose = _nh.subscribe(robotname + "/base_pose_ground_truth", 1, &TP8::realPoseCallback, this); _sub_odom = _nh.subscribe(robotname + "/odom", 10, &TP8::odomCallback, this); _sub_laser = _nh.subscribe(robotname + "/base_scan", 1, &TP8::laserCallback, this); _sub_markers = _nh.subscribe(robotname + "/markers", 1, &TP8::markersCallback, this); /// Setup publisher for linear and angular velocity _vel_pub = _nh.advertise<geometry_msgs::Twist>(robotname + "/cmd_vel", 1); /// Wait until we get an initial robot pose (from odometry) ros::Rate cycle(10.0); // Cycle rate while( _odom_updated == false ) { ros::spinOnce(); cycle.sleep(); } /// Stop the robot (if not stopped already) _vel_cmd.linear.x = _lin_vel; _vel_cmd.angular.z = _ang_vel; _vel_pub.publish(_vel_cmd); }