diff --git a/geometry_tutorials/package.xml b/geometry_tutorials/package.xml index f69c281..174449a 100644 --- a/geometry_tutorials/package.xml +++ b/geometry_tutorials/package.xml @@ -15,6 +15,7 @@ catkin turtle_tf + turtle_tf2 diff --git a/turtle_tf2/CMakeLists.txt b/turtle_tf2/CMakeLists.txt new file mode 100644 index 0000000..d0b8b9d --- /dev/null +++ b/turtle_tf2/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 2.8.3) +project(turtle_tf2) +## Find dependencies +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + roscpp + rospy + std_msgs + tf2 + tf2_ros + turtlesim +) + +################################### +## catkin specific configuration ## +################################### +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES turtle_tf2 + CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs tf2 tf2_ros +# DEPENDS system_lib +) + +########### +## Build ## +########### + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## C++ examples +add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp) +target_link_libraries(turtle_tf2_broadcaster ${catkin_LIBRARIES}) +if(catkin_EXPORTED_TARGETS) + add_dependencies(turtle_tf2_broadcaster ${catkin_EXPORTED_TARGETS}) +endif() + +add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp) +target_link_libraries(turtle_tf2_listener ${catkin_LIBRARIES}) +if(catkin_EXPORTED_TARGETS) + add_dependencies(turtle_tf2_listener ${catkin_EXPORTED_TARGETS}) +endif() + +## Install Python Examples +install(PROGRAMS + nodes/dynamic_tf2_broadcaster.py + nodes/fixed_tf2_broadcaster.py + nodes/turtle_tf2_broadcaster.py + nodes/turtle_tf2_listener.py + nodes/turtle_tf2_listener_wait.py + nodes/turtle_tf2_message_broadcaster.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Install C++ Examples +install(TARGETS turtle_tf2_broadcaster turtle_tf2_listener + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Install Other Resources +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(DIRECTORY rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/turtle_tf2/launch/turtle_tf2_demo.launch b/turtle_tf2/launch/turtle_tf2_demo.launch new file mode 100644 index 0000000..86e67c0 --- /dev/null +++ b/turtle_tf2/launch/turtle_tf2_demo.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/turtle_tf2/launch/turtle_tf2_sensor.launch b/turtle_tf2/launch/turtle_tf2_sensor.launch new file mode 100644 index 0000000..4f0adcd --- /dev/null +++ b/turtle_tf2/launch/turtle_tf2_sensor.launch @@ -0,0 +1,4 @@ + + + + diff --git a/turtle_tf2/package.xml b/turtle_tf2/package.xml new file mode 100644 index 0000000..0b163f6 --- /dev/null +++ b/turtle_tf2/package.xml @@ -0,0 +1,35 @@ + + + turtle_tf2 + 0.0.1 + + turtle_tf2 demonstrates how to write a tf2 broadcaster and listener with the turtlesim. The tutle_tf2_listener commands turtle2 to follow turtle1 around as you drive turtle1 using the keyboard. + + Denis Štogl + BSD + + + https://github.com/ros/geometry_tutorials + https://github.com/ros/geometry_tutorials/issues + + Denis Štogl + + catkin + + geometry_msgs + roscpp + rospy + std_msgs + tf2 + tf2_ros + turtlesim + + geometry_msgs + roscpp + rospy + std_msgs + tf2 + tf2_ros + turtlesim + + \ No newline at end of file diff --git a/turtle_tf2/rviz/turtle_rviz.rviz b/turtle_tf2/rviz/turtle_rviz.rviz new file mode 100644 index 0000000..6a87e8d --- /dev/null +++ b/turtle_tf2/rviz/turtle_rviz.rviz @@ -0,0 +1,117 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + Splitter Ratio: 0.5 + Tree Height: 479 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Scale: 1 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: /world + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 11.8991 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.454796 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.5554 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 756 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000013c0000026dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000002540000011a00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002c0000026d000000dc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000026dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002c0000026d000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fc0000003efc0100000002fb0000000800540069006d00650100000000000004fc0000020b00fffffffb0000000800540069006d00650100000000000004500000000000000000000002af0000026d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1276 + X: 4 + Y: 22 diff --git a/turtle_tf2/rviz/turtle_rviz_groovy.rviz b/turtle_tf2/rviz/turtle_rviz_groovy.rviz new file mode 100644 index 0000000..ea0d72c --- /dev/null +++ b/turtle_tf2/rviz/turtle_rviz_groovy.rviz @@ -0,0 +1,112 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /TF1/Tree1 + Splitter Ratio: 0.5 + Tree Height: 542 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: /world + Name: root + Tools: + - Class: rviz/MoveCamera + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/Select + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 25.6515 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.785398 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 756 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000013c000002acfc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002c000002ac000000dc00ffffff000000010000010f000002acfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002c000002ac000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000020b00fffffffb0000000800540069006d00650100000000000004500000000000000000000002af000002ac00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1276 + X: 7 + Y: 22 diff --git a/turtle_tf2/scripts/dynamic_tf2_broadcaster.py b/turtle_tf2/scripts/dynamic_tf2_broadcaster.py new file mode 100755 index 0000000..6cbde41 --- /dev/null +++ b/turtle_tf2/scripts/dynamic_tf2_broadcaster.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, 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. + +import roslib +roslib.load_manifest('turtle_tf2') + +import rospy +import tf2_ros +import geometry_msgs.msg +import math + +if __name__ == '__ STATIC main__': + rospy.init_node('my_tf2_broadcaster') + br = tf2_ros.TransformBroadcaster() + t = geometry_msgs.msg.TransformStamped() + + t.header.frame_id = "turtle1" + t.child_frame_id = "carrot1" + t.transform.translation.x = 0.0 + t.transform.translation.y = 2.0 + t.transform.translation.z = 0.0 + t.transform.rotation.x = 0.0 + t.transform.rotation.y = 0.0 + t.transform.rotation.z = 0.0 + t.transform.rotation.w = 1.0 + + rate = rospy.Rate(10.0) + while not rospy.is_shutdown(): + t.header.stamp = rospy.Time.now() + br.sendTransform(t) + rate.sleep() + +if __name__ == '__main__': + rospy.init_node('my_tf2_broadcaster') + br = tf2_ros.TransformBroadcaster() + t = geometry_msgs.msg.TransformStamped() + + t.header.frame_id = "turtle1" + t.child_frame_id = "carrot1" + + rate = rospy.Rate(10.0) + while not rospy.is_shutdown(): + x = rospy.Time.now().to_sec() * math.pi + + t.header.stamp = rospy.Time.now() + t.transform.translation.x = 10 * math.sin(x) + t.transform.translation.y = 10 * math.cos(x) + t.transform.translation.z = 0.0 + t.transform.rotation.x = 0.0 + t.transform.rotation.y = 0.0 + t.transform.rotation.z = 0.0 + t.transform.rotation.w = 1.0 + + br.sendTransform(t) + rate.sleep() diff --git a/turtle_tf2/scripts/fixed_tf2_broadcaster.py b/turtle_tf2/scripts/fixed_tf2_broadcaster.py new file mode 100755 index 0000000..c871150 --- /dev/null +++ b/turtle_tf2/scripts/fixed_tf2_broadcaster.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, 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. + +import roslib +roslib.load_manifest('turtle_tf2') + +import rospy +import tf2_ros +import tf2_msgs.msg +import geometry_msgs.msg + + +class FixedTFBroadcaster: + + def __init__(self): + self.pub_tf = rospy.Publisher("/tf", tf2_msgs.msg.TFMessage, queue_size=1) + + while not rospy.is_shutdown(): + # Run this loop at about 10Hz + rospy.sleep(0.1) + + t = geometry_msgs.msg.TransformStamped() + t.header.frame_id = "turtle1" + t.header.stamp = rospy.Time.now() + t.child_frame_id = "carrot1" + t.transform.translation.x = 0.0 + t.transform.translation.y = 2.0 + t.transform.translation.z = 0.0 + + t.transform.rotation.x = 0.0 + t.transform.rotation.y = 0.0 + t.transform.rotation.z = 0.0 + t.transform.rotation.w = 1.0 + + tfm = tf2_msgs.msg.TFMessage([t]) + self.pub_tf.publish(tfm) + +if __name__ == '__main__': + rospy.init_node('my_tf2_broadcaster') + tfb = FixedTFBroadcaster() + + rospy.spin() diff --git a/turtle_tf2/scripts/turtle_tf2_broadcaster.py b/turtle_tf2/scripts/turtle_tf2_broadcaster.py new file mode 100755 index 0000000..2330417 --- /dev/null +++ b/turtle_tf2/scripts/turtle_tf2_broadcaster.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, 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. +#!/usr/bin/env python + +import roslib +roslib.load_manifest('turtle_tf2') +import rospy + +# Because of transformations +import tf + +import tf2_ros +import geometry_msgs.msg +import turtlesim.msg + + +def handle_turtle_pose(msg, turtlename): + br = tf2_ros.TransformBroadcaster() + t = geometry_msgs.msg.TransformStamped() + + t.header.stamp = rospy.Time.now() + t.header.frame_id = "world" + t.child_frame_id = turtlename + t.transform.translation.x = msg.x + t.transform.translation.y = msg.y + t.transform.translation.z = 0.0 + q = tf.transformations.quaternion_from_euler(0, 0, msg.theta) + t.transform.rotation.x = q[0] + t.transform.rotation.y = q[1] + t.transform.rotation.z = q[2] + t.transform.rotation.w = q[3] + + br.sendTransform(t) + +if __name__ == '__main__': + rospy.init_node('tf_turtle') + turtlename = rospy.get_param('~turtle') + rospy.Subscriber('/%s/pose' % turtlename, + turtlesim.msg.Pose, + handle_turtle_pose, + turtlename) + rospy.spin() diff --git a/turtle_tf2/scripts/turtle_tf2_listener.py b/turtle_tf2/scripts/turtle_tf2_listener.py new file mode 100755 index 0000000..a62f0eb --- /dev/null +++ b/turtle_tf2/scripts/turtle_tf2_listener.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, 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. + +import roslib +roslib.load_manifest('turtle_tf2') +import rospy + +import math +import tf2_ros +import geometry_msgs.msg +import turtlesim.srv + +if __name__ == '__main__': + rospy.init_node('tf2_turtle') + + tfBuffer = tf2_ros.Buffer() + listener = tf2_ros.TransformListener(tfBuffer) + + rospy.wait_for_service('spawn') + spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn) + spawner(4, 2, 0, 'turtle2') + + turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist, queue_size=1) + + rate = rospy.Rate(10.0) + while not rospy.is_shutdown(): + try: + trans = tfBuffer.lookup_transform('turtle2', 'turtle1', rospy.Time()) + except (tf2_ros.LookupException, tf2_ros.ConnectivityException): + rospy.sleep(1.0) + continue + + msg = geometry_msgs.msg.Twist() + + msg.angular.z = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x) + msg.linear.x = 0.5 * math.sqrt(trans.transform.translation.x ** 2 + trans.transform.translation.y ** 2) + + turtle_vel.publish(msg) + + rate.sleep() diff --git a/turtle_tf2/scripts/turtle_tf2_listener_wait.py b/turtle_tf2/scripts/turtle_tf2_listener_wait.py new file mode 100755 index 0000000..089b110 --- /dev/null +++ b/turtle_tf2/scripts/turtle_tf2_listener_wait.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, 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. + +import roslib +roslib.load_manifest('turtle_tf2') +import rospy + +import math +import tf2_ros +import geometry_msgs.msg +import turtlesim.srv + +if __name__ == '__main__': + rospy.init_node('tf2_turtle') + + tfBuffer = tf2_ros.Buffer() + listener = tf2_ros.TransformListener(tfBuffer) + + rospy.wait_for_service('spawn') + spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn) + spawner(4, 2, 0, 'turtle2') + + turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist, queue_size=1) + + rate = rospy.Rate(10.0) + while not rospy.is_shutdown(): + try: + trans = tfBuffer.lookup_transform('turtle2', 'turtle1', rospy.Time.now(), rospy.Duration(1.0)) + except tf2_ros.Exception: + continue + + angular = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x) + linear = 0.5 * math.sqrt(trans.transform.translation.x ** 2 + trans.transform.translation.y ** 2) + msg = geometry_msgs.msg.Twist() + msg.linear.x = linear + msg.angular.z = angular + turtle_vel.publish(msg) + + rate.sleep() diff --git a/turtle_tf2/scripts/turtle_tf2_message_broadcaster.py b/turtle_tf2/scripts/turtle_tf2_message_broadcaster.py new file mode 100755 index 0000000..dfab3a2 --- /dev/null +++ b/turtle_tf2/scripts/turtle_tf2_message_broadcaster.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, 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. +#!/usr/bin/env python + +import roslib +roslib.load_manifest('turtle_tf2') +import rospy + +import turtlesim.msg +import geometry_msgs.msg +import turtlesim.srv +from geometry_msgs.msg import PointStamped, Point +from std_msgs.msg import Header + + +class PointPublisher: + def handle_turtle_pose(self, msg, turtlename): + self.pub.publish(PointStamped(Header(0, rospy.rostime.get_rostime(), "/world"), Point(msg.x, msg.y, 0))) + + def __init__(self): + self.turtlename = "turtle3" # rospy.get_param('~turtle') + self.sub = rospy.Subscriber('/%s/pose' % self.turtlename, + turtlesim.msg.Pose, + self.handle_turtle_pose, + self.turtlename) + self.pub = rospy.Publisher('turtle_point_stamped', PointStamped, queue_size=1) + +if __name__ == '__main__': + rospy.init_node('tf2_turtle_stamped_msg_publisher') + rospy.wait_for_service('spawn') + spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn) + spawner(4, 2, 0, 'turtle3') + + pp = PointPublisher() + + pub = rospy.Publisher("turtle3/cmd_vel", geometry_msgs.msg.Twist, queue_size=1) + while not rospy.is_shutdown(): + msg = geometry_msgs.msg.Twist() + msg.linear.x = 1 + msg.angular.z = 1 + pub.publish(msg) + rospy.sleep(rospy.Duration(0.1)) diff --git a/turtle_tf2/src/turtle_tf2_broadcaster.cpp b/turtle_tf2/src/turtle_tf2_broadcaster.cpp new file mode 100644 index 0000000..e1c1596 --- /dev/null +++ b/turtle_tf2/src/turtle_tf2_broadcaster.cpp @@ -0,0 +1,42 @@ +#include +#include +#include +#include +#include + +std::string turtle_name; + + + +void poseCallback(const turtlesim::PoseConstPtr& msg){ + static tf2_ros::TransformBroadcaster br; + geometry_msgs::TransformStamped transformStamped; + + transformStamped.header.stamp = ros::Time::now(); + transformStamped.header.frame_id = "world"; + transformStamped.child_frame_id = turtle_name; + transformStamped.transform.translation.x = msg->x; + transformStamped.transform.translation.y = msg->y; + transformStamped.transform.translation.z = 0.0; + tf2::Quaternion q; + q.setRPY(msg->theta, 0, 0); + transformStamped.transform.rotation.x = q.x(); + transformStamped.transform.rotation.y = q.y(); + transformStamped.transform.rotation.z = q.z(); + transformStamped.transform.rotation.w = q.w(); + + br.sendTransform(transformStamped); +} + +int main(int argc, char** argv){ + ros::init(argc, argv, "my_tf2_broadcaster"); + if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;}; + turtle_name = argv[1]; + + ros::NodeHandle node; + ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback); + + ros::spin(); + return 0; +}; + diff --git a/turtle_tf2/src/turtle_tf2_listener.cpp b/turtle_tf2/src/turtle_tf2_listener.cpp new file mode 100644 index 0000000..c504b91 --- /dev/null +++ b/turtle_tf2/src/turtle_tf2_listener.cpp @@ -0,0 +1,46 @@ +#include +#include +#include +#include +#include + +int main(int argc, char** argv){ + ros::init(argc, argv, "my_tf2_listener"); + + ros::NodeHandle node; + + ros::service::waitForService("spawn"); + ros::ServiceClient add_turtle = + node.serviceClient("spawn"); + turtlesim::Spawn srv; + add_turtle.call(srv); + + ros::Publisher turtle_vel = + node.advertise("turtle2/cmd_vel", 10); + + tf2_ros::Buffer tfBuffer; + tf2_ros::TransformListener tfListener(tfBuffer); + + ros::Rate rate(10.0); + while (node.ok()){ + geometry_msgs::TransformStamped transformStamped; + try{ + transformStamped = tfBuffer.lookupTransform("/turtle2", "/turtle1", + ros::Time(0)); + } + catch (tf2::TransformException &ex) { + ROS_ERROR("%s",ex.what()); + ros::Duration(1.0).sleep(); + } + + geometry_msgs::Twist vel_msg; + vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y, + transformStamped.transform.translation.x); + vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) + + pow(transformStamped.transform.translation.y, 2)); + turtle_vel.publish(vel_msg); + + rate.sleep(); + } + return 0; +};