/**
* 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);
}
