mirror of
https://github.com/MAPIRlab/rf2o_laser_odometry.git
synced 2023-04-06 19:28:55 +08:00
fix indentation + credits
This commit is contained in:
Jeremie Deray
@@ -11,6 +11,8 @@
|
||||
*
|
||||
* Maintainer: Javier G. Monroy
|
||||
* MAPIR group: http://mapir.isa.uma.es/
|
||||
*
|
||||
* Modifications: Jeremie Deray
|
||||
******************************************************************************************** */
|
||||
|
||||
#ifndef CLaserOdometry2D_H
|
||||
@@ -72,74 +74,74 @@ public:
|
||||
using MatrixS31 = Eigen::Matrix<Scalar, 3, 1>;
|
||||
using IncrementCov = Eigen::Matrix<Scalar, 3, 3>;
|
||||
|
||||
CLaserOdometry2D();
|
||||
virtual ~CLaserOdometry2D() = default;
|
||||
CLaserOdometry2D();
|
||||
virtual ~CLaserOdometry2D() = default;
|
||||
|
||||
void init(const sensor_msgs::LaserScan& scan,
|
||||
const geometry_msgs::Pose& initial_robot_pose);
|
||||
void init(const sensor_msgs::LaserScan& scan,
|
||||
const geometry_msgs::Pose& initial_robot_pose);
|
||||
|
||||
bool is_initialized();
|
||||
bool is_initialized();
|
||||
|
||||
bool odometryCalculation(const sensor_msgs::LaserScan& scan);
|
||||
bool odometryCalculation(const sensor_msgs::LaserScan& scan);
|
||||
|
||||
void setLaserPose(const Pose3d& laser_pose);
|
||||
void setLaserPose(const Pose3d& laser_pose);
|
||||
|
||||
const Pose3d& getIncrement() const;
|
||||
const Pose3d& getIncrement() const;
|
||||
|
||||
const IncrementCov& getIncrementCovariance() const;
|
||||
const IncrementCov& getIncrementCovariance() const;
|
||||
|
||||
Pose3d& getPose();
|
||||
const Pose3d& getPose() const;
|
||||
Pose3d& getPose();
|
||||
const Pose3d& getPose() const;
|
||||
|
||||
protected:
|
||||
|
||||
bool verbose, module_initialized, first_laser_scan;
|
||||
|
||||
// Internal Data
|
||||
std::vector<Eigen::MatrixXf> range;
|
||||
std::vector<Eigen::MatrixXf> range_old;
|
||||
std::vector<Eigen::MatrixXf> range_inter;
|
||||
std::vector<Eigen::MatrixXf> range_warped;
|
||||
std::vector<Eigen::MatrixXf> xx;
|
||||
std::vector<Eigen::MatrixXf> xx_inter;
|
||||
std::vector<Eigen::MatrixXf> xx_old;
|
||||
std::vector<Eigen::MatrixXf> xx_warped;
|
||||
std::vector<Eigen::MatrixXf> yy;
|
||||
std::vector<Eigen::MatrixXf> yy_inter;
|
||||
std::vector<Eigen::MatrixXf> yy_old;
|
||||
std::vector<Eigen::MatrixXf> yy_warped;
|
||||
std::vector<Eigen::MatrixXf> transformations;
|
||||
// Internal Data
|
||||
std::vector<Eigen::MatrixXf> range;
|
||||
std::vector<Eigen::MatrixXf> range_old;
|
||||
std::vector<Eigen::MatrixXf> range_inter;
|
||||
std::vector<Eigen::MatrixXf> range_warped;
|
||||
std::vector<Eigen::MatrixXf> xx;
|
||||
std::vector<Eigen::MatrixXf> xx_inter;
|
||||
std::vector<Eigen::MatrixXf> xx_old;
|
||||
std::vector<Eigen::MatrixXf> xx_warped;
|
||||
std::vector<Eigen::MatrixXf> yy;
|
||||
std::vector<Eigen::MatrixXf> yy_inter;
|
||||
std::vector<Eigen::MatrixXf> yy_old;
|
||||
std::vector<Eigen::MatrixXf> yy_warped;
|
||||
std::vector<Eigen::MatrixXf> transformations;
|
||||
|
||||
Eigen::MatrixXf range_wf;
|
||||
Eigen::MatrixXf dtita;
|
||||
Eigen::MatrixXf dt;
|
||||
Eigen::MatrixXf rtita;
|
||||
Eigen::MatrixXf normx, normy, norm_ang;
|
||||
Eigen::MatrixXf weights;
|
||||
Eigen::MatrixXi null;
|
||||
Eigen::MatrixXf range_wf;
|
||||
Eigen::MatrixXf dtita;
|
||||
Eigen::MatrixXf dt;
|
||||
Eigen::MatrixXf rtita;
|
||||
Eigen::MatrixXf normx, normy, norm_ang;
|
||||
Eigen::MatrixXf weights;
|
||||
Eigen::MatrixXi null;
|
||||
|
||||
Eigen::MatrixXf A,Aw;
|
||||
Eigen::MatrixXf B,Bw;
|
||||
Eigen::MatrixXf A,Aw;
|
||||
Eigen::MatrixXf B,Bw;
|
||||
|
||||
MatrixS31 Var; //3 unknowns: vx, vy, w
|
||||
IncrementCov cov_odo;
|
||||
|
||||
//std::string LaserVarName; //Name of the topic containing the scan lasers \laser_scan
|
||||
float fps; //In Hz
|
||||
float fovh; //Horizontal FOV
|
||||
unsigned int cols;
|
||||
unsigned int cols_i;
|
||||
unsigned int width;
|
||||
unsigned int ctf_levels;
|
||||
unsigned int image_level, level;
|
||||
unsigned int num_valid_range;
|
||||
unsigned int iter_irls;
|
||||
float g_mask[5];
|
||||
//std::string LaserVarName; //Name of the topic containing the scan lasers \laser_scan
|
||||
float fps; //In Hz
|
||||
float fovh; //Horizontal FOV
|
||||
unsigned int cols;
|
||||
unsigned int cols_i;
|
||||
unsigned int width;
|
||||
unsigned int ctf_levels;
|
||||
unsigned int image_level, level;
|
||||
unsigned int num_valid_range;
|
||||
unsigned int iter_irls;
|
||||
float g_mask[5];
|
||||
|
||||
double lin_speed, ang_speed;
|
||||
|
||||
ros::WallDuration m_runtime;
|
||||
ros::Time last_odom_time, current_scan_time;
|
||||
ros::Time last_odom_time, current_scan_time;
|
||||
|
||||
MatrixS31 kai_abs_;
|
||||
MatrixS31 kai_loc_;
|
||||
@@ -154,24 +156,23 @@ protected:
|
||||
Pose3d robot_pose_;
|
||||
Pose3d robot_oldpose_;
|
||||
|
||||
bool test;
|
||||
std::vector<double> last_m_lin_speeds;
|
||||
std::vector<double> last_m_ang_speeds;
|
||||
bool test;
|
||||
std::vector<double> last_m_lin_speeds;
|
||||
std::vector<double> last_m_ang_speeds;
|
||||
|
||||
|
||||
// Methods
|
||||
void createImagePyramid();
|
||||
void calculateCoord();
|
||||
void performWarping();
|
||||
void calculaterangeDerivativesSurface();
|
||||
void computeNormals();
|
||||
void computeWeights();
|
||||
void findNullPoints();
|
||||
void solveSystemOneLevel();
|
||||
void solveSystemNonLinear();
|
||||
void filterLevelSolution();
|
||||
void PoseUpdate();
|
||||
void Reset(const Pose3d& ini_pose/*, CObservation2DRangeScan scan*/);
|
||||
// Methods
|
||||
void createImagePyramid();
|
||||
void calculateCoord();
|
||||
void performWarping();
|
||||
void calculaterangeDerivativesSurface();
|
||||
void computeNormals();
|
||||
void computeWeights();
|
||||
void findNullPoints();
|
||||
void solveSystemOneLevel();
|
||||
void solveSystemNonLinear();
|
||||
void filterLevelSolution();
|
||||
void PoseUpdate();
|
||||
void Reset(const Pose3d& ini_pose/*, CObservation2DRangeScan scan*/);
|
||||
};
|
||||
|
||||
#endif // CLaserOdometry2D_H
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -11,6 +11,8 @@
|
||||
*
|
||||
* Maintainer: Javier G. Monroy
|
||||
* MAPIR group: http://mapir.isa.uma.es/
|
||||
*
|
||||
* Modifications: Jeremie Deray
|
||||
******************************************************************************************** */
|
||||
|
||||
#include "rf2o_laser_odometry/CLaserOdometry2D.h"
|
||||
@@ -63,48 +65,48 @@ public:
|
||||
CLaserOdometry2DNode::CLaserOdometry2DNode() :
|
||||
CLaserOdometry2D()
|
||||
{
|
||||
ROS_INFO("Initializing RF2O node...");
|
||||
ROS_INFO("Initializing RF2O node...");
|
||||
|
||||
//Read Parameters
|
||||
//----------------
|
||||
ros::NodeHandle pn("~");
|
||||
pn.param<std::string>("laser_scan_topic",laser_scan_topic,"/laser_scan");
|
||||
pn.param<std::string>("odom_topic", odom_topic, "/odom_rf2o");
|
||||
pn.param<std::string>("base_frame_id", base_frame_id, "/base_link");
|
||||
pn.param<std::string>("odom_frame_id", odom_frame_id, "/odom");
|
||||
pn.param<bool>("publish_tf", publish_tf, true);
|
||||
pn.param<std::string>("init_pose_from_topic", init_pose_from_topic, "/base_pose_ground_truth");
|
||||
pn.param<double>("freq",freq,10.0);
|
||||
pn.param<bool>("verbose", verbose, true);
|
||||
//Read Parameters
|
||||
//----------------
|
||||
ros::NodeHandle pn("~");
|
||||
pn.param<std::string>("laser_scan_topic",laser_scan_topic,"/laser_scan");
|
||||
pn.param<std::string>("odom_topic", odom_topic, "/odom_rf2o");
|
||||
pn.param<std::string>("base_frame_id", base_frame_id, "/base_link");
|
||||
pn.param<std::string>("odom_frame_id", odom_frame_id, "/odom");
|
||||
pn.param<bool>("publish_tf", publish_tf, true);
|
||||
pn.param<std::string>("init_pose_from_topic", init_pose_from_topic, "/base_pose_ground_truth");
|
||||
pn.param<double>("freq",freq,10.0);
|
||||
pn.param<bool>("verbose", verbose, true);
|
||||
|
||||
//Publishers and Subscribers
|
||||
//--------------------------
|
||||
odom_pub = pn.advertise<nav_msgs::Odometry>(odom_topic, 5);
|
||||
laser_sub = n.subscribe<sensor_msgs::LaserScan>(laser_scan_topic,1,&CLaserOdometry2DNode::LaserCallBack,this);
|
||||
//Publishers and Subscribers
|
||||
//--------------------------
|
||||
odom_pub = pn.advertise<nav_msgs::Odometry>(odom_topic, 5);
|
||||
laser_sub = n.subscribe<sensor_msgs::LaserScan>(laser_scan_topic,1,&CLaserOdometry2DNode::LaserCallBack,this);
|
||||
|
||||
//init pose??
|
||||
if (init_pose_from_topic != "")
|
||||
{
|
||||
initPose_sub = n.subscribe<nav_msgs::Odometry>(init_pose_from_topic,1,&CLaserOdometry2DNode::initPoseCallBack,this);
|
||||
GT_pose_initialized = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
GT_pose_initialized = true;
|
||||
initial_robot_pose.pose.pose.position.x = 0;
|
||||
initial_robot_pose.pose.pose.position.y = 0;
|
||||
initial_robot_pose.pose.pose.position.z = 0;
|
||||
initial_robot_pose.pose.pose.orientation.w = 0;
|
||||
initial_robot_pose.pose.pose.orientation.x = 0;
|
||||
initial_robot_pose.pose.pose.orientation.y = 0;
|
||||
initial_robot_pose.pose.pose.orientation.z = 0;
|
||||
}
|
||||
//init pose??
|
||||
if (init_pose_from_topic != "")
|
||||
{
|
||||
initPose_sub = n.subscribe<nav_msgs::Odometry>(init_pose_from_topic,1,&CLaserOdometry2DNode::initPoseCallBack,this);
|
||||
GT_pose_initialized = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
GT_pose_initialized = true;
|
||||
initial_robot_pose.pose.pose.position.x = 0;
|
||||
initial_robot_pose.pose.pose.position.y = 0;
|
||||
initial_robot_pose.pose.pose.position.z = 0;
|
||||
initial_robot_pose.pose.pose.orientation.w = 0;
|
||||
initial_robot_pose.pose.pose.orientation.x = 0;
|
||||
initial_robot_pose.pose.pose.orientation.y = 0;
|
||||
initial_robot_pose.pose.pose.orientation.z = 0;
|
||||
}
|
||||
|
||||
setLaserPoseFromTf();
|
||||
setLaserPoseFromTf();
|
||||
|
||||
//Init variables
|
||||
module_initialized = false;
|
||||
first_laser_scan = true;
|
||||
//Init variables
|
||||
module_initialized = false;
|
||||
first_laser_scan = true;
|
||||
|
||||
ROS_INFO_STREAM("Listening laser scan from topic: " << laser_sub.getTopic());
|
||||
}
|
||||
@@ -119,14 +121,14 @@ bool CLaserOdometry2DNode::setLaserPoseFromTf()
|
||||
transform.setIdentity();
|
||||
try
|
||||
{
|
||||
tf_listener.lookupTransform(base_frame_id, last_scan.header.frame_id, ros::Time(0), transform);
|
||||
retrieved = true;
|
||||
tf_listener.lookupTransform(base_frame_id, last_scan.header.frame_id, ros::Time(0), transform);
|
||||
retrieved = true;
|
||||
}
|
||||
catch (tf::TransformException &ex)
|
||||
{
|
||||
ROS_ERROR("%s",ex.what());
|
||||
ros::Duration(1.0).sleep();
|
||||
retrieved = false;
|
||||
ROS_ERROR("%s",ex.what());
|
||||
ros::Duration(1.0).sleep();
|
||||
retrieved = false;
|
||||
}
|
||||
|
||||
//TF:transform -> Eigen::Isometry3d
|
||||
@@ -135,8 +137,8 @@ bool CLaserOdometry2DNode::setLaserPoseFromTf()
|
||||
Eigen::Matrix3d R;
|
||||
|
||||
for(int r = 0; r < 3; r++)
|
||||
for(int c = 0; c < 3; c++)
|
||||
R(r,c) = basis[r][c];
|
||||
for(int c = 0; c < 3; c++)
|
||||
R(r,c) = basis[r][c];
|
||||
|
||||
Pose3d laser_tf(R);
|
||||
|
||||
@@ -152,21 +154,21 @@ bool CLaserOdometry2DNode::setLaserPoseFromTf()
|
||||
|
||||
bool CLaserOdometry2DNode::scan_available()
|
||||
{
|
||||
return new_scan_available;
|
||||
return new_scan_available;
|
||||
}
|
||||
|
||||
void CLaserOdometry2DNode::process(const ros::TimerEvent&)
|
||||
{
|
||||
if( is_initialized() && scan_available() )
|
||||
{
|
||||
//Process odometry estimation
|
||||
odometryCalculation(last_scan);
|
||||
publish();
|
||||
new_scan_available = false; //avoids the possibility to run twice on the same laser scan
|
||||
//Process odometry estimation
|
||||
odometryCalculation(last_scan);
|
||||
publish();
|
||||
new_scan_available = false; //avoids the possibility to run twice on the same laser scan
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Waiting for laser_scans....") ;
|
||||
ROS_WARN("Waiting for laser_scans....") ;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -176,36 +178,36 @@ void CLaserOdometry2DNode::process(const ros::TimerEvent&)
|
||||
|
||||
void CLaserOdometry2DNode::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan)
|
||||
{
|
||||
if (GT_pose_initialized)
|
||||
{
|
||||
//Keep in memory the last received laser_scan
|
||||
last_scan = *new_scan;
|
||||
current_scan_time = last_scan.header.stamp;
|
||||
if (GT_pose_initialized)
|
||||
{
|
||||
//Keep in memory the last received laser_scan
|
||||
last_scan = *new_scan;
|
||||
current_scan_time = last_scan.header.stamp;
|
||||
|
||||
//Initialize module on first scan
|
||||
if (!first_laser_scan)
|
||||
{
|
||||
//copy laser scan to internal variable
|
||||
for (unsigned int i = 0; i<width; i++)
|
||||
range_wf(i) = new_scan->ranges[i];
|
||||
new_scan_available = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
init(last_scan, initial_robot_pose.pose.pose);
|
||||
first_laser_scan = false;
|
||||
}
|
||||
//Initialize module on first scan
|
||||
if (!first_laser_scan)
|
||||
{
|
||||
//copy laser scan to internal variable
|
||||
for (unsigned int i = 0; i<width; i++)
|
||||
range_wf(i) = new_scan->ranges[i];
|
||||
new_scan_available = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
init(last_scan, initial_robot_pose.pose.pose);
|
||||
first_laser_scan = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CLaserOdometry2DNode::initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose)
|
||||
{
|
||||
//Initialize module on first GT pose. Else do Nothing!
|
||||
if (!GT_pose_initialized)
|
||||
{
|
||||
initial_robot_pose = *new_initPose;
|
||||
GT_pose_initialized = true;
|
||||
}
|
||||
//Initialize module on first GT pose. Else do Nothing!
|
||||
if (!GT_pose_initialized)
|
||||
{
|
||||
initial_robot_pose = *new_initPose;
|
||||
GT_pose_initialized = true;
|
||||
}
|
||||
}
|
||||
|
||||
void CLaserOdometry2DNode::publish()
|
||||
@@ -214,17 +216,17 @@ void CLaserOdometry2DNode::publish()
|
||||
//---------------------------------------
|
||||
if (publish_tf)
|
||||
{
|
||||
//ROS_INFO("[rf2o] Publishing TF: [base_link] to [odom]");
|
||||
geometry_msgs::TransformStamped odom_trans;
|
||||
odom_trans.header.stamp = ros::Time::now();
|
||||
odom_trans.header.frame_id = odom_frame_id;
|
||||
odom_trans.child_frame_id = base_frame_id;
|
||||
odom_trans.transform.translation.x = robot_pose_.translation()(0);
|
||||
odom_trans.transform.translation.y = robot_pose_.translation()(1);
|
||||
odom_trans.transform.translation.z = 0.0;
|
||||
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(rf2o::getYaw(robot_pose_.rotation()));
|
||||
//send the transform
|
||||
odom_broadcaster.sendTransform(odom_trans);
|
||||
//ROS_INFO("[rf2o] Publishing TF: [base_link] to [odom]");
|
||||
geometry_msgs::TransformStamped odom_trans;
|
||||
odom_trans.header.stamp = ros::Time::now();
|
||||
odom_trans.header.frame_id = odom_frame_id;
|
||||
odom_trans.child_frame_id = base_frame_id;
|
||||
odom_trans.transform.translation.x = robot_pose_.translation()(0);
|
||||
odom_trans.transform.translation.y = robot_pose_.translation()(1);
|
||||
odom_trans.transform.translation.z = 0.0;
|
||||
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(rf2o::getYaw(robot_pose_.rotation()));
|
||||
//send the transform
|
||||
odom_broadcaster.sendTransform(odom_trans);
|
||||
}
|
||||
|
||||
//next, we'll publish the odometry message over ROS
|
||||
@@ -252,22 +254,22 @@ void CLaserOdometry2DNode::publish()
|
||||
//-----------------------------------------------------------------------------------
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "RF2O_LaserOdom");
|
||||
ros::init(argc, argv, "RF2O_LaserOdom");
|
||||
|
||||
CLaserOdometry2DNode myLaserOdomNode;
|
||||
CLaserOdometry2DNode myLaserOdomNode;
|
||||
|
||||
ros::TimerOptions timer_opt;
|
||||
timer_opt.oneshot = false;
|
||||
timer_opt.autostart = true;
|
||||
timer_opt.callback_queue = ros::getGlobalCallbackQueue();
|
||||
timer_opt.tracked_object = ros::VoidConstPtr();
|
||||
ros::TimerOptions timer_opt;
|
||||
timer_opt.oneshot = false;
|
||||
timer_opt.autostart = true;
|
||||
timer_opt.callback_queue = ros::getGlobalCallbackQueue();
|
||||
timer_opt.tracked_object = ros::VoidConstPtr();
|
||||
|
||||
timer_opt.callback = boost::bind(&CLaserOdometry2DNode::process, &myLaserOdomNode, _1);
|
||||
timer_opt.period = ros::Rate(myLaserOdomNode.freq).expectedCycleTime();
|
||||
timer_opt.callback = boost::bind(&CLaserOdometry2DNode::process, &myLaserOdomNode, _1);
|
||||
timer_opt.period = ros::Rate(myLaserOdomNode.freq).expectedCycleTime();
|
||||
|
||||
ros::Timer rf2o_timer = ros::NodeHandle("~").createTimer(timer_opt);
|
||||
ros::Timer rf2o_timer = ros::NodeHandle("~").createTimer(timer_opt);
|
||||
|
||||
ros::spin();
|
||||
ros::spin();
|
||||
|
||||
return EXIT_SUCCESS;
|
||||
return EXIT_SUCCESS;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user