def talker():
torso_pub = rospy.Publisher('/torso_controller/command', Float64MultiArray, queue_size=10)
left_leg_pub = rospy.Publisher('/left_leg_controller/command', Float64MultiArray, queue_size=10)
right_leg_pub = rospy.Publisher('/right_leg_controller/command', Float64MultiArray, queue_size=10)
rospy.init_node('publish_to_controller', anonymous=True)
rate = rospy.Rate(1)
msg = Float64MultiArray()
# hipyaw, hiproll, hippitch, kneepitch, anklepitch, ankleroll
msg.data = [0.0, 0.0, -0.63, 1.03, -0.44, 0.0]
# msg.data = [0.0, 0.0, 0., 0.0, 0., 0.0]
dim = MultiArrayDimension()
dim.size = len(msg.data)
dim.label = "command"
dim.stride = len(msg.data)
msg.layout.dim.append(dim)
msg.layout.data_offset = 0
torso_msg = Float64MultiArray()
torso_msg.data = [0.0, 0.2, 0.0]
dim = MultiArrayDimension()
dim.size = len(torso_msg.data)
dim.label = "command"
dim.stride = len(torso_msg.data)
torso_msg.layout.dim.append(dim)
torso_msg.layout.data_offset = 0
while not rospy.is_shutdown():
print msg
left_leg_pub.publish(msg)
right_leg_pub.publish(msg)
torso_pub.publish(torso_msg)
rate.sleep()
def __init__(self):
# Define the dimensions of the message
rightHandGoalDim = MultiArrayDimension()
rightHandGoalDim.size = NUM_DOFS
rightHandGoalDim.label = "rightHandGoal"
rightHandGoalDim.stride = 1
# Define the goal messages
self.rightHandGoalMsg = Float64MultiArray()
for ii in range(0, NUM_DOFS):
self.rightHandGoalMsg.data.append(0)
self.rightHandGoalMsg.layout.dim.append(rightHandGoalDim)
self.rightHandGoalMsg.layout.data_offset = 0
leftHandGoalDim = MultiArrayDimension()
leftHandGoalDim.size = NUM_DOFS
leftHandGoalDim.label = "leftHandGoal"
leftHandGoalDim.stride = 1
self.leftHandGoalMsg = Float64MultiArray()
for ii in range(0, NUM_DOFS):
self.leftHandGoalMsg.data.append(0)
self.leftHandGoalMsg.layout.dim.append(leftHandGoalDim)
self.leftHandGoalMsg.layout.data_offset = 0
def __init__(self):
# Define the dimensions of the message
rightHandGoalDim = MultiArrayDimension()
rightHandGoalDim.size = NUM_DOFS
rightHandGoalDim.label = "rightHandGoal"
rightHandGoalDim.stride = 1
# Define the goal messages
self.rightHandGoalMsg = Float64MultiArray()
for ii in range(0, NUM_DOFS):
self.rightHandGoalMsg.data.append(0)
self.rightHandGoalMsg.layout.dim.append(rightHandGoalDim)
self.rightHandGoalMsg.layout.data_offset = 0
leftHandGoalDim = MultiArrayDimension()
leftHandGoalDim.size = NUM_DOFS
leftHandGoalDim.label = "leftHandGoal"
leftHandGoalDim.stride = 1
self.leftHandGoalMsg = Float64MultiArray()
for ii in range(0, NUM_DOFS):
self.leftHandGoalMsg.data.append(0)
self.leftHandGoalMsg.layout.dim.append(leftHandGoalDim)
self.leftHandGoalMsg.layout.data_offset = 0
def default(self, ci='unused'):
arrayList = []
arrayList.append(self.data['w_fl'])
arrayList.append(self.data['w_fr'])
arrayList.append(self.data['w_rl'])
arrayList.append(self.data['w_rr'])
dim_fl = MultiArrayDimension()
dim_fl.size = sys.getsizeof(self.data['w_fl'])
dim_fl.label = 'FrontLeftWheelSpeed'
dim_fl.stride = sys.getsizeof(self.data['w_fl'])
dim_fr = MultiArrayDimension()
dim_fr.size = sys.getsizeof(self.data['w_fr'])
dim_fr.label = 'FrontRightWheelSpeed'
dim_fr.stride = sys.getsizeof(self.data['w_fr'])
dim_rl = MultiArrayDimension()
dim_rl.size = sys.getsizeof(self.data['w_rl'])
dim_rl.label = 'RearLeftWheelSpeed'
dim_rl.stride = sys.getsizeof(self.data['w_rl'])
dim_rr = MultiArrayDimension()
dim_rr.size = sys.getsizeof(self.data['w_rr'])
dim_rr.label = 'RearRightWheelSpeed'
dim_rr.stride = sys.getsizeof(self.data['w_rr'])
wheelspeed = Float64MultiArray()
wheelspeed.data = arrayList
wheelspeed.layout.dim.append(dim_fl)
wheelspeed.layout.dim.append(dim_fr)
wheelspeed.layout.dim.append(dim_rl)
wheelspeed.layout.dim.append(dim_rr)
wheelspeed.layout.data_offset = 0
self.publish(wheelspeed)
def __request_callback(self, msg):
'''
Callback if image is received, faces are detected and
added to the global array of people
A message is sent with the ids, locations and face widths
in a multiarray.
'''
cv_image = self.bridge.imgmsg_to_cv2(msg, 'rgb8')
if SHOW_RECEIVED_IMAGE:
cv2.imwrite('rec_img.png', cv_image)
ret = self.fr.recognize_people(cv_image)
simple_msg = Float32MultiArray()
if ret:
print('new face detected')
locs, ids = ret
for i, loc in zip(ids, locs):
if i in self.recognized_ids:
index = self.recognized_ids.index(i)
self.recognized_locs[index] = loc
self.recognized_times[index] = datetime.datetime.now()
else:
self.recognized_ids.append(i)
self.recognized_locs.append(loc)
self.recognized_times.append(datetime.datetime.now())
else:
print('No faces detected')
# Publish the computed info on the simple_people topic
# as a multiarray. The structure is defined as [id, xpos, ypos, width]
# in a repeated fashion for multiple faces.
# The width might be used as a metric for how far the face is away
dim = MultiArrayDimension()
dim.label = 'id_and_pos'
dim.size = 3
dim.stride = 0
dim2 = MultiArrayDimension()
dim2.label = 'length_faces'
dim2.size = len(self.recognized_ids)
dim2.stride = 0
simple_msg.layout.dim = [dim, dim2]
simple_msg.layout.data_offset = 0
data = []
# Add the recognized faces around the table to the message
for i, loc in zip(self.recognized_ids, self.recognized_locs):
width = self.__get_width(loc)
x, y, valid = self.__calculate_people_positions(width)
data = np.concatenate((data, [i, x, y, width]), axis=0)
simple_msg.data = data
print(simple_msg)
# Publish the message
self.recognition_pub.publish(simple_msg)
# Check if person is seen in the last TIME_BEFORE_FORGOTTEN seconds and otherwise remove
for index, time in enumerate(self.recognized_times):
if abs((time - datetime.datetime.now()
).total_seconds()) > TIME_BEFORE_FORGOTTEN:
del self.recognized_ids[index]
del self.recognized_locs[index]
del self.recognized_times[index]
def __init__(self):
pub1 = rospy.Publisher('tactile', UBI0All)
pub2 = rospy.Publisher('palm_extras', Float64MultiArray)
pub3 = rospy.Publisher('tactile_mid_prox', MidProxDataAll)
pub4 = rospy.Publisher('tactile_aux_spi', AuxSpiData)
ubiall = UBI0All()
ubiall.tactiles = []
ubi = UBI0()
ubi.distal = [
200, 300, 200, 300, 400, 500, 600, 700, 800, 900, 1000, 900
]
midproxall = MidProxDataAll()
midproxall.sensors = []
midprox = MidProxData()
midprox.middle = [1500, 2400, 0, 0]
midprox.proximal = [100, 400, 800, 0]
for iFinger in range(0, 5):
ubiall.tactiles.append(ubi)
midproxall.sensors.append(midprox)
palmextras = Float64MultiArray()
dim = MultiArrayDimension()
dim.label = "accelerometer"
dim.size = 3
palmextras.layout.dim.append(dim)
dim = MultiArrayDimension()
dim.label = "gyrometer"
dim.size = 3
palmextras.layout.dim.append(dim)
dim = MultiArrayDimension()
dim.label = "analog_inputs"
dim.size = 4
palmextras.layout.dim.append(dim)
palmextras.data = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 250.0, 550.0, 850.0, 1150.0
]
auxspi = AuxSpiData()
auxspi.sensors = [
100, 400, 700, 1000, 1300, 1700, 2000, 2300, 100, 200, 300, 400,
500, 600, 700, 800
]
rospy.init_node('agni_tactile_test_sensors')
r = rospy.Rate(100) # 100hz
while not rospy.is_shutdown():
pub1.publish(ubiall)
pub2.publish(palmextras)
pub3.publish(midproxall)
pub4.publish(auxspi)
r.sleep()
def __init__(self):
pub1 = rospy.Publisher('tactile', UBI0All)
pub2 = rospy.Publisher('palm_extras', Float64MultiArray)
pub3 = rospy.Publisher('tactile_mid_prox', MidProxDataAll)
pub4 = rospy.Publisher('tactile_aux_spi', AuxSpiData)
ubiall = UBI0All()
ubiall.tactiles=[]
ubi= UBI0()
ubi.distal = [200, 300, 200, 300, 400, 500, 600, 700, 800, 900, 1000, 900]
midproxall = MidProxDataAll()
midproxall.sensors=[]
midprox = MidProxData()
midprox.middle = [1500, 2400, 0, 0]
midprox.proximal = [100, 400, 800, 0]
for iFinger in range(0,5):
ubiall.tactiles.append(ubi)
midproxall.sensors.append(midprox)
palmextras = Float64MultiArray()
dim = MultiArrayDimension()
dim.label = "accelerometer"
dim.size = 3
palmextras.layout.dim.append(dim)
dim = MultiArrayDimension()
dim.label = "gyrometer"
dim.size = 3
palmextras.layout.dim.append(dim)
dim = MultiArrayDimension()
dim.label = "analog_inputs"
dim.size = 4
palmextras.layout.dim.append(dim)
palmextras.data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 250.0, 550.0,850.0, 1150.0]
auxspi = AuxSpiData()
auxspi.sensors = [100, 400, 700, 1000, 1300, 1700, 2000, 2300, 100, 200, 300, 400, 500, 600, 700, 800]
rospy.init_node('agni_tactile_test_sensors')
r = rospy.Rate(100) # 100hz
while not rospy.is_shutdown():
pub1.publish(ubiall)
pub2.publish(palmextras)
pub3.publish(midproxall)
pub4.publish(auxspi)
r.sleep()
def __init__(self):
rospy.init_node('save_hitting_sound', anonymous=True)
self.length = rospy.get_param('/mini_microphone/length')
self.rate = rospy.get_param('/mini_microphone/rate')
self.cutoff_rate = rospy.get_param('~cutoff_rate')
self.wave_strength_thre = 2.0
self.visualize_data_length = min(
int(self.length * self.cutoff_rate / self.rate), self.length / 2)
self.time_to_listen = rospy.get_param('~time_to_listen')
self.queue_size = int(self.time_to_listen * (self.rate / self.length))
self.wave_spec_queue = np.zeros(
(self.queue_size,
self.visualize_data_length)) # remove folding noise
# publish
self.hitting_sound_pub = rospy.Publisher(
'/mini_microphone/hitting_sound', Float32MultiArray)
self.sound_image_pub = rospy.Publisher('/mini_microphone/sound_image',
Image)
self.hit_sound_image_pub = rospy.Publisher(
'/mini_microphone/hit_sound_image', Image)
self.hitting_sound = Float32MultiArray()
self.hitting_sound.layout = []
dim = MultiArrayDimension()
dim.label = 'time'
dim.size = self.queue_size
dim.stride = self.queue_size * self.length
self.hitting_sound.layout.append(dim)
dim.label = 'wave_spec'
dim.size = self.length
dim.stride = self.length
self.hitting_sound.layout.append(dim)
self.bridge = CvBridge()
self.count_from_last_hitting = 0
# subscribe
rospy.Subscriber('/mini_microphone/sound_spec', Float32MultiArray,
self.sound_spec_cb)
# save data
self.save_image = rospy.get_param('~save_image')
self.save_spectrum = rospy.get_param('~save_spectrum')
self.target_class = rospy.get_param('~target_class',
'unspecified_data')
self.save_dir = osp.join(os.environ['HOME'], 'hitting_sound_data')
self.spectrum_save_dir = osp.join(self.save_dir, 'spectrum',
self.target_class)
if self.save_spectrum and not os.path.exists(self.spectrum_save_dir):
os.makedirs(self.spectrum_save_dir)
self.image_save_dir = osp.join(self.save_dir, 'image', 'origin',
self.target_class)
if self.save_image and not os.path.exists(self.image_save_dir):
os.makedirs(self.image_save_dir)
def callback(msg,prevMarkers):
detectedMarkers = msg.markers
now = rospy.get_time() #the current time in seconds
for detectedMarker in detectedMarkers:
measuredTime = detectedMarker.header.stamp.secs
markerID = detectedMarker.id
prevMarkers[markerID] = measuredTime
detected_markers = []
for marker in prevMarkers.keys():
if abs(prevMarkers[marker]-now) > 5: #if the measurement has been stale for 5 seconds
del prevMarkers[marker]
else:
detected_markers.append(marker)
array1 = MultiArrayDimension()
array1.label = 'numMarkers'
array1.size = len(detected_markers)
array1.size = len(detected_markers)
layout = MultiArrayLayout()
layout.dim = [array1,]
layout.data_offset = 0
msg = Int16MultiArray()
msg.layout = layout
msg.data = detected_markers
pub.publish(msg)
def numpyArray2MultiArray(na, ma):
s = na.shape
for i in np.arange(len(s)):
d = MultiArrayDimension()
d.size = s[i]
ma.layout.dim.append(d)
ma.data = np.reshape(na, -1)
def publish(self):
'''
'''
PathMsg = PathDisplay()
PathMsg.xGlbPath = self.dataPlanning['xGlbPath']
PathMsg.yGlbPath = self.dataPlanning['yGlbPath']
# different from the usage in std_msgs
dim = MultiArrayDimension()
dim.stride = int(self.dataPlanning['indOptimal'])
dim.size = self.dataPlanning['pathCandidate'].shape[0]
PathMsg.xLocPath.data = np.array(self.dataPlanning['pathCandidate'][:,0]).flatten()
PathMsg.xLocPath.layout.dim = [dim]
PathMsg.yLocPath.data = np.array(self.dataPlanning['pathCandidate'][:,1]).flatten()
PathMsg.yLocPath.layout.dim = [dim]
PathMsg.header.stamp = rospy.Time.now()
basePathPub.publish(PathMsg)
PlanMsg = PlanOP()
PlanMsg.prePoint.x = self.prePoint[0]
PlanMsg.prePoint.y = self.prePoint[1]
PlanMsg.prePoint.z = 0.
PlanMsg.xCtrPath = self.dataPlanning['xCtrPath']
PlanMsg.yCtrPath = self.dataPlanning['yCtrPath']
PlanMsg.distObst = self.dataPlanning['distObst']
PlanMsg.desireDirDrive = self.desireDirDrive
PlanMsg.curDirDrive = self.curDirDrive
PlanMsg.flagIsShifting = self.flagIsShifting
PlanMsg.distYClose = self.distYClose
PlanMsg.modeDrive = self.modeDrive
PlanMsg.vbrMode = self.vbrMode
PlanMsg.startedFlag = self.toCloudStartedFlag
PlanMsg.endTaskFlag = self.toCloudEndTaskFlag
PlanMsg.header.stamp = rospy.Time.now()
planPub.publish(PlanMsg)
def __init__(self, rosTopic, minAmplitude, maxAmplitude, numDoFs, period):
"""
The constructor.
Keyword arguments:
rosTopic -- The ROS topic on which to publish the goal.
minAmplitude -- The minimum amplitude in radians.
maxAmplitude -- The maximum amplitude in radians.
numDoFs -- The number of DoFs.
period -- The period of the square wave in seconds.
"""
self.period = period
self.rosTopic = rosTopic
# Define the dimensions of the message
dim = MultiArrayDimension()
dim.size = numDoFs
dim.label = "goalMsg"
dim.stride = 1
# Define the goal message containing the upper bound of the square wave
self.goalMsgHigh = Float64MultiArray()
for ii in range(0, numDoFs):
self.goalMsgHigh.data.append(maxAmplitude)
self.goalMsgHigh.layout.dim.append(dim)
self.goalMsgHigh.layout.data_offset = 0
# Define the goal message containing the lower bound of the square wave
self.goalMsgLow = Float64MultiArray()
for ii in range(0, numDoFs):
self.goalMsgLow.data.append(minAmplitude)
self.goalMsgLow.layout.dim.append(dim)
self.goalMsgLow.layout.data_offset = 0
def publish(self):
#pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=10)
pub = rospy.Publisher('remote_readings',
Float64MultiArray,
queue_size=1)
rate = rospy.Rate(50) #10Hz
a = [0.0, 0.0]
dim = MultiArrayDimension()
dim.size = len(a)
dim.label = "REMOTEmsg"
dim.stride = len(a)
apub = Float64MultiArray()
apub.data = a
apub.layout.dim.append(dim)
apub.layout.data_offset = 0
while not rospy.is_shutdown():
#UART_read = ser.readline()
#UART_read = ser.Serial.readline()
UART_read = self.dev.readline()
steer_cmd = int(UART_read[0:4])
throt_cmd = int(UART_read[4:8])
a = [throt_cmd, steer_cmd]
rospy.loginfo(a)
apub.data = a
pub.publish(apub)
rate.sleep()
def publish(self):
# compose the multiarray message
jointVelocities = Float32MultiArray()
myLayout = MultiArrayLayout()
myMultiArrayDimension = MultiArrayDimension()
myMultiArrayDimension.label = "joint_velocities"
myMultiArrayDimension.size = 1
myMultiArrayDimension.stride = self.numOfWheels
myLayout.dim = [myMultiArrayDimension]
myLayout.data_offset = 0
jointVelocities.layout = myLayout
if rospy.get_time() - self.lastTwistTime < self.timeout:
self.right = 1.0 * self.linearVelocity + self.angularVelocity * self.baseWidth / 2
self.left = 1.0 * self.linearVelocity - self.angularVelocity * self.baseWidth / 2
rospy.loginfo("wheels velocities vl, vr [{0:.3f},{1:.3f}]".format(self.left, self.right))
if self.numOfWheels == 2:
# first item is left and second is right
jointVelocities.data = [self.left, self.right]
elif self.numOfWheels == 4:
jointVelocities.data = [self.left, self.right, self.left, self.right]
else:
if self.numOfWheels == 2:
jointVelocities.data = [0.0, 0.0]
# first item is left and second is right
elif self.numOfWheels == 4:
jointVelocities.data = [0.0, 0.0, 0.0, 0.0]
self.joint_velocities_Pub.publish(jointVelocities)
def publish(self):
#pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=10)
pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=1)
rate = rospy.Rate(50) #10Hz
a = [0.0, 0.0]
dim = MultiArrayDimension()
dim.size = len(a)
dim.label = "REMOTEmsg"
dim.stride = len(a)
apub = Float64MultiArray()
apub.data = a
apub.layout.dim.append(dim)
apub.layout.data_offset = 0
while not rospy.is_shutdown():
#UART_read = ser.readline()
#UART_read = ser.Serial.readline()
UART_read = self.dev.readline()
steer_cmd = int(UART_read[0:4])
throt_cmd = int(UART_read[4:8])
a = [throt_cmd, steer_cmd]
rospy.loginfo(a)
apub.data = a
pub.publish(apub)
rate.sleep()
def enableGravityCompMode(self):
index = raw_input("Put robot into gravity compensation mode? Y/n\n")
if index == "N" or index == "n":
return True
else:
# Define the dimensions of the message
dim = MultiArrayDimension()
dim.size = NUM_DOFS
dim.label = "goalMsg"
dim.stride = 1
kpMsg = Float64MultiArray()
for ii in range(0, NUM_DOFS):
kpMsg.data.append(0) # Kp gains for gravity compensation mode
kpMsg.layout.dim.append(dim)
kpMsg.layout.data_offset = 0
print "Setting posture task Kp gains to be zero..."
self.postureTaskKpPublisher.publish(kpMsg)
kdMsg = Float64MultiArray()
for ii in range(0, NUM_DOFS):
kdMsg.data.append(5) # Kd gains for gravity compensation mode
kdMsg.layout.dim.append(dim)
kdMsg.layout.data_offset = 0
print "Setting posture task Kd gains to be zero..."
self.postureTaskKdPublisher.publish(kdMsg)
print "Done setting robot into gravity compensation mode."
return not rospy.is_shutdown()
def callback(msg, prevMarkers):
detectedMarkers = msg.markers
# The current time in seconds
now = rospy.get_time()
for detectedMarker in detectedMarkers:
measuredTime = detectedMarker.header.stamp.secs
markerID = detectedMarker.id
prevMarkers[markerID] = measuredTime
detected_markers = list()
for marker in prevMarkers.keys():
if abs(prevMarkers[marker] - now) > 5:
del prevMarkers[marker]
else:
detected_markers.append(marker)
array = MultiArrayDimension()
array.label = 'numMarkers'
array.size = len(detected_markers)
array.size = len(detected_markers)
layout = MultiArrayLayout()
layout.dim = [
array,
]
layout.data_offset = 0
msg = Int16MultiArray()
msg.layout = layout
msg.data = detected_markers
pub.publish(msg)
def motor_joint_positions_publisher():
#Initiate node for controlling joint1 and joint2 positions.
rospy.init_node('motor_positions_node', anonymous=True)
#Define publishers for each joint position controller commands.
pub = rospy.Publisher('bicopter_motor_controller/command',
Float64MultiArray,
queue_size=10)
rate = rospy.Rate(100) #100 Hz
#Defining msg as float64MultiArray
msg = Float64MultiArray()
msg.layout.data_offset = 0
myMultiArrayDimensions = MultiArrayDimension()
myMultiArrayDimensions.label = ''
myMultiArrayDimensions.size = 2
myMultiArrayDimensions.stride = 1
msg.layout.dim.append(myMultiArrayDimensions)
#While loop to have joints follow a certain position, while rospy is not shutdown.
i = 0
while not rospy.is_shutdown():
#Have each joint follow a sine movement of sin(i/100).
msg.data = [0.00, 0.00]
#Publish the same sine movement to each joint.
pub.publish(msg)
i = i + 1 #increment i
rate.sleep() #sleep for rest of rospy.Rate(100)
def __init__(self, rosTopic, minAmplitude, maxAmplitude, numDoFs, period):
"""
The constructor.
Keyword arguments:
rosTopic -- The ROS topic on which to publish the goal.
minAmplitude -- The minimum amplitude in radians.
maxAmplitude -- The maximum amplitude in radians.
numDoFs -- The number of DoFs.
period -- The period of the square wave in seconds.
"""
self.period = period
self.rosTopic = rosTopic
# Define the dimensions of the message
dim = MultiArrayDimension()
dim.size = numDoFs
dim.label = "goalMsg"
dim.stride = 1
# Define the goal message containing the upper bound of the square wave
self.goalMsgHigh = Float64MultiArray()
for ii in range(0, numDoFs):
self.goalMsgHigh.data.append(maxAmplitude)
self.goalMsgHigh.layout.dim.append(dim)
self.goalMsgHigh.layout.data_offset = 0
# Define the goal message containing the lower bound of the square wave
self.goalMsgLow = Float64MultiArray()
for ii in range(0, numDoFs):
self.goalMsgLow.data.append(minAmplitude)
self.goalMsgLow.layout.dim.append(dim)
self.goalMsgLow.layout.data_offset = 0
def numpyArray2MultiArray(na, ma):
s = na.shape
for i in np.arange(len(s)):
d = MultiArrayDimension()
d.size = s[i]
ma.layout.dim.append(d)
ma.data = np.reshape(na, -1)
def publish_state(self, state, labels):
message = Int32MultiArray()
#message.header.frame_id = REFERENCE_FRAME
#message.header.stamp = rospy.Time(0)
dim1 = MultiArrayDimension()
dim2 = MultiArrayDimension()
dim1.label = labels[0]
dim1.size = state.shape[0]
dim1.stride = state.size
message.layout.dim.append(dim1)
dim2.label = labels[1]
dim2.size = state.shape[1]
dim2.stride = state.shape[1]
message.layout.dim.append(dim2)
for a in state.flat[:]:
message.data.append(a)
self._state_pub.publish(message)
pass
def PublishtoPurePursuit(self, waypoints):
pub_points = Float32MultiArray()
wp_len = len(waypoints)
dim0 = MultiArrayDimension()
dim1 = MultiArrayDimension()
dim0.label = "height"
dim0.size = wp_len/3
dim1.label = "width"
dim1.size = 3
dimensions = MultiArrayLayout()
dimensions.dim = [dim0, dim1]
pub_points.layout = dimensions
pub_points.data = waypoints
self.pursuit_pub.publish(pub_points)
def pubLabelMsg(self, known_labels):
msg = Int8MultiArray()
dim = MultiArrayDimension()
dim.label = "length"
dim.size = len(known_labels)
dim.stride = len(known_labels)
msg.layout.dim.append(dim)
msg.layout.data_offset = 0
msg.data = known_labels
self.labels_pub.publish(msg)
def sendCommand(self, ts_rostime):
msg = Float64MultiArray()
dim0 = MultiArrayDimension()
dim0.label = 'ts_rostime, numOfsamples, dt'
dim0.size = 3
layout_var = MultiArrayLayout()
layout_var.dim = [dim0]
msg.layout = layout_var
msg.data = [ts_rostime, self.numOfSamples, self.dt]
self.sampleParticalTrajectory_pub.publish(msg)
def make_hand_configuration(self, points):
dim = MultiArrayDimension()
dim.label = 'fingers'
dim.size = 5
dim.stride = 5
msg = Float64MultiArray()
msg.layout.dim = [dim]
msg.layout.data_offset = 0
msg.data = points
return msg
def gen_msg(self, val): fa = Float64MultiArray() mad = MultiArrayDimension() mad.label = 'commands' mad.size = 2 mad.stride = 2 fa.layout.dim = [mad] fa.layout.data_offset = 0 fa.data = [val, val] return fa
def pack_data():
msg = Float32MultiArray()
msg.data = MySensors.get_meters()
msg.layout.data_offset = 0
dim = MultiArrayDimension()
dim.label = 'num_sensors'
dim.size = 3
dim.stride = 1
msg.layout.dim = [dim]
return msg
def callback(data):
# tempolary hardcode the parameter
x_lim = -10
y_lim = -10
x_width = 20
y_width = 20
resol = 0.1
map_meta = data.info
N = int(x_width / resol) + 1
temp = np.ones((N, N)) * 50
dimh = MultiArrayDimension()
dimw = dimh
dimh.size = data.info.height
dimh.label = 'height'
dimh.stride = 48
dimw.size = data.info.width
dimw.label = 'width'
dimw.stride = 64
datanp = np.array(data.data, dtype=float)
datanp = datanp
datanp[datanp < 0] = 50
for i in range(data.info.height):
for j in range(data.info.width):
idx_x = round(
N / 2 -
map_meta.height / 2) #-data.info.origin.position.x/0.1)
idx_y = round(N / 2 - -data.info.origin.position.x / 0.1)
# temp[int(idx_x + i),int(idx_y + j)] = datanp[(i-1)*data.info.width+j]
for i in range(80):
for j in range(60):
temp[int(N / 2 - 40 + i), int(N / 2 - 30 + j)] = 0.1
map = OccupancyGrid()
map.data = temp.flatten()
map.info.width = int(N)
map.info.height = int(N)
map.info.resolution = 0.1
map.info.origin = map_meta.origin
# data.data
pub = rospy.Publisher('/TheMap', Float64MultiArray, queue_size=10)
pub_map = rospy.Publisher('/map_fixed', OccupancyGrid, queue_size=10)
a = Float64MultiArray()
a.layout.dim.append(dimh)
a.layout.dim.append(dimw)
a.layout.data_offset = 0
a.data = temp.flatten() / 100.
pub_map.publish(map)
pub.publish(a)
br = tf.TransformBroadcaster()
br.sendTransform((-map_meta.origin.position.x - 10,
-map_meta.origin.position.y - 10, 0.0),
(0.0, 0.0, 0.0, 1.0), rospy.Time.now(), "map", "world")
def matrixListToMultiarray(matrix):
rows, columns = matrix.shape
msg = Float64MultiArray()
# Set up layout
msg.layout.data_offset = 0
row_dim = MultiArrayDimension()
row_dim.label = "rows"
row_dim.size = rows
row_dim.stride = columns * rows
col_dim = MultiArrayDimension()
col_dim.label = "cols"
col_dim.size = columns
col_dim.stride = columns
msg.layout.dim = [row_dim, col_dim]
msg.data = matrix.flatten().tolist()
return msg
def cb_score(event):
global solveResult
ret=Bool();ret.data=True;pub_Y2.publish(ret)
cb_master(event)
score=Float32MultiArray()
for sc in solveResult:
d=MultiArrayDimension()
d.label=sc
d.size=len(solveResult[sc])
d.stride=1
print "score",d.label
def create_array_message():
array_dim_0 = MultiArrayDimension()
array_dim_0.label = 'dim_0'
array_dim_0.size = 4
array_dim_0.stride = 0
array_dim_1 = MultiArrayDimension()
array_dim_1.label = 'dim_1'
array_dim_1.size = len_faces
array_dim_1.stride = 0
array_layout = MultiArrayDimension()
array_layout.label = "face_array"
array_message = Float32MultiArray()
array_message.layout.dim = [array_dim_0, array_dim_1]
array_message.layout.data_offset = 0
face_0 = [0.0, 1.0, 1.0, 200.0]
face_1 = [1.0, 0.0, 1.0, 100.0]
face_2 = [2.0, 1.0, 0.0, 300.0]
face_3 = [3.0, 0.0, 0.0, 400.0]
array_message.data = face_0 + face_1 + face_2 + face_3
return array_message
def create_depth_msg(depth_img):
command = Float64MultiArray()
layout = MultiArrayLayout()
dimension = MultiArrayDimension()
dimension.label = "depth_img"
dimension.size = len(depth_img)
dimension.stride = len(depth_img)
layout.data_offset = 0
layout.dim = [dimension]
command.layout = layout
command.data = depth_img
return command
def sa_right_set_callback(self):
sa_right_cam = Float64MultiArray()
sa_right_cam_dim_h = MultiArrayDimension()
sa_right_cam_dim_h.label = "height"
sa_right_cam_dim_h.size = 1
sa_right_cam_dim_h.stride = 2
sa_right_cam.layout.dim.append( sa_right_cam_dim_h )
sa_right_cam_dim_w = MultiArrayDimension()
sa_right_cam_dim_w.label = "width"
sa_right_cam_dim_w.size = 2
sa_right_cam_dim_w.stride = 2
sa_right_cam.layout.dim.append(sa_right_cam_dim_w)
sa_right_cam.data.append( self.sa_right_gain.value() )
sa_right_cam.data.append( self.sa_right_exp.value() )
self.sa_right_cam_pub.publish( sa_right_cam )
def adjustLeftHand(position):
Lmsg = Float64MultiArray()
dim = MultiArrayDimension()
dim.label = 'fingers'
dim.size = 5
dim.stride = 5
Lmsg.layout.dim = [dim]
Lmsg.layout.data_offset = 0
Lmsg.data = position
left_hand_publisher.publish(Lmsg)
def adjustRightHand(position):
Rmsg = Float64MultiArray()
dim = MultiArrayDimension()
dim.label = 'fingers'
dim.size = 5
dim.stride = 5
Rmsg.layout.dim = [dim]
Rmsg.layout.data_offset = 0
Rmsg.data = position
right_hand_publisher.publish(Rmsg)
def publish(vals):
command = Float64MultiArray()
layout = MultiArrayLayout()
dimension = MultiArrayDimension()
dimension.label = "keyboard_control"
dimension.size = 4
dimension.stride = 4
layout.data_offset = 0
layout.dim = [dimension]
command.layout = layout
command.data = vals
control_pub.publish(command)
def main():
thread1 = myThread()
thread1.start()
rospy.init_node('reconstruction_node',
anonymous=True) # node_name: reconstruction_node
pub = rospy.Publisher('topic_name', Float32MultiArray,
queue_size=10) # topic_name: topic_name
instance_multi_array = Float32MultiArray()
instance_multi_array.layout.data_offset = 0
temp_a = MultiArrayDimension()
temp_b = MultiArrayDimension()
temp_a.label = 'LED'
temp_a.size = np.size(thread1.reconstruction_data_array, 0)
temp_a.stride = np.size(thread1.reconstruction_data_array, 1) * np.size(
thread1.reconstruction_data_array, 0)
temp_b.label = 'Sensor'
temp_b.size = np.size(thread1.reconstruction_data_array, 1)
temp_b.stride = 1 * np.size(thread1.reconstruction_data_array, 1)
instance_multi_array.layout.dim.append(temp_a)
instance_multi_array.layout.dim.append(temp_b)
temp_c = np.reshape(thread1.reconstruction_data_array,
(1, thread1.reconstruction_data_array.size), 'C')
instance_multi_array.data = temp_c[0]
rate = rospy.Rate(1)
rospy.loginfo("Node_Publishes")
while not rospy.is_shutdown():
# rospy.loginfo(instance_multi_array)
pub.publish(instance_multi_array)
rate.sleep()
def publisher(recieved):
pub = rospy.Publisher('joint_goals', Float64MultiArray, queue_size=10)
rospy.loginfo('Publishing the goal')
rospy.loginfo(recieved)
published_msg = Float64MultiArray()
multi_dim = MultiArrayDimension()
multi_dim.label = 'foo'
multi_dim.size = 1
multi_dim.stride = 1
published_msg.layout.dim = [multi_dim]
published_msg.layout.data_offset = 1
published_msg.data = recieved.position
pub.publish(published_msg)
def publish(self):
#pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=10)
pub = rospy.Publisher('remote_readings',
Float64MultiArray,
queue_size=1)
rate = rospy.Rate(20) #10Hz
a = [0.0, 0.0]
dim = MultiArrayDimension()
dim.size = len(a)
dim.label = "REMOTEmsg"
dim.stride = len(a)
apub = Float64MultiArray()
apub.data = a
apub.layout.dim.append(dim)
apub.layout.data_offset = 0
cnt = 0
while not rospy.is_shutdown():
begin = time.time()
#UART_read = ser.readline()
#UART_read = ser.Serial.readline()
#self.dev.flushInput()
while self.dev.inWaiting() < 40:
pass
UART_read = self.dev.readline()
self.dev.flushInput()
#if self.dev.inWaiting() > 80:
# self.dev.flushInput()
#if self.dev.inWaiting() > 50:
# self.dev.flushInput()
# cnt=0
#self.dev.flushInput()
#rospy.loginfo(UART_read)
steer_cmd = int(UART_read[0:4])
throt_cmd = int(UART_read[4:8])
#self.dev.flushInput()
self.dev1.pulsewidth_us(throt_cmd)
self.dev2.pulsewidth_us(steer_cmd)
a = [throt_cmd, steer_cmd]
rospy.loginfo(a)
apub.data = a
pub.publish(apub)
buffer = self.dev.inWaiting()
#rospy.loginfo(buffer)
#cnt=cnt+1
#self.dev.flushInput()
#length = time.time() - begin
#rospy.loginfo(length)
rate.sleep()
def makeMultiArray(iterable, label):
arrayList = []
for el in iterable:
arrayList.append(el)
dim = MultiArrayDimension()
dim.size = len(arrayList)
dim.label = label
dim.stride = len(arrayList)
tempArray = Float64MultiArray()
tempArray.data = arrayList
tempArray.layout.dim.append(dim)
tempArray.layout.data_offset = 0
return tempArray
def publish(self):
#pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=10)
pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=1)
rate = rospy.Rate(20) #10Hz
a = [0.0, 0.0]
dim = MultiArrayDimension()
dim.size = len(a)
dim.label = "REMOTEmsg"
dim.stride = len(a)
apub = Float64MultiArray()
apub.data = a
apub.layout.dim.append(dim)
apub.layout.data_offset = 0
cnt=0
while not rospy.is_shutdown():
begin = time.time()
#UART_read = ser.readline()
#UART_read = ser.Serial.readline()
#self.dev.flushInput()
while self.dev.inWaiting() < 40:
pass
UART_read = self.dev.readline()
self.dev.flushInput()
#if self.dev.inWaiting() > 80:
# self.dev.flushInput()
#if self.dev.inWaiting() > 50:
# self.dev.flushInput()
# cnt=0
#self.dev.flushInput()
#rospy.loginfo(UART_read)
steer_cmd = int(UART_read[0:4])
throt_cmd = int(UART_read[4:8])
#self.dev.flushInput()
self.dev1.pulsewidth_us(throt_cmd)
self.dev2.pulsewidth_us(steer_cmd)
a = [throt_cmd, steer_cmd]
rospy.loginfo(a)
apub.data = a
pub.publish(apub)
buffer = self.dev.inWaiting()
#rospy.loginfo(buffer)
#cnt=cnt+1
#self.dev.flushInput()
#length = time.time() - begin
#rospy.loginfo(length)
rate.sleep()
def ms_set_callback(self):
ms_cam = Float64MultiArray()
ms_cam_dim_h = MultiArrayDimension()
ms_cam_dim_h.label = "height"
ms_cam_dim_h.size = 1
ms_cam_dim_h.stride = 2
ms_cam.layout.dim.append( ms_cam_dim_h )
ms_cam_dim_w = MultiArrayDimension()
ms_cam_dim_w.label = "width"
ms_cam_dim_w.size = 2
ms_cam_dim_w.stride = 2
ms_cam.layout.dim.append(ms_cam_dim_w)
ms_cam.data.append( self.ms_gain.value() )
if self.ms_exp_auto.isChecked() :
ms_cam.data.append( 1 )
else:
ms_cam.data.append( self.ms_exp.value() )
self.ms_cam_pub.publish( ms_cam )
light = Float64()
light.data = self.ms_light.value()/100.0
self.ms_light_pub.publish( light )
spindle_speed = Float64()
spindle_speed.data = self.ms_spindle.value()
self.ms_spindle_pub.publish( spindle_speed )
def __init__(self):
"""
The constructor.
"""
# Define the joint info
self.jointSpecs = []
self.jointSpecs.append(NeckJointDetails("lower_neck_pitch", 0, -40, 15))
self.jointSpecs.append(NeckJointDetails("upper_neck_yaw", 1, -80, 80))
self.jointSpecs.append(NeckJointDetails("upper_neck_roll", 2, -14, 14))
self.jointSpecs.append(NeckJointDetails("upper_neck_pitch", 3, -7, 37))
self.jointSpecs.append(NeckJointDetails("eye_pitch", 4, -34, 34))
self.jointSpecs.append(NeckJointDetails("right_eye_yaw", 5, -34, 34))
self.jointSpecs.append(NeckJointDetails("left_eye_yaw", 6, -34, 34))
# Define the goal message
# Create the goal message. Store the current joint states into the messages
dimPos = MultiArrayDimension()
dimPos.size = len(self.jointSpecs)
dimPos.label = "goalPosMsg"
dimPos.stride = 1
self.goalPosMsg = Float64MultiArray()
for ii in range(0, len(self.jointSpecs)):
self.goalPosMsg.data.append(0)
self.goalPosMsg.layout.dim.append(dimPos)
self.goalPosMsg.layout.data_offset = 0
# dimVel = MultiArrayDimension()
# dimVel.size = len(self.jointSpecs)
# dimVel.label = "goalVelMsg"
# dimVel.stride = 1
# self.goalVelMsg = Float64MultiArray()
# for ii in range(0, len(self.jointSpecs)):
# self.goalVelMsg.data.append(0)
# self.goalVelMsg.layout.dim.append(dimVel)
# self.goalVelMsg.layout.data_offset = 0
self.cmdPublisher = rospy.Publisher("/dreamer_controller/controlit/head/position_cmd", Float64MultiArray, queue_size=1)
def __init__(self):
# Define the dimensions of the message
dim = MultiArrayDimension()
dim.size = NUM_DOFS
dim.label = "rightHandOrientationGoal"
dim.stride = 1
# Define the goal messages
self.rightHandGoalMsg = Float64MultiArray()
for ii in range(0, NUM_DOFS):
self.rightHandGoalMsg.data.append(0)
self.rightHandGoalMsg.layout.dim.append(dim)
self.rightHandGoalMsg.layout.data_offset = 0
self.rightHandGoalMsg.data[2] = 1 # initial goal is [0, 0, 1] in world frame
self.leftHandGoalMsg = Float64MultiArray()
for ii in range(0, NUM_DOFS):
self.leftHandGoalMsg.data.append(0)
self.leftHandGoalMsg.layout.dim.append(dim)
self.leftHandGoalMsg.layout.data_offset = 0
self.leftHandGoalMsg.data[2] = 1 # initial goal is [0, 0, 1] in world frame
def __init__(self, rosTopic, amplitude, offset, initGoal, numDoFs, jointIndex, period, updateFreq):
"""
The constructor.
Keyword arguments:
rosTopic -- The ROS topic on which to publish the goal.
initGoal -- The initial goal (the value the sine wave starts at)
amplitude -- The amplitude.
offset -- The offset.
numDoFs -- The number of DoFs.
jointIndex -- The index of the joint being controlled.
period -- The period of the sine wave in seconds.
updateFreq -- The frequency at which the reference position should be sent
"""
self.period = period
self.frequency = 1.0 / period
self.rosTopic = rosTopic
self.amplitude = amplitude
self.offset = offset
self.numDoFs = numDoFs
self.jointIndex = jointIndex
self.updateFreq = updateFreq
self.updatePeriod = 1.0 / updateFreq
# Define the dimensions of the message
dim = MultiArrayDimension()
dim.size = numDoFs
dim.label = "goalMsg"
dim.stride = 1
# Define the goal message
self.goalMsg = Float64MultiArray()
for ii in range(0, numDoFs):
self.goalMsg.data.append(initGoal)
self.goalMsg.layout.dim.append(dim)
self.goalMsg.layout.data_offset = 0
def __init__(self):
# Define the goal messages
rightHandCartesianGoalMsgDim = MultiArrayDimension()
rightHandCartesianGoalMsgDim.size = NUM_CARTESIAN_DOFS
rightHandCartesianGoalMsgDim.label = "rightHandCartesianGoal"
rightHandCartesianGoalMsgDim.stride = 1
self.rightHandCartesianGoalMsg = Float64MultiArray()
for ii in range(0, NUM_CARTESIAN_DOFS):
self.rightHandCartesianGoalMsg.data.append(0)
self.rightHandCartesianGoalMsg.layout.dim.append(rightHandCartesianGoalMsgDim)
self.rightHandCartesianGoalMsg.layout.data_offset = 0
#-----------------------------------------------------------------------------'
rightHandOrientationGoalMsgDim = MultiArrayDimension()
rightHandOrientationGoalMsgDim.size = NUM_ORIENTATION_DOFS
rightHandOrientationGoalMsgDim.label = "rightHandOrientationGoal"
rightHandOrientationGoalMsgDim.stride = 1
self.rightHandOrientationGoalMsg = Float64MultiArray()
for ii in range(0, NUM_ORIENTATION_DOFS):
self.rightHandOrientationGoalMsg.data.append(0)
self.rightHandOrientationGoalMsg.layout.dim.append(rightHandOrientationGoalMsgDim)
self.rightHandOrientationGoalMsg.layout.data_offset = 0
#-----------------------------------------------------------------------------'
postureGoalMsgDim = MultiArrayDimension()
postureGoalMsgDim.size = NUM_ROBOT_DOFS
postureGoalMsgDim.label = "postureGoal"
postureGoalMsgDim.stride = 1
self.postureGoalMsg = Float64MultiArray()
for ii in range(0, NUM_ROBOT_DOFS):
self.postureGoalMsg.data.append(0)
self.postureGoalMsg.layout.dim.append(postureGoalMsgDim)
self.postureGoalMsg.layout.data_offset = 0
#-----------------------------------------------------------------------------'
self.rightHandCmdMsg = Bool()
self.rightHandCmdMsg.data = False # relax hand
self.rightIndexFingerCmdMsg = Bool()
self.rightIndexFingerCmdMsg.data = True # include index finger in power grasp
self.rightMiddleFingerCmdMsg = Bool()
self.rightMiddleFingerCmdMsg.data = True # include middle finger in power grasp
self.enableMsg = Int32()
self.enableMsg.data = 1
#-----------------------------------------------------------------------------'
# Initialize member variables
self.currentPosture = None
self.postureError = None
self.currentRightCartesianPos = None
self.rightCartesianPosError = None
self.currentRightOrientation = None
self.rightOrientationError = None
# Create the ROS topic subscriptions
self.postureTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/JPosTask/actualPosition", Float64MultiArray, self.postureTaskActualCallback)
self.postureTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/JPosTask/error", Float64MultiArray, self.postureTaskErrorCallback)
self.rightCartesianTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/RightHandPosition/actualWorldPosition", Float64MultiArray, self.rightCartesianTaskActualCallback)
self.rightCartesianTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/RightHandPosition/error", Float64MultiArray, self.rightCartesianTaskErrorCallback)
self.rightOrientationTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/RightHandOrientation/actualHeading", Float64MultiArray, self.rightOrientationTaskActualCallback)
self.rightOrientationTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/RightHandOrientation/errorAngle", Float64, self.rightOrientationTaskErrorCallback)
# Create the ROS topic publishers
self.postureTaskGoalPublisher = rospy.Publisher("/dreamer_controller/JPosTask/goalPosition", Float64MultiArray, queue_size=1)
self.rightCartesianTaskGoalPublisher = rospy.Publisher("/dreamer_controller/RightHandPosition/goalPosition", Float64MultiArray, queue_size=1)
self.rightCartesianTaskEnablePublisher = rospy.Publisher("/dreamer_controller/RightHandPosition/enabled", Int32, queue_size=1)
self.rightOrientationTaskGoalPublisher = rospy.Publisher("/dreamer_controller/RightHandOrientation/goalVector", Float64MultiArray, queue_size=1)
self.rightOrientationTaskEnablePublisher = rospy.Publisher("/dreamer_controller/RightHandOrientation/enabled", Int32, queue_size=1)
self.rightHandCmdPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/powerGrasp", Bool, queue_size=1)
self.selectIndexFingerPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/includeRightIndexFinger", Bool, queue_size=1)
self.selectMiddleFingerPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/includeRightMiddleFinger", Bool, queue_size=1)
self.selectPinkyFingerPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/includeRightPinkyFinger", Bool, queue_size=1)
def __init__(self, enableUserPrompts = False, useQuaternionControl = False):
'''
The constructor.
Keyword arguments:
- enableUserPrompts: Whether to enable user prompts (default is False)
- userQuaternionControl: Whether to use quaternions for orientation control (default is False)
'''
self.enableUserPrompts = enableUserPrompts
self.useQuaternionControl = useQuaternionControl
if self.useQuaternionControl:
NUM_ORIENTATION_DOFS = 4 # Orientation is defined using a w, x, y, z vector
else:
NUM_ORIENTATION_DOFS = 3 # Orientation is defined using a x, y, z vector
# Define the goal messages
rightHandCartesianGoalMsgDim = MultiArrayDimension()
rightHandCartesianGoalMsgDim.size = NUM_CARTESIAN_DOFS
rightHandCartesianGoalMsgDim.label = "rightHandCartesianGoal"
rightHandCartesianGoalMsgDim.stride = 1
self.rightHandCartesianGoalMsg = Float64MultiArray()
for ii in range(0, NUM_CARTESIAN_DOFS):
self.rightHandCartesianGoalMsg.data.append(0)
self.rightHandCartesianGoalMsg.layout.dim.append(rightHandCartesianGoalMsgDim)
self.rightHandCartesianGoalMsg.layout.data_offset = 0
#-----------------------------------------------------------------------------'
leftHandCartesianGoalMsgDim = MultiArrayDimension()
leftHandCartesianGoalMsgDim.size = NUM_CARTESIAN_DOFS
leftHandCartesianGoalMsgDim.label = "leftHandCartesianGoal"
leftHandCartesianGoalMsgDim.stride = 1
self.leftHandCartesianGoalMsg = Float64MultiArray()
for ii in range(0, NUM_CARTESIAN_DOFS):
self.leftHandCartesianGoalMsg.data.append(0)
self.leftHandCartesianGoalMsg.layout.dim.append(leftHandCartesianGoalMsgDim)
self.leftHandCartesianGoalMsg.layout.data_offset = 0
#-----------------------------------------------------------------------------'
if not self.useQuaternionControl:
rightHandOrientationGoalMsgDim = MultiArrayDimension()
rightHandOrientationGoalMsgDim.size = NUM_ORIENTATION_DOFS
rightHandOrientationGoalMsgDim.label = "rightHandOrientationGoal"
rightHandOrientationGoalMsgDim.stride = 1
self.rightHandOrientationGoalMsg = Float64MultiArray()
for ii in range(0, NUM_ORIENTATION_DOFS):
self.rightHandOrientationGoalMsg.data.append(0)
self.rightHandOrientationGoalMsg.layout.dim.append(rightHandOrientationGoalMsgDim)
self.rightHandOrientationGoalMsg.layout.data_offset = 0
#-----------------------------------------------------------------------------'
leftHandOrientationGoalMsgDim = MultiArrayDimension()
leftHandOrientationGoalMsgDim.size = NUM_ORIENTATION_DOFS
leftHandOrientationGoalMsgDim.label = "leftHandOrientationGoal"
leftHandOrientationGoalMsgDim.stride = 1
self.leftHandOrientationGoalMsg = Float64MultiArray()
for ii in range(0, NUM_ORIENTATION_DOFS):
self.leftHandOrientationGoalMsg.data.append(0)
self.leftHandOrientationGoalMsg.layout.dim.append(leftHandOrientationGoalMsgDim)
self.leftHandOrientationGoalMsg.layout.data_offset = 0
else:
self.orientationUpdateMsg = QuatInterpMsg()
#-----------------------------------------------------------------------------'
postureGoalMsgDim = MultiArrayDimension()
postureGoalMsgDim.size = NUM_ROBOT_DOFS
postureGoalMsgDim.label = "postureGoal"
postureGoalMsgDim.stride = 1
self.postureGoalMsg = Float64MultiArray()
for ii in range(0, NUM_ROBOT_DOFS):
self.postureGoalMsg.data.append(0)
self.postureGoalMsg.layout.dim.append(postureGoalMsgDim)
self.postureGoalMsg.layout.data_offset = 0
#-----------------------------------------------------------------------------'
self.rightHandCmdMsg = Bool()
self.rightHandCmdMsg.data = False # relax hand
self.rightIndexFingerCmdMsg = Bool()
self.rightIndexFingerCmdMsg.data = True # include index finger in power grasp
self.rightMiddleFingerCmdMsg = Bool()
self.rightMiddleFingerCmdMsg.data = True # include middle finger in power grasp
self.rightPinkyFingerCmdMsg = Bool()
self.rightPinkyFingerCmdMsg.data = True # include pinky finger in power grasp
self.leftGripperCmdMsg = Bool()
self.leftGripperCmdMsg.data = False # relax gripper
self.tareMsg = Int32()
self.tareMsg.data = 1
self.enableMsg = Int32()
self.enableMsg.data = 1
#-----------------------------------------------------------------------------'
# Initialize member variables
self.currentPosture = None
self.postureError = None
self.currentRightCartesianPos = None
self.rightCartesianPosError = None
self.currentLeftCartesianPos = None
self.leftCartesianPosError = None
self.currentRightOrientation = None
self.rightOrientationError = None
self.currentLeftOrientation = None
self.leftOrientationError = None
# Create the ROS topic subscriptions
self.postureTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/Posture/actualPosition", Float64MultiArray, self.postureTaskActualCallback)
self.postureTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/Posture/error", Float64MultiArray, self.postureTaskErrorCallback)
self.rightCartesianTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/RightHandPosition/actualWorldPosition", Float64MultiArray, self.rightCartesianTaskActualCallback)
self.rightCartesianTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/RightHandPosition/error", Float64MultiArray, self.rightCartesianTaskErrorCallback)
self.leftCartesianTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/LeftHandPosition/actualWorldPosition", Float64MultiArray, self.leftCartesianTaskActualCallback)
self.leftCartesianTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/LeftHandPosition/error", Float64MultiArray, self.leftCartesianTaskErrorCallback)
if self.useQuaternionControl:
self.rightOrientationTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/RightHandOrientation/actualWorldOrientation", Float64MultiArray, self.rightOrientationTaskActualCallback)
else:
self.rightOrientationTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/RightHandOrientation/actualHeading", Float64MultiArray, self.rightOrientationTaskActualCallback)
self.rightOrientationTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/RightHandOrientation/errorAngle", Float64, self.rightOrientationTaskErrorCallback)
if self.useQuaternionControl:
self.leftOrientationTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/LeftHandOrientation/actualWorldOrientation", Float64MultiArray, self.leftOrientationTaskActualCallback)
else:
self.leftOrientationTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/LeftHandOrientation/actualHeading", Float64MultiArray, self.leftOrientationTaskActualCallback)
self.leftOrientationTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/LeftHandOrientation/errorAngle", Float64, self.leftOrientationTaskErrorCallback)
# Create the ROS topic publishers
self.postureTaskGoalPublisher = rospy.Publisher("/dreamer_controller/Posture/goalPosition", Float64MultiArray, queue_size=1)
self.postureTaskTarePublisher = rospy.Publisher("/dreamer_controller/Posture/tare", Int32, queue_size=1)
self.rightCartesianTaskGoalPublisher = rospy.Publisher("/dreamer_controller/RightHandPosition/goalPosition", Float64MultiArray, queue_size=1)
self.rightCartesianTaskEnablePublisher = rospy.Publisher("/dreamer_controller/RightHandPosition/enableState", Int32, queue_size=1)
self.rightCartesianTaskTarePublisher = rospy.Publisher("/dreamer_controller/RightHandPosition/tare", Int32, queue_size=1)
self.leftCartesianTaskGoalPublisher = rospy.Publisher("/dreamer_controller/LeftHandPosition/goalPosition", Float64MultiArray, queue_size=1)
self.leftCartesianTaskEnablePublisher = rospy.Publisher("/dreamer_controller/LeftHandPosition/enableState", Int32, queue_size=1)
self.leftCartesianTaskTarePublisher = rospy.Publisher("/dreamer_controller/LeftHandPosition/tare", Int32, queue_size=1)
if not self.useQuaternionControl:
self.rightOrientationTaskGoalPublisher = rospy.Publisher("/dreamer_controller/RightHandOrientation/goalVector", Float64MultiArray, queue_size=1)
self.rightOrientationTaskEnablePublisher = rospy.Publisher("/dreamer_controller/RightHandOrientation/enableState", Int32, queue_size=1)
self.rightOrientationTaskTarePublisher = rospy.Publisher("/dreamer_controller/RightHandOrientation/tare", Int32, queue_size=1)
if not self.useQuaternionControl:
self.leftOrientationTaskGoalPublisher = rospy.Publisher("/dreamer_controller/LeftHandOrientation/goalVector", Float64MultiArray, queue_size=1)
self.leftOrientationTaskEnablePublisher = rospy.Publisher("/dreamer_controller/LeftHandOrientation/enableState", Int32, queue_size=1)
self.leftOrientationTaskTarePublisher = rospy.Publisher("/dreamer_controller/LeftHandOrientation/tare", Int32, queue_size=1)
if self.useQuaternionControl:
self.orientationGoalPublisher = rospy.Publisher("/dreamer_controller/trajectory_generator/update_orientation", QuatInterpMsg, queue_size=1)
self.rightHandCmdPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/powerGrasp", Bool, queue_size=1)
self.selectIndexFingerPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/includeRightIndexFinger", Bool, queue_size=1)
self.selectMiddleFingerPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/includeRightMiddleFinger", Bool, queue_size=1)
self.selectPinkyFingerPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/includeRightPinkyFinger", Bool, queue_size=1)
self.leftGripperCmdPublisher = rospy.Publisher("/dreamer_controller/controlit/leftGripper/powerGrasp", Bool, queue_size=1)
