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