Public Member Functions | |
Mapper (ros::NodeHandle &n, ros::NodeHandle &pn) | |
Mapper (ros::NodeHandle &n, ros::NodeHandle &pn) | |
~Mapper () | |
~Mapper () | |
Protected Member Functions | |
bool | correctPose (ethzasl_icp_mapper::CorrectPose::Request &req, ethzasl_icp_mapper::CorrectPose::Response &res) |
bool | correctPose (ethzasl_icp_mapper::CorrectPose::Request &req, ethzasl_icp_mapper::CorrectPose::Response &res) |
bool | getBoundedMap (ethzasl_icp_mapper::GetBoundedMap::Request &req, ethzasl_icp_mapper::GetBoundedMap::Response &res) |
bool | getBoundedMap (ethzasl_icp_mapper::GetBoundedMap::Request &req, ethzasl_icp_mapper::GetBoundedMap::Response &res) |
bool | getMode (ethzasl_icp_mapper::GetMode::Request &req, ethzasl_icp_mapper::GetMode::Response &res) |
bool | getMode (ethzasl_icp_mapper::GetMode::Request &req, ethzasl_icp_mapper::GetMode::Response &res) |
bool | getPointMap (map_msgs::GetPointMap::Request &req, map_msgs::GetPointMap::Response &res) |
bool | getPointMap (map_msgs::GetPointMap::Request &req, map_msgs::GetPointMap::Response &res) |
string | getTransParamCsvHeader (const PM::TransformationParameters T, const string prefix) |
void | gotCloud (const sensor_msgs::PointCloud2 &cloudMsgIn) |
void | gotCloud (const sensor_msgs::PointCloud2 &cloudMsgIn) |
void | gotScan (const sensor_msgs::LaserScan &scanMsgIn) |
void | gotScan (const sensor_msgs::LaserScan &scanMsgIn) |
bool | loadMap (ethzasl_icp_mapper::LoadMap::Request &req, ethzasl_icp_mapper::LoadMap::Response &res) |
bool | loadMap (ethzasl_icp_mapper::LoadMap::Request &req, ethzasl_icp_mapper::LoadMap::Response &res) |
void | processCloud (unique_ptr< DP > cloud, const std::string &scannerFrame, const ros::Time &stamp, uint32_t seq) |
void | processCloud (unique_ptr< DP > cloud, const std::string &scannerFrame, const ros::Time &stamp, uint32_t seq) |
void | processNewMapIfAvailable () |
void | processNewMapIfAvailable () |
void | publishLoop (double publishPeriod) |
void | publishLoop (double publishPeriod) |
void | publishTransform () |
void | publishTransform () |
bool | reset (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) |
bool | reset (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) |
bool | saveMap (map_msgs::SaveMap::Request &req, map_msgs::SaveMap::Response &res) |
bool | saveMap (map_msgs::SaveMap::Request &req, map_msgs::SaveMap::Response &res) |
void | savePathToCsv (string fileName) |
string | serializeTransParamCSV (const PM::TransformationParameters T, const bool getHeader=false, const string prefix="") |
void | setMap (DP *newPointCloud) |
void | setMap (DP *newPointCloud) |
bool | setMode (ethzasl_icp_mapper::SetMode::Request &req, ethzasl_icp_mapper::SetMode::Response &res) |
bool | setMode (ethzasl_icp_mapper::SetMode::Request &req, ethzasl_icp_mapper::SetMode::Response &res) |
DP * | updateMap (DP *newPointCloud, const PM::TransformationParameters Ticp, bool updateExisting) |
DP * | updateMap (DP *newPointCloud, const PM::TransformationParameters Ticp, bool updateExisting) |
void | waitForMapBuildingCompleted () |
void | waitForMapBuildingCompleted () |
Private Types | |
typedef PM::DataPoints | DP |
typedef PM::DataPoints | DP |
typedef PM::Matches | Matches |
typedef Nabo::NearestNeighbourSearch < float > | NNS |
typedef NNS::SearchType | NNSearchType |
typedef PointMatcher< float > | PM |
typedef PointMatcher< float > | PM |
Private Attributes | |
const float | alpha |
ratio. Propability of staying static given that the point was dynamic | |
const float | beta |
ratio. Propability of staying dynamic given that the point was static | |
ros::Subscriber | cloudSub |
ros::ServiceServer | correctPoseSrv |
ros::Publisher | debugPub |
const float | eps |
const float | eps_a |
ratio. Error proportional to the laser distance | |
const float | eps_d |
in meter. Fix error on the laser distance | |
std::vector < PM::TransformationParameters > | errors |
vector of poses representing the correction applied by ICP. | |
ros::ServiceServer | getBoundedMapSrv |
ros::ServiceServer | getModeSrv |
ros::ServiceServer | getPointMapSrv |
PM::ICPSequence | icp |
PM::DataPointsFilters | inputFilters |
int | inputQueueSize |
ros::Time | lastPoinCloudTime |
ros::ServiceServer | loadMapSrv |
bool | localizing |
ros::Time | mapCreationTime |
const double | mapElevation |
string | mapFrame |
bool | mapping |
PM::DataPoints * | mapPointCloud |
PM::DataPointsFilters | mapPostFilters |
PM::DataPointsFilters | mapPreFilters |
ros::Publisher | mapPub |
const float | maxAngle |
in rad. Openning angle of a laser beam | |
const float | maxDistNewPoint |
in meter. Distance at which a new point will be added in the global map. | |
const float | maxDyn |
ratio. Threshold for which a point will stay dynamic | |
double | maxOverlapToMerge |
int | minMapPointCount |
double | minOverlap |
int | minReadingPointCount |
ros::NodeHandle & | n |
ros::Publisher | odomErrorPub |
string | odomFrame |
ros::Publisher | odomPub |
ros::Publisher | outlierPub |
std::vector < PM::TransformationParameters > | path |
vector of poses representing the path after ICP correction. | |
ros::NodeHandle & | pn |
const float | priorDyn |
ratio. Prior to be dynamic when a new point is added | |
const float | priorStatic |
ratio. Prior to be static when a new point is added | |
bool | processingNewCloud |
boost::mutex | publishLock |
bool | publishMapTf |
ros::Time | publishStamp |
boost::thread | publishThread |
ros::ServiceServer | resetSrv |
ros::ServiceServer | saveMapSrv |
ros::Subscriber | scanSub |
const float | sensorMaxRange |
in meter. Radius in which the global map needs to be updated when a new scan arrived. | |
ros::ServiceServer | setModeSrv |
tf::TransformBroadcaster | tfBroadcaster |
tf::TransformListener | tfListener |
double | tfRefreshPeriod |
if set to zero, tf will be publish at the rate of the incoming point cloud messages | |
PM::TransformationParameters | TOdomToMap |
unique_ptr< PM::Transformation > | transformation |
bool | useConstMotionModel |
string | vtkFinalMapName |
name of the final vtk map |
Definition at line 43 of file dynamic_mapper.cpp.
typedef PM::DataPoints Mapper::DP [private] |
Definition at line 42 of file mapper.cpp.
typedef PM::DataPoints Mapper::DP [private] |
Definition at line 46 of file dynamic_mapper.cpp.
typedef PM::Matches Mapper::Matches [private] |
Definition at line 47 of file dynamic_mapper.cpp.
typedef Nabo::NearestNeighbourSearch<float> Mapper::NNS [private] |
Definition at line 49 of file dynamic_mapper.cpp.
typedef NNS::SearchType Mapper::NNSearchType [private] |
Definition at line 50 of file dynamic_mapper.cpp.
typedef PointMatcher<float> Mapper::PM [private] |
Definition at line 41 of file mapper.cpp.
typedef PointMatcher<float> Mapper::PM [private] |
Definition at line 45 of file dynamic_mapper.cpp.
Mapper::Mapper | ( | ros::NodeHandle & | n, |
ros::NodeHandle & | pn | ||
) |
Definition at line 230 of file dynamic_mapper.cpp.
Mapper::~Mapper | ( | ) |
Definition at line 376 of file dynamic_mapper.cpp.
Mapper::Mapper | ( | ros::NodeHandle & | n, |
ros::NodeHandle & | pn | ||
) |
Mapper::~Mapper | ( | ) |
bool Mapper::correctPose | ( | ethzasl_icp_mapper::CorrectPose::Request & | req, |
ethzasl_icp_mapper::CorrectPose::Response & | res | ||
) | [protected] |
bool Mapper::correctPose | ( | ethzasl_icp_mapper::CorrectPose::Request & | req, |
ethzasl_icp_mapper::CorrectPose::Response & | res | ||
) | [protected] |
Definition at line 1153 of file dynamic_mapper.cpp.
bool Mapper::getBoundedMap | ( | ethzasl_icp_mapper::GetBoundedMap::Request & | req, |
ethzasl_icp_mapper::GetBoundedMap::Response & | res | ||
) | [protected] |
bool Mapper::getBoundedMap | ( | ethzasl_icp_mapper::GetBoundedMap::Request & | req, |
ethzasl_icp_mapper::GetBoundedMap::Response & | res | ||
) | [protected] |
Definition at line 1196 of file dynamic_mapper.cpp.
bool Mapper::getMode | ( | ethzasl_icp_mapper::GetMode::Request & | req, |
ethzasl_icp_mapper::GetMode::Response & | res | ||
) | [protected] |
bool Mapper::getMode | ( | ethzasl_icp_mapper::GetMode::Request & | req, |
ethzasl_icp_mapper::GetMode::Response & | res | ||
) | [protected] |
Definition at line 1187 of file dynamic_mapper.cpp.
bool Mapper::getPointMap | ( | map_msgs::GetPointMap::Request & | req, |
map_msgs::GetPointMap::Response & | res | ||
) | [protected] |
bool Mapper::getPointMap | ( | map_msgs::GetPointMap::Request & | req, |
map_msgs::GetPointMap::Response & | res | ||
) | [protected] |
Definition at line 1067 of file dynamic_mapper.cpp.
string Mapper::getTransParamCsvHeader | ( | const PM::TransformationParameters | T, |
const string | prefix | ||
) | [protected] |
Definition at line 172 of file dynamic_mapper.cpp.
void Mapper::gotCloud | ( | const sensor_msgs::PointCloud2 & | cloudMsgIn | ) | [protected] |
void Mapper::gotCloud | ( | const sensor_msgs::PointCloud2 & | cloudMsgIn | ) | [protected] |
Definition at line 411 of file dynamic_mapper.cpp.
void Mapper::gotScan | ( | const sensor_msgs::LaserScan & | scanMsgIn | ) | [protected] |
void Mapper::gotScan | ( | const sensor_msgs::LaserScan & | scanMsgIn | ) | [protected] |
Definition at line 401 of file dynamic_mapper.cpp.
bool Mapper::loadMap | ( | ethzasl_icp_mapper::LoadMap::Request & | req, |
ethzasl_icp_mapper::LoadMap::Response & | res | ||
) | [protected] |
bool Mapper::loadMap | ( | ethzasl_icp_mapper::LoadMap::Request & | req, |
ethzasl_icp_mapper::LoadMap::Response & | res | ||
) | [protected] |
Definition at line 1100 of file dynamic_mapper.cpp.
void Mapper::processCloud | ( | unique_ptr< DP > | cloud, |
const std::string & | scannerFrame, | ||
const ros::Time & | stamp, | ||
uint32_t | seq | ||
) | [protected] |
void Mapper::processCloud | ( | unique_ptr< DP > | cloud, |
const std::string & | scannerFrame, | ||
const ros::Time & | stamp, | ||
uint32_t | seq | ||
) | [protected] |
Definition at line 437 of file dynamic_mapper.cpp.
void Mapper::processNewMapIfAvailable | ( | ) | [protected] |
void Mapper::processNewMapIfAvailable | ( | ) | [protected] |
Definition at line 662 of file dynamic_mapper.cpp.
void Mapper::publishLoop | ( | double | publishPeriod | ) | [protected] |
void Mapper::publishLoop | ( | double | publishPeriod | ) | [protected] |
Definition at line 1040 of file dynamic_mapper.cpp.
void Mapper::publishTransform | ( | ) | [protected] |
void Mapper::publishTransform | ( | ) | [protected] |
Definition at line 1052 of file dynamic_mapper.cpp.
bool Mapper::reset | ( | std_srvs::Empty::Request & | req, |
std_srvs::Empty::Response & | res | ||
) | [protected] |
bool Mapper::reset | ( | std_srvs::Empty::Request & | req, |
std_srvs::Empty::Response & | res | ||
) | [protected] |
Definition at line 1139 of file dynamic_mapper.cpp.
bool Mapper::saveMap | ( | map_msgs::SaveMap::Request & | req, |
map_msgs::SaveMap::Response & | res | ||
) | [protected] |
bool Mapper::saveMap | ( | map_msgs::SaveMap::Request & | req, |
map_msgs::SaveMap::Response & | res | ||
) | [protected] |
Definition at line 1077 of file dynamic_mapper.cpp.
void Mapper::savePathToCsv | ( | string | fileName | ) | [protected] |
Definition at line 208 of file dynamic_mapper.cpp.
string Mapper::serializeTransParamCSV | ( | const PM::TransformationParameters | T, |
const bool | getHeader = false , |
||
const string | prefix = "" |
||
) | [protected] |
Definition at line 178 of file dynamic_mapper.cpp.
void Mapper::setMap | ( | DP * | newPointCloud | ) | [protected] |
void Mapper::setMap | ( | DP * | newPointCloud | ) | [protected] |
Definition at line 672 of file dynamic_mapper.cpp.
bool Mapper::setMode | ( | ethzasl_icp_mapper::SetMode::Request & | req, |
ethzasl_icp_mapper::SetMode::Response & | res | ||
) | [protected] |
bool Mapper::setMode | ( | ethzasl_icp_mapper::SetMode::Request & | req, |
ethzasl_icp_mapper::SetMode::Response & | res | ||
) | [protected] |
Definition at line 1175 of file dynamic_mapper.cpp.
DP* Mapper::updateMap | ( | DP * | newPointCloud, |
const PM::TransformationParameters | Ticp, | ||
bool | updateExisting | ||
) | [protected] |
Mapper::DP * Mapper::updateMap | ( | DP * | newPointCloud, |
const PM::TransformationParameters | Ticp, | ||
bool | updateExisting | ||
) | [protected] |
Definition at line 692 of file dynamic_mapper.cpp.
void Mapper::waitForMapBuildingCompleted | ( | ) | [protected] |
void Mapper::waitForMapBuildingCompleted | ( | ) | [protected] |
Definition at line 1028 of file dynamic_mapper.cpp.
const float Mapper::alpha [private] |
ratio. Propability of staying static given that the point was dynamic
Definition at line 121 of file dynamic_mapper.cpp.
const float Mapper::beta [private] |
ratio. Propability of staying dynamic given that the point was static
Definition at line 122 of file dynamic_mapper.cpp.
ros::Subscriber Mapper::cloudSub [private] |
Definition at line 57 of file dynamic_mapper.cpp.
ros::ServiceServer Mapper::correctPoseSrv [private] |
Definition at line 70 of file dynamic_mapper.cpp.
ros::Publisher Mapper::debugPub [private] |
Definition at line 61 of file dynamic_mapper.cpp.
const float Mapper::eps [private] |
Definition at line 139 of file dynamic_mapper.cpp.
const float Mapper::eps_a [private] |
ratio. Error proportional to the laser distance
Definition at line 119 of file dynamic_mapper.cpp.
const float Mapper::eps_d [private] |
in meter. Fix error on the laser distance
Definition at line 120 of file dynamic_mapper.cpp.
vector of poses representing the correction applied by ICP.
Definition at line 129 of file dynamic_mapper.cpp.
ros::ServiceServer Mapper::getBoundedMapSrv [private] |
Definition at line 73 of file dynamic_mapper.cpp.
ros::ServiceServer Mapper::getModeSrv [private] |
Definition at line 72 of file dynamic_mapper.cpp.
ros::ServiceServer Mapper::getPointMapSrv [private] |
Definition at line 66 of file dynamic_mapper.cpp.
PM::ICPSequence Mapper::icp [private] |
Definition at line 84 of file dynamic_mapper.cpp.
PM::DataPointsFilters Mapper::inputFilters [private] |
Definition at line 80 of file dynamic_mapper.cpp.
int Mapper::inputQueueSize [private] |
Definition at line 105 of file dynamic_mapper.cpp.
ros::Time Mapper::lastPoinCloudTime [private] |
Definition at line 77 of file dynamic_mapper.cpp.
ros::ServiceServer Mapper::loadMapSrv [private] |
Definition at line 68 of file dynamic_mapper.cpp.
bool Mapper::localizing [private] |
Definition at line 101 of file dynamic_mapper.cpp.
ros::Time Mapper::mapCreationTime [private] |
Definition at line 76 of file dynamic_mapper.cpp.
const double Mapper::mapElevation [private] |
Definition at line 113 of file dynamic_mapper.cpp.
string Mapper::mapFrame [private] |
Definition at line 110 of file dynamic_mapper.cpp.
bool Mapper::mapping [private] |
Definition at line 102 of file dynamic_mapper.cpp.
PM::DataPoints * Mapper::mapPointCloud [private] |
Definition at line 83 of file dynamic_mapper.cpp.
PM::DataPointsFilters Mapper::mapPostFilters [private] |
Definition at line 82 of file dynamic_mapper.cpp.
PM::DataPointsFilters Mapper::mapPreFilters [private] |
Definition at line 81 of file dynamic_mapper.cpp.
ros::Publisher Mapper::mapPub [private] |
Definition at line 60 of file dynamic_mapper.cpp.
const float Mapper::maxAngle [private] |
in rad. Openning angle of a laser beam
Definition at line 118 of file dynamic_mapper.cpp.
const float Mapper::maxDistNewPoint [private] |
in meter. Distance at which a new point will be added in the global map.
Definition at line 124 of file dynamic_mapper.cpp.
const float Mapper::maxDyn [private] |
ratio. Threshold for which a point will stay dynamic
Definition at line 123 of file dynamic_mapper.cpp.
double Mapper::maxOverlapToMerge [private] |
Definition at line 107 of file dynamic_mapper.cpp.
int Mapper::minMapPointCount [private] |
Definition at line 104 of file dynamic_mapper.cpp.
double Mapper::minOverlap [private] |
Definition at line 106 of file dynamic_mapper.cpp.
int Mapper::minReadingPointCount [private] |
Definition at line 103 of file dynamic_mapper.cpp.
ros::NodeHandle & Mapper::n [private] |
Definition at line 52 of file dynamic_mapper.cpp.
ros::Publisher Mapper::odomErrorPub [private] |
Definition at line 63 of file dynamic_mapper.cpp.
string Mapper::odomFrame [private] |
Definition at line 109 of file dynamic_mapper.cpp.
ros::Publisher Mapper::odomPub [private] |
Definition at line 62 of file dynamic_mapper.cpp.
ros::Publisher Mapper::outlierPub [private] |
Definition at line 53 of file mapper.cpp.
std::vector<PM::TransformationParameters> Mapper::path [private] |
vector of poses representing the path after ICP correction.
Definition at line 128 of file dynamic_mapper.cpp.
ros::NodeHandle & Mapper::pn [private] |
Definition at line 53 of file dynamic_mapper.cpp.
const float Mapper::priorDyn [private] |
ratio. Prior to be dynamic when a new point is added
Definition at line 117 of file dynamic_mapper.cpp.
const float Mapper::priorStatic [private] |
ratio. Prior to be static when a new point is added
Definition at line 116 of file dynamic_mapper.cpp.
bool Mapper::processingNewCloud [private] |
Definition at line 96 of file dynamic_mapper.cpp.
boost::mutex Mapper::publishLock [private] |
Definition at line 133 of file dynamic_mapper.cpp.
bool Mapper::publishMapTf [private] |
Definition at line 99 of file dynamic_mapper.cpp.
ros::Time Mapper::publishStamp [private] |
Definition at line 134 of file dynamic_mapper.cpp.
boost::thread Mapper::publishThread [private] |
Definition at line 132 of file dynamic_mapper.cpp.
ros::ServiceServer Mapper::resetSrv [private] |
Definition at line 69 of file dynamic_mapper.cpp.
ros::ServiceServer Mapper::saveMapSrv [private] |
Definition at line 67 of file dynamic_mapper.cpp.
ros::Subscriber Mapper::scanSub [private] |
Definition at line 56 of file dynamic_mapper.cpp.
const float Mapper::sensorMaxRange [private] |
in meter. Radius in which the global map needs to be updated when a new scan arrived.
Definition at line 125 of file dynamic_mapper.cpp.
ros::ServiceServer Mapper::setModeSrv [private] |
Definition at line 71 of file dynamic_mapper.cpp.
Definition at line 137 of file dynamic_mapper.cpp.
tf::TransformListener Mapper::tfListener [private] |
Definition at line 136 of file dynamic_mapper.cpp.
double Mapper::tfRefreshPeriod [private] |
if set to zero, tf will be publish at the rate of the incoming point cloud messages
Definition at line 108 of file dynamic_mapper.cpp.
Definition at line 131 of file dynamic_mapper.cpp.
unique_ptr< PM::Transformation > Mapper::transformation [private] |
Definition at line 85 of file dynamic_mapper.cpp.
bool Mapper::useConstMotionModel [private] |
Definition at line 100 of file dynamic_mapper.cpp.
string Mapper::vtkFinalMapName [private] |
name of the final vtk map
Definition at line 111 of file dynamic_mapper.cpp.