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()