Skip to content

Commit

Permalink
Update for fw 1.14-beta.10. See changelog for details
Browse files Browse the repository at this point in the history
  • Loading branch information
Dima Garbuzov authored and dmitrig committed Dec 3, 2020
1 parent 0b44a59 commit eb3e122
Show file tree
Hide file tree
Showing 49 changed files with 5,509 additions and 3,384 deletions.
39 changes: 38 additions & 1 deletion CHANGELOG.md
Original file line number Diff line number Diff line change
@@ -1,6 +1,27 @@
# Changelog

## [1.14.0-beta4] - unreleased (5bb6fe1)
## [1.14.0-beta.10] - 2020-05-21
### Added
- preliminary support for Windows and Mac 10.15 for `ouster_viz` and
`ouster_client`

### Changed
- replaced VTK visualizer library with one based on GLFW
- renamed all instances of "OS1" including namespaces, headers, node and topic
names, to reflect support for other product lines
- updated all xyz point cloud calculations to take into account new
`lidar_origin_to_beam_origin` parameter reported by sensors
- client and `os_node` and `simple_viz` now avoid setting the lidar and timestamp
modes when connecting to a client unless values are explicitly specicified

### Fixed
- increase the UDP receive buffer size in the client to reduce chances of
dropping packets on platforms with low defaults
- `os_cloud_node` output now uses the updated point cloud calculation, taking
into account the lidar origin offset
- minor regression with destaggering in img_node output in previous beta

## [1.14.0-beta.4] - 2020-03-17
### Added
- support for gen2 hardware in client, visualizer, and ROS sample code
- support for updated "packed" lidar UDP data format for 16 and
Expand All @@ -12,6 +33,22 @@
### Changed
- use random ports for lidar and imu data by default when unspecified

## [1.13.0] - 2020-03-16
### Added
- post-processing to improve ambient image uniformity in visualizer
- make timestamp mode configurable via the client (PR #97)

### Changed
- turn on position-independent code by default to make using code in libraries
easier (PR #65)
- use random ports for lidar and imu data by default when unspecified

### Fixed
- prevent legacy tf prefix from making invalid frame names (PR #56)
- use `iterator_traits` to make `batch_to_iter` work with more types (PR #70)
- 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
### Added
- install directives for `ouster_ros` build (addresses #50)
Expand Down
10 changes: 5 additions & 5 deletions README.md
Original file line number Diff line number Diff line change
@@ -1,12 +1,12 @@
# OS1 Example Code
Sample code for connecting to and configuring the OS1, reading and visualizing
data, and interfacing with ROS.
# Ouster Example Code
Sample code for connecting to and configuring ouster sensors, reading and
visualizing data, and interfacing with ROS.

See the `README.md` in each subdirectory for details.

## Contents
* [ouster_client/](ouster_client/README.md) contains an example C++ client for the OS1 sensor
* [ouster_viz/](ouster_viz/README.md) contains a visualizer for the OS1 sensor
* [ouster_client/](ouster_client/README.md) contains an example C++ client for ouster sensors
* [ouster_viz/](ouster_viz/README.md) contains a basic visualizer ouster sensors
* [ouster_ros/](ouster_ros/README.md) contains example ROS nodes for publishing point cloud messages

## Sample Data
Expand Down
58 changes: 39 additions & 19 deletions ouster_client/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -4,22 +4,36 @@ cmake_minimum_required(VERSION 3.1.0)
project(ouster_client)

# ==== Requirements ====
SET(OUSTER_CLIENT_ADDITIONAL_LIBS "")
SET(OUSTER_CLIENT_ADDITIONAL_LIB_DIRS "")
IF (WIN32)
find_package(jsoncpp CONFIG REQUIRED)
SET(OUSTER_CLIENT_ADDITIONAL_LIBS
jsoncpp
ws2_32)
get_target_property(jsoncpp_INCLUDE_DIRS jsoncpp_lib INTERFACE_INCLUDE_DIRECTORIES)
SET(OUSTER_CLIENT_ADDITIONAL_LIB_DIRS ${_VCPKG_INSTALLED_DIR}/x64-windows-static/lib)
ELSE()
set(OUSTER_CLIENT_ADDITIONAL_LIBS "")
set(OUSTER_CLIENT_ADDITIONAL_LIB_DIRS "")

if(WIN32)
find_package(jsoncpp CONFIG REQUIRED)
string(FIND ${CMAKE_CXX_FLAGS} "/MT" IS_MT)
if(IS_MT EQUAL -1)
set(OUSTER_CLIENT_ADDITIONAL_LIBS jsoncpp_lib ws2_32)
else()
set(OUSTER_CLIENT_ADDITIONAL_LIBS jsoncpp ws2_32)
endif()

get_target_property(jsoncpp_INCLUDE_DIRS jsoncpp_lib
INTERFACE_INCLUDE_DIRECTORIES)
set(OUSTER_CLIENT_ADDITIONAL_LIB_DIRS
${_VCPKG_INSTALLED_DIR}/x64-windows-static/lib)
add_definitions(-D_USE_MATH_DEFINES)
elseif(APPLE)
find_package(PkgConfig REQUIRED)
pkg_check_modules(JSONCPP REQUIRED jsoncpp)
set(OUSTER_CLIENT_ADDITIONAL_INCLUDES ${JSONCPP_INCLUDEDIR})
set(OUSTER_CLIENT_ADDITIONAL_LIBS ${JSONCPP_LIBRARIES})
set(OUSTER_CLIENT_ADDITIONAL_LIB_DIRS ${JSONCPP_LIBDIR})
add_compile_options(-Wextra -Werror)
else()
find_package(PkgConfig REQUIRED)
pkg_check_modules(jsoncpp REQUIRED jsoncpp)
SET(OUSTER_CLIENT_ADDITIONAL_LIBS
jsoncpp)
set(OUSTER_CLIENT_ADDITIONAL_LIBS jsoncpp)
add_compile_options(-Wextra -Werror)
ENDIF()
endif()

find_package(Eigen3 REQUIRED)

Expand All @@ -28,13 +42,19 @@ set(CMAKE_CXX_STANDARD 11)
add_compile_options(-Wall)

# ==== Libraries ====
add_library(ouster_client STATIC src/os1.cpp src/os1_util.cpp src/compat.cpp)
add_library(ouster_client STATIC src/client.cpp src/types.cpp src/compat.cpp src/lidar_scan.cpp)
set_target_properties(ouster_client PROPERTIES POSITION_INDEPENDENT_CODE ON)
target_link_libraries(ouster_client PUBLIC ${OUSTER_CLIENT_ADDITIONAL_LIBS})
target_include_directories(ouster_client PUBLIC include ${OUSTER_CLIENT_ADDITIONAL_INCLUDES})
target_include_directories(ouster_client SYSTEM PUBLIC ${jsoncpp_INCLUDE_DIRS} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR})
IF (WIN32)
target_link_directories(ouster_client PUBLIC ${OUSTER_CLIENT_ADDITIONAL_LIB_DIRS})
ENDIF()
target_include_directories(ouster_client
PUBLIC include ${OUSTER_CLIENT_ADDITIONAL_INCLUDES})
target_include_directories(
ouster_client SYSTEM
PUBLIC ${jsoncpp_INCLUDE_DIRS} SYSTEM
PRIVATE ${EIGEN3_INCLUDE_DIR})
if(WIN32 OR APPLE)
target_link_directories(ouster_client PUBLIC
${OUSTER_CLIENT_ADDITIONAL_LIB_DIRS})
endif()
# ==== Executables ====
add_executable(ouster_client_example src/main.cpp)
target_link_libraries(ouster_client_example ouster_client)
Expand Down
27 changes: 13 additions & 14 deletions ouster_client/README.md
Original file line number Diff line number Diff line change
@@ -1,24 +1,23 @@
# OS1 Example Client
# Example Sensor Client

## Contents
* `ouster_client/` contains a simple C++ client for the OS1 sensor that
prints lidar data to the terminal
* `ouster_client/` contains a simple C++ client application that prints lidar
data to the terminal
* can be built both with and without ROS. See the instructions in
[ouster_ros/](../ouster_ros/README.md) for building in a ROS environment

## Building the Sample Client
* The sample client requires a compiler supporting C++11 or newer and
CMake
* Build with `cd /path/to/ouster_example/ouster_client && mkdir build
&& cd build && cmake .. && make` where `/path/to/ouster_example` is where you've cloned the repository
* The sample client requires a compiler supporting C++11 or newer and CMake
* Build with `cd /path/to/ouster_example/ouster_client && mkdir build && cd
build && cmake .. && make` where `/path/to/ouster_example` is where you've
cloned the repository

## Running the Sample Client
* Make sure the OS1 is connected to the network and has obtained a
DHCP lease. See section 3.1 in the accompanying
* Make sure the sensor is connected to the network and has obtained a DHCP
lease. See section 3.1 in the accompanying
[software user guide](https://www.ouster.io/downloads) for more details
* An executable called `ouster_client_example` is generated in the build
* An executable called `ouster_client_example` is generated in the build
directory on success
* Run `./ouster_client_example <os1_hostname> <udp_data_dest_ip>` where
`<os1_hostname>` is the hostname or IP address of the OS1 sensor,
and `<udp_data_dest_ip>` is the IP to which the sensor should send
lidar data
* Run `./ouster_client_example <sensor_hostname> <udp_data_dest_ip>` where
`<sensor_hostname>` is the hostname or IP address of the sensor, and
`<udp_data_dest_ip>` is the IP to which the sensor should send lidar data
99 changes: 99 additions & 0 deletions ouster_client/include/ouster/client.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,99 @@
/**
* @file
* @brief sample sensor client
*/

#pragma once

#include <cstdint>
#include <memory>
#include <string>

#include "ouster/types.h"
#include "ouster/version.h"

namespace ouster {
namespace sensor {

struct client;

enum client_state {
TIMEOUT = 0,
CLIENT_ERROR = 1,
LIDAR_DATA = 2,
IMU_DATA = 4,
EXIT = 8
};

/**
* Minimum supported version
*/
const util::version min_version = {1, 12, 0};

/**
* Listen for sensor data on the specified ports; do not configure the sensor
* @param lidar_port port on which the sensor will send lidar data
* @param imu_port port on which the sensor will send imu data
* @return pointer owning the resources associated with the connection
*/
std::shared_ptr<client> init_client(const std::string& hostname = "",
int lidar_port = 7502, int imu_port = 7503);

/**
* Connect to and configure the sensor and start listening for data
* @param hostname hostname or ip of the sensor
* @param udp_dest_host hostname or ip where the sensor should send data
* @param lidar_port port on which the sensor will send lidar data
* @param imu_port port on which the sensor will send imu data
* @param timeout_sec how long to wait for the sensor to initialize
* @return pointer owning the resources associated with the connection
*/
std::shared_ptr<client> init_client(const std::string& hostname,
const std::string& udp_dest_host,
lidar_mode mode = MODE_UNSPEC,
timestamp_mode ts_mode = TIME_FROM_UNSPEC,
int lidar_port = 0, int imu_port = 0,
int timeout_sec = 30);

/**
* Block for up to timeout_sec until either data is ready or an error occurs
* NOTE: will return immediately if LIDAR_DATA or IMU_DATA are set and not
* cleared by read_lidar_data() and read_imu_data() before the next call
* @param cli client returned by init_client associated with the connection
* @param timeout_sec seconds to block while waiting for data
* @return client_state s where (s & ERROR) is true if an error occured, (s &
* LIDAR_DATA) is true if lidar data is ready to read, and (s & IMU_DATA) is
* true if imu data is ready to read
*/
client_state poll_client(const client& cli, int timeout_sec = 1);

/**
* Read lidar data from the sensor. Will not block
* @param cli client returned by init_client associated with the connection
* @param buf buffer to which to write lidar data. Must be at least
* lidar_packet_bytes + 1 bytes
* @return true if a lidar packet was successfully read
*/
bool read_lidar_packet(const client& cli, uint8_t* buf,
const packet_format& pf);

/**
* Read imu data from the sensor. Will not block
* @param cli client returned by init_client associated with the connection
* @param buf buffer to which to write imu data. Must be at least
* imu_packet_bytes + 1 bytes
* @return true if an imu packet was successfully read
*/
bool read_imu_packet(const client& cli, uint8_t* buf, const packet_format& pf);

/**
* Get metadata text blob from the sensor. Attempt to fetch from the network if
* not already populated
* @param cli client returned by init_client associated with the connection
* @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);

} // namespace sensor
} // namespace ouster
28 changes: 13 additions & 15 deletions ouster_client/include/ouster/compat.h
Original file line number Diff line number Diff line change
@@ -1,9 +1,13 @@
/**
* @file
* @brief Compatibility with windows (unsupported)
*/

#pragma once

#include <string>
#include <vector>

#if defined _WIN32 // --------- Compiling on Windows ---------
#if defined _WIN32
#define WPCAP 1
// Try and limit the stuff windows brings in
#define WIN32_LEAN_AND_MEAN
Expand All @@ -15,13 +19,13 @@
#define VTK_FALLTHROUGH [[fallthrough]]

// Windows headers
#include <process.h>
#include <WS2tcpip.h>
#include <WinInet.h>
#include <intrin.h>
#include <process.h>
#include <winbase.h>
#include <winsock2.h>
#include <windows.h>
#include <WinInet.h>
#include <winsock2.h>

// ssize_t is available in vs
#ifdef _MSC_VER
Expand Down Expand Up @@ -58,30 +62,24 @@ struct udphdr {

#define ETHERTYPE_IP 0x0800

// Fix windows issue with M_PI
#define _USE_MATH_DEFINES // for C++
#include <math.h>
#include <cmath>

#define SET_SIN_ADDR(data, address) data.addr.sin_addr.S_un.S_addr = (address)
#else // --------- Compiling on Linux ---------
// Linux headers
#include <arpa/inet.h>
#include <fcntl.h>
#include <math.h>
#include <net/ethernet.h>
#include <netdb.h>
#include <netinet/ip.h>
#include <netinet/udp.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>
#include <net/ethernet.h>
#include <netinet/ip.h>
#include <netinet/udp.h>

// Define windows types for linux
typedef int SOCKET;
typedef fd_set FDSET;
#define SOCKET_ERROR -1
#define SET_SIN_ADDR(data, address) data.addr.sin_addr = { address }
#define SET_SIN_ADDR(data, address) data.addr.sin_addr = {address}
#endif // --------- End Platform Differentiation Block ---------

/**
Expand Down
Original file line number Diff line number Diff line change
@@ -1,11 +1,12 @@
#pragma once

#include <json/json.h>

#include "ouster/compat.h"
#include "stdio.h"

namespace ouster {
namespace OS1 {
namespace sensor {
struct client {
SOCKET lidar_fd;
SOCKET imu_fd;
Expand All @@ -16,5 +17,5 @@ struct client {
socket_close(imu_fd);
}
};
}
}
} // namespace sensor
} // namespace ouster
Loading

0 comments on commit eb3e122

Please sign in to comment.