How to use transform_out method in pandera

Best Python code snippet using pandera_python

twist_frame_integrator.py

Source:twist_frame_integrator.py

1#!/usr/bin/env python2import roslib; roslib.load_manifest('lcsr_tf_tools')3import rospy4import tf5import geometry_msgs.msg as geometry_msgs6import threading7import math8import PyKDL9import copy10def multiply_quat(q0,q1):11    [x0,y0,z0,w0] = q012    [x1,y1,z1,w1] = q113    return ( (w0*x1 + x0*w1 + y0*z1 - z0*y1),14             (w0*y1 - x0*z1 + y0*w1 + z0*x1),15             (w0*z1 + x0*y1 - y0*x1 + z0*w1),16             (w0*w1 - x0*x1 - y0*y1 - z0*z1) )17class TwistFrameIntegrator(object):18    def __init__(self):19        # Get the node params20        self.linear_multiplier = rospy.get_param('~linear_multiplier')21        self.angular_multiplier = rospy.get_param('~angular_multiplier')22        self.broadcast_rate = rospy.get_param('~broadcast_rate')23        self.body_fixed = rospy.get_param('~body_fixed')24        # Initialize the frame we're going to publish25        xyzw = [float(f) for f in rospy.get_param('~xyzw','0 0 0 1').split()]26        xyzw_offset = [float(f) for f in rospy.get_param('~xyzw_offset','0 0 0 1').split()]27        xyz = [float(f) for f in rospy.get_param('~xyz','0 0 0').split()]28        rospy.logwarn("xyzw: " + str(xyzw))29        rospy.logwarn("xyz: " + str(xyz))30        self.transform = PyKDL.Frame(31            PyKDL.Rotation.Quaternion(*xyzw)*PyKDL.Rotation.Quaternion(*xyzw_offset),32            PyKDL.Vector(*xyz))33        self.transform_out = PyKDL.Frame()34        self.rotation = self.transform.M.GetQuaternion()35        self.time = rospy.Time.now()36        self.frame_id = rospy.get_param('~frame_id')37        self.child_frame_id = rospy.get_param('~child_frame_id')38        # Synchronization39        self.broadcast_lock = threading.Lock()40        # Initialze TF structures41        self.broadcaster = tf.TransformBroadcaster()42        # Subscribe to twist inputs43        self.twist_sub = rospy.Subscriber(44                "twist", geometry_msgs.Twist,45                self.twist_cb)46        self.twiststamped_sub = rospy.Subscriber(47                "twist_stamped", geometry_msgs.TwistStamped,48                self.twiststamped_cb)49        # Broadcast the frame at a given rate50        self.broadcast_thread = threading.Thread(target=self.broadcast_loop)51        self.broadcast_thread.start()52    def broadcast_loop(self):53        """54        Broadcast the integrated TF frame at a fixed rate.55        """56        rate = rospy.Rate(self.broadcast_rate)57        while not rospy.is_shutdown():58            # Acquire broadcast lock59            self.broadcast_lock.acquire()60            # Broadcast the frame61            try:62                self.broadcaster.sendTransform(63                        (self.transform_out.p.x(), self.transform_out.p.y(), self.transform_out.p.z()),64                        self.transform_out.M.GetQuaternion(),65                        rospy.Time.now(),66                        self.child_frame_id,67                        self.frame_id)68            except Exception as ex:69                rospy.logerr("Failed to broadcast transform: "+str(ex))70            # Release broadcast lock71            self.broadcast_lock.release()72            # Don't spam TF73            rate.sleep()74    def twiststamped_cb(self,msg):75        """76        This callback pulls the twist out of a TwistStamped message. For now,77        it disregards the origin frame.78        """79        twist_cb(msg.twist)80    def twist_cb(self,msg):81        """82        This callback integrates the linear and angular velocities into a TF83        frame, and then broadcasts the frame.84        """85        try:86            # Acquire broadcast lock87            self.broadcast_lock.acquire()88            # Convert angular rotation vector to quaternion89            q_velocity = (90                    msg.angular.x * math.sin(self.angular_multiplier/2),91                    msg.angular.y * math.sin(self.angular_multiplier/2),92                    msg.angular.z * math.sin(self.angular_multiplier/2),93                    math.cos(self.angular_multiplier/2))94            q_velocity_norm = math.sqrt(math.pow(q_velocity[0],2) + math.pow(q_velocity[1],2) + math.pow(q_velocity[2],2) + math.pow(q_velocity[3],2))95            q_velocity = [i/q_velocity_norm for i in q_velocity]96            if self.body_fixed:97                # Integrate linear velocity in local frame98                self.transform.p += self.transform.M*(self.linear_multiplier*PyKDL.Vector(msg.linear.x,msg.linear.y,msg.linear.z))99                # Integrate angular velocity in local frame100                self.rotation = multiply_quat(self.rotation,q_velocity)101                self.transform.M = PyKDL.Rotation.Quaternion(*self.rotation)102                103                # Copy this transform104                self.transform_out = PyKDL.Frame(self.transform)105            else:106                # Integrate linear velocity107                self.transform.p += self.linear_multiplier*PyKDL.Vector(msg.linear.x,msg.linear.y,msg.linear.z)108                # Integrate angular velocity109                self.rotation = multiply_quat(q_velocity,self.rotation)110                self.transform.M = PyKDL.Rotation.Quaternion(*self.rotation)111                # Invert the transform to get parent-frame relative transform112                self.transform_out = self.transform113            self.transform_out.M.DoRotZ(-math.pi/2)114            self.transform_out.M.DoRotX(-math.pi/2)115        finally:116            # Release broadcast lock117            self.broadcast_lock.release()118def main():119    rospy.init_node('twist_frame_integrator')120    121    twist_frame_integrator = TwistFrameIntegrator()122    rospy.loginfo("Spinning...")123    rospy.spin()124    rospy.loginfo("Done.")125if __name__ == '__main__':...

LinkedTransformTest.py

Source:LinkedTransformTest.py

1import unittest2from kivy.event import EventDispatcher3from .LinkedTransform import LinkedTransform4import numpy as np5import cv26from pathlib import Path7class LinkedTransformMock(LinkedTransform):8    def __init__(self, *args, **kwargs):9        self.received = []10        super().__init__(*args, **kwargs)11    def receive_frame(self, frame: np.ndarray):12        self.received.append(frame)13class SimpleObserver(EventDispatcher):14    pass15class LinkedTransformTest(unittest.TestCase):16    @classmethod17    def get_test_image(cls) -> np.ndarray:18        return cv2.imread(str((Path(__file__).parent.parent / Path('resource/test/Archaeologist-Tux-icon.png'))))19    def test_frames_are_passed_through(self):20        transform_in = LinkedTransform()21        transform_out = LinkedTransformMock()22        transform_in.attach_sink(transform_out)23        frame = LinkedTransformTest.get_test_image()24        transform_in.receive_frame(frame)25        self.assertIn(frame, transform_out.received, 'Transform didn\'t passthrough the frame')26    def test_frame_is_being_transformed(self):27        transform_in = LinkedTransform()28        transform_out = LinkedTransformMock()29        transform_in.attach_sink(transform_out)30        frame = LinkedTransformTest.get_test_image()31        transform_in.transform_fn = np.transpose32        transform_in.receive_frame(frame)33        self.assertTrue((frame.T == transform_out.received).all())34    def test_observer_is_being_notified(self):35        transform_in = LinkedTransform()36        transform_out = LinkedTransformMock()37        transform_in.attach_sink(transform_out)38        results = {'received': False, 'processed': False}39        def register_frame_received(*args):40            results['received'] = True41        def register_frame_processed(*args):42            results['processed'] = True43        transform_in.bind(on_frame_received=register_frame_received, on_frame_processed=register_frame_processed)44        frame = LinkedTransformTest.get_test_image()45        transform_in.receive_frame(frame)46        self.assertTrue(results['received'], 'Observer did not receive "on_frame_received" event')...

PoseAverage.py

Source:PoseAverage.py

1from geometry_msgs.msg import Transform2import tf.transformations as tr3import math4class PoseAverage(object):5    def __init__(self):6        self.translation = [0.0, 0.0, 0.0]7        self.n = 08        self.sum_sines = 09        self.sum_cosines = 010    def add_pose(self, transform_in):11        self.n += 112        translation_in = transform_in.translation13        self.translation = [(self.translation[0]*(self.n - 1) + translation_in.x)/self.n,\14                            (self.translation[1]*(self.n - 1) + translation_in.y)/self.n,\15                            (self.translation[2]*(self.n - 1) + translation_in.z)/self.n]16        rotation_in = transform_in.rotation17        quaternion = (rotation_in.x, rotation_in.y, rotation_in.z, rotation_in.w)18        theta_in = tr.euler_from_quaternion(quaternion)[2]19        self.sum_sines += math.sin(theta_in)20        self.sum_cosines += math.cos(theta_in)21    def get_average(self):22        if self.n == 0:23            return None24        transform_out = Transform()25        theta_out = math.atan2(self.sum_sines, self.sum_cosines)26        trans = transform_out.translation27        rot = transform_out.rotation28        (trans.x,trans.y,trans.z)=self.translation29        (rot.x,rot.y,rot.z,rot.w)=tr.quaternion_from_euler(0,0,theta_out)...

Automation Testing Tutorials

Learn to execute automation testing from scratch with LambdaTest Learning Hub. Right from setting up the prerequisites to run your first automation test, to following best practices and diving deeper into advanced test scenarios. LambdaTest Learning Hubs compile a list of step-by-step guides to help you be proficient with different test automation frameworks i.e. Selenium, Cypress, TestNG etc.