pose.py
1 #!/usr/bin/env python3 2 ################################################################################ 3 # Copyright 2022 FZI Research Center for Information Technology 4 # 5 # Redistribution and use in source and binary forms, with or without 6 # modification, are permitted provided that the following conditions are met: 7 # 8 # 1. Redistributions of source code must retain the above copyright notice, 9 # this list of conditions and the following disclaimer. 10 # 11 # 2. Redistributions in binary form must reproduce the above copyright notice, 12 # this list of conditions and the following disclaimer in the documentation 13 # and/or other materials provided with the distribution. 14 # 15 # 3. Neither the name of the copyright holder nor the names of its 16 # contributors may be used to endorse or promote products derived from this 17 # software without specific prior written permission. 18 # 19 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 # POSSIBILITY OF SUCH DAMAGE. 30 ################################################################################ 31 32 # ----------------------------------------------------------------------------- 33 # \file pose.py 34 # 35 # \author Stefan Scherzinger <scherzin@fzi.de> 36 # \date 2022/11/09 37 # 38 # ----------------------------------------------------------------------------- 39 40 import rclpy 41 from rclpy.node import Node 42 import numpy as np 43 import quaternion 44 from geometry_msgs.msg import Twist 45 from geometry_msgs.msg import PoseStamped 46 import tf2_ros 47 import sys 48 import time 49 import threading 50 import os 51 52 53 class converter(Node): 54 """Convert Twist messages to PoseStamped 55 56 Use this node to integrate twist messages into a moving target pose in 57 Cartesian space. An initial TF lookup assures that the target pose always 58 starts at the robot's end-effector. 59 """ 60 61 def __init__(self): 62 super().__init__("converter") 63 64 self.twist_topic = self.declare_parameter("twist_topic", "my_twist").value 65 self.pose_topic = self.declare_parameter("pose_topic", "my_pose").value 66 self.frame_id = self.declare_parameter("frame_id", "base_link").value 67 self.end_effector = self.declare_parameter("end_effector", "tool0").value 68 69 self.tf_buffer = tf2_ros.Buffer() 70 self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) 71 self.rot = np.quaternion(0, 0, 0, 1) 72 self.pos = [0, 0, 0] 73 74 self.pub = self.create_publisher(PoseStamped, self.pose_topic, 3) 75 self.sub = self.create_subscription(Twist, self.twist_topic, self.twist_cb, 1) 76 self.last = time.time() 77 78 self.startup_done = False 79 period = 1.0 / self.declare_parameter("publishing_rate", 100).value 80 self.timer = self.create_timer(period, self.publish) 81 82 self.thread = threading.Thread(target=self.startup, daemon=True) 83 self.thread.start() 84 85 def startup(self): 86 """Make sure to start at the robot's current pose""" 87 # Wait until we entered spinning in the main thread. 88 time.sleep(1) 89 try: 90 start = self.tf_buffer.lookup_transform( 91 target_frame=self.frame_id, 92 source_frame=self.end_effector, 93 time=rclpy.time.Time(), 94 ) 95 96 except ( 97 tf2_ros.InvalidArgumentException, 98 tf2_ros.LookupException, 99 tf2_ros.ConnectivityException, 100 tf2_ros.ExtrapolationException, 101 ) as e: 102 print(f"Startup failed: {e}") 103 os._exit(1) 104 105 self.pos[0] = start.transform.translation.x 106 self.pos[1] = start.transform.translation.y 107 self.pos[2] = start.transform.translation.z 108 self.rot.x = start.transform.rotation.x 109 self.rot.y = start.transform.rotation.y 110 self.rot.z = start.transform.rotation.z 111 self.rot.w = start.transform.rotation.w 112 self.startup_done = True 113 114 def twist_cb(self, data): 115 """Numerically integrate twist message into a pose 116 117 Use global self.frame_id as reference for the navigation commands. 118 """ 119 now = time.time() 120 dt = now - self.last 121 self.last = now 122 123 # Position update 124 self.pos[0] += data.linear.x * dt 125 self.pos[1] += data.linear.y * dt 126 self.pos[2] += data.linear.z * dt 127 128 # Orientation update 129 wx = data.angular.x 130 wy = data.angular.y 131 wz = data.angular.z 132 133 _, q = quaternion.integrate_angular_velocity( 134 lambda _: (wx, wy, wz), 0, dt, self.rot 135 ) 136 137 self.rot = q[-1] # the last one is after dt passed 138 139 def publish(self): 140 if not self.startup_done: 141 return 142 try: 143 msg = PoseStamped() 144 msg.header.stamp = self.get_clock().now().to_msg() 145 msg.header.frame_id = self.frame_id 146 msg.pose.position.x = self.pos[0] 147 msg.pose.position.y = self.pos[1] 148 msg.pose.position.z = self.pos[2] 149 msg.pose.orientation.x = self.rot.x 150 msg.pose.orientation.y = self.rot.y 151 msg.pose.orientation.z = self.rot.z 152 msg.pose.orientation.w = self.rot.w 153 154 self.pub.publish(msg) 155 except Exception: 156 # Swallow 'publish() to closed topic' error. 157 # This rarely happens on killing this node. 158 pass 159 160 161 def main(args=None): 162 rclpy.init(args=args) 163 node = converter() 164 rclpy.spin(node) 165 166 167 if __name__ == "__main__": 168 try: 169 main() 170 except KeyboardInterrupt: 171 rclpy.shutdown() 172 sys.exit(0) 173 except Exception as e: 174 print(e) 175 sys.exit(1)