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

merged master -> ros2

Browse Source
This commit is contained in:
matlabbe
2023-03-04 17:21:00 -08:00
parent 580b0c57dc f2714c8b2a
commit c0e4927f88
10 changed files with 144 additions and 152 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h
+14 -14
View File
@@ -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_ */
rtabmap_conversions/src/MsgConversion.cpp
+108 -107
View File
@@ -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]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<","<<pt_out[1]<<","<<pt_out[2]<<"\n";
}
}
else
{
pt_out = transform * pt;
}
if (max_range_point)
{
// Save x value in distance again
*(float*)(&out.data[distance_ptr_offset]) = pt_out[0];
pt_out[0] = std::numeric_limits<float>::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]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<","<<pt_out[1]<<","<<pt_out[2]<<"\n";
}
}
else
{
pt_out = transform * pt;
}
if (max_range_point)
{
// Save x value in distance again
*(float*)(&out.data[distance_ptr_offset]) = pt_out[0];
pt_out[0] = std::numeric_limits<float>::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];
}
}
}
}
rtabmap_examples/launch/calibration/euroc_left.yaml → rtabmap_examples/launch/config/euroc_left.yaml
View File
rtabmap_examples/launch/calibration/euroc_right.yaml → rtabmap_examples/launch/config/euroc_right.yaml
View File
rtabmap_examples/launch/euroc_datasets.launch.py
+2 -2
View File
@@ -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')],
rtabmap_rviz_plugins/rviz_plugins.xml
-9
View File
@@ -23,13 +23,4 @@
</description>
<message_type>rtabmap_rviz_plugins/msg/Info</message_type>
</class>
<!--
<class name="rtabmap_rviz_plugins/OrbitOriented"
type="rtabmap_rviz_plugins::OrbitOrientedViewController"
base_class_type="rviz::ViewController">
<description>
Camera orbit with orientation.
</description>
</class>
-->
</library>
rtabmap_util/src/MapOptimizerNode.cpp
+1 -1
View File
@@ -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));
}
}
rtabmap_viz/src/GuiNode.cpp
+4 -4
View File
@@ -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;
rtabmap_viz/src/GuiWrapper.cpp
+13 -13
View File
@@ -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<rclcpp::Parameter> 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;
}
}
rtabmap_viz/src/PreferencesDialogROS.cpp
+2 -2
View File
@@ -85,7 +85,7 @@ QString PreferencesDialogROS::getParamMessage()
bool PreferencesDialogROS::hasAllParameters()
{
auto node = std::make_shared<rclcpp::Node>("rtabmapviz");
auto node = std::make_shared<rclcpp::Node>("rtabmap_viz");
auto client = std::make_shared<rclcpp::AsyncParametersClient>(node, rtabmapNodeName_);
return client->service_is_ready();
}
@@ -98,7 +98,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
path = filePath;
}
auto node = std::make_shared<rclcpp::Node>("rtabmapviz");
auto node = std::make_shared<rclcpp::Node>("rtabmap_viz");
RCLCPP_INFO(node->get_logger(), "%s", this->getParamMessage().toStdString().c_str());
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
// remove Odom parameters