#!/usr/bin/env python2
import rospy

from std_msgs.msg import String
from std_msgs.msg import Float32MultiArray
from std_msgs.msg import MultiArrayLayout
import sys
sys.path.append ('./python')
import sim
from sim import *

actCmd = list()

def callback(data):
    if (len(data.data)!=0):
        rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
        actCmd = list(data.data)
        # print(actCmd)
    	res, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'setRobotVel', [],actCmd, [], '',sim.simx_opmode_blocking)


def listener():
    sub = rospy.Subscriber('act_cmd', Float32MultiArray, callback)

    rospy.init_node('listener', anonymous=False)

    rate = rospy.Rate(250)

    while not rospy.is_shutdown():
        rate.sleep()

if __name__ == '__main__':
	print('Vrep Sim Program started 2')
	sim.simxFinish(-1)  # just in case, close all opened connections
	clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)  # Connect to CoppeliaSim
	if clientID != -1:
		print('Connected to remote API server 2')
		# Now try to retrieve data in a blocking fashion (i.e. a service call):
		res, objs = sim.simxGetObjects(clientID, sim.sim_handle_all, sim.simx_opmode_blocking)
		if res == sim.simx_return_ok:
			print('Number of objects in the scene 2: ', len(objs))
		else:
			print('Remote API function call returned with error code: ', res)
		# time.sleep(2)
	else:
		print('Failed connecting to remote API server')
	listener()

#
# rostopic pub -1 /act_cmd std_msgs/Float32MultiArray -- "{\"data\":[0.30720001459121704,0.00019999999494757503],\"layout\":{\"dim\":[],\"data_offset\":0}}"


# "{\"data\":[0.30720001459121704,0.00019999999494757503],\"layout\":{\"dim\":[],\"data_offset\":0}}"