import PyKDL
# DA SPOSTARE
import message_filters
from std_msgs.msg import Float64MultiArray
from sensor_msgs.msg import JointState, Image, CompressedImage
from geometry_msgs.msg import Twist
import datetime
import copy
import os
import shutil
#⬢⬢⬢⬢⬢➤ NODE
node = RosNode("images_calibration_extraction")
node.setupParameter("hz", 10)
node.setHz(node.getParameter("hz"))
images_folder = node.setupParameter(
"images_folder",
"/home/daniele/Desktop/datasets/roars_2017/indust/indust_calibration/camera_rgb_image_raw_compressed"
)
poses_file = node.setupParameter(
"poses_file",
"/home/daniele/Desktop/datasets/roars_2017/indust/indust_calibration/tf#_comau_smart_six_link6.txt"
)
zero_padding = node.setupParameter("zero_padding", 5)
image_extension = node.setupParameter("image_extension", "jpg")
output_folder = node.setupParameter("output_folder", "/tmp/images_output")
subset_percentage = node.setupParameter("subset_percentage", 0.022)
poses = np.loadtxt(poses_file)
from rocup.proxy.command_proxy import CommandProxyClient, CommandMessage, CommandProxyServer
from superros.comm import RosNode
import message_filters
from sensor_msgs.msg import Image, CameraInfo
import cv2
import aruco
import rospkg
import numpy as np
import math
import random
import time
import threading
#⬢⬢⬢⬢⬢➤ NODE
node = RosNode("command_proxy_example_server")
node.setupParameter("hz", 60)
#⬢⬢⬢⬢⬢➤ SIMULA UN TASK ASINCRONO CHE IMPIEGA RANDOM SECONDI PER FINIRE
def sleepAndResolve(message):
if random.randint(0, 100) < 10:
time.sleep(random.uniform(0.01, 5.0))
print("Resolved...")
message.addResponseData("pino", 1)
message.addResponseData("aaa", 2)
command_proxy_server.resolveCommand(message)
return
def example_callback(message):
print("Received", str(message))
from std_msgs.msg import Header, Float64, Float32, Float64MultiArray
import superros.transformations as transformations
from superros.logger import Logger
from superros.comm import RosNode
from rocup.sensors.sensor_manager import SensorManager
def tactile_callback(msg):
global sensor
sensor.update(msg)
if __name__ == '__main__':
node = RosNode("tactile_supervisor_node")
node.setupParameter("hz", 250)
node.setHz(node.getParameter("hz"))
sensor_name = "tactile"
sensor = SensorManager(sensor_name)
reset_publisher = node.createPublisher("/tactile_reset", String)
sensor.uploadResetPublisher(reset_publisher)
node.createSubscriber("/tactile", Float64MultiArray, tactile_callback)
try:
while node.isActive():
node.tick()
except rospy.ROSInterruptException:
pass
from superros.logger import Logger
from rocup.storage.tf_storage import TfStorage
from superros.comm import RosNode
from rocup.proxy.proxy_message import SimpleMessage, SimpleMessageProxy
import message_filters
import rospkg
import numpy as np
import math
import sys
import random
#⬢⬢⬢⬢⬢➤ NODE
node = RosNode("tf_manager")
node.setupParameter("hz", 30)
node.setupParameter("world_tf", "world")
node.setupParameter("target_tf", "target")
node.setupParameter("storage_prefix", "saved_")
node.setupParameter("max_save_iterations", 30)
node.setHz(node.getParameter("hz"))
#⬢⬢⬢⬢⬢➤ variables
message_proxy = SimpleMessageProxy()
tf_storage = TfStorage()
all_tfs = []
publishing = True
def command_cb(message):
global current_tf_name, publishing, all_tfs
from geometry_msgs.msg import Twist
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")
#!/usr/bin/env python
import cv2
import argparse
import PyKDL
from superros.comm import RosNode
from baxterpkg.robot import BaxterRobot
from baxterpkg.cameras import BaxterCamera
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
# ROS node
node = RosNode("test")
node.setHz(node.setupParameter("hz", 1))
# camera
camera_right = BaxterCamera(node, limb=BaxterCamera.RIGHT)
camera_right.setting(resolution=(960, 600), exposure=100, gain=40)
# robot class
arm_right = BaxterRobot(node, limb=BaxterRobot.RIGHT)
Ftr_eef = PyKDL.Frame()
Ftr_eef.p = PyKDL.Vector(0.0, 0.0001, 0.0)
key = raw_input(".............. reset? [y]/n: ")
if str(key) not in ["n", "N", "no", "NO"]:
arm_right.reset()
while node.isActive():
#!/usr/bin/env python
# -*- encoding: utf-8 -*-
from superros.comm import RosNode
from std_msgs.msg import Float32
import math
node = RosNode("math_publisher_sin")
node.setHz(node.setupParameter("hz", 30))
topic_name = node.setupParameter("topic_name", "math_publisher_sin")
frequency = node.setupParameter("frequency", 1.0)
node.createPublisher(topic_name, Float32)
while node.isActive():
data = Float32(
math.sin(2 * math.pi * frequency * node.getCurrentTimeInSecs()))
node.getPublisher(topic_name).publish(data)
node.tick()
if path_node.type == 'base' or index == 0:
return path_node.frame
else:
return self.getFrame(index - 1) * path_node.frame
class PathNode(object):
def __init__(self, frame, tp):
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)
#!/usr/bin/env python
# -*- encoding: utf-8 -*-
from superros.comm import RosNode
from superros.cameras import CameraRGB
import cv2
node = RosNode("example_rgbcamera")
node.setHz(node.setupParameter("hz", 30))
rgb_topic = node.setupParameter("camera_topic", "")
# new frame callback
def newFrame(frame):
cv2.imshow("image", frame.rgb_image)
cv2.waitKey(1)
# camera Object
camera = CameraRGB(node,
rgb_topic=rgb_topic,
compressed_image='compressed' in rgb_topic)
camera.registerUserCallabck(newFrame)
while node.isActive():
node.tick()
def serviceCallback(msg):
"""Enable Service Callback
Arguments:
msg {BoolMsg} -- TRUE/FALSE
"""
global enabled
enabled = msg.data
response = SetBoolResponse()
response.success = True
return response
node = RosNode("ping")
node.setupParameter("hz", 30)
node.setHz(node.getParameter("hz"))
enabled = node.setupParameter("enabled", True)
# Creates a Float Publisher
output_publisher = node.createPublisher("~output", Float32)
# Creates an Enable Service
enable_service = node.createService("~enable", SetBool, serviceCallback)
# Remote Service
remote_name = "ping" if "pong" in node.getName() else "pong"
remote_enable_service = node.getServiceProxy(remote_name + "/enable", SetBool)
# start
start_time = 0.0
from sensor_msgs.msg import JointState, Image, CompressedImage, JointState
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')
from rocup.proxy.command_proxy import CommandProxyClient
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
import PyKDL
import tf
from rocup.proxy.command_proxy import CommandProxyClient
from superros.comm import RosNode
import message_filters
from sensor_msgs.msg import Image, CameraInfo
import cv2
import aruco
import rospkg
import numpy as np
import math
import json
#⬢⬢⬢⬢⬢➤ NODE
node = RosNode("command_proxy_example_client")
node.setupParameter("hz", 1)
node.setHz(node.getParameter("hz"))
counter = 0
active_command = None
def sendNewMessage():
global counter, active_command
counter += 1
command = "Garbage Command {}".format(counter)
#⬢⬢⬢⬢⬢➤ SEND COMMAND
print("Sending command: " + command)
active_command = command_proxy_client.sendCommand(command)
from superros.comm import RosNode
import superros.transformations as transformations
import os
import cv2
from cv_bridge import CvBridge, CvBridgeError
import cv_bridge
import numpy as np
import PyKDL
import glob
import collections
import shutil
#⬢⬢⬢⬢⬢➤ NODE
node = RosNode("topic_export_filtering")
node.setupParameter("hz", 30)
node.setHz(node.getParameter("hz"))
image_path = node.setupParameter("image_path", "")
pose_file = node.setupParameter("pose_file", "")
signal_file = node.setupParameter("signal_file", "")
output_path = node.setupParameter("output_path", "/tmp/topic_export_filtering/")
zfill = node.setupParameter("zfill", 5)
if not os.path.exists(output_path):
os.makedirs(output_path)
images = sorted(glob.glob(os.path.join(image_path, "*.*")))
pose_file = np.loadtxt(pose_file)
signal_file = np.loadtxt(signal_file)
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)
import PyKDL
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
import numpy as np
import rospy
from superros.logger import Logger
from rocup.param.global_parameters import Parameters
from superros.comm import RosNode
from sensor_msgs.msg import JointState
def command_callback(msg):
global joints
joints = msg.position
if __name__ == '__main__':
robot_name = Parameters.get("BONMET_NAME")
node = RosNode('joint_command_sequencer'.format(robot_name))
node_rate = Parameters.get(obj=robot_name, param="NODE_FREQUENCY")
node.setupParameter("hz", node_rate)
node.createSubscriber("/bonmetc60/joint_command", JointState, command_callback)
pub = node.createPublisher("/bonmetc60/joint_command_seq", JointState)
joints = [0, 0, 0, 0, 0, 0]
joints_msg = JointState()
while node.isActive():
joints_msg.position = joints
pub.publish(joints_msg)
node.tick()
from geometry_msgs.msg import Twist
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))
import os
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
self._send_command_result(True)
else:
Logger.error("INVALID input")
self._send_command_result(False)
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,