void SlamNode::initialize(const sensor_msgs::LaserScan& initScan)
{
double xOffFactor = 0.0;
double yOffFactor = 0.0;
double yawOffset = 0.0;
double minRange = 0.0;
double maxRange = 0.0;
double lowReflectivityRange = 0.0;
double footPrintWidth = 0.0;
double footPrintHeight = 0.0;
double footPrintXoffset = 0.0;
bool icpSac = false;
ros::NodeHandle prvNh("~");
prvNh.param<double>("x_off_factor", xOffFactor, 0.2);
prvNh.param<double>("y_off_factor", yOffFactor, 0.5);
prvNh.param<double>("yaw_offset", yawOffset, 0.0);
prvNh.param<double>("min_range", minRange, 0.01);
prvNh.param<double>("low_reflectivity_range", lowReflectivityRange, 2.0);
prvNh.param<double>("footprint_width" , footPrintWidth, 0.1);
prvNh.param<double>("footprint_height", footPrintHeight, 0.1);
prvNh.param<double>("footprint_x_offset", footPrintXoffset, 0.28);
prvNh.param<bool> ("use_icpsac", icpSac, true);
_sensor=new obvious::SensorPolar2D(initScan.ranges.size(), initScan.angle_increment, initScan.angle_min, _maxRange, minRange, lowReflectivityRange);
_sensor->setRealMeasurementData(initScan.ranges, 1.0);
const double phi = yawOffset;
const double gridWidth = _grid->getCellsX() * _grid->getCellSize();
const double gridHeight = _grid->getCellsY() * _grid->getCellSize();
const obfloat startX = static_cast<obfloat>(gridWidth * xOffFactor);
const obfloat startY = static_cast<obfloat>(gridHeight * yOffFactor);
double tf[9] = {cos(phi), -sin(phi), gridWidth * xOffFactor,
sin(phi), cos(phi), gridHeight * yOffFactor,
0, 0, 1};
obvious::Matrix Tinit(3, 3);
Tinit.setData(tf);
_sensor->transform(&Tinit);
_threadMapping = new ThreadMapping(_grid);
const obfloat t[2] = {startX + footPrintXoffset, startY};
if(!_grid->freeFootprint(t, footPrintWidth, footPrintHeight))
std::cout << __PRETTY_FUNCTION__ << " warning! Footprint could not be freed!\n";
_threadMapping->initPush(_sensor);
_threadLocalizer = new ThreadLocalization(_grid, _threadMapping, _nh, xOffFactor, yOffFactor, icpSac);
_threadGrid = new ThreadGrid(_grid, _nh, xOffFactor, yOffFactor);
_initialized = true;
}
SlamNode::SlamNode(void)
{
omp_set_num_threads(8);
ros::NodeHandle prvNh("~");
std::string strVar;
int octaveFactor = 0;
double cellsize = 0.0;
double dVar = 0;
int iVar = 0;
double truncationRadius = 0.0;
prvNh.param ("laser_topic", strVar, std::string("simon/scan"));
prvNh.param<int> ("cell_octave_factor", octaveFactor, 10);
prvNh.param<double>("cellsize", cellsize, 0.01);
prvNh.param<int> ("truncation_radius", iVar, 3);
truncationRadius = static_cast<double>(iVar);
prvNh.param<double>("max_range", _maxRange, 30.0);
prvNh.param<double>("occ_grid_time_interval", _gridPublishInterval, 2.0);
prvNh.param<double>("loop_rate", _loopRate, 100.0);
_laserSubs = _nh.subscribe(strVar, 1, &SlamNode::laserScanCallBack, this);
unsigned int uiVar = static_cast<unsigned int>(octaveFactor);
if((uiVar > 15) || (uiVar < 5))
{
std::cout << __PRETTY_FUNCTION__ << " error! Unknown / Invalid cell_octave_factor -> set to default!" << std::endl;
uiVar = 10;
}
_initialized = false;
_grid = new obvious::TsdGrid(cellsize, obvious::LAYOUT_32x32, static_cast<obvious::EnumTsdGridLayout>(uiVar));
_grid->setMaxTruncation(truncationRadius * cellsize);
unsigned int cellsPerSide = std::pow(2, uiVar);
std::cout << __PRETTY_FUNCTION__ << " creating representation with " << cellsPerSide << "x" << cellsPerSide;
double sideLength = static_cast<double>(cellsPerSide) * cellsize;
std::cout << " cells, representing "<< sideLength << "x" << sideLength << "m^2" << std::endl;
_sensor = NULL;
_threadLocalizer = NULL;
_threadMapping = NULL;
_threadGrid = NULL;
}
ThreadGrid::ThreadGrid(obvious::TsdGrid* grid, ros::NodeHandle* const nh, const double xOffset, const double yOffset):
ThreadSLAM(*grid),
_occGrid(new nav_msgs::OccupancyGrid),
_occGridContent(new char[grid->getCellsX() * grid->getCellsY()]),
_gridCoords(new double[grid->getCellsX() * grid->getCellsY()]),
_width(grid->getCellsX()),
_height(grid->getCellsY()),
_cellSize(grid->getCellSize())
{
for(unsigned int i = 0; i < _grid.getCellsX() * _grid.getCellsY(); ++i)
_occGridContent[i] = -1;
_occGrid->info.resolution = static_cast<double>(_grid.getCellSize());
_occGrid->info.width = _grid.getCellsX();
_occGrid->info.height = _grid.getCellsY();
_occGrid->info.origin.orientation.w = 0.0;
_occGrid->info.origin.orientation.x = 0.0;
_occGrid->info.origin.orientation.y = 0.0;
_occGrid->info.origin.orientation.z = 0.0;
_occGrid->info.origin.position.x = 0.0 - (_grid.getCellsX() * _grid.getCellSize() * 0.5 + xOffset);
_occGrid->info.origin.position.y = 0.0 - (_grid.getCellsY() * _grid.getCellSize() * 0.5 + yOffset);
_occGrid->info.origin.position.z = 0.0;
_occGrid->data.resize(_grid.getCellsX() * _grid.getCellsY());
ros::NodeHandle prvNh("~");
std::string mapTopic;
std::string getMapTopic;
int intVar = 0;
prvNh.param("map_topic", mapTopic, std::string("map"));
prvNh.param("get_map_topic", getMapTopic, std::string("map"));
prvNh.param<int>("object_inflation_factor", intVar, 2);
prvNh.param<bool>("use_object_inflation", _objectInflation, false); //toDo: exchange with if inflation > 0
_gridPub = nh->advertise<nav_msgs::OccupancyGrid>(mapTopic, 1);
_getMapServ = nh->advertiseService(getMapTopic, &ThreadGrid::getMapServCallBack, this);
_objInflateFactor = static_cast<unsigned int>(intVar);
}
SlamNode::SlamNode(void)
{
ros::NodeHandle prvNh("~");
int iVar = 0;
double gridPublishInterval = 0.0;
double loopRateVar = 0.0;
double truncationRadius = 0.0;
double cellSize = 0.0;
unsigned int octaveFactor = 0;
double xOffset = 0.0;
double yOffset = 0.0;
std::string topicLaser;
prvNh.param<int>("robot_nbr", iVar, 1);
unsigned int robotNbr = static_cast<unsigned int>(iVar);
prvNh.param<double>("x_off_factor", _xOffFactor, 0.5);
prvNh.param<double>("y_off_factor", _yOffFactor, 0.5);
prvNh.param<double>("x_offset", xOffset, 0.0);
prvNh.param<double>("y_offset", yOffset, 0.0);
prvNh.param<int>("map_size", iVar, 10);
octaveFactor = static_cast<unsigned int>(iVar);
prvNh.param<double>("cellsize", cellSize, 0.025);
prvNh.param<int>("truncation_radius", iVar, 3);
truncationRadius = static_cast<double>(iVar);
prvNh.param<double>("occ_grid_time_interval", gridPublishInterval, 2.0);
prvNh.param<double>("loop_rate", loopRateVar, 40.0);
prvNh.param<std::string>("laser_topic", topicLaser, "scan");
_loopRate = new ros::Rate(loopRateVar);
_gridInterval = new ros::Duration(gridPublishInterval);
if(octaveFactor > 15)
{
ROS_ERROR_STREAM("Error! Unknown map size -> set to default!" << std::endl);
octaveFactor = 10;
}
//instanciate representation
_grid = new obvious::TsdGrid(cellSize, obvious::LAYOUT_32x32, static_cast<obvious::EnumTsdGridLayout>(octaveFactor)); //obvious::LAYOUT_8192x8192
_grid->setMaxTruncation(truncationRadius * cellSize);
unsigned int cellsPerSide = pow(2, octaveFactor);
double sideLength = static_cast<double>(cellsPerSide) * cellSize;
ROS_INFO_STREAM("Creating representation with " << cellsPerSide << "x" << cellsPerSide << "cells, representating " <<
sideLength << "x" << sideLength << "m^2" << std::endl);
//instanciate mapping threads
_threadMapping = new ThreadMapping(_grid);
_threadGrid = new ThreadGrid(_grid, &_nh, xOffset, yOffset);
ThreadLocalize* threadLocalize = NULL;
ros::Subscriber subs;
std::string nameSpace = "";
//instanciate localization threads
if(robotNbr == 1) //single slam
{
threadLocalize = new ThreadLocalize(_grid, _threadMapping, &_nh, nameSpace, xOffset, yOffset);
subs = _nh.subscribe(topicLaser, 1, &ThreadLocalize::laserCallBack, threadLocalize);
_subsLaser.push_back(subs);
_localizers.push_back(threadLocalize);
ROS_INFO_STREAM("Single SLAM started" << std::endl);
}
else
{
for(unsigned int i = 0; i < robotNbr; i++) //multi slam
{
std::stringstream sstream;
sstream << "robot";
sstream << i << "/namespace";
std::string dummy = sstream.str();
prvNh.param(dummy, nameSpace, std::string("default_ns"));
threadLocalize = new ThreadLocalize(_grid, _threadMapping, &_nh, nameSpace, xOffset, yOffset);
subs = _nh.subscribe(nameSpace + "/" + topicLaser, 1, &ThreadLocalize::laserCallBack, threadLocalize);
_subsLaser.push_back(subs);
_localizers.push_back(threadLocalize);
ROS_INFO_STREAM("started for thread for " << nameSpace << std::endl);
}
ROS_INFO_STREAM("Multi SLAM started!");
}
}
void ThreadLocalize::init(const sensor_msgs::LaserScan& scan)
{
double localXoffset = 0.0;
double localYoffset = 0.0;
double localYawOffset = 0.0;
double maxRange = 0.0;
double minRange = 0.0;
double lowReflectivityRange = 0.0;
double footPrintWidth = 0.0;
double footPrintHeight = 0.0;
double footPrintXoffset = 0.0;
//std::string frameSensorMount;
ros::NodeHandle prvNh("~");
prvNh.param<double>(_nameSpace + "local_offset_x",localXoffset, 0.0);
prvNh.param<double>(_nameSpace + "local_offset_y", localYoffset, 0.0);
prvNh.param<double>(_nameSpace + "local_offset_yaw", localYawOffset, 0.0);
prvNh.param<double>(_nameSpace + "max_range", maxRange, 30.0);
prvNh.param<double>(_nameSpace + "min_range", minRange, 0.001);
prvNh.param<double>(_nameSpace + "low_reflectivity_range", lowReflectivityRange, 2.0);
prvNh.param<double>(_nameSpace + "footprint_width", footPrintWidth, 1.0);
prvNh.param<double>(_nameSpace + "footprint_height", footPrintHeight, 1.0);
prvNh.param<double>(_nameSpace + "footprint_x_offset", footPrintXoffset, 0.28);
//prvNh.param<std::string>(_nameSpace + "frame_sensor_mount", frameSensorMount, "base_link");
// if(!_tfListener.waitForTransform(scan.header.frame_id, frameSensorMount, ros::Time::now(), ros::Duration(0.5)))
// {
// std::cout << __PRETTY_FUNCTION__ << "looking up sensor mount frame failed " << std::endl;
// _tfFrameSensorMount.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
// _tfFrameSensorMount.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
// }
// else
// {
// try
// {
// _tfListener.lookupTransform(scan.header.frame_id, frameSensorMount, ros::Time(0), _tfFrameSensorMount);
// }
// catch(tf::TransformException& ex)
// {
// std::cout << __PRETTY_FUNCTION__ << "looking up sensor mount frame failed " << ex.what() << std::endl;
// _tfFrameSensorMount.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
// _tfFrameSensorMount.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
// }
// }
const double phi = localYawOffset;
const double startX = _gridWidth * 0.5 + _xOffset + localXoffset;
const double startY = _gridHeight * 0.5 + _yOffset + localYoffset;
double tf[9] = {std::cos(phi), -std::sin(phi), startX,
std::sin(phi), std::cos(phi), startY,
0, 0, 1};
// obvious::Matrix Tbla(3, 3);
// Tbla = this->tfToObviouslyMatrix3x3(_tfFrameSensorMount);
//Tbla.invert();
// std::cout << __PRETTY_FUNCTION__ << " inverted\n";
// Tbla.print();
//std::cout << std::endl;
obvious::Matrix Tinit(3,3);
Tinit.setData(tf);
// Tinit = Tbla * Tinit;
//std::cout << __PRETTY_FUNCTION__ << "Tinit = \n " << Tinit << std::endl;
double inc = scan.angle_increment;
double angle_min = scan.angle_min;
vector<float> ranges = scan.ranges;
if(scan.angle_increment < 0.0 && scan.angle_min > 0)
{
_reverseScan = true;
inc = -inc;
angle_min = -angle_min;
std::reverse(ranges.begin(), ranges.end());
}
_sensor = new obvious::SensorPolar2D(ranges.size(), inc, angle_min, maxRange, minRange, lowReflectivityRange);
_sensor->setRealMeasurementData(ranges, 1.0);
_sensor->setStandardMask();
_sensor->transform(&Tinit);
obfloat t[2] = {startX + footPrintXoffset, startY};
if(!_grid.freeFootprint(t, footPrintWidth, footPrintHeight))
ROS_ERROR_STREAM("Localizer (" << _nameSpace << ") warning! Footprint could not be freed! \n");
if(!_mapper.initialized())
_mapper.initPush(_sensor);
_initialized = true;
//_sensor->transform()
this->unblock(); //Method from ThreadSLAM to set a thread from sleep mode to run mode
}
ThreadLocalize::ThreadLocalize(obvious::TsdGrid* grid, ThreadMapping* mapper, ros::NodeHandle* nh, std::string nameSpace,
const double xOffset, const double yOffset):
ThreadSLAM(*grid),
_nh(nh),
_mapper(*mapper),
_sensor(NULL), ///todo nullptr?
_initialized(false),
_gridWidth(grid->getCellsX() * grid->getCellSize()),
_gridHeight(grid->getCellsY() * grid->getCellSize()),
_gridOffSetX(-1.0 * (grid->getCellsX() * grid->getCellSize() * 0.5 + xOffset)),
_gridOffSetY(-1.0 * (grid->getCellsY() * grid->getCellSize() * 0.5 + yOffset)),
_xOffset(xOffset),
_yOffset(yOffset),
_nameSpace(nameSpace),
_stampLaser(ros::Time::now())
{
// ThreadLocalize* threadLocalize = NULL; ///todo wofür hab ich das hier eig. variable wird nicht benutzt?
double distFilterMax = 0.0;
double distFilterMin = 0.0;
int icpIterations = 0;
std::string poseTopic;
double durationWaitForOdom = 0.0;
///RandomMatcher options
int trials = 0;
int sizeControlSet = 0;
double epsThresh = 0.0;
double zhit = 0.0;
double zphi = 0.0;
double zshort = 0.0;
double zmax = 0.0;
double zrand = 0.0;
double percentagePointsInC = 0.0;
double rangemax = 0.0;
double sigphi = 0.0;
double sighit = 0.0;
double lamshort = 0.0;
double maxAngleDiff = 0.0;
double maxAnglePenalty = 0.0;
int paramInt = 0;
int iVar = 0;
/*** Read parameters from ros parameter server. Use namespace if provided. Multirobot use. ***/
_nameSpace = nameSpace;
std::string::iterator it = _nameSpace.end() - 1; //stores last symbol of nameSpace
if(*it != '/' && _nameSpace.size()>0)
_nameSpace += "/";
//pose
poseTopic = _nameSpace + poseTopic;
ros::NodeHandle prvNh("~");
//ICP Options
prvNh.param<double>(_nameSpace + "dist_filter_max", distFilterMax, DIST_FILT_MAX);
prvNh.param<double>(_nameSpace + "dist_filter_min", distFilterMin, DIST_FILT_MIN);
prvNh.param<int>(_nameSpace + "icp_iterations", icpIterations, ICP_ITERATIONS);
prvNh.param(_nameSpace + "pose_topic", poseTopic, std::string("default_ns/pose"));
prvNh.param("tf_base_frame", _tfBaseFrameId, std::string("/map"));
prvNh.param(_nameSpace + "tf_child_frame", _tfChildFrameId, std::string("default_ns/laser"));
// prvNh.param("tf_odom_frame", _tfOdomFrameId, std::string("wheelodom"));
prvNh.param("tf_footprint_frame", _tfFootprintFrameId, std::string("base_footprint"));
prvNh.param<double>("reg_trs_max", _trnsMax, TRNS_THRESH);
prvNh.param<double>("reg_sin_rot_max", _rotMax, ROT_THRESH);
prvNh.param<double>("max_velocity_lin", _trnsVelocityMax, TRNS_VEL_MAX);
prvNh.param<double>("max_velocity_rot", _rotVelocityMax, ROT_VEL_MAX);
prvNh.param<bool>("ude_odom_rescue", _useOdomRescue, false);
prvNh.param<double>("wait_for_odom_tf", durationWaitForOdom, 1.0);
prvNh.param<double>("laser_min_range", _lasMinRange, 0.0);
prvNh.param<int>("trials", trials, 100);
prvNh.param<int>("sizeControlSet", sizeControlSet, 140);
prvNh.param<double>("epsThresh", epsThresh, 0.15);
prvNh.param<double>("zhit", zhit, 0.45);
prvNh.param<double>("zphi", zphi, 0);
prvNh.param<double>("zshort", zshort, 0.25);
prvNh.param<double>("zmax", zmax, 0.05);
prvNh.param<double>("zrand", zrand, 0.25);
prvNh.param<double>("percentagePointsInC", percentagePointsInC, 0.9);
prvNh.param<double>("rangemax", rangemax, 20);
prvNh.param<double>("sigphi", sigphi, M_PI / 180.0 * 3);
prvNh.param<double>("sighit", sighit, 0.2);
prvNh.param<double>("lamshort", lamshort, 0.08);
prvNh.param<double>("maxAngleDiff", maxAngleDiff, 3.0);
prvNh.param<double>("maxAnglePenalty", maxAnglePenalty, 0.5);
//ransac options
prvNh.param<int>(nameSpace + "ransac_trials", paramInt, RANSAC_TRIALS); ///todo WARUM HIER nameSpace und nicht _nameSpace wie unten
_ranTrials = static_cast<unsigned int>(paramInt);
prvNh.param<double>(nameSpace + "ransac_eps_thresh", _ranEpsThresh, RANSAC_EPS_THRESH);
prvNh.param<int>(nameSpace + "ransac_ctrlset_size", paramInt, RANSAC_CTRL_SET_SIZE);
_ranSizeCtrlSet = static_cast<unsigned int>(paramInt);
prvNh.param<double>(_nameSpace + "ransac_phi_max", _ranPhiMax, 30.0);
prvNh.param<int>(_nameSpace + "registration_mode", iVar, ICP);
_regMode = static_cast<EnumRegModes>(iVar);
//Align laserscans
switch(_regMode)
{
case ICP:
//no instance needed
break;
case EXP:
_RandomNormalMatcher = new obvious::RandomNormalMatching(trials, epsThresh, sizeControlSet);
break;
case PDF:
_PDFMatcher = new obvious::PDFMatching(trials, epsThresh, sizeControlSet, zhit, zphi, zshort, zmax, zrand, percentagePointsInC, rangemax,
sigphi, sighit, lamshort, maxAngleDiff, maxAnglePenalty);
break;
case TSD:
_TSD_PDFMatcher = new obvious::TSD_PDFMatching(_grid, trials, epsThresh, sizeControlSet, zrand);
break;
default:
ROS_ERROR_STREAM("Unknown registration mode " << _regMode << " use default = ICP." << std::endl);
}
//_odomAnalyzer = NULL; ///todo nullptr? unten auch?
_waitForOdomTf = ros::Duration(durationWaitForOdom);
_odomTfIsValid = false;
_regMode = static_cast<EnumRegModes>(iVar);
_modelCoords = NULL;
_modelNormals = NULL;
_maskM = NULL;
_rayCaster = NULL; ///todo wofür wird der genullt? wird gleich hier unten initialisiert
_scene = NULL;
_maskS = NULL;
_lastPose = new obvious::Matrix(3, 3);
_rayCaster = new obvious::RayCastPolar2D();
_assigner = new obvious::FlannPairAssignment(2);
_filterDist = new obvious::DistanceFilter(distFilterMax, distFilterMin, icpIterations - 10);
_filterReciprocal = new obvious::ReciprocalFilter();
_estimator = new obvious::ClosedFormEstimator2D();
//configure ICP
_filterBounds = new obvious::OutOfBoundsFilter2D(grid->getMinX(), grid->getMaxX(), grid->getMinY(), grid->getMaxY());
_assigner->addPreFilter(_filterBounds);
_assigner->addPostFilter(_filterDist);
_assigner->addPostFilter(_filterReciprocal);
_icp = new obvious::Icp(_assigner, _estimator);
_icp->setMaxRMS(0.0);
_icp->setMaxIterations(icpIterations);
_icp->setConvergenceCounter(icpIterations);
_posePub = _nh->advertise<geometry_msgs::PoseStamped>(poseTopic, 1);
_poseStamped.header.frame_id = _tfBaseFrameId;
_tf.frame_id_ = _tfBaseFrameId;
_tf.child_frame_id_ = _nameSpace + _tfChildFrameId;
_reverseScan = false;
//_odomAnalyzer = new OdometryAnalyzer(_grid);
}
