Compare commits

..

No commits in common. "180bbe225f7e6f65528fd0f8127f3ec7466b0d0e" and "c041ac26f0ca013f2944e85f1285b47ac739392f" have entirely different histories.

2 changed files with 6 additions and 73 deletions

View file

@ -6,7 +6,6 @@ from geometry_msgs.msg import Quaternion
from sensor_msgs.msg import JointState from sensor_msgs.msg import JointState
from tf2_ros import TransformBroadcaster, TransformStamped from tf2_ros import TransformBroadcaster, TransformStamped
import socket import socket
import struct
class StatePublisher(Node): class StatePublisher(Node):
@ -39,18 +38,16 @@ class StatePublisher(Node):
cli.settimeout(0) cli.settimeout(0)
cli.setblocking(0) cli.setblocking(0)
print(cli, addr) print(cli, addr)
self.prev_time = time.time()
while rclpy.ok(): while rclpy.ok():
try: try:
msg = cli.recv(2048) msg = cli.recv(4096)
if msg == b'': if msg == b'':
cli.close()
print('bailing') print('bailing')
break break
print(repr(msg)) # print(repr(msg))
if len(msg) >= 13 and msg[12] == 10: if len(msg) >= 7 and msg[6] == 10:
self.cmd_joints = struct.unpack('hhhhhh', msg[:-1]) self.cmd_joints = [msg[0], msg[1], msg[2], msg[3], msg[4], msg[5]]
self.cmd_joints = [a / 180. * pi for a in self.cmd_joints] self.cmd_joints = [a / 180. * pi for a in self.cmd_joints]
except BlockingIOError as e: except BlockingIOError as e:
pass pass

View file

@ -1,70 +1,6 @@
import rclpy def main():
from rclpy.node import Node print('Hi from net_robot.')
from rclpy.qos import QoSProfile
from std_msgs.msg import Float32
import socket
import struct
class NetRobot(Node):
def __init__(self):
super().__init__('state_publisher')
qos = QoSProfile(depth=10)
self.create_subscription(Float32, '~/drives/j1', self._on_j1, qos)
self.create_subscription(Float32, '~/drives/j2', self._on_j2, qos)
self.create_subscription(Float32, '~/drives/j3', self._on_j3, qos)
self.create_subscription(Float32, '~/drives/j4', self._on_j4, qos)
self.create_subscription(Float32, '~/drives/j5', self._on_j5, qos)
self.create_subscription(Float32, '~/drives/j6', self._on_j6, qos)
self._sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# self._sock.connect(('tmuller.xyz', 9999))
self._sock.connect(('127.0.0.1', 9999))
self._joints = [0.] * 6
self._prev_joints = [0.] * 6
def _send(self):
if self._joints != self._prev_joints:
self._sock.send(struct.pack('hhhhhh', *[int(j) for j in self._joints]) + b'\n')
self._prev_joints = self._joints.copy()
def _on_j1(self, msg):
self._joints[0] = msg.data
self._send()
def _on_j2(self, msg):
self._joints[1] = msg.data
self._send()
def _on_j3(self, msg):
self._joints[2] = msg.data
self._send()
def _on_j4(self, msg):
self._joints[3] = msg.data
self._send()
def _on_j5(self, msg):
self._joints[4] = msg.data
self._send()
def _on_j6(self, msg):
self._joints[5] = msg.data
self._send()
def main(args=None):
rclpy.init(args=args)
node = NetRobot()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__': if __name__ == '__main__':
main() main()