#!/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()
Logger.error("'tf_name' must be not None")
return
if tf_parent == None:
Logger.error("'tf_parent' must be not None")
return
if saving_name == None:
Logger.error("'saving_name' must be not None")
return
while iterations > 0:
frame = node.retrieveTransform(tf_name, tf_parent, -1)
if frame:
tf_storage.saveFrame(frame, saving_name, tf_parent)
print("Saving ", tf_name, tf_parent)
return
Logger.error(
"Max iterations reached, retrieving '{}'".format(tf_name))
message_proxy.register(command_cb)
while node.isActive():
if publishing:
for tf in all_tfs:
tf_parent = tf[1]["parent_tf"]
tf_name = tf[1]["name"]
tf_data = tf[0]
# print("Publish", tf_parent, tf_name, tf_data)
node.broadcastTransform(tf_data, tf_name, tf_parent,
node.getCurrentTime())
node.tick()
"/home/daniele/Scrivania/new_wires_ws/src/rocup/data/paths/test_path.txt")
path_name = node.getParameter("path_name")
if path_name == "":
path_name = node.getParameter("target")
#⬢⬢⬢⬢⬢➤ LOAD FILE
file = node.getParameter("path_file")
data = np.loadtxt(file, usecols=[0, 1, 2, 3, 4, 5])
types = np.loadtxt(file, usecols=[6], dtype=str)
path = Path()
print(data.shape)
for row in range(0, data.shape[0]):
path_node = PathNode(transformations.KDLFromArray(data[row, :]),
types[row])
path.append(path_node)
while node.isActive():
base_frame = node.retrieveTransform(node.getParameter("target"),
node.getParameter("base_frame"), -1)
for i in range(0, path.size()):
frame = path.getFrame(i)
node.broadcastTransform(frame, "{}_{}".format(path_name, i),
node.getParameter("target"),
node.getCurrentTime())
# marker_pub.publish(plane)
node.tick()
node.setupParameter("hz", 1)
node.setHz(node.getParameter("hz"))
node.setupParameter("scan", "default")
scan_name = node.getParameter("scan")
node.setupParameter("robot", "bonmetc60")
robot_name = node.getParameter("robot")
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ TF MATRIX ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
#⬢⬢⬢⬢⬢➤ Retrive Chessboard TF
base_frame = None
while base_frame is None:
base_frame = node.retrieveTransform("tf_storage_scan_center",
"/{}/base_link".format(robot_name), -1)
print("CENTRAL TF FOUND!!!!!!!!")
matrix_cube = getTfMatrix(robot_name, scan_name, base_frame)
print matrix_cube.frames_names
while node.isActive():
for name, frame in matrix_cube.frames_map.iteritems():
node.broadcastTransform(frame, name,
"/{}/base_link".format(robot_name),
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()