From 575af5c5a17bbc0a987309b5b0d927c48a5c3930 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 24 Oct 2018 08:53:44 +1200 Subject: [PATCH] Added /gps/fix input topic, data_player can also publish GPS and global poses if they are set in database --- include/rtabmap_ros/CoreWrapper.h | 4 ++ launch/rtabmap.launch | 3 ++ src/CoreWrapper.cpp | 78 +++++++++++++++++++++++++++++++ src/DbPlayerNode.cpp | 51 ++++++++++++++++++++ 4 files changed, 136 insertions(+) diff --git a/include/rtabmap_ros/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h index 52c753fb..6c6ddb0e 100644 --- a/include/rtabmap_ros/CoreWrapper.h +++ b/include/rtabmap_ros/CoreWrapper.h @@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include #include @@ -121,6 +122,7 @@ private: void userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & dataMsg); void globalPoseAsyncCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & globalPoseMsg); + void gpsFixAsyncCallback(const sensor_msgs::NavSatFixConstPtr & gpsFixMsg); void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg); @@ -282,6 +284,8 @@ private: ros::Subscriber globalPoseAsyncSub_; geometry_msgs::PoseWithCovarianceStamped globalPose_; + ros::Subscriber gpsFixAsyncSub_; + rtabmap::GPS gps_; bool stereoToDepth_; bool odomSensorSync_; diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch index b8c82951..4bfb2d51 100644 --- a/launch/rtabmap.launch +++ b/launch/rtabmap.launch @@ -98,6 +98,8 @@ + + @@ -270,6 +272,7 @@ + diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 86d48725..6d5c77b5 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -669,6 +669,7 @@ void CoreWrapper::onInit() userDataAsyncSub_ = nh.subscribe("user_data_async", 1, &CoreWrapper::userDataAsyncCallback, this); globalPoseAsyncSub_ = nh.subscribe("global_pose", 1, &CoreWrapper::globalPoseAsyncCallback, this); + gpsFixAsyncSub_ = nh.subscribe("gps/fix", 1, &CoreWrapper::gpsFixAsyncCallback, this); } CoreWrapper::~CoreWrapper() @@ -1268,6 +1269,12 @@ void CoreWrapper::commonDepthCallbackImpl( } globalPose_.header.stamp = ros::Time(0); + if(gps_.stamp() > 0.0) + { + data.setGPS(gps_); + } + gps_ = rtabmap::GPS(); + OdometryInfo odomInfo; if(odomInfoMsg.get()) { @@ -1496,6 +1503,51 @@ void CoreWrapper::commonStereoCallback( userData); data.setGroundTruth(groundTruthPose); + //global pose + if(!globalPose_.header.stamp.isZero()) + { + // assume sensor is fixed + Transform sensorToBase = rtabmap_ros::getTransform( + globalPose_.header.frame_id, + frameId_, + lastPoseStamp_, + tfListener_, + waitForTransform_?waitForTransformDuration_:0.0); + if(!sensorToBase.isNull()) + { + Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose); + globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame + + // Correction of the global pose accounting the odometry movement since we received it + Transform correction = rtabmap_ros::getTransform( + frameId_, + odomFrameId, + globalPose_.header.stamp, + lastPoseStamp_, + tfListener_, + waitForTransform_?waitForTransformDuration_:0.0); + if(!correction.isNull()) + { + globalPose *= correction; + } + else + { + NODELET_WARN("Could not adjust global pose accordingly to latest odometry pose. " + "If odometry is small since it received the global pose and " + "covariance is large, this should not be a problem."); + } + cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone(); + data.setGlobalPose(globalPose, globalPoseCovariance); + } + } + globalPose_.header.stamp = ros::Time(0); + + if(gps_.stamp() > 0.0) + { + data.setGPS(gps_); + } + gps_ = rtabmap::GPS(); + OdometryInfo odomInfo; if(odomInfoMsg.get()) { @@ -1804,6 +1856,30 @@ void CoreWrapper::globalPoseAsyncCallback(const geometry_msgs::PoseWithCovarianc } } +void CoreWrapper::gpsFixAsyncCallback(const sensor_msgs::NavSatFixConstPtr & gpsFixMsg) +{ + if(!paused_) + { + double error = 10.0; + if(gpsFixMsg->position_covariance_type != sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN) + { + double variance = uMax3(gpsFixMsg->position_covariance.at(0), gpsFixMsg->position_covariance.at(4), gpsFixMsg->position_covariance.at(8)); + if(variance>0.0) + { + error = sqrt(variance); + } + } + gps_ = rtabmap::GPS( + gpsFixMsg->header.stamp.toSec(), + gpsFixMsg->longitude, + gpsFixMsg->latitude, + gpsFixMsg->altitude, + error, + 0); + } +} + + void CoreWrapper::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg) { Transform intialPose = rtabmap_ros::transformFromPoseMsg(msg->pose.pose); @@ -2032,6 +2108,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt mapsManager_.clear(); previousStamp_ = ros::Time(0); globalPose_.header.stamp = ros::Time(0); + gps_ = rtabmap::GPS(); userDataMutex_.lock(); userData_ = cv::Mat(); userDataMutex_.unlock(); @@ -2092,6 +2169,7 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em userData_ = cv::Mat(); userDataMutex_.unlock(); globalPose_.header.stamp = ros::Time(0); + gps_ = rtabmap::GPS(); NODELET_INFO("Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str()); UFile::copy(databasePath_, databasePath_+".back"); diff --git a/src/DbPlayerNode.cpp b/src/DbPlayerNode.cpp index e8fc1c04..0d26d3f1 100644 --- a/src/DbPlayerNode.cpp +++ b/src/DbPlayerNode.cpp @@ -31,6 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include +#include #include #include #include @@ -170,6 +172,8 @@ int main(int argc, char** argv) ros::Publisher odometryPub; ros::Publisher scanPub; ros::Publisher scanCloudPub; + ros::Publisher globalPosePub; + ros::Publisher gpsFixPub; ros::Publisher clockPub; tf2_ros::TransformBroadcaster tfBroadcaster; @@ -311,6 +315,26 @@ int main(int argc, char** argv) } } + if(!odom.data().globalPose().isNull() && + odom.data().globalPoseCovariance().cols==6 && + odom.data().globalPoseCovariance().rows==6) + { + if(globalPosePub.getTopic().empty()) + { + globalPosePub = nh.advertise("global_pose", 1); + ROS_INFO("Global pose will be published."); + } + } + + if(odom.data().gps().stamp() > 0.0) + { + if(gpsFixPub.getTopic().empty()) + { + gpsFixPub = nh.advertise("gps/fix", 1); + ROS_INFO("GPS will be published."); + } + } + // publish transforms first if(publishTf) { @@ -372,6 +396,33 @@ int main(int argc, char** argv) } } + // Publish async topics first (so that they can catched by rtabmap before the image topics) + if(globalPosePub.getNumSubscribers() > 0 && + !odom.data().globalPose().isNull() && + odom.data().globalPoseCovariance().cols==6 && + odom.data().globalPoseCovariance().rows==6) + { + geometry_msgs::PoseWithCovarianceStamped msg; + rtabmap_ros::transformToPoseMsg(odom.data().globalPose(), msg.pose.pose); + memcpy(msg.pose.covariance.data(), odom.data().globalPoseCovariance().data, 36*sizeof(double)); + msg.header.frame_id = frameId; + msg.header.stamp = time; + globalPosePub.publish(msg); + } + + if(odom.data().gps().stamp() > 0.0) + { + sensor_msgs::NavSatFix msg; + msg.longitude = odom.data().gps().longitude(); + msg.latitude = odom.data().gps().latitude(); + msg.altitude = odom.data().gps().altitude(); + msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + msg.position_covariance.at(0) = msg.position_covariance.at(4) = msg.position_covariance.at(8)= odom.data().gps().error()* odom.data().gps().error(); + msg.header.frame_id = frameId; + msg.header.stamp.fromSec(odom.data().gps().stamp()); + gpsFixPub.publish(msg); + } + if(type >= 0) { if(rgbCamInfoPub.getNumSubscribers() && type == 0)