#!/usr/bin/env python # -*- encoding: utf-8 -*- from superros.comm import RosNode import PyKDL node = RosNode("testing_node") node.setHz(node.setupParameter("hz", 30)) while node.isActive(): frame = node.retrieveTransform("base_link", "odom") if frame is not None: t = PyKDL.Frame(PyKDL.Rotation.RotZ(1.57), PyKDL.Vector(0, 0, 0.5)) frame = frame * t node.broadcastTransform(frame, "base_link_2", "odom", node.getCurrentTime()) node.tick()
color=Color.pickFromPalette(2), transform=PyKDL.Frame(PyKDL.Vector(-2.0, 0, 0)) ) # Cone2 Frame cone_2_frame = PyKDL.Frame(PyKDL.Vector(2.0, 0, 0)) while node.isActive(): # Update Cone2 Pose getting stored Object cone_2_frame.M.DoRotZ(0.02) scene.getObjectByName("cone2").setTransform(cone_2_frame) # Update Cone1 Pose overwriting old Object scene.createCone(name="cone1", transform=PyKDL.Frame( PyKDL.Vector( 0.0, 0.0, math.sin(1.0+2*math.pi*1.0*node.getCurrentTimeInSecs()) ) )) # Publish World Frame node.broadcastTransform(PyKDL.Frame(), "world", "base", node.getCurrentTime()) # Update Scene scene.show() # Node Forward node.tick()