From 550655c88733c2b7b49e5c7f616a83f1a4044001 Mon Sep 17 00:00:00 2001 From: Mathieu Labbe Date: Thu, 14 May 2015 00:42:21 -0400 Subject: [PATCH] Added tf2_ros dependency --- CMakeLists.txt | 6 ++--- include/rtabmap_ros/MsgConversion.h | 3 +-- package.xml | 4 +++ src/CoreWrapper.cpp | 39 +++++++++++++++------------- src/CoreWrapper.h | 11 ++++---- src/DbPlayerNode.cpp | 23 +++++++++------- src/MapOptimizerNode.cpp | 18 ++++++++----- src/MsgConversion.cpp | 38 +++++++++++++++++---------- src/OdomMsgToTFNode.cpp | 14 +++++----- src/OdometryROS.cpp | 17 +++++++----- src/OdometryROS.h | 5 ++-- src/nodelets/obstacles_detection.cpp | 1 - 12 files changed, 104 insertions(+), 75 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bdcb5a71..7c5fb3a5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,7 +6,7 @@ project(rtabmap_ros) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs visualization_msgs - image_transport tf tf_conversions laser_geometry pcl_conversions + image_transport tf tf_conversions tf2_ros eigen_conversions laser_geometry pcl_conversions pcl_ros nodelet dynamic_reconfigure rviz message_filters class_loader genmsg stereo_msgs move_base_msgs ) @@ -87,9 +87,9 @@ catkin_package( INCLUDE_DIRS include LIBRARIES rtabmap_ros CATKIN_DEPENDS cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs visualization_msgs - image_transport tf tf_conversions laser_geometry pcl_conversions + image_transport tf tf_conversions tf2_ros eigen_conversions laser_geometry pcl_conversions pcl_ros nodelet dynamic_reconfigure rviz message_filters class_loader - stereo_msgs move_base_msgs + stereo_msgs move_base_msgs ) ########### diff --git a/include/rtabmap_ros/MsgConversion.h b/include/rtabmap_ros/MsgConversion.h index 4943853e..1d17b37e 100644 --- a/include/rtabmap_ros/MsgConversion.h +++ b/include/rtabmap_ros/MsgConversion.h @@ -28,8 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef MSGCONVERSION_H_ #define MSGCONVERSION_H_ -#include - +#include #include #include diff --git a/package.xml b/package.xml index 9c3be8cd..4398dd09 100644 --- a/package.xml +++ b/package.xml @@ -25,6 +25,8 @@ image_transport tf tf_conversions + tf2_ros + eigen_conversions laser_geometry pcl_conversions pcl_ros @@ -53,6 +55,8 @@ image_transport_plugins tf tf_conversions + tf2_ros + eigen_conversions laser_geometry pcl_conversions pcl_ros diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 947a7cd9..b345371e 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include "CoreWrapper.h" + #include #include #include @@ -35,30 +36,24 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include + #include -#include -#include -#include -#include -#include + +#include +#include +#include +#include +#include #include +#include #include #include #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include + #include #include @@ -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::Ptr pclScan(new pcl::PointCloud); 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(); diff --git a/src/CoreWrapper.h b/src/CoreWrapper.h index 8d79543b..17f80fac 100644 --- a/src/CoreWrapper.h +++ b/src/CoreWrapper.h @@ -31,12 +31,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include -#include -#include - #include +#include +#include + #include #include @@ -260,7 +259,7 @@ private: double mapFilterAngle_; bool mapCacheCleanup_; - tf::Transform mapToOdom_; + rtabmap::Transform mapToOdom_; boost::mutex mapToOdomMutex_; std::map::Ptr > clouds_; @@ -376,7 +375,7 @@ private: sensor_msgs::CameraInfo> MyStereoExactTFSyncPolicy; message_filters::Synchronizer * stereoExactTFSync_; - tf::TransformBroadcaster tfBroadcaster_; + tf2_ros::TransformBroadcaster tfBroadcaster_; tf::TransformListener tfListener_; ros::ServiceServer updateSrv_; diff --git a/src/DbPlayerNode.cpp b/src/DbPlayerNode.cpp index 83cba56d..cc08104a 100644 --- a/src/DbPlayerNode.cpp +++ b/src/DbPlayerNode.cpp @@ -34,8 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include -#include +#include #include #include #include @@ -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()) diff --git a/src/MapOptimizerNode.cpp b/src/MapOptimizerNode.cpp index 778ae14b..7ebad30b 100644 --- a/src/MapOptimizerNode.cpp +++ b/src/MapOptimizerNode.cpp @@ -35,8 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include -#include +#include #include 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 > cachedUserDatas_; std::multimap cachedConstraints_; - tf::TransformBroadcaster tfBroadcaster_; + tf2_ros::TransformBroadcaster tfBroadcaster_; boost::thread* transformThread_; }; diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index 994a975e..a46f53b8 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -32,8 +32,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include #include +#include +#include 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 & bytes) diff --git a/src/OdomMsgToTFNode.cpp b/src/OdomMsgToTFNode.cpp index 8cd5bbe0..b45b14d4 100644 --- a/src/OdomMsgToTFNode.cpp +++ b/src/OdomMsgToTFNode.cpp @@ -27,8 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include -#include +#include #include 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_; }; diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 1e63d2d4..4c9cd6c5 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -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 diff --git a/src/OdometryROS.h b/src/OdometryROS.h index b4f3808b..a5aa7a66 100644 --- a/src/OdometryROS.h +++ b/src/OdometryROS.h @@ -30,8 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include -#include +#include #include #include @@ -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_; diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 48733414..1f47dae4 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -33,7 +33,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include #include #include