def Trigger(data=None): global is_task_active is_task_active = True is_done = False # ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ if __name__ == '__main__': #⬢⬢⬢⬢⬢➤ NODE node = RosNode("grasshopper_grasp") node.setupParameter("hz", 10) # node.setHz(node.getParameter("hz")) proxy_server = ConnectionsScanProxyServer() is_task_active = False is_done = False #▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ #▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ LOOP ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ #▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ while node.isActive(): if is_task_active: if is_done: proxy_server.sendResponse(True) is_task_active = False
from sensor_msgs.msg import Image from cv_bridge import CvBridge, CvBridgeError import rospkg import numpy as np import math import json from rocup.utils.tfmatrix import TfMatrixCube, TfMatrixHyperCube, TfMatrixSphere from std_msgs.msg import Int32 from geometry_msgs.msg import Pose import threading from tf_matricies import getTfMatrix #⬢⬢⬢⬢⬢➤ NODE node = RosNode("matrix_tf_sequencer") node.setupParameter("hz", 30) node.setHz(node.getParameter("hz")) node.setupParameter("scan", "default") scan_name = node.getParameter("scan") node.setupParameter("robot", "bonmetc60") robot_name = node.getParameter("robot") moving_pub = node.createPublisher("~moving_flag", Int32) pose_pub = node.createPublisher("~tool_pose", Pose) counter = 0 active_command = None moving_flag = -1 number_of_frames = 0.0 done_frames = 0.0
import datetime import copy def topicsCallback(*args): print(args) timestamp = str(datetime.datetime.now()).replace('.', '_').replace( ":", "_").replace(' ', '_') #⬢⬢⬢⬢⬢➤ NODE node = RosNode("topic_export") node.setupParameter("hz", 120) node.setHz(node.getParameter("hz")) node.setupParameter("output_path", "/tmp/topic_export/") node.setupParameter("dataset_name", timestamp) node.setupParameter("time_window", 9999999999.0) node.setupParameter("tf_base", '/comau_smart_six/base_link') node.setupParameter("tf_target", '/comau_smart_six/link6') node.setupParameter("default_image_format", 'jpg') node.setupParameter("default_image_format_depth", 'png') default_image_size = node.setupParameter("default_image_size", '640;480', array_type=int) node.setupParameter("default_image_padding", 5) print("Dataset: {}".format(timestamp)) tf_base = node.getParameter("tf_base")
import datetime import copy def topicsCallback(*args): print(args) timestamp = str(datetime.datetime.now()).replace( '.', '_').replace(":", "_").replace(' ', '_') #⬢⬢⬢⬢⬢➤ NODE node = RosNode("topic_export") node.setupParameter("hz", 120) node.setHz(node.getParameter("hz")) node.setupParameter("output_path", "/tmp/topic_export/") node.setupParameter("dataset_name", timestamp) node.setupParameter("camera_topic", "/usb_cam_1/image_raw/compressed") node.setupParameter("time_window", 9999999999.0) node.setupParameter("tf_base", 'world') node.setupParameter("tf_target", 'tf_camera') node.setupParameter("default_image_format", 'jpg') node.setupParameter("default_image_format_depth", 'png') default_image_size = node.setupParameter("default_image_size", '640;480', array_type=int) node.setupParameter("default_image_padding", 5) print("Dataset: {}".format(timestamp)) tf_base = node.getParameter("tf_base") tf_list = [node.getParameter("tf_target")]
import rospy import math import numpy import time import json from numpy import * import tf from tf.msg import tfMessage from std_msgs.msg import String, Float64MultiArray #⬢⬢⬢⬢⬢➤ NODE node = RosNode("custom_window_test") node.setupParameter("robot_name", "comau_smart_six") # bonmetc60 robot_name = node.getParameter("robot_name") class CustomWindow(PyQtWindow): def __init__(self, uifile, node): super(CustomWindow, self).__init__(uifile=uifile) self.robot_name = robot_name ############# Variable Initialization ############# self.available_tfs_map = {} self.available_shapes_map = {} self.available_ctrlpar_map = {} self.wire_z_offset = 0 self.wire_x_offset = 0 self.wire_angle = 0
self.frame = frame self.type = tp #⬢⬢⬢⬢⬢➤ NODE node = RosNode("path_from_tf") node.setupParameter("hz", 5) node.setupParameter("path_name", "") node.setupParameter("target", "comau_smart_six/base_link") node.setupParameter("base_frame", "comau_smart_six/link3") node.setupParameter( "path_file", "/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():
from geometry_msgs.msg import Twist, Pose import datetime import time def topicsCallback(*args): print(args) timestamp = str(datetime.datetime.now()).replace('.', '_').replace( ":", "_").replace(' ', '_') #⬢⬢⬢⬢⬢➤ NODE node = RosNode("topic_export") node.setupParameter("hz", 30) node.setHz(node.getParameter("hz")) node.setupParameter("output_path", "/tmp/topic_export/") node.setupParameter("dataset_name", timestamp) node.setupParameter("time_window", 9999999.0) node.setupParameter("default_image_format", 'jpg') node.setupParameter("default_image_padding", 5) print("Dataset: {}".format(timestamp)) topic_map = { # '/matrix_tf_sequencer/moving_flag': eval("Int32"), # '/matrix_tf_sequencer/tool_pose': eval("Pose"), '/comau_smart_six/tool': eval("Pose"), # '/usb_cam/image_raw/compressed': eval('CompressedImage') '/usb_cam_1/image_raw/compressed': eval('CompressedImage') #'/darknet_rgb_detector/predictions': eval('Float64MultiArray')
from superros.comm import RosNode import superros.transformations as transformations import message_filters from sensor_msgs.msg import Image, CameraInfo import cv2 import aruco import rospkg import numpy as np import math import json from rocup.utils.tfmatrix import TfMatrixCube, TfMatrixHyperCube, TfMatrixSphere #⬢⬢⬢⬢⬢➤ NODE node = RosNode("matrix_tf_generator") node.setupParameter("hz", 1) node.setHz(node.getParameter("hz")) # ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ # ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ TF MATRIX ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ # ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ node.setupParameter("offset_cube", [0.0, 0, 0]) nX = 3 nY = 2 # 4 nZ = 3 dX = 0.2 dY = 0.15 # 0.1 dZ = -0.05
except Exception as e: print(e) self._send_command_result(False) #⬢⬢⬢⬢⬢➤ 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)