Code: Select all
function sysCall_init ()
jointHandles={-1,-1,-1,-1,-1,-1}
jointHandles[1]=sim.getObjectHandle('joint_1')
jointHandles[2]=sim.getObjectHandle('joint_2')
jointHandles[3]=sim.getObjectHandle('joint_3')
jointHandles[4]=sim.getObjectHandle('joint_4')
jointHandles[5]=sim.getObjectHandle('joint_5')
jointHandles[6]=sim.getObjectHandle('joint_6')
if simROS then
print ("ROS INTERFACE WAS FOUND")
--enable joint subscriber
sub1=simROS.subscribe('/kukakr16/joint_1/ctrl', "std_msgs/Float64", "callback1")
simROS.subscriberTreatUInt8ArrayAsString(sub1)
sub2=simROS.subscribe('/kukakr16/joint_a2/ctrl', "std_msgs/Float64","callback2")
simROS.subscriberTreatUInt8ArrayAsString(sub2)
sub3=simROS.subscribe('/kukakr16/joint_3/ctrl', "std_msgs/Float64","callback3")
simROS.subscriberTreatUInt8ArrayAsString(sub3)
sub4=simROS.subscribe('/kukakr16/joint_4/ctrl', "std_msgs/Float64","callback4")
simROS.subscriberTreatUInt8ArrayAsString(sub4)
sub5=simROS.subscribe('/kukakr16/joint_5/ctrl', "std_msgs/Float64","callback5")
simROS.subscriberTreatUInt8ArrayAsString(sub5)
sub6=simROS.subscribe('/kukakr16/joint_6/ctrl', "std_msgs/Float64","callback6")
simROS.subscriberTreatUInt8ArrayAsString(sub6)
else
print ("ROS INTERFACE WAS NOT FOUND")
end
end
function callback1 (config)
if config then
sim.setJointTargetPosition(jointHandles[1],config.data)
end
end
function callback2 (config)
if config then
sim.setJointTargetPosition(jointHandles[2],config.data)
end
end
function callback3 (config)
if config then
sim.setJointTargetPosition(jointHandles[3],config.data)
end
end
function callback4 (config)
if config then
sim.setJointTargetPosition(jointHandles[4],config.data)
end
end
function callback5 (config)
if config then
sim.setJointTargetPosition(jointHandles[5],config.data)
end
end
function callback6 (config)
if config then
sim.setJointTargetPosition(jointHandles[6],config.data)
end
end
function callback7 (config)
if config then
sim.setJointTargetPosition(gripper_roll_handle,config.data)
end
end