diff --git a/CMakeLists.txt b/CMakeLists.txt index 4814c82..36bf399 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,13 +69,16 @@ if (CATKIN_ENABLE_TESTING) add_rostest_gtest(test_two_links_moving_joint ${CMAKE_CURRENT_SOURCE_DIR}/test/test_two_links_moving_joint.launch test/test_two_links_moving_joint.cpp) target_link_libraries(test_two_links_moving_joint ${catkin_LIBRARIES} ${PROJECT_NAME}_solver) + add_rostest_gtest(test_frames_and_slashes ${CMAKE_CURRENT_SOURCE_DIR}/test/test_frames_and_slashes.launch test/test_frames_and_slashes.cpp) + target_link_libraries(test_frames_and_slashes ${catkin_LIBRARIES}) + add_rostest_gtest(test_joint_states_bag ${CMAKE_CURRENT_SOURCE_DIR}/test/test_joint_states_bag.launch test/test_joint_states_bag.cpp) target_link_libraries(test_joint_states_bag ${catkin_LIBRARIES} ${PROJECT_NAME}_solver) add_rostest_gtest(test_subclass ${CMAKE_CURRENT_SOURCE_DIR}/test/test_subclass.launch test/test_subclass.cpp) target_link_libraries(test_subclass ${catkin_LIBRARIES} ${PROJECT_NAME}_solver joint_state_listener) - install(FILES test/one_link.urdf test/pr2.urdf test/two_links_fixed_joint.urdf test/two_links_moving_joint.urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test) + install(FILES test/one_link.urdf test/pr2.urdf test/two_links_fixed_joint.urdf test/two_links_moving_joint.urdf test/frames_and_slashes.urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test) endif() diff --git a/include/robot_state_publisher/joint_state_listener.h b/include/robot_state_publisher/joint_state_listener.h index 16dac0c..ee03951 100644 --- a/include/robot_state_publisher/joint_state_listener.h +++ b/include/robot_state_publisher/joint_state_listener.h @@ -71,6 +71,9 @@ class JointStateListener { /// Destructor ~JointStateListener(); +private: + std::string getTFPrefix(); + protected: virtual void callbackJointState(const JointStateConstPtr& state); virtual void callbackFixedJoint(const ros::TimerEvent& e); diff --git a/include/robot_state_publisher/robot_state_publisher.h b/include/robot_state_publisher/robot_state_publisher.h index ca363d2..2a7b30e 100644 --- a/include/robot_state_publisher/robot_state_publisher.h +++ b/include/robot_state_publisher/robot_state_publisher.h @@ -83,6 +83,11 @@ class RobotStatePublisher virtual void publishTransforms(const std::map& joint_positions, const ros::Time& time); virtual void publishFixedTransforms(bool use_tf_static = false); + /** Publish transforms with tf_prefix + */ + void publishTransforms(const std::map& joint_positions, const ros::Time& time, const std::string & tf_prefix); + void publishFixedTransforms(const std::string & tf_prefix, bool use_tf_static = false); + protected: virtual void addChildren(const KDL::SegmentMap::const_iterator segment); diff --git a/src/joint_state_listener.cpp b/src/joint_state_listener.cpp index 0c6f3d0..c89fede 100644 --- a/src/joint_state_listener.cpp +++ b/src/joint_state_listener.cpp @@ -90,11 +90,23 @@ JointStateListener::JointStateListener(const std::shared_ptrpublishFixedTransforms(use_tf_static_); + state_publisher_->publishFixedTransforms(getTFPrefix(), use_tf_static_); } void JointStateListener::callbackJointState(const JointStateConstPtr& state) @@ -150,7 +162,7 @@ void JointStateListener::callbackJointState(const JointStateConstPtr& state) } } - state_publisher_->publishTransforms(joint_positions, state->header.stamp); + state_publisher_->publishTransforms(joint_positions, state->header.stamp, getTFPrefix()); // store publish time in joint map for (size_t i = 0; iname.size(); ++i) { diff --git a/src/robot_state_publisher.cpp b/src/robot_state_publisher.cpp index fbedb90..4ae89f1 100644 --- a/src/robot_state_publisher.cpp +++ b/src/robot_state_publisher.cpp @@ -91,8 +91,35 @@ std::string stripSlash(const std::string & in) return in; } +// Replicate behavior of tf 1 tf_prefix +std::string prefix_frame(const std::string & tf_prefix, const std::string & frame) +{ + // If the frame begins with a slash, remove the first slash and don't add prefix at all + if (frame.size() && frame[0] == '/') { + return frame.substr(1); + } + + std::string prefixed_frame; + + // If the prefix begins with a slash, remove it + if (tf_prefix.size() && tf_prefix[0] == '/') { + prefixed_frame = tf_prefix.substr(1); + } else { + prefixed_frame = tf_prefix; + } + + // Add a slash after a non-empty prefix + if (!tf_prefix.empty()) { + prefixed_frame += '/'; + } + + prefixed_frame += frame; + + return prefixed_frame; +} + // publish moving transforms -void RobotStatePublisher::publishTransforms(const std::map& joint_positions, const ros::Time& time) +void RobotStatePublisher::publishTransforms(const std::map& joint_positions, const ros::Time& time, const std::string & tf_prefix) { ROS_DEBUG("Publishing transforms for moving joints"); std::vector tf_transforms; @@ -103,8 +130,8 @@ void RobotStatePublisher::publishTransforms(const std::map& if (seg != segments_.end()) { geometry_msgs::TransformStamped tf_transform = tf2::kdlToTransform(seg->second.segment.pose(jnt->second)); tf_transform.header.stamp = time; - tf_transform.header.frame_id = stripSlash(seg->second.root); - tf_transform.child_frame_id = stripSlash(seg->second.tip); + tf_transform.header.frame_id = prefix_frame(tf_prefix, seg->second.root); + tf_transform.child_frame_id = prefix_frame(tf_prefix, seg->second.tip); tf_transforms.push_back(tf_transform); } else { @@ -115,7 +142,7 @@ void RobotStatePublisher::publishTransforms(const std::map& } // publish fixed transforms -void RobotStatePublisher::publishFixedTransforms(bool use_tf_static) +void RobotStatePublisher::publishFixedTransforms(const std::string & tf_prefix, bool use_tf_static) { ROS_DEBUG("Publishing transforms for fixed joints"); std::vector tf_transforms; @@ -128,8 +155,8 @@ void RobotStatePublisher::publishFixedTransforms(bool use_tf_static) if (!use_tf_static) { tf_transform.header.stamp += ros::Duration(0.5); } - tf_transform.header.frame_id = stripSlash(seg->second.root); - tf_transform.child_frame_id = stripSlash(seg->second.tip); + tf_transform.header.frame_id = prefix_frame(tf_prefix, seg->second.root); + tf_transform.child_frame_id = prefix_frame(tf_prefix, seg->second.tip); tf_transforms.push_back(tf_transform); } if (use_tf_static) { @@ -140,4 +167,14 @@ void RobotStatePublisher::publishFixedTransforms(bool use_tf_static) } } +void RobotStatePublisher::publishTransforms(const std::map& joint_positions, const ros::Time& time) +{ + publishTransforms(joint_positions, time, ""); +} + +void RobotStatePublisher::publishFixedTransforms(bool use_tf_static) +{ + publishFixedTransforms("", use_tf_static); +} + } diff --git a/test/frames_and_slashes.urdf b/test/frames_and_slashes.urdf new file mode 100644 index 0000000..9e9ea9a --- /dev/null +++ b/test/frames_and_slashes.urdf @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/test_frames_and_slashes.cpp b/test/test_frames_and_slashes.cpp new file mode 100644 index 0000000..fb49398 --- /dev/null +++ b/test/test_frames_and_slashes.cpp @@ -0,0 +1,135 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2021, Open Source Robotics Foundation, Inc. +* 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 Willow Garage 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 OWNER 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 +#include + +#include +#include +#include +#include + +#define EPS 0.01 + +class TestFramesAndSlashes : public testing::Test +{ +protected: + TestFramesAndSlashes() + { + tf_static_sub_ = n_.subscribe( + "/tf_static", 1, &TestFramesAndSlashes::tf_static_callback, this); + } + + ~TestFramesAndSlashes() = default; + + void + tf_static_callback(const tf2_msgs::TFMessage::ConstPtr msg) + { + transforms_received_ = *msg; + // avoid writing another message while test is reading the one received + tf_static_sub_.shutdown(); + got_transforms_ = true; + } + + ros::NodeHandle n_; + ros::Subscriber tf_sub_; + ros::Subscriber tf_static_sub_; + tf2_msgs::TFMessage transforms_received_; + bool got_transforms_ = false; +}; + +TEST_F(TestFramesAndSlashes, test) +{ + std::vector expected_links(13); + + { + ros::NodeHandle n_tilde; + std::string robot_description; + ASSERT_TRUE(n_tilde.getParam("robot_description", robot_description)); + ASSERT_TRUE(n_tilde.getParam("expected_name_link_1", expected_links[0])); + ASSERT_TRUE(n_tilde.getParam("expected_name_link_2", expected_links[1])); + ASSERT_TRUE(n_tilde.getParam("expected_name_link_3", expected_links[2])); + ASSERT_TRUE(n_tilde.getParam("expected_name_link_4", expected_links[3])); + ASSERT_TRUE(n_tilde.getParam("expected_name_link_5", expected_links[4])); + ASSERT_TRUE(n_tilde.getParam("expected_name_link_6", expected_links[5])); + ASSERT_TRUE(n_tilde.getParam("expected_name_link_7", expected_links[6])); + ASSERT_TRUE(n_tilde.getParam("expected_name_link_8", expected_links[7])); + ASSERT_TRUE(n_tilde.getParam("expected_name_link_9", expected_links[8])); + ASSERT_TRUE(n_tilde.getParam("expected_name_link_10", expected_links[9])); + ASSERT_TRUE(n_tilde.getParam("expected_name_link_11", expected_links[10])); + ASSERT_TRUE(n_tilde.getParam("expected_name_link_12", expected_links[11])); + ASSERT_TRUE(n_tilde.getParam("expected_name_link_13", expected_links[12])); + } + + for (int i = 0; i < 100 && !got_transforms_; ++i) { + ros::Duration(0.1).sleep(); + ros::spinOnce(); + } + ASSERT_TRUE(got_transforms_); + + for (size_t n = 1; n < expected_links.size(); ++n) { + const std::string & link_1 = expected_links.at(0); + const std::string & link_n = expected_links.at(n); + + bool found_transform_ = false; + geometry_msgs::Transform transform; + for (const geometry_msgs::TransformStamped & tf_stamped : transforms_received_.transforms) { + if (tf_stamped.header.frame_id == link_1 && + tf_stamped.child_frame_id == link_n) + { + transform = tf_stamped.transform; + found_transform_ = true; + } + } + + EXPECT_TRUE(found_transform_) << "expected " << link_1 << " and " << link_n; + if (found_transform_) { + EXPECT_NEAR(transform.translation.x, n + 1, EPS) << "frames " << link_1 << " and " << link_n; + } + } + + // Print transforms received for easier debugging + EXPECT_FALSE(testing::Test::HasFailure()) << transforms_received_; +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_frames_and_slashes"); + + int res = RUN_ALL_TESTS(); + + return res; +} diff --git a/test/test_frames_and_slashes.launch b/test/test_frames_and_slashes.launch new file mode 100644 index 0000000..f24aac5 --- /dev/null +++ b/test/test_frames_and_slashes.launch @@ -0,0 +1,197 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +