done_frames += 1.0
print("{0:.2f} %".format(done_frames / number_of_frames * 100.0))
sendNewMessage()
command_proxy_client = CommandProxyClient("{}_supervisor".format(robot_name))
command_proxy_client.registerDoneCallback(supervisor_done_callback)
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ 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("Chessboard TF FOUND!!!!!!!!")
matrix_cube = getTfMatrix(robot_name, scan_name, base_frame)
matrix_tf = matrix_cube
number_of_frames = len(matrix_cube.frames_names)
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
sendNewMessage()
moving_msg = Int32()
callback_map[topic] = CallbackManager.createACallback(topic)
node.createSubscriber(topic, msg_type, callback_map[topic])
for tf in tf_list:
datatf_map[tf] = []
print("Started")
while node.isActive():
if node.getElapsedTimeInSecs() >= node.getParameter("time_window"):
print("Time Window")
break
try:
found = True
tf_frames = {}
for tf in tf_list:
tf_frame = node.retrieveTransform(
tf, tf_base, -1)
if tf_frame != None:
tf_frames[tf] = tf_frame
else:
found = False
break
if found:
pushNewData()
print("Sync time ready!")
node.tick()
except KeyboardInterrupt:
print("Interrupt")
print("Ended")
#!/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()
"/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()
#addScanPose(msgs, "shape_scan_tf_3")
addScanPose(msgs, "shape_scan_tf_4")
addScanPose(msgs, "shape_scan_tf_5")
addScanPose(msgs, "shape_scan_tf_6")
addScanPose(msgs, "shape_scan_tf_7")
addScanPose(msgs, "shape_scan_tf_8")
addScanPose(msgs, "shape_scan_tf_13")
addScanPose(msgs, "shape_scan_tf_14")
addScanPose(msgs, "shape_scan_tf_15")
addScanPose(msgs, "shape_scan_tf_0")
addScanPose(msgs, "shape_scan_tf_1")
addScanPose(msgs, "shape_extra_s1")
addScanPose(msgs, "shape_extra_s2")
addScanPose(msgs, "shape_extra_s3")
addScanPose(msgs, "shape_extra_s4")
addScanPose(msgs, "shape_extra_s5")
addScanPose(msgs, "shape_extra_s6")
print msgs
sendNewMessage()
while node.isActive():
wrist = node.retrieveTransform("comau_smart_six/link6",
"comau_smart_six/base_link", -1)
if wrist:
pose = transformations.KDLToPose(wrist)
pose_pub.publish(pose)
node.tick()
#⬢⬢⬢⬢⬢➤ NODE
if __name__ == '__main__':
node = RosNode("tf_filter")
node.setupParameter("hz", 30)
node.setupParameter("world_tf", "world")
node.setupParameter("target_tf", "target")
node.setupParameter("tf_buffer_size", 1300)
node.setupParameter("tf_slot_size", 100)
node.setHz(node.getParameter("hz"))
tffilter = TfFilter()
try:
while node.isActive():
if tffilter.current_tf_name != '':
current_tf = None
while not current_tf:
current_tf = node.retrieveTransform(
tffilter.current_tf_name,
node.getParameter("world_tf"), rospy.Time(0))
size = node.getParameter("tf_buffer_size")
tffilter.updateAndBroadcast(node, current_tf, size)
node.tick()
except rospy.ROSInterruptException:
pass
