#!/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()
