Commit dfde615c authored by Niklas Yann Wettengel's avatar Niklas Yann Wettengel
Browse files

WIP find initial position

parent ca0fa84f
......@@ -5,6 +5,9 @@
/homer_mapping/backside_checking: true # Enable checking to avoid matching front- and backside of obstacles, e.g. walls. Useful when creating high resolution maps
/homer_mapping/obstacle_borders: true # Leaves a small border around obstacles unchanged when inserting a laser scan. Improves stability of generated map
/homer_mapping/measure_sampling_step: 0.1 # m Minimum distance in m between two samples for probability calculation
/homer_mapping/initial_guess/particle_num: 50000
/homer_mapping/initial_guess/particle_num_run_2: 10000
/homer_mapping/initial_guess/particle_radius: 0.5
/homer_mapping/laser_scanner/free_reading_distance: 0.1 # Minimum distance in m to be classified as free in case of errorneous measurement
......
......@@ -156,7 +156,7 @@ public:
/**
* @return The resolution of the map in m.
*/
// int resolution() const;
float resolution() const;
geometry_msgs::Pose origin() const;
......
......@@ -189,6 +189,8 @@ public:
*/
void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg);
Pose findMapPosition(sensor_msgs::LaserScanConstPtr laserData, ros::Publisher &poseArrayPublisher);
private:
/**
* This method filter outliers in the given laser scan
......
......@@ -91,6 +91,8 @@ private:
Pose getInterpolatedPose(nav_msgs::Odometry::ConstPtr pose1, nav_msgs::Odometry::ConstPtr pose2,
ros::Time laserTime);
void guessInitialPose();
/**
* This variables stores the last odometry measurement as reference that is
* used
......
......@@ -111,6 +111,16 @@ void OccupancyMap::changeMapSize(int x_add_left, int y_add_up, int x_add_right,
m_MapPoints = tmpMap;
}
float OccupancyMap::resolution() const
{
return m_metaData.resolution;
}
geometry_msgs::Pose OccupancyMap::origin() const
{
return m_metaData.origin;
}
int OccupancyMap::width() const
{
return m_metaData.width;
......
#include <homer_mapping/ParticleFilter/SlamFilter.h>
#include <omp.h>
#include <geometry_msgs/PoseArray.h>
// minimum move for translation in m
const float MIN_MOVE_DISTANCE2 = 0.05 * 0.05;
......@@ -505,3 +506,173 @@ void SlamFilter::applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
m_OccupancyMap->applyMasking(msg);
}
Pose SlamFilter::findMapPosition(sensor_msgs::LaserScanConstPtr laserData, ros::Publisher &poseArrayPublisher)
{
const float map_resolution = m_OccupancyMap->resolution();
const double map_width = map_resolution * m_OccupancyMap->width();
const double map_height = map_resolution * m_OccupancyMap->height();
geometry_msgs::Pose map_origin = m_OccupancyMap->origin();
for (int i = 0; i < m_ParticleNum; ++i)
{
bool found_free_space = false;
geometry_msgs::Point p;
while(!found_free_space)
{
const double x = randomGauss() * map_width / 2;
const double y = randomGauss() * map_height / 2;
Eigen::Vector2i map_pixel;
map_pixel.x() = (x - map_origin.position.x) / map_resolution + 0.51;
map_pixel.y() = (y - map_origin.position.y) / map_resolution + 0.51;
float occupancy = m_OccupancyMap->getOccupancyProbability(map_pixel);
if ( occupancy > 0.1 )
{
continue;
}
else
{
found_free_space = true;
}
const double theta = randomGauss() * M_PI;
m_CurrentList[i]->setRobotPose(x, y, theta);
m_LastList[i]->setRobotPose(x, y, theta);
}
}
// Pose Array publishing
geometry_msgs::PoseArray poseArray = geometry_msgs::PoseArray();
poseArray.header.stamp = ros::Time::now();
poseArray.header.frame_id = "/map";
for (int i = 0; i < m_ParticleNum; ++i)
{
geometry_msgs::Pose tmpPose = geometry_msgs::Pose();
float x, y, theta;
m_CurrentList[i]->getRobotPose(x, y, theta);
tmpPose.position.x = x;
tmpPose.position.y = y;
tf::Quaternion quatTF = tf::createQuaternionFromYaw(theta);
geometry_msgs::Quaternion quatMsg;
tf::quaternionTFToMsg(quatTF, quatMsg);
tmpPose.orientation = quatMsg;
poseArray.poses.push_back(tmpPose);
}
poseArrayPublisher.publish(poseArray);
sensor_msgs::LaserScanPtr laserScan = boost::make_shared<sensor_msgs::LaserScan>(
*laserData); // copy const ptr to be able to change values
laserScan->ranges = filterOutliers(laserData, 0.3);
measure(laserScan);
normalize();
sort(0, m_ParticleNum - 1);
ROS_INFO_STREAM("FIRST RUN COMPLETE");
//ros::Duration(2.5).sleep();
float particle_radius;
ros::param::get("/homer_mapping/initial_guess/particle_radius", particle_radius);
// Pose Array publishing
poseArray = geometry_msgs::PoseArray();
poseArray.header.stamp = ros::Time::now();
poseArray.header.frame_id = "/map";
std::vector<Pose> likeliest_poses;
std::vector<float> likeliest_poses_weights;
for (int i = 0; i < 0.01*m_ParticleNum; ++i)
{
bool found = false;
Pose p = m_CurrentList[i]->getRobotPose();
geometry_msgs::Pose tmpPose = geometry_msgs::Pose();
tmpPose.position.x = p.x();
tmpPose.position.y = p.y();
tf::Quaternion quatTF = tf::createQuaternionFromYaw(p.theta());
geometry_msgs::Quaternion quatMsg;
tf::quaternionTFToMsg(quatTF, quatMsg);
tmpPose.orientation = quatMsg;
poseArray.poses.push_back(tmpPose);
for (int j = 0; j < likeliest_poses.size(); j++)
{
Pose q = likeliest_poses[j];
if(sqrt( (p.x()-q.x())*(p.x()-q.x()) + (p.y()-q.y())*(p.y()-q.y()) ) < particle_radius)
{
found = true;
break;
}
}
if(!found)
{
likeliest_poses.push_back(p);
likeliest_poses_weights.push_back(m_CurrentList[i]->getWeight());
}
}
poseArrayPublisher.publish(poseArray);
//ros::Duration(2.5).sleep();
// Pose Array publishing
poseArray = geometry_msgs::PoseArray();
poseArray.header.stamp = ros::Time::now();
poseArray.header.frame_id = "/map";
for (int i = 0; i < likeliest_poses.size(); ++i)
{
geometry_msgs::Pose tmpPose = geometry_msgs::Pose();
tmpPose.position.x = likeliest_poses[i].x();
tmpPose.position.y = likeliest_poses[i].y();
tf::Quaternion quatTF = tf::createQuaternionFromYaw(likeliest_poses[i].theta());
geometry_msgs::Quaternion quatMsg;
tf::quaternionTFToMsg(quatTF, quatMsg);
tmpPose.orientation = quatMsg;
poseArray.poses.push_back(tmpPose);
}
poseArrayPublisher.publish(poseArray);
//ros::Duration(2.5).sleep();
int new_particle_num;
ros::param::get("/homer_mapping/initial_guess/particle_num_run_2", new_particle_num);
reduceParticleNumber(new_particle_num);
ROS_INFO_STREAM("STARTING SECOND RUN");
for (int i = 0; i < likeliest_poses.size(); ++i)
{
ROS_INFO_STREAM(i << " of " << likeliest_poses.size());
setRobotPose(likeliest_poses[i], particle_radius, M_PI);
measure(laserScan);
normalize();
sort(0, m_ParticleNum - 1);
likeliest_poses[i] = getLikeliestPose();
likeliest_poses_weights[i] = 0;
for( int j = 0; j < 0.02*m_ParticleNum; j++ )
{
likeliest_poses_weights[i] += m_CurrentList[j]->getWeight();
}
}
// Pose Array publishing
poseArray = geometry_msgs::PoseArray();
poseArray.header.stamp = ros::Time::now();
poseArray.header.frame_id = "/map";
for (int i = 0; i < likeliest_poses.size(); ++i)
{
geometry_msgs::Pose tmpPose = geometry_msgs::Pose();
tmpPose.position.x = likeliest_poses[i].x();
tmpPose.position.y = likeliest_poses[i].y();
tf::Quaternion quatTF = tf::createQuaternionFromYaw(likeliest_poses[i].theta());
geometry_msgs::Quaternion quatMsg;
tf::quaternionTFToMsg(quatTF, quatMsg);
tmpPose.orientation = quatMsg;
poseArray.poses.push_back(tmpPose);
}
poseArrayPublisher.publish(poseArray);
//ros::Duration(2.5).sleep();
float max_weight = 0;
int max_weight_index = -1;
for (int i = 0; i < likeliest_poses_weights.size(); ++i)
{
if( likeliest_poses_weights[i] > max_weight )
{
max_weight = likeliest_poses_weights[i];
max_weight_index = i;
}
}
ROS_INFO_STREAM("max weight: " << max_weight);
return likeliest_poses[max_weight_index];
}
......@@ -290,6 +290,30 @@ Pose SlamNode::getInterpolatedPose(
return lastOdometryPose.interpolate(currentOdometryPose, timeFactor);
}
void SlamNode::guessInitialPose()
{
int particleNum;
ros::param::get("/homer_mapping/initial_guess/particle_num", particleNum);
while( m_laser_queue.size() == 0 )
{
ros::spinOnce();
ros::Duration( 0.01 ).sleep();
}
std::unique_ptr<SlamFilter> tmp_SlamFilter = std::make_unique<SlamFilter>(particleNum);
tmp_SlamFilter->setOccupancyMap(new OccupancyMap(*m_HyperSlamFilter->getBestSlamFilter()->getLikeliestMap()));
tmp_SlamFilter->setMapping(false);
sendPoseArray(tmp_SlamFilter->getParticlePoses());
sensor_msgs::LaserScan::ConstPtr laserData = m_laser_queue.at(0);
m_laser_queue.erase(m_laser_queue.begin());
Pose pose = tmp_SlamFilter->findMapPosition(laserData, m_PoseArrayPublisher);
m_HyperSlamFilter->setRobotPose(pose, m_ScatterVarXY, m_ScatterVarTheta);
sendTfAndPose(pose, laserData->header.stamp);
}
void SlamNode::callbackDoMapping(const std_msgs::Bool::ConstPtr& msg)
{
m_DoMapping = msg->data;
......@@ -312,6 +336,7 @@ void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr& msg)
m_DoMapping = false;
ROS_INFO_STREAM("Replacing occupancy map");
sendMapDataMessage(msg->info.map_load_time);
guessInitialPose();
}
void SlamNode::callbackMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment