import math
import time
import PyKDL
import random
from PyKDL import Frame, Vector, Rotation
from sensor_msgs.msg import JointState
from std_msgs.msg import String
from geometry_msgs.msg import Twist
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
if __name__ == '__main__':
node = RosNode("atift_manager_node")
node.setupParameter("hz", 250)
node.setHz(node.getParameter("hz"))
sensor_name = "atift"
sens = SensorManager(sensor_name)
node.createSubscriber("/atift", Twist, sens.sensor_callback)
try:
while node.isActive():
node.tick()
except rospy.ROSInterruptException:
pass
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
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:
#!/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():
from std_msgs.msg import Float64MultiArray, String
from sensor_msgs.msg import JointState, Image, CompressedImage
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))
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
import tf
import superros.transformations as transformations
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
#! /usr/bin/env python
# -*- coding: utf-8 -*-
#
import os
from PyQt4 import QtCore, QtGui, uic
from PyQt4.QtGui import *
import sys
from rocup.ui.pyqt import PyQtWindow
from superros.comm import RosNode
#⬢⬢⬢⬢⬢➤ NODE
node = RosNode("ui_window_example")
ui_file = node.getFileInPackage("rocup", "data/ui/main.ui")
w = PyQtWindow(uifile=ui_file)
w.run()
import message_filters
from std_msgs.msg import Float64MultiArray, Int32
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"),
#!/usr/bin/env python
# -*- encoding: utf-8 -*-
import rospy
from superros.comm import RosNode
#⬢⬢⬢⬢⬢➤ NODE
node = RosNode("tf_export_test")
node.setupParameter("hz", 30)
node.setHz(node.getParameter("hz"))
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
#!/usr/bin/env python
# -*- encoding: utf-8 -*-
from superros.comm import RosNode
from visualization_msgs.msg import MarkerArray
from superros.draw import VisualizationScene, Color
import PyKDL
import math
node = RosNode("example_3dscene")
node.setHz(node.setupParameter("hz", 30))
pub = node.createPublisher("test", MarkerArray)
# Visualization Scene
scene = VisualizationScene(node, "test")
scene.createCone(name="cone1", color=Color.pickFromPalette(0))
scene.createCone(name="cone2", color=Color.pickFromPalette(1))
scene.createCube(
name="cube1",
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)
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
from PyKDL import Frame, Vector, Rotation
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)
#!/usr/bin/env python
# -*- encoding: utf-8 -*-
from superros.comm import RosNode
from std_msgs.msg import Float32
node = RosNode("amplifier")
node.setHz(node.setupParameter("hz", 30))
node.createBufferedSubscriber("in", Float32)
node.createPublisher("out", Float32)
while node.isActive():
data = Float32(node.getBufferedData("in").data * 2.0)
node.getPublisher("out").publish(data)
node.tick()
#!/usr/bin/env python
# -*- encoding: utf-8 -*-
import rospy
from superros.comm import RosNode
from rocup.taskmanager.task_manager_state_machine import TaskManagerSM
if __name__ == '__main__':
node = RosNode("task_manager_template")
node.setupParameter("hz", 50)
node.setHz(node.getParameter("hz"))
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ PARAMETERS ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
task_name = "task_manager_template"
robot_list = []
sensor_list = []
subtask_list = []
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ INSTRUCTIONS LIST ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
instruction_list = ["system sleep 2"]
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
tskm = TaskManagerSM(task_name)
tskm.start(robot_list, sensor_list, instruction_list, subtask_list)
#!/usr/bin/env python
# -*- encoding: utf-8 -*-
import math
import cv2
import numpy as np
from superros.comm import RosNode
from superros.cameras import CameraRGBD
# Node Creation
node = RosNode("test_rgbdepth_camera")
node.setHz(node.setupParameter("hz", 30))
def imageCallback(frame):
"""Callback called every new FrameRGBD. Each frame contains both
an RGB image (Color 8UC3) along with a Depth image (Gray 16UC1)
Arguments:
frame {FrameRGBD} -- A Frame with RGB+Depth information
"""
# Converts RGB image to Grayscale and float (values between 0.0 and 1.0)
gray = cv2.cvtColor(frame.rgb_image, cv2.COLOR_BGR2GRAY)
gray = gray.astype(float) / 255.0
# Converts Depth image for visualization purposes (values ~ between 0.0 and 1.0)
depth = frame.depth_image.astype(float) / 8000.0
# Stacks images horizontally
whole = np.hstack((gray, depth))
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 rospkg
import numpy as np
import math
import json
from rocup.utils.tfmatrix import TfMatrixCube, TfMatrixHyperCube, TfMatrixSphere
from tf_matricies import getTfMatrix
#⬢⬢⬢⬢⬢➤ NODE
node = RosNode("matrix_tf_generator")
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",
import tf
from rocup.proxy.command_proxy import CommandProxyClient
from rocup.proxy.proxy_message import SimpleMessage
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
#⬢⬢⬢⬢⬢➤ NODE
node = RosNode("supervisor_proxy_test_2")
node.setupParameter("hz", 60)
node.setHz(node.getParameter("hz"))
pose_pub = node.createPublisher("/robot_pose", Pose)
counter = 0
active_command = None
def sendNewMessage():
global counter
if counter >= len(msgs):
return
command = msgs[counter % len(msgs)]
counter += 1
#!/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()
def getFrame(self, index):
path_node = self.nodes[index]
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")
#!/usr/bin/env python
# -*- encoding: utf-8 -*-
from superros.comm import RosNode
from std_msgs.msg import Float32
import math
node = RosNode("sin")
node.setHz(node.setupParameter("hz", 30))
node.createPublisher("in", Float32)
node.createPublisher("in2", Float32)
node.createPublisher("in3", Float32)
while node.isActive():
data = Float32(math.sin(2 * 3.14 * 1.0 * node.getCurrentTimeInSecs()))
data2 = Float32(math.sin(2 * 3.14 * 2.0 * node.getCurrentTimeInSecs()))
data3 = Float32(math.sin(2 * 3.14 * 3.0 * node.getCurrentTimeInSecs()))
node.getPublisher("in").publish(data)
node.getPublisher("in2").publish(data2)
node.getPublisher("in3").publish(data3)
node.tick()
#!/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()
import tf
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):
from sensor_msgs.msg import JointState
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
import cv_bridge
import numpy as np
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)
from std_msgs.msg import Float64MultiArray, String
from sensor_msgs.msg import JointState, Image, CompressedImage
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)
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()
# ------------------------------------------------------------------------------------------
# PARAMETERS
CAMERA_CALIBRATION_FILE = "camera_calibration.json"
# object plane coefficients
TABLE_HEIGHT = -0.2075 # fixed z value
GRIPPER_HEIGHT = -0.19
PLANE_COEFFS = [0, 0, -1, TABLE_HEIGHT]
# default poses
TABLE_TOP_POSE = Pose(position=Point(x=0.60, y=-0.45, z=-0.05),
orientation=Quaternion(x=1.0, y=0.0, z=0.0, w=0.0))
# ROS node
node = RosNode("openloopvs")
node.setHz(node.setupParameter("hz", 30))
# CAMERA CLASS
cameraparasm = BaxterParamsServer(CAMERA_CALIBRATION_FILE)
leftcameraparams = cameraparasm.getParam("right")
camera_right = BaxterCamera(node,
limb=BaxterCamera.RIGHT,
camera_matrix=np.array(
leftcameraparams["camera_matrix"]))
camera_right.setting(resolution=(960, 600), exposure=30, gain=60)
# ROBOT CLASS
arm_right = BaxterMoveit(node, limb=BaxterMoveit.RIGHT)
arm_right.scene.remove_world_object("table")
import message_filters
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
#! /usr/bin/env python
# -*- coding: utf-8 -*-
#
import os
from PyQt4 import QtCore, QtGui, uic
from PyQt4.QtGui import *
import sys
from rocup.ui.uiproxy import UiProxy
from superros.comm import RosNode
import math
#⬢⬢⬢⬢⬢➤ NODE
node = RosNode("ui_slave_example")
node.setHz(2)
flag1 = True
# Receive Data From Ui
def eventCallback(evt):
global flag1
print("*** New Event Received ***")
print(evt)
if evt.name == "flag1":
flag1 = evt.value > 0
ui_proxy = UiProxy()
