In [1]:
import socket
import sys
from collections import namedtuple
from struct import unpack, pack
import asyncore
import rospy
from std_msgs.msg import Float32, Float64, String, Header
from std_srvs.srv import Trigger, SetBool
import std_srvs.srv
import std_msgs.msg
from sensor_msgs.msg import JointState
In [2]:
Message = namedtuple('Message', ['position1','speed1','torque1','position2','speed2','torque2'])
control = [0.,0.,0.,0.,0.,0.]
rospy.init_node('ROS_PLC_bridge')
class HTTPClient(asyncore.dispatcher):
def __init__(self, host="192.168.1.120", port=8000, message_size=48):
self.pub1 = rospy.Publisher('/rail1_state', JointState, queue_size=10)
self.pub2 = rospy.Publisher('/rail2_state', JointState, queue_size=10)
asyncore.dispatcher.__init__(self)
self.message_size=message_size #message_size=48
self.create_socket()
self.connect( (host, port) ) #connect to port 8000, host 192.168.1.120
#self.buffer = bytes('testing','utf8')
self.counter=0
self.last_message=""
self.s_start = rospy.Service('start_stop', SetBool, self.start_stop_communication)
self.rail_subscriber1 = rospy.Subscriber("/rail1_control", JointState, self.control_rail1)
self.rail_subscriber2 = rospy.Subscriber("/rail2_control", JointState, self.control_rail2)
def __del__(self):
#print("trying to clean ")
self.s_start.shutdown()
self.s_mode.shutdown()
self.pub1.unregister()
self.pub2.unregister()
self.rail_subscriber1.unregister()
self.rail_subscriber2.unregister()
def start_stop_communication(self, message, host="192.168.1.120", port=8000, message_size=50):
'''this methond allows or stops communication between ROS and the PLC'''
if message.data:
print("Establishing communication")
try:
self.connect( (host, port) ) #connect to port 8000, host 192.168.1.120
return std_srvs.srv.SetBoolResponse(1, "Communication established")
except:
print("Communication failure")
return std_srvs.srv.SetBoolResponse(0, "Communication could not be established")
else:
print("Shutting down communication")
self.close()
self.create_socket()
return std_srvs.srv.SetBoolResponse(1, "Communication shutdown")
def control_rail1(self,message):
pos1=message.position[0]
vel1=message.velocity[0]
accel1=message.effort[0]
control[0]=pos1
control[1]=vel1
control[2]=accel1
self.send_control1(control)
def control_rail2(self,message):
pos2=message.position[0]
vel2=message.velocity[0]
accel2=message.effort[0]
control[3]=pos2
control[4]=vel2
control[5]=accel2
self.send_control2(control)
def send_control1(self,control):
control_sent=pack('dddddd',control[0],control[1],control[2],control[3],control[4],control[5])
self.send(control_sent)
def send_control2(self,control):
control_sent=pack('dddddd',control[0],control[1],control[2],control[3],control[4],control[5])
self.send(control_sent)
def handle_connect(self):
#print("connected")
pass
def decode_message(self,message):
'''this method decodes the message received through TCP/IP'''
if(len(message)==self.message_size):
touple_from_tcp=unpack('dddddd', message)
my_message=Message(*touple_from_tcp)
return my_message
return False
def handle_read(self):
'''this method receives a message through TCP/IP and publishes it to ROS topics'''
byte_message=self.recv(self.message_size)
self.last_message=self.decode_message(byte_message)
if(self.last_message):
try:
message1 = JointState()
message1.header = Header()
message1.header.stamp = rospy.Time.now()
message1.name = ["tor1"]
message1.position = [self.last_message.position1]
message1.velocity = [self.last_message.speed1]
message1.effort = [self.last_message.torque1]
self.pub1.publish(message1)
message2 = JointState()
message2.header = Header()
message2.header.stamp = rospy.Time.now()
message2.name = ["tor2"]
message2.position = [self.last_message.position2]
message2.velocity = [self.last_message.speed2]
message2.effort = [self.last_message.torque2]
self.pub2.publish(message2)
except:
print("Error")
self.counter+=1
In [3]:
client = HTTPClient('192.168.1.120',8000)
asyncore.loop()
In [ ]:
In [ ]:
asyncore.loop()
In [ ]:
#received_message=sock.recv(50)
#print(received_message)
In [ ]:
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
ip="192.168.1.120"
port=8000
# Connect the socket to the port where the server is listening
server_address = (ip,port)
print( 'connecting to %s port %s' % server_address)
somesocket=sock.connect(server_address)
#sock.sendall("testing".encode())
received_message=sock.recv(50)
In [ ]:
print(len(received_message))
In [ ]:
touple_from_tcp=unpack('ddddddcc', received_message)
touple_from_tcp
In [ ]:
Message = namedtuple('Message', ['position1','speed1','torque1','position2','speed2','torque2','first_letter','second_letter'])
my_message=Message(*touple_from_tcp)
list(my_message)
In [ ]:
my_message.position1
In [ ]:
client.socket.close()
In [ ]:
client.counter
In [ ]:
#!/usr/bin/env python
import rospy
def start_stop_communication(inside):
#start stop rail
print("inside service response")
print(inside)
return std_srvs.srv.SetBoolResponse(1,"we have suceeded")
In [ ]:
? std_srvs.srv.SetBoolResponse
In [ ]:
s_start.shutdown()
In [ ]:
s_start = rospy.Service('start_stop4', SetBool, start_stop_communication)
In [ ]:
trigger_experiment = rospy.ServiceProxy('start_stop4', SetBool)
In [ ]:
response=trigger_experiment(True)
In [ ]:
print(response.message)
print(response.success)
In [ ]:
sth=Trigger()
In [ ]:
print(sth.)
In [ ]:
? sth
In [ ]: