You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
54 lines
1.8 KiB
54 lines
1.8 KiB
#!/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}}"
|