diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 00000000..29b9ef04 --- /dev/null +++ b/.dockerignore @@ -0,0 +1,2 @@ +python/Dockerfile +ouster_ros/Dockerfile diff --git a/CHANGELOG.rst b/CHANGELOG.rst index ec8310ed..1b75e4e2 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,27 +2,60 @@ Changelog ========= -[unreleased] -============ -Added ------ -* first release of Python bindings for the provided C++ code. See the README under the ``python`` - directory for details and links to documentation -* early version of a C++ API covering the full sensor configuration interface -* preliminary C++ API for working with pcap files containing a single sensor packet capture - - -Changed -------- - -* reflectivity visualization for changes in the upcoming 2.1 firmware -* moved most of the visualizer code out of public headers and hid some implementation details +[20210608] +========== -Fixed ------ +ouster_client +------------- +* update cmake package version to 0.2.0 +* add support for new signal multiplier config parameter +* add early version of a C++ API covering the full sensor configuration interface +* increase default initialization timeout to 60 seconds to account for the worst case: waking up + from STANDBY mode + +ouster_pcap +----------- +* ``record_packet()`` now requires passing in a capture timestamp instead of using current time +* work around libtins issue where capture timestamps for pcaps recorded on Windows are always zero +* add preliminary C++ API for working with pcap files containing a single sensor packet capture + +ouster_ros +---------- +* update ROS package version to 0.2.0 +* add Dockerfile to easily set up a build environment or run nodes +* ``img_node`` now outputs 16-bit images, which should be more useful. Range image output is now in + units of 4mm instead of arbitrary scaling (addresses #249) +* ``img_node`` now outputs reflectivity images as well on the ``reflec_image`` topic +* change ``img_node`` topics to match terminology in sensor documentation: ``ambient_image`` is now + ``nearir_image`` and ``intensity_image`` is now ``signal_image`` +* update rviz config to use flat squares by default to work around `a bug on intel systems + `_ +* remove viz_node and all graphics stack dependencies from the package. The ``viz`` flag on the + launch file now runs rviz (addresses #236) +* clean up package.xml and ensure that dependencies are installable with rosdep (PR #219) +* the ``metadata`` argument to ouster_ros launch file is now required. No longer defaults to a name + based on the hostname of the sensor + +ouster_viz +---------- +* update reflectivity visualization for changes in the upcoming 2.1 firmware. Add new colormap and + handle 8-bit reflectivity values +* move most of the visualizer code out of public headers and hide some implementation details +* fix visualizer bug causing a small viewport when resizing the window on macos with a retina + display + +python +------ +* update ouster-sdk version to 0.2.1 +* fix bug in determining if a scan is complete with single-column azimuth windows +* closed PacketSource iterators will now raise an exception on read +* add examples for visualization using open3d (see: ``ouster.sdk.examples.open3d``) +* add support for the new signal multiplier config parameter +* preserve capture timestamps on packets read from pcaps +* first release: version 0.2.0 of ouster-sdk. See the README under the ``python`` directory for + details and links to documentation -* visualizer bug causing a small viewport when resizing the window on macos with a retina display [20201209] ========== @@ -47,6 +80,7 @@ Changed * add client version field to metadata json, logs, and help text * client API renaming to better reflect the Sensor Software Manual + [1.14.0-beta.14] - 2020-08-27 ============================= @@ -79,16 +113,19 @@ Fixed * the reference frame of point cloud topics in ``ouster_ros`` is now correctly reported as the "sensor frame" defined in the user guide + [1.14.0-beta.12] - 2020-07-10 ============================= *no changes* + [1.14.0-beta.11] - 2020-06-17 ============================= *no changes* + [1.14.0-beta.10] - 2020-05-21 ============================= @@ -117,6 +154,7 @@ Fixed origin offset * minor regression with destaggering in img_node output in previous beta + [1.14.0-beta.4] - 2020-03-17 ============================ @@ -134,6 +172,7 @@ Changed * use random ports for lidar and imu data by default when unspecified + [1.13.0] - 2020-03-16 ===================== @@ -157,6 +196,7 @@ Fixed * use correct name for json dependency in ``package.xml`` (PR #116) * handle udp socket creation error gracefully in client + [1.12.0] - 2019-05-02 ===================== @@ -177,6 +217,7 @@ Fixed * visualizer issue where the point cloud would occasionally occasionally not be displayed using newer versions of Eigen + [1.11.0] - 2019-03-26 ===================== @@ -199,6 +240,7 @@ Fixed * misplaced sine in azimuth angle calculation (addresses #42) * populate timestamps on image node output (addresses #39) + [1.10.0] - 2019-01-27 ===================== diff --git a/CMakeLists.txt b/CMakeLists.txt index e8679968..4f2b3189 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,6 +10,7 @@ project(ouster_example) option(CMAKE_POSITION_INDEPENDENT_CODE "Build position independent code." ON) option(BUILD_SHARED_LIBS "Build shared libraries." OFF) option(BUILD_VIZ "Build Ouster visualizer." ON) +option(BUILD_PCAP "Build pcap utils." OFF) set(CMAKE_CXX_STANDARD 11) if(MSVC) @@ -21,7 +22,10 @@ endif() # === Subdirectories === add_subdirectory(ouster_client) -add_subdirectory(ouster_pcap) + +if(BUILD_PCAP) + add_subdirectory(ouster_pcap) +endif() if(BUILD_VIZ) add_subdirectory(ouster_viz) diff --git a/README.rst b/README.rst index 982db7db..3571e542 100644 --- a/README.rst +++ b/README.rst @@ -12,7 +12,8 @@ Summary ======= To get started building the client and visualizer libraries, see the `Sample Client and Visualizer`_ -section below. For instructions on ROS, start with the `Example ROS Code`_ section. +section below. For instructions on ROS, start with the `Example ROS Code`_ section. Python SDK users +should proceed straight to our `python SDK homepage `_. This repository contains sample code for connecting to and configuring ouster sensors, reading and visualizing data, and interfacing with ROS. @@ -20,6 +21,7 @@ visualizing data, and interfacing with ROS. * `ouster_client `_ contains an example C++ client for ouster sensors * `ouster_viz `_ contains a basic point cloud visualizer * `ouster_ros `_ contains example ROS nodes for publishing point cloud messages +* `python `_ contains the code for the ouster sensor python SDK Sample Client and Visualizer @@ -35,7 +37,7 @@ Building on Linux / macOS To install build dependencies on Ubuntu, run:: sudo apt install build-essential cmake libglfw3-dev libglew-dev libeigen3-dev \ - libjsoncpp-dev libtclap-dev libtins-dev libpcap-dev + libjsoncpp-dev libtclap-dev On macOS, install XCode and `homebrew `_ and run:: @@ -52,6 +54,7 @@ where ```` is the location of the ``ouster_example`` sou CMake build script supports several optional flags:: -DBUILD_VIZ=OFF Do not build the sample visualizer + -DBUILD_PCAP=ON Build pcap tools. Requres libpcap and libtins dev packages -DBUILD_SHARED_LIBS Build shared libraries (.dylib or .so) -DCMAKE_POSITION_INDEPENDENT_CODE Standard flag for position independent code @@ -142,11 +145,10 @@ Keyboard controls: ``p`` Increase point size ``o`` Decrease point size ``m`` Cycle point cloud coloring mode - ``v`` Toggle color cycling in range image + ``v`` Toggle range cycling ``n`` Toggle display near-IR image from the sensor - ``r`` Toggle auto-rotating ``shift + r`` Reset camera - ``e`` Change range and signal image size + ``e`` Change size of displayed 2D images ``;`` Increase spacing in range markers ``'`` Decrease spacing in range markers ``r`` Toggle auto rotate @@ -180,9 +182,9 @@ Building The build dependencies include those of the sample code:: sudo apt install build-essential cmake libglfw3-dev libglew-dev libeigen3-dev \ - libjsoncpp-dev libtclap-dev libtins-dev libpcap-dev + libjsoncpp-dev libtclap-dev -and, additionally:: +Additionally, you should install the ros dependencies:: sudo apt install ros--ros-core ros--pcl-ros \ ros--tf2-geometry-msgs ros--rviz @@ -191,6 +193,7 @@ where ```` is ``kinetic``, ``melodic``, or ``noetic``. Alternatively, if you would like to install dependencies with `rosdep`:: + rosdep install --from-paths To build:: @@ -226,14 +229,14 @@ where: * ```` can be the hostname (os-99xxxxxxxxxx) or IP of the sensor * ```` is the hostname or IP to which the sensor should send data -* ```` is an optional path to json file to save calibration metadata +* ```` is the path to the json file to which to save calibration metadata * ```` is one of ``512x10``, ``512x20``, ``1024x10``, ``1024x20``, or ``2048x10``, and * ```` is either ``true`` or ``false``: if true, a window should open and start displaying data after a few seconds. -Note that if the ``metadata`` parameter is not specified, this command will write metadata to -``${ROS_HOME}``. By default, the name of this file is based on the hostname of the sensor, e.g. -``os-99xxxxxxxxxx.json``. +Note that by default the working directory of all ROS nodes is set to ``${ROS_HOME}``, generally +``$HOME/.ros``, so if ``metadata`` is a relative path, it will write to that path inside +``${ROS_HOME}``. To avoid this, you can provide an absolute path to ``metadata``. Recording Data -------------- @@ -245,10 +248,10 @@ another terminal, run:: This will save a bag file of recorded data in the current working directory. -You should copy and save the metadata file alongside your data. The metadata file be saved either at -the provided path to `roslaunch` or at ``$(ROS_HOME)/.json`` if you did not provide -a metadata argument to `roslaunch`. If you do not save the metadata file, you will not be able to -replay your data later. +You should copy and save the metadata file alongside your data. The metadata file will be saved at +the provided path to `roslaunch`. If you run the node and cannot find the metadata file, try looking +inside your ``${ROS_HOME}``, generally ``$HOME/.ros``. Regardless, you must retain the metadata +file, as you will not be able to replay your data later without it. .. _rosbag record: https://wiki.ros.org/rosbag/Commandline#rosbag_record @@ -269,16 +272,11 @@ to obtain the metadata file when recording your data. .. _rosbag play: https://wiki.ros.org/rosbag/Commandline#rosbag_play -Visualizing Data in Rviz ------------------------- - -To display sensor output using built-in ROS tools (rviz), follow the instructions above for running -the example ROS code with a sensor or recorded data. Then, run:: - rviz -d ouster_example/ouster_ros/viz.rviz +Ouster Python SDK +================= -in another terminal with the ROS environment set up. To view lidar signal, near-IR, and range -images, add ``image:=true`` to the ``roslaunch`` command above. +Python SDK users should proceed straight to the `Ouster python SDK homepage `_. Additional Information diff --git a/ouster_client/CMakeLists.txt b/ouster_client/CMakeLists.txt index 9d488e3a..f3184e63 100644 --- a/ouster_client/CMakeLists.txt +++ b/ouster_client/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.1.0) # ==== Project Name ==== project(ouster_client VERSION 0.2.0) -set(ouster_client_VERSION_SUFFIX "-dev0") +set(ouster_client_VERSION_SUFFIX "") # ==== Requirements ==== find_package(Eigen3 REQUIRED) @@ -24,7 +24,7 @@ target_include_directories(ouster_build INTERFACE ${CMAKE_CURRENT_BINARY_DIR}/in add_dependencies(ouster_build generate_build_header) # ==== Libraries ==== -add_library(ouster_client src/client.cpp src/types.cpp src/netcompat.cpp src/lidar_scan.cpp) +add_library(ouster_client src/client.cpp src/types.cpp src/netcompat.cpp src/lidar_scan.cpp src/image_processing.cpp) target_link_libraries(ouster_client PUBLIC jsoncpp_lib Eigen3::Eigen ouster_build) if(WIN32) target_link_libraries(ouster_client PUBLIC ws2_32) diff --git a/ouster_client/include/ouster/client.h b/ouster_client/include/ouster/client.h index 5503017e..1cf479e0 100644 --- a/ouster_client/include/ouster/client.h +++ b/ouster_client/include/ouster/client.h @@ -53,7 +53,7 @@ std::shared_ptr init_client(const std::string& hostname, lidar_mode mode = MODE_UNSPEC, timestamp_mode ts_mode = TIME_FROM_UNSPEC, int lidar_port = 0, int imu_port = 0, - int timeout_sec = 30); + int timeout_sec = 60); /** * Block for up to timeout_sec until either data is ready or an error occurs. @@ -98,7 +98,7 @@ bool read_imu_packet(const client& cli, uint8_t* buf, const packet_format& pf); * @param timeout_sec how long to wait for the sensor to initialize * @return a text blob of metadata parseable into a sensor_info struct */ -std::string get_metadata(client& cli, int timeout_sec = 30); +std::string get_metadata(client& cli, int timeout_sec = 60); /** * Get sensor config from the sensor diff --git a/ouster_viz/include/ouster/image_processing.h b/ouster_client/include/ouster/image_processing.h similarity index 100% rename from ouster_viz/include/ouster/image_processing.h rename to ouster_client/include/ouster/image_processing.h diff --git a/ouster_client/include/ouster/types.h b/ouster_client/include/ouster/types.h index 78fe1810..cd09aaa5 100644 --- a/ouster_client/include/ouster/types.h +++ b/ouster_client/include/ouster/types.h @@ -114,6 +114,7 @@ struct sensor_config { optional multipurpose_io_mode; optional azimuth_window; + optional signal_multiplier; optional nmea_in_polarity; optional nmea_ignore_valid_char; diff --git a/ouster_client/src/client.cpp b/ouster_client/src/client.cpp index 539cf31f..3b9aadbe 100644 --- a/ouster_client/src/client.cpp +++ b/ouster_client/src/client.cpp @@ -339,6 +339,11 @@ bool set_config_helper(SOCKET sock_fd, const sensor_config& config, !set_param("azimuth_window", to_string(config.azimuth_window.value()))) return false; + if (config.signal_multiplier && + !set_param("signal_multiplier", + std::to_string(config.signal_multiplier.value()))) + return false; + if (config.sync_pulse_out_angle && !set_param("sync_pulse_out_angle", std::to_string(config.sync_pulse_out_angle.value()))) @@ -446,8 +451,6 @@ bool set_config(const std::string& hostname, const sensor_config& config, std::string res; bool success = true; - success = set_config_helper(sock_fd, config, config_flags); - if (config_flags & CONFIG_UDP_DEST_AUTO) { if (config.udp_dest) { impl::socket_close(sock_fd); @@ -458,6 +461,10 @@ bool set_config(const std::string& hostname, const sensor_config& config, success &= res == "set_udp_dest_auto"; } + if (success) { + success = set_config_helper(sock_fd, config, config_flags); + } + impl::socket_close(sock_fd); return success; @@ -548,7 +555,7 @@ std::shared_ptr init_client(const std::string& hostname, // wake up from STANDBY, if necessary success &= do_tcp_cmd( - sock_fd, {"set_config_param", "operating_mode", "NORMAL"}, res); + sock_fd, {"set_config_param", "auto_start_flag", "1"}, res); success &= res == "set_config_param"; // reinitialize to activate new settings diff --git a/ouster_client/src/example.cpp b/ouster_client/src/example.cpp index 893d697b..565b07a1 100644 --- a/ouster_client/src/example.cpp +++ b/ouster_client/src/example.cpp @@ -68,7 +68,7 @@ int main(int argc, char* argv[]) { // azimuth_window config param reduce the amount of valid columns per scan // that we will receive int column_window_length = - std::abs(column_window.second - column_window.first) + 1; + (column_window.second - column_window.first + w) % w + 1; std::cerr << " Firmware version: " << info.fw_rev << "\n Serial number: " << info.sn diff --git a/ouster_viz/src/image_processing.cpp b/ouster_client/src/image_processing.cpp similarity index 100% rename from ouster_viz/src/image_processing.cpp rename to ouster_client/src/image_processing.cpp diff --git a/ouster_client/src/netcompat.cpp b/ouster_client/src/netcompat.cpp index fa537c7b..39ace147 100644 --- a/ouster_client/src/netcompat.cpp +++ b/ouster_client/src/netcompat.cpp @@ -75,7 +75,7 @@ bool socket_exit() { int socket_set_non_blocking(SOCKET value) { #ifdef _WIN32 - u_long non_blocking_mode = 0; + u_long non_blocking_mode = 1; return ioctlsocket(value, FIONBIO, &non_blocking_mode); #else return fcntl(value, F_SETFL, fcntl(value, F_GETFL, 0) | O_NONBLOCK); diff --git a/ouster_client/src/types.cpp b/ouster_client/src/types.cpp index da0bf217..2661d0ca 100644 --- a/ouster_client/src/types.cpp +++ b/ouster_client/src/types.cpp @@ -98,6 +98,7 @@ bool operator==(const sensor_config& lhs, const sensor_config& rhs) { lhs.ts_mode == rhs.ts_mode && lhs.ld_mode == rhs.ld_mode && lhs.operating_mode == rhs.operating_mode && lhs.azimuth_window == rhs.azimuth_window && + lhs.signal_multiplier == rhs.signal_multiplier && lhs.sync_pulse_out_angle == rhs.sync_pulse_out_angle && lhs.sync_pulse_out_pulse_width == rhs.sync_pulse_out_pulse_width && lhs.nmea_in_polarity == rhs.nmea_in_polarity && @@ -162,8 +163,6 @@ static double default_lidar_origin_to_beam_origin(std::string prod_line) { return lidar_origin_to_beam_origin_mm; } - - sensor_info default_sensor_info(lidar_mode mode) { return sensor::sensor_info{"UNKNOWN", "000000000000", @@ -386,6 +385,9 @@ sensor_config parse_config(const Json::Value& root) { std::make_pair(root["azimuth_window"][0].asInt(), root["azimuth_window"][1].asInt()); + if (!root["signal_multiplier"].empty()) + config.signal_multiplier = root["signal_multiplier"].asInt(); + if (!root["operating_mode"].empty()) { auto operating_mode = operating_mode_of_string(root["operating_mode"].asString()); @@ -715,6 +717,10 @@ std::string to_string(const sensor_config& config) { root["azimuth_window"] = azimuth_window; } + if (config.signal_multiplier) { + root["signal_multiplier"] = config.signal_multiplier.value(); + } + if (config.sync_pulse_out_angle) { root["sync_pulse_out_angle"] = config.sync_pulse_out_angle.value(); } diff --git a/ouster_pcap/include/ouster/os_pcap.h b/ouster_pcap/include/ouster/os_pcap.h index d603fb80..91545ade 100644 --- a/ouster_pcap/include/ouster/os_pcap.h +++ b/ouster_pcap/include/ouster/os_pcap.h @@ -28,7 +28,7 @@ struct packet_info { int src_port; ///< The source port size_t payload_size; ///< The size of the packet payload std::chrono::microseconds - timestamp; ///< The packet timestamp in microseconds + timestamp; ///< The packet timestamp in std::chrono::duration }; /** @@ -197,9 +197,11 @@ void record_uninitialize(record_handle& handle); * @param[in] dst_port The destination port to label the packets with * @param[in] buf The buffer to record to the pcap file * @param[in] buffer_size The size of the buffer to record to the pcap file + * @param[in] microsecond_timestamp The timestamp to record the packet as microseconds */ void record_packet(record_handle& handle, int src_port, int dst_port, - const uint8_t* buf, size_t buffer_size); + const uint8_t* buf, size_t buffer_size, + uint64_t microsecond_timestamp); } // namespace sensor_utils } // namespace ouster diff --git a/ouster_pcap/src/os_pcap.cpp b/ouster_pcap/src/os_pcap.cpp index 06815ae7..ec693941 100644 --- a/ouster_pcap/src/os_pcap.cpp +++ b/ouster_pcap/src/os_pcap.cpp @@ -439,11 +439,15 @@ std::vector buffer_to_frag_packets(record_handle& handle, int src_port, } void record_packet(record_handle& handle, int src_port, int dst_port, - const uint8_t* buf, size_t buffer_size) { + const uint8_t* buf, size_t buffer_size, + uint64_t microsecond_timestamp) { + using namespace std::chrono; + // For each of the packets write it to the pcap file for (auto item : - buffer_to_frag_packets(handle, src_port, dst_port, buf, buffer_size)) { - handle.pcap_file_writer->write(item); + buffer_to_frag_packets(handle, src_port, dst_port, buf, buffer_size)) { + Packet packet(item, Timestamp(static_cast(microsecond_timestamp))); + handle.pcap_file_writer->write(packet); } } diff --git a/ouster_ros/CMakeLists.txt b/ouster_ros/CMakeLists.txt index d34853c6..4e762e4e 100644 --- a/ouster_ros/CMakeLists.txt +++ b/ouster_ros/CMakeLists.txt @@ -8,8 +8,8 @@ project(ouster_ros) # ==== Requirements ==== find_package(Eigen3 REQUIRED) -find_package(GLEW REQUIRED) -find_package(glfw3 REQUIRED) +find_package(PCL REQUIRED COMPONENTS common) +find_package(tf2_eigen REQUIRED) find_package( catkin REQUIRED @@ -17,12 +17,10 @@ find_package( std_msgs sensor_msgs geometry_msgs - pcl_ros pcl_conversions roscpp tf2 - tf2_ros - tf2_geometry_msgs) + tf2_ros) # ==== Options ==== set(CMAKE_CXX_STANDARD 14) @@ -35,7 +33,7 @@ add_service_files(FILES OSConfigSrv.srv) generate_messages(DEPENDENCIES std_msgs sensor_msgs geometry_msgs) set(_ouster_ros_INCLUDE_DIRS - "include;../ouster_client/include;../ouster_client/include/optional-lite;../ouster_viz/include") + "include;../ouster_client/include;../ouster_client/include/optional-lite") catkin_package( INCLUDE_DIRS @@ -45,12 +43,11 @@ catkin_package( CATKIN_DEPENDS roscpp message_runtime - pcl_ros std_msgs sensor_msgs geometry_msgs DEPENDS - EIGEN3 GLFW3 GLEW) + EIGEN3) # ==== Libraries ==== # Build static libraries and bundle them into ouster_ros using the `--whole-archive` flag. This is @@ -59,15 +56,14 @@ catkin_package( set(_SAVE_BUILD_SHARED_LIBS ${BUILD_SHARED_LIBS}) set(BUILD_SHARED_LIBS OFF) add_subdirectory(../ouster_client ouster_client EXCLUDE_FROM_ALL) -add_subdirectory(../ouster_viz ouster_viz EXCLUDE_FROM_ALL) set(BUILD_SHARED_LIBS ${_SAVE_BUILD_SHARED_LIBS}) # catkin adds all include dirs to a single variable, don't try to use targets -include_directories(${_ouster_ros_INCLUDE_DIRS} SYSTEM ${catkin_INCLUDE_DIRS}) +include_directories(${_ouster_ros_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) add_library(ouster_ros src/ros.cpp) -target_link_libraries(ouster_ros PUBLIC ${catkin_LIBRARIES} ouster_build PRIVATE - -Wl,--whole-archive ouster_client ouster_viz -Wl,--no-whole-archive) +target_link_libraries(ouster_ros PUBLIC ${catkin_LIBRARIES} ouster_build pcl_common PRIVATE + -Wl,--whole-archive ouster_client -Wl,--no-whole-archive) add_dependencies(ouster_ros ${PROJECT_NAME}_gencpp) # ==== Executables ==== @@ -79,17 +75,13 @@ add_executable(os_cloud_node src/os_cloud_node.cpp) target_link_libraries(os_cloud_node ouster_ros ${catkin_LIBRARIES}) add_dependencies(os_cloud_node ${PROJECT_NAME}_gencpp) -add_executable(viz_node src/viz_node.cpp) -target_link_libraries(viz_node ouster_ros ${catkin_LIBRARIES} glfw GLEW) -add_dependencies(viz_node ${PROJECT_NAME}_gencpp) - add_executable(img_node src/img_node.cpp) target_link_libraries(img_node ouster_ros ${catkin_LIBRARIES}) add_dependencies(img_node ${PROJECT_NAME}_gencpp) # ==== Install ==== install( - TARGETS ouster_ros os_node os_cloud_node viz_node img_node + TARGETS ouster_ros os_node os_cloud_node img_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/ouster_ros/Dockerfile b/ouster_ros/Dockerfile new file mode 100644 index 00000000..fc497516 --- /dev/null +++ b/ouster_ros/Dockerfile @@ -0,0 +1,61 @@ +ARG ROS_DISTRO=melodic + +FROM ros:${ROS_DISTRO}-ros-core AS build-env +ENV DEBIAN_FRONTEND=noninteractive \ + BUILD_HOME=/var/lib/build \ + OUSTER_SDK_PATH=/opt/ouster_example + +RUN set -xue \ +# Kinetic and melodic have python3 packages but they seem to conflict +&& [ $ROS_DISTRO = "noetic" ] && PY=python3 || PY=python \ +# Turn off installing extra packages globally to slim down rosdep install +&& echo 'APT::Install-Recommends "0";' > /etc/apt/apt.conf.d/01norecommend \ +&& apt-get update \ +&& apt-get install -y \ + build-essential cmake \ + fakeroot dpkg-dev debhelper \ + $PY-rosdep $PY-rospkg $PY-bloom + +# Set up non-root build user +ARG BUILD_UID=1000 +ARG BUILD_GID=${BUILD_UID} + +RUN set -xe \ +&& groupadd -o -g ${BUILD_GID} build \ +&& useradd -o -u ${BUILD_UID} -d ${BUILD_HOME} -rm -s /bin/bash -g build build + +# Install build dependencies using rosdep +COPY --chown=build:build ouster_ros/package.xml ${OUSTER_SDK_PATH}/ouster_ros/package.xml + +RUN set -xe \ +&& apt-get update \ +&& rosdep init \ +&& rosdep update --rosdistro=${ROS_DISTRO} \ +&& rosdep install -y --from-paths ${OUSTER_SDK_PATH} + +# Set up build environment +COPY --chown=build:build cmake ${OUSTER_SDK_PATH}/cmake +COPY --chown=build:build ouster_client ${OUSTER_SDK_PATH}/ouster_client +COPY --chown=build:build ouster_viz ${OUSTER_SDK_PATH}/ouster_viz +COPY --chown=build:build ouster_ros ${OUSTER_SDK_PATH}/ouster_ros + +USER build:build +WORKDIR ${BUILD_HOME} + +RUN set -xe \ +&& mkdir src \ +&& ln -s ${OUSTER_SDK_PATH} ./src + + +FROM build-env + +RUN /opt/ros/${ROS_DISTRO}/env.sh catkin_make -DCMAKE_BUILD_TYPE=Release + +# Entrypoint for running Ouster ros: +# +# Usage: docker run --rm -it ouster-ros [ouster.launch parameters ..] +# +ENTRYPOINT ["bash", "-c", "set -e \ +&& . ./devel/setup.bash \ +&& roslaunch ouster_ros ouster.launch \"$@\" \ +", "ros-entrypoint"] diff --git a/ouster_ros/ouster.launch b/ouster_ros/ouster.launch index 39323766..0a208c29 100644 --- a/ouster_ros/ouster.launch +++ b/ouster_ros/ouster.launch @@ -8,8 +8,8 @@ - - + + @@ -30,12 +30,8 @@ - - - - - - + + diff --git a/ouster_ros/package.xml b/ouster_ros/package.xml index 14c0f7a0..9ad1606f 100644 --- a/ouster_ros/package.xml +++ b/ouster_ros/package.xml @@ -1,7 +1,7 @@ ouster_ros - 0.1.0 + 0.2.0 Ouster example ROS client ouster developers BSD @@ -11,17 +11,14 @@ std_msgs sensor_msgs geometry_msgs - pcl_ros - pcl_conversions + tf2_ros libjsoncpp-dev eigen message_generation - tf2 - tf2_ros - tf2_geometry_msgs - libglew-dev - libglfw3-dev + tf2_eigen + libpcl-all-dev + pcl_conversions libtclap-dev libjsoncpp diff --git a/ouster_ros/src/img_node.cpp b/ouster_ros/src/img_node.cpp index 1fc4eeb9..108e4a73 100644 --- a/ouster_ros/src/img_node.cpp +++ b/ouster_ros/src/img_node.cpp @@ -1,19 +1,20 @@ /** * @file - * @brief Example node to visualize range, ambient and intensity images + * @brief Example node to visualize range, near ir and signal images * - * Publishes ~/range_image, ~/ambient_image, and ~/intensity_image. Please bear + * Publishes ~/range_image, ~/nearir_image, and ~/signal_image. Please bear * in mind that there is rounding/clamping to display 8 bit images. For computer * vision applications, use higher bit depth values in /os_cloud_node/points */ #include +#include #include #include -#include #include #include #include +#include #include #include @@ -31,11 +32,21 @@ namespace sensor = ouster::sensor; namespace viz = ouster::viz; -using pixel_type = uint8_t; -constexpr size_t bit_depth = 8 * sizeof(pixel_type); +using pixel_type = uint16_t; const size_t pixel_value_max = std::numeric_limits::max(); -constexpr double range_multiplier = - 1.0 / 200.0; // assuming 200 m range typical + +sensor_msgs::ImagePtr make_image_msg(size_t H, size_t W, + const ros::Time& stamp) { + sensor_msgs::ImagePtr msg{new sensor_msgs::Image{}}; + msg->width = W; + msg->height = H; + msg->step = W * sizeof(pixel_type); + msg->encoding = sensor_msgs::image_encodings::MONO16; + msg->data.resize(W * H * sizeof(pixel_type)); + msg->header.stamp = stamp; + + return msg; +} int main(int argc, char** argv) { ros::init(argc, argv, "img_node"); @@ -57,98 +68,81 @@ int main(int argc, char** argv) { ros::Publisher range_image_pub = nh.advertise("range_image", 100); - ros::Publisher ambient_image_pub = - nh.advertise("ambient_image", 100); - ros::Publisher intensity_image_pub = - nh.advertise("intensity_image", 100); + ros::Publisher nearir_image_pub = + nh.advertise("nearir_image", 100); + ros::Publisher signal_image_pub = + nh.advertise("signal_image", 100); + ros::Publisher reflec_image_pub = + nh.advertise("reflec_image", 100); ouster_ros::Cloud cloud{}; - viz::AutoExposure ambient_ae, intensity_ae; - viz::BeamUniformityCorrector ambient_buc; + viz::AutoExposure nearir_ae, signal_ae, reflec_ae; + viz::BeamUniformityCorrector nearir_buc; - std::stringstream encoding_ss; - encoding_ss << "mono" << bit_depth; - std::string encoding = encoding_ss.str(); + ouster::img_t nearir_image_eigen(H, W); + ouster::img_t signal_image_eigen(H, W); + ouster::img_t reflec_image_eigen(H, W); auto cloud_handler = [&](const sensor_msgs::PointCloud2::ConstPtr& m) { pcl::fromROSMsg(*m, cloud); - sensor_msgs::Image range_image; - sensor_msgs::Image ambient_image; - sensor_msgs::Image intensity_image; - - range_image.width = W; - range_image.height = H; - range_image.step = W; - range_image.encoding = encoding; - range_image.data.resize(W * H * bit_depth / - (8 * sizeof(*range_image.data.data()))); - range_image.header.stamp = m->header.stamp; - - ambient_image.width = W; - ambient_image.height = H; - ambient_image.step = W; - ambient_image.encoding = encoding; - ambient_image.data.resize(W * H * bit_depth / - (8 * sizeof(*ambient_image.data.data()))); - ambient_image.header.stamp = m->header.stamp; - - intensity_image.width = W; - intensity_image.height = H; - intensity_image.step = W; - intensity_image.encoding = encoding; - intensity_image.data.resize(W * H * bit_depth / - (8 * sizeof(*intensity_image.data.data()))); - intensity_image.header.stamp = m->header.stamp; - - ouster::img_t ambient_image_eigen(H, W); - ouster::img_t intensity_image_eigen(H, W); - + auto range_image = make_image_msg(H, W, m->header.stamp); + auto nearir_image = make_image_msg(H, W, m->header.stamp); + auto signal_image = make_image_msg(H, W, m->header.stamp); + auto reflec_image = make_image_msg(H, W, m->header.stamp); + + // views into message data + auto range_image_map = Eigen::Map>( + (pixel_type*)range_image->data.data(), H, W); + auto nearir_image_map = Eigen::Map>( + (pixel_type*)nearir_image->data.data(), H, W); + auto signal_image_map = Eigen::Map>( + (pixel_type*)signal_image->data.data(), H, W); + auto reflec_image_map = Eigen::Map>( + (pixel_type*)reflec_image->data.data(), H, W); + + // copy data out of Cloud message, with destaggering for (size_t u = 0; u < H; u++) { for (size_t v = 0; v < W; v++) { const size_t vv = (v + W - px_offset[u]) % W; - const size_t index = u * W + vv; - const auto& pt = cloud[index]; - - if (pt.range == 0) { - reinterpret_cast( - range_image.data.data())[u * W + v] = 0; - } else { - reinterpret_cast( - range_image.data.data())[u * W + v] = - pixel_value_max - - std::min(std::round(pt.range * range_multiplier), - static_cast(pixel_value_max)); - } - ambient_image_eigen(u, v) = pt.ambient; - intensity_image_eigen(u, v) = pt.intensity; - } - } + const auto& pt = cloud[u * W + vv]; - ambient_buc(ambient_image_eigen); - ambient_ae(ambient_image_eigen); - intensity_ae(intensity_image_eigen); - ambient_image_eigen = ambient_image_eigen.sqrt(); - intensity_image_eigen = intensity_image_eigen.sqrt(); - for (size_t u = 0; u < H; u++) { - for (size_t v = 0; v < W; v++) { - reinterpret_cast( - ambient_image.data.data())[u * W + v] = - ambient_image_eigen(u, v) * pixel_value_max; - reinterpret_cast( - intensity_image.data.data())[u * W + v] = - intensity_image_eigen(u, v) * pixel_value_max; + // 16 bit img: use 4mm resolution and throw out returns > 260m + auto r = (pt.range + 0b10) >> 2; + range_image_map(u, v) = r > pixel_value_max ? 0 : r; + + nearir_image_eigen(u, v) = pt.ambient; + signal_image_eigen(u, v) = pt.intensity; + reflec_image_eigen(u, v) = pt.reflectivity; } } + // image processing + nearir_buc(nearir_image_eigen); + nearir_ae(nearir_image_eigen); + signal_ae(signal_image_eigen); + reflec_ae(reflec_image_eigen); + nearir_image_eigen = nearir_image_eigen.sqrt(); + signal_image_eigen = signal_image_eigen.sqrt(); + + // copy data into image messages + nearir_image_map = + (nearir_image_eigen * pixel_value_max).cast(); + signal_image_map = + (signal_image_eigen * pixel_value_max).cast(); + reflec_image_map = + (reflec_image_eigen * pixel_value_max).cast(); + + // publish range_image_pub.publish(range_image); - ambient_image_pub.publish(ambient_image); - intensity_image_pub.publish(intensity_image); + nearir_image_pub.publish(nearir_image); + signal_image_pub.publish(signal_image); + reflec_image_pub.publish(reflec_image); }; auto pc_sub = - nh.subscribe("points", 500, cloud_handler); + nh.subscribe("points", 100, cloud_handler); ros::spin(); return EXIT_SUCCESS; diff --git a/ouster_ros/src/os_node.cpp b/ouster_ros/src/os_node.cpp index 72d324ae..e01fa35a 100644 --- a/ouster_ros/src/os_node.cpp +++ b/ouster_ros/src/os_node.cpp @@ -56,17 +56,22 @@ void populate_metadata_defaults(sensor::sensor_info& info, } // try to write metadata file -void write_metadata(const std::string& meta_file, const std::string& metadata) { +bool write_metadata(const std::string& meta_file, const std::string& metadata) { std::ofstream ofs; ofs.open(meta_file); ofs << metadata << std::endl; ofs.close(); if (ofs) { - ROS_INFO("Wrote metadata to $ROS_HOME/%s", meta_file.c_str()); + ROS_INFO("Wrote metadata to %s", meta_file.c_str()); } else { - ROS_WARN("Failed to write metadata to %s; check that the path is valid", - meta_file.c_str()); + ROS_WARN( + "Failed to write metadata to %s; check that the path is valid. If " + "you provided a relative path, please note that the working " + "directory of all ROS nodes is set by default to $ROS_HOME", + meta_file.c_str()); + return false; } + return true; } int connection_loop(ros::NodeHandle& nh, sensor::client& cli, @@ -126,10 +131,6 @@ int main(int argc, char** argv) { auto lidar_mode_arg = nh.param("lidar_mode", std::string{}); auto timestamp_mode_arg = nh.param("timestamp_mode", std::string{}); - // fall back to metadata file name based on hostname, if available - auto meta_file = nh.param("metadata", std::string{}); - if (!meta_file.size() && hostname.size()) meta_file = hostname + ".json"; - // set lidar mode from param sensor::lidar_mode lidar_mode = sensor::MODE_UNSPEC; if (lidar_mode_arg.size()) { @@ -155,6 +156,16 @@ int main(int argc, char** argv) { } } + auto meta_file = nh.param("metadata", std::string{}); + if (!meta_file.size()) { + if (replay) { + ROS_ERROR("Must specify metadata file in replay mode"); + } else { + ROS_ERROR("Must specify path for metadata output"); + } + return EXIT_FAILURE; + } + if (!replay && (!hostname.size() || !udp_dest.size())) { ROS_ERROR("Must specify both hostname and udp destination"); return EXIT_FAILURE; @@ -197,7 +208,10 @@ int main(int argc, char** argv) { // write metadata file to cwd (usually ~/.ros) auto metadata = sensor::get_metadata(*cli); - write_metadata(meta_file, metadata); + if (!write_metadata(meta_file, metadata)) { + ROS_ERROR("Exiting because of failure to write metadata path"); + return EXIT_FAILURE; + } // populate sensor info auto info = sensor::parse_metadata(metadata); diff --git a/ouster_ros/src/ros.cpp b/ouster_ros/src/ros.cpp index 91ed09d1..4f77708e 100644 --- a/ouster_ros/src/ros.cpp +++ b/ouster_ros/src/ros.cpp @@ -4,7 +4,6 @@ #include #include #include -#include #include #include diff --git a/ouster_ros/src/viz_node.cpp b/ouster_ros/src/viz_node.cpp deleted file mode 100644 index cf1069d2..00000000 --- a/ouster_ros/src/viz_node.cpp +++ /dev/null @@ -1,70 +0,0 @@ -/** - * Example node to visualize ouster lidar data - */ - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "ouster/lidar_scan.h" -#include "ouster/lidar_scan_viz.h" -#include "ouster/point_viz.h" -#include "ouster/types.h" -#include "ouster_ros/OSConfigSrv.h" -#include "ouster_ros/PacketMsg.h" -#include "ouster_ros/ros.h" - -namespace viz = ouster::viz; -namespace sensor = ouster::sensor; -using PacketMsg = ouster_ros::PacketMsg; - -int main(int argc, char** argv) { - ros::init(argc, argv, "viz_node"); - ros::NodeHandle nh("~"); - - ouster_ros::OSConfigSrv cfg{}; - auto client = nh.serviceClient("os_config"); - client.waitForExistence(); - if (!client.call(cfg)) { - ROS_ERROR("Calling config service failed"); - return EXIT_FAILURE; - } - - auto info = sensor::parse_metadata(cfg.response.metadata); - const size_t H = info.format.pixels_per_column; - const size_t W = info.format.columns_per_frame; - - auto packet_format = sensor::get_format(info); - - auto xyz_lut = ouster::make_xyz_lut(info); - - viz::PointViz point_viz( - {viz::CloudSetup{ - xyz_lut.direction.data(), xyz_lut.offset.data(), - info.format.columns_per_frame * info.format.pixels_per_column, - info.format.columns_per_frame, info.extrinsic.data()}}, - "Ouster Viz (ROS)"); - - viz::LidarScanViz lidar_scan_viz(info, point_viz); - - ouster::LidarScan ls(W, H); - - ouster::ScanBatcher batch(W, packet_format); - - auto lidar_handler = [&](const PacketMsg& pm) mutable { - if (batch(pm.buf.data(), ls)) lidar_scan_viz.draw(ls); - }; - - auto lidar_packet_sub = nh.subscribe( - "lidar_packets", 2048, lidar_handler); - - ros::spin(); - - return EXIT_SUCCESS; -} diff --git a/ouster_ros/viz.rviz b/ouster_ros/viz.rviz index d001a450..763a7e95 100644 --- a/ouster_ros/viz.rviz +++ b/ouster_ros/viz.rviz @@ -8,8 +8,8 @@ Panels: - /Status1 - /PointCloud21 - /TF1/Frames1 - Splitter Ratio: 0.5 - Tree Height: 573 + Splitter Ratio: 0.5654885768890381 + Tree Height: 756 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -18,7 +18,7 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 + Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 @@ -29,6 +29,10 @@ Panels: Name: Time SyncMode: 0 SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: false +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -36,9 +40,9 @@ Visualization Manager: Cell Size: 1 Class: rviz/Grid Color: 160; 160; 164 - Enabled: true + Enabled: false Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -49,73 +53,85 @@ Visualization Manager: Plane: XY Plane Cell Count: 10 Reference Frame: - Value: true + Value: false - Alpha: 1 - Autocompute Intensity Bounds: true + Autocompute Intensity Bounds: false Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z - Channel Name: intensity + Channel Name: z Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: true - Invert Rainbow: false + Invert Rainbow: true Max Color: 255; 255; 255 - Max Intensity: 1724 + Max Intensity: 15 Min Color: 0; 0; 0 - Min Intensity: 0 + Min Intensity: -10 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true - Size (Pixels): 3 - Size (m): 0.00999999978 - Style: Points + Size (Pixels): 2 + Size (m): 0.029999999329447746 + Style: Flat Squares Topic: /os_cloud_node/points Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Class: rviz/Image - Enabled: true - Image Topic: /img_node/ambient_image + Enabled: false + Image Topic: /img_node/nearir_image Max Value: 1 Median window: 5 Min Value: 0 - Name: ambient + Name: near ir Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false - Value: true + Value: false - Class: rviz/Image Enabled: true - Image Topic: /img_node/intensity_image + Image Topic: /img_node/signal_image Max Value: 1 Median window: 5 Min Value: 0 - Name: intensity + Name: signal Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true - Class: rviz/Image - Enabled: true - Image Topic: /img_node/range_image + Enabled: false + Image Topic: /img_node/reflec_image Max Value: 1 Median window: 5 Min Value: 0 + Name: reflectivity + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /img_node/range_image + Max Value: 1 + Median window: 50 + Min Value: 0 Name: range Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false - Value: true + Value: false - Class: rviz/TF Enabled: true Frame Timeout: 15 @@ -155,7 +171,10 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -165,9 +184,9 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 10.2322998 + Distance: 45.77256774902344 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false @@ -176,22 +195,22 @@ Visualization Manager: Y: 0 Z: 0 Focal Shape Fixed Size: false - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.595398128 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7153978943824768 Target Frame: Value: Orbit (rviz) - Yaw: 2.49040008 + Yaw: 4.228582382202148 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1051 + Height: 1080 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000003170000052ffc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000005fb000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000005f0000000180000000000000000fb0000000800480065006c007000000002540000046e000000000000000000000001000002a8000003c7fc0200000008fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00720061006e00670065010000003a000000620000001600fffffffb000000120069006e00740065006e007300690074007901000000a2000000730000001600fffffffb0000000e0061006d006200690065006e0074010000011b000000690000001600fffffffb000000100044006900730070006c006100790073010000018a00000277000000c600fffffffb000000100044006900730070006c0061007900730100000189000002d10000000000000000fb0000000a00560069006500770073000000036d000002060000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200001001000000a7fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000082b00000043fc0100000001fc000000000000082b0000000000fffffffa000000010200000002fb0000000800540069006d00650000000000ffffffff0000003600fffffffb0000000800540069006d006501000006c90000003a0000000000000000000004d2000003c700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -202,10 +221,12 @@ Window Geometry: collapsed: false Width: 1920 X: 0 - Y: 29 - ambient: - collapsed: false - intensity: + Y: 1151 + near ir: collapsed: false range: collapsed: false + reflectivity: + collapsed: false + signal: + collapsed: false diff --git a/ouster_viz/CMakeLists.txt b/ouster_viz/CMakeLists.txt index d6a4d8a9..8190f29f 100644 --- a/ouster_viz/CMakeLists.txt +++ b/ouster_viz/CMakeLists.txt @@ -12,7 +12,7 @@ find_package(glfw3 CONFIG REQUIRED) # ==== Libraries ==== add_library(ouster_viz src/point_viz.cpp src/cloud.cpp src/camera.cpp src/image.cpp - src/image_processing.cpp src/lidar_scan_viz.cpp) + src/lidar_scan_viz.cpp) target_link_libraries(ouster_viz PUBLIC Threads::Threads glfw GLEW::GLEW OpenGL::GL PRIVATE ouster_client) diff --git a/ouster_viz/src/point_viz.cpp b/ouster_viz/src/point_viz.cpp index aaee70c7..094d2272 100644 --- a/ouster_viz/src/point_viz.cpp +++ b/ouster_viz/src/point_viz.cpp @@ -133,7 +133,6 @@ struct PointViz::impl { initialized(false) {} }; - static void draw(PointViz::impl& pimpl) { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // draw images @@ -246,13 +245,13 @@ static void handleKeyPress(GLFWwindow* window, int key, int /*scancode*/, } if (key == GLFW_KEY_SEMICOLON) { pimpl->rings.ring_size = std::min(2, pimpl->rings.ring_size + 1); - std::cerr << "ring size increased: 10^" << pimpl->rings.ring_size - << std::endl; + std::cerr << "ring size radius increased: 10^" + << pimpl->rings.ring_size << std::endl; } if (key == GLFW_KEY_APOSTROPHE) { pimpl->rings.ring_size = std::max(-2, pimpl->rings.ring_size - 1); - std::cerr << "ring size decreased: 10^" << pimpl->rings.ring_size - << std::endl; + std::cerr << "ring size radius decreased: 10^" + << pimpl->rings.ring_size << std::endl; } if (key == GLFW_KEY_EQUAL) { pimpl->camera.zoomIn(); @@ -500,12 +499,14 @@ void PointViz::cuboidSwap() { void PointViz::setCameraTarget(const mat4d& target) { pimpl->camera.setTarget(target); } - -void PointViz::setPointCloudPalette(const float palette[][3], size_t palette_size) { - if(pimpl->palette_texture_id != (GLuint)-1) { + +void PointViz::setPointCloudPalette(const float palette[][3], + size_t palette_size) { + if (pimpl->palette_texture_id != (GLuint)-1) { load_texture(palette, palette_size, 1, pimpl->palette_texture_id); } else { - std::cerr << "Cannot set custom palette before initialization" << std::endl; + std::cerr << "Cannot set custom palette before initialization" + << std::endl; } } diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 973de2a9..bf817c69 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -10,7 +10,8 @@ set(CMAKE_CXX_STANDARD 14) find_package(pybind11 2.0 REQUIRED) find_package(Eigen3 REQUIRED) -option(BUILD_VIZ OFF) +option(BUILD_VIZ "Disabled for Python build" OFF) +option(BUILD_PCAP "Enabled for Python build" ON) find_package(ouster_example REQUIRED) # Several deprecations in pybind11 since 2.0. Check when changing version reqs. diff --git a/python/Dockerfile b/python/Dockerfile index 981b8604..46951af8 100644 --- a/python/Dockerfile +++ b/python/Dockerfile @@ -55,6 +55,7 @@ COPY . ${OUSTER_SDK_PATH} # ENTRYPOINT ["sh", "-c", "set -e \ && rm -rf ./src && cp -a ${OUSTER_SDK_PATH} ./src \ +&& export ARTIFACT_DIR=${ARTIFACT_DIR:-$BUILD_HOME/artifacts} \ && . /etc/os-release && export ID VERSION_ID \ && exec python3 -m tox -c ./src/python --workdir ${HOME}/tox \"$@\" \ ", "tox-entrypoint"] diff --git a/python/README.rst b/python/README.rst index 2407ca46..2e97247a 100644 --- a/python/README.rst +++ b/python/README.rst @@ -14,14 +14,16 @@ data. The SDK includes APIs for: - Conversion of raw data to range/signal/near_ir/reflectivity images (de-staggering) - Efficiently projecting range measurements to cartesian (X, Y, Z) coordinates +.. _supported platforms: + Supported Platforms -=================== +------------------- -Pre-built binaries are provided on `PyPI `_ for x86-64 platforms including: +Pre-built binaries are provided on `PyPI`_ for x86_64 platforms including: - Most glibc-based Linux distributions (``manylinux2010_x86_64``) -- Macos >= 10.13 (``macosx_10_13_x86_64``) +- macOS >= 10.13 (``macosx_10_13_x86_64``) - Windows 10 (``win_amd64``) Building from source is supported on: @@ -30,28 +32,26 @@ Building from source is supported on: - macOS >= 10.13 (x86-64) - Windows 10 (x86-64) -See the `build instructions`_ for requirements needed to build from a source distribution or from a -clone of the repository. +.. _PyPI: https://pypi.org/project/ouster-sdk/ Installation -============ - -The Ouster Python SDK requires Python >= 3.6 and pip >= 19.0. To install, run:: +------------ - $ python3 -m pip install ouster-sdk +The Ouster Python SDK requires Python >= 3.6 and pip >= 19.0. To install, use ``pip`` to grab the +``ouster-sdk`` package. See the `quick start`_ section of the documentation for more details and to +begin working with Ouster data! If you're running ARM, a non-glibc-based linux distribution, or wish to modify the Ouster Python -SDK, you will need to build from source. - -See the `quick start`_ section of the documentation to begin working with Ouster data! +SDK, you will need to build from source. See the `build instructions`_ for requirements needed to +build from a source distribution or from a clone of the repository. .. _quick start: https://static.ouster.dev/sdk-docs/quickstart.html .. _build instructions: https://static.ouster.dev/sdk-docs/devel.html Status and Contact Info -======================= +----------------------- The Ouster Python SDK is currently provided as an early pre-1.0 preview. The APIs are subject to change in every release. diff --git a/python/docs/_static/css/ouster_rtd_tweaks.css b/python/docs/_static/css/ouster_rtd_tweaks.css index d5d82cb4..667240cc 100644 --- a/python/docs/_static/css/ouster_rtd_tweaks.css +++ b/python/docs/_static/css/ouster_rtd_tweaks.css @@ -18,3 +18,12 @@ when needed on the page as strikethrough text text-decoration: line-through; } +/* remove default browser outline (in some browsers, ...) */ +.sphinx-tabs :focus { + outline: none; +} + +/* remove extraspace on highlight ... */ +.sphinx-tabs div[class^=highlight] { + margin-bottom: 4px; +} diff --git a/python/docs/api.rst b/python/docs/api.rst index 858c5057..e4d1ae0d 100644 --- a/python/docs/api.rst +++ b/python/docs/api.rst @@ -163,6 +163,15 @@ PCAP Examples :mod:`ouster.sdk.examples.pcap` ---- +Open3D Examples :mod:`ouster.sdk.examples.open3d` +------------------------------------------------- + +.. automodule:: ouster.sdk.examples.open3d + :members: + +---- + + Reference Code :mod:`ouster.sdk.examples.reference` --------------------------------------------------- diff --git a/python/docs/conf.py b/python/docs/conf.py index a70da544..08bb3e22 100644 --- a/python/docs/conf.py +++ b/python/docs/conf.py @@ -24,8 +24,8 @@ author = 'Ouster SW' # The full version, including alpha/beta/rc tags -release = '0.2.0' -version = '0.2.0' +release = '0.2.1' +version = '0.2.1' # -- General configuration --------------------------------------------------- @@ -40,6 +40,8 @@ 'sphinx.ext.todo', 'sphinx_autodoc_typehints', 'sphinx_rtd_theme', + 'sphinx_copybutton', + 'sphinx_tabs.tabs', ] # Add any paths that contain templates here, relative to this directory. @@ -121,3 +123,11 @@ todo_include_todos = True todo_link_only = True todo_emit_warnings = True + +# copybutton configs +# Note: last entry treats four spaces as a prompt to support "continuation lines" +copybutton_prompt_text = r'>>> |\.\.\. |\$ |PS > |C:\\> |> | ' +copybutton_prompt_is_regexp = True + +# tabs behavior +sphinx_tabs_disable_tab_closing = True diff --git a/python/docs/devel.rst b/python/docs/devel.rst index c6f9ceed..1d71d611 100644 --- a/python/docs/devel.rst +++ b/python/docs/devel.rst @@ -21,49 +21,59 @@ Building the Python SDK from source requires several dependencies: Linux and macos --------------- -On supported Debian-based linux systems, you can install all build dependencies by running:: +On supported Debian-based linux systems, you can install all build dependencies by running: - $ sudo apt install build-essential cmake \ - libeigen3-dev libjsoncpp-dev libtins-dev libpcap-dev \ - python3-dev python3-pip pybind11-dev +.. code:: console -On macos >= 10.13, using homebrew, you should be able to run:: + $ sudo apt install build-essential cmake \ + libeigen3-dev libjsoncpp-dev libtins-dev libpcap-dev \ + python3-dev python3-pip pybind11-dev + +On macos >= 10.13, using homebrew, you should be able to run: + +.. code:: console $ brew install cmake eigen jsoncpp libtins python3 pybind11 -After you have the system dependencies, you can build the SDK with:: +After you have the system dependencies, you can build the SDK with: + +.. code:: console - # first, specify the path to the ouster_example repository - export OUSTER_SDK_PATH= + # first, specify the path to the ouster_example repository + $ export OUSTER_SDK_PATH= - # then, build an installable "wheel" package - python3 -m pip wheel --no-deps $OUSTER_SDK_PATH/python + # then, build an installable "wheel" package + $ python3 -m pip wheel --no-deps $OUSTER_SDK_PATH/python - # or just install directly (virtualenv recommended) - python3 -m pip install $OUSTER_SDK_PATH/python + # or just install directly (virtualenv recommended) + $ python3 -m pip install $OUSTER_SDK_PATH/python Windows 10 ---------- On Windows 10, you'll have to install Visual Studio, Python and the `vcpkg`_ package manager and -run:: +run: + +.. code:: powershell + + PS > vcpkg install eigen3 jsoncpp libtins pybind11 - > vcpkg install eigen3 jsoncpp libtins pybind11 +The currently tested vcpkg tag is ``2020.11-1``. After that, using a developer powershell prompt: -The currently tested vcpkg tag is ``2020.11-1``. After that, using a developer powershell prompt:: +.. code:: powershell - # first, specify the path to the ouster_example repository - > $env:OUSTER_SDK_PATH= + # first, specify the path to the ouster_example repository + PS > $env:OUSTER_SDK_PATH= - # point cmake to the location of vcpkg - > $env:CMAKE_TOOLCHAIN_FILE=/scripts/buildsystems/vcpkg.cmake + # point cmake to the location of vcpkg + PS > $env:CMAKE_TOOLCHAIN_FILE=/scripts/buildsystems/vcpkg.cmake - # then, build an installable "wheel" package - > py -m pip wheel --no-deps $OUSTER_SDK_PATH\python + # then, build an installable "wheel" package + PS > py -m pip wheel --no-deps $env:OUSTER_SDK_PATH\python - # or just install directly (virtualenv recommended) - > py -m pip install $env:OUSTER_SDK_PATH\python + # or just install directly (virtualenv recommended) + PS > py -m pip install $env:OUSTER_SDK_PATH\python See the top-level README in the `Ouster Example repository`_ for more details on setting up a development environment on Windows. @@ -78,31 +88,37 @@ Install in editable mode with pip using ``pip install -e``. For a faster develop rebuild using ``python3 setup.py build_ext -i`` instead of reinstalling the package after every change. For a local debug build, you can also add the ``-g`` flag. -The Ouster SDK package includes configuration for ``flake8`` and ``mypy``. To run:: +The Ouster SDK package includes configuration for ``flake8`` and ``mypy``. To run: - # install and run flake8 linter - $ python3 -m pip install flake8 - $ cd ${OUSTEr_SDK_PATH}/python - $ flake8 +.. code:: console - # install and run mypy in an environment with - $ python3 -m pip install mypy - $ mypy -p ouster.sdk -p ouster.client -p ouster.pcap + # install and run flake8 linter + $ python3 -m pip install flake8 + $ cd ${OUSTER_SDK_PATH}/python + $ flake8 + + # install and run mypy in an environment with + $ python3 -m pip install mypy + $ mypy src/ Running Tests ============= To run tests while developing, install the ``pytest`` package and run it from the root of the Python -SDK package:: +SDK package: + +.. code:: console + + $ cd ${OUSTER_SDK_PATH}/python + $ pytest - $ cd ${OUSTER_SDK_PATH}/python - $ pytest +To run tests against multiple Python versions simultaneously, use the ``tox`` package: -To run tests against multiple Python versions simultaneously, use the ``tox`` package:: +.. code:: console - $ cd ${OUSTER_SDK_PATH}/python - $ tox + $ cd ${OUSTER_SDK_PATH}/python + $ tox This will take longer, since it will build the package from a source distribution for each supported Python version available. @@ -113,13 +129,17 @@ Using Dockerfile To simplify testing on multiple linux distros, a Dockerfile is included for running ``tox`` on a variety of Debian-based distros with all packaged Python versions pre-installed. To build a test -image, run:: +image, run: - docker build ${OUSTER_SDK_PATH} -f ${OUSTER_SDK_PATH}/python/Dockerfile \ - --build-arg BASE=ubuntu:20.04 \ - -t ouster-sdk-tox \ +.. code:: console + + $ docker build ${OUSTER_SDK_PATH} -f ${OUSTER_SDK_PATH}/python/Dockerfile \ + --build-arg BASE=ubuntu:20.04 \ + -t ouster-sdk-tox \ the ``BASE`` argument will default to ``ubuntu:18.04``, but can also be set to other docker tags, -e.g. ``ubuntu:20.04`` or ``debian:10``. Then, run the container to invoke tox:: +e.g. ``ubuntu:20.04`` or ``debian:10``. Then, run the container to invoke tox: + +.. code:: console - docker run -it --rm ouster-sdk-tox + $ docker run -it --rm ouster-sdk-tox diff --git a/python/docs/examples.rst b/python/docs/examples.rst index a2ceb727..7d92b57c 100644 --- a/python/docs/examples.rst +++ b/python/docs/examples.rst @@ -6,7 +6,11 @@ A loosely connected collection of examples and concepts useful for working with are just starting, please see :ref:`quickstart`. For convenience, throughout the examples and concepts we will use ``pcap_path`` and -``metadata_path`` to refer to the variables from the Quick Start Guide. +``metadata_path`` to refer to the path to a Ouster pcap and metadata file. The pictures below are +taken from the `OS1 sample data`_. + + +.. _OS1 sample data: https://data.ouster.io/sdk-samples/OS1/OS1_128_sample.zip .. _ex-metadata: @@ -16,17 +20,30 @@ Obtaining Sensor Metadata ========================= Ouster sensors require metadata to interpret the readings of the sensor. Represented by the object -:py:class:`.SensorInfo`, its fields include configuration parameters such as -``lidar_mode`` and and sensor intrinsics like ``beam_azimuth_angles``. +:py:class:`.SensorInfo`, its fields include configuration parameters such as ``lidar_mode`` and +sensor intrinsics like ``beam_azimuth_angles``. When you work with a sensor, the client will automatically fetch the metadata. Recorded ``pcaps``, however, must always be accompanied by a ``json`` file containing the metadata of the -sensor as it was running when the data was recorded. +sensor as it was running when the data was recorded. Since it's crucial to save the correct metadata file, let's see how we can get that from a sensor. -Try running the following example:: +Try running the following example: + +.. tabs:: + + .. code-tab:: console Linux/macOS x64 + + $ python3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME fetch-metadata + + .. code-tab:: console macOS M1 + + $ arch --x86_64 python3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME fetch-metadata + + .. code-tab:: powershell Windows x64 + + PS > py -3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME fetch-metadata - $ python -m ouster.sdk.examples.client $SENSOR_HOSTNAME fetch-metadata And now let's look inside the example we just ran: @@ -52,7 +69,9 @@ Let's make a :py:class:`.PacketSource` from our sample data using :py:class:`.pc .. code:: python - metadata = client.SensorInfo(metadata_path) + with open(metadata_path, 'r') as f: + metadata = client.SensorInfo(f.read()) + source = pcap.Pcap(pcap_path, metadata) Now we can read packets from ``source`` with the following code: @@ -86,13 +105,12 @@ surprise us since its columns pertain to timestamps instead of azimuth angles. Let's destagger the image, changing the columns to represent the azimuth angles: -.. code:: +.. code:: python import matplotlib.pyplot as plt from more_itertools import nth - metadata = client.SensorInfo(metadata_path) - source = pcap.Pcap(pcap_path, metadata) + # ... `metadata` and `source` variables created as in the previous examples scans = client.Scans(source) @@ -106,6 +124,7 @@ Let's destagger the image, changing the columns to represent the azimuth angles: plt.imshow(ranges_destaggered, cmap='gray', resample=False) + This should give the scene below, of which we have magnified two patches for better visiblity. .. figure:: images/lidar_scan_destaggered.png @@ -121,7 +140,7 @@ This image now makes visual sense, and we can easily use this data in common vis By the way, you can view this particular scene in both 2D and 3D at Ouster's `Web Slam`_! Use your mouse to click and move the 3D scene, and the listed controls to rotate between different destaggered image views. The video at the bottom shows the registered point clouds of our - internal SLAM algorithm. + internal SLAM algorithm. .. _Web Slam: https://webslam.ouster.dev/slam/1610482355.9361048.rVdW_dgws/ @@ -129,43 +148,70 @@ This image now makes visual sense, and we can easily use this data in common vis .. _ex-xyzlut: -Working with 3D Points and the XYZLut +Projecting into Cartesian Coordinates ===================================== -To facilitate working with 3D points, you can create a function via :py:func:`.client.XYZLut` which -will project any :py:class:`.LidarScan` into cartesian coordinates by referencing a precalculated XYZ -Look-uptable. This function can then be applied to any scan to create a numpy array of ``H x W x -3``, which represents the cartesian coordintes of the points in the sensor coordinate frame. +To facilitate working with 3D points, you can call :py:func:`.client.XYZLut` to create a function +which will project a :py:class:`.LidarScan` into Cartesian coordinates using a precomputed lookup +table. The result of calling this function will be a point cloud represented as a numpy array. See +the API documentation for :py:func:`.client.XYZLut` for more details. .. literalinclude:: /../src/ouster/sdk/examples/client.py :start-after: [doc-stag-plot-xyz-points] :end-before: [doc-etag-plot-xyz-points] - :emphasize-lines: 2-5 + :emphasize-lines: 2-3 :linenos: :dedent: -If you have a sensor, you can run this code with one of our examples:: +If you have a sensor, you can run this code with one of our examples: - $ python -m ouster.sdk.examples.client $SENSOR_HOSTNAME plot-xyz-points +.. tabs:: + + .. code-tab:: console Linux/macOS x64 + + $ python3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME plot-xyz-points + + .. code-tab:: console macOS M1 + + $ arch --x86_64 python3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME plot-xyz-points + + .. code-tab:: powershell Windows x64 + + PS > py -3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME plot-xyz-points That should open a 3D plot of a single scan of your location taken just now by your sensor. You should be able to recognize the contours of the scene around you. -If you don’t have a sensor, you can run this code with our pcap examples:: +If you don’t have a sensor, you can run this code with our pcap examples: + +.. tabs:: - $ python -m ouster.sdk.examples.pcap OS1_128.pcap OS1_2048x10_128.json \ - plot-xyz-points --scan-num 84 + .. code-tab:: console Linux/macOS x64 + + $ python3 -m ouster.sdk.examples.pcap OS1_128.pcap OS1_2048x10_128.json plot-xyz-points --scan-num 84 + + .. code-tab:: console macOS M1 + + $ arch --x86_64 python3 -m ouster.sdk.examples.pcap OS1_128.pcap OS1_2048x10_128.json plot-xyz-points --scan-num 84 + + .. code-tab:: powershell Windows x64 + + PS > python3 -m ouster.sdk.examples.pcap OS1_128.pcap OS1_2048x10_128.json plot-xyz-points --scan-num 84 .. figure:: images/lidar_scan_xyz_84.png :align: center - Point cloud from sample data (scan 84). Points colored by ``SIGNAL`` value. + Point cloud from OS1 sample data (scan 84). Points colored by ``SIGNAL`` value. For details check the source code of an example :func:`.examples.pcap.pcap_display_xyz_points` +Also check out a more powerful way of visualizing ``xyz`` 3d points with :ref:`ex-open3d` + + .. _ex-correlating-2d-and-3d: + Working with 2D and 3D Representations Simultaneously ===================================================== @@ -178,6 +224,7 @@ the 3D points within a certain range and from certain azimuth angles. :end-before: [doc-etag-filter-3d] :emphasize-lines: 10-11, 15 :linenos: + :dedent: Since we'd like to filter on azimuth angles, first we first destagger both the 2D and 3D points, so that our columns in the ``HxW`` representation correspond to azimuth angle, not timestamp. (See @@ -192,9 +239,21 @@ columns correspond with timestamp). Finally, we select only the azimuth columns we're interested in. In this case, we've arbitrarily chosen the first 270 degrees of rotation. -If you have a sensor, you can run this code with an example:: - - $ python -m ouster.sdk.examples.client $SENSOR_HOSTNAME filter-3d-by-range-and-azimuth +If you have a sensor, you can run this code with an example: + +.. tabs:: + + .. code-tab:: console Linux/macOS x64 + + $ python3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME filter-3d-by-range-and-azimuth + + .. code-tab:: console macOS M1 + + $ arch --x86_64 python3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME filter-3d-by-range-and-azimuth + + .. code-tab:: powershell Windows x64 + + PS > python3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME filter-3d-by-range-and-azimuth .. _ex-streaming: @@ -203,9 +262,21 @@ Streaming Live Data =================== Instead of working with a recorded dataset or a few captured frames of data, let's see if we can get -a live feed from the sensor:: - - $ python -m ouster.sdk.examples.client $SENSOR_HOSTNAME live-plot-signal +a live feed from the sensor: + +.. tabs:: + + .. code-tab:: console Linux/macOS x64 + + $ python3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME live-plot-signal + + .. code-tab:: console macOS M1 + + $ arch --x86_64 python3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME live-plot-signal + + .. code-tab:: powershell Windows x64 + + PS > py -3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME live-plot-signal This should give you a live feed from your sensor that looks like a black and white moving image. Try waving your hand or moving around to find yourself within the image! @@ -233,9 +304,22 @@ Recording Sensor Data ===================== It's easy to record data to a pcap file from a sensor programatically. Let's try it first -with the following example:: +with the following example: + +.. tabs:: + + .. code-tab:: console Linux/macOS x64 + + $ python3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME record-pcap + + .. code-tab:: console macOS M1 + + $ arch --x86_64 python3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME record-pcap + + .. code-tab:: powershell Windows x64 + + PS > py -3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME record-pcap - $ python -m ouster.sdk.examples.client $SENSOR_HOSTNAME record-pcap This will capture the :class:`.client.LidarPacket`'s and :class:`.client.ImuPacket`'s data for 10 seconds and store the pcap file along with the metadata json file into the current directory. @@ -256,15 +340,27 @@ module. .. _ex-pcap-live-preview: -Pcap Live Data Preview +PCAP Live Data Preview ======================= We can easily view the data that was recorded in the previous section. Based on an example from :ref:`ex-streaming` above we have a pcap viewer with additional *stagger*/*destagger* handler on key `D` and pause on `SPACE` key (source code: :func:`.examples.pcap.pcap_2d_viewer`). To run it try -the following command:: +the following command: + +.. tabs:: + + .. code-tab:: console Linux/macOS x64 - $ python -m ouster.sdk.examples.pcap OS1_128.pcap OS1_2048x10_128.json 2d-viewer + $ python3 -m ouster.sdk.examples.pcap OS1_128.pcap OS1_2048x10_128.json 2d-viewer + + .. code-tab:: console macOS M1 + + $ arch --x86_64 python3 -m ouster.sdk.examples.pcap OS1_128.pcap OS1_2048x10_128.json 2d-viewer + + .. code-tab:: powershell Windows x64 + + PS > python3 -m ouster.sdk.examples.pcap OS1_128.pcap OS1_2048x10_128.json 2d-viewer Or substitute example data with pcap and json that you just recorded. @@ -272,23 +368,35 @@ Or substitute example data with pcap and json that you just recorded. .. _ex-pcap-to-csv: -Pcap to CSV +Converting PCAPs to CSV ======================= Sometimes we want to get a point cloud (``XYZ`` + other fields) as a ``CSV`` file for further analysis with other tools. -To convert the first ``5`` scans of sample data from a pcap file, you can try:: +To convert the first ``5`` scans of our sample data from a pcap file, you can try: + +.. tabs:: + + .. code-tab:: console Linux/macOS x64 + + $ python3 -m ouster.sdk.examples.pcap OS1_128.pcap OS1_2048x10_128.json pcap-to-csv --scan-num 5 + + .. code-tab:: console macOS M1 + + $ arch --x86_64 python3 -m ouster.sdk.examples.pcap OS1_128.pcap OS1_2048x10_128.json pcap-to-csv --scan-num 5 + + .. code-tab:: powershell Windows x64 + + PS > py -3 -m ouster.sdk.examples.pcap OS1_128.pcap OS1_2048x10_128.json pcap-to-csv --scan-num 5 - $ python -m ouster.sdk.examples.pcap OS1_128.pcap OS1_2048x10_128.json \ - pcap-to-csv --scan-num 5 The source code of an example below: .. literalinclude:: /../src/ouster/sdk/examples/pcap.py :start-after: [doc-stag-pcap-to-csv] :end-before: [doc-etag-pcap-to-csv] - :emphasize-lines: 37-41 + :emphasize-lines: 33-37 :linenos: :dedent: @@ -310,11 +418,106 @@ We used ``128`` while restoring 2D image from a CSV file because it's the number Check :func:`.examples.pcap.pcap_to_csv` documentation for further details. + +.. _ex-open3d: + + +Visualization with Open3d +========================= + +The `Open3d library`_ contains Python bindings for a variety of tools for working with point cloud +data. Loading data into Open3d is just a matter of reshaping the numpy representation of a point +cloud, as demonstrated in the :func:`.examples.pcap.pcap_3d_one_scan` example: + +.. literalinclude:: /../src/ouster/sdk/examples/pcap.py + :start-after: [doc-stag-open3d-one-scan] + :end-before: [doc-etag-open3d-one-scan] + :emphasize-lines: 1-6 + :linenos: + :dedent: + +The :mod:`.examples.open3d` module contains a more fully-featured visualizer built using the Open3d +library, which can be used to replay pcap files or visualize a running sensor. The bulk of the +visualizer is implemented in the :func:`.examples.open3d.viewer_3d` function. + +.. note:: + + You'll have to install the `Open3d package`_ from PyPI to run this example. Note that as of + version ``0.13.0``, binaries are not yet provided for Python 3.9 or ARM systems. + + +As an example, you can view frame ``84`` from the sample data by running the following command: + +.. tabs:: + + .. code-tab:: console Linux/macOS x64 + + $ python3 -m ouster.sdk.examples.open3d \ + --pcap OS1_128.pcap --meta OS1_2048x10_128.json --start 84 --pause + + .. code-tab:: console macOS M1 + + $ arch --x86_64 python3 -m ouster.sdk.examples.open3d \ + --pcap OS1_128.pcap --meta OS1_2048x10_128.json --start 84 --pause + + .. code-tab:: powershell Windows x64 + + PS > py -3 -m ouster.sdk.examples.open3d ^ + --pcap OS1_128.pcap --meta OS1_2048x10_128.json --start 84 --pause + +You may also want to try the ``--sensor`` option to display the output of a running sensor. Use the +``-h`` flag to see a full list of command line options and flags. + +Running the example above should open a window displaying a scene from a city intersection, +reproduced below: + +.. figure:: images/lidar_scan_xyz_84_3d.png + :align: center + + Open3D visualization of OS1 sample data (frame 84). Points colored by ``SIGNAL`` field. + +You should be able to click and drag the mouse to look around. You can zoom in and out using the +mouse wheel, and hold control or shift while dragging to pan and roll, respectively. + +Hitting the spacebar will start playing back the rest of the pcap in real time. Note that reasonable +performance for realtime playback requires relatively fast hardware, since Open3d runs all rendering +and processing in a single thread. + +All of the visualizer controls are listed in the table below: + +.. list-table:: Open3d Visualizer Controls + :widths: 15 30 + :header-rows: 1 + :align: center + + * - Key + - What it does + * - **Mouse wheel** + - Zoom in an out + * - **Left click + drag** + - Tilt and rotate the camera + * - **Ctrl + left click + drag** + - Pan the camera laterally + * - **Shift + left click + drag** + - Roll the camera + * - **"+" / "-"** + - Increase or decrease point sizes + * - **Spacebar** + - Pause or resume playback + * - **"M"** + - Cycle through channel fields used for visualization + * - **Right arrow key** + - When reading a pcap, jump 10 frames forward + +.. _Open3d library: http://www.open3d.org/ +.. _Open3d package: https://pypi.org/project/open3d/ + .. _ex-imu: -Working with IMU data from the Ouster sensor +Working with IMU data from the Ouster Sensor ============================================ + IMU data from the Ouster sensor can be read as :py:class:`~.client.ImuPacket`. Let's do something easy, like graph the acceleration in z direction over time. Let's look at some code: @@ -324,9 +527,21 @@ easy, like graph the acceleration in z direction over time. Let's look at some c :emphasize-lines: 4-6 :linenos: -Like other ``Packets``, we'll want to get them from a :py:class:`.PacketSource`. After getting ``imu_packet_list``, we obtain the ``sys_ts`` and ``z`` part of ``accel`` and plot them. +Like other ``Packets``, we'll want to get them from a :py:class:`.PacketSource`. After getting +``imu_packet_list``, we obtain the ``sys_ts`` and ``z`` part of ``accel`` and plot them. + +If you have a sensor, you can run the code above with the ``plot-imu-z-accel`` example: + +.. tabs:: + + .. code-tab:: console Linux/macOS x64 + + $ python3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME plot-imu-z-accel + + .. code-tab:: console macOS M1 -If you have a sensor, you can run the code above with the ``plot-imu-z-accel`` example:: + $ arch --x86_64 python3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME plot-imu-z-accel - $ python -m ouster.sdk.examples.client $SENSOR_HOSTNAME plot-imu-z-accel + .. code-tab:: powershell Windows x64 + PS > py -3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME plot-imu-z-accel diff --git a/python/docs/images/brooklyn_bridge_ls_50_range_image.png b/python/docs/images/brooklyn_bridge_ls_50_range_image.png new file mode 100644 index 00000000..3cafd95e Binary files /dev/null and b/python/docs/images/brooklyn_bridge_ls_50_range_image.png differ diff --git a/python/docs/images/brooklyn_bridge_ls_50_xyz_cut.png b/python/docs/images/brooklyn_bridge_ls_50_xyz_cut.png new file mode 100644 index 00000000..eb46f32e Binary files /dev/null and b/python/docs/images/brooklyn_bridge_ls_50_xyz_cut.png differ diff --git a/python/docs/images/lidar_scan_range_image.png b/python/docs/images/lidar_scan_range_image.png deleted file mode 100644 index 4f8e4864..00000000 Binary files a/python/docs/images/lidar_scan_range_image.png and /dev/null differ diff --git a/python/docs/images/lidar_scan_xyz.png b/python/docs/images/lidar_scan_xyz.png deleted file mode 100644 index e7d96151..00000000 Binary files a/python/docs/images/lidar_scan_xyz.png and /dev/null differ diff --git a/python/docs/images/lidar_scan_xyz_84_3d.png b/python/docs/images/lidar_scan_xyz_84_3d.png new file mode 100644 index 00000000..97523c25 Binary files /dev/null and b/python/docs/images/lidar_scan_xyz_84_3d.png differ diff --git a/python/docs/quickstart.rst b/python/docs/quickstart.rst index a064b8c5..45673287 100644 --- a/python/docs/quickstart.rst +++ b/python/docs/quickstart.rst @@ -11,28 +11,55 @@ sample data or a sensor connected to your machine. Installation ============ -The Ouster Python SDK requires Python >= 3.6 and pip >= 19.0. To install on `supported platforms`_, run:: +The Ouster Python SDK requires Python >= 3.6 and pip >= 19.0. To install on :ref:`supported platforms`, run: - $ python3 -m pip install ouster-sdk[examples] +.. tabs:: -.. note:: + .. code-tab:: console Linux/macOS x64 + + $ python3 -m pip install 'ouster-sdk[examples]' + + .. code-tab:: console macOS M1 - Newer users to Python should create a suitable `venv`_, `activate`_ it, and ensure that they have - `upgraded pip`_ once their venv is activated. + $ arch --x86_64 python3 -m pip install 'ouster-sdk[examples]' + + .. code-tab:: powershell Windows x64 + + PS > py -3 -m pip install 'ouster-sdk[examples]' -To check that you've succesfully installed the latest version of the Ouster Python SDK, run:: - - $ pip list .. note:: - To run the example code on Windows 10, you may also find that you need the ``PyQt5`` library. + **Using a virtual environment** is recommended. Users newer to Python should read the official + `venv instructions`_ and ensure that they have `upgraded pip`_ once their venv is activated. If + you're using venv on Windows, you'll want to use ``python`` and ``pip`` instead of ``py -3`` and + ``py -3 -m pip`` in the following Powershell snippets. + + **Apple M1 users** should be aware that they will need to prepend all python3 commands with + ``arch --x86_64`` when working with ouster-sdk to force macOS to run the intel versions of python + as numpy support on native M1 has not arrived yet. + + +To check that you've successfully installed the latest version of the Ouster Python SDK, run the +following command and make sure that the ``ouster-sdk`` package is included in the output: + +.. tabs:: + + .. code-tab:: console Linux/macOS x64 + + $ python3 -m pip list + + .. code-tab:: console macOS M1 + + $ arch --x86_64 python3 -m pip list + + .. code-tab:: powershell Windows x64 + + PS > py -3 -m pip list -.. _supported platforms: https://static.ouster.dev/sdk-docs/index.html#installation .. _upgraded pip: https://pip.pypa.io/en/stable/installing/#upgrading-pip -.. _venv: https://packaging.python.org/guides/installing-using-pip-and-virtual-environments/#creating-a-virtual-environment -.. _activate: https://packaging.python.org/guides/installing-using-pip-and-virtual-environments/#activating-a-virtual-environment +.. _venv instructions: https://packaging.python.org/guides/installing-using-pip-and-virtual-environments/#creating-a-virtual-environment Using this Guide @@ -41,49 +68,57 @@ Using this Guide You'll want to start an interactive Python session and keep it open through the sections, as we'll be reusing variables created in earlier parts while explaining what we're doing as we go. -To get started, open a new terminal window and start a python interpreter:: +To get started, open a new console/Powershell window and start a python interpreter: - $ python3 +.. tabs:: -In the python session that opens, import the Ouster Python Client: + .. code-tab:: console Linux/macOS x64 + + $ python3 + + .. code-tab:: console macOS M1 + + $ arch --x86_64 python3 + + .. code-tab:: powershell Windows x64 + + PS > py -3 -.. code:: python - - >>> from ouster import client Throughout this guide we will indicate console commands with ``$`` and python interpreter commands with ``>>>``, just as we have above. -If you'd like to start by working with sample data, continue to the section below. If you'd prefer -to start capturing data from a sensor, you can skip to `Using an Ouster Sensor`_ below. +If you'd like to start by working with sample data, continue to the next section. If you'd prefer to +start capturing data from a sensor, you can skip to `Using an Ouster Sensor`_ below. Using Sample Data ================= -Download the `sample data`_ (**1.6 GB**) and unzip the contents. You should have two files: +Download the `OS2 bridge sample data`_ (**80 MB**) and unzip the contents. To use subsequent code +snippets as-is you should extract this file into the same directory from which you're running your +Python interpreter. You should have two files: - * ``OS1_128.pcap`` - * ``OS1_2048x10_128.json`` + * ``OS2_128_bridge_sample.pcap`` + * ``OS2_2048x10_128.json`` -The downloaded pcap file contains lidar and imu packets captured from the network . You can read +The downloaded pcap file contains lidar and imu packets captured from the network. You can read more about the `IMU Data Format`_ and `Lidar Data Format`_ in the Ouster Sensor Documentation. The JSON file contains metadata queried from the sensor TCP interface necessary for interpreting the packet data. -Let's load the paths into your open session of python: - .. code:: python - >>> pcap_path = '/path/to/OS1_128.pcap' - >>> metadata_path = '/path/to/OS1_2048x10_128.json' - + >>> pcap_path = 'OS2_128_bridge_sample.pcap' + >>> metadata_path = 'OS2_2048x10_128.json' -Because our pcap file contains the UDP packet stream but not the sensor metadata, we load the -metadata from ``metadata_path`` first: +You may have do adjust these paths to the directory where the unzipped ``pcap`` and ``json`` file +are located. Because our pcap file contains the UDP packet stream but not the sensor metadata, we +load the metadata from ``metadata_path`` first, using the client module: .. code:: python - + + >>> from ouster import client >>> with open(metadata_path, 'r') as f: ... metadata = client.SensorInfo(f.read()) @@ -99,10 +134,9 @@ captured UDP data by instantiating :py:class:`.pcap.Pcap`. This class acts as a To visualize data from this pcap file, proceed to `Visualizing Lidar Data`_ below. -.. _sample data: https://data.ouster.io/sdk-samples/OS1/OS1_128_sample.zip +.. _OS2 bridge sample data: https://data.ouster.io/sdk-samples/OS2/OS2_128_bridge_sample.zip .. _Lidar Data Format: https://data.ouster.io/downloads/software-user-manual/software-user-manual-v2p0.pdf#10 .. _IMU Data Format: https://data.ouster.io/downloads/software-user-manual/software-user-manual-v2p0.pdf#13 -.. _Ouster Sample Data: https://ouster.com/resources/lidar-sample-data/ Using an Ouster Sensor @@ -117,29 +151,47 @@ If you have access to sensor hardware, you can start reading data by instantiati Sensor Documentation. In the following, ```` should be substituted for the actual hostname or IP of your -sensor and ```` should be the hostname or IP of the machine reading sensor data, per the -network configuration. +sensor. To make sure everything is connected, open a separate console window and try pinging the sensor. You -should see some output like:: +should see some output like: + +.. tabs:: + + .. code-tab:: console Linux/macOS x64 + + $ ping -c1 + PING (192.0.2.42) 56(84) bytes of data. + 64 bytes from (192.0.2.42): icmp_seq=1 ttl=64 time=0.217 ms + + .. code-tab:: console macOS M1 + + $ ping -c1 + PING (192.0.2.42) 56(84) bytes of data. + 64 bytes from (192.0.2.42): icmp_seq=1 ttl=64 time=0.217 ms + + .. code-tab:: powershell Windows x64 - $ ping -c1 - PING (192.0.2.42) 56(84) bytes of data. - 64 bytes from (192.0.2.42): icmp_seq=1 ttl=64 time=0.217 ms + PS > ping /n 10 + Pinging (192.0.2.42) with 32 bytes of data: + Reply from 192.0.2.42: bytes=32 time=101ms TTL=124 -Next, you'll need to configure the sensor with the config parameters. In your open python session: + +Next, you'll need to configure the sensor with the config parameters using the client module. In +your open python session: .. code:: python >>> hostname = '' + >>> from ouster import client >>> config = client.SensorConfig() >>> config.udp_port_lidar = 7502 >>> config.udp_port_imu = 7503 >>> config.operating_mode = client.OperatingMode.OPERATING_NORMAL - >>> client.set_config(hostname, config, persist=True, udp_dest_auto=True) + >>> client.set_config(hostname, config, persist=True, udp_dest_auto = True) Just like with the sample data, you can create a :py:class:`.PacketSource` from the sensor: - + .. code:: python >>> source = client.Sensor(hostname) @@ -155,21 +207,19 @@ Visualizing Lidar Data ====================== At this point, you should have defined ``source`` using either a pcap file or UDP data streaming -directly from a sensor. Let's read from ``source`` until we get to the 84th frame of data: +directly from a sensor. Let's read from ``source`` until we get to the 50th frame of data: .. code:: python >>> from contextlib import closing >>> from more_itertools import nth >>> with closing(client.Scans(source)) as scans: - ... scan = nth(client.Scans(source), 84) - >>> scan - + ... scan = nth(scans, 50) .. note:: - If you're using a sensor and it takes a few seconds, don't be alarmed! It has to get to the 84th - frame of data, which would be 8.4 seconds into recording for a sensor in 1024x10 mode. + If you're using a sensor and it takes a few seconds, don't be alarmed! It has to get to the 50th + frame of data, which would be 5.0 seconds for a sensor running in 1024x10 mode. Now that we have a frame of data available as a :py:class:`.LidarScan` datatype, we can extract the range measurements and turn them into a range image where each column corresponds to a single @@ -181,26 +231,30 @@ azimuth angle: >>> range_img = client.destagger(source.metadata, range_field) We can plot the results using standard Python tools that work with numpy datatypes. Here, we extract -the first 512 columns of range data and display the result: +a column segment of the range data and display the result: .. code:: python >>> import matplotlib.pyplot as plt - >>> plt.imshow(range_img[:, 0:512], cmap='gray', resample=False) + >>> plt.imshow(range_img[:, 640:1024], resample=False) >>> plt.axis('off') >>> plt.show() .. note:: - + If running ``plt.show`` gives you an error about your Matplotlib backend, you will need a `GUI backend`_ such as TkAgg or Qt5Agg in order to visualize your data with matplotlib. -.. figure:: images/lidar_scan_range_image.png - :align: center - First 512 columns of LidarScan ``RANGE`` field of sample data with simple gray colormapping. + +.. figure:: images/brooklyn_bridge_ls_50_range_image.png + :align: center + :figwidth: 100% + + Range image of OS2 sample data. Data taken at Brooklyn Bridge, NYC. + In addition to viewing the data in 2D, we can also plot the results in 3D by projecting the range -measurements into cartesian coordinates. To do this, we first create a lookup table, then use it to +measurements into Cartesian coordinates. To do this, we first create a lookup table, then use it to produce X, Y, Z coordinates from our scan data with shape (H x W x 3): .. code:: python @@ -215,19 +269,24 @@ Now we rearrange the resulting numpy array into a shape that's suitable for plot >>> import numpy as np >>> [x, y, z] = [c.flatten() for c in np.dsplit(xyz, 3)] >>> ax = plt.axes(projection='3d') - >>> r = 30 + >>> r = 10 >>> ax.set_xlim3d([-r, r]) >>> ax.set_ylim3d([-r, r]) - >>> ax.set_zlim3d([0, 2 * r]) - >>> ax.scatter(x, y, z, c=z / max(z), s=0.2) + >>> ax.set_zlim3d([-r/2, r/2]) + >>> plt.axis('off') + >>> z_col = np.minimum(np.absolute(z), 5) + >>> ax.scatter(x, y, z, c=z_col, s=0.2) >>> plt.show() -To learn more about manipulating lidar data, see :ref:`ex-staggered-and-destaggered`, :ref:`ex-xyzlut` and :ref:`ex-correlating-2d-and-3d`. +You should be able to rotate the resulting scene to view it from different angles. -.. figure:: images/lidar_scan_xyz.png +To learn more about manipulating lidar data, see :ref:`ex-staggered-and-destaggered`, +:ref:`ex-xyzlut` and :ref:`ex-correlating-2d-and-3d`. + +.. figure:: images/brooklyn_bridge_ls_50_xyz_cut.png :align: center - Point cloud from sample data. Points colored by Z coordinate value. + Point cloud from OS2 sample data with colormap on z. Data taken at Brooklyn Bridge, NYC. .. _GUI backend: https://matplotlib.org/stable/tutorials/introductory/usage.html#the-builtin-backends @@ -250,5 +309,5 @@ Here are a few things you might be interested in: * :ref:`ex-xyzlut` * :ref:`ex-correlating-2d-and-3d` * :ref:`ex-pcap-to-csv` + * :ref:`ex-open3d` * :ref:`ex-imu` - diff --git a/python/mypy.ini b/python/mypy.ini index f6de557c..f762999a 100644 --- a/python/mypy.ini +++ b/python/mypy.ini @@ -1,5 +1,10 @@ [mypy] +# To avoid "Source file found twice under different module names" when using src +# layout + namespace packages. See: +# https://mypy.readthedocs.io/en/stable/running_mypy.html#mapping-file-paths-to-modules +mypy_path=src namespace_packages = True +explicit_package_bases = True -[mypy-numpy.*] +[mypy-open3d.*] ignore_missing_imports = True \ No newline at end of file diff --git a/python/setup.py b/python/setup.py index 329855d1..ebde7102 100644 --- a/python/setup.py +++ b/python/setup.py @@ -58,6 +58,11 @@ def build_extension(self, ext): if toolchain: cmake_args += ['-DCMAKE_TOOLCHAIN_FILE=' + toolchain] + # specify VCPKG triplet in env + triplet = env.get('VCPKG_TARGET_TRIPLET') + if triplet: + cmake_args += ['-DVCPKG_TARGET_TRIPLET=' + triplet] + # use sdk path from env or location in sdist sdk_path = env.get('OUSTER_SDK_PATH') sdist_sdk_path = os.path.join(ext.sourcedir, "sdk") @@ -92,7 +97,7 @@ def run(self): setup( name='ouster-sdk', url='https://github.com/ouster-lidar/ouster_example', - version='0.2.0', + version='0.2.1', package_dir={'': 'src'}, packages=find_namespace_packages(where='src'), namespace_packages=['ouster'], @@ -121,13 +126,20 @@ def run(self): extras_require={ 'test': ['pytest', 'tox'], 'dev': [ - 'flake8', 'future', 'mypy', 'pyls-mypy', 'python-language-server', - 'yapf' + 'flake8', 'future', 'mypy', 'mypy-ls', 'python-lsp-server', 'yapf' ], 'docs': [ 'Sphinx >=3.5', 'sphinx-autodoc-typehints ==1.11.1', 'sphinx-rtd-theme ==0.5.2', + 'sphinx-copybutton ==0.3.1', + 'docutils <0.17', + 'sphinx-tabs ==3.0.0', + 'open3d', + ], + 'examples': [ + 'matplotlib', + 'opencv-python', + 'PyQt5; platform_system=="Windows"', ], - 'examples': ['matplotlib', 'opencv-python'], }) diff --git a/python/src/cpp/client.cpp b/python/src/cpp/client.cpp index dfda8bb6..f1788ff3 100644 --- a/python/src/cpp/client.cpp +++ b/python/src/cpp/client.cpp @@ -525,6 +525,7 @@ directly. .def_readwrite("operating_mode", &sensor_config::operating_mode, "Operating Mode of sensor. See class OperatingMode.") .def_readwrite("multipurpose_io_mode", &sensor_config::multipurpose_io_mode, "Mode of MULTIPURPOSE_IO pin. See class MultipurposeIOMode.") .def_readwrite("azimuth_window", &sensor_config::azimuth_window, "Tuple representing the visible region of interest of the sensor in millidegrees, .e.g., (0, 360000) for full visibility.") + .def_readwrite("signal_multiplier", &sensor_config::signal_multiplier, "Multiplier for signal strength of sensor, corresponding to maximum allowable azimuth_window. Gen 2 Only.") .def_readwrite("sync_pulse_out_angle", &sensor_config::sync_pulse_out_angle, "Polarity of SYNC_PULSE_OUT output. See sensor documentaiton for details." ) .def_readwrite("sync_pulse_out_pulse_width", &sensor_config::sync_pulse_out_pulse_width, "SYNC_PULSE_OUT pulse width in ms. See sensor documentaiton for details.") .def_readwrite("nmea_in_polarity", &sensor_config::nmea_in_polarity, "Polarity of NMEA UART input $GPRMC messages. See sensor documentaiton for details.") @@ -609,7 +610,7 @@ directly. py::arg("lidar_port") = 0, py::arg("imu_port") = 0, py::arg("timeout_sec") = 30, py::arg("capacity") = 128) .def("get_metadata", &PyClient::get_metadata, - py::arg("timeout_sec") = 30) + py::arg("timeout_sec") = 60) .def("shutdown", &PyClient::shutdown) .def("consume", &PyClient::consume) .def("produce", &PyClient::produce) diff --git a/python/src/cpp/pcap.cpp b/python/src/cpp/pcap.cpp index 2d09ae70..319997db 100644 --- a/python/src/cpp/pcap.cpp +++ b/python/src/cpp/pcap.cpp @@ -9,6 +9,7 @@ #include #include +#include #include #include @@ -105,7 +106,10 @@ directly. .def_readonly("src_ip", &ouster::sensor_utils::packet_info::src_ip) .def_readonly("dst_port", &ouster::sensor_utils::packet_info::dst_port) .def_readonly("src_port", &ouster::sensor_utils::packet_info::src_port) - .def_readonly("timestamp", &ouster::sensor_utils::packet_info::timestamp) + .def_property_readonly("timestamp", + [](ouster::sensor_utils::packet_info& packet_info) -> double { + return packet_info.timestamp.count() / 1e6; + }) .def_readonly("payload_size", &ouster::sensor_utils::packet_info::payload_size); @@ -136,12 +140,13 @@ directly. m.def("record_uninitialize", &ouster::sensor_utils::record_uninitialize); m.def("record_packet", [](ouster::sensor_utils::record_handle& handle, - int src_port, int dst_port, py::buffer buf) { + int src_port, int dst_port, py::buffer buf, double timestamp) { ouster::sensor_utils::record_packet(handle, src_port, dst_port, getptr(buf), - getptrsize(buf)); + getptrsize(buf), + llround(timestamp * 1e6)); }); // clang-format on diff --git a/python/src/ouster/client/_client.pyi b/python/src/ouster/client/_client.pyi index dcdf9d09..676b4ff5 100644 --- a/python/src/ouster/client/_client.pyi +++ b/python/src/ouster/client/_client.pyi @@ -135,6 +135,9 @@ class SensorInfo: def __eq__(self, other: object) -> bool: ... + def __copy__(self) -> SensorInfo: + ... + class DataFormat: columns_per_frame: int @@ -431,6 +434,7 @@ class SensorConfig: operating_mode: Optional[OperatingMode] multipurpose_io_mode: Optional[MultipurposeIOMode] azimuth_window: Optional[tuple] + signal_multiplier: Optional[int] sync_pulse_out_angle: Optional[int] sync_pulse_out_pulse_width: Optional[int] nmea_in_polarity: Optional[Polarity] @@ -457,9 +461,14 @@ class SensorConfig: def __ne__(self, other: object) -> bool: ... + def __copy__(self) -> SensorConfig: + ... + -def set_config(hostname: str, config: SensorConfig, persist: bool, - udp_dest_auto: bool) -> None: +def set_config(hostname: str, + config: SensorConfig, + persist: bool = ..., + udp_dest_auto: bool = ...) -> None: ... diff --git a/python/src/ouster/client/core.py b/python/src/ouster/client/core.py index baca6fa7..37757cf0 100644 --- a/python/src/ouster/client/core.py +++ b/python/src/ouster/client/core.py @@ -203,9 +203,10 @@ def __iter__(self) -> Iterator[Packet]: the sensor may cause packets to be dropped. Raises: - ClientTimeout: If no packets are received within the configured + ClientTimeout: if no packets are received within the configured timeout. - ClientError: If the client enters an unspecified error state. + ClientError: if the client enters an unspecified error state. + ValueError: if the packet source has already been closed """ # Attempt to flush any old data before producing packets @@ -217,7 +218,7 @@ def __iter__(self) -> Iterator[Packet]: if p is not None: yield p else: - return + raise ValueError("I/O operation on closed packet source") def flush(self, n_frames: int = 3, *, full=False) -> int: """Drop some data to clear internal buffers. @@ -435,4 +436,4 @@ def stream(cls, timeout=timeout, _flush_before_read=True) - return cls(source, timeout=timeout, complete=complete, _max_latency=1) + return cls(source, timeout=timeout, complete=complete, _max_latency=2) diff --git a/python/src/ouster/client/data.py b/python/src/ouster/client/data.py index c05fd88a..5379f987 100644 --- a/python/src/ouster/client/data.py +++ b/python/src/ouster/client/data.py @@ -17,8 +17,9 @@ class ImuPacket: """Read IMU Packet data from a bufer.""" _pf: _client.PacketFormat _data: np.ndarray + capture_timestamp: Optional[float] - def __init__(self, data: BufferT, info: SensorInfo) -> None: + def __init__(self, data: BufferT, info: SensorInfo, timestamp: Optional[float] = None) -> None: """ Args: data: Buffer containing the packet payload @@ -34,6 +35,8 @@ def __init__(self, data: BufferT, info: SensorInfo) -> None: dtype=np.uint8, count=self._pf.imu_packet_size) + self.capture_timestamp = timestamp + @property def sys_ts(self) -> int: """System timestamp in nanoseconds.""" @@ -107,8 +110,9 @@ class LidarPacket: _pf: _client.PacketFormat _data: np.ndarray _column_bytes: int + capture_timestamp: Optional[float] - def __init__(self, data: BufferT, info: SensorInfo) -> None: + def __init__(self, data: BufferT, info: SensorInfo, timestamp: Optional[float] = None) -> None: """ This will always alias the supplied buffer-like object. Pass in a copy to avoid unintentional aliasing. @@ -128,6 +132,7 @@ def __init__(self, data: BufferT, info: SensorInfo) -> None: self._column_bytes = LidarPacket._COL_PREAMBLE_BYTES + \ (LidarPacket._PIXEL_BYTES * self._pf.pixels_per_column) + \ LidarPacket._COL_FOOTER_BYTES + self.capture_timestamp = timestamp def field(self, field: ChanField) -> np.ndarray: """Create a view of the specified channel field. @@ -200,18 +205,17 @@ def _complete(self, column_window: metadata.format.column_window if it's not default to full scan """ - if column_window: - win_start, win_end = column_window - if win_start < win_end: - return (self.header(ColHeader.STATUS)[win_start:win_end + - 1] == 0xFFFFFFFF).all() - else: - valid = (self.header( - ColHeader.STATUS)[win_start:] == 0xFFFFFFFF).all() - valid = valid and (self.header(ColHeader.STATUS)[:win_end + 1] - == 0xFFFFFFFF).all() - return valid - return (self.header(ColHeader.STATUS) == 0xFFFFFFFF).all() + if column_window is None: + column_window = (0, self.w - 1) + + win_start, win_end = column_window + status = self.header(ColHeader.STATUS) + + if win_start <= win_end: + return (status[win_start:win_end + 1] == 0xFFFFFFFF).all() + else: + return ((status[:win_end + 1] == 0xFFFFFFFF).all() + and (status[win_start:] == 0xFFFFFFFF).all()) def field(self, field: ChanField) -> np.ndarray: """Return a view of the specified channel field.""" @@ -315,15 +319,18 @@ def XYZLut(info: SensorInfo) -> Callable[[LidarScan], np.ndarray]: """Return a function that can project scans into cartesian coordinates. Internally, this will pre-compute a lookup table using the supplied - intrinsic parameters. XYZ points are returned as a H * W x 3 array of + intrinsic parameters. XYZ points are returned as a H x W x 3 array of doubles, where H is the number of beams and W is the horizontal resolution of the scan. + The coordinates are reported in meters in the *sensor frame* as + defined in the sensor documentation. + Args: info: sensor metadata Returns: - A function that computes a numpy array of a scan's point coordinates in meters + A function that computes a numpy array of a scan's point coordinates """ lut = _client.XYZLut(info) diff --git a/python/src/ouster/pcap/_pcap.pyi b/python/src/ouster/pcap/_pcap.pyi index 3195d732..1fdb611d 100644 --- a/python/src/ouster/pcap/_pcap.pyi +++ b/python/src/ouster/pcap/_pcap.pyi @@ -1,8 +1,6 @@ """Type annotations for pcap python bindings.""" - -import datetime from ouster.client import BufferT -from typing import Dict, Tuple +from typing import Dict class playback_handle: @@ -77,7 +75,7 @@ class packet_info: ... @property - def timestamp(self) -> datetime.timedelta: + def timestamp(self) -> float: ... @@ -89,7 +87,8 @@ def read_packet(handle: playback_handle, buf: BufferT) -> int: ... -def record_initialize(file_name: str, src_ip: str, dst_ip: str, frag_size: int) -> record_handle: +def record_initialize(file_name: str, src_ip: str, dst_ip: str, + frag_size: int) -> record_handle: ... @@ -97,15 +96,18 @@ def record_uninitialize(handle: record_handle) -> None: ... -def record_packet(handle: record_handle, src_port: int, dst_port: int, buf: BufferT) -> None: +def record_packet(handle: record_handle, src_port: int, dst_port: int, + buf: BufferT, timestamp: float) -> None: ... -def replay_get_pcap_info(file_name: str, packets_to_process: int) -> stream_info: +def replay_get_pcap_info(file_name: str, + packets_to_process: int) -> stream_info: ... -def replay_initialize(file_name: str, src_ip: str, dst_ip: str, port_map: Dict[int, int]) -> playback_handle: +def replay_initialize(file_name: str, src_ip: str, dst_ip: str, + port_map: Dict[int, int]) -> playback_handle: ... diff --git a/python/src/ouster/pcap/pcap.py b/python/src/ouster/pcap/pcap.py index 401ac5dd..604c579b 100644 --- a/python/src/ouster/pcap/pcap.py +++ b/python/src/ouster/pcap/pcap.py @@ -1,4 +1,5 @@ -from dataclasses import dataclass +from dataclasses import dataclass, field +import os import time from threading import Lock from typing import Dict, Iterable, Iterator, Optional, Tuple @@ -8,6 +9,7 @@ from ouster.pcap import _pcap +# TODO: Look into at possible custom OusterExceptions to raise @dataclass class PcapInfo: """Information queried about a packet capture from an Ouster sensor. @@ -32,11 +34,11 @@ class PcapInfo: non_udp_packets: int = 0 guessed_lidar_port: Optional[int] = None guessed_imu_port: Optional[int] = None - ports: Optional[Dict[int, Tuple[int, int]]] = None + ports: Dict[int, Tuple[int, int]] = field(default_factory=dict) -def _guess_imu_port(stream_data: _pcap.stream_info) -> int: - result = 0 +def _guess_imu_port(stream_data: _pcap.stream_info) -> Optional[int]: + result = None imu_size = 48 if (imu_size in stream_data.packet_size_to_port): correct_packet_size = stream_data.packet_size_to_port[imu_size] @@ -48,29 +50,29 @@ def _guess_imu_port(stream_data: _pcap.stream_info) -> int: return result -def _guess_lidar_port(stream_data: _pcap.stream_info) -> int: - result = 0 +def _guess_lidar_port(stream_data: _pcap.stream_info) -> Optional[int]: + result = None lidar_sizes = {3392, 6464, 12608, 24896} hit_count = 0 - + multiple_error_msg = "Error: Multiple possible lidar packets found" for s in lidar_sizes: if s in stream_data.packet_size_to_port: correct_packet_size = stream_data.packet_size_to_port[s] hit_count += 1 if (len(correct_packet_size) > 1): - raise ValueError( - "Error: Multiple possible lidar packets found") + raise ValueError(multiple_error_msg) else: result = list(correct_packet_size)[0] if (hit_count > 1): - raise ValueError("Error: Multiple possible lidar packets found") + raise ValueError(multiple_error_msg) return result -def _guess_ports(stream_data: _pcap.stream_info) -> Tuple[int, int]: +def _guess_ports( + stream_data: _pcap.stream_info) -> Tuple[Optional[int], Optional[int]]: """Guess the ports for lidar and imu streams from a stream_info struct. Args: @@ -138,9 +140,10 @@ def __iter__(self) -> Iterator[Packet]: while True: with self._lock: if self._handle is None: - break + raise ValueError("I/O operation on closed packet source") if not _pcap.next_packet_info(self._handle, packet_info): break + timestamp = packet_info.timestamp n = _pcap.read_packet(self._handle, buf) @@ -150,14 +153,14 @@ def __iter__(self) -> Iterator[Packet]: real_delta = time.monotonic() - real_start_ts pcap_delta = (packet_info.timestamp - pcap_start_ts) / self._rate - delta = max(0, pcap_delta.total_seconds() - real_delta) + delta = max(0, pcap_delta - real_delta) time.sleep(delta) if packet_info.dst_port == self._lidar_port and n != 0: - yield LidarPacket(buf[0:n], self._metadata) + yield LidarPacket(buf[0:n], self._metadata, timestamp) elif packet_info.dst_port == self._imu_port and n != 0: - yield ImuPacket(buf[0:n], self._metadata) + yield ImuPacket(buf[0:n], self._metadata, timestamp) @property def metadata(self) -> SensorInfo: @@ -231,12 +234,13 @@ def _replay(pcap_path: str, dst_ip: str, dst_lidar_port: int, """ pcap_info = _pcap.replay_get_pcap_info(pcap_path, 10000) - pcap_port_guess = _guess_ports(pcap_info) + lidar_port_guess, imu_port_guess = _guess_ports(pcap_info) - pcap_handle = _pcap.replay_initialize(pcap_path, dst_ip, dst_ip, { - pcap_port_guess[0]: dst_lidar_port, - pcap_port_guess[1]: dst_imu_port - }) + pcap_handle = _pcap.replay_initialize( + pcap_path, dst_ip, dst_ip, { + (lidar_port_guess or 0): dst_lidar_port, + (imu_port_guess or 0): dst_imu_port + }) try: packet_info = _pcap.packet_info() @@ -268,7 +272,8 @@ def record(packets: Iterable[Packet], Returns: Number of packets captured """ - + has_timestamp = None + error = False buf_size = 2**16 n = 0 handle = _pcap.record_initialize(pcap_path, src_ip, dst_ip, buf_size) @@ -282,9 +287,21 @@ def record(packets: Iterable[Packet], dst_port = imu_port else: raise ValueError("Unexpected packet type") - _pcap.record_packet(handle, src_port, dst_port, packet._data) + + if has_timestamp is None: + has_timestamp = (packet.capture_timestamp is not None) + elif has_timestamp != (packet.capture_timestamp is not None): + raise ValueError("Mixing timestamped/untimestamped packets") + + ts = packet.capture_timestamp or time.time() + _pcap.record_packet(handle, src_port, dst_port, packet._data, ts) n += 1 + except Exception: + error = True + raise finally: _pcap.record_uninitialize(handle) + if error and os.path.exists(pcap_path) and n == 0: + os.remove(pcap_path) return n diff --git a/python/src/ouster/sdk/examples/client.py b/python/src/ouster/sdk/examples/client.py index 6b4b1e77..5852e145 100644 --- a/python/src/ouster/sdk/examples/client.py +++ b/python/src/ouster/sdk/examples/client.py @@ -8,8 +8,8 @@ import argparse from contextlib import closing -import numpy as np from more_itertools import time_limited +import numpy as np from ouster import client @@ -136,8 +136,8 @@ def live_plot_signal(hostname: str, lidar_port: int = 7502) -> None: for scan in stream: # uncomment if you'd like to see frame id printed # print("frame id: {} ".format(scan.frame_id)) - signal = client.destagger( - stream.metadata, scan.field(client.ChanField.SIGNAL)) + signal = client.destagger(stream.metadata, + scan.field(client.ChanField.SIGNAL)) signal = (signal / np.max(signal) * 255).astype(np.uint8) cv2.imshow("scaled signal", signal) key = cv2.waitKey(1) & 0xFF diff --git a/python/src/ouster/sdk/examples/colormaps.py b/python/src/ouster/sdk/examples/colormaps.py new file mode 100644 index 00000000..841e02a6 --- /dev/null +++ b/python/src/ouster/sdk/examples/colormaps.py @@ -0,0 +1,303 @@ +"""Ouster ``spezia`` colormap.""" + +import numpy as np + + +def colorize(image: np.ndarray): + """Use Ouster spezia colormap to get from gray to color space. + + Args: + image: 2D array of values in the range [0, 1] + + Returns: + Array of RGB values of the same dimension selected from the color map + """ + key_img_indices = (255 * image).astype(np.uint8) + return np.reshape(np.take(spezia, key_img_indices.flat, axis=0), + [image.shape[0], image.shape[1], 3]) + + +def normalize(data: np.ndarray, percentile: float = 0.05): + """Normalize and clamp data for better color mapping. + + This is a utility function used ONLY for the purpose of 2D image + visualization. The resulting values are not fully reversible because the + final clipping step discards values outside of [0, 1]. + + Args: + data: array of data to be transformed for visualization + percentile: values in the bottom/top percentile are clambed to 0 and 1 + + Returns: + An array of doubles with the same shape as ``image`` with values + normalized to the range [0, 1]. + """ + min_val = np.percentile(data, 100 * percentile) + max_val = np.percentile(data, 100 * (1 - percentile)) + # to protect from division by zero + spread = max(max_val - min_val, 1) + field_res = (data.astype(np.float64) - min_val) / spread + return field_res.clip(0, 1.0) + + +# generated from: +# https://daniel.lawrence.lu/public/colortransform/#0_2423_964_352_6_2624_1000_513_11_3248_1000_617_15_415_1000_774 +# const int spezia_n = 256; +spezia = np.array( + # [0.04890922165917825, 0.34265700288230266, 0.5139042200196196], + [[0.0, 0.0, 0.0], + [0.04895672077739804, 0.34399228711079705, 0.5173325088859984], + [0.04899969158023907, 0.34532432182766976, 0.5207851330769154], + [0.049038068929181285, 0.34665300013643424, 0.5242624999557384], + [0.0490717860366443, 0.3479782119131098, 0.5277650273921529], + [0.04910077440233592, 0.34929984367863964, 0.5312931441090918], + [0.04912496374647964, 0.35061777846523556, 0.5348472900437968], + [0.049144281939685876, 0.35193189567631167, 0.5384279167237124], + [0.04915865492929047, 0.3532420709396423, 0.5420354876579142], + [0.04916800666192803, 0.3545481759533582, 0.5456704787448663], + [0.04917225900211732, 0.3558500783243678, 0.5493333786972924], + [0.04917133164659893, 0.35714764139876426, 0.553024689485032], + [0.0491651420341628, 0.35844072408375016, 0.5567449267967906], + [0.049153605250673076, 0.35972918066057785, 0.5604946205217287], + [0.04913663392897654, 0.36101286058797066, 0.5642743152519267], + [0.04911413814335756, 0.36229160829545354, 0.5680845708067875], + [0.04908602529819959, 0.36356526296598163, 0.5719259627805287], + [0.04905220001042406, 0.36483365830721187, 0.5757990831139734], + [0.04901256398533129, 0.36609662231071893, 0.5797045406919258], + [0.04896701588534969, 0.36735397699840217, 0.5836429619674972], + [0.04891545119124254, 0.36860553815528246, 0.5876149916148347], + [0.04885776205520153, 0.36985111504782353, 0.5916212932117864], + [0.048793837145294165, 0.371090510126853, 0.5956625499541581], + [0.048723561480604215, 0.37232351871408936, 0.5997394654032839], + [0.04864681625641982, 0.37354992867120285, 0.6038527642687842], + [0.0485634786587359, 0.37476952005026626, 0.6080031932284756], + [0.04847342166723854, 0.3759820647243526, 0.6121915217875443], + [0.04837651384597603, 0.37718732599695254, 0.6164185431792271], + [0.04827261912068898, 0.3783850581887729, 0.6206850753093874], + [0.04816159654185025, 0.37957500620037093, 0.6249919617475522], + [0.04804330003224206, 0.38075690504895116, 0.6293400727671268], + [0.047917578117875524, 0.3819304793775204, 0.633730306437712], + [0.04778427364089425, 0.38309544293445374, 0.6381635897726399], + [0.04764322345301101, 0.38425149802135766, 0.6426408799350484], + [0.04749425808786458, 0.385398334906948, 0.6471631655060938], + [0.04733720141054259, 0.3865356312044689, 0.6517314678190856], + [0.04717187024231324, 0.3876630512099673, 0.6563468423636755], + [0.046998073958454976, 0.38878024519851034, 0.6610103802644818], + [0.046815614056824016, 0.3898868486751851, 0.6657232098388559], + [0.04662428369457814, 0.3909824815774357, 0.6704864982388766], + [0.04642386719018477, 0.39206674742499825, 0.6753014531830023], + [0.04621413948754389, 0.39313923241335524, 0.6801693247832367], + [0.045994865578738504, 0.3941995044462622, 0.6850914074741193], + [0.04576579988147745, 0.39524711210249736, 0.6900690420503143], + [0.04552668556693947, 0.3962815835315315, 0.6951036178201221], + [0.04527725383318241, 0.39730242527232407, 0.7001965748827989], + [0.04501722311872807, 0.39830912098889804, 0.7053494065382041], + [0.04474629825033485, 0.39930113011574186, 0.7105636618379779], + [0.044464169518219306, 0.4002778864054065, 0.7158409482881979], + [0.044170511671191286, 0.4012387963699213, 0.7211829347142875], + [0.04386498282321687, 0.4021832376068135, 0.7265913542998228], + [0.04354722326188234, 0.4031105569995846, 0.7320680078119023], + [0.04321685414797862, 0.40402006878146585, 0.7376147670267773], + [0.0428734760940282, 0.40491105245010933, 0.743233578370643], + [0.042516667607970175, 0.40578275051957646, 0.748926466791789], + [0.04214598338630927, 0.4066343660945334, 0.7546955398817109], + [0.04176095243886018, 0.40746506024993384, 0.7605429922643745], + [0.04136107602475044, 0.40827394919762916, 0.766471110274553], + [0.04094582537627162, 0.4090601012192915, 0.7724822769480404], + [0.04051463918382638, 0.40982253334270374, 0.7785789773486957], + [0.040061502782456945, 0.4105602077358398, 0.7847638042595603], + [0.03959294089889664, 0.41127202779018696, 0.7910394642679004], + [0.039109793546916495, 0.4119568338613871, 0.7974087842769024], + [0.03861172210191932, 0.41261339863144436, 0.803874718479878], + [0.0380983735795864, 0.4132404220523802, 0.8104403558364525], + [0.03756937968562651, 0.4138365258262561, 0.8171089280940507], + [0.03702435578736771, 0.4144002473707861, 0.8238838184024792], + [0.0364628997996382, 0.4149300332132621, 0.8307685705742502], + [0.03588459097638143, 0.4154242317480496, 0.8377668990487521], + [0.035288988598694025, 0.4158810852842974, 0.844882699624589], + [0.03467563054866628, 0.4162987213006144, 0.8521200610312002], + [0.03404403175731731, 0.41667514281199364, 0.8594832774186676], + [0.033393682513460185, 0.41700821774098445, 0.8669768618532854], + [0.03272404661867004, 0.41729566716967786, 0.8746055609162682], + [0.032034559371859575, 0.4175350523310705, 0.8823743705140761], + [0.03132462536474723, 0.41772376017735885, 0.8902885530212784], + [0.03059361606719027, 0.417858987338036, 0.8983536558911435], + [0.029840867178669222, 0.41793772225168413, 0.9065755318852089], + [0.02906567571902483, 0.4179567252211435, 0.9149603610913213], + [0.028267296828018075, 0.41791250610119823, 0.9235146749206897], + [0.027444940239127507, 0.41780129927982523, 0.9322453822980893], + [0.026597766388240202, 0.4176190355565933, 0.9411597982868389], + [0.02572488211232861, 0.41736131045306674, 0.9502656754213602], + [0.02482533588680886, 0.41702334840740857, 0.9595712380560552], + [0.023898112542860842, 0.416599962205498, 0.9690852200808441], + [0.02294212739712791, 0.41608550687982504, 0.9788169064013666], + [0.02195621971619119, 0.4154738271597193, 0.9887761786374855], + [0.03533572637548167, 0.4150344767837667, 0.9966419438918287], + [0.08206748636661013, 0.4154760610454022, 0.996875442497312], + [0.1131664468320158, 0.4159292422424467, 0.9971067037505105], + [0.1377759789309851, 0.4163940123475041, 0.9973357493609963], + [0.1586260932452447, 0.4168703621191211, 0.9975626007042689], + [0.17695881259992585, 0.41735828111703227, 0.997787278826484], + [0.19346029551091778, 0.4178577577177723, 0.9980098044491156], + [0.2085556849234767, 0.4183687791306285, 0.9982301979735458], + [0.22252938052310162, 0.41889133141394447, 0.9984484794855942], + [0.2355824089832244, 0.4194253994917421, 0.9986646687599702], + [0.24786290560296725, 0.4199709671706614, 0.9988787852646682], + [0.25948364869956886, 0.42052801715720073, 0.9990908481652964], + [0.2705327829044692, 0.42109653107524325, 0.9993008763293371], + [0.2810807045979947, 0.4216764894838623, 0.9995088883303488], + [0.2911846624744039, 0.4222678718953844, 0.9997149024521047], + [0.30089193496804306, 0.4228706567937021, 0.9999189366926701], + [0.3199598560384707, 0.4211529467871777, 1.0000000000000044], + [0.3436114893370144, 0.4178742172053897, 1.0000000000000047], + [0.36539676089694495, 0.41458308629177515, 1.0000000000000044], + [0.3856661632570949, 0.41127775518053283, 1.0000000000000042], + [0.404675301565696, 0.407956362084171, 1.0000000000000044], + [0.4226172861700883, 0.4046169767859018, 1.0000000000000047], + [0.43964219386021874, 0.40125759469274436, 1.0000000000000047], + [0.45586938841351193, 0.3978761303980185, 1.0000000000000047], + [0.47139565849043324, 0.39447041069519134, 1.0000000000000047], + [0.4863007849418988, 0.3910381669772773, 1.0000000000000047], + [0.5006514638539757, 0.3875770269469873, 1.0000000000000044], + [0.5145041416968924, 0.3840845055522841, 1.0000000000000047], + [0.5279071095300848, 0.3805579950497078, 1.0000000000000047], + [0.5409020797263486, 0.3769947540834305, 1.0000000000000044], + [0.5535253932438766, 0.3733918956509583, 1.0000000000000044], + [0.5658089579546876, 0.3697463738064324, 1.0000000000000042], + [0.577780987780821, 0.366054968928604, 1.0000000000000049], + [0.589466591997403, 0.3623142713523205, 1.0000000000000047], + [0.6008882502481963, 0.35852066312849035, 1.0000000000000044], + [0.6120661992793963, 0.3546702976368881, 1.0000000000000047], + [0.6230187506929341, 0.35075907672718176, 1.0000000000000047], + [0.6337625542333337, 0.34678262500419443, 1.0000000000000047], + [0.6443128176539651, 0.3427362608011279, 1.0000000000000044], + [0.6546834916623888, 0.33861496329592544, 1.0000000000000047], + [0.664887426552217, 0.3344133351169368, 1.0000000000000044], + [0.6749365057066918, 0.3301255596489445, 1.0000000000000047], + [0.6848417600790246, 0.32574535208217403, 1.0000000000000047], + [0.6946134669261637, 0.32126590303548275, 1.0000000000000049], + [0.7042612354316643, 0.31667981331755896, 1.0000000000000047], + [0.7137940813531695, 0.3119790180493533, 1.0000000000000049], + [0.7232204924365964, 0.3071546979334297, 1.0000000000000049], + [0.7325484860275505, 0.30219717488892517, 1.0000000000000047], + [0.7417856600618409, 0.2970957885292609, 1.000000000000005], + [0.7509392384175178, 0.2918387489798506, 1.0000000000000047], + [0.760016111449703, 0.28641296022435003, 1.0000000000000047], + [0.7690228723986646, 0.2808038063993306, 1.0000000000000049], + [0.7779658502549104, 0.27499489103633235, 1.0000000000000049], + [0.7868511395774846, 0.2689677158905533, 1.0000000000000047], + [0.7956846276897148, 0.26270128126132847, 1.0000000000000047], + [0.804472019617065, 0.2561715829275765, 1.0000000000000047], + [0.8132188610824966, 0.2493509709254887, 1.0000000000000047], + [0.8219305598337341, 0.24220732066040862, 1.0000000000000049], + [0.8306124055427538, 0.23470294440057987, 1.0000000000000049], + [0.8392695884894237, 0.2267931361345682, 1.0000000000000047], + [0.847907217217596, 0.21842418639150069, 1.0000000000000047], + [0.8565303353323375, 0.20953060994411976, 1.0000000000000049], + [0.8651439375907393, 0.20003116767718654, 1.0000000000000049], + [0.8737529854254381, 0.18982297245453064, 1.0000000000000049], + [0.8823624220291222, 0.17877241522237444, 1.0000000000000047], + [0.8909771871196978, 0.1667005280966983, 1.0000000000000047], + [0.8996022314990386, 0.15335795616479617, 1.000000000000005], + [0.9082425315133318, 0.13837882372526109, 1.0000000000000049], + [0.9169031035195819, 0.12118667725012405, 1.0000000000000049], + [0.9255890184609986, 0.10077304980525353, 1.0000000000000047], + [0.9343054166534386, 0.07504334998300113, 1.0000000000000049], + [0.9430575228859241, 0.03781952178921804, 1.000000000000005], + [0.9509350420238839, 1.4218570765223148e-13, 0.9989984483716071], + [0.9554497353124459, 1.4191675612451605e-13, 0.9943640499109371], + [0.9599176427714787, 1.4433731987395504e-13, 0.9897799632511853], + [0.9643412154073002, 1.4245465917994694e-13, 0.9852425190239346], + [0.9687227616942858, 1.4191675612451605e-13, 0.9807481714229297], + [0.9730644583865243, 1.411995520506082e-13, 0.9762934885028384], + [0.9773683603724937, 1.3931689135660008e-13, 0.9718751430792824], + [0.9816364096714153, 1.3886863881040766e-13, 0.9674899041721569], + [0.9858704436584534, 1.4039269746746187e-13, 0.9631346289394122], + [0.9900722025959202, 1.4397871783700112e-13, 0.9588062550529955], + [0.9942433365389557, 1.4155815408756212e-13, 0.954501793472642], + [0.9983854116765075, 1.3752388117183045e-13, 0.9502183215767478], + [0.9999999999999819, 0.02804423714351181, 0.9437140548413381], + [0.9999999999999823, 0.0675265531658979, 0.9359017685954015], + [0.9999999999999826, 0.09447578037166751, 0.9282451825736049], + [0.9999999999999823, 0.11567880450339993, 0.920737795368809], + [0.9999999999999826, 0.13352190503381375, 0.9133734552831144], + [0.9999999999999823, 0.1491028314594674, 0.906146335428585], + [0.9999999999999826, 0.16303259275115084, 0.8990509109121838], + [0.9999999999999826, 0.17569199214531872, 0.8920819378992011], + [0.9999999999999826, 0.18733702217610845, 0.8852344343724449], + [0.9999999999999826, 0.19814940356609517, 0.8785036624245576], + [0.9999999999999823, 0.20826355122506324, 0.8718851119384158], + [0.9999999999999823, 0.21778214249596284, 0.8653744855260821], + [0.9999999999999826, 0.22678566871532468, 0.8589676846103573], + [0.9999999999999823, 0.2353385863611125, 0.8526607965450058], + [0.9999999999999828, 0.24349343831907827, 0.8464500826803465], + [0.9999999999999826, 0.2512937077092952, 0.840331967290248], + [0.9999999999999826, 0.2587758499993201, 0.8343030272849384], + [0.999999999999983, 0.26739099502162367, 0.8275538904243963], + [0.999999999999983, 0.2793555475103376, 0.8187524096848618], + [0.9999999999999828, 0.29067538241472596, 0.810154074771914], + [0.999999999999983, 0.3014349177286362, 0.8017491111724352], + [0.9999999999999826, 0.31170258039783083, 0.7935283442712853], + [0.9999999999999826, 0.3215347049761315, 0.7854831467895685], + [0.9999999999999826, 0.3309782925632311, 0.7776053911816436], + [0.9999999999999826, 0.3400730122474594, 0.7698874064041857], + [0.9999999999999826, 0.34885268450644075, 0.7623219385454285], + [0.999999999999983, 0.35734640143399626, 0.7549021148665397], + [0.9999999999999826, 0.3655793867737775, 0.7476214108616114], + [0.9999999999999826, 0.3735736659274856, 0.7404736199894286], + [0.9999999999999828, 0.381348594792351, 0.7334528257702123], + [0.9999999999999826, 0.38892128210540905, 0.7265533759748873], + [0.9999999999999823, 0.3963069303390571, 0.7197698586639263], + [0.9999999999999823, 0.4035191135203492, 0.7130970798581467], + [0.9999999999999823, 0.410570005644612, 0.7065300426455539], + [0.9999999999999821, 0.4174705699878856, 0.700063927546916], + [0.9999999999999819, 0.4242307171780247, 0.6936940739785828], + [0.9999999999999821, 0.4308594380852102, 0.6874159626644994], + [0.9999999999999821, 0.4373649162525338, 0.6812251988606219], + [0.9999999999999819, 0.44375462357781925, 0.6751174962642902], + [0.9999999999999819, 0.4500354021895003, 0.6690886614886871], + [0.9999999999999821, 0.45621353486890187, 0.6631345789884755], + [0.9999999999999817, 0.4622948059133914, 0.657251196327135], + [0.9999999999999817, 0.4682845539768576, 0.6514345096795133], + [0.9999999999999817, 0.474187718141824, 0.645680549464667], + [0.9999999999999817, 0.4800088782535285, 0.6399853660042518], + [0.9999999999999815, 0.4857522903672667, 0.6343450151004509], + [0.9999999999999815, 0.4914219180162633, 0.6287555434246979], + [0.9999999999999815, 0.497021459890778, 0.6232129736041581], + [0.9999999999999815, 0.5025543744242497, 0.6177132888869281], + [0.9999999999999815, 0.5080239017046412, 0.6122524172590773], + [0.999999999999981, 0.5134330830652836, 0.606826214876734], + [0.9999999999999808, 0.518784778656747, 0.6014304486641499], + [0.9999999999999808, 0.5240816832574693, 0.5960607779137368], + [0.9999999999999806, 0.5293263405443853, 0.5907127347060119], + [0.9999999999999806, 0.5345211560142691, 0.5853817029456958], + [0.9999999999999808, 0.5396684087209026, 0.580062895784249], + [0.9999999999999808, 0.5447702619716198, 0.5747513311680923], + [0.9999999999999806, 0.5498287731085955, 0.5694418052146554], + [0.9999999999999803, 0.5548459024848833, 0.5641288630740176], + [0.9999999999999801, 0.5598235217321937, 0.5588067668806895], + [0.9999999999999799, 0.5647634214064047, 0.5534694603362047], + [0.9999999999999799, 0.569667318087479, 0.5481105293861371], + [0.9999999999999801, 0.5745368610026079, 0.5427231583620321], + [0.9999999999999797, 0.5793736382348097, 0.5373000808456486], + [0.9999999999999797, 0.5841791825736894, 0.5318335243749407], + [0.9999999999999797, 0.58895497706055, 0.5263151479421893], + [0.9999999999999795, 0.5937024602763533, 0.5207359710263567], + [0.9999999999999795, 0.5984230314181602, 0.5150862926436902], + [0.9999999999999792, 0.6031180552074987, 0.5093555985787912], + [0.9999999999999792, 0.607788866672662, 0.5035324545546109], + [0.999999999999979, 0.6124367758461117, 0.4976043825895365], + [0.999999999999979, 0.6170630724180334, 0.4915577171399405], + [0.9999999999999788, 0.6216690303876014, 0.48537743679248463], + [0.9999999999999788, 0.6262559127547657, 0.4790469661903673], + [0.9999999999999784, 0.6308249762973255, 0.4725479414659382], + [0.9999999999999786, 0.6353774764808859, 0.46585993058805514], + [0.9999999999999784, 0.6399146725529954, 0.45896009754439654], + [0.9999999999999784, 0.644437832877538, 0.45182279591800384], + [0.9999999999999781, 0.6489482405714118, 0.4444190728188997], + [0.9999999999999779, 0.6534471995128909, 0.4367160577509657], + [0.9999999999999779, 0.6579360408000906, 0.4286762020035964], + [0.9999999999999779, 0.6624161297489367, 0.42025632127341656], + [0.9999999999999777, 0.6668888735333959, 0.41140637540952824], + [0.9999999999999777, 0.6713557295869282, 0.40206789113388525], + [0.9999999999999775, 0.6758182149038043, 0.3921718908087272]]) diff --git a/python/src/ouster/sdk/examples/open3d.py b/python/src/ouster/sdk/examples/open3d.py new file mode 100644 index 00000000..79675249 --- /dev/null +++ b/python/src/ouster/sdk/examples/open3d.py @@ -0,0 +1,287 @@ +"""Ouster Open3D visualizations examples.""" +import time + +from more_itertools import consume, nth +import numpy as np +import open3d as o3d # type: ignore + +from ouster import client +from .colormaps import normalize, colorize + +Z_NEAR = 1.0 + + +def view_from(vis: o3d.visualization.Visualizer, + from_point: np.ndarray, + to_point: np.ndarray = np.array([0, 0, 0])): + """Helper to setup view direction for Open3D Visualiser. + + Args: + from_point: camera location in 3d space as ``[x,y,z]`` + to_point: camera view direction in 3d space as ``[x,y,z]`` + """ + ctr = vis.get_view_control() + up_v = np.array([0, 0, 1]) + dir_v = to_point - from_point + left_v = np.cross(up_v, dir_v) + up = np.cross(dir_v, left_v) + ctr.set_lookat(to_point) + ctr.set_front(-dir_v) + ctr.set_up(up) + + +def create_canvas(w: int, h: int) -> o3d.geometry.TriangleMesh: + """Create canvas for 2D image. + + Args: + w: width of the 2D image in screen coords (pixels) + h: height of the 2D image in screen coords (pixels) + """ + pic = o3d.geometry.TriangleMesh() + pic.vertices = o3d.utility.Vector3dVector( + np.array([[0, 1, 1], [0, -1, 1], [0, -1, -1], [0, 1, -1]])) + pic.triangles = o3d.utility.Vector3iVector( + np.array([ + [0, 3, 2], + [2, 1, 0], + ])) + pic.triangle_uvs = o3d.utility.Vector2dVector( + np.array([[0.0, 0.0], [0.0, 1.0], [1.0, 1.0], [1.0, 1.0], [1.0, 0.0], + [0.0, 0.0]])) + im = np.zeros((h, w, 3), dtype=np.float32) + pic.textures = [o3d.geometry.Image(im)] + pic.triangle_material_ids = o3d.utility.IntVector( + np.array([0, 0], dtype=np.int32)) + return pic + + +def canvas_set_viewport( + pic: o3d.geometry.TriangleMesh, + camera_params: o3d.camera.PinholeCameraParameters) -> None: + """Set the position of the 2D image in space so it seems as static. + + The method should be called on every animation update (animation callback) + before rendering so the 2D mesh with texture always appear in the position + that would seem "static" for the observer of the scene through the current + camera parameters. + + Args: + pic: canvas with 2D image, created with :func:`.create_canvas` + camera_params: current open3d camera parameters + """ + + # calculate viewport + screen_width = camera_params.intrinsic.width + + pic_img_data = np.asarray(pic.textures[0]) + canvas_height, canvas_width, _ = pic_img_data.shape + + viewport = ((screen_width - canvas_width) / 2, 0, canvas_width, + canvas_height) + + # grab camera parameters + intrinsics = camera_params.intrinsic.intrinsic_matrix + extrinsics = camera_params.extrinsic + + x_pos, y_pos, width, height = viewport + pict_pos = np.array([[x_pos, y_pos], [x_pos + width, y_pos], + [x_pos + width, y_pos + height], + [x_pos, y_pos + height]]) + + # put canvas in correct camera view (img frame -> camera frame) + assert intrinsics[0, 0] == intrinsics[1, 1] + pict_pos = np.append((pict_pos - intrinsics[:2, 2]) / intrinsics[0, 0], + np.ones((pict_pos.shape[0], 1)), + axis=1) + pict_pos = pict_pos * (Z_NEAR + 0.001) + + # move canvas to world coords (camera frame -> world frame) + + # invert camera extrinsics: inv([R|T]) = [R'|-R'T] + tr = np.eye(extrinsics.shape[0]) + tr[:3, :3] = np.transpose(extrinsics[:3, :3]) + tr[:3, 3] = -np.transpose(extrinsics[:3, :3]) @ extrinsics[:3, 3] + + pict_pos = tr @ np.transpose( + np.append(pict_pos, np.ones((pict_pos.shape[0], 1)), axis=1)) + pict_pos = np.transpose(pict_pos / pict_pos[-1]) + + # set canvas position + pict_vertices = np.asarray(pic.vertices) + np.copyto(pict_vertices, pict_pos[:, :3]) + + +def canvas_set_image_data(pic: o3d.geometry.TriangleMesh, + img_data: np.ndarray) -> None: + """Set 2D image data to 2D canvas. + + Args: + pic: 2D canvas creates with :func:`.create_canvas` + img_data: image data RGB (i.e. shape ``[h, w, 3]``) + """ + pic_img_data = np.asarray(pic.textures[0]) + assert pic_img_data.shape == img_data.shape + np.copyto(pic_img_data, img_data) + + +def viewer_3d(scans: client.Scans, paused: bool = False) -> None: + """Render one scan in Open3D viewer from pcap file with 2d image. + + Args: + pcap_path: path to the pcap file + metadata_path: path to the .json with metadata (aka :class:`.SensorInfo`) + num: scan number in a given pcap file (satrs from *0*) + """ + + channels = [c for c in client.ChanField] + + # visualizer state + scans_iter = iter(scans) + scan = next(scans_iter) + metadata = scans.metadata + xyzlut = client.XYZLut(metadata) + + channel_ind = 2 + + def next_channel(vis): + nonlocal channel_ind + channel_ind = (channel_ind + 1) % len(channels) + update_data(vis) + print(f"Visualizing: {channels[channel_ind].name}") + + def toggle_pause(vis): + nonlocal paused + paused = not paused + print(f"Paused: {paused}") + + def right_arrow(vis, action, mods): + nonlocal scan + if action == 1: + print("Skipping forward 10 frames") + scan = nth(scans_iter, 10) + if scan is None: + raise StopIteration + update_data(vis) + + # create geometries + axes = o3d.geometry.TriangleMesh.create_coordinate_frame(0.2) + cloud = o3d.geometry.PointCloud() + image = create_canvas(metadata.format.columns_per_frame, + metadata.format.pixels_per_column) + + def update_data(vis: o3d.visualization.Visualizer): + xyz = xyzlut(scan) + key = scan.field(channels[channel_ind]) + + # apply colormap to field values + key_img = normalize(key) + color_img = colorize(key_img) + + # prepare point cloud for Open3d Visualiser + cloud.points = o3d.utility.Vector3dVector(xyz.reshape((-1, 3))) + cloud.colors = o3d.utility.Vector3dVector(color_img.reshape((-1, 3))) + + # prepare canvas for 2d image + gray_img = np.dstack([key_img] * 3) + canvas_set_image_data(image, client.destagger(metadata, gray_img)) + + # signal that point cloud and needs to be re-rendered + vis.update_geometry(cloud) + + # initialize vis + vis = o3d.visualization.VisualizerWithKeyCallback() + vis.create_window() + + ropt = vis.get_render_option() + ropt.point_size = 1.0 + ropt.background_color = np.asarray([0, 0, 0]) + ropt.light_on = False + + # populate point cloud before adding geometry to avoid camera issues + update_data(vis) + + vis.add_geometry(cloud) + vis.add_geometry(image) + vis.add_geometry(axes) + + # initialize camera settings + ctr = vis.get_view_control() + ctr.set_constant_z_near(Z_NEAR) + ctr.set_zoom(0.2) + view_from(vis, np.array([2, 1, 2]), np.array([0, 0, 0])) + + # register keys + vis.register_key_callback(ord(" "), toggle_pause) + vis.register_key_callback(ord("M"), next_channel) + vis.register_key_action_callback(262, right_arrow) + + # main loop + last_ts = 0.0 + while vis.poll_events(): + ts = time.monotonic() + + # update data at scan frequency to avoid blocking the rendering thread + if not paused and ts - last_ts >= 1 / metadata.mode.frequency: + scan = next(scans_iter) + update_data(vis) + last_ts = ts + + # always update 2d image to follow camera + canvas_set_viewport(image, ctr.convert_to_pinhole_camera_parameters()) + vis.update_geometry(image) + vis.update_renderer() + + vis.destroy_window() + + +def main() -> None: + import argparse + import os + import ouster.pcap as pcap + + descr = """Example visualizer using the open3d library. + + Visualize either pcap data (specified using --pcap) or a running sensor + (specified using --sensor). If no metadata file is specified, this will look + for a file with the same name as the pcap with the '.json' extension, or + query it directly from the sensor. + + Visualizing a running sensor requires the sensor to be configured and + sending lidar data to the default UDP port (7502) on the host machine. + """ + + parser = argparse.ArgumentParser(description=descr) + parser.add_argument('--pause', action='store_true', help='start paused') + parser.add_argument('--start', type=int, help='skip to frame number') + parser.add_argument('--meta', metavar='PATH', help='path to metadata json') + + required = parser.add_argument_group('one of the following is required') + group = required.add_mutually_exclusive_group(required=True) + group.add_argument('--sensor', metavar='HOST', help='sensor hostname') + group.add_argument('--pcap', metavar='PATH', help='path to pcap file') + + args = parser.parse_args() + + if args.sensor: + scans = client.Scans.stream(args.sensor, metadata=args.meta) + elif args.pcap: + pcap_path = args.pcap + metadata_path = args.meta or os.path.splitext(pcap_path)[0] + ".json" + + with open(metadata_path, 'r') as f: + metadata = client.SensorInfo(f.read()) + + source = pcap.Pcap(pcap_path, metadata) + scans = client.Scans(source) + consume(scans, args.start or 0) + + try: + viewer_3d(scans, paused=args.pause) + except (KeyboardInterrupt, StopIteration): + pass + finally: + scans.close() + + +if __name__ == "__main__": + main() diff --git a/python/src/ouster/sdk/examples/pcap.py b/python/src/ouster/sdk/examples/pcap.py index 2fb2b88f..74d64948 100644 --- a/python/src/ouster/sdk/examples/pcap.py +++ b/python/src/ouster/sdk/examples/pcap.py @@ -4,81 +4,30 @@ $ python -m ouster.sdk.examples.pcap -h """ - import os import argparse -import numpy as np from contextlib import closing -from more_itertools import nth - -import ouster.pcap as pcap -import ouster.client as client - - -def read_metadata(metadata_path: str) -> client.SensorInfo: - """Read metadata json file as :class:`.client.SensorInfo` object - - Args: - metadata_path: path to json file with sensor info - - Returns: - :class:`.SensorInfo`: object initialized from the give json file - """ - with open(metadata_path, 'r') as f: - return client.SensorInfo(f.read()) - - -def ae(field: np.ndarray, percentile: float = 0.05): - """Shift/Normalize for better color mapping (non reversible, only viz use - cases) +from itertools import islice +import time - WARNING: - It's a utility function used ONLY for the purpose of 2D images - visualization, resulting values are not fully reversible (because - of final clipping step that discards values outside of [0,1] range), - so you can't use this function in serious processing of Lidar Data - because of information loss. +import numpy as np +from more_itertools import nth - Args: - field: field values. Usually returned from channel field - :class:`.ChanField` of a scan (can be staggered or destaggered) - percentile: param to control the amount of high/low value outliers - removed from the *field*. - - Returns: - :class:`.ndarray`: Output array of the same shape as *field* but values - between [0..1] that produce better color ranges (tones and highlights) - when visualizing. - """ - min_val = np.percentile(field, 100 * percentile) - max_val = np.percentile(field, 100 * (1 - percentile)) - field_res = (field.astype(np.float64) - min_val) / (max_val - min_val) - return field_res.clip(0, 1.0) +from ouster import client, pcap +from .colormaps import normalize -def pcap_display_xyz_points(pcap_path: str, - metadata_path: str, +def pcap_display_xyz_points(source: client.PacketSource, + metadata: client.SensorInfo, num: int = 0) -> None: - """Display range from a specified scan number (*num*) as 3D points from - pcap file located at *pcap_path* - - Args: - pcap_path: path to the pcap file - metadata_path: path to the .json with metadata (aka :class:`.SensorInfo`) - num: scan number in a given pcap file (satrs from *0*) - """ + """Plot point cloud using matplotlib.""" import matplotlib.pyplot as plt # type: ignore # [doc-stag-pcap-plot-xyz-points] - metadata = read_metadata(metadata_path) - source = pcap.Pcap(pcap_path, metadata) - - # get single scan - scans = client.Scans(source) - scan = nth(scans, num) + scan = nth(client.Scans(source), num) if not scan: - print(f'ERROR: Scan # {num} in not present in pcap file: {pcap_path}') - return + print(f"ERROR: Scan # {num} in not present in pcap file") + exit(1) # set up figure plt.figure() @@ -97,67 +46,38 @@ def pcap_display_xyz_points(pcap_path: str, key = scan.field(client.ChanField.SIGNAL) [x, y, z] = [c.flatten() for c in np.dsplit(xyz, 3)] - ax.scatter(x, y, z, c=ae(key.flatten()), s=0.2) + ax.scatter(x, y, z, c=normalize(key.flatten()), s=0.2) plt.show() - # [doc-etag-pcap-plot-xyz-points] -def pcap_2d_viewer( - pcap_path: str, - metadata_path: str, - num: int = 0, # not used in this example - rate: float = 0.0) -> None: - """Simple sensor field visualization pipeline as 2D images from pcap file - (*pcap_path*) - - Args: - pcap_path: path to the pcap file - metadata_path: path to the .json with metadata (aka :class:`.SensorInfo`) - rate: read speed of packets from the pcap file (**1.0** - corresponds to - real-time by packets timestamp, **0.0** - as fast as it reads from - file without any delay) - - """ +def pcap_2d_viewer(source: client.PacketSource, + metadata: client.SensorInfo, + num: int = 0) -> None: + """Visualize channel fields in 2D using opencv.""" import cv2 # type: ignore # [doc-stag-pcap-display-live] - metadata = read_metadata(metadata_path) - - source = pcap.Pcap(pcap_path, metadata, rate=rate) - - with closing(source) as source: - scans = iter(client.Scans(source)) - - print("press ESC from visualization to exit") + print("press ESC from visualization to exit") - channels = [ - client.ChanField.RANGE, client.ChanField.SIGNAL, - client.ChanField.NEAR_IR, client.ChanField.REFLECTIVITY - ] + quit = False + paused = False + destagger = True + num = 0 + for scan in client.Scans(source): + print("frame id: {}, num = {}".format(scan.frame_id, num)) - paused = False - destagger = True - num = 0 - scan = next(scans, None) - while scan: - print("frame id: {}, num = {}".format(scan.frame_id, num)) - - fields_values = [scan.field(ch) for ch in channels] - - if destagger: - fields_values = [ - client.destagger(metadata, field_val) - for field_val in fields_values - ] - - fields_images = [ae(field_val) for field_val in fields_values] + fields = [scan.field(ch) for ch in client.ChanField] + if destagger: + fields = [client.destagger(metadata, f) for f in fields] - combined_images = np.vstack( - [np.pad(img, 2, constant_values=1.0) for img in fields_images]) + combined_images = np.vstack( + [np.pad(normalize(f), 2, constant_values=1.0) for f in fields]) - cv2.imshow("4 channels: ", combined_images) + cv2.imshow("4 channels: ", combined_images) + # handle keys presses + while True: key = cv2.waitKey(1) & 0xFF # 100 is d if key == 100: @@ -167,31 +87,26 @@ def pcap_2d_viewer( paused = not paused # 27 is ESC elif key == 27: - break + quit = True if not paused: - scan = next(scans, None) - num += 1 + break + time.sleep(0.1) - cv2.destroyAllWindows() + if quit: + break + num += 1 + + cv2.destroyAllWindows() # [doc-etag-pcap-display-live] def pcap_read_packets( - pcap_path: str, - metadata_path: str, + source: client.PacketSource, + metadata: client.SensorInfo, num: int = 0 # not used in this examples ) -> None: - """Basic read packets example from pcap file (*pcap_path*) - - Args: - pcap_path: path to the pcap file - metadata_path: path to the .json with metadata (aka :class:`.SensorInfo`) - """ - - metadata = read_metadata(metadata_path) - source = pcap.Pcap(pcap_path, metadata) - + """Basic read packets example from pcap file. """ # [doc-stag-pcap-read-packets] for packet in source: if isinstance(packet, client.LidarPacket): @@ -204,209 +119,219 @@ def pcap_read_packets( print(f' timestamps = {timestamps.shape}') print(f' ranges = {ranges.shape}') - if isinstance(packet, client.ImuPacket): + elif isinstance(packet, client.ImuPacket): # and access ImuPacket content print(f' acceleration = {packet.accel}') print(f' angular_velocity = {packet.angular_vel}') # [doc-etag-pcap-read-packets] -def pcap_show_one_scan(pcap_path: str, - metadata_path: str, +def pcap_show_one_scan(source: client.PacketSource, + metadata: client.SensorInfo, num: int = 0, destagger: bool = True) -> None: - """Show all 4 channels of one scan (*num*) form pcap file (*pcap_path*) - - Args: - pcap_path: path to the pcap file - metadata_path: path to the .json with metadata (aka :class:`.SensorInfo`) - num: scan number in a given pcap file (satrs from *0*) - """ + """Plot all channels of one scan in 2D using matplotlib.""" import matplotlib.pyplot as plt # type: ignore + scan = nth(client.Scans(source), num) + if not scan: + print(f"ERROR: Scan # {num} in not present in pcap file") + exit(1) + # [doc-stag-pcap-show-one] - metadata = read_metadata(metadata_path) + fig = plt.figure(constrained_layout=True) + axs = fig.subplots(len(client.ChanField), 1, sharey=True) - def prepare_field_image(scan, key, metadata, destagger=True): - f = ae(scan.field(key)) + for ax, field in zip(axs, client.ChanField): + img = normalize(scan.field(field)) if destagger: - return client.destagger(metadata, f) - return f - - show_fields = [('range', client.ChanField.RANGE), - ('signal', client.ChanField.SIGNAL), - ('near_ir', client.ChanField.NEAR_IR), - ('reflectivity', client.ChanField.REFLECTIVITY)] - - with closing(pcap.Pcap(pcap_path, metadata)) as source: - scan = nth(client.Scans(source), num) - if not scan: - return - - fields_images = [(sf[0], - prepare_field_image(scan, sf[1], source.metadata)) - for sf in show_fields] - - fig = plt.figure(constrained_layout=True) - - axs = fig.subplots(len(fields_images), 1, sharey=True) - - for ax, field in zip(axs, fields_images): - ax.set_title(field[0], fontdict={'fontsize': 10}) - ax.imshow(field[1], cmap='gray', resample=False) - ax.set_yticklabels([]) - ax.set_yticks([]) - ax.set_xticks([0, scan.w]) - plt.show() + img = client.destagger(metadata, img) + + ax.set_title(str(field), fontdict={'fontsize': 10}) + ax.imshow(img, cmap='gray', resample=False) + ax.set_yticklabels([]) + ax.set_yticks([]) + ax.set_xticks([0, scan.w]) + plt.show() # [doc-etag-pcap-show-one] -def pcap_to_csv(pcap_path: str, - metadata_path: str, +def pcap_to_csv(source: client.PacketSource, + metadata: client.SensorInfo, num: int = 0, csv_dir: str = ".", - csv_prefix: str = "pcap_out", + csv_base: str = "pcap_out", csv_ext: str = "csv") -> None: - """Write scans from pcap file (*pcap_path*) to plain csv files (one per - lidar scan). - - If the *csv_ext* ends in ``.gz``, the file is automatically saved in - compressed gzip format. :func:`.numpy.loadtxt` can be used to read gzipped - files transparently back to :class:`.numpy.ndarray`. + """Write scans from a pcap to csv files (one per lidar scan). - Number of saved lines per csv file is always [H x W], which corresponds - to a full 2D image representation of a lidar scan. + The number of saved lines per csv file is always H x W, which corresponds to + a full 2D image representation of a lidar scan. Each line in a csv file is: - RANGE (mm), SIGNAL, NEAR_IR, REFLECTIVITY, X (m), Y (m), Z (m) + TIMESTAMP, RANGE (mm), SIGNAL, NEAR_IR, REFLECTIVITY, X (mm), Y (mm), Z (mm) + + If ``csv_ext`` ends in ``.gz``, the file is automatically saved in + compressed gzip format. :func:`.numpy.loadtxt` can be used to read gzipped + files transparently back to :class:`.numpy.ndarray`. Args: pcap_path: path to the pcap file metadata_path: path to the .json with metadata (aka :class:`.SensorInfo`) num: number of scans to save from pcap to csv files csv_dir: path to the directory where csv files will be saved - csv_prefix: the filename prefix that will be appended with frame number - and *csv_ext* - csv_ext: file extension to use. If it ends with ``.gz`` the output is - gzip compressed - + csv_base: string to use as the base of the filename for pcap output + csv_ext: file extension to use, "csv" by default """ - from itertools import islice # ensure that base csv_dir exists if not os.path.exists(csv_dir): os.makedirs(csv_dir) - metadata = read_metadata(metadata_path) - source = pcap.Pcap(pcap_path, metadata) + field_names = 'TIMESTAMP (ns), RANGE (mm), SIGNAL, NEAR_IR, REFLECTIVITY, X (mm), Y (mm), Z (mm)' + field_fmts = ['%d', '%d', '%d', '%d', '%d', '%d', '%d', '%d'] # [doc-stag-pcap-to-csv] - field_names = 'RANGE (mm), SIGNAL, NEAR_IR, REFLECTIVITY, X (m), Y (m), Z (m)' - field_fmts = ['%d', '%d', '%d', '%d', '%.8f', '%.8f', '%.8f'] + # precompute xyzlut to save computation in a loop + xyzlut = client.XYZLut(metadata) - channels = [ - client.ChanField.RANGE, client.ChanField.SIGNAL, - client.ChanField.NEAR_IR, client.ChanField.REFLECTIVITY - ] + # create an iterator of LidarScans from pcap and bound it if num is specified + scans = iter(client.Scans(source)) + if num: + scans = islice(scans, num) - with closing(pcap.Pcap(pcap_path, metadata)) as source: + for idx, scan in enumerate(scans): - # precompute xyzlut to save computation in a loop - xyzlut = client.XYZLut(metadata) + # copy per-column timestamps for each channel + col_timestamps = scan.header(client.ColHeader.TIMESTAMP) + timestamps = np.tile(col_timestamps, (scan.h, 1)) - # create an iterator of LidarScans from pcap and bound it if num is specified - scans = iter(client.Scans(source)) - if num: - scans = islice(scans, num) + # grab channel data + fields_values = [scan.field(ch) for ch in client.ChanField] - for idx, scan in enumerate(scans): + # use integer mm to avoid loss of precision casting timestamps + xyz = (xyzlut(scan) * 1000).astype(np.int64) - fields_values = [scan.field(ch) for ch in channels] - xyz = xyzlut(scan) + # get all data as one H x W x 8 int64 array for savetxt() + frame = np.dstack((timestamps, *fields_values, xyz)) - # get lidar data as one frame of [H x W x 7], "fat" 2D image - frame = np.dstack((*fields_values, xyz)) - frame = client.destagger(metadata, frame) + # not necessary, but output points in "image" vs. staggered order + frame = client.destagger(metadata, frame) - csv_path = os.path.join(csv_dir, - f'{csv_prefix}_{idx:06d}.{csv_ext}') + # write csv out to file + csv_path = os.path.join(csv_dir, f'{csv_base}_{idx:06d}.{csv_ext}') + print(f'write frame #{idx}, to file: {csv_path}') - header = '\n'.join([ - f'pcap file: {pcap_path}', f'frame num: {idx}', - f'metadata file: {metadata_path}', field_names - ]) + header = '\n'.join([f'frame num: {idx}', field_names]) - print(f'write frame #{idx}, to file: {csv_path}') + np.savetxt(csv_path, + frame.reshape(-1, frame.shape[2]), + fmt=field_fmts, + delimiter=',', + header=header) + # [doc-etag-pcap-to-csv] - np.savetxt(csv_path, - np.reshape(frame, (-1, frame.shape[2])), - fmt=field_fmts, - delimiter=',', - header=header) - # [doc-etag-pcap-to-csv] +def pcap_3d_one_scan(source: client.PacketSource, + metadata: client.SensorInfo, + num: int = 0) -> None: + """Render one scan from a pcap file in the Open3D viewer. + Args: + pcap_path: path to the pcap file + metadata_path: path to the .json with metadata (aka :class:`.SensorInfo`) + num: scan number in a given pcap file (satrs from *0*) + """ + import open3d as o3d -def main(): + # get single scan by index + scan = nth(client.Scans(source), num) + + if not scan: + print(f"ERROR: Scan # {num} in not present in pcap file") + exit(1) + # [doc-stag-open3d-one-scan] + # compute point cloud using client.SensorInfo and client.LidarScan + xyz = client.XYZLut(metadata)(scan) + + # create point cloud and coordinate axes geometries + cloud = o3d.geometry.PointCloud( + o3d.utility.Vector3dVector(xyz.reshape((-1, 3)))) + axes = o3d.geometry.TriangleMesh.create_coordinate_frame(1.0) + # [doc-etag-open3d-one-scan] + + # initialize visualizer and rendering options + vis = o3d.visualization.Visualizer() + vis.create_window() + vis.add_geometry(cloud) + vis.add_geometry(axes) + ropt = vis.get_render_option() + ropt.point_size = 1.0 + ropt.background_color = np.asarray([0, 0, 0]) + + # initialize camera settings + ctr = vis.get_view_control() + ctr.set_zoom(0.1) + ctr.set_lookat([0, 0, 0]) + ctr.set_up([1, 0, 0]) + + # run visualizer main loop + print("Press Q or Excape to exit") + vis.run() + vis.destroy_window() + + +def main(): + """Pcap examples runner.""" examples = { "plot-xyz-points": pcap_display_xyz_points, "2d-viewer": pcap_2d_viewer, "read-packets": pcap_read_packets, "plot-one-scan": pcap_show_one_scan, - "pcap-to-csv": pcap_to_csv + "pcap-to-csv": pcap_to_csv, + "open3d-one-scan": pcap_3d_one_scan, } - description = "Ouster Python SDK Pcap examples. The EXAMPLE must be one of:\n " + str.join( + description = "Ouster Python SDK Pcap examples. The EXAMPLE must be one of:\n" + str.join( '\n ', examples.keys()) parser = argparse.ArgumentParser( description=description, formatter_class=argparse.RawTextHelpFormatter) - parser.add_argument('pcap_path', - type=str, - help='pcap file with Ouster data packets') - - parser.add_argument( - 'metadata_path', - type=str, - help='sensor info .json file (usually stored together with recordings)' - ) - + parser.add_argument('pcap_path', metavar='PCAP', help='path to pcap file') + parser.add_argument('metadata_path', + metavar='METADATA', + help='path to metadata json') parser.add_argument('example', metavar='EXAMPLE', - type=str, choices=examples.keys(), - help='Name of the example to run') - + help='name of the example to run') parser.add_argument('--scan-num', - dest='scan_num', type=int, - default=0, - help='LidarScan # to use', - required=False) - + default=1, + help='index of scan to use') args = parser.parse_args() - print(f'Check basic pcap file info: {args.pcap_path}') - pcap_info = pcap.info(args.pcap_path) - print('pcap_info = ', pcap_info) - try: example = examples[args.example] except KeyError: - print(f'No such example: {args.example}') + print(f"No such example: {args.example}") print(description) exit(1) - if not args.metadata_path: - print("WARNING: Can't do more without sensor info .json " - "file, please provide one with --info-json param") - return 1 + if not args.metadata_path or not os.path.exists(args.metadata_path): + print(f"Metadata file does not exist: {args.metadata_path}") + exit(1) print(f'example: {args.example}') - example(args.pcap_path, args.metadata_path, args.scan_num) # type: ignore + + with open(args.metadata_path, 'r') as f: + metadata = client.SensorInfo(f.read()) + source = pcap.Pcap(args.pcap_path, metadata) + + with closing(source): + example(source, metadata, args.scan_num) # type: ignore if __name__ == "__main__": diff --git a/python/tests/test_config.py b/python/tests/test_config.py index 2375f13a..86ec310c 100644 --- a/python/tests/test_config.py +++ b/python/tests/test_config.py @@ -114,6 +114,7 @@ def test_optional_config() -> None: assert config.operating_mode is None assert config.phase_lock_enable is None assert config.phase_lock_offset is None + assert config.signal_multiplier is None assert config.sync_pulse_out_pulse_width is None assert config.sync_pulse_out_frequency is None assert config.sync_pulse_in_polarity is None @@ -138,6 +139,7 @@ def test_write_config() -> None: config.operating_mode = client.OperatingMode.OPERATING_STANDBY config.phase_lock_enable = True config.phase_lock_offset = 180000 + config.signal_multiplier = 2 config.sync_pulse_out_pulse_width = 5 config.sync_pulse_out_frequency = 2 config.sync_pulse_in_polarity = client.Polarity.POLARITY_ACTIVE_HIGH @@ -167,6 +169,7 @@ def complete_config_string() -> str: "operating_mode": "NORMAL", "phase_lock_enable": false, "phase_lock_offset": 0, + "signal_multiplier": 2, "sync_pulse_in_polarity": "ACTIVE_HIGH", "sync_pulse_out_angle": 360, "sync_pulse_out_frequency": 1, @@ -194,6 +197,7 @@ def test_read_config(complete_config_string: str) -> None: assert config.operating_mode == client.OperatingMode.OPERATING_NORMAL assert config.phase_lock_enable is False assert config.phase_lock_offset == 0 + assert config.signal_multiplier == 2 assert config.sync_pulse_out_pulse_width == 10 assert config.sync_pulse_out_frequency == 1 assert config.sync_pulse_in_polarity == client.Polarity.POLARITY_ACTIVE_HIGH diff --git a/python/tests/test_core.py b/python/tests/test_core.py index 7dcb730c..37fc7d73 100644 --- a/python/tests/test_core.py +++ b/python/tests/test_core.py @@ -29,18 +29,26 @@ def meta(stream_digest: digest.StreamDigest): def test_sensor_init(meta: client.SensorInfo) -> None: """Initializing a data stream with metadata makes no network calls.""" - with closing(client.Sensor("os.invalid", metadata=meta)): + with closing(client.Sensor("os.invalid", 0, 0, metadata=meta)): pass def test_sensor_timeout(meta: client.SensorInfo) -> None: """Setting a zero timeout reliably raises an exception.""" - with closing(client.Sensor("os.invalid", metadata=meta, + with closing(client.Sensor("os.invalid", 0, 0, metadata=meta, timeout=0.0)) as source: with pytest.raises(client.ClientTimeout): next(iter(source)) +def test_sensor_closed(meta: client.SensorInfo) -> None: + """Check reading from a closed source raises an exception.""" + with closing(client.Sensor("os.invalid", 0, 0, metadata=meta)) as source: + source.close() + with pytest.raises(ValueError): + next(iter(source)) + + @pytest.mark.skipif(sys.platform == "win32", reason="winsock is OK with this; not sure why") def test_sensor_port_in_use(meta: client.SensorInfo) -> None: @@ -81,6 +89,15 @@ def test_scans_simple(packets: client.PacketSource) -> None: next(scans) +def test_scans_closed(meta: client.SensorInfo) -> None: + """Check reading from closed scans raises an exception.""" + with closing(client.Sensor("os.invalid", 0, 0, metadata=meta)) as source: + scans = client.Scans(source) + scans.close() + with pytest.raises(ValueError): + next(iter(scans)) + + def test_scans_meta(packets: client.PacketSource) -> None: """Sanity check metadata and column headers of a batched scan.""" scans = iter(client.Scans(packets)) diff --git a/python/tests/test_data.py b/python/tests/test_data.py index 74363ae6..be3d64ff 100644 --- a/python/tests/test_data.py +++ b/python/tests/test_data.py @@ -67,7 +67,6 @@ def test_imu_packet(info: client.SensorInfo) -> None: def test_lidar_packet(info: client.SensorInfo) -> None: pf = _client.PacketFormat.from_info(info) - """Test reading and writing values from empty packets.""" p = client.LidarPacket(bytes(pf.lidar_packet_size), info) w = pf.columns_per_packet @@ -191,3 +190,76 @@ def test_scan_to_native() -> None: ls._data[0, 0] = 42 native.data[:] = 1 assert ls._data[0, 0] == 42 + + +def test_scan_not_complete() -> None: + """Test that not all scans are considered complete.""" + ls = client.LidarScan(32, 1024) + + status = ls.header(client.ColHeader.STATUS) + assert not ls._complete() + + status[0] = 0x01 + assert not ls._complete() + assert not ls._complete((0, 0)) + + status[1:] = 0xFFFFFFFF + assert not ls._complete() + + status[:] = 0xFFFFFFFF + status[-1] = 0x01 + assert not ls._complete() + + # windows are inclusive but python slicing is not + status[:] = 0x00 + status[:10] = 0xFFFFFFFF + assert not ls._complete((0, 10)) + + status[:] = 0x00 + status[11:21] = 0xFFFFFFFF + assert not ls._complete((10, 20)) + + # window [i, i] + status[:] = 0x00 + status[0] = 0xFFFFFFFF + assert not ls._complete() + assert not ls._complete((0, 1)) + assert ls._complete((0, 0)) + + status[:] = 0x00 + status[128] = 0xFFFFFFFF + assert not ls._complete() + assert not ls._complete((127, 128)) + assert ls._complete((128, 128)) + + +@pytest.mark.parametrize("w, win_start, win_end", [ + (512, 0, 511), + (512, 1, 0), + (512, 256, 0), + (512, 256, 1), + (1024, 0, 1023), + (1024, 0, 512), + (1024, 0, 0), + (1024, 1023, 1023), + (1024, 1023, 0), + (1024, 1023, 1), + (2048, 0, 2047), + (2048, 1024, 512), + (2048, 1024, 0), + (2048, 1024, 1), + (2048, 511, 511), +]) +def test_scan_complete(w, win_start, win_end) -> None: + """Set the status headers to the specified window and check _complete().""" + ls = client.LidarScan(32, w) + + status = ls.header(client.ColHeader.STATUS) + + if win_start <= win_end: + status[win_start:win_end + 1] = 0xFFFFFFFF + else: + status[0:win_end + 1] = 0xFFFFFFFF + status[win_start:] = 0xFFFFFFFF + + assert ls._complete((win_start, win_end)) diff --git a/python/tests/test_pcap.py b/python/tests/test_pcap.py index ae45edc6..5e11c1d6 100644 --- a/python/tests/test_pcap.py +++ b/python/tests/test_pcap.py @@ -1,51 +1,144 @@ import os -import tempfile +from random import getrandbits, shuffle, random +from typing import Iterator +from itertools import chain, islice +from more_itertools import roundrobin import pytest +import time -import ouster.pcap._pcap as _pcap - - -def write_test_pcap(test_file, - total_complete_packets, - total_size, - frag_size, - port, - dst_port, - record=None): - '''Write bitpatterns of the specified size to a pcap file. - - Args: - test_file: The output file to record pcap to - total_complete_packets: The total number of completed packets to record - total_size: The number of bytes to record - frag_size: The fragmentation size in bytes - port: The port to record to - dst_port: The target port to record to - record: If we have a previous record object, dont recreate it - ''' - if record is None: - record = _pcap.record_initialize(test_file, "127.0.0.1", "127.0.0.1", - frag_size) - frag_size = min(frag_size, total_size) - frags = max(int(total_size / frag_size), 1) - result = [] - for i in range(0, total_complete_packets): - data_out = [] - for j in range(0, frags): - data_out.append(i) - data_out.append(j) - for k in range(0, (frag_size - 2)): - data_out.append(k % 255) - _pcap.record_packet(record, port, dst_port, bytearray(data_out)) - - result.extend(data_out) - result.extend([0 for x in range(len(result), (i + 1) * total_size)]) - - return (bytearray(result), record) - - -@pytest.mark.parametrize("lidar_packets, imu_packets", [ +from ouster import pcap +from ouster.pcap import _pcap +from ouster import client +from ouster.client import _client, _digest + +DATA_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__)), "data") + +NO_RANDOM_TIME = 0 +RANDOM_FLOAT = 1 + +# Seed the start timestamp +current_timestamp = time.time() + + +def random_lidar_packets(metadata, random_time=NO_RANDOM_TIME) -> Iterator[client.LidarPacket]: + global current_timestamp + + pf = _client.PacketFormat.from_info(metadata) + + timestamp = None + current_timestamp += random() * 4.0 + if random_time == RANDOM_FLOAT: + timestamp = current_timestamp + + while True: + buf = bytearray(getrandbits(8) for _ in range(pf.lidar_packet_size)) + yield client.LidarPacket(buf, metadata, timestamp) + + +def random_imu_packets(metadata, random_time=NO_RANDOM_TIME) -> Iterator[client.ImuPacket]: + global current_timestamp + + pf = _client.PacketFormat.from_info(metadata) + + timestamp = None + current_timestamp += random() * 4.0 + if random_time == RANDOM_FLOAT: + timestamp = current_timestamp + + while True: + buf = bytearray(getrandbits(8) for _ in range(pf.imu_packet_size)) + yield client.ImuPacket(buf, metadata, timestamp) + + +@pytest.fixture +def n_packets() -> int: + return 10 + + +@pytest.fixture +def metadata() -> client.SensorInfo: + digest_path = os.path.join(DATA_DIR, "os-992011000121_digest.json") + with open(digest_path, 'r') as f: + return _digest.StreamDigest.from_json(f.read()).meta + + +@pytest.fixture +def pcap_path(metadata, n_packets, tmpdir): + file_path = os.path.join(tmpdir, "pcap_test.pcap") + + packets = islice( + roundrobin(random_lidar_packets(metadata), + random_imu_packets(metadata)), n_packets) + + pcap.record(packets, file_path) + + yield file_path + + +@pytest.fixture +def pcap_obj(metadata, pcap_path): + pc = pcap.Pcap(pcap_path, metadata) + yield pc + pc.close() + + +@pytest.mark.parametrize('n_packets', [0]) +def test_pcap_info_empty(pcap_path) -> None: + """Test that querying an empty pcap returns an empty PcapInfo.""" + res = pcap.info(pcap_path) + assert res == pcap.PcapInfo() + + +@pytest.mark.parametrize('n_packets', [0]) +def test_pcap_read_empty(pcap_obj) -> None: + """Check that reading an empty pcap yields an empty list.""" + assert list(pcap_obj) == [] + + +@pytest.mark.parametrize('n_packets', [10]) +def test_pcap_read_10(pcap_obj) -> None: + """Check that reading a test pcap produces the right number of packets.""" + assert len(list(pcap_obj)) == 10 + + +@pytest.mark.parametrize('n_packets', [10]) +def test_pcap_info_10(pcap_path) -> None: + """Check that reading a test pcap produces the right number of packets.""" + res = pcap.info(pcap_path) + assert res.ipv6_packets == 0 + assert res.ipv4_packets == 10 + assert res.packets_processed == 10 + assert res.packets_reassembled == 10 + assert res.non_udp_packets == 0 + + # default ports + assert res.guessed_lidar_port == 7502 + assert res.guessed_imu_port == 7503 + + # roundrobin -> 5 of each + assert res.ports == {7502: (6464, 5), 7503: (48, 5)} + + +def test_pcap_reset(pcap_obj) -> None: + """Test that resetting a pcap after reading works.""" + packets1 = list(pcap_obj) + pcap_obj.reset() + packets2 = list(pcap_obj) + + bufs1 = [bytes(p._data) for p in packets1] + bufs2 = [bytes(p._data) for p in packets2] + assert bufs1 == bufs2 + + +def test_pcap_read_closed(pcap_obj) -> None: + """Check that reading from a closed pcap raises an error.""" + pcap_obj.close() + with pytest.raises(ValueError): + next(iter(pcap_obj)) + + +@pytest.mark.parametrize("n_lidar, n_imu", [ pytest.param(1, 0, id="one lidar"), pytest.param(20, 0, id="multi lidar"), pytest.param(0, 1, id="one imu"), @@ -53,52 +146,142 @@ def write_test_pcap(test_file, pytest.param(1, 1, id="one each"), pytest.param(20, 20, id="multi each"), ]) -def test_read_write_lidar_imu(lidar_packets, imu_packets): - lidar_size = 12608 - imu_size = 48 - frag_size = 1480 - lidar_src_port = 7000 - lidar_dst_port = 7001 - imu_src_port = 8000 - imu_dst_port = 8001 - src_address = "127.0.0.1" - dst_address = src_address - lidar_buf = bytearray(lidar_size) - imu_buf = bytearray(imu_size) - - tmp_dir = tempfile.mkdtemp() - file_path = os.path.join(tmp_dir, "pcap_test.pcap") +def test_read_write_lidar_imu(n_lidar, n_imu, metadata, tmpdir): + """Test that random packets read back from pcap are identical.""" + lidar_packets = islice(random_lidar_packets(metadata), n_lidar) + imu_packets = islice(random_imu_packets(metadata), n_imu) + in_packets = list(chain(lidar_packets, imu_packets)) + + shuffle(in_packets) + + file_path = os.path.join(tmpdir, "pcap_test.pcap") + + pcap.record(in_packets, file_path) + out_packets = list(pcap.Pcap(file_path, metadata)) + out_bufs = [bytes(p._data) for p in out_packets] + in_bufs = [bytes(p._data) for p in in_packets] + assert in_bufs == out_bufs + + +@pytest.mark.parametrize("mode", [ + pytest.param(RANDOM_FLOAT, id="random float timestamp"), +]) +def test_timestamp_float_read_write(mode, metadata, tmpdir): + lidar_packets = islice(random_lidar_packets(metadata, random_time=mode), 10) + imu_packets = islice(random_imu_packets(metadata, random_time=mode), 10) + in_packets = list(chain(lidar_packets, imu_packets)) + + file_path = os.path.join(tmpdir, "pcap_test.pcap") + + pcap.record(in_packets, file_path) + out_packets = list(pcap.Pcap(file_path, metadata)) + out_timestamps = [] + in_timestamps = [] + + out_timestamps = [p.capture_timestamp for p in out_packets] + in_timestamps = [p.capture_timestamp for p in in_packets] + + assert len(in_timestamps) == len(out_timestamps) + for i, o in zip(in_timestamps, out_timestamps): + # Make sure to deal with float rounding issues in the compare + assert i == pytest.approx(o, abs=1e-6) + + +def test_no_timestamp_read_write(metadata, tmpdir): + mode = NO_RANDOM_TIME + current_timestamp = time.time() + lidar_packets = islice(random_lidar_packets(metadata, random_time=mode), 10) + imu_packets = islice(random_imu_packets(metadata, random_time=mode), 10) + in_packets = list(chain(lidar_packets, imu_packets)) + + file_path = os.path.join(tmpdir, "pcap_test.pcap") + + pcap.record(in_packets, file_path) + out_packets = list(pcap.Pcap(file_path, metadata)) + out_timestamps = [] + in_timestamps = [] + + out_timestamps = [p.capture_timestamp for p in out_packets] + in_timestamps = [p.capture_timestamp for p in in_packets] + + assert len(in_timestamps) == len(out_timestamps) + for i, o in zip(in_timestamps, out_timestamps): + assert i is None + assert current_timestamp == pytest.approx(o, abs=5e-1) + + +@pytest.mark.parametrize("n_lidar_timestamp, n_lidar_no_timestamp, n_imu_timestamp, n_imu_no_timestamp", [ + pytest.param(10, 0, 10, 0, id="yes timestamps"), + pytest.param(0, 10, 0, 10, id="no timestamps"), + pytest.param(10, 10, 0, 0, id="mixed: lidar"), + pytest.param(0, 0, 10, 10, id="mixed: imu"), + pytest.param(10, 0, 0, 10, id="mixed: lidar ts, imu no ts"), + pytest.param(0, 10, 10, 10, id="mixed: lidar no ts, imu ts"), + pytest.param(10, 10, 10, 10, id="mixed: lidar ts, lidar no ts, imu ts, imu no ts"), +]) +def test_mixed_timestamp_write(n_lidar_timestamp, n_lidar_no_timestamp, n_imu_timestamp, + n_imu_no_timestamp, metadata, tmpdir): + + lidar_timestamp_packets = islice(random_lidar_packets(metadata, random_time=RANDOM_FLOAT), + n_lidar_timestamp) + lidar_no_timestamp_packets = islice(random_lidar_packets(metadata, random_time=NO_RANDOM_TIME), + n_lidar_no_timestamp) + imu_timestamp_packets = islice(random_imu_packets(metadata, random_time=RANDOM_FLOAT), + n_imu_timestamp) + imu_no_timestamp_packets = islice(random_imu_packets(metadata, random_time=NO_RANDOM_TIME), + n_imu_no_timestamp) + in_packets = list(chain(lidar_timestamp_packets, lidar_no_timestamp_packets, imu_timestamp_packets, + imu_no_timestamp_packets)) + + file_path = os.path.join(tmpdir, "pcap_test.pcap") + + yes_timestamps = n_lidar_timestamp + n_imu_timestamp + no_timestamps = n_lidar_no_timestamp + n_imu_no_timestamp + + if yes_timestamps > 0 and no_timestamps > 0: + with pytest.raises(ValueError): + pcap.record(in_packets, file_path) + else: + pcap.record(in_packets, file_path) + + +def test_write_nonsensical_packet_type(metadata, tmpdir): + file_path = os.path.join(tmpdir, "pcap_test.pcap") + + in_packets = [42] + with pytest.raises(ValueError): + pcap.record(in_packets, file_path) + + assert not os.path.exists(file_path), "Didn't clean up empty file" + +def test_lidar_guess_error(metadata, tmpdir): + packets = islice(random_lidar_packets(metadata), 2) + file_path = os.path.join(tmpdir, "pcap_test.pcap") + + buf_size = 2**16 + handle = _pcap.record_initialize(file_path, "127.0.0.1", "127.0.0.1", buf_size) try: - lidar_data, record = write_test_pcap(file_path, lidar_packets, - lidar_size, frag_size, - lidar_src_port, lidar_dst_port) - imu_data, record = write_test_pcap(file_path, - imu_packets, - imu_size, - frag_size, - imu_src_port, - imu_dst_port, - record=record) - _pcap.record_uninitialize(record) - lidar_result = bytearray() - imu_result = bytearray() - pcap_read = _pcap.replay_initialize(file_path, src_address, - dst_address, {}) - - info = _pcap.packet_info() - while _pcap.next_packet_info(pcap_read, info): - if info.dst_port == imu_dst_port: - _pcap.read_packet(pcap_read, imu_buf) - imu_result += imu_buf - if info.dst_port == lidar_dst_port: - _pcap.read_packet(pcap_read, lidar_buf) - lidar_result += lidar_buf - - assert lidar_data == lidar_result - assert imu_data == imu_result - - _pcap.replay_uninitialize(pcap_read) + _pcap.record_packet(handle, 7502, 7502, (next(packets))._data, 1) + _pcap.record_packet(handle, 7503, 7503, (next(packets))._data, 2) finally: - os.remove(file_path) - os.rmdir(tmp_dir) + _pcap.record_uninitialize(handle) + + with pytest.raises(ValueError): + pcap.Pcap(file_path, metadata) + + +def test_imu_guess_error(metadata, tmpdir): + packets = islice(random_imu_packets(metadata), 2) + file_path = os.path.join(tmpdir, "pcap_test.pcap") + + buf_size = 2**16 + handle = _pcap.record_initialize(file_path, "127.0.0.1", "127.0.0.1", buf_size) + try: + _pcap.record_packet(handle, 7502, 7502, (next(packets))._data, 1) + _pcap.record_packet(handle, 7503, 7503, (next(packets))._data, 2) + finally: + _pcap.record_uninitialize(handle) + + with pytest.raises(ValueError): + pcap.Pcap(file_path, metadata) diff --git a/python/tox.ini b/python/tox.ini index e2d6676f..38c1129b 100644 --- a/python/tox.ini +++ b/python/tox.ini @@ -21,7 +21,7 @@ description = installs ouster-sdk-python from wheels and runs tests passenv = WHEELS_DIR skip_install = true commands = - pip install --upgrade --pre -f {env:WHEELS_DIR} ouster-sdk + pip install --upgrade --pre -f {env:WHEELS_DIR} --no-index ouster-sdk pytest tests/ -o junit_suite_name="ouster-sdk-{env:ID}-{env:VERSION_ID}-{envname}" \ --junit-prefix="{env:ID}__{env:VERSION_ID}__{envname}" \ --junitxml="{env:ARTIFACT_DIR}/tox-tests/ouster-sdk-{env:ID}-{env:VERSION_ID}-{envname}.xml" @@ -65,4 +65,4 @@ ignore = [pydocstyle] # used by flake8-docstrings plugin in flake env -convention = google \ No newline at end of file +convention = google