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
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ SUPERVISOR COMUNIOCATION ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
def goToTf(tf_name, tool="camera"):
#⬢⬢⬢⬢⬢➤ Go To Tf Helper Function
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 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()
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
message = SimpleMessage(receiver="comau_smart_six_supervisor",
command="gotoshape")
message.setData("shape_name", command)
print("Sending", command)
#!/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 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)
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
start_offset = 0.0 if "ping" in node.getName() else math.pi
# Main Loop
while node.isActive():
#!/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 std_msgs.msg import String
def topic_collback(msg):
print msg.data
#⬢⬢⬢⬢⬢➤ NODE
node = RosNode("tf_export_test")
node.setupParameter("hz", 30)
node.setHz(node.getParameter("hz"))
topic_pub = node.createPublisher("/topic_1", String)
node.createSubscriber("/topic_2", String, topic_collback)
while node.isActive():
msg = String()
msg.data = "hello world"
topic_pub.publish(msg)
node.tick()
#!/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()
