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