def publish(self, rgb, temp_array):
# Publish RGB image
rgb_img_msg = self.bridge.cv2_to_imgmsg(rgb)
rgb_img_msg.header.frame_id = self.evo_thermal_frame
rgb_img_msg.header.stamp = rospy.Time.now()
self.rgb_publisher.publish(rgb_img_msg)
# Publish temperatures array
raw_temp_array = Float64MultiArray()
raw_temp_array.layout.dim.append(MultiArrayDimension())
raw_temp_array.layout.dim[0].size = 32
raw_temp_array.layout.dim[0].stride = 1
raw_temp_array.layout.dim[0].label = "x-axis"
raw_temp_array.layout.dim.append(MultiArrayDimension())
raw_temp_array.layout.dim[1].size = 32
raw_temp_array.layout.dim[1].stride = 1
raw_temp_array.layout.dim[1].label = "y-axis"
data = np.reshape(temp_array, (1024))
np.round(data, 2)
raw_temp_array.data = data
self.raw_publisher.publish(raw_temp_array)
# Publish internal sensor temperature
self.ptat_publisher.publish(self.ptat / self.scaling -
self.celsius_offset)
def reconstruction(data):
arduino_data = data.data
# recosntruction
#just to test dummy reconstruction
#have to delete when the actual reconstruction is implemented
arduino_data = np.array(arduino_data)
arduino_data = arduino_data + 0.12
#reconstruction end here
# send message
mat_size = m * n
reconstruction_data = Float32MultiArray()
reconstruction_data.layout.dim.append(MultiArrayDimension())
reconstruction_data.layout.dim.append(MultiArrayDimension())
reconstruction_data.layout.dim[0].label = "W"
reconstruction_data.layout.dim[1].label = "H"
reconstruction_data.layout.dim[0].size = m
reconstruction_data.layout.dim[1].size = n
reconstruction_data.layout.dim[0].stride = m * n
reconstruction_data.layout.dim[1].stride = m
reconstruction_data.layout.data_offset = 0
reconstruction_data.data = arduino_data #[0]* mat_size
print("--------Start of Reconstruction data------")
rospy.loginfo(reconstruction_data)
print("---------End of Reconstruction data ------")
pub.publish(reconstruction_data)
def __init__(self):
#Initialize arbotix comunications
print"\nArbotix initialization, wait 10 seconds..."
ArbotiX.__init__(self)
for x in xrange(1,21):
time.sleep(0.5)
print(str(x*0.5) + "/10s")
#Setupr velocities and positions vectors and messages
self.joints_poses = [0,0,0,0,0,0]
self.joints_vels = [0,0,0,0,0]
self.ee_closed = 0
self.vels_to_pub = Float32MultiArray()
self.poses_to_pub = Float32MultiArray()
self.poses_layout = MultiArrayDimension('joints_poses', 6, 0)
self.vels_layout = MultiArrayDimension('joints_vels', 5, 0)
self.poses_to_pub.layout.dim = [self.poses_layout]
self.poses_to_pub.layout.data_offset = 0
self.vels_to_pub.layout.dim = [self.vels_layout]
self.vels_to_pub.layout.data_offset = 0
#ROS pubblisher for joint velocities and positions
self.pos_pub = rospy.Publisher('/windowx_3links/joints_poses', Float32MultiArray, queue_size=1)
self.vel_pub = rospy.Publisher('/windowx_3links/joints_vels', Float32MultiArray, queue_size=1)
print"Windowx_3link node created, whaiting for messages in:"
print" windowx_2links/torque"
print"Publishing joints' positions and velocities in:"
print" /windowx_3links/joints_poses"
print" /windowx_3links/joints_vels"
#Start publisher
self.publish()
def im_callback(self, data):
outputs, im = self.objsegmenter.segment(data, y_crop_idx=650)
labels = self.objsegmenter.get_labels(outputs)
box_list = np.asarray(
self.objsegmenter.get_boxes(outputs).tensor.tolist())
if self.visualize:
out = self.objsegmenter.draw_output(outputs, im)
# Publish segmented image
self.image_pub.publish(cv2_to_imgmsg(out.get_image(), 'bgr8'))
# Publish segmentation data as Segmentation message
seg_msg = Segmentation()
# Add labels
seg_msg.classes = labels
# Flatten data into 1d array
seg_msg.boxes.data = list(box_list.flatten())
# Populate layout info
dims = box_list.shape
if len(dims) > 1:
seg_msg.boxes.layout.dim = [
MultiArrayDimension(),
MultiArrayDimension()
]
seg_msg.boxes.layout.dim[0].label = 'classes'
seg_msg.boxes.layout.dim[0].size = dims[0]
seg_msg.boxes.layout.dim[0].stride = dims[0] * dims[1]
seg_msg.boxes.layout.dim[1].label = 'bbox coordinates'
seg_msg.boxes.layout.dim[1].size = dims[1]
seg_msg.boxes.layout.dim[1].stride = dims[1]
print(seg_msg.classes)
print(seg_msg.boxes.data)
else:
print('No objects found')
self.segment_pub.publish(seg_msg)
def publish_bounding_boxes(self, bbs, image_width, image_height):
# Given a list of bounding boxes, publish same.
# The detector returns an array of dlib.rectangle's.
if len(bbs) > 0:
msg = Int32MultiArray()
msg.layout.data_offset = 0
msg.layout.dim.append(
MultiArrayDimension(label='height',
size=len(bbs) + 1,
stride=4 * (len(bbs) + 1)))
msg.layout.dim.append(
MultiArrayDimension(label='width', size=4, stride=1))
msg.layout.dim.append(
MultiArrayDimension(label='channel', size=1, stride=1))
data = []
# The first entry is the size of the source image
data.append(0)
data.append(0)
data.append(image_width)
data.append(image_height)
for rect in bbs:
data.append(rect.left())
data.append(rect.top())
data.append(rect.right())
data.append(rect.bottom())
msg.data = data
self.bounding_box_publisher.publish(msg)
return
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 demo():
'''
Publish sample data to ROS
'''
# Read demo binvox as (64*64*64) array
with open('demo/occupy.binvox', 'rb') as f:
occ = binvox_rw.read_as_3d_array(f).data
with open('demo/non_occupy.binvox', 'rb') as f:
non = binvox_rw.read_as_3d_array(f).data
msg = ByteMultiArray()
msg.data = (occ.astype(int) - non.astype(int)).flatten().tolist()
msg.layout.dim.append(
MultiArrayDimension(label='x', size=DIM, stride=DIM * DIM * DIM))
msg.layout.dim.append(
MultiArrayDimension(label='y', size=DIM, stride=DIM * DIM))
msg.layout.dim.append(MultiArrayDimension(label='z', size=DIM, stride=DIM))
rospy.init_node('shape_demo_loader')
pub = rospy.Publisher('local_occupancy',
numpy_msg(ByteMultiArray),
queue_size=10)
rospy.Subscriber("local_occupancy_predicted", numpy_msg(ByteMultiArray),
callback)
time.sleep(5)
pub.publish(msg)
rospy.spin()
def vox_to_float_array(voxel_grid, dim):
out_msg = Float32MultiArray()
out_msg.data = voxel_grid.astype(np.float32).flatten().tolist()
out_msg.layout.dim.append(MultiArrayDimension(label='x', size=dim, stride=dim*dim*dim))
out_msg.layout.dim.append(MultiArrayDimension(label='y', size=dim, stride=dim*dim))
out_msg.layout.dim.append(MultiArrayDimension(label='z', size=dim, stride=dim))
return out_msg
def determine_vision_pattern(self):
resolution = self.occ_grid.info.resolution
sensor_range = self.sensor.range_max
dimension = 2 * int(
np.round(math.ceil((sensor_range) / resolution - 0.5))) + 1
self.vision_pattern.data = []
self.vision_pattern.layout.dim.append(MultiArrayDimension())
self.vision_pattern.layout.dim.append(MultiArrayDimension())
self.vision_pattern.layout.dim[0].size = dimension
self.vision_pattern.layout.dim[1].size = dimension
self.vision_pattern.layout.dim[0].label = "rows"
self.vision_pattern.layout.dim[1].label = "cols"
self.vision_pattern.layout.data_offset = 0
centre_index = (dimension + 1) / 2 - 1 # -1 since index starts at 0
for i in range(0, dimension):
for j in range(0, dimension):
distance = resolution * np.sqrt((i - centre_index)**2 +
(j - centre_index)**2)
if distance <= sensor_range:
self.vision_pattern.data.append(1)
else:
self.vision_pattern.data.append(0)
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):
'''
'''
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 moveit_callback(msg):
pub = rospy.Publisher('/update_joint_position', Float64MultiArray)
stock_pub = rospy.Publisher('/stock_joint_position', Float64MultiArray)
global plan_only
r = rospy.Rate(10)
names = msg.result.planned_trajectory.joint_trajectory.joint_names
points = msg.result.planned_trajectory.joint_trajectory.points
for point in points:
pos_msg = Float64MultiArray()
i = 0
for name in names:
dim = MultiArrayDimension()
dim.label = name
pos_msg.layout.dim.append(dim)
pos_msg.data = point.positions
if rospy.get_param('~sim_mode') and plan_only == False:
pub.publish(pos_msg)
elif plan_only:
stock_pub.publish(pos_msg)
r.sleep()
if plan_only:
global robot_trajectory
robot_trajectory = msg.result.planned_trajectory
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 __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 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 __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 test_strify_message(self):
from genpy.message import Message, strify_message, fill_message_args
def roundtrip(m):
yaml_text = strify_message(m)
print(yaml_text)
loaded = yaml.load(yaml_text)
print("loaded", loaded)
new_inst = m.__class__()
if loaded is not None:
fill_message_args(new_inst, [loaded])
else:
fill_message_args(new_inst, [])
return new_inst
# The following tests have not been ported to genpy yet
# test array of Messages field. We can't use M4 or M5 because fill_message_args has to instantiate the embedded type
from test_roslib_comm.msg import ArrayOfMsgs
from std_msgs.msg import String, Time, MultiArrayLayout, MultiArrayDimension
dims1 = [MultiArrayDimension(*args) for args in [('', 0, 0), ('x', 1, 2), ('y of z', 3, 4)]]
dims2 = [MultiArrayDimension('hello world', 91280, 1983274)]
times = [Time(genpy.Time(*args)) for args in [(0,), (12345, 6789), (1, 1)]]
val = ArrayOfMsgs([String(''), String('foo'), String('bar of soap')],
times,
[MultiArrayLayout(dims1, 0), MultiArrayLayout(dims2, 12354)],
)
self.assertEquals(val, roundtrip(val))
def test_dictionary_with_child_message(self):
from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
expected_message = Float64MultiArray(layout=MultiArrayLayout(
dim=[
MultiArrayDimension(label='Dimension1', size=12, stride=7),
MultiArrayDimension(label='Dimension2', size=90, stride=8)
],
data_offset=1),
data=[1.1, 2.2, 3.3])
dictionary = {
'layout': {
'dim': [{
'label': expected_message.layout.dim[0].label,
'size': expected_message.layout.dim[0].size,
'stride': expected_message.layout.dim[0].stride
}, {
'label': expected_message.layout.dim[1].label,
'size': expected_message.layout.dim[1].size,
'stride': expected_message.layout.dim[1].stride
}],
'data_offset':
expected_message.layout.data_offset
},
'data': expected_message.data
}
message = message_converter.convert_dictionary_to_ros_message(
'std_msgs/Float64MultiArray', dictionary)
expected_message = serialize_deserialize(expected_message)
self.assertEqual(message, expected_message)
def get_example_cov(example_id=0):
"""
Creates an example covariance matrix. The data in the matrix was acquired from the maven-1.bag file.
:param example_id: A constant id of the example in the list of stored examples. Minimum value is 0.
Values above the max value will be set to 0 instead.
Current maximum value is: 1
:return: The covariance matrix in format std_msgs/Float64MultiArray
"""
MAX_SAMPLES = 1
if example_id > MAX_SAMPLES:
example_id = 0
nan = float("NaN")
examples = []
# Here, just copy-paste some more data from maven-1.bag so that you can choose which example you want
examples.append([0.2256878004660412, 0.004743161046803848, nan, nan, nan, 0.4799785723084568, 6.694665570936009e-05, 0.004743161046804125, 0.03333087258142175, nan, nan, nan, 0.0064891656361629295, 0.08962944598353237, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, 0.47997857230845664, 0.006489165636162725, nan, nan, nan, 2.4285886983849885, -0.06696549234373295, 6.694665570941213e-05, 0.08962944598353195, nan, nan, nan, -0.06696549234373322, 1.424665892676366])
examples.append([0.5758368975539181, -0.10477581457466455, nan, nan, nan, 3.003595362397707, -0.39924730334819275, -0.10477581457466437, 0.24081097327513568, nan, nan, nan, -0.4539729065847597, 1.439606811146181, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, 3.003595362397709, -0.45397290658476147, nan, nan, nan, 18.613535338951223, -1.9899796692884495, -0.39924730334819236, 1.4396068111461806, nan, nan, nan, -1.9899796692884442, 10.681110693507879])
# -----------------
cov_list = examples[example_id]
# Create Float64MultiArray similar to those in the bag files
dim_0 = MultiArrayDimension(label="", size=7, stride=49)
dim_1 = MultiArrayDimension(label="", size=7, stride=7)
cov_dim = [dim_0, dim_1]
cov_layout = MultiArrayLayout(dim=cov_dim, data_offset=0)
return Float64MultiArray(layout=cov_layout, data=cov_list)
def publish_depth(self, frames):
depth_frame = frames.get_depth_frame()
if self.DISPLAY:
depth_image = np.asanyarray(
self.colorizer.colorize(depth_frame).get_data())
cv2.imshow("Original Depth", depth_image)
cv2.waitKey(2)
depth = np.asanyarray(depth_frame.get_data(), dtype=np.float64)
depth = self.crop_depth(depth)
# Create depth Float64MultiArray message to publish
depth_data = Float64MultiArray()
dim1 = MultiArrayDimension(label="height", size=depth.shape[0])
dim2 = MultiArrayDimension(label="width", size=depth.shape[1])
depth_data.layout.dim.extend([dim1, dim2])
depth_data.data = depth.flatten()
# Display Cropped Image
if self.DANIEL_DEBUG and not self.DISPLAY:
displayed_depth = np.asanyarray(
self.colorizer.colorize(depth_frame).get_data())
cv2.imshow("Published Depth", self.crop_depth(displayed_depth))
cv2.waitKey(2)
self.safe_publish(self.depth_pub, depth_data)
def main():
global pub
rospy.init_node('go_to_point')
pub = rospy.Publisher('/diff_drive_controller/cmd_vel', Twist, queue_size=1)
status_pub = rospy.Publisher('/move_bot/status', Bool, queue_size=1)
sub_odom = rospy.Subscriber('/odom',Odometry, clbk_odom)
target_sub = rospy.Subscriber('/move_bot/goal',Pose, clbk_goal)
mat = Float64MultiArray()
mat.layout.dim.append(MultiArrayDimension())
mat.layout.dim.append(MultiArrayDimension())
gripper_pub = rospy.Publisher('/gripper_controller/command', Float64MultiArray, queue_size=1)
rate =rospy.Rate(200)
extend = 0
gripper_extend = 0
global message_arrived
while not rospy.is_shutdown():
if message_arrived:
if state_ == 0:
fix_yaw(desired_position_)
elif state_ == 1:
go_straight_ahead(desired_position_)
elif state_ == 2:
orient_yaw(yaw_required)
elif state_ ==3:
done()
change_state(0)
message_arrived = False
status = True
status_msg = Bool()
status_msg.data = status
status_pub.publish(status_msg)
print("done")
def image_to_saliency(t, saliency, saliency_pub, saliency_image_pub, bridge,
image, last_time, elapsed):
if t < 1.0:
return
if image.value is None:
return
if last_time.value is None:
last_time.value = t
current_time = t
dt = current_time - last_time.value
last_time.value = current_time
elapsed.value = elapsed.value + dt
if elapsed.value < 0.01:
return
else:
elapsed.value = 0.
image = bridge.value.imgmsg_to_cv2(image.value, "bgr8")
saliency_map = saliency.value.compute_saliency_map(image)
saliency_map_image = bridge.value.cv2_to_imgmsg(
np.uint8(saliency_map * 255.), "mono8")
saliency_image_pub.value.publish(saliency_map_image)
from std_msgs.msg import Float32MultiArray, MultiArrayDimension, MultiArrayLayout
height = MultiArrayDimension(size=len(saliency_map))
width = MultiArrayDimension(size=len(saliency_map[0]))
lo = MultiArrayLayout([height, width], 0)
saliency_pub.value.publish(
Float32MultiArray(layout=lo, data=saliency_map.flatten()))
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 handle_p2b(req):
global theta
print("here")
theta = list(req.configs.angles)
delta = 0.0001
f_original = computeH(0, 0, 0, 0)
f_theta1 = computeH(delta, 0, 0, 0)
f_theta2 = computeH(0, delta, 0, 0)
f_theta3 = computeH(0, 0, delta, 0)
f_theta4 = computeH(0, 0, 0, delta)
J1 = (f_theta1 - f_original) / delta
J2 = (f_theta2 - f_original) / delta
J3 = (f_theta3 - f_original) / delta
J4 = (f_theta4 - f_original) / delta
jacobian = Float64MultiArray()
jacobian.layout.dim = [
MultiArrayDimension('height', 4, 3 * 4),
MultiArrayDimension('width', 3, 3)
]
jacobian.data = [
J1[0], J2[0], J3[0], J4[0], J1[1], J2[1], J3[1], J4[1], J1[2], J2[2],
J3[2], J4[2]
]
return jacobian
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 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 __init__(self, name):
self.name = name
self.position_stddev = 0.50 # [m]
self.ref_frame = "/map"
rospy.init_node('kf_example')
self.position_stddev = rospy.get_param("~position_stddev_m",
self.position_stddev)
rospy.loginfo("Starting KF Example")
self.array = Float64MultiArray()
self.array.layout = MultiArrayLayout()
self.array.layout.data_offset = 0
self.array.layout.dim = [MultiArrayDimension('data', 3, 3)]
self.array.data = [0.0] * 3
self.array2 = Float64MultiArray()
self.array2.layout = MultiArrayLayout()
self.array2.layout.data_offset = 0
self.array2.layout.dim = [MultiArrayDimension('measure', 2, 2)]
self.array2.data = [0.0] * 2
# Initialisation of the matrices
self.A = numpy.mat([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
self.P = numpy.mat([[1, 0, 0], [0, 1, 0], [0, 0, 3.5]])
self.Q = numpy.mat([[pow(0.5, 2), 0, 0], [0, pow(0.5, 2), 0],
[0, 0, 0.001]])
self.H = numpy.mat([[1, 0, 0], [0, 1, 0]])
self.R = numpy.mat([[0.1, 0], [0, 0.1]])
self.U = numpy.vstack([0.0, 0.0])
# State is x, y, theta
self.state = numpy.vstack([0.0, 0.0, 0.0])
self.last_update = rospy.Time.now()
def callback(self, joint_state):
if len(self.joint_position) == 0:
self.joint_position = np.array(joint_state.position)
else:
if np.all(self.joint_position == joint_state.position):
return
else:
self.joint_position = copy.deepcopy(joint_state.position)
print self.joint_position
mat = UInt16MultiArray()
mat.layout.dim.append(MultiArrayDimension())
mat.layout.dim.append(MultiArrayDimension())
mat.layout.dim[0].label = "servo_number"
mat.layout.dim[0].size = 6
mat.layout.dim[0].stride = 3 * 6
mat.layout.dim[1].label = "single_command"
mat.layout.dim[1].size = 3
mat.layout.dim[1].stride = 3
mat.layout.data_offset = 0
mat.data = [
1, 1500, 1000, 2, 1500, 1000, 3, 1500, 1000, 4, 1500, 1000, 5,
1500, 1000, 6, 1500, 1000
]
dstride1 = mat.layout.dim[1].stride
offset = mat.layout.data_offset
for i in range(6):
if i == 1 or i == 2:
PMW_value = -self.joint_position[i] / 1.5708 * 1000 + 1500
else:
PMW_value = self.joint_position[i] / 1.5708 * 1000 + 1500
mat.data[offset + 1 + i * dstride1] = PMW_value
print mat
self.PMW_value_pub.publish(mat)
def take_pictures_test(self):
#input: None
#output: None
#Description: Used for validating, AI is working with camera or not
#Getting the bird eye view of the image
#persImg = perspective(cv_image)
persImg = skimage.io.imread(INPUT_TEST_IMAGE_PATH)
objCnt,objCord = self.weedLocation(persImg)
#Making up the object location data in format for publishing
if objCnt !=0:
objCord = np.reshape(objCord,(objCnt*3,)) #reshaping in the form of single column
objCord = np.ndarray.tolist(objCord)
else:
objCord = np.reshape(objCord,(3,)) #reshaping in the form of single column
objCord = np.ndarray.tolist(objCord)
msg = Float32MultiArray()
msg.layout.dim.append(MultiArrayDimension())
msg.layout.dim.append(MultiArrayDimension())
msg.layout.dim[0].label = "row"
msg.layout.dim[1].label = "col"
msg.layout.dim[0].size = objCnt
msg.layout.dim[1].size = 1
msg.layout.dim[0].stride = 3
msg.layout.dim[1].stride = 1
msg.layout.data_offset = 0
msg.data = objCord
self.publisher.publish(msg)
def think(msgs):
global curx, cury, positions, r
print(msgs.data)
data = [int(x) for x in msgs.data]
idxs = sorted(range(9), key=lambda k:data[k])
mat = Float32MultiArray()
mat.layout.dim.append(MultiArrayDimension())
mat.layout.dim.append(MultiArrayDimension())
mat.layout.dim[0].label = "height"
mat.layout.dim[1].label = "width"
mat.layout.dim[0].size = 2
mat.layout.dim[1].size = 9
mat.data = [0]*18
print(idxs)
for i in range(len(idxs)):
pos = positions[idxs[i]]
print(pos)
tx = pos[0] - curx
ty = pos[1] - cury
curx = pos[0]
cury = pos[1]
mat.data[i * 2] = tx
mat.data[i * 2 + 1] = ty
print(mat)
pub.publish(mat)
r.sleep()
def to_multiarray(img_shape, boxes, scores, classes):
# image_shape : the shape of the image used by yolo
# boxes: An `array` of shape (num_boxes, 4) containing box corners as
# (y_min, x_min, y_max, x_max).
# `scores`: A `list` of scores for each box.
# classes: A `list` of indicies into `class_names`.
# return: multiarray
data = []
for i in range(len(boxes)):
y_min = boxes[i][0] / img_shape[0]
x_min = boxes[i][1] / img_shape[1]
y_max = boxes[i][2] / img_shape[0]
x_max = boxes[i][3] / img_shape[1]
confidence = scores[i]
_class = classes[i]
data.append(float(_class))
data.append(float(y_min))
data.append(float(x_min))
data.append(float(y_max))
data.append(float(x_max))
data.append(float(confidence))
message = Float32MultiArray()
message.layout.dim.append(MultiArrayDimension("boxes", len(boxes), 1))
message.layout.dim.append(MultiArrayDimension("box_data", 6, 1))
message.data = data
return message
def test_ros_message_with_child_message(self):
from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
expected_dictionary = {
'layout': {
'dim': [{
'label': 'Dimension1',
'size': 12,
'stride': 7
}, {
'label': 'Dimension2',
'size': 24,
'stride': 14
}],
'data_offset':
0
},
'data': [1.1, 2.2, 3.3]
}
dimension1 = MultiArrayDimension(
label=expected_dictionary['layout']['dim'][0]['label'],
size=expected_dictionary['layout']['dim'][0]['size'],
stride=expected_dictionary['layout']['dim'][0]['stride'])
dimension2 = MultiArrayDimension(
label=expected_dictionary['layout']['dim'][1]['label'],
size=expected_dictionary['layout']['dim'][1]['size'],
stride=expected_dictionary['layout']['dim'][1]['stride'])
layout = MultiArrayLayout(
dim=[dimension1, dimension2],
data_offset=expected_dictionary['layout']['data_offset'])
message = Float64MultiArray(layout=layout,
data=expected_dictionary['data'])
message = serialize_deserialize(message)
dictionary = message_converter.convert_ros_message_to_dictionary(
message)
self.assertEqual(dictionary, expected_dictionary)
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 image_to_saliency(t, image, bridge, saliency, saliency_pub,
saliency_image_pub, points, camera_model,
camera_info_left, last_time, elapsed, pan, tilt,
saliency_map):
if image.value is None or camera_info_left.value is None:
return
if last_time.value is None:
last_time.value = t
current_time = t
dt = current_time - last_time.value
last_time.value = current_time
elapsed.value = elapsed.value + dt
if elapsed.value < 0.01:
return
else:
elapsed.value = 0.
if saliency_map.value is None:
image = bridge.value.imgmsg_to_cv2(image.value, "bgr8")
image = np.zeros(image.shape)
saliency_map.value = saliency.value.compute_saliency_map(image)
saliency_map_current = saliency_map.value.copy()
# apply curiosity
camera_model.value.fromCameraInfo(camera_info_left.value)
for point in points.value:
# call service
pixel = camera_model.value.project3dToPixel(
(point.point.x - pan.value, point.point.y - tilt.value,
point.point.z))
x = int(pixel[0] * (len(saliency_map_current[0]) /
float(camera_info_left.value.width)))
x = x + 6 # correction, bug in opencv?
y = int(
pixel[1] *
(len(saliency_map_current) / float(camera_info_left.value.height)))
if x >= 0 and x < len(saliency_map_current[0]) and y >= 0 and y < len(
saliency_map_current):
from skimage.draw import circle
rr, cc = circle(
y, x, 25,
(len(saliency_map_current), len(saliency_map_current[0])))
saliency_map[rr, cc] = saliency_map[rr, cc] * min(
1, (t - point.header.stamp.to_sec()))
saliency_map_image = bridge.value.cv2_to_imgmsg(
np.uint8(saliency_map_current * 255.), "mono8")
saliency_image_pub.value.publish(saliency_map_image)
from std_msgs.msg import Float32MultiArray, MultiArrayDimension, MultiArrayLayout
height = MultiArrayDimension(size=len(saliency_map_current))
width = MultiArrayDimension(size=len(saliency_map_current[0]))
lo = MultiArrayLayout([height, width], 0)
saliency_pub.value.publish(
Float32MultiArray(layout=lo, data=saliency_map_current.flatten()))
return
def bno055_pub():
rospy.init_node('bno055_pub', anonymous=True)
pub = rospy.Publisher('bno055_state', Float32MultiArray, queue_size=1)
rate = rospy.Rate(10) # 10hz
bno_controller = AdafruitBno055Controller()
mat = Float32MultiArray()
mat.layout.dim.append(MultiArrayDimension())
mat.layout.dim.append(MultiArrayDimension())
mat.layout.dim[0].label = "height"
mat.layout.dim[1].label = "width"
mat.layout.dim[0].size = 3
mat.layout.dim[1].size = 1
mat.layout.dim[0].stride = 3 * 1 #note dim[0] stride is just size of image
mat.layout.dim[1].stride = 1
mat.layout.data_offset = 0
mat.data = [0] * 3
while not rospy.is_shutdown():
x, y, z = bno_controller.gravity()
gravity_str = "gravity is{}".format([x, y, z])
rospy.loginfo(gravity_str)
mat.data[0] = x
mat.data[1] = y
mat.data[2] = z
pub.publish(mat)
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 feedback(feedback):
global pub, initial_joint_position, fixed_frame, joint_names, group_name
server.setPose(feedback.marker_name, feedback.pose)
server.applyChanges()
if feedback.event_type == 0:
return
rospy.wait_for_service('compute_ik')
try:
service = rospy.ServiceProxy('compute_ik', GetPositionIK)
request = GetPositionIKRequest()
request.ik_request.group_name = group_name
request.ik_request.timeout = rospy.Duration.from_sec(0.0001)
# initial robot state
robot_state = RobotState()
robot_state.joint_state.header.frame_id = fixed_frame
robot_state.joint_state.name = joint_names
robot_state.joint_state.position = initial_joint_position
robot_state.joint_state.velocity = []
request.ik_request.robot_state = robot_state
# goal end pose
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = fixed_frame
pose_stamped.pose.position.x = feedback.pose.position.x
pose_stamped.pose.position.y = feedback.pose.position.y
pose_stamped.pose.position.z = feedback.pose.position.z
pose_stamped.pose.orientation.x = feedback.pose.orientation.x
pose_stamped.pose.orientation.y = feedback.pose.orientation.y
pose_stamped.pose.orientation.z = feedback.pose.orientation.z
pose_stamped.pose.orientation.w = feedback.pose.orientation.w
request.ik_request.pose_stamped = pose_stamped
response = service(request)
print response
if len(response.solution.joint_state.position) != 0:
print "success"
msg = Float64MultiArray()
for i,joint_name in enumerate(response.solution.joint_state.name):
for j, name in enumerate(joint_names):
if joint_name == name:
initial_joint_position[j] = response.solution.joint_state.position[i]
dim = MultiArrayDimension()
dim.label = name
msg.layout.dim.append(dim)
msg.data.append(response.solution.joint_state.position[i])
pub.publish(msg)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def make_bounding_box_msg(bbox, selected=False):
bb = Float64MultiArray()
bb.layout.data_offset = 0
dim = MultiArrayDimension()
dim.label = "bb" if not selected else "select" # we use the label of the first dimension to carry the selected/not selected infomation
bb.layout.dim = [dim]
x_min, y_min, x_max, y_max = bbox
bb.data = [x_min, y_min, x_max, y_max]
return bb
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 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 __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 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 __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):
"""
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 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 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, 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)
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)
