gmapping原始碼分析以及收穫
功能:建立執行緒
boost::thread
舉例說明:
#include <boost/thread/thread.hpp>
#include <iostream>
void hello()
{
std::cout <<
"Hello world, I'm a thread!"
<< std::endl;
}
int main(int argc, char* argv[])
{
boost::thread thrd(&hello);//這裡用一個回撥函式函式
thrd.join();//這裡讓一個執行緒開始
return 0;
}
功能:互斥體
boost::mutex
任何寫過多執行緒的人都知道,避免不同執行緒同時訪問共享區域的重要性。如果一個執行緒想要更改他共享區域中的某個資料,這時,另一個執行緒正在讀取這個資料。在這個時候就要用到互斥體。一個互斥體只允許一個執行緒訪問共享區。當另一個執行緒想要訪問共享區的時候,首先要做的就是鎖住(lock)
參考部落格:http://www.cnblogs.com/chengmin/archive/2011/12/29/2306416.html
建構函式初始化列表,
引數說明:
從地圖到世界座標
map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0 , 0, 0 ), tf::Point(0, 0, 0 )))從RPY中創造四元數
laser_count_(0),鐳射雷達計數
private_nh_("~"),私有的控制代碼
scan_filter_sub_(NULL),鐳射雷達訂閱
scan_filter_(NULL),鐳射雷達濾波器
transform_thread_(NULL),變換的執行緒,
unsigned long int seed_;這是一個長整型
目前是用roboware進行跳轉,真的很easy。
這裡建構函式三次過載。
在定義的時候,用一個*,然後將來例項化的就是&
gmapping這個名稱空間是在basic GridFastSLAM演算法的基礎上,每個粒子都有其自己的地圖和機器人的位姿。
第一個是初始化預設引數
第二個是預設的輸出流 infoStr是輸出流
所以,這裡的是為了初始化引數
然後跳轉到這裡,是傳遞的引數
讓其里程計的資料為NULL,空指標
bool型別的採用false
然後進行實時的傳值
throttle_scans表示處理的容忍度吧,最小的容忍度。如果設定很高,就會漏掉更多的點
設定引數的名字,已經數字,和string型別,都可以,然後對這個引數進行設定
以及這裡的數字
返回的是一些引數
以及TF釋出的時間
map_update_interval地圖更新的間隔
這些引數都是關於gmapping的封裝wrapper
gmapping自己的引數
minmumScore最小化評分,來評價scan的資料是不是好,也就是對他的置信度,在小範圍餓鐳射雷達例如5米的情況下
srr里程計的平移的/平移
srt里程計的平移/旋轉
stt里程計的旋轉/旋轉
str里程計的旋轉/平移
NULL是巨集 false是關鍵字
lskip 每束鐳射雷達跳過的鐳射束
ogain為了重新取樣而評估的增益
lstep 平移優化的步長
astep 旋轉優化的步長
kernelSize尋找關聯的
sigma用於暴力匹配
lsigma 每束鐳射雷達掃描的可能性
void startLiveSlam:
entropy熵(平均值)
這三個引數依次是 話題的名稱,佇列的長度,和是否釋出訊息
這裡是釋出的訊息
訂閱的話題
最後開始一個執行緒
關於獲得引數的補充:
訪問私有名稱作為工作空間
開始一個迴應startReplay
輸入的的 bag的的名字 和 鐳射雷達的topic
開啟一個新的
如果不能夠處理的話,5條訊息。
std::queue是容器介面卡用來適配FIFO
std::pair<int,float>將兩個資料融合成一個數據。
這裡是例項化了一個物件,下面肯定會有s_queue.first 和 s_queue.second
這裡是一個智慧指標
foreach用來遍歷
在32位系統中的size_t是4個位元組,size_t是8個位元組 size_t一般用來表示計數,比如有多少東西拷貝
這是使用了一個定時器
示例程式:
#include "ros/ros.h"
d callback1(const ros::TimerEvent&)
{
ROS_INFO("Callback 1 triggered");
}
void callback2(const ros::TimerEvent&)
{
ROS_INFO("Callback 2 triggered");
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "talker");
ros::NodeHandle n;
ros::Timer timer1 = n.createTimer(ros::Duration(0.1), callback1);
ros::Timer timer2 = n.createTimer(ros::Duration(1.0), callback2);
ros::spin();
return 0;
}
TimerEvent結構體提供當前定時器的時間資訊
callbacks回撥函式
單執行緒spinning
ros::spin();
在這個程式中,使用者的回撥函式在ros::spin()中被呼叫,ros::spin()在節點關閉或者在ros::shutdown();或者按下ctrl+c才會返回。
定期執行ros::spinonce();
ros::Rate r(19);//19Hz的釋出頻率
while(ros::ok())
{
ros::spinOnce();//會呼叫所有的回撥函式
r.sleep();
}
ros當中的多執行緒
方式1:這是一種阻塞式的
ros::MultiThreadedSpinner spinner(4);//使用了4個執行緒
spinner.spin();//spin()直到節點被關閉
方式2:這是一個非阻塞式的
ros::AsyncSpinner spinner(4);
spinner.start();
ros::waitForShutdown();
callbackqueue回撥佇列
#include<ros/callback_queue.h>
ros::CallbackQueue my_queue;
Logging(日誌)
ROS_DEBUG(...)
ROS_DEBUG_STREAM(args)
舉例
#include <ros/console.h>
ROS_DEBUG("Hello %s", "World");
ROS_DEBUG_STREAM("Hello " << "World");
cond表示 condition(條件)
當條件為真,就會輸出日誌資訊
include <ros/console.h>
ROS_DEBUG_COND(x < 0, "Uh oh, x = %d, this is bad", x);
ros當中轉換時間和持續時間
double secs =ros::Time::now().toSec();
ros::Duration d(0.5);
secs = d.toSec();
指定程式的休眠時間
ros::Duration(0.5).sleep(); // sleep for half a second
ros的釋出和訂閱
ros::Publisher pub = nh.advertise<std_msgs::String>("topic_name", 5);
std_msgs::String str;
str.data = "hello world";
pub.publish(str);
後面那個引數表示佇列的長度。
tf的資料結構:
tf::Quaternion tf::Vector3 tf::Point tf::Pose tf::Transform
tf::Stamped模板,就是一個模板類 附帶元素 frame_id_和stamp_
tf::StampedTransform 是tf::Transforms的特例,它要求frame_id、stamp、child_frame_id
函式:tf::Quaternion createIdentityQuaternion()
作用:返回四元數控制代碼
函式:tf::Quaternion createQuaternionFromRPY(double roll,double pitch,double yaw)
作用:返回從固定軸的Roll, Pitch and Yaw(滾動,俯仰和偏轉)構造的tf::Quaternion四元數
函式:geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll,double pitch,double yaw)
作用:返回從固定軸的Roll, Pitch and Yaw(滾動,俯仰和偏轉)構造geometry_msgs::Quaternion四元數
tf::TransformBroadcaster();傳送變換。
傳送變換通過呼叫sendTransform()函式實現
canTransform()函式 返回bool ,判斷能否實現變換。不會丟擲異常,如果出錯,會返回error_msg的內容。
waitForTransform()函式 返回bool值,評估變換是否有效
異常類:tf::ConnectivityException
作用:如果由於兩個座標系ID不在同一個連線的樹中而無法完成請求,則丟擲。
異常類:tf::ExtrapolationException
作用:如果請求的座標系id之間存在連線,但一個或多個變換已過期,則丟擲。
異常類:tf::InvalidArgument
作用:如果引數無效則丟擲。 最常見的情況是非規範化的四元數。
異常類:tf::LookupException
作用:如果引用了未釋出的座標系ID,則丟擲。
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg){
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_broadcaster");
if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
turtle_name = argv[1];
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
ros::spin();
return 0;
};
在C語言當中 atan2(double x,double y)返回的原點到(x,y)的方位角,也就是與x軸的夾角。
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_listener");
ros::NodeHandle node;
ros::service::waitForService("spawn");
ros::ServiceClient add_turtle =
node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);
ros::Publisher turtle_vel =
node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok()){
tf::StampedTransform transform;
try{
listener.lookupTransform("/turtle2", "/turtle1",
ros::Time(0), transform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
geometry_msgs::Twist vel_msg;
vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
transform.getOrigin().x());
vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
pow(transform.getOrigin().y(), 2));
turtle_vel.publish(vel_msg);
rate.sleep();
}
return 0;
};
增加一個carrot的座標系
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_broadcaster");
ros::NodeHandle node;
tf::TransformBroadcaster br;
tf::Transform transform;
ros::Rate rate(10.0);
while (node.ok()){
transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
rate.sleep();
}
return 0;
};
tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1")
同樣這各也是一個框架希望自己在使用的時候能夠記住
ros::Rate rate(10.0);
while(ros::ok())
{
rate.sleep();
}
ros::Time(0); 說明察覺到這個時間之後立即行動。遲疑的時間是0秒。
同樣的意思
ros::Time::now();
ros::Duration(3.0);持續時間是3秒
actionlib裡面很多內容暫時跳過去,因為在gmapping裡面看到的比較少。
pluginlib外掛的介紹
在package.xml檔案中包含export項
這裡寫學的不是很好
nodelet的用途和使用
nodelet包用來設計在演算法之間的具有零拷貝傳輸相同的程序中執行多個演算法
基本的用法:
nodelet usage:
nodelet load pkg/Type manager - launch一個nodelet的管理器
nodelet standalone pkg/Type - 匯入一個單個節點 node
nodelet unload name manager - 從manager匯入輸出一個單個的節點
nodelet manager - 開始manager的節點
可以在launch檔案中
匯入一些節點。
同時我們可以開發屬於自己的基控制器然後把這個事情做好。
我覺得這個nodelet的地方,到時候需要看看一下
然後這個下面還有一些實現路徑規劃功能的包。
言歸正傳,我們接著看gmapping的原始碼
找到一個不錯的看原始碼的工具scientific toolworks understand,然後安裝連線啥的,大家自行百度,剛剛沒存檔,就給沒了,哈哈。
其實我覺著就是一個很好的影象化顯示的東西,能夠讓你很直觀的看到
然你旋轉用ctrol flow的時候,一定要注意這點著函式然後走,我嘗試過,這個軟體不能看ros的原始碼,只能在幾個標頭檔案當中相互跳轉,但是實際用的時候,還是可以用roboware跳轉來看ros的原始碼
就是先從這個laserCallback函式,然後中間嵌套了
void
SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
laser_count_++;
if ((laser_count_ % throttle_scans_) != 0)
return;
static ros::Time last_map_update(0,0);
// We can't initialize the mapper until we've got the first scan
if(!got_first_scan_)
{
if(!initMapper(*scan))
return;
got_first_scan_ = true;
}
GMapping::OrientedPoint odom_pose;
if(addScan(*scan, odom_pose))
{
ROS_DEBUG("scan processed");
GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();
tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));
map_to_odom_mutex_.lock();
map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
map_to_odom_mutex_.unlock();
if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
{
updateMap(*scan);
last_map_update = scan->header.stamp;
ROS_DEBUG("Updated the map");
}
} else
ROS_DEBUG("cannot process scan");
}
首先給鐳射雷達的極速韓,然後初始化地圖。
bool
SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
{
laser_frame_ = scan.header.frame_id;
tf::Stamped<tf::Pose> ident; 設定單位矩陣
tf::Stamped<tf::Transform> laser_pose; 這是他的鐳射雷達的位姿
ident.setIdentity(); 設定單位矩陣
ident.frame_id_ = laser_frame_; 這是frameid
ident.stamp_ = scan.header.stamp; 這是ID
try 這裡用來丟擲一個異常,其實就是檢視著兩個TF之間的關係
{
tf_.transformPose(base_frame_, ident, laser_pose);兩個TF之間的關係
}
catch(tf::TransformException e)這裡輸出一個e
{
ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
e.what());
return false;
}
創造一個點在鐳射雷達laser_pose是他的位姿。然後就是在原點
// 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();對gmapping 進行計算數值
double angle_center = (scan.angle_min + scan.angle_max)/2;得到中間的角度值。
if (up.z() > 0)
{
do_reverse_range_ = scan.angle_min > scan.angle_max;如果最小的值比最大的值大的話,那麼返回一個bool值
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_);得到中心值,鐳射雷達啊的TF。然後這個裡面實現的過就是通尤拉角轉換輸出的四元數,然後設定3維的限量,最後輸出的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::Stamped 對上述資料型別做模板化(除了tf::Transform),並附帶元素frame_id_和stamp_ 。
identity matrix 單位矩陣,
我們定義tf::Transform transform 我們定義設定資料型別用來存放轉換資訊的(簡單來說 平移和旋轉的向量)
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );我懂了,這個地方是設定鐳射雷達餓起始位置,這個還是不錯的
當然你也可以定義旋轉
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
然後在將這個tf廣播出去
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
同樣定義一個監聽器
tf::TransformListener listener
定義存放一個關係變數
tf::StampedTransform transform;
listener.lookupTransform("/turtle2", "/turtle1",
ros::Time(0), transform);
然後通過這個函式lookupTransform用來監聽兩個TF之間的關係。
設定鐳射的角度,
通過resize這種方式,傳遞引數
將所有的鐳射累的資料進行累加,
這裡就是實現的將所有的鐳射累的資料進行的累計的疊加。
然後這就是輸出鐳射雷達的一些增量。
設定最大的R(也就是半徑的範圍)以及設定最大的U的範圍
這個鐳射雷達的名字必須被叫做FLASER,我們通過一些絕對值來計算增量。gmapping需要計算一些正向的角度增量。如果這個角度增量式一個複製,我們需要交換角度的順序才能將鐳射雷達的進行填充。
這裡需要例項化一個物件
position在哪裡插入點。插入點的數量n。pt 插入的點。
GMapping::SensorMap smap;
smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));//insert
gsp_->setSensorMap(smap);//將這個地圖得到地圖
make_pai
make_pair(gsp_laser_->getName(), gsp_laser_);//返回的值是一個值,就是將他們的資料結構。
然後在通過,insert將兩個資料結合起來。
gsp_->setSensorMap(smap);
然後
void GridSlamProcessor::setSensorMap(const SensorMap& smap){
SensorMap::const_iterator laser_it=smap.find(std::string("FLASER"));
if (laser_it==smap.end()){
cerr << "Attempting to load the new carmen log format" << endl;
laser_it=smap.find(std::string("ROBOTLASER1"));
assert(laser_it!=smap.end());
}
const RangeSensor* rangeSensor=dynamic_cast<const RangeSensor*>((laser_it->second));
assert(rangeSensor && rangeSensor->beams().size());
m_beams=static_cast<unsigned int>(rangeSensor->beams().size());
double* angles=new double[rangeSensor->beams().size()];
for (unsigned int i=0; i<m_beams; i++){
angles[i]=rangeSensor->beams()[i].pose.theta;
}
m_matcher.setLaserParameters(m_beams, angles, rangeSensor->getPose());
delete [] angles;
}
.find(std::string("FLASER")全域性尋找FLASER標誌
然後將角度進行賦值。,對著將這裡匹配到的地圖資料,釋出到鐳射雷達的引數當中。
他的輸入就是一個map的資料型別。
然後例項化一個里程計的型別。然後里程計資訊進行賦值。
下面我們打算設定初始化的位姿。
首先初始化一個位姿。,
這個bool型別,然後在bool型別當中,進行初始化很資料型別
stamp表示的時間戳吧。centered_laser_pose_.stamp_ = t;
得到鐳射雷達的位姿。佳作odom_pose
tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);
初始化一些他們之間的位姿關係。得到yaw ,然後將這個值,給了傳入的引數的數值。
gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(),
odom_pose.getOrigin().y(),
yaw);
然後做出了一些判斷,就是判斷,其實就是判斷有沒有得到位姿。
然後進行匹配一些引數
第一步就是匹配鐳射雷達的引數,然後設定運動模型的引數,第三步,更新距離的引數,第四步,設定更新的時間,第五步 設定更新的地圖。 第六步,進行初始化粒子的大小,第七步,設定取樣的範圍。第八步,設定鐳射雷達的步長 第九步,設定最小化得分。
第一步:
設定鐳射雷達的引數。
第二步:
設定運動模型,本質上還是傳遞引數。
第三步:
同樣將傳遞的引數進行賦值。
第四步:同樣傳遞引數。
第五步:設定丟他
回到lasercallback執行緒當中,然後將scan和odom_pose結合起來。一般bool型別,就是用來if語句來進行判斷。首先我得到getOdompose函式,這是一種bool型別。將num_ranges理邏輯。就是這兩個for型別。然後讀取reading函式,將ranges_double,然後設定位姿 其實就是一個傳參。將reading這個數值,給的資料傳遞給processscan
這裡返回的是一個bool型別。這個bool型別。bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles)
從reading當中,獲得位姿,同時計算里程計。首先我們初始化,然後獲得位姿。將上一個幀的位姿進行傳參。使用運動模型來更新例子。初始化一個引數,畫出運動模型,
OrientedPoint
MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
double sxy=0.3*srr;
OrientedPoint delta=absoluteDifference(pnew, pold);
OrientedPoint noisypoint(delta);
noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));
noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));
noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));
noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);
if (noisypoint.theta>M_PI)
noisypoint.theta-=2*M_PI;
return absoluteSum(p,noisypoint);
}
就是當前時刻的位姿,前一個時刻的位姿,
計算新舊幀的絕對誤差
absoluteDifference(pnew, pold);
從這裡看的話,我覺得是進行添加了一下高斯的擾動模型
最後輸出值,我估計是P
pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose);
將這些的更新的狀態給了pose
然後更新輸出的檔案,判斷是不是打開了這個檔案,然後設定了了一下精度。將當前的位置狀態的資訊,進行了了輸出。然後將一些粒子的更新進行了輸出。以及讀取的時間。
最後生成一個回撥函式,這個函式是空的。
接下來計算機器人的平移和旋轉。
計算機器人的旋轉和平移
計算角度的角度的變化 通過使用move將前一時刻機器人的位姿和當前時刻機器人的位姿進行做差。然後就是知道move.theta=atan2(sin(move.theta), cos(move.theta));
就是線速度的變換和加速度的變換。
隨後進行一次將來原來的pose資訊給了當前的位置資訊。進行了一次迭代。處理了一次迭代。更新當前幀
其實本質上就是次scan-matcher,然後就是一寫賦值。如果計數是大於0的話,然後就進行的掃描的匹配。
使用這個scanMatch來匹配每一幀的粒子。
inline void GridSlamProcessor::scanMatch(