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 ros2->jazzy-devel

Browse Source
This commit is contained in:
matlabbe
2025-06-08 16:04:29 -07:00
parent 4a1c257133 945101697a
commit 6ebb8e3184
55 changed files with 1043 additions and 221 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_util/CMakeLists.txt
+4
View File
@@ -56,6 +56,10 @@ SET(Libraries
rtabmap_conversions
)
if("$ENV{ROS_DISTRO}" STRLESS "jazzy")
add_definitions(-DPRE_ROS_JAZZY)
endif()
###########
## Build ##
###########
rtabmap_util/package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>rtabmap_util</name>
<version>0.21.10</version>
<version>0.22.0</version>
<description>RTAB-Map's various useful nodes and nodelets.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
rtabmap_util/src/nodelets/imu_to_tf.cpp
+6 -5
View File
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_conversions/MsgConversion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2/LinearMath/Transform.h>
#include <tf2/utils.hpp>
namespace rtabmap_util
{
@@ -63,8 +64,7 @@ void ImuToTF::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
{
tf2::Quaternion q;
tf2::fromMsg(msg->orientation, q);
tf2::Transform st;
st.setRotation(q);
tf2::Transform st(q);
std::string childFrameId = msg->header.frame_id;
@@ -81,10 +81,12 @@ void ImuToTF::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
return;
}
geometry_msgs::msg::TransformStamped tmp = tfBuffer_->lookupTransform(msg->header.frame_id, baseFrameId_, msg->header.stamp);
geometry_msgs::msg::TransformStamped tmp = tfBuffer_->lookupTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp);
tf2::Transform tmp_t;
tf2::fromMsg(tmp.transform, tmp_t);
tf2::Transform t = tmp_t.inverse()*st*tmp_t;
tf2::Quaternion q;
q.setRPY(0.0,0.0,tf2::getYaw(tmp_t.getRotation()));
tf2::Transform t = tf2::Transform(q)*st*tmp_t.inverse(); // base_frame orientation
st.setRotation(t.getRotation());
childFrameId = baseFrameId_;
}
@@ -94,7 +96,6 @@ void ImuToTF::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
return;
}
}
st.setOrigin(tf2::Vector3(0,0,0));
geometry_msgs::msg::TransformStamped output;
output.header.frame_id = fixedFrameId_;
rtabmap_util/src/nodelets/map_assembler.cpp
+7 -1
View File
@@ -40,6 +40,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#endif
#endif
#ifdef PRE_ROS_JAZZY
namespace rclcpp{
rmw_qos_profile_t ServicesQoS() {return rmw_qos_profile_services_default;}
}
#endif
using namespace std::chrono_literals;
namespace rtabmap_util
@@ -187,7 +193,7 @@ MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) :
// We cannot call the service and wait in the constructor, lets call it later and subscribe afterwards
serviceCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
timerCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
client_ = this->create_client<rtabmap_msgs::srv::GetMap>(getMapSrv, rmw_qos_profile_services_default, serviceCbGroup_); // Put it in a different group than the timer
client_ = this->create_client<rtabmap_msgs::srv::GetMap>(getMapSrv, rclcpp::ServicesQoS(), serviceCbGroup_); // Put it in a different group than the timer
timer_ = this->create_wall_timer(1s, std::bind(&MapAssembler::timerCallback, this), timerCbGroup_);
}
rtabmap_util/src/nodelets/point_cloud_aggregator.cpp
+11 -10
View File
@@ -111,10 +111,10 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
get_name(),
approx?"approx":"exact",
approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
cloudSub_1_.getTopic().c_str(),
cloudSub_2_.getTopic().c_str(),
cloudSub_3_.getTopic().c_str(),
cloudSub_4_.getTopic().c_str());
cloudSub_1_.getSubscriber()->get_topic_name(),
cloudSub_2_.getSubscriber()->get_topic_name(),
cloudSub_3_.getSubscriber()->get_topic_name(),
cloudSub_4_.getSubscriber()->get_topic_name());
}
else if(count == 3)
{
@@ -135,9 +135,9 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
this->get_name(),
approx?"approx":"exact",
approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
cloudSub_1_.getTopic().c_str(),
cloudSub_2_.getTopic().c_str(),
cloudSub_3_.getTopic().c_str());
cloudSub_1_.getSubscriber()->get_topic_name(),
cloudSub_2_.getSubscriber()->get_topic_name(),
cloudSub_3_.getSubscriber()->get_topic_name());
}
else
{
@@ -157,8 +157,8 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
this->get_name(),
approx?"approx":"exact",
approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
cloudSub_1_.getTopic().c_str(),
cloudSub_2_.getTopic().c_str());
cloudSub_1_.getSubscriber()->get_topic_name(),
cloudSub_2_.getSubscriber()->get_topic_name());
}
@@ -175,10 +175,11 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
this->get_name(),
approx?"":"Parameter \"approx_sync\" is false, which means that input "
"topics should have all the exact timestamp for the callback to be called.",
subscribedTopicsMsg.c_str());
subscribedTopicsMsg.c_str());
}
}
});
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
}
PointCloudAggregator::~PointCloudAggregator()
rtabmap_util/src/nodelets/point_cloud_assembler.cpp
+5 -5
View File
@@ -154,9 +154,9 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
exactInfoSync_->registerCallback(std::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdomInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s",
get_name(),
syncCloudSub_.getTopic().c_str(),
syncOdomSub_.getTopic().c_str(),
syncOdomInfoSub_.getTopic().c_str());
syncCloudSub_.getSubscriber()->get_topic_name(),
syncOdomSub_.getSubscriber()->get_topic_name(),
syncOdomInfoSub_.getSubscriber()->get_topic_name());
}
else
{
@@ -166,8 +166,8 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
exactSync_->registerCallback(std::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdom, this, std::placeholders::_1, std::placeholders::_2));
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s",
get_name(),
syncCloudSub_.getTopic().c_str(),
syncOdomSub_.getTopic().c_str());
syncCloudSub_.getSubscriber()->get_topic_name(),
syncOdomSub_.getSubscriber()->get_topic_name());
}
warningThread_ = new std::thread([&](){