ROS - PLC bridge__5



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','first_letter','second_letter'])
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=50): 

        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=50
        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.s_mode = rospy.Service('change_mode', SetBool, self.change_mode)
        
        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 :(")
                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 change_mode(self,message):
        #if message.data:
            #return std_srvs.srv.SetBoolResponse(1, "Mode changed to option 1")
        #else:
            #return std_srvs.srv.SetBoolResponse(0, "Mode changed to option 0")
     
    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('ddddddcc', 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(":-(")
        self.counter+=1

In [ ]:
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 [ ]: