Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

switch to tf2 #31

Open
wants to merge 1 commit into
base: hydro-devel
Choose a base branch
from
Open

switch to tf2 #31

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion gmapping/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
cmake_minimum_required(VERSION 2.8)
project(gmapping)

find_package(catkin REQUIRED nav_msgs nodelet openslam_gmapping roscpp tf rosbag_storage)
find_package(catkin REQUIRED nav_msgs nodelet openslam_gmapping roscpp tf2 tf2_geometry_msgs tf2_ros rosbag_storage)

find_package(Boost REQUIRED signals)

Expand Down
12 changes: 8 additions & 4 deletions gmapping/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,17 +15,21 @@
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>

<build_depend>nav_msgs</build_depend>
<build_depend>nodelet</build_depend>
<build_depend>openslam_gmapping</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rostest</build_depend>
<build_depend>tf</build_depend>
<build_depend>nodelet</build_depend>
<build_depend>tf2</build_depend>
<build_depend>tf2_geometry_msgs</build_depend>
<build_depend>tf2_ros</build_depend>

<run_depend>nav_msgs</run_depend>
<run_depend>nodelet</run_depend>
<run_depend>openslam_gmapping</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>tf</run_depend>
<run_depend>nodelet</run_depend>
<run_depend>tf2</run_depend>
<run_depend>tf2_geometry_msgs</run_depend>
<run_depend>tf2_ros</run_depend>

<export>
<nodelet plugin="${prefix}/nodelet_plugins.xml" />
Expand Down
127 changes: 80 additions & 47 deletions gmapping/src/slam_gmapping.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,6 +114,9 @@ Initial map dimensions and resolution:
#include "ros/ros.h"
#include "ros/console.h"
#include "nav_msgs/MapMetaData.h"
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_msgs/TFMessage.h>

#include "gmapping/sensor/sensor_range/rangesensor.h"
#include "gmapping/sensor/sensor_odometry/odometrysensor.h"
Expand All @@ -126,27 +129,41 @@ Initial map dimensions and resolution:
// compute linear index for given map coords
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))

/* This function is only useful to have the whole code work
* with old rosbags that have trailing slashes for their frames
*/
inline
std::string stripSlash(const std::string& in)
{
std::string out = in;
if ( ( !in.empty() ) && (in[0] == '/') )
out.erase(0,1);
return out;
}

SlamGMapping::SlamGMapping():
map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
laser_count_(0), private_nh_("~"), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL)
{
tfL_.reset(new tf2_ros::TransformListener(tf_));
map_to_odom_.setIdentity();
seed_ = time(NULL);
init();
}

SlamGMapping::SlamGMapping(ros::NodeHandle& nh, ros::NodeHandle& pnh):
map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
laser_count_(0),node_(nh), private_nh_(pnh), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL)
{
tfL_.reset(new tf2_ros::TransformListener(tf_));
map_to_odom_.setIdentity();
seed_ = time(NULL);
init();
}

SlamGMapping::SlamGMapping(long unsigned int seed, long unsigned int max_duration_buffer):
map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
laser_count_(0), private_nh_("~"), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL),
seed_(seed), tf_(ros::Duration(max_duration_buffer))
seed_(seed), tf_(ros::Duration(max_duration_buffer)), tfL_(new tf2_ros::TransformListener(tf_))
{
map_to_odom_.setIdentity();
init();
}

Expand All @@ -160,7 +177,7 @@ void SlamGMapping::init()
gsp_ = new GMapping::GridSlamProcessor();
ROS_ASSERT(gsp_);

tfB_ = new tf::TransformBroadcaster();
tfB_ = new tf2_ros::TransformBroadcaster();
ROS_ASSERT(tfB_);

gsp_laser_ = NULL;
Expand All @@ -176,10 +193,13 @@ void SlamGMapping::init()
throttle_scans_ = 1;
if(!private_nh_.getParam("base_frame", base_frame_))
base_frame_ = "base_link";
base_frame_ = stripSlash(base_frame_);
if(!private_nh_.getParam("map_frame", map_frame_))
map_frame_ = "map";
map_frame_ = stripSlash(map_frame_);
if(!private_nh_.getParam("odom_frame", odom_frame_))
odom_frame_ = "odom";
odom_frame_ = stripSlash(odom_frame_);

private_nh_.param("transform_publish_period", transform_publish_period_, 0.05);

Expand Down Expand Up @@ -260,7 +280,7 @@ void SlamGMapping::startLiveSlam()
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
scan_filter_ = new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5, node_);
scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));

transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
Expand All @@ -287,15 +307,11 @@ void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_t
std::queue<std::pair<sensor_msgs::LaserScan::ConstPtr, std::string> > s_queue;
foreach(rosbag::MessageInstance const m, viewall)
{
tf::tfMessage::ConstPtr cur_tf = m.instantiate<tf::tfMessage>();
tf2_msgs::TFMessage::ConstPtr cur_tf = m.instantiate<tf2_msgs::TFMessage>();
if (cur_tf != NULL) {
for (size_t i = 0; i < cur_tf->transforms.size(); ++i)
{
geometry_msgs::TransformStamped transformStamped;
tf::StampedTransform stampedTf;
transformStamped = cur_tf->transforms[i];
tf::transformStampedMsgToTF(transformStamped, stampedTf);
tf_.setTransform(stampedTf);
tf_.setTransform(cur_tf->transforms[i], "slam_gmapping");
}
}

Expand All @@ -318,8 +334,8 @@ void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_t
{
try
{
tf::StampedTransform t;
tf_.lookupTransform(s_queue.front().first->header.frame_id, odom_frame_, s_queue.front().first->header.stamp, t);
tf_.lookupTransform(stripSlash(s_queue.front().first->header.frame_id),
odom_frame_, s_queue.front().first->header.stamp);
this->laserCallback(s_queue.front().first);
s_queue.pop();
}
Expand Down Expand Up @@ -369,90 +385,98 @@ 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;
centered_laser_pose_.header.stamp = t;
// Get the laser's pose that is centered
tf::Stamped<tf::Transform> odom_pose;
geometry_msgs::PoseStamped odom_pose;
try
{
tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);
tf_.transform(centered_laser_pose_, odom_pose, odom_frame_);
}
catch(tf::TransformException e)
catch(tf2::TransformException e)
{
ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
return false;
}
double yaw = tf::getYaw(odom_pose.getRotation());
double yaw = tf2::getYaw(odom_pose.pose.orientation);

gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(),
odom_pose.getOrigin().y(),
gmap_pose = GMapping::OrientedPoint(odom_pose.pose.position.x,
odom_pose.pose.position.y,
yaw);
return true;
}

bool
SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
{
laser_frame_ = scan.header.frame_id;
laser_frame_ = stripSlash(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;
geometry_msgs::PoseStamped laser_pose;
try
{
tf_.transformPose(base_frame_, ident, laser_pose);
geometry_msgs::PoseStamped ident;
ident.header.stamp = scan.header.stamp;
ident.header.frame_id = laser_frame_;
tf2::Transform transform;
transform.setIdentity();
tf2::toMsg(transform, ident.pose);
tf_.transform(ident, laser_pose, base_frame_);
}
catch(tf::TransformException e)
catch(tf2::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_);
geometry_msgs::PointStamped up;
up.header.stamp = scan.header.stamp;
up.header.frame_id = base_frame_;
up.point.x = up.point.y = 0;
up.point.z = 1 + laser_pose.pose.position.z;
try
{
tf_.transformPoint(laser_frame_, up, up);
ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
tf_.transform(up, up, laser_frame_);
ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.point.z);
}
catch(tf::TransformException& e)
catch(tf2::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)
if (fabs(fabs(up.point.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());
up.point.z);
return false;
}

gsp_laser_beam_count_ = scan.ranges.size();

double angle_center = (scan.angle_min + scan.angle_max)/2;

if (up.z() > 0)
centered_laser_pose_.header.frame_id = laser_frame_;
centered_laser_pose_.header.stamp = ros::Time::now();
tf2::Quaternion q;
if (up.point.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_);
q.setEuler(angle_center, 0, 0);
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_);
q.setEuler(-angle_center, 0, M_PI);
ROS_INFO("Laser is mounted upside down.");
}
centered_laser_pose_.pose.position.x = 0;
centered_laser_pose_.pose.position.y = 0;
centered_laser_pose_.pose.position.z = 0;
convert(q, centered_laser_pose_.pose.orientation);

// Compute the angles of the laser from -x to x, basically symmetric and in increasing order
laser_angles_.resize(scan.ranges.size());
Expand Down Expand Up @@ -621,8 +645,11 @@ SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
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));
tf2::Quaternion q;
q.setRPY(0, 0, mpose.theta);
tf2::Transform laser_to_map = tf2::Transform(q, tf2::Vector3(mpose.x, mpose.y, 0.0)).inverse();
q.setRPY(0, 0, odom_pose.theta);
tf2::Transform odom_to_laser = tf2::Transform(q, tf2::Vector3(odom_pose.x, odom_pose.y, 0.0));

map_to_odom_mutex_.lock();
map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
Expand Down Expand Up @@ -762,7 +789,7 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)

//make sure to set the header information on the map
map_.map.header.stamp = ros::Time::now();
map_.map.header.frame_id = tf_.resolve( map_frame_ );
map_.map.header.frame_id = map_frame_;

sst_.publish(map_.map);
sstm_.publish(map_.map.info);
Expand All @@ -786,6 +813,12 @@ void SlamGMapping::publishTransform()
{
map_to_odom_mutex_.lock();
ros::Time tf_expiration = ros::Time::now() + ros::Duration(tf_delay_);
tfB_->sendTransform( tf::StampedTransform (map_to_odom_, tf_expiration, map_frame_, odom_frame_));
geometry_msgs::TransformStamped transform;
transform.header.frame_id = map_frame_;
transform.header.stamp = tf_expiration;
transform.child_frame_id = odom_frame_;
transform.transform = tf2::toMsg(map_to_odom_);

tfB_->sendTransform( transform );
map_to_odom_mutex_.unlock();
}
19 changes: 11 additions & 8 deletions gmapping/src/slam_gmapping.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,10 +20,12 @@
#include "sensor_msgs/LaserScan.h"
#include "std_msgs/Float64.h"
#include "nav_msgs/GetMap.h"
#include "tf/transform_listener.h"
#include "tf/transform_broadcaster.h"
#include <tf2/LinearMath/Transform.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include "message_filters/subscriber.h"
#include "tf/message_filter.h"
#include <tf2_ros/message_filter.h>

#include "gmapping/gridfastslam/gridslamprocessor.h"
#include "gmapping/sensor/sensor_base/sensor.h"
Expand Down Expand Up @@ -54,18 +56,19 @@ class SlamGMapping
ros::Publisher sst_;
ros::Publisher sstm_;
ros::ServiceServer ss_;
tf::TransformListener tf_;
tf2_ros::Buffer tf_;
boost::shared_ptr<tf2_ros::TransformListener> tfL_;
message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_;
tf::MessageFilter<sensor_msgs::LaserScan>* scan_filter_;
tf::TransformBroadcaster* tfB_;
tf2_ros::MessageFilter<sensor_msgs::LaserScan>* scan_filter_;
tf2_ros::TransformBroadcaster* tfB_;

GMapping::GridSlamProcessor* gsp_;
GMapping::RangeSensor* gsp_laser_;
// The angles in the laser, going from -x to x (adjustment is made to get the laser between
// symmetrical bounds as that's what gmapping expects)
std::vector<double> laser_angles_;
// The pose, in the original laser frame, of the corresponding centered laser with z facing up
tf::Stamped<tf::Pose> centered_laser_pose_;
geometry_msgs::PoseStamped centered_laser_pose_;
// Depending on the order of the elements in the scan and the orientation of the scan frame,
// We might need to change the order of the scan
bool do_reverse_range_;
Expand All @@ -78,7 +81,7 @@ class SlamGMapping
nav_msgs::GetMap::Response map_;

ros::Duration map_update_interval_;
tf::Transform map_to_odom_;
tf2::Transform map_to_odom_;
boost::mutex map_to_odom_mutex_;
boost::mutex map_mutex_;

Expand Down
2 changes: 1 addition & 1 deletion gmapping/test/basic_localization_stage_replay.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,5 +4,5 @@
<!--test time-limit="30" test-name="basic_localization_stage" pkg="gmapping"
type="test_map.py" args="90.0"/-->
<!-- TODO: Fix the rtest to work with replay, for now it's not working -->
<test time-limit="250" test-name="map_data_test_replay" pkg="gmapping" type="gmapping-rtest" args="200.0 0.05 4000 4000 0.005 0.010"/>
<test time-limit="300" test-name="map_data_test_replay" pkg="gmapping" type="gmapping-rtest" args="200.0 0.05 4000 4000 0.005 0.010"/>
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The changes to tf -> tf2 should not change the behavior. tf is using tf2 under the hood these days so if anything it should be slightly faster due to fewer function calls due to no indirection. This suggests that there's some timeouts or delays coming in somewhere.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

performance was actually very close to that limit (I have it to fail sometimes at that limit before this patch).

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah, you don't want to be right on the cusp anyway, that a forumla for flaky tests.

</launch>
2 changes: 1 addition & 1 deletion gmapping/test/basic_localization_upside_down.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
<launch>
<node pkg="gmapping" type="slam_gmapping_replay" name="slam_gmapping_replay" output="screen"
args="--bag_filename $(find gmapping)/test/test_upside_down.bag --scan_topic /laserscan/fix/raw _base_frame:=base_floor"/>
<test time-limit="190" test-name="test_symmetry" pkg="gmapping" type="gmapping-rtest" args="180.0 0.05 4000 4000 0.002 0.010"/>
<test time-limit="250" test-name="test_symmetry" pkg="gmapping" type="gmapping-rtest" args="180.0 0.05 4000 4000 0.002 0.010"/>
</launch>