Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add support for rev7 fw v3.0.1 ouster sensors #137

Closed
wants to merge 11 commits into from
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 3 additions & 2 deletions ouster_msgs/msg/Metadata.msg
Original file line number Diff line number Diff line change
Expand Up @@ -3,9 +3,10 @@ string lidar_mode
string timestamp_mode
float64[] beam_azimuth_angles
float64[] beam_altitude_angles
float64[] beam_to_lidar_transform
float64[] imu_to_sensor_transform
float64[] lidar_to_sensor_transform
string serial_no
string firmware_rev
int8 imu_port
int8 lidar_port
uint16 imu_port
uint16 lidar_port
5 changes: 5 additions & 0 deletions ros2_ouster/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,8 @@ find_package(pcl_conversions REQUIRED)
find_package(ouster_msgs REQUIRED)
find_package(PCL REQUIRED COMPONENTS common)
find_package(jsoncpp REQUIRED)
find_package(CURL REQUIRED)
find_package(tf2_eigen REQUIRED)

include_directories(
include
Expand All @@ -48,6 +50,7 @@ set(dependencies
ouster_msgs
tf2_geometry_msgs
pcl_conversions
tf2_eigen
)

add_library(${library_name} SHARED
Expand All @@ -60,6 +63,7 @@ add_library(${library_name} SHARED
src/client/lidar_scan.cpp
src/client/netcompat.cpp
src/client/types.cpp
src/client/parsing.cpp
)

ament_target_dependencies(${library_name}
Expand All @@ -69,6 +73,7 @@ ament_target_dependencies(${library_name}
target_link_libraries(${library_name}
jsoncpp_lib
tins
CURL::libcurl
${PCL_LIBRARIES}
)

Expand Down
90 changes: 60 additions & 30 deletions ros2_ouster/include/ros2_ouster/client/client.h
Original file line number Diff line number Diff line change
Expand Up @@ -17,13 +17,14 @@ namespace ouster {

struct client;

/** Returned by poll_client. */
enum client_state
{
TIMEOUT = 0,
CLIENT_ERROR = 1,
LIDAR_DATA = 2,
IMU_DATA = 4,
EXIT = 8
TIMEOUT = 0, ///< Client has timed out
CLIENT_ERROR = 1,///< Client has reported an error
LIDAR_DATA = 2, ///< New lidar data available
IMU_DATA = 4, ///< New IMU data available
EXIT = 8 ///< Client has exited
};

/** Minimum supported version. */
Expand Down Expand Up @@ -96,40 +97,69 @@ namespace ouster {
bool read_imu_packet(const client & cli, uint8_t * buf, const packet_format & pf);

/**
* Get metadata text blob from the sensor.
*
* Will 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 = 60);
* Get metadata text blob from the sensor.
*
* Will attempt to fetch from the network if not already populated.
*
* @throw runtime_error if the sensor is in ERROR state, the firmware version
* used to initialize the HTTP or TCP client is invalid, the metadata could
* not be retrieved from the sensor, or the response could not be parsed.
*
* @param[in] cli client returned by init_client associated with the connection.
* @param[in] timeout_sec how long to wait for the sensor to initialize.
* @param[in] legacy_format whether to use legacy format of metadata output.
*
* @return a text blob of metadata parseable into a sensor_info struct.
*/
std::string get_metadata(client& cli, int timeout_sec = 60,
bool legacy_format = false);

/**
* Get sensor config from the sensor
*
* Populates passed in config with the results of get_config
*
* @param hostname sensor hostname
* @param config sensor config to populate
* @param active whether to pull active or passive configs
* @return true if sensor config successfully populated
*/
* Get sensor config from the sensor.
*
* Populates passed in config with the results of get_config.
*
* @param[in] hostname sensor hostname.
* @param[out] config sensor config to populate.
* @param[in] active whether to pull active or passive configs.
*
* @return true if sensor config successfully populated.
*/
bool get_config(const std::string& hostname, sensor_config& config,
bool active = true);

/**
* Set sensor config on sensor
*
* @param hostname sensor hostname
* @param sensor config
* @param flags flags to pass in
* @return true if config params successfuly set on sensor
*/
* Set sensor config on sensor.
*
* @throw runtime_error on failure to communcate with the sensor.
* @throw invalid_argument when config parameters fail validation.
*
* @param[in] hostname sensor hostname.
* @param[in] config sensor config.
* @param[in] config_flags flags to pass in.
*
* @return true if config params successfuly set on sensor.
*/
bool set_config(const std::string& hostname, const sensor_config& config,
uint8_t config_flags = 0);

/**
* Return the port used to listen for lidar UDP data.
*
* @param[in] cli client returned by init_client associated with the connection.
*
* @return the port number.
*/
int get_lidar_port(client& cli);

/**
* Return the port used to listen for imu UDP data.
*
* @param[in] cli client returned by init_client associated with the connection.
*
* @return the port number.
*/
int get_imu_port(client& cli);

} // namespace sensor
} // namespace ouster
52 changes: 52 additions & 0 deletions ros2_ouster/include/ros2_ouster/client/client_factory.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
//
// Created by user on 23/05/23.
//

#ifndef ROS2_OUSTER__OS1__OS1_BASE_FACTORY_HPP_
#define ROS2_OUSTER__OS1__OS1_BASE_FACTORY_HPP_

#include "ros2_ouster/client/interfaces/client_interface.hpp"
#include "ros2_ouster/client/impl/http.hpp"
#include "ros2_ouster/client/impl/tcp.hpp"

namespace ouster
{
namespace sensor
{

/**
* Creates an instance of the SensorHttp interface.
*
* @param[in] hostname hostname of the sensor to communicate with.
*/
inline std::unique_ptr<util::ClientInterface>
util::ClientInterface::create(const std::string &hostname)
{
auto fw = firmware_version(hostname);

if (fw == ouster::util::invalid_version || fw.major < 2) {
throw std::runtime_error(
"SensorHttp:: create firmware version information unavailable or "
"not fully supported version. Please upgrade your sensor to FW "
"2.0 or later.");
}

if (fw.major == 2) {
switch (fw.minor) {
case 0:
// FW 2.0 doesn't work properly with http
return std::make_unique<impl::TcpImpl>(hostname);
// case 1:
// return std::make_unique<SensorHttpImp_2_1>(hostname);
// case 2:
// return std::make_unique<SensorHttpImp_2_2>(hostname);
}
}

return std::make_unique<impl::HttpImpl>(hostname);
}

}// namespace sensor
}// namespace ouster

#endif//ROS2_OUSTER_OS1_BASE_FACTORY_H
116 changes: 116 additions & 0 deletions ros2_ouster/include/ros2_ouster/client/impl/curl_client.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,116 @@
#ifndef ROS2_OUSTER__CLIENT__INTERFACES__CURL_CLIENT_HPP_
#define ROS2_OUSTER__CLIENT__INTERFACES__CURL_CLIENT_HPP_

#include <curl/curl.h>

#include <cstring>
#include <utility>

namespace ouster
{
namespace util
{

class CurlClient
{
public:
explicit CurlClient(std::string base_url_) : base_url(std::move(base_url_))
{
curl_global_init(CURL_GLOBAL_ALL);
curl_handle = curl_easy_init();
curl_easy_setopt(curl_handle, CURLOPT_WRITEFUNCTION,
&CurlClient::write_memory_callback);
curl_easy_setopt(curl_handle, CURLOPT_WRITEDATA, this);
}

~CurlClient()
{
curl_easy_cleanup(curl_handle);
curl_global_cleanup();
}

public:
std::string get(const std::string & url) const
{
auto full_url = url_combine(base_url, url);
return execute_get(full_url);
}

std::string encode(const std::string & str) const
{
auto curl_str_deleter = [](char * s) { curl_free(s); };
auto encoded_str = std::unique_ptr<char, decltype(curl_str_deleter)>(
curl_easy_escape(curl_handle, str.c_str(),
static_cast<int>(str.length())),
curl_str_deleter);
return {encoded_str.get()};
}

private:
static std::string url_combine(const std::string & url1,
const std::string & url2)
{
if (!url1.empty() && !url2.empty()) {
if (url1[url1.length() - 1] == '/' && url2[0] == '/') {
return url1 + url2.substr(1);
}
if (url1[url1.length() - 1] != '/' && url2[0] != '/') {
return url1 + '/' + url2;
}
}

return url1 + url2;
}

std::string execute_get(const std::string & url) const
{
curl_easy_setopt(curl_handle, CURLOPT_URL, url.c_str());
curl_easy_setopt(curl_handle, CURLOPT_HTTPGET, 1L);
buffer.clear();
auto res = curl_easy_perform(curl_handle);
if (res == CURLE_SEND_ERROR) {
// Specific versions of curl doesn't play well with the sensor http
// server. When CURLE_SEND_ERROR happens for the first time silently
// re-attempting the http request resolves the problem.
res = curl_easy_perform(curl_handle);
}
if (res != CURLE_OK) {
throw std::runtime_error(
"CurlClient::execute_get failed for the url: [" + url +
"] with the error message: " + curl_easy_strerror(res));
}
long http_code = 0;
curl_easy_getinfo(curl_handle, CURLINFO_RESPONSE_CODE, &http_code);
if (http_code != 200) {
throw std::runtime_error(
std::string("CurlClient::execute_get failed for url: [" + url +
"] with the code: [") +
std::to_string(http_code) + std::string("] - and return: ") +
buffer);
}
return buffer;
}

static size_t write_memory_callback(void * contents, size_t element_size,
size_t elements_count,
void * user_pointer)
{
size_t size_increment = element_size * elements_count;
auto cc = static_cast<CurlClient *>(user_pointer);
auto size_current = cc->buffer.size();
cc->buffer.resize(size_current + size_increment);
memcpy((void *) &cc->buffer[size_current], contents, size_increment);
size_current += size_increment;
return size_increment;
}

private:
CURL * curl_handle;
mutable std::string buffer;
std::string base_url;
};

}// namespace util
}// namespace ouster

#endif//ROS2_OUSTER__CLIENT__INTERFACES__CURL_CLIENT_HPP_
Loading