mirrors/rf2o_laser_odometry
1
0
Fork 0
mirror of https://github.com/MAPIRlab/rf2o_laser_odometry.git synced 2023-04-06 19:28:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix indentation + credits

Browse Source
This commit is contained in:
Jeremie Deray
2017-08-16 10:03:13 +02:00
parent f159d3eb2e
commit 77fb8e3a86
3 changed files with 779 additions and 789 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1245
src/CLaserOdometry2D.cpp
View File

File diff suppressed because it is too large Load Diff

196
src/CLaserOdometry2DNode.cpp
View File

@@ -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;
}