Skip to content

Commit bd35056

Browse files
author
tsl
committed
Added frame publisher that allows fixing rotations in transform from one frame to another
1 parent ee2b303 commit bd35056

File tree

4 files changed

+265
-0
lines changed

4 files changed

+265
-0
lines changed

‎CMakeLists.txt‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,12 @@ target_link_libraries(frame_publisher ${catkin_LIBRARIES})
3131
add_executable(frame_publisher_node src/frame_publisher_node.cpp)
3232
target_link_libraries(frame_publisher_node frame_publisher ${catkin_LIBRARIES})
3333

34+
add_library(fixable_rotation_frame_publisher src/fixable_rotation_frame_publisher.cpp)
35+
target_link_libraries(fixable_rotation_frame_publisher ${catkin_LIBRARIES})
36+
37+
add_executable(fixable_rotation_frame_publisher_node src/fixable_rotation_frame_publisher_node.cpp)
38+
target_link_libraries(fixable_rotation_frame_publisher_node fixable_rotation_frame_publisher ${catkin_LIBRARIES})
39+
3440
add_library(roi_outlier_removal src/roi_outlier_removal_nodelet.cpp)
3541
target_link_libraries(roi_outlier_removal ${catkin_LIBRARIES})
3642

‎include/pointcloud_to_laserscan/fixable_rotation_frame_publisher.h‎

Lines changed: 81 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
/*
2+
* Software License Agreement (BSD License)
3+
*
4+
* Copyright (c) 2016, Fraunhofer IPA.
5+
* All rights reserved.
6+
*
7+
* Redistribution and use in source and binary forms, with or without
8+
* modification, are permitted provided that the following conditions
9+
* are met:
10+
*
11+
* * Redistributions of source code must retain the above copyright
12+
* notice, this list of conditions and the following disclaimer.
13+
* * Redistributions in binary form must reproduce the above
14+
* copyright notice, this list of conditions and the following
15+
* disclaimer in the documentation and/or other materials provided
16+
* with the distribution.
17+
* * Neither the name of the copyright holder nor the names of its
18+
* contributors may be used to endorse or promote products derived
19+
* from this software without specific prior written permission.
20+
*
21+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32+
* POSSIBILITY OF SUCH DAMAGE.
33+
*
34+
*
35+
*/
36+
37+
/*
38+
* Basic implementation copied from FramePublisher.
39+
*
40+
* Author: Thomas Lindemeier
41+
*/
42+
43+
#ifndef FIXABLE_ROTATION_FRAME_PUBLISHER_H
44+
#define FIXABLE_ROTATION_FRAME_PUBLISHER_H
45+
46+
#include <ros/ros.h>
47+
48+
#include <tf2_ros/buffer.h>
49+
#include <tf2_ros/transform_listener.h>
50+
#include <tf2_ros/transform_broadcaster.h>
51+
#include <tf2/LinearMath/Transform.h>
52+
#include <tf2/transform_datatypes.h>
53+
#include <tf2/convert.h>
54+
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
55+
56+
class FixableRotationFramePublisher
57+
{
58+
public:
59+
FixableRotationFramePublisher();
60+
~FixableRotationFramePublisher();
61+
bool initialize();
62+
63+
private:
64+
void frameBroadcastCallback(const ros::TimerEvent& event);
65+
66+
private:
67+
ros::NodeHandle nh_, priv_nh_;
68+
69+
tf2_ros::Buffer tf_buffer_;
70+
tf2_ros::TransformListener tf_listener_;
71+
tf2_ros::TransformBroadcaster tf_broadcaster_;
72+
73+
ros::Timer frame_broadcast_timer_;
74+
double update_rate_;
75+
std::string reference_frame_;
76+
std::string desired_frame_;
77+
std::string published_frame_;
78+
bool rot_z_, rot_x_, rot_y_;
79+
};
80+
81+
#endif // FIXABLE_ROTATION_FRAME_PUBLISHER_H

‎src/fixable_rotation_frame_publisher.cpp‎

Lines changed: 120 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,120 @@
1+
2+
/*
3+
* Software License Agreement (BSD License)
4+
*
5+
* Copyright (c) 2016, Fraunhofer IPA.
6+
* All rights reserved.
7+
*
8+
* Redistribution and use in source and binary forms, with or without
9+
* modification, are permitted provided that the following conditions
10+
* are met:
11+
*
12+
* * Redistributions of source code must retain the above copyright
13+
* notice, this list of conditions and the following disclaimer.
14+
* * Redistributions in binary form must reproduce the above
15+
* copyright notice, this list of conditions and the following
16+
* disclaimer in the documentation and/or other materials provided
17+
* with the distribution.
18+
* * Neither the name of the copyright holder nor the names of its
19+
* contributors may be used to endorse or promote products derived
20+
* from this software without specific prior written permission.
21+
*
22+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33+
* POSSIBILITY OF SUCH DAMAGE.
34+
*
35+
*
36+
*/
37+
38+
/*
39+
* Author: Thomas Lindemeier
40+
*/
41+
42+
#include <pointcloud_to_laserscan/fixable_rotation_frame_publisher.h>
43+
44+
FixableRotationFramePublisher::FixableRotationFramePublisher() : tf_listener_(tf_buffer_)
45+
{
46+
}
47+
48+
FixableRotationFramePublisher::~FixableRotationFramePublisher()
49+
{
50+
}
51+
52+
bool FixableRotationFramePublisher::initialize()
53+
{
54+
priv_nh_ = ros::NodeHandle("~");
55+
56+
priv_nh_.param<double>("update_rate", update_rate_, 0.01); // 100Hz
57+
58+
priv_nh_.param<std::string>("reference_frame", reference_frame_, "base_link");
59+
priv_nh_.param<std::string>("desired_frame", desired_frame_, "torso_center_link");
60+
priv_nh_.param<std::string>("published_frame", published_frame_, "torso_rotated_base_link");
61+
62+
priv_nh_.param<bool>("rot_z", rot_z_, false);
63+
priv_nh_.param<bool>("rot_x", rot_x_, false);
64+
priv_nh_.param<bool>("rot_y", rot_y_, false);
65+
66+
frame_broadcast_timer_ = nh_.createTimer(ros::Duration(update_rate_), &FixableRotationFramePublisher::frameBroadcastCallback, this);
67+
68+
ros::Duration(1.0).sleep(); // give tf_listener some time
69+
70+
return true;
71+
}
72+
73+
/// Broadcast a frame aligned with the base frame but rotated around specified axes as rotation_frame
74+
void FixableRotationFramePublisher::frameBroadcastCallback(const ros::TimerEvent& event)
75+
{
76+
geometry_msgs::TransformStamped transform_msg;
77+
try
78+
{
79+
transform_msg = tf_buffer_.lookupTransform(reference_frame_, desired_frame_, ros::Time(0), ros::Duration(0.1));
80+
}
81+
catch (tf2::TransformException& ex)
82+
{
83+
ROS_ERROR("FixableRotationFramePublisher::frameBroadcastCallback: \n%s", ex.what());
84+
return;
85+
}
86+
87+
tf2::Stamped<tf2::Transform> transform_tf;
88+
tf2::fromMsg(transform_msg, transform_tf);
89+
double rot_frame_roll, rot_frame_pitch, rot_frame_yaw;
90+
transform_tf.getBasis().getRPY(rot_frame_roll, rot_frame_pitch, rot_frame_yaw);
91+
92+
// Use rotations according to settings
93+
double published_frame_roll = 0;
94+
double published_frame_pitch = 0;
95+
double published_frame_yaw = 0;
96+
if (rot_z_)
97+
{
98+
published_frame_yaw = rot_frame_yaw;
99+
}
100+
if (rot_x_)
101+
{
102+
published_frame_roll = rot_frame_roll;
103+
}
104+
if (rot_y_)
105+
{
106+
published_frame_pitch = rot_frame_pitch;
107+
}
108+
109+
tf2::Stamped<tf2::Transform> target_tf(transform_tf); // keep header and translation
110+
// overwrite rotation
111+
target_tf.getBasis().setRPY(published_frame_roll, published_frame_pitch, published_frame_yaw);
112+
113+
// Broadcast new frame
114+
geometry_msgs::TransformStamped target_msg = tf2::toMsg(target_tf);
115+
// set frame ids
116+
target_msg.child_frame_id = published_frame_;
117+
target_msg.header.frame_id = reference_frame_;
118+
119+
tf_broadcaster_.sendTransform(target_msg);
120+
}

‎src/fixable_rotation_frame_publisher_node.cpp‎

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
2+
/*
3+
* Software License Agreement (BSD License)
4+
*
5+
* Copyright (c) 2016, Fraunhofer IPA.
6+
* All rights reserved.
7+
*
8+
* Redistribution and use in source and binary forms, with or without
9+
* modification, are permitted provided that the following conditions
10+
* are met:
11+
*
12+
* * Redistributions of source code must retain the above copyright
13+
* notice, this list of conditions and the following disclaimer.
14+
* * Redistributions in binary form must reproduce the above
15+
* copyright notice, this list of conditions and the following
16+
* disclaimer in the documentation and/or other materials provided
17+
* with the distribution.
18+
* * Neither the name of the copyright holder nor the names of its
19+
* contributors may be used to endorse or promote products derived
20+
* from this software without specific prior written permission.
21+
*
22+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33+
* POSSIBILITY OF SUCH DAMAGE.
34+
*
35+
*
36+
*/
37+
38+
/*
39+
* Author: Thomas Lindemeier
40+
*/
41+
42+
#include <ros/ros.h>
43+
#include <pointcloud_to_laserscan/fixable_rotation_frame_publisher.h>
44+
45+
int main(int argc, char **argv)
46+
{
47+
ros::init(argc, argv, "fixable_rotation_frame_publisher_node");
48+
FixableRotationFramePublisher ad;
49+
50+
if (!ad.initialize())
51+
{
52+
ROS_ERROR("Failed to initialize FixableRotationFramePublisher");
53+
return -1;
54+
}
55+
56+
ros::spin();
57+
return 0;
58+
}

0 commit comments

Comments
 (0)