From fa53c7c0009fd61c6c7bea7ef1489b11d471304e Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 20 Feb 2022 18:07:49 -0500 Subject: [PATCH] CoreWrapper: forward odom twist to rtabmap library --- include/rtabmap_ros/CoreWrapper.h | 2 + src/nodelets/point_cloud_assembler.cpp | 249 +++++++++---------------- 2 files changed, 88 insertions(+), 163 deletions(-) diff --git a/include/rtabmap_ros/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h index 41abfd71..4994937b 100644 --- a/include/rtabmap_ros/CoreWrapper.h +++ b/include/rtabmap_ros/CoreWrapper.h @@ -195,6 +195,7 @@ private: const ros::Time & stamp, rtabmap::SensorData & data, const rtabmap::Transform & odom = rtabmap::Transform(), + const std::vector & odomVelocity = std::vector(), const std::string & odomFrameId = "", const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1), const rtabmap::OdometryInfo & odomInfo = rtabmap::OdometryInfo(), @@ -260,6 +261,7 @@ private: bool paused_; rtabmap::Transform lastPose_; ros::Time lastPoseStamp_; + std::vector lastPoseVelocity_; bool lastPoseIntermediate_; cv::Mat covariance_; rtabmap::Transform currentMetricGoal_; diff --git a/src/nodelets/point_cloud_assembler.cpp b/src/nodelets/point_cloud_assembler.cpp index 84284247..6e91a10f 100644 --- a/src/nodelets/point_cloud_assembler.cpp +++ b/src/nodelets/point_cloud_assembler.cpp @@ -42,7 +42,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include #include #include @@ -51,7 +50,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include + +//ADC +#include namespace rtabmap_ros { @@ -77,17 +78,16 @@ public: skipClouds_(0), cloudsSkipped_(0), circularBuffer_(false), - linearUpdate_(0), - angularUpdate_(0), waitForTransformDuration_(0.1), rangeMin_(0), rangeMax_(0), voxelSize_(0), noiseRadius_(0), noiseMinNeighbors_(5), - removeZ_(false), fixedFrameId_("odom"), - frameId_("") + frameId_(""), + //ADC + use_lidar_topics_(false) {} virtual ~PointCloudAssembler() @@ -119,16 +119,20 @@ private: pnh.param("assembling_time", assemblingTime_, assemblingTime_); pnh.param("skip_clouds", skipClouds_, skipClouds_); pnh.param("circular_buffer", circularBuffer_, circularBuffer_); - pnh.param("linear_update", linearUpdate_, linearUpdate_); - pnh.param("angular_update", angularUpdate_, angularUpdate_); pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_); pnh.param("range_min", rangeMin_, rangeMin_); pnh.param("range_max", rangeMax_, rangeMax_); pnh.param("voxel_size", voxelSize_, voxelSize_); pnh.param("noise_radius", noiseRadius_, noiseRadius_); pnh.param("noise_min_neighbors", noiseMinNeighbors_, noiseMinNeighbors_); - pnh.param("remove_z", removeZ_, removeZ_); pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo); + ROS_ASSERT(maxClouds_>0 || assemblingTime_ >0.0); + //ADC + pnh.param("use_lidar_topics", use_lidar_topics_, use_lidar_topics_); + std::vector lidar_topics; + pnh.param("lidar_topics", lidar_topics, lidar_topics); + lidarSub_.resize(lidar_topics.size()); + ROS_INFO("%s: queue_size=%d", getName().c_str(), queueSize); ROS_INFO("%s: fixed_frame_id=%s", getName().c_str(), fixedFrameId_.c_str()); @@ -137,31 +141,40 @@ private: ROS_INFO("%s: assembling_time=%fs", getName().c_str(), assemblingTime_); ROS_INFO("%s: skip_clouds=%d", getName().c_str(), skipClouds_); ROS_INFO("%s: circular_buffer=%s", getName().c_str(), circularBuffer_?"true":"false"); - ROS_INFO("%s: linear_update=%f m", getName().c_str(), linearUpdate_); - ROS_INFO("%s: angular_update=%f rad", getName().c_str(), angularUpdate_); ROS_INFO("%s: wait_for_transform_duration=%f", getName().c_str(), waitForTransformDuration_); ROS_INFO("%s: range_min=%f", getName().c_str(), rangeMin_); ROS_INFO("%s: range_max=%f", getName().c_str(), rangeMax_); ROS_INFO("%s: voxel_size=%fm", getName().c_str(), voxelSize_); ROS_INFO("%s: noise_radius=%fm", getName().c_str(), noiseRadius_); ROS_INFO("%s: noise_min_neighbors=%d", getName().c_str(), noiseMinNeighbors_); - ROS_INFO("%s: remove_z=%s", getName().c_str(), removeZ_?"true":"false"); - - if(maxClouds_==0 && assemblingTime_ ==0.0) - { - ROS_ERROR("point_cloud_assembler: max_cloud or assembling_time parameters should be set!"); - exit(-1); - } + //ADC + ROS_INFO("%s: use_lidar_topics=%d", getName().c_str(), use_lidar_topics_); cloudsSkipped_ = skipClouds_; std::string subscribedTopicsMsg; if(!fixedFrameId_.empty()) { - cloudSub_ = nh.subscribe("cloud", queueSize, &PointCloudAssembler::callbackCloud, this); - subscribedTopicsMsg = uFormat("\n%s subscribed to %s", - getName().c_str(), - cloudSub_.getTopic().c_str()); + if(!use_lidar_topics_) + { + ROS_INFO("ADC: fixedFrameId != empty"); + cloudSub_ = nh.subscribe("cloud", queueSize, &PointCloudAssembler::callbackCloud, this); + subscribedTopicsMsg = uFormat("\n%s subscribed to %s", + getName().c_str(), + cloudSub_.getTopic().c_str()); + } + else + { + if (lidar_topics.size() == 0) + { + ROS_WARN("Missing lidar topics!"); + } + for (int i = 0; i < lidar_topics.size(); ++i) + { + ROS_INFO("Subscribing to lidar_topics `%s`", lidar_topics[i].c_str()); + lidarSub_[i] = nh.subscribe(lidar_topics[i], queueSize, &PointCloudAssembler::callbackLidar, this); + } + } } else if(subscribeOdomInfo) { @@ -241,50 +254,29 @@ private: } } - sensor_msgs::PointCloud2 removeField(const sensor_msgs::PointCloud2 & input, const std::string & field) + void callbackLidar(const sensor_msgs::RangeConstPtr & lidarMsg) { - sensor_msgs::PointCloud2 output; - int offset = 0; - std::vector inputFieldIndex; - for(size_t i=0; irange > 0.1) { - if(input.fields[i].name.compare(field) == 0) - { - continue; - } - else - { - sensor_msgs::PointField outputField = input.fields[i]; - outputField.offset = offset; - offset += outputField.count * sizeOfPointField(outputField.datatype); - output.fields.push_back(outputField); - inputFieldIndex.push_back(i); - } + callbackCalled_ = true; + + //transform lidar to PC + pcl::PointCloud* pcl_cloud = new pcl::PointCloud; + pcl_cloud->push_back (pcl::PointXYZ (lidarMsg->range, 0, 0)); + + //const sensor_msgs::PointCloudConstPtr pcl_ptr(new pcl::PointCloud); + //pcl::PointCloud::Ptr assembledGround_(pcl); + sensor_msgs::PointCloud2::Ptr pc2(new sensor_msgs::PointCloud2); + + pcl::toROSMsg(*pcl_cloud, *pc2); + pc2->header = lidarMsg->header; + callbackCloud(pc2); } - output.header = input.header; - output.height = input.height; - output.width = input.width; - output.is_bigendian = input.is_bigendian; - output.is_dense = input.is_dense; - output.point_step = offset; - output.row_step = output.width * output.point_step; - output.data.resize(output.height*output.row_step); - int total = output.height*output.width; - for(int i=0; i= skipClouds_) { cloudsSkipped_ = 0; - - rtabmap::Transform pose = rtabmap_ros::getTransform( + rtabmap::Transform t = rtabmap_ros::getTransform( fixedFrameId_, //fromFrame cloudMsg->header.frame_id, //toFrame cloudMsg->header.stamp, - tfListener_, + *tfListener_, waitForTransformDuration_); - if(pose.isNull()) + if(t.isNull()) { - ROS_ERROR("Cloud not transform all clouds! Resetting..."); - clouds_.clear(); + ROS_WARN("Cloud not use transform! Ignoring..."); + //clouds_.clear(); //comment out to prevent PC reset return; } - bool isMoving = true; - if(!previousPose_.isNull() && (linearUpdate_>0 || angularUpdate_>0)) - { - rtabmap::Transform delta = previousPose_.inverse()*pose; - float roll, pitch, yaw; - delta.getEulerAngles(roll, pitch, yaw); - isMoving = fabs(delta.x()) > linearUpdate_ || - fabs(delta.y()) > linearUpdate_ || - fabs(delta.z()) > linearUpdate_ || - (angularUpdate_>0.0f && ( - fabs(roll) > angularUpdate_ || - fabs(pitch) > angularUpdate_ || - fabs(yaw) > angularUpdate_)); - } - pcl::PCLPointCloud2::Ptr newCloud(new pcl::PCLPointCloud2); if(rangeMin_ > 0.0 || rangeMax_ > 0.0 || voxelSize_ > 0.0f) { @@ -336,13 +312,13 @@ private: #else pcl::uint64_t stamp = newCloud->header.stamp; #endif - newCloud = rtabmap::util3d::laserScanToPointCloud2(scan, pose); + newCloud = rtabmap::util3d::laserScanToPointCloud2(scan, t); newCloud->header.stamp = stamp; } else { sensor_msgs::PointCloud2 output; - pcl_ros::transformPointCloud(pose.toEigen4f(), *cloudMsg, output); + pcl_ros::transformPointCloud(t.toEigen4f(), *cloudMsg, output); pcl_conversions::toPCL(output, *newCloud); } @@ -392,52 +368,12 @@ private: sensor_msgs::PointCloud2 rosCloud; if(voxelSize_>0.0) { - // estimate if there would be an overflow - int x_idx=-1, y_idx=-1, z_idx=-1; - for (std::size_t d = 0; d < assembled->fields.size (); ++d) - { - if (assembled->fields[d].name.compare("x")==0) - x_idx = d; - if (assembled->fields[d].name.compare("y")==0) - y_idx = d; - if (assembled->fields[d].name.compare("z")==0) - z_idx = d; - } - bool overflow = false; - if(x_idx>=0 && y_idx>=0 && z_idx>=0) { - Eigen::Vector4f min_p, max_p; - pcl::getMinMax3D(assembled, x_idx, y_idx, z_idx, min_p, max_p); - float inverseVoxelSize = 1.0f/voxelSize_; - std::int64_t dx = static_cast((max_p[0] - min_p[0]) * inverseVoxelSize)+1; - std::int64_t dy = static_cast((max_p[1] - min_p[1]) * inverseVoxelSize)+1; - std::int64_t dz = static_cast((max_p[2] - min_p[2]) * inverseVoxelSize)+1; - - if ((dx*dy*dz) > static_cast(std::numeric_limits::max())) - { - overflow = true; - } - } - if(overflow) - { - rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(*assembled); - scan = rtabmap::util3d::commonFiltering(scan, 1, 0, 0, voxelSize_); -#if PCL_VERSION_COMPARE(>=, 1, 10, 0) - std::uint64_t stamp = assembled->header.stamp; -#else - pcl::uint64_t stamp = assembled->header.stamp; -#endif - assembled = rtabmap::util3d::laserScanToPointCloud2(scan); - assembled->header.stamp = stamp; - } - else - { - pcl::VoxelGrid filter; - filter.setLeafSize(voxelSize_, voxelSize_, voxelSize_); - filter.setInputCloud(assembled); - pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2); - filter.filter(*output); - assembled = output; - } + pcl::VoxelGrid filter; + filter.setLeafSize(voxelSize_, voxelSize_, voxelSize_); + filter.setInputCloud(assembled); + pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2); + filter.filter(*output); + assembled = output; } if(noiseRadius_>0.0 && noiseMinNeighbors_>0) { @@ -451,7 +387,6 @@ private: } pcl_conversions::moveFromPCL(*assembled, rosCloud); - rtabmap::Transform t = pose; if(!frameId_.empty()) { // transform in target frame_id instead of sensor frame @@ -459,57 +394,41 @@ private: fixedFrameId_, //fromFrame frameId_, //toFrame cloudMsg->header.stamp, - tfListener_, + *tfListener_, waitForTransformDuration_); if(t.isNull()) { - ROS_ERROR("Cloud not transform back assembled clouds in target frame \"%s\"! Resetting...", frameId_.c_str()); - clouds_.clear(); + ROS_ERROR("Cloud not transform back assembled clouds in target frame \"%s\"! Ignoring...", frameId_.c_str()); + //clouds_.clear(); // don;t clear the cloud return; } } pcl_ros::transformPointCloud(t.toEigen4f().inverse(), rosCloud, rosCloud); - if(removeZ_) - { - rosCloud = removeField(rosCloud, "z"); - } rosCloud.header = cloudMsg->header; if(!frameId_.empty()) { rosCloud.header.frame_id = frameId_; } - cloudPub_.publish(rosCloud); + ros::Duration rate_sleep = ros::Duration(0.1); + if((rosCloud.header.stamp - cloudPub_timestamp) > rate_sleep ) + { + cloudPub_timestamp = rosCloud.header.stamp; + cloudPub_.publish(rosCloud); + } if(circularBuffer_) { - if(!isMoving) + if(reachedMaxSize) { - clouds_.pop_back(); - } - else - { - previousPose_ = pose; - if(reachedMaxSize) - { - clouds_.pop_front(); - } + clouds_.pop_front(); } } else { clouds_.clear(); - previousPose_.setNull(); } } - else if(!isMoving) - { - clouds_.pop_back(); - } - else - { - previousPose_ = pose; - } } else { @@ -554,8 +473,6 @@ private: int skipClouds_; int cloudsSkipped_; bool circularBuffer_; - double linearUpdate_; - double angularUpdate_; double assemblingTime_; double waitForTransformDuration_; double rangeMin_; @@ -563,13 +480,19 @@ private: double voxelSize_; double noiseRadius_; int noiseMinNeighbors_; - bool removeZ_; std::string fixedFrameId_; std::string frameId_; - tf::TransformListener tfListener_; - rtabmap::Transform previousPose_; + //tf::TransformListener tfListener_; + ros::Duration not_time = ros::Duration(1000); + tf::TransformListener* tfListener_ = new tf::TransformListener(not_time, true); std::list clouds_; + + //ADC + bool use_lidar_topics_; + std::vector lidarSub_; + sensor_msgs::PointCloud2 LidarCloudMsg; + ros::Time cloudPub_timestamp = ros::Time(0.0); }; PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudAssembler, nodelet::Nodelet);