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