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)
