i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-10 11:39:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added tf2_ros dependency

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-14 00:42:21 -04:00
parent 2b46da2773
commit 550655c887
12 changed files with 104 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+21 -18
View File
@@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "CoreWrapper.h"
#include <stdio.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
@@ -35,30 +36,24 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <nav_msgs/OccupancyGrid.h>
#include <std_msgs/Int32MultiArray.h>
#include <std_msgs/Bool.h>
#include <visualization_msgs/MarkerArray.h>
#include <rtabmap/core/RtabmapEvent.h>
#include <rtabmap/core/Camera.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/core/util3d_mapping.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d_conversions.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/VWDictionary.h>
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UMath.h>
#include <opencv2/highgui/highgui.hpp>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <laser_geometry/laser_geometry.h>
#include <image_geometry/stereo_camera_model.h>
@@ -112,7 +107,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
mapFilterRadius_(0.5),
mapFilterAngle_(30.0), // degrees
mapCacheCleanup_(true),
mapToOdom_(tf::Transform::getIdentity()),
mapToOdom_(rtabmap::Transform::getIdentity()),
depthSync_(0),
depthScanSync_(0),
stereoScanSync_(0),
@@ -508,7 +503,12 @@ void CoreWrapper::publishLoop(double tfDelay)
{
mapToOdomMutex_.lock();
ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfDelay);
tfBroadcaster_.sendTransform( tf::StampedTransform (mapToOdom_, tfExpiration, mapFrameId_, odomFrameId_));
geometry_msgs::TransformStamped msg;
msg.child_frame_id = odomFrameId_;
msg.header.frame_id = mapFrameId_;
msg.header.stamp = tfExpiration;
rtabmap_ros::transformToGeometryMsg(mapToOdom_, msg.transform);
tfBroadcaster_.sendTransform(msg);
mapToOdomMutex_.unlock();
}
r.sleep();
@@ -627,6 +627,7 @@ bool CoreWrapper::commonOdomTFUpdate(const ros::Time & stamp)
{
if(waitForTransform_)
{
//if(!tfBuffer_.canTransform(odomFrameId_, frameId_, stamp, ros::Duration(1)))
if(!tfListener_.waitForTransform(odomFrameId_, frameId_, stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", odomFrameId_.c_str(), frameId_.c_str());
@@ -676,6 +677,7 @@ Transform CoreWrapper::getTransform(const std::string & fromFrameId, const std::
{
if(waitForTransform_)
{
//if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", fromFrameId.c_str(), toFrameId.c_str());
@@ -872,6 +874,7 @@ void CoreWrapper::commonStereoCallback(
//transform in frameId_ frame
sensor_msgs::PointCloud2 scanOut;
laser_geometry::LaserProjection projection;
//projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfBuffer_);
projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(scanOut, *pclScan);
@@ -1119,7 +1122,7 @@ void CoreWrapper::process(
{
timeRtabmap = timer.ticks();
mapToOdomMutex_.lock();
rtabmap_ros::transformToTF(rtabmap_.getMapCorrection(), mapToOdom_);
mapToOdom_ = rtabmap_.getMapCorrection();
odomFrameId_ = odomFrameId;
mapToOdomMutex_.unlock();
src/CoreWrapper.h
+5 -6
View File
@@ -31,12 +31,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <std_srvs/Empty.h>
#include <tf/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <cv_bridge/cv_bridge.h>
#include <std_msgs/Empty.h>
@@ -260,7 +259,7 @@ private:
double mapFilterAngle_;
bool mapCacheCleanup_;
tf::Transform mapToOdom_;
rtabmap::Transform mapToOdom_;
boost::mutex mapToOdomMutex_;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds_;
@@ -376,7 +375,7 @@ private:
sensor_msgs::CameraInfo> MyStereoExactTFSyncPolicy;
message_filters::Synchronizer<MyStereoExactTFSyncPolicy> * stereoExactTFSync_;
tf::TransformBroadcaster tfBroadcaster_;
tf2_ros::TransformBroadcaster tfBroadcaster_;
tf::TransformListener tfListener_;
ros::ServiceServer updateSrv_;
src/DbPlayerNode.cpp
+14 -9
View File
@@ -34,8 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <nav_msgs/Odometry.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <std_srvs/Empty.h>
#include <rtabmap_ros/MsgConversion.h>
#include <rtabmap/utilite/ULogger.h>
@@ -135,7 +134,7 @@ int main(int argc, char** argv)
ros::Publisher rightCamInfoPub;
ros::Publisher odometryPub;
ros::Publisher scanPub;
tf::TransformBroadcaster tfBroadcaster;
tf2_ros::TransformBroadcaster tfBroadcaster;
rtabmap::SensorData data = reader.getNextData();
while(ros::ok() && data.isValid())
@@ -229,16 +228,22 @@ int main(int argc, char** argv)
ros::Time tfExpiration = time + ros::Duration(1.0/rate);
if(!data.localTransform().isNull())
{
tf::Transform baseToCamera;
rtabmap_ros::transformToTF(data.localTransform(), baseToCamera);
tfBroadcaster.sendTransform( tf::StampedTransform (baseToCamera, tfExpiration, frameId, cameraFrameId));
geometry_msgs::TransformStamped baseToCamera;
baseToCamera.child_frame_id = cameraFrameId;
baseToCamera.header.frame_id = frameId;
baseToCamera.header.stamp = tfExpiration;
rtabmap_ros::transformToGeometryMsg(data.localTransform(), baseToCamera.transform);
tfBroadcaster.sendTransform(baseToCamera);
}
if(!data.pose().isNull())
{
tf::Transform odomToBase;
rtabmap_ros::transformToTF(data.pose(), odomToBase);
tfBroadcaster.sendTransform( tf::StampedTransform (odomToBase, tfExpiration, odomFrameId, frameId));
geometry_msgs::TransformStamped odomToBase;
odomToBase.child_frame_id = frameId;
odomToBase.header.frame_id = odomFrameId;
odomToBase.header.stamp = tfExpiration;
rtabmap_ros::transformToGeometryMsg(data.pose(), odomToBase.transform);
tfBroadcaster.sendTransform(odomToBase);
}
}
if(!data.pose().isNull())
src/MapOptimizerNode.cpp
+11 -7
View File
@@ -35,8 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UTimer.h>
#include <ros/subscriber.h>
#include <ros/publisher.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <boost/thread.hpp>
using namespace rtabmap;
@@ -52,7 +51,7 @@ public:
ignoreVariance_(false),
globalOptimization_(true),
optimizeFromLastNode_(false),
mapToOdom_(tf::Transform::getIdentity()),
mapToOdom_(rtabmap::Transform::getIdentity()),
transformThread_(0)
{
ros::NodeHandle nh;
@@ -103,7 +102,12 @@ public:
{
mapToOdomMutex_.lock();
ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfDelay);
tfBroadcaster_.sendTransform( tf::StampedTransform (mapToOdom_, tfExpiration, mapFrameId_, odomFrameId_));
geometry_msgs::TransformStamped msg;
msg.child_frame_id = odomFrameId_;
msg.header.frame_id = mapFrameId_;
msg.header.stamp = tfExpiration;
rtabmap_ros::transformToGeometryMsg(mapToOdom_, msg.transform);
tfBroadcaster_.sendTransform(msg);
mapToOdomMutex_.unlock();
r.sleep();
}
@@ -248,7 +252,7 @@ public:
mapToOdomMutex_.lock();
mapCorrection = optimizedPoses.at(poses.rbegin()->first) * poses.rbegin()->second.inverse();
rtabmap_ros::transformToTF(mapCorrection, mapToOdom_);
mapToOdom_ = mapCorrection;
mapToOdomMutex_.unlock();
}
else if(poses.size() == 1 && constraints.size() == 0)
@@ -289,7 +293,7 @@ private:
bool globalOptimization_;
bool optimizeFromLastNode_;
tf::Transform mapToOdom_;
rtabmap::Transform mapToOdom_;
boost::mutex mapToOdomMutex_;
ros::Subscriber mapDataTopic_;
@@ -303,7 +307,7 @@ private:
std::map<int, std::vector<unsigned char> > cachedUserDatas_;
std::multimap<int, Link> cachedConstraints_;
tf::TransformBroadcaster tfBroadcaster_;
tf2_ros::TransformBroadcaster tfBroadcaster_;
boost::thread* transformThread_;
};
src/MsgConversion.cpp
+25 -13
View File
@@ -32,8 +32,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <ros/ros.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/utilite/UStl.h>
#include <tf_conversions/tf_eigen.h>
#include <pcl_conversions/pcl_conversions.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf_conversions/tf_eigen.h>
namespace rtabmap_ros {
@@ -60,9 +61,7 @@ void transformToGeometryMsg(const rtabmap::Transform & transform, geometry_msgs:
{
if(!transform.isNull())
{
tf::Transform tfTransform;
transformToTF(transform, tfTransform);
tf::transformTFToMsg(tfTransform, msg);
tf::transformEigenToMsg(transform.toEigen3d(), msg);
}
else
{
@@ -73,18 +72,24 @@ void transformToGeometryMsg(const rtabmap::Transform & transform, geometry_msgs:
rtabmap::Transform transformFromGeometryMsg(const geometry_msgs::Transform & msg)
{
tf::Transform tfTransform;
tf::transformMsgToTF(msg, tfTransform);
return transformFromTF(tfTransform);
if(msg.rotation.w == 0 &&
msg.rotation.x == 0 &&
msg.rotation.y == 0 &&
msg.rotation.z ==0)
{
return rtabmap::Transform();
}
Eigen::Affine3d tfTransform;
tf::transformMsgToEigen(msg, tfTransform);
return rtabmap::Transform::fromEigen3d(tfTransform);
}
void transformToPoseMsg(const rtabmap::Transform & transform, geometry_msgs::Pose & msg)
{
if(!transform.isNull())
{
tf::Transform tfTransform;
transformToTF(transform, tfTransform);
tf::poseTFToMsg(tfTransform, msg);
tf::poseEigenToMsg(transform.toEigen3d(), msg);
}
else
{
@@ -94,9 +99,16 @@ void transformToPoseMsg(const rtabmap::Transform & transform, geometry_msgs::Pos
rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg)
{
tf::Pose tfTransform;
tf::poseMsgToTF(msg, tfTransform);
return transformFromTF(tfTransform);
if(msg.orientation.w == 0 &&
msg.orientation.x == 0 &&
msg.orientation.y == 0 &&
msg.orientation.z ==0)
{
return rtabmap::Transform();
}
Eigen::Affine3d tfPose;
tf::poseMsgToEigen(msg, tfPose);
return rtabmap::Transform::fromEigen3d(tfPose);
}
void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & bytes)
src/OdomMsgToTFNode.cpp
+8 -6
View File
@@ -27,8 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <rtabmap_ros/MsgConversion.h>
class OdomMsgToTF
@@ -59,7 +58,7 @@ public:
{
odomFrameId_ = msg->header.frame_id;
}
tf::StampedTransform t;
geometry_msgs::TransformStamped t;
rtabmap::Transform pose = rtabmap_ros::transformFromPoseMsg(msg->pose.pose);
if(pose.isNull())
{
@@ -67,8 +66,11 @@ public:
}
else
{
rtabmap_ros::transformToTF(pose, t);
tfBroadcaster_.sendTransform(tf::StampedTransform (t, msg->header.stamp, odomFrameId_, frameId_));
t.child_frame_id = frameId_;
t.header.frame_id = odomFrameId_;
t.header.stamp = msg->header.stamp;
rtabmap_ros::transformToGeometryMsg(pose, t.transform);
tfBroadcaster_.sendTransform(t);
}
}
@@ -77,7 +79,7 @@ private:
std::string odomFrameId_;
ros::Subscriber odomTopic_;
tf::TransformBroadcaster tfBroadcaster_;
tf2_ros::TransformBroadcaster tfBroadcaster_;
};
src/OdometryROS.cpp
+10 -7
View File
@@ -316,12 +316,15 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
//*********************
// Update odometry
//*********************
tf::Transform poseTF;
rtabmap_ros::transformToTF(pose, poseTF);
geometry_msgs::TransformStamped poseMsg;
poseMsg.child_frame_id = frameId_;
poseMsg.header.frame_id = odomFrameId_;
poseMsg.header.stamp = header.stamp;
rtabmap_ros::transformToGeometryMsg(pose, poseMsg.transform);
if(publishTf_)
{
tfBroadcaster_.sendTransform( tf::StampedTransform (poseTF, header.stamp, odomFrameId_, frameId_));
tfBroadcaster_.sendTransform(poseMsg);
}
if(odomPub_.getNumSubscribers())
@@ -333,10 +336,10 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
odom.child_frame_id = frameId_;
//set the position
odom.pose.pose.position.x = poseTF.getOrigin().x();
odom.pose.pose.position.y = poseTF.getOrigin().y();
odom.pose.pose.position.z = poseTF.getOrigin().z();
tf::quaternionTFToMsg(poseTF.getRotation().normalized(), odom.pose.pose.orientation);
odom.pose.pose.position.x = poseMsg.transform.translation.x;
odom.pose.pose.position.y = poseMsg.transform.translation.y;
odom.pose.pose.position.z = poseMsg.transform.translation.z;
odom.pose.pose.orientation = poseMsg.transform.rotation;
//set covariance
odom.pose.covariance.at(0) = info.variance; // xx
src/OdometryROS.h
+2 -3
View File
@@ -30,8 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <std_srvs/Empty.h>
@@ -88,7 +87,7 @@ private:
ros::ServiceServer resetToPoseSrv_;
ros::ServiceServer pauseSrv_;
ros::ServiceServer resumeSrv_;
tf::TransformBroadcaster tfBroadcaster_;
tf2_ros::TransformBroadcaster tfBroadcaster_;
tf::TransformListener tfListener_;
bool paused_;
src/nodelets/obstacles_detection.cpp
-1
View File
@@ -33,7 +33,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/PointCloud2.h>