def echoes_callback(echo):
echo['data'] = echo['data'][np.bitwise_and(
echo['data']['flags'],
0x01).astype(np.bool)] #keep valid echoes only
indices, flags, distances, amplitudes, x, y, z = [
echo['data'][x] for x in
['indices', 'flags', 'distances', 'amplitudes', 'x', 'y', 'z']
]
stamp = rospy.Time.now()
#rospy.loginfo('pixell: echoes callback')
exportation_echoes.add_sample(echo)
if pub_raw.get_num_connections() > 0:
pub_raw.publish(
ros_numpy.msgify(PointCloud2, echo['data'], stamp, frame_id))
if pub_cloud.get_num_connections() > 0:
struct_cloud = np.empty(indices.size,
dtype=[('x', np.float32),
('y', np.float32),
('z', np.float32),
('intensity', np.float32)])
struct_cloud['x'] = x
struct_cloud['y'] = y
struct_cloud['z'] = z
struct_cloud['intensity'] = amplitudes
pub_cloud.publish(
ros_numpy.msgify(PointCloud2, struct_cloud, stamp, frame_id))
def offset_goal(pose, offset):
position = numpify(pose.pose.position)
orientation = numpify(pose.pose.orientation)
position += qv_mult(orientation, np.array([offset, 0.0, 0.0]))
pose.pose.position = msgify(Point, position)
pose.pose.orientation = msgify(Quaternion, orientation)
return pose
def sub_image(self, im_msg, image_info):
"""
Take a thresholded image and output marker position metrics.
"""
print("sub_image in marker_locator")
# Announce that we have received a packet
self.pub_packet_entered_node.publish(im_msg.header)
# Extract the image
im = numpify(im_msg)
height, width = im.shape[:2]
# Label each connected blob in the image.
im2 = (im > 128).astype(bool)
labels, num_labels = scipy.ndimage.measurements.label(im2)
# no blobs?
if num_labels == 0:
self.pub_marker.publish(PointStamped(
header=Header(frame_id="base_link", stamp=im_msg.header.stamp),
point=Point(x=np.nan, y=np.nan, z=0)
))
return
# Calculate the size of each blob.
blob_sizes = scipy.ndimage.measurements.sum(im2, labels=labels, index=xrange(0,num_labels+1))
# Find the biggest blob.
biggest_blob = np.argmax(blob_sizes) - 1
pp = scipy.ndimage.measurements.find_objects(labels)[biggest_blob]
bbox = BoundingBox(pp[1].start, pp[0].start, pp[1].stop, pp[0].stop)
# Calculate centroid (units of pixels).
cx, cy = np.mean([bbox.maxx, bbox.minx]), np.mean([bbox.maxy, bbox.miny])
p_matrix = np.array(image_info.P).reshape((3, 4))
marker_position = self.get_position(cx, cy, p_matrix, im_msg.header)
if marker_position is None:
return
self.pub_marker.publish(PointStamped(
header=Header(frame_id="base_link", stamp=im_msg.header.stamp),
point=msgify(Point, marker_position)
))
# Make overlay visualizations.
debug_overlay = np.zeros((height, width, 3), dtype=np.uint8)
debug_overlay[...,0] = im
debug_overlay[...,1] = im
debug_overlay[...,2] = im
self.draw_bbox(debug_overlay, bbox)
self.draw_point(debug_overlay, cx, cy)
debug_overlay_msg = msgify(Image, debug_overlay, encoding='rgb8')
debug_overlay_msg.header = im_msg.header
self.pub_debug_overlay.publish(debug_overlay_msg)
# Announce that a packet has left the node.
self.pub_packet_left_node.publish(im_msg.header)
def offset_goal():
pose = PoseStamped()
pose.header.frame_id = 'map'
goal_pose = goal() # type: PoseStamped
position = numpify(goal_pose.pose.position)
orientation = numpify(goal_pose.pose.orientation)
position += qv_mult(orientation, np.array([-offset, 0.0, 0.0]))
pose.pose.position = msgify(Point, position)
pose.pose.orientation = msgify(Quaternion, orientation)
return pose
def test_point(self):
p = Point(1, 2, 3)
p_arr = ros_numpy.numpify(p)
np.testing.assert_array_equal(p_arr, [1, 2, 3])
p_arrh = ros_numpy.numpify(p, hom=True)
np.testing.assert_array_equal(p_arrh, [1, 2, 3, 1])
self.assertEqual(p, ros_numpy.msgify(Point, p_arr))
self.assertEqual(p, ros_numpy.msgify(Point, p_arrh))
self.assertEqual(p, ros_numpy.msgify(Point, p_arrh * 2))
def test_point(self):
p = Point(1, 2, 3)
p_arr = ros_numpy.numpify(p)
np.testing.assert_array_equal(p_arr, [1, 2, 3])
p_arrh = ros_numpy.numpify(p, hom=True)
np.testing.assert_array_equal(p_arrh, [1, 2, 3, 1])
self.assertEqual(p, ros_numpy.msgify(Point, p_arr))
self.assertEqual(p, ros_numpy.msgify(Point, p_arrh))
self.assertEqual(p, ros_numpy.msgify(Point, p_arrh * 2))
def on_enter(self, userdata):
post = posts[userdata.leg_number]
if not post.has_seen():
self._error = True
target_pose = PoseStamped()
point = numpify(post.position.point)
normal = numpify(post.normal.vector)
target_pose.header.frame_id = post.pose.header.frame_id
target_pose.pose.position = msgify(Point, point + normal * 2)
target_pose.pose.orientation = msgify(Quaternion,
normal_to_quaternion(-normal))
userdata.target_pose = target_pose
def __init__(self, number): # int: (str) -> None
pose_name = 'S{}'.format(number)
position = np.array([float(x) for x in rospy.get_param('named_poses/{}/position'.format(pose_name))])
orientation = np.array([float(x) for x in rospy.get_param('named_poses/{}/orientation'.format(pose_name))])
self._pose = PoseStamped()
self._pose.header.frame_id = 'map'
self._pose.pose.position = msgify(Point, position)
self._pose.pose.orientation = msgify(Quaternion, orientation)
self._contains = set()
self._number = number
self._pose_publisher = rospy.Publisher('/viz/parking_square_{}'.format(number), PoseStamped, latch=True, queue_size=1)
self._pose_publisher.publish(self.pose)
def test_vector3(self):
v = Vector3(1, 2, 3)
v_arr = ros_numpy.numpify(v)
np.testing.assert_array_equal(v_arr, [1, 2, 3])
v_arrh = ros_numpy.numpify(v, hom=True)
np.testing.assert_array_equal(v_arrh, [1, 2, 3, 0])
self.assertEqual(v, ros_numpy.msgify(Vector3, v_arr))
self.assertEqual(v, ros_numpy.msgify(Vector3, v_arrh))
with self.assertRaises(AssertionError):
ros_numpy.msgify(Vector3, np.array([0, 0, 0, 1]))
def test_vector3(self):
v = Vector3(1, 2, 3)
v_arr = ros_numpy.numpify(v)
np.testing.assert_array_equal(v_arr, [1, 2, 3])
v_arrh = ros_numpy.numpify(v, hom=True)
np.testing.assert_array_equal(v_arrh, [1, 2, 3, 0])
self.assertEqual(v, ros_numpy.msgify(Vector3, v_arr))
self.assertEqual(v, ros_numpy.msgify(Vector3, v_arrh))
with self.assertRaises(AssertionError):
ros_numpy.msgify(Vector3, np.array([0, 0, 0, 1]))
def on_enter(self, userdata):
gate = gates[userdata.leg_number]
if not gate.has_seen():
self._error = True
return
point = numpify(gate.position.point)
normal = numpify(gate.normal.vector)
target_pose = PoseStamped()
target_pose.header.frame_id = gate.position.header.frame_id
target_pose.pose.position = msgify(Point,
point + normal * self._offset)
target_pose.pose.orientation = msgify(Quaternion,
normal_to_quaternion(-normal))
userdata.target_pose = target_pose
def echoes_callback(echo):
echo['data'] = echo['data'][np.bitwise_and(
echo['data']['flags'],
0x01).astype(np.bool)] #keep valid echoes only
indices, flags, distances, amplitudes = [
echo['data'][x]
for x in ['indices', 'flags', 'distances', 'amplitudes']
]
stamp = rospy.Time.now()
if pub_raw.get_num_connections() > 0:
pub_raw.publish(
ros_numpy.msgify(PointCloud2, echo['data'], stamp, frame_id))
if pub_cloud.get_num_connections() > 0:
struct_cloud = np.empty(indices.size,
dtype=[('x', np.float32),
('y', np.float32),
('z', np.float32),
('intensity', np.float32)])
points = clouds.to_point_cloud(indices, distances, directions,
np.float32)
struct_cloud['x'] = points[:, 0]
struct_cloud['y'] = points[:, 1]
struct_cloud['z'] = points[:, 2]
struct_cloud['intensity'] = amplitudes
pub_cloud.publish(
ros_numpy.msgify(PointCloud2, struct_cloud, stamp, frame_id))
if pub_triangles.get_num_connections() > 0:
points, quad_amplitudes, quad_indices = clouds.to_quad_cloud(
indices, distances, amplitudes, quad_directions, specs.v,
specs.h, np.float32)
stl_points = points[quad_indices]
stl_amplitudes = quad_amplitudes[quad_indices]
marker = Marker()
marker.header.stamp = stamp
marker.header.frame_id = frame_id
marker.type = Marker.TRIANGLE_LIST
marker.scale = Vector3(1, 1, 1)
marker.points = [Point(x[0], x[1], x[2])
for x in stl_points] #TODO: optimize me
marker.colors = [ColorRGBA(a, a, a, 1)
for a in stl_amplitudes] #TODO: optimize me
pub_triangles.publish(marker)
def main():
colorama.init(autoreset=True)
parser = argparse.ArgumentParser(formatter_class=my_formatter)
parser.add_argument("results_dir",
type=pathlib.Path,
help='directory containing metrics.json')
parser.add_argument("--trial-idx",
type=int,
help='which plan to show',
default=0)
rospy.init_node("play_dragging_plan")
action_srv = rospy.ServiceProxy("execute_dual_gripper_action",
DualGripperTrajectory)
args = parser.parse_args()
with (args.results_dir / 'metadata.json').open('r') as metadata_file:
metadata_str = metadata_file.read()
metadata = json.loads(metadata_str)
scenario = get_scenario(metadata['scenario'])
with gzip.open(args.results_dir / f'{args.trial_idx}_metrics.json.gz',
'rb') as metrics_file:
metrics_str = metrics_file.read()
datum = json.loads(metrics_str.decode("utf-8"))
all_actions = []
steps = datum['steps']
for step_idx, step in enumerate(steps):
if step['type'] == 'executed_plan':
actions = step['planning_result']['actions']
elif step['type'] == 'executed_recovery':
actions = [step['recovery_action']]
all_actions.extend(actions)
for action in all_actions:
target_gripper1_point = ros_numpy.msgify(
Point, np.array(action['gripper_position']))
target_gripper1_point.z = -0.39
target_gripper2_point = ros_numpy.msgify(Point,
np.array([0.45, -0.2, 0.08]))
req = DualGripperTrajectoryRequest()
req.gripper1_points.append(target_gripper1_point)
req.gripper2_points.append(target_gripper2_point)
action_srv(req)
def test_bad_encodings(self):
mono_arr = np.random.randint(0, 256, size=(240, 360)).astype(np.uint8)
mono_arrf = np.random.randint(0, 256, size=(240, 360)).astype(np.float32)
rgb_arr = np.random.randint(0, 256, size=(240, 360, 3)).astype(np.uint8)
rgb_arrf = np.random.randint(0, 256, size=(240, 360, 3)).astype(np.float32)
with self.assertRaises(TypeError):
msg = ros_numpy.msgify(Image, rgb_arr, encoding='mono8')
with self.assertRaises(TypeError):
msg = ros_numpy.msgify(Image, mono_arrf, encoding='mono8')
with self.assertRaises(TypeError):
msg = ros_numpy.msgify(Image, rgb_arrf, encoding='rgb8')
with self.assertRaises(TypeError):
msg = ros_numpy.msgify(Image, mono_arr, encoding='rgb8')
def camera_callback(rgb_image, depth_image, camera_info):
global wgan
global is_initialized
global underwater_image_pub
global underwater_camera_info_pub
global max_depth
if is_initialized:
try:
cv_rgb_image = CvBridge().imgmsg_to_cv2(rgb_image, "rgb8")
data = ros_numpy.numpify(depth_image)
# fill NaNs with maximum depth value
# because simulated Kinect reports NaN where depth is higher than maximum hardware depth
where_are_NaNs = np.isnan(data)
data[where_are_NaNs] = max_depth
#cv2.normalize(data, data, 0, 255, cv2.NORM_MINMAX)
cv_depth_image = data
except CvBridgeError as e:
print(e)
cv_underwater_image = wgan.predict(cv_rgb_image, cv_depth_image)
#cv2.normalize(cv_underwater_image, cv_underwater_image, 0, 255, cv2.NORM_MINMAX)
#cv_underwater_image = np.array(cv_underwater_image).astype(np.uint8)
underwater_image = ros_numpy.msgify(Image, cv_underwater_image, "rgb8")
underwater_image.header = rgb_image.header
underwater_image_pub.publish(underwater_image)
underwater_camera_info_pub.publish(camera_info)
def to_point_cloud(self, color_map=None):
points = self.to_points(color_map)
point_cloud = ros_numpy.msgify(PointCloud2,
points,
stamp=self.last_update_time,
frame_id=self.voi.frame_id)
return point_cloud
def test_roundtrip_little_endian(self):
arr = np.random.randint(0, 256, size=(240, 360)).astype('<u2')
msg = ros_numpy.msgify(Image, arr, encoding='mono16')
self.assertEqual(msg.is_bigendian, False)
arr2 = ros_numpy.numpify(msg)
np.testing.assert_equal(arr, arr2)
def run(self):
while True:
#We process detection
detections, img = detect.Imageprocessing(self.net, "./treated_current_pic_"+self.target+".png")
#Send image to ROS topic
glob.pub.MUT.acquire()
glob.pub.image.publish(msgify(Image, jetson.utils.cudaToNumpy(img), "rgb8"))
glob.pub.MUT.release()
#if we detect something set flag to block ROS periodic sending, ROS thread clears it
if (len(detections) != 0):
print('size detections: ', len(detections))
glob.detection_flag.MUT.acquire()
glob.detection_flag.value = 1
glob.detection_flag.MUT.release()
glob.detection_flag.MUT.acquire() #Debug
print("FLAG = ", glob.detection_flag.value) #Debug
glob.detection_flag.MUT.release() #Debug
#Enumerate all the detections for hurdles detection
for detection in detections:
print("\nWe detected a " + self.net.GetClassDesc(detection.ClassID) + "\n")
glob.pub.MUT.acquire()
glob.pub.value.publish(self.detection_to_ROS_number(detection.ClassID))
glob.pub.left.publish(detection.Left)
glob.pub.right.publish(detection.Right)
glob.pub.top.publish(detection.Top)
glob.pub.bottom.publish(detection.Bottom)
glob.pub.MUT.release()
def sub_image(self, im_msg):
"""
Take in an image, and publish a black and white image indicatng which pixels are cone-like
"""
if self.lookup is None: return
old_message_header = deepcopy(im_msg.header)
# Announce that we have received a packet
self.pub_packet_entered_node.publish(old_message_header)
# extract the image
im = numpify(im_msg)
# Downsample.
if self.downsample < 1:
im = scipy.misc.imresize(im, self.downsample)
im_mask = self.lookup[self.lookup_idx(im)]
if self.morph_open:
open_size = self.morph_size
open_structure = np.ones((open_size, open_size))
im_mask = scipy.ndimage.morphology.binary_opening(
im_mask, iterations=self.morph_iter, structure=open_structure)
im_mask = (im_mask * 255).astype(np.uint8)
# build the message
im_mask_msg = msgify(Image, im_mask, encoding='mono8')
im_mask_msg.header = old_message_header
self.pub_mask.publish(im_mask_msg)
# Announce that a packet has left the node.
self.pub_packet_left_node.publish(old_message_header)
def main():
rospy.init_node("visualization_node", anonymous=True)
node = visualization_node()
context_box = zmq.Context()
socket_box = context_box.socket(zmq.SUB)
socket_box.setsockopt(zmq.SUBSCRIBE, b"")
print("Collecting bboxs...")
socket_box.connect("tcp://localhost:5557")
context_grid = zmq.Context()
socket_grid = context_grid.socket(zmq.SUB)
socket_grid.setsockopt(zmq.SUBSCRIBE, b"")
print("Collecting occupancy grid...")
socket_grid.connect("tcp://localhost:5558")
while not rospy.is_shutdown():
if socket_box.poll(timeout=1) != 0:
bbox = recv_array(socket_box)
line_msg = make_box_msg(bbox[0:8, :])
node.bbox_pub.publish(line_msg)
arrow_msg = make_arrow(bbox[16:18, :])
node.arrow_pub.publish(arrow_msg)
if socket_grid.poll(timeout=1) != 0:
grid_image = recv_array(socket_grid)
grid_image_msg = ros_numpy.msgify(Image,
grid_image,
encoding='rgb8')
node.grid_image_pub.publish(grid_image_msg)
def __init__(self, number): # int: (str) -> None
pose_name = 'S{}'.format(number)
position = np.array([
float(x) for x in rospy.get_param('named_poses/{}/position'.format(
pose_name))
])
orientation = np.array([
float(x) for x in rospy.get_param(
'named_poses/{}/orientation'.format(pose_name))
])
self._pose = PoseStamped()
self._pose.header.frame_id = 'map'
self._pose.pose.position = msgify(Point, position)
self._pose.pose.orientation = msgify(Quaternion, orientation)
self._contains = set()
self._number = number
def callback(data):
data_np = ros_numpy.numpify(data)
coords = np.zeros((2, data_np.shape[0]))
coords[0] = data_np['x']
coords[1] = data_np['y']
z = data_np['z']
coords = np.floor(
np.multiply(np.divide(np.add(coords, 100), 200),
img_size - 1)).astype(int)
img_np = np.zeros((img_size, img_size, 3), dtype=np.uint8)
for i in range(data_np.shape[0]):
img_np[coords[0][i], coords[1][i]] += (z[i] * gain).astype(np.uint8)
top_left_bounds = np.ceil(img_size / 2).astype(int) - mask_size // 2
bottom_right_bounds = np.ceil(img_size / 2).astype(int) + mask_size // 2
img_mask = np.flip(img_np[top_left_bounds:bottom_right_bounds,
top_left_bounds:bottom_right_bounds])
img_mask[25, 25] = (255, 0, 0)
img_mask[24, 25] = (0, 255, 0)
img_mask[23, 25] = (0, 255, 0)
pubMask.publish(ros_numpy.msgify(Image, img_mask, encoding='rgb8'))
def calculate_joints(self, position):
self.joint_angle_matricies = []
for angle in position:
self.joint_angle_matricies.append(
tf.transformations.euler_matrix(0, 0, angle))
T_c = [np.identity(4)]
link_states = LinkStates()
for index in xrange(len(self.urdf_tranform_matricies)):
urdf_transform_matrix = self.urdf_tranform_matricies[index]
joint_angle_matrix = self.joint_angle_matricies[index]
transform_matrix = np.dot(urdf_transform_matrix,
joint_angle_matrix)
if index in (6, 7, 8): #sets parent of fingers to link6
index = 6
elif index in (9, 10, 11): #sets parent of fingertip
index -= 2
T_c.append(np.dot(T_c[index], transform_matrix))
link_states.pose.append(msgify(Pose, T_c[-1]))
link_states.name.append(self.urdf_transforms[index].child_frame_id)
#print transforms.header
#print link_states.name[-1]
#print link_states.pose[-1]
#print '-----------------------------------------------'
self.link_states = link_states
def _video_thread(self):
container = av.open(self._tello.get_udp_video_address())
frame_skip = 300
seq = 0
while not rospy.is_shutdown():
for frame in container.decode(video=0):
if 0 < frame_skip:
frame_skip = frame_skip - 1
continue
start_time = time.time()
image_msg_time = rospy.Time.now()
image = np.array(frame.to_image())
image_msg = ros_numpy.msgify(Image, image, encoding='rgb8')
image_msg.header.stamp = image_msg_time
image_msg.header.seq = seq
self._camera_raw_pub.publish(image_msg)
image_msg = self._bridge.cv2_to_compressed_imgmsg(image)
image_msg.header.stamp = image_msg_time
image_msg.header.seq = seq
self._camera_pub.publish(image_msg)
if frame.time_base < 1.0 / 60:
time_base = 1.0 / 60
else:
time_base = frame.time_base
frame_skip = int((time.time() - start_time) / time_base)
seq += 1
def slam_callback(self, pose):
self.slam_pose_raw = ros_numpy.numpify(
pose.pose) #homogeneous transformation matrix from the origin
self.slam_pose_np = np.dot(self.slam_pose_correction,
self.slam_pose_raw)
self.slam_pose_np = self.slam_pose_np * self.scale_factor_matrix
self.slam_pose = ros_numpy.msgify(Pose, self.slam_pose_np)
#bebop camera link correction and camera calibration correction
ang_z = self.euler_from_pose(self.slam_pose)[2]
self.slam_pose.position.x += -0.11 * np.cos(ang_z)
self.slam_pose.position.y += -0.11 * np.sin(ang_z)
self.slam_pose_np = ros_numpy.numpify(self.slam_pose)
self.last_slam_time = time.time()
if not self.odom_pose == None:
#calculates the transfor matrix for the odom position to the modified slam coords system (assumed as true value)
self.odom_pose_correction = np.dot(
np.linalg.inv(self.odom_pose_raw_np), self.slam_pose_np)
else:
rospy.loginfo("Havent received any odom coord yet!")
self.current_pose = self.slam_pose
self.current_pose_np = self.slam_pose_np
def test_roundtrip_little_endian(self):
arr = np.random.randint(0, 256, size=(240, 360)).astype('<u2')
msg = ros_numpy.msgify(Image, arr, encoding='mono16')
self.assertEqual(msg.is_bigendian, False)
arr2 = ros_numpy.numpify(msg)
np.testing.assert_equal(arr, arr2)
def _callback(self, goal):
t_start = rospy.get_time()
# Get image
image = cv2.cvtColor(self.bridge.compressed_imgmsg_to_cv2(goal.image),
cv2.COLOR_BGR2RGB)
# Calculate mask
with torch.no_grad():
image_tensor = torch.tensor(
prepare_img(image).transpose(2, 0, 1)[None]).float()
image_input = image_tensor.cuda(self.gpu)
mask = self.model(image_input)[0].data.cpu().numpy().transpose(
1, 2, 0)
mask = cv2.resize(mask,
image.shape[:2][::-1],
interpolation=cv2.INTER_CUBIC)
mask = mask.argmax(axis=2).astype(np.uint8)
# Visualize results
if self.visualization_activated:
image[:, :, 0][mask == 0] = 0
image[:, :, 1][mask == 0] = 0
image[:, :, 2][mask == 0] = 0
self.pub_visualization.publish(
ros_numpy.msgify(Image, image, encoding='8UC3'))
# Publish results
print('Body detection successful. Current Hz-rate:\t' +
str(1 / (rospy.get_time() - t_start)))
self.server.set_succeeded(
SemSegBodyActResult(mask=self.bridge.cv2_to_compressed_imgmsg(
mask, dst_format='png')))
def test_image_encoding(self):
if not rospy.get_node_uri():
rospy.init_node("topic_store_tests")
client = MongoStorage(collection="python_tests")
# Test ROS_Message Serialisation with common Image Types
from topic_store.utils import _numpy_types as numpy_types
for encoding, np_type in numpy_types.items():
size = (32, 32, np_type[1]) if np_type[1] != 1 else (32, 32)
random_array = np.random.random(size)
typed_array = (random_array * 255.0).astype(np_type[0])
message = ros_numpy.msgify(Image, typed_array, encoding=encoding)
# Insert image message
im_document = TopicStore({"image": message})
im_result = client.insert_one(im_document)
# Get image message and check data is the same
returned_im_document = client.find_by_id(im_result.inserted_id)
assert returned_im_document.id == im_result.inserted_id
retrieved_array = ros_numpy.numpify(
returned_im_document.msgs["image"])
np.testing.assert_equal(typed_array, retrieved_array)
# Delete image message
client.delete_by_id(im_result.inserted_id)
def _callback(self, goal):
t_start = rospy.get_time()
# Get image
image = cv2.cvtColor(self.bridge.compressed_imgmsg_to_cv2(goal.image),
cv2.COLOR_BGR2RGB)
# Calculate Mask
mask = hand_segmentation(image, self.segmentation_module)
# Visualize results
if self.visualization_activated:
image[:, :, 0][mask == 0] = 0
image[:, :, 1][mask == 0] = 0
image[:, :, 2][mask == 0] = 0
self.pub_visualization.publish(
ros_numpy.msgify(Image, image, encoding='8UC3'))
# Publish results
print('Body detection successful. Current Hz-rate:\t' +
str(1 / (rospy.get_time() - t_start)))
self.server.set_succeeded(
SemSegHandActResult(mask=self.bridge.cv2_to_compressed_imgmsg(
mask, dst_format='png')))
def lidar_callback(self, msg):
# time1 = time.time()
pc = ros_numpy.numpify(msg)[:, self.ind_list].flatten()
pc = pc[np.where((pc['x'] < -1.5) & (pc['y'] > -1.5)
& (pc['y'] < 1.5))]
pc_cropped = np.empty((3, pc['x'].shape[0]))
pc_cropped[0, :] = pc['x']
pc_cropped[1, :] = pc['y']
pc_cropped[2, :] = pc['z']
pc_transformed = self.rotation(pc_cropped)
pc_transformed = np.concatenate(
(pc_transformed, np.expand_dims(pc['intensity'], axis=0)), axis=0)
# print(pc_transformed.shape)
pc['x'] = pc_transformed[0, :]
pc['y'] = pc_transformed[1, :]
pc['z'] = pc_transformed[2, :]
msg_transformed = ros_numpy.msgify(PointCloud2, pc)
msg_transformed.header.frame_id = '/os1_lidar'
self.pc_pub.publish(msg_transformed)
# print('elapsed time: ', time.time() - time1)
# self.send_array(self.socket, pc_transformed)
print(str(msg.header.stamp))
np.save(str(msg.header.stamp) + '.npy', pc_transformed)
rospy.sleep(1)
def sub_image(self, im_msg):
"""
Take in an image, and publish a black and white image indicatng which pixels are cone-like
"""
if self.lookup is None: return
old_message_header = deepcopy(im_msg.header)
# Announce that we have received a packet
self.pub_packet_entered_node.publish(old_message_header)
# extract the image
im = numpify(im_msg)
# Downsample.
if self.downsample < 1:
im = scipy.misc.imresize(im, self.downsample)
im_mask = self.lookup[self.lookup_idx(im)]
if self.morph_open:
open_size = self.morph_size
open_structure = np.ones((open_size, open_size))
im_mask = scipy.ndimage.morphology.binary_opening(
im_mask, iterations=self.morph_iter, structure=open_structure)
im_mask = (im_mask * 255).astype(np.uint8)
# build the message
im_mask_msg = msgify(Image, im_mask, encoding='mono8')
im_mask_msg.header = old_message_header
self.pub_mask.publish(im_mask_msg)
# Announce that a packet has left the node.
self.pub_packet_left_node.publish(old_message_header)
def test_roundtrip_numpy(self):
points_arr = self.makeArray(100)
cloud_msg = ros_numpy.msgify(numpy_msg(PointCloud2), points_arr)
new_points_arr = ros_numpy.numpify(cloud_msg)
np.testing.assert_equal(points_arr, new_points_arr)
def calculate_joints(self, position):
self.joint_angle_matricies = []
for angle in position:
self.joint_angle_matricies.append(transformations.euler_matrix(0, 0, angle))
T_c = [np.identity(4)]
tf_msg = TFMessage()
for index in xrange(len(self.urdf_transform_matricies)):
urdf_transform_matrix = self.urdf_transform_matricies[index]
joint_angle_matrix = self.joint_angle_matricies[index]
transform_matrix = np.dot(urdf_transform_matrix, joint_angle_matrix)
tf_stamp = TransformStamped()
tf_stamp.child_frame_id = self.urdf_transforms[index].child_frame_id
tf_stamp.header.frame_id = self.urdf_transforms[index].header.frame_id
if index in (8, 10): #sets parent of fingers to link6
index = 6
T_c.append(np.dot(T_c[index], transform_matrix))
tf_stamp.transform = msgify(Transform, T_c[-1])
#tf_stamp.transform = msgify(Transform, transform_matrix)
tf_msg.transforms.append(tf_stamp)
#print transforms.header
#print link_states.name[-1]
#print link_states.pose[-1]
#print '-----------------------------------------------'
return tf_msg
def merged_point_publish(self, point_center, point_left, point_right):
print('start merging...')
self.merhing_flag = True
self.start_time = rospy.Time.now().to_sec()
point_left = do_transform_cloud(point_left, self.tf_left)
point_right = do_transform_cloud(point_right, self.tf_right)
point_center = numpify(point_center)
point_left = numpify(point_left)
point_right = numpify(point_right)
point_set = np.append(np.append(point_center, point_left), point_right)
# point_set = np.append(point_right, point_left)
merged_points = msgify(PointCloud2, point_set)
pub1.publish(merged_points)
self.elapsed = rospy.Time.now().to_sec() - self.start_time
pub2.publish(self.elapsed)
print('Merged PointCloud Data Set is Published')
self.center_flag = False
self.left_flag = False
self.right_flag = False
self.merhing_flag = False
def test_convert_dtype_inner(self):
fields = [
PointField(name='x',
offset=0,
count=1,
datatype=PointField.FLOAT32),
PointField(name='y',
offset=4,
count=1,
datatype=PointField.FLOAT32),
PointField(name='vectors',
offset=8,
count=3,
datatype=PointField.FLOAT32)
]
dtype = np.dtype([('x', np.float32), ('y', np.float32),
('vectors', np.float32, (3, ))])
conv_fields = ros_numpy.msgify(PointField, dtype, plural=True)
self.assertSequenceEqual(fields, conv_fields,
'dtype->Pointfield with inner dimensions')
conv_dtype = ros_numpy.numpify(fields, point_step=8)
self.assertEqual(dtype, conv_dtype,
'Pointfield->dtype with inner dimensions')
def publish_pc_bnds(self, pc_bnds_dict):
pc_bnds = np.concatenate(
(
pc_bnds_dict["frontal_low_pc"],
#pc_bnds_dict["frontal_mid_pc"],
pc_bnds_dict["fl_flipper_pc"],
pc_bnds_dict["fr_flipper_pc"],
pc_bnds_dict["rl_flipper_pc"],
pc_bnds_dict["rr_flipper_pc"]),
axis=1)
pc_bnds_data = np.zeros(len(pc_bnds[0]),
dtype=[('x', np.float32), ('y', np.float32),
('z', np.float32),
('vectors', np.float32, (3, ))])
pc_bnds_data['x'] = pc_bnds[0, :]
pc_bnds_data['y'] = pc_bnds[1, :]
pc_bnds_data['z'] = pc_bnds[2, :]
pc_bnds_data['vectors'] = np.arange(len(pc_bnds[0]))[:, np.newaxis]
msg = ros_numpy.msgify(PointCloud2, pc_bnds_data)
msg.header.frame_id = self.config["robot_prefix"] + "/" + self.config[
"bl_zpr_frame_name"]
msg.header.stamp = rospy.Time.now()
self.pc_bnds_publisher.publish(msg)
def map_timer_callback(self, event):
if self.debug_merged_map:
map_msg = ros_numpy.msgify(ros_numpy.numpy_msg(OccupancyGrid), self.map.grid, info=self.map.info)
map_msg.header = Header()
map_msg.header.stamp = rospy.Time.now()
map_msg.header.frame_id = self.map.frame
self.pub_merged_map.publish(map_msg)
def _make_local_map(self, scan):
#target frame is map_frame, source frame is laser_link
trans = self.tf_buffer.lookup_transform(
target_frame=self.map_frame,
source_frame=scan.header.frame_id,
time=scan.header.stamp,
timeout=rospy.Duration(5)
)
pos = trans.transform.translation
orient = trans.transform.rotation
# transform from base to map
transform = np.dot(
transformations.translation_matrix([pos.x, pos.y, pos.z]),
transformations.quaternion_matrix([orient.x, orient.y, orient.z, orient.w])
)
self.map_info.origin.position.x = pos.x - self.map_info.resolution*self.width/2.0
self.map_info.origin.position.y = pos.y - self.map_info.resolution*self.height/2.0
self.ros_map.info = self.map_info
self.ros_map.data[...] = -1
mcl_local_map = mcl.Map(self.ros_map)
mask = (scan.ranges < scan.range_max) & (scan.ranges > scan.range_min)
angles = np.arange(scan.angle_min,scan.angle_max,scan.angle_increment)
x = scan.ranges[mask]*np.cos(angles[mask])
y = scan.ranges[mask]*np.sin(angles[mask])
# set the last component to zero to ignore translation
ii, jj = mcl_local_map.index_at(
np.vstack((x,y,np.zeros(np.sum(mask)),np.ones(np.sum(mask)))).T,
world_to_map=transform
)
ok = (
(jj >= 0) & (jj < self.width) &
(ii >= 0) & (ii < self.height)
)
ii = ii[ok]
jj = jj[ok]
mcl_local_map.grid[ii,jj] = 100
### TODO: figure out faster serialization issue
map_msg = msgify(numpy_msg(OccupancyGrid), mcl_local_map.grid, info=self.map_info)
map_msg.header = scan.header
map_msg.header.frame_id = 'map'
return map_msg
def _make_local_map(self, point):
#target frame is map_frame, source frame is laser_link
self.map_info.origin.position = point.point
self.ros_map.info = self.map_info
self.ros_map.data[...] = -1
mcl_local_map = mcl.Map(self.ros_map)
mcl_local_map.grid[:,:] = 100
### TODO: figure out faster serialization issue
map_msg = msgify(numpy_msg(OccupancyGrid), mcl_local_map.grid, info=self.map_info)
map_msg.header = point.header
map_msg.header.frame_id = 'map'
return map_msg
def sub_image(self, im_msg):
"""
Take in an image, and publish a black and white image indicatng which pixels are cone-like
"""
old_message_header = deepcopy(im_msg.header)
# Announce that we have received a packet
self.pub_packet_entered_node.publish(old_message_header)
# extract the image
im = numpify(im_msg)
# Downsample.
if self.downsample < 1:
im = scipy.misc.imresize(im, self.downsample)
if self.mode == 'linear':
is_cone = np.ones(im.shape[:-1], dtype=np.bool)
im = im.astype(np.float32)
# red channel
for theta in thetas:
is_cone = is_cone & (im.dot(theta[:-1]) + theta[-1] > 0)
im_mask = np.uint8(is_cone * 255)
elif self.mode == 'lookup':
self._init_lookup()
im_mask = self.lookup[self.lookup_idx(im)]
else:
raise ValueError("Unrecognized thresholding mode.")
if self.morph_open:
open_size = self.morph_size
open_structure = np.ones((open_size, open_size))
im_mask = scipy.ndimage.morphology.binary_opening(
im_mask, iterations=self.morph_iter, structure=open_structure)
im_mask = (im_mask * 255).astype(np.uint8)
# build the message
im_mask_msg = msgify(Image, im_mask, encoding='mono8')
im_mask_msg.header = old_message_header
self.pub_mask.publish(im_mask_msg)
# Announce that a packet has left the node.
self.pub_packet_left_node.publish(old_message_header)
def test_pose(self):
t = Pose(
position=Point(1.0, 2.0, 3.0),
orientation=Quaternion(*transformations.quaternion_from_euler(np.pi, 0, 0))
)
t_mat = ros_numpy.numpify(t)
np.testing.assert_allclose(t_mat.dot([0, 0, 1, 1]), [1.0, 2.0, 2.0, 1.0])
msg = ros_numpy.msgify(Pose, t_mat)
np.testing.assert_allclose(msg.position.x, t.position.x)
np.testing.assert_allclose(msg.position.y, t.position.y)
np.testing.assert_allclose(msg.position.z, t.position.z)
np.testing.assert_allclose(msg.orientation.x, t.orientation.x)
np.testing.assert_allclose(msg.orientation.y, t.orientation.y)
np.testing.assert_allclose(msg.orientation.z, t.orientation.z)
np.testing.assert_allclose(msg.orientation.w, t.orientation.w)
def test_transform(self):
t = Transform(
translation=Vector3(1, 2, 3),
rotation=Quaternion(*transformations.quaternion_from_euler(np.pi, 0, 0))
)
t_mat = ros_numpy.numpify(t)
np.testing.assert_allclose(t_mat.dot([0, 0, 1, 1]), [1.0, 2.0, 2.0, 1.0])
msg = ros_numpy.msgify(Transform, t_mat)
np.testing.assert_allclose(msg.translation.x, t.translation.x)
np.testing.assert_allclose(msg.translation.y, t.translation.y)
np.testing.assert_allclose(msg.translation.z, t.translation.z)
np.testing.assert_allclose(msg.rotation.x, t.rotation.x)
np.testing.assert_allclose(msg.rotation.y, t.rotation.y)
np.testing.assert_allclose(msg.rotation.z, t.rotation.z)
np.testing.assert_allclose(msg.rotation.w, t.rotation.w)
def sub_marker(self, marker):
marker_loc = numpify(marker.point, hom=True)
if np.isnan(marker_loc).any():
self.marker_target_pose = None
return
trans = self.tf_buffer.lookup_transform(
target_frame="base_link",
source_frame=self.map_frame,
time=marker.header.stamp,
timeout=rospy.Duration(1)
)
transform = numpify(trans.transform)
marker_loc = marker_loc.dot(transform.T)
marker_target_pose = PoseStamped()
marker_target_pose.header = marker.header
marker_target_pose.header.frame_id = 'map'
marker_target_pose.pose = Pose(
msgify(Point, marker_loc),
Quaternion(0,0,0,1)
)
self.marker_target_pose = marker_target_pose
def sub_image(self, im_msg, image_info):
"""
Take a thresholded image and output cone position metrics.
"""
# Announce that we have received a packet
self.pub_packet_entered_node.publish(im_msg.header)
# Extract the image
im = numpify(im_msg)
im2 = (im > 160).astype(bool)
height, width = im.shape[:2]
# Label each connected blob in the image.
labels, num_labels = scipy.ndimage.measurements.label(im2)
msg = CameraObjectsStamped(header=im_msg.header)
if num_labels == 0:
self.pub_obj.publish(msg)
# No objects.
return
# Calculate the size of each blob.
blob_sizes = scipy.ndimage.measurements.sum(im2, labels=labels, index=xrange(0,num_labels+1))
# Find the biggest blob.
biggest_blob = np.argmax(blob_sizes) - 1
pp = scipy.ndimage.measurements.find_objects(labels)[biggest_blob]
bbox = BoundingBox(pp[1].start, pp[0].start, pp[1].stop, pp[0].stop)
# Calculate centroid (units of pixels).
cx, cy = np.mean([bbox.maxx, bbox.minx]), np.mean([bbox.maxy, bbox.miny])
sx, sy = (bbox.maxx - bbox.minx), (bbox.maxy - bbox.miny)
# geometric mean
p_matrix = np.array(image_info.P).reshape((3, 4))
uvw = p_matrix.dot([self.WIDTH, self.HEIGHT, 0, 0])
u, v, w = uvw
cz = np.min([(u / sx), (v / sy)])
# load the depth image message
# d_im = numpify(d_im_msg) * 0.001
# TODO
# cz = np.mean(d_im[(labels == biggest_blob) & (d_im != 0)]) # get_depth_for(biggest_blob)
# print u / sx, v / sy, sx, sy, uvw #, np.max(d_im)
# projection matrix
# [px, py, 1] = P.dot([X, Y, Z, 1])
# solve the simultaneous equations
# [cx, cy, 1] = P.dot([X, Y, Z, 1])
# cz = Z
# => 0 = np.array([0, 0, 1, -cz]).dot([X, Y, Z, 1])
try:
position = np.linalg.solve(
np.vstack((
p_matrix,
[0, 0, 1, -cz]
)),
np.array([cx, cy, 1, 0])
)
except np.linalg.LinAlgError:
rospy.logwarn("Matrix was singular %s", np.vstack((
p_matrix,
[0, 0, 1, -cz]
)))
return
# theses are homogeneous coordinates
position /= position[-1]
position = Point(*position[:3])
msg.objects = [
CameraObject(
label='cone',
center=position,
size=Vector3(
x=remap((bbox.maxx - bbox.minx), 0, width, 0, 1),
y=remap((bbox.maxy - bbox.miny), 0, height, 0, 1),
z=0
)
)
]
self.pub_obj.publish(msg)
# Make overlay visualizations.
debug_overlay = np.zeros((height, width, 3), dtype=np.uint8)
debug_overlay[...,0] = im
debug_overlay[...,1] = im
debug_overlay[...,2] = im
self.draw_bbox(debug_overlay, bbox)
self.draw_point(debug_overlay, cx, cy)
debug_overlay_msg = msgify(Image, debug_overlay, encoding='rgb8')
debug_overlay_msg.header = im_msg.header
self.pub_debug_overlay.publish(debug_overlay_msg)
# Announce that a packet has left the node.
self.pub_packet_left_node.publish(im_msg.header)
def test_roundtrip_rgb8(self):
arr = np.random.randint(0, 256, size=(240, 360, 3)).astype(np.uint8)
msg = ros_numpy.msgify(Image, arr, encoding='rgb8')
arr2 = ros_numpy.numpify(msg)
np.testing.assert_equal(arr, arr2)
