diff --git a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h index 95c1950f..170d9399 100644 --- a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h +++ b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h @@ -266,6 +266,20 @@ bool convertScan3dMsg( float maxRange = 0.0f, bool is2D = false); +bool deskew( + const sensor_msgs::msg::PointCloud2 & input, + sensor_msgs::msg::PointCloud2 & output, + const std::string & fixedFrameId, + tf2_ros::Buffer & tfBuffer, + double waitForTransform, + bool slerp = false); + +bool deskew( + const sensor_msgs::msg::PointCloud2 & input, + sensor_msgs::msg::PointCloud2 & output, + double previousStamp, + const rtabmap::Transform & velocity); + // Missing function in ros2 (from old pcl_ros) void transformPointCloud ( const Eigen::Matrix4f &transform, @@ -295,20 +309,6 @@ inline int sizeOfPointField(int datatype) } return -1; } - -bool deskew( - const sensor_msgs::msg::PointCloud2 & input, - sensor_msgs::msg::PointCloud2 & output, - const std::string & fixedFrameId, - tf2_ros::Buffer & tfBuffer, - double waitForTransform, - bool slerp = false); - -bool deskew( - const sensor_msgs::msg::PointCloud2 & input, - sensor_msgs::msg::PointCloud2 & output, - double previousStamp, - const rtabmap::Transform & velocity); } #endif /* MSGCONVERSION_H_ */ diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 1eb28b77..e19b7ed1 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -2535,113 +2535,6 @@ bool convertScan3dMsg( return true; } -void -transformPointCloud ( - const Eigen::Matrix4f &transform, - const sensor_msgs::msg::PointCloud2 &in, - sensor_msgs::msg::PointCloud2 &out) -{ - // Get X-Y-Z indices - int x_idx = pcl::getFieldIndex (in, "x"); - int y_idx = pcl::getFieldIndex (in, "y"); - int z_idx = pcl::getFieldIndex (in, "z"); - - if (x_idx == -1 || y_idx == -1 || z_idx == -1) - { - UERROR ("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format."); - return; - } - - if (in.fields[x_idx].datatype != sensor_msgs::msg::PointField::FLOAT32 || - in.fields[y_idx].datatype != sensor_msgs::msg::PointField::FLOAT32 || - in.fields[z_idx].datatype != sensor_msgs::msg::PointField::FLOAT32) - { - UERROR ("X-Y-Z coordinates not floats. Currently only floats are supported."); - return; - } - - // Check if distance is available - int dist_idx = pcl::getFieldIndex (in, "distance"); - - // Copy the other data - if (&in != &out) - { - out.header = in.header; - out.height = in.height; - out.width = in.width; - out.fields = in.fields; - out.is_bigendian = in.is_bigendian; - out.point_step = in.point_step; - out.row_step = in.row_step; - out.is_dense = in.is_dense; - out.data.resize (in.data.size ()); - // Copy everything as it's faster than copying individual elements - memcpy (&out.data[0], &in.data[0], in.data.size ()); - } - - Eigen::Array4i xyz_offset (in.fields[x_idx].offset, in.fields[y_idx].offset, in.fields[z_idx].offset, 0); - - for (size_t i = 0; i < in.width * in.height; ++i) - { - Eigen::Vector4f pt (*(float*)&in.data[xyz_offset[0]], *(float*)&in.data[xyz_offset[1]], *(float*)&in.data[xyz_offset[2]], 1); - Eigen::Vector4f pt_out; - - bool max_range_point = false; - int distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset; - float* distance_ptr = (dist_idx < 0 ? NULL : (float*)(&in.data[distance_ptr_offset])); - if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2])) - { - if (distance_ptr==NULL || !std::isfinite(*distance_ptr)) // Invalid point - { - pt_out = pt; - } - else // max range point - { - pt[0] = *distance_ptr; // Replace x with the x value saved in distance - pt_out = transform * pt; - max_range_point = true; - //std::cout << pt[0]<<","< "<::quiet_NaN(); - } - - memcpy (&out.data[xyz_offset[0]], &pt_out[0], sizeof (float)); - memcpy (&out.data[xyz_offset[1]], &pt_out[1], sizeof (float)); - memcpy (&out.data[xyz_offset[2]], &pt_out[2], sizeof (float)); - - - xyz_offset += in.point_step; - } - - // Check if the viewpoint information is present - int vp_idx = pcl::getFieldIndex (in, "vp_x"); - if (vp_idx != -1) - { - // Transform the viewpoint info too - for (size_t i = 0; i < out.width * out.height; ++i) - { - float *pstep = (float*)&out.data[i * out.point_step + out.fields[vp_idx].offset]; - // Assume vp_x, vp_y, vp_z are consecutive - Eigen::Vector4f vp_in (pstep[0], pstep[1], pstep[2], 1); - Eigen::Vector4f vp_out = transform * vp_in; - - pstep[0] = vp_out[0]; - pstep[1] = vp_out[1]; - pstep[2] = vp_out[2]; - } - } -} - bool deskew_impl( const sensor_msgs::msg::PointCloud2 & input, sensor_msgs::msg::PointCloud2 & output, @@ -3055,4 +2948,112 @@ bool deskew( return deskew_impl(input, output, "", 0, 0, true, velocity, previousStamp); } + +void +transformPointCloud ( + const Eigen::Matrix4f &transform, + const sensor_msgs::msg::PointCloud2 &in, + sensor_msgs::msg::PointCloud2 &out) +{ + // Get X-Y-Z indices + int x_idx = pcl::getFieldIndex (in, "x"); + int y_idx = pcl::getFieldIndex (in, "y"); + int z_idx = pcl::getFieldIndex (in, "z"); + + if (x_idx == -1 || y_idx == -1 || z_idx == -1) + { + UERROR ("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format."); + return; + } + + if (in.fields[x_idx].datatype != sensor_msgs::msg::PointField::FLOAT32 || + in.fields[y_idx].datatype != sensor_msgs::msg::PointField::FLOAT32 || + in.fields[z_idx].datatype != sensor_msgs::msg::PointField::FLOAT32) + { + UERROR ("X-Y-Z coordinates not floats. Currently only floats are supported."); + return; + } + + // Check if distance is available + int dist_idx = pcl::getFieldIndex (in, "distance"); + + // Copy the other data + if (&in != &out) + { + out.header = in.header; + out.height = in.height; + out.width = in.width; + out.fields = in.fields; + out.is_bigendian = in.is_bigendian; + out.point_step = in.point_step; + out.row_step = in.row_step; + out.is_dense = in.is_dense; + out.data.resize (in.data.size ()); + // Copy everything as it's faster than copying individual elements + memcpy (&out.data[0], &in.data[0], in.data.size ()); + } + + Eigen::Array4i xyz_offset (in.fields[x_idx].offset, in.fields[y_idx].offset, in.fields[z_idx].offset, 0); + + for (size_t i = 0; i < in.width * in.height; ++i) + { + Eigen::Vector4f pt (*(float*)&in.data[xyz_offset[0]], *(float*)&in.data[xyz_offset[1]], *(float*)&in.data[xyz_offset[2]], 1); + Eigen::Vector4f pt_out; + + bool max_range_point = false; + int distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset; + float* distance_ptr = (dist_idx < 0 ? NULL : (float*)(&in.data[distance_ptr_offset])); + if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2])) + { + if (distance_ptr==NULL || !std::isfinite(*distance_ptr)) // Invalid point + { + pt_out = pt; + } + else // max range point + { + pt[0] = *distance_ptr; // Replace x with the x value saved in distance + pt_out = transform * pt; + max_range_point = true; + //std::cout << pt[0]<<","< "<::quiet_NaN(); + } + + memcpy (&out.data[xyz_offset[0]], &pt_out[0], sizeof (float)); + memcpy (&out.data[xyz_offset[1]], &pt_out[1], sizeof (float)); + memcpy (&out.data[xyz_offset[2]], &pt_out[2], sizeof (float)); + + + xyz_offset += in.point_step; + } + + // Check if the viewpoint information is present + int vp_idx = pcl::getFieldIndex (in, "vp_x"); + if (vp_idx != -1) + { + // Transform the viewpoint info too + for (size_t i = 0; i < out.width * out.height; ++i) + { + float *pstep = (float*)&out.data[i * out.point_step + out.fields[vp_idx].offset]; + // Assume vp_x, vp_y, vp_z are consecutive + Eigen::Vector4f vp_in (pstep[0], pstep[1], pstep[2], 1); + Eigen::Vector4f vp_out = transform * vp_in; + + pstep[0] = vp_out[0]; + pstep[1] = vp_out[1]; + pstep[2] = vp_out[2]; + } + } +} + } diff --git a/rtabmap_examples/launch/calibration/euroc_left.yaml b/rtabmap_examples/launch/config/euroc_left.yaml similarity index 100% rename from rtabmap_examples/launch/calibration/euroc_left.yaml rename to rtabmap_examples/launch/config/euroc_left.yaml diff --git a/rtabmap_examples/launch/calibration/euroc_right.yaml b/rtabmap_examples/launch/config/euroc_right.yaml similarity index 100% rename from rtabmap_examples/launch/calibration/euroc_right.yaml rename to rtabmap_examples/launch/config/euroc_right.yaml diff --git a/rtabmap_examples/launch/euroc_datasets.launch.py b/rtabmap_examples/launch/euroc_datasets.launch.py index cc02338b..01112ed9 100644 --- a/rtabmap_examples/launch/euroc_datasets.launch.py +++ b/rtabmap_examples/launch/euroc_datasets.launch.py @@ -88,7 +88,7 @@ def generate_launch_description(): # Image rectification and publishing synchronized camera_info Node( package='rtabmap_util', executable='yaml_to_camera_info.py', output='screen', - parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/launch/calibration/euroc_left.yaml']}], + parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/launch/config/euroc_left.yaml']}], remappings=[ ('image', '/cam0/image_raw'), ('camera_info', 'left/camera_info')], @@ -96,7 +96,7 @@ def generate_launch_description(): Node( package='rtabmap_util', executable='yaml_to_camera_info.py', output='screen', - parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/launch/calibration/euroc_right.yaml']}], + parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/launch/config/euroc_right.yaml']}], remappings=[ ('image', '/cam1/image_raw'), ('camera_info', 'right/camera_info')], diff --git a/rtabmap_rviz_plugins/rviz_plugins.xml b/rtabmap_rviz_plugins/rviz_plugins.xml index fa784bb8..699c38e0 100644 --- a/rtabmap_rviz_plugins/rviz_plugins.xml +++ b/rtabmap_rviz_plugins/rviz_plugins.xml @@ -23,13 +23,4 @@ rtabmap_rviz_plugins/msg/Info - diff --git a/rtabmap_util/src/MapOptimizerNode.cpp b/rtabmap_util/src/MapOptimizerNode.cpp index 957dfd32..16919057 100644 --- a/rtabmap_util/src/MapOptimizerNode.cpp +++ b/rtabmap_util/src/MapOptimizerNode.cpp @@ -103,7 +103,7 @@ public: ROS_INFO("map_optimizer: map_frame_id = %s", mapFrameId_.c_str()); ROS_INFO("map_optimizer: odom_frame_id = %s", odomFrameId_.c_str()); ROS_INFO("map_optimizer: tf_delay = %f", tfDelay); - transformThread_ = new boost::thread(std::bind(&MapOptimizer::publishLoop, this, tfDelay)); + transformThread_ = new boost::thread(boost::bind(&MapOptimizer::publishLoop, this, tfDelay)); } } diff --git a/rtabmap_viz/src/GuiNode.cpp b/rtabmap_viz/src/GuiNode.cpp index a50c601d..c01125a3 100644 --- a/rtabmap_viz/src/GuiNode.cpp +++ b/rtabmap_viz/src/GuiNode.cpp @@ -36,7 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. QApplication * app = 0; void my_handler(int){ - UINFO("rtabmapviz: ctrl-c catched! Exiting Qt app..."); + UINFO("rtabmap_viz: ctrl-c catched! Exiting Qt app..."); app->exit(-1); } @@ -82,15 +82,15 @@ int main(int argc, char** argv) // Launch executer std::thread execution_thread(spin_executor); - RCLCPP_INFO(node->get_logger(), "rtabmapviz started."); + RCLCPP_INFO(node->get_logger(), "rtabmap_viz started."); // Now wait for application to finish r = app->exec();// MUST be called by the Main Thread - RCLCPP_INFO(node->get_logger(), "rtabmapviz stopping spinner..."); + RCLCPP_INFO(node->get_logger(), "rtabmap_viz stopping spinner..."); rclcpp::shutdown(); execution_thread.join(); - RCLCPP_INFO(node->get_logger(), "rtabmapviz: All done! Closing..."); + RCLCPP_INFO(node->get_logger(), "rtabmap_viz: All done! Closing..."); } delete app; diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp index 9c38913b..f820c61f 100644 --- a/rtabmap_viz/src/GuiWrapper.cpp +++ b/rtabmap_viz/src/GuiWrapper.cpp @@ -99,7 +99,7 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) : rtabmapNodeName_ = this->declare_parameter("rtabmap", rtabmapNodeName_); - RCLCPP_INFO(this->get_logger(), "rtabmapviz: Using configuration from \"%s\"", configFile.toStdString().c_str()); + RCLCPP_INFO(this->get_logger(), "rtabmap_viz: Using configuration from \"%s\"", configFile.toStdString().c_str()); uSleep(500); prefDialog_ = new PreferencesDialogROS(this, configFile, rtabmapNodeName_); mainWindow_ = new MainWindow(prefDialog_); @@ -126,12 +126,12 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) : { initCachePath = UDirectory::currentDir(true) + initCachePath; } - RCLCPP_INFO(this->get_logger(), "rtabmapviz: Initializing cache with local database \"%s\"", initCachePath.c_str()); + RCLCPP_INFO(this->get_logger(), "rtabmap_viz: Initializing cache with local database \"%s\"", initCachePath.c_str()); if(!callMapDataService("get_map_data", false, true, true)) { RCLCPP_ERROR(this->get_logger(), "The cache will still be loaded " - "but the clouds won't be created until next time rtabmapviz " + "but the clouds won't be created until next time rtabmap_viz " "receives the optimized graph."); } QMetaObject::invokeMethod(mainWindow_, "updateCacheFromDatabase", Q_ARG(QString, QString(initCachePath.c_str()))); @@ -174,7 +174,7 @@ void GuiWrapper::infoMapCallback( const rtabmap_msgs::msg::Info::ConstSharedPtr infoMsg, const rtabmap_msgs::msg::MapData::ConstSharedPtr mapMsg) { - //RCLCPP_INFO(this->get_logger(), "rtabmapviz: RTAB-Map info ex received!"); + //RCLCPP_INFO(this->get_logger(), "rtabmap_viz: RTAB-Map info ex received!"); // Map from ROS struct to rtabmap struct rtabmap::Statistics stat; @@ -290,7 +290,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters(); rtabmap::ParametersMap parameters = ((rtabmap::ParamEvent *)anEvent)->getParameters(); std::vector rosParameters; - auto node = rclcpp::Node::make_shared("rtabmapviz"); + auto node = rclcpp::Node::make_shared("rtabmap_viz"); for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i) { //save only parameters with valid names @@ -612,7 +612,7 @@ void GuiWrapper::commonMultiCameraCallback( waitForTransform_, imagesAlreadyRectified)) { - RCLCPP_ERROR(this->get_logger(), "Could not convert rgb/depth msgs! Aborting rtabmapviz update..."); + RCLCPP_ERROR(this->get_logger(), "Could not convert rgb/depth msgs! Aborting rtabmap_viz update..."); return; } } @@ -628,7 +628,7 @@ void GuiWrapper::commonMultiCameraCallback( *tfBuffer_, waitForTransform_)) { - RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmapviz update..."); + RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmap_viz update..."); return; } } @@ -643,7 +643,7 @@ void GuiWrapper::commonMultiCameraCallback( *tfBuffer_, waitForTransform_)) { - RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmapviz update..."); + RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmap_viz update..."); return; } } @@ -801,7 +801,7 @@ void GuiWrapper::commonStereoCallback( waitForTransform_, imagesAlreadyRectified)) { - RCLCPP_ERROR(this->get_logger(), "Could not convert stereo msgs! Aborting rtabmapviz update..."); + RCLCPP_ERROR(this->get_logger(), "Could not convert stereo msgs! Aborting rtabmap_viz update..."); return; } @@ -816,7 +816,7 @@ void GuiWrapper::commonStereoCallback( *tfBuffer_, waitForTransform_)) { - RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmapviz update..."); + RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmap_viz update..."); return; } } @@ -831,7 +831,7 @@ void GuiWrapper::commonStereoCallback( *tfBuffer_, waitForTransform_)) { - RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmapviz update..."); + RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmap_viz update..."); return; } } @@ -963,7 +963,7 @@ void GuiWrapper::commonLaserScanCallback( *tfBuffer_, waitForTransform_)) { - RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmapviz update..."); + RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmap_viz update..."); return; } } @@ -978,7 +978,7 @@ void GuiWrapper::commonLaserScanCallback( *tfBuffer_, waitForTransform_)) { - RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmapviz update..."); + RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmap_viz update..."); return; } } diff --git a/rtabmap_viz/src/PreferencesDialogROS.cpp b/rtabmap_viz/src/PreferencesDialogROS.cpp index 915aedab..d3e64897 100644 --- a/rtabmap_viz/src/PreferencesDialogROS.cpp +++ b/rtabmap_viz/src/PreferencesDialogROS.cpp @@ -85,7 +85,7 @@ QString PreferencesDialogROS::getParamMessage() bool PreferencesDialogROS::hasAllParameters() { - auto node = std::make_shared("rtabmapviz"); + auto node = std::make_shared("rtabmap_viz"); auto client = std::make_shared(node, rtabmapNodeName_); return client->service_is_ready(); } @@ -98,7 +98,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath) path = filePath; } - auto node = std::make_shared("rtabmapviz"); + auto node = std::make_shared("rtabmap_viz"); RCLCPP_INFO(node->get_logger(), "%s", this->getParamMessage().toStdString().c_str()); rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); // remove Odom parameters