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

- New param to set the topic name where to publish the odometry

Browse Source 
- New param to set if tf transformations are published or not (usefull when comparing different odometries)
- new option to set the initial pose of the robot (from another topic). This is useful for simulation when the robot spans at pose (X,Y).
- New launch file with the added params
This commit is contained in:
jgmonroy
2017-02-27 11:47:53 +01:00
parent 59a969448c
commit d8dc993f9e
3 changed files with 110 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
include/rf2o_laser_odometry/CLaserOdometry2D.h
View File

@@ -61,24 +61,29 @@ public:
void Init();
void odometryCalculation();
std::string laser_scan_topic;
std::string base_frame_id;
std::string odom_frame_id;
double freq;
std::string laser_scan_topic;
std::string odom_topic;
bool publish_tf;
std::string base_frame_id;
std::string odom_frame_id;
std::string init_pose_from_topic;
double freq;
protected:
ros::NodeHandle n;
sensor_msgs::LaserScan last_scan;
bool module_initialized,first_laser_scan,new_scan_available;
tf::TransformListener tf_listener; //Do not put inside the callback
tf::TransformBroadcaster odom_broadcaster;
ros::NodeHandle n;
sensor_msgs::LaserScan last_scan;
bool module_initialized,first_laser_scan,new_scan_available, GT_pose_initialized;
tf::TransformListener tf_listener; //Do not put inside the callback
tf::TransformBroadcaster odom_broadcaster;
nav_msgs::Odometry initial_robot_pose;
//Subscriptions & Publishers
ros::Subscriber laser_sub;
ros::Subscriber laser_sub, initPose_sub;
ros::Publisher odom_pub;
//CallBacks
void LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan);
void initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose);
// Internal Data
std::vector<Eigen::MatrixXf> range;

14
launch/rf2o_laser_odometry.launch
View File

@@ -10,10 +10,14 @@
<launch>
<node pkg="rf2o_laser_odometry" type="rf2o_laser_odometry_node" name="rf2o_laser_odometry">
<param name="laser_scan_topic" value="/laser_scan"/> <!-- topic where the lidar scans are being published -->
<param name="base_frame_id" value="/base_link"/> <!-- frame_id (tf) of the mobile robot base. A tf transform from the laser_frame to the base_frame is mandatory -->
<param name="odom_frame_id" value="/odom" /> <!-- frame_id (tf) to publish the odometry estimations -->
<param name="freq" value="6.0"/> <!-- Execution frequency. See "Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA'16"-->
<node pkg="rf2o_laser_odometry" type="rf2o_laser_odometry_node" name="rf2o_laser_odometry" output="screen">
<param name="laser_scan_topic" value="/laser_scan"/> # topic where the lidar scans are being published
<param name="odom_topic" value="/odom_rf2o" /> # topic where tu publish the odometry estimations
<param name="publish_tf" value="false" /> # wheter or not to publish the tf::transform (base->odom)
<param name="base_frame_id" value="/base_link"/> # frame_id (tf) of the mobile robot base. A tf transform from the laser_frame to the base_frame is mandatory
<param name="odom_frame_id" value="/odom" /> # frame_id (tf) to publish the odometry estimations
<param name="init_pose_from_topic" value="/base_pose_ground_truth" /> # (Odom topic) Leave empty to start at point (0,0)
<param name="freq" value="6.0"/> # Execution frequency. See "Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA'16"
</node>
</launch>

114
src/CLaserOdometry2D.cpp
View File

@@ -34,15 +34,36 @@ CLaserOdometry2D::CLaserOdometry2D()
//----------------
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);
//Publishers and Subscribers
//--------------------------
odom_pub = pn.advertise<nav_msgs::Odometry>(odom_frame_id, 5);
odom_pub = pn.advertise<nav_msgs::Odometry>(odom_topic, 5);
laser_sub = n.subscribe<sensor_msgs::LaserScan>(laser_scan_topic,1,&CLaserOdometry2D::LaserCallBack,this);
//init pose??
if (init_pose_from_topic != "")
{
initPose_sub = n.subscribe<nav_msgs::Odometry>(init_pose_from_topic,1,&CLaserOdometry2D::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 variables
module_initialized = false;
first_laser_scan = true;
@@ -101,12 +122,32 @@ void CLaserOdometry2D::Init()
LaserPoseOnTheRobot.setRotationMatrix(R);
//Set the initial pose
laser_pose = LaserPoseOnTheRobot;
laser_oldpose = LaserPoseOnTheRobot;
//Robot initial pose (see MQTT:bridge)
mrpt::poses::CPose3D robotInitialPose;
geometry_msgs::Pose _src = initial_robot_pose.pose.pose;
// Init module
//-------------
robotInitialPose.x(_src.position.x);
robotInitialPose.y(_src.position.y);
mrpt::math::CQuaternionDouble quat;
quat.x(_src.orientation.x);
quat.y(_src.orientation.y);
quat.z(_src.orientation.z);
quat.r(_src.orientation.w);
double roll, pitch, yaw;
quat.rpy(roll, pitch, yaw);
robotInitialPose.setYawPitchRoll(yaw,pitch,roll);
//robotInitialPose.phi(yaw);
//Set the initial pose
laser_pose = CPose2D(robotInitialPose + LaserPoseOnTheRobot);
laser_oldpose = CPose2D(robotInitialPose + LaserPoseOnTheRobot);
// Init module (internal)
//------------------------
range_wf.setSize(1, width);
//Resize vectors according to levels
@@ -976,16 +1017,19 @@ void CLaserOdometry2D::PoseUpdate()
//first, we'll publish the odometry over tf
//---------------------------------------
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = ros::Time::now();
odom_trans.header.frame_id = base_frame_id;
odom_trans.child_frame_id = odom_frame_id;
odom_trans.transform.translation.x = robot_pose.x();
odom_trans.transform.translation.y = robot_pose.y();
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(robot_pose.yaw());
//send the transform
odom_broadcaster.sendTransform(odom_trans);
if (publish_tf)
{
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = ros::Time::now();
odom_trans.header.frame_id = base_frame_id;
odom_trans.child_frame_id = odom_frame_id;
odom_trans.transform.translation.x = robot_pose.x();
odom_trans.transform.translation.y = robot_pose.y();
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(robot_pose.yaw());
//send the transform
odom_broadcaster.sendTransform(odom_trans);
}
//next, we'll publish the odometry message over ROS
//-------------------------------------------------
@@ -1014,21 +1058,35 @@ void CLaserOdometry2D::PoseUpdate()
void CLaserOdometry2D::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan)
{
//Keep in memory the last received laser_scan
last_scan = *new_scan;
if (GT_pose_initialized)
{
//Keep in memory the last received laser_scan
last_scan = *new_scan;
//Initialize module on first scan
if (first_laser_scan)
{
Init();
first_laser_scan = false;
//Initialize module on first scan
if (first_laser_scan)
{
Init();
first_laser_scan = false;
}
else
{
//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
}
void CLaserOdometry2D::initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose)
{
//Initialize module on first GT pose. Else do Nothing!
if (!GT_pose_initialized)
{
//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;
initial_robot_pose = *new_initPose;
GT_pose_initialized = true;
}
}