diff --git a/CMakeLists.txt b/CMakeLists.txt
index 75571c67..77f2970f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -209,6 +209,7 @@ SET(rtabmap_plugins_lib_src
src/nodelets/point_cloud_aggregator.cpp
src/nodelets/point_cloud_assembler.cpp
src/nodelets/undistort_depth.cpp
+ src/nodelets/imu_to_tf.cpp
)
IF(${cv_bridge_VERSION_MAJOR} GREATER 1 OR ${cv_bridge_VERSION_MINOR} GREATER 10)
@@ -365,6 +366,10 @@ add_executable(rtabmap_map_assembler src/MapAssemblerNode.cpp)
target_link_libraries(rtabmap_map_assembler rtabmap_ros)
set_target_properties(rtabmap_map_assembler PROPERTIES OUTPUT_NAME "map_assembler")
+add_executable(rtabmap_imu_to_tf src/ImuToTFNode.cpp)
+target_link_libraries(rtabmap_imu_to_tf ${Libraries})
+set_target_properties(rtabmap_imu_to_tf PROPERTIES OUTPUT_NAME "imu_to_tf")
+
add_executable(rtabmap_wifi_signal_pub src/WifiSignalPubNode.cpp)
target_link_libraries(rtabmap_wifi_signal_pub rtabmap_ros)
set_target_properties(rtabmap_wifi_signal_pub PROPERTIES OUTPUT_NAME "wifi_signal_pub")
diff --git a/launch/tests/test_ouster.launch b/launch/tests/test_ouster.launch
new file mode 100644
index 00000000..27dacd0d
--- /dev/null
+++ b/launch/tests/test_ouster.launch
@@ -0,0 +1,133 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml
index 5d3165ff..ab4a1113 100644
--- a/nodelet_plugins.xml
+++ b/nodelet_plugins.xml
@@ -152,6 +152,14 @@
+
+
+ This is my nodelet.
+
+
+
diff --git a/src/ImuToTFNode.cpp b/src/ImuToTFNode.cpp
new file mode 100644
index 00000000..4c4c3dc5
--- /dev/null
+++ b/src/ImuToTFNode.cpp
@@ -0,0 +1,48 @@
+/*
+Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+ * Neither the name of the Universite de Sherbrooke nor the
+ names of its contributors may be used to endorse or promote products
+ derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "ros/ros.h"
+#include "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "imu_to_tf");
+
+ nodelet::V_string nargv;
+ for(int i=1;i
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace rtabmap_ros
+{
+
+class ImuToTF : public nodelet::Nodelet
+{
+public:
+ ImuToTF() :
+ fixedFrameId_("odom")
+ {}
+
+ virtual ~ImuToTF()
+ {
+ }
+
+private:
+ virtual void onInit()
+ {
+ ros::NodeHandle & nh = getNodeHandle();
+ ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+ pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
+ pnh.param("base_frame_id", baseFrameId_, baseFrameId_);
+ NODELET_INFO("fixed_frame_id: %s", fixedFrameId_.c_str());
+ NODELET_INFO("base_frame_id: %s", baseFrameId_.c_str());
+
+ sub_ = nh.subscribe("imu/data", 1, &ImuToTF::imuCallback, this);
+ }
+
+ void imuCallback(const sensor_msgs::ImuConstPtr & msg)
+ {
+ tf::Quaternion q;
+ tf::quaternionMsgToTF(msg->orientation, q);
+ tf::StampedTransform st;
+ st.setRotation(q);
+
+ st.frame_id_ = fixedFrameId_;
+ st.stamp_ = msg->header.stamp;
+
+ if(!baseFrameId_.empty() &&
+ baseFrameId_.compare(msg->header.frame_id) != 0)
+ {
+ try
+ {
+ std::string errorMsg;
+ if(!tfListener_.waitForTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp, ros::Duration(0.1), ros::Duration(0.01), &errorMsg))
+ {
+ NODELET_ERROR("Could not get transform from %s to %s after %f seconds (for stamp=%f)! Error=\"%s\".",
+ baseFrameId_.c_str(), msg->header.frame_id.c_str(), 0.1, msg->header.stamp.toSec(), errorMsg.c_str());
+ return;
+ }
+
+ tf::StampedTransform tmp;
+ tfListener_.lookupTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp, tmp);
+ tmp *= st;
+ st.setRotation(tmp.getRotation());
+ st.child_frame_id_ = baseFrameId_;
+ }
+ catch(tf::TransformException & ex)
+ {
+ NODELET_ERROR("(getting transform %s -> %s) %s", baseFrameId_.c_str(), msg->header.frame_id.c_str(), ex.what());
+ return;
+ }
+ }
+ else
+ {
+ st.child_frame_id_ = msg->header.frame_id;
+ }
+ st.setOrigin(tf::Vector3(0,0,0));
+
+ pub_.sendTransform(st);
+ }
+
+private:
+ ros::Subscriber sub_;
+ tf::TransformBroadcaster pub_;
+ std::string fixedFrameId_;
+ std::string baseFrameId_;
+ tf::TransformListener tfListener_;
+};
+
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ImuToTF, nodelet::Nodelet);
+}
+