From 627b6e78bd5c90079487118526254e35403e983a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 14 Mar 2019 14:22:26 -0400 Subject: [PATCH] Added imu_to_tf node, added test_ouster.launch --- CMakeLists.txt | 5 ++ launch/tests/test_ouster.launch | 133 ++++++++++++++++++++++++++++++++ nodelet_plugins.xml | 8 ++ src/ImuToTFNode.cpp | 48 ++++++++++++ src/nodelets/imu_to_tf.cpp | 119 ++++++++++++++++++++++++++++ 5 files changed, 313 insertions(+) create mode 100644 launch/tests/test_ouster.launch create mode 100644 src/ImuToTFNode.cpp create mode 100644 src/nodelets/imu_to_tf.cpp 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); +} +