initMapper() инициализирует карту
функция
Получите лазерные данные первого кадра и инициализируйте данные.
bool
SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
{
laser_frame_ = scan.header.frame_id;
// Get the laser's pose, relative to base.
tf::Stamped<tf::Pose> ident;
tf::Stamped<tf::Transform> laser_pose;
ident.setIdentity();
ident.frame_id_ = laser_frame_;
ident.stamp_ = scan.header.stamp;
try
{
tf_.transformPose(base_frame_, ident, laser_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
e.what());
return false;
}
// create a point 1m above the laser position and transform it into the laser-frame
tf::Vector3 v;
v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
tf::Stamped<tf::Vector3> up(v, scan.header.stamp,base_frame_);
try
{
tf_.transformPoint(laser_frame_, up, up);
ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
}
catch(tf::TransformException& e)
{
ROS_WARN("Unable to determine orientation of laser: %s",
e.what());
return false;
}
// gmapping doesnt take roll or pitch into account. So check for correct sensor alignment.
if (fabs(fabs(up.z()) - 1) > 0.001)
{
ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f",
up.z());
return false;
}
gsp_laser_beam_count_ = scan.ranges.size();
double angle_center = (scan.angle_min + scan.angle_max)/2;
if (up.z() > 0)
{
do_reverse_range_ = scan.angle_min > scan.angle_max;
centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(0,0,angle_center),
tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
ROS_INFO("Laser is mounted upwards.");
}
else
{
do_reverse_range_ = scan.angle_min < scan.angle_max;
centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(M_PI,0,-angle_center),
tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
ROS_INFO("Laser is mounted upside down.");
}
// Compute the angles of the laser from -x to x, basically symmetric and in increasing order
laser_angles_.resize(scan.ranges.size());
// Make sure angles are started so that they are centered
double theta = - std::fabs(scan.angle_min - scan.angle_max)/2;
for(unsigned int i=0; i<scan.ranges.size(); ++i)
{
laser_angles_[i]=theta;
theta += std::fabs(scan.angle_increment);
}
ROS_DEBUG("Laser angles in laser-frame: min: %.3f max: %.3f inc: %.3f", scan.angle_min, scan.angle_max,
scan.angle_increment);
ROS_DEBUG("Laser angles in top-down centered laser-frame: min: %.3f max: %.3f inc: %.3f", laser_angles_.front(),
laser_angles_.back(), std::fabs(scan.angle_increment));
GMapping::OrientedPoint gmap_pose(0, 0, 0);
// setting maxRange and maxUrange here so we can set a reasonable default
ros::NodeHandle private_nh_("~");
if(!private_nh_.getParam("maxRange", maxRange_))
maxRange_ = scan.range_max - 0.01;
if(!private_nh_.getParam("maxUrange", maxUrange_))
maxUrange_ = maxRange_;
// The laser must be called "FLASER".
// We pass in the absolute value of the computed angle increment, on the
// assumption that GMapping requires a positive angle increment. If the
// actual increment is negative, we'll swap the order of ranges before
// feeding each scan to GMapping.
gsp_laser_ = new GMapping::RangeSensor("FLASER",
gsp_laser_beam_count_,
fabs(scan.angle_increment),
gmap_pose,
0.0,
maxRange_);
ROS_ASSERT(gsp_laser_);
GMapping::SensorMap smap;
smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));
gsp_->setSensorMap(smap);
gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
ROS_ASSERT(gsp_odom_);
/// @todo Expose setting an initial pose
GMapping::OrientedPoint initialPose;
if(!getOdomPose(initialPose, scan.header.stamp))
{
ROS_WARN("Unable to determine inital pose of laser! Starting point will be set to zero.");
initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);
}
gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_,
kernelSize_, lstep_, astep_, iterations_,
lsigma_, ogain_, lskip_);
gsp_->setMotionModelParameters(srr_, srt_, str_, stt_);
gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
gsp_->setUpdatePeriod(temporalUpdate_);
gsp_->setgenerateMap(false);
gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
delta_, initialPose);
gsp_->setllsamplerange(llsamplerange_);
gsp_->setllsamplestep(llsamplestep_);
/// @todo Check these calls; in the gmapping gui, they use
/// llsamplestep and llsamplerange intead of lasamplestep and
/// lasamplerange. It was probably a typo, but who knows.
gsp_->setlasamplerange(lasamplerange_);
gsp_->setlasamplestep(lasamplestep_);
gsp_->setminimumScore(minimum_score_);
// Call the sampling function once to set the seed.
GMapping::sampleGaussian(1,seed_);
ROS_INFO("Initialization complete");
return true;
}
функциональная основа
- Используйте монитор tf_, чтобы получить положение лазера относительно базового laser_pose.
- Создайте точку на высоте 1 м над лазером (под base_frame (0, 0, laser_pose.z+1)), а затем преобразуйте в координаты относительно лазера.
- Если координата вверх по оси Z отлична от 1, это означает, что лазер установлен не горизонтально. инициализация не удалась
- Получите положение центра лазера и назначьте исходные данные лазера для laser_angles (тип — вектор);
- Инициализируйте объект модели лазерного датчика RangeSensor и объект модели одометра OdometrySensor.
- Используйте getOdomPose, чтобы установить начальную позу лазера, если настройка не удалась, установите ее на ноль (0,0,0)
- Установите соответствующие параметры процессора, расстояние обновления, частоту обновления, диапазон выборки, шаги выборки и другие параметры.
- Вызовите функцию выборки один раз, чтобы установить случайное начальное число
разбор функции
-
Сначала мы видим, что существует функция tf
tf_.transformPose(base_frame_, ident, laser_pose);Функция этой функции заключается в переносе данных из системы координат A в систему координат B.Функция определяется как:
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
target_frame — это поза, в которой вы хотите преобразовать исходную позу. Здесь нужно передать laser_pose (система координат радара) в base_frame_ (базовая система координат), поэтому target_frame этой функции — base_frame_.
Stamped_in — исходная поза, а Stamped_Out — целевые данные, то есть преобразованные данные.Следует отметить, что с точки зрения параметров, указывать исходный frame_id при конвертации не обязательно, так как он уже включен вstamped_in.Иными словами, неявное условие этой функции заключается в том, чтоstamped_in должен указывать Какой кадр исходная поза принадлежит.
- Эта функция создает точку на высоте 1 м над положением радара и преобразует ее в кадр лидара.
Во-первых, давайте взглянем на класс tf::Stamped, который представляет собой структуру данных, содержащую несколько других базовых структур данных, помимо Transform:
template <typename T> //模版结构可以是tf::Pose tf:Point 这些基本的结构
class Stamped : public T{
public:
ros::Time stamp_; //记录时间
std::string frame_id_; //ID
Stamped() :frame_id_ ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){}; //Default constructor used only for preallocation
Stamped(const T& input, const ros::Time& timestamp, const std::string & frame_id);
void setData(const T& input);
};
Сначала инициализируйте эту точку, затем перенесите эту точку в laser_frame_ (система координат лидара).
- gmapping не имеет крена и шага. Так что надо проверить, в норме ли датчик
- Проверьте, не слишком ли высокий или слишком низкий датчик
- Вычисляет, является ли угол лазера от -x до x симметричным и последовательно увеличивающимся.
- Убедитесь, что начальный угол является инкрементным
-
Получите положение центра лазера и назначьте исходные данные лазера для laser_angles
-
Инициализировать объект лазерного датчика RangeSensor и объект датчика одометра OdometrySensor.
Затем мы приходим к этой фразе:
gsp_laser_ = new GMapping::RangeSensor("FLASER",
gsp_laser_beam_count_,
fabs(scan.angle_increment),
gmap_pose,
0.0,
maxRange_);
Этот класс определяет объект лазерного датчика (обсуждается позже).
gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
Затем определите объект одометра (обсуждается отдельно позже).
- Используйте getOdomPose, чтобы установить начальную позу лазера, если настройка не удалась, установите ее на ноль (0,0,0)
Функция getOdomPose() внутри получает положение одометра.
- Установите соответствующие параметры процессора, расстояние обновления, частоту обновления, диапазон выборки, шаги выборки и другие параметры.
Обработка выполняется с помощью объекта класса GridSlamProcessor gsp_.
- Вызовите функцию выборки один раз, чтобы установить случайное начальное число
заполнить яму
Во введении функции вырыто много ям, некоторые классы, используемые в функции, используемые функции не объясняются подробно, это краткое изложение будет заполнять ямы одну за другой.
class RangeSensor
class RangeSensor: public Sensor{
friend class Configuration;
friend class CarmenConfiguration;
friend class CarmenWrapper;
public:
struct Beam{
OrientedPoint pose; //pose relative to the center of the sensor
double span; //spam=0 indicates a line-like beam
double maxRange; //maximum range of the sensor
double s,c; //sinus and cosinus of the beam (optimization);
};
RangeSensor(std::string name);
RangeSensor(std::string name, unsigned int beams, double res, const OrientedPoint& position=OrientedPoint(0,0,0), double span=0, double maxrange=89.0);
inline const std::vector<Beam>& beams() const {return m_beams;}
inline std::vector<Beam>& beams() {return m_beams;}
inline OrientedPoint getPose() const {return m_pose;}
void updateBeamsLookup();
bool newFormat;
protected:
OrientedPoint m_pose;
std::vector<Beam> m_beams;//放置激光的距离
};
Этот класс объявляет объект лазерного датчика, который наследует класс датчика.SensorВ этом классе нет ничего, в основном переменная имени датчика.
Классы друзей этого класса:Configuration,CarmenConfiguration,CarmenWrapper. Вот краткое описание класса друзей.
Ключевое слово friend в C++ на самом деле делает следующее: указывает, что другие классы (или) функции в классе могут напрямую обращаться к закрытым и защищенным членам этого класса.
Публичные члены класса определяютBeamструктура. В то же время также определяются переменная m_pose положения и расстояние m_beams лазера.
class OdometrySensor
class OdometrySensorНичего в нем нет, главное определитьm_idealЕго роль пока не ясна.
getOdomPose() получает положение одометра
bool SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t)
{
// Get the pose of the centered laser at the right time
centered_laser_pose_.stamp_ = t;
// Get the laser's pose that is centered
tf::Stamped<tf::Transform> odom_pose;
try
{
tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
return false;
}
double yaw = tf::getYaw(odom_pose.getRotation());
gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(),
odom_pose.getOrigin().y(),
yaw);
return true;
}
Функция вводит метку времени и возвращает соответствующую позу одометра. Примечание. Преобразуйте centered_laser_pose (лидарное положение) в систему координат odom_frame_, которая является текущей одометрической позицией. В следующей главе мы подробно рассмотрим функцию addScan(), которая также является основной частью gmapping.