def _convert_voxels_to_msg(self, voxels_with_level):
voxels_msg = []
for position, level in voxels_with_level:
voxel_msg = Voxel()
# voxel_msg = geometry_msgs.msg.Point()
ros_utils.point_numpy_to_ros(position, voxel_msg.position)
voxel_msg.level = level
# voxel_msg.x = voxel[0]
# voxel_msg.y = voxel[1]
# voxel_msg.z = voxel[2]
voxels_msg.append(voxel_msg)
return voxels_msg
def perform_query_bbox(self,
bbox,
obs_level,
dense=False,
only_voxels_inside_bbox=False):
query_bbox_msg = QueryBBoxRequest()
ros_utils.point_numpy_to_ros(bbox.minimum(), query_bbox_msg.bbox_min)
ros_utils.point_numpy_to_ros(bbox.maximum(), query_bbox_msg.bbox_max)
query_bbox_msg.level = obs_level
query_bbox_msg.dense = dense
query_bbox_msg.only_voxels_inside_bbox = only_voxels_inside_bbox
logger.debug("Requesting query bbox")
return self._request_query_bbox(query_bbox_msg)
def _get_transform_msg_quat(self, location, orientation_quat):
# Create transform message
transform = Transform()
ros_utils.point_numpy_to_ros(location, transform.translation)
# transform.translation.x = location[0]
# transform.translation.y = location[1]
# transform.translation.z = location[2]
ros_utils.quaternion_numpy_to_ros(orientation_quat, transform.rotation)
# transform.rotation.x = orientation_quat[0]
# transform.rotation.y = orientation_quat[1]
# transform.rotation.z = orientation_quat[2]
# transform.rotation.w = orientation_quat[3]
return transform
def _convert_rays_to_msg(self, rays):
rays_msg = []
for ray in rays:
ray_msg = Ray()
ros_utils.point_numpy_to_ros(ray.origin(), ray_msg.origin)
# ray_msg.origin.x = ray.origin()[0]
# ray_msg.origin.y = ray.origin()[1]
# ray_msg.origin.z = ray.origin()[2]
ros_utils.point_numpy_to_ros(ray.direction(), ray_msg.direction)
# ray_msg.direction.x = ray.direction()[0]
# ray_msg.direction.y = ray.direction()[1]
# ray_msg.direction.z = ray.direction()[2]
rays_msg.append(ray_msg)
return rays_msg
def _fill_ros_marker_msg_with_pose(self, marker_msg, pose):
marker_msg.pose.position = ros_utils.point_numpy_to_ros(
pose.location())
orientation_quat = math_utils.convert_rpy_to_quat(
pose.orientation_rpy())
marker_msg.pose.orientation = ros_utils.quaternion_numpy_to_ros(
orientation_quat)
def _request_clear_bounding_box_voxels(self, bbox, densify):
try:
request = ClearBoundingBoxRequest()
ros_utils.point_numpy_to_ros(bbox.minimum(), request.min)
ros_utils.point_numpy_to_ros(bbox.maximum(), request.max)
request.densify = densify
response = self._clear_bounding_box_service(request)
except (rospy.ServiceException,
rospy.exceptions.TransportTerminated) as exc:
logger.exception(
"WARNING: Clear bounding box service did not process request: {}"
.format(str(exc)))
logger.warn("WARNING: Trying to reconnect to service")
self._connect_services()
raise self.Exception("Clear bounding box failed: {}".format(exc))
assert response.success
return response
def _request_set_score_bounding_box(self, score_bbox, verbose=False):
try:
request = SetScoreBoundingBoxRequest()
ros_utils.point_numpy_to_ros(score_bbox.minimum(), request.min)
ros_utils.point_numpy_to_ros(score_bbox.maximum(), request.max)
response = self._set_score_bounding_box_service(request)
except (rospy.ServiceException,
rospy.exceptions.TransportTerminated) as exc:
logger.exception(
"WARNING: Set score bounding box service did not process request: {}"
.format(str(exc)))
logger.warn("WARNING: Trying to reconnect to service")
self._connect_services()
raise self.Exception("Set score bounding box: {}".format(exc))
if verbose:
logger.info("Set score bounding box took {}s".format(
response.elapsed_seconds))
return response
def perform_query_subvolume_quat(self,
center,
orientation_quat,
level,
size_x,
size_y,
size_z,
axis_mode=0):
subvolume_msg = QuerySubvolumeRequest()
ros_utils.point_numpy_to_ros(center, subvolume_msg.center)
ros_utils.quaternion_numpy_to_ros(orientation_quat,
subvolume_msg.orientation)
subvolume_msg.level = level
subvolume_msg.size_x = size_x
subvolume_msg.size_y = size_y
subvolume_msg.size_z = size_z
subvolume_msg.axis_mode = axis_mode
logger.debug("Requesting query subvolume")
return self._request_query_subvolume(subvolume_msg)
def _pose_to_ros_pose_msg(self, pose, time=None):
pose_msg = PoseStamped()
pose_msg.header.frame_id = self._world_frame
if time is None:
pose_msg.header.stamp = rospy.Time.now()
pose_msg.pose.position = ros_utils.point_numpy_to_ros(pose.location())
orientation_quat = math_utils.convert_rpy_to_quat(
pose.orientation_rpy())
pose_msg.pose.orientation = ros_utils.quaternion_numpy_to_ros(
orientation_quat)
return pose_msg
def _request_override_bounding_box_voxels(self, bbox, occupancy,
observation_count, densify):
try:
request = OverrideBoundingBoxRequest()
ros_utils.point_numpy_to_ros(bbox.minimum(), request.min)
ros_utils.point_numpy_to_ros(bbox.maximum(), request.max)
request.occupancy = occupancy
request.observation_count = observation_count
request.densify = densify
response = self._override_bounding_box_service(request)
except (rospy.ServiceException,
rospy.exceptions.TransportTerminated) as exc:
logger.warn(
"WARNING: Override bounding box service did not process request: {}"
.format(str(exc)))
logger.warn("WARNING: Trying to reconnect to service")
self._connect_services()
raise self.Exception("Override bounding box: {}".format(exc))
assert response.success
return response