Skip to content

Commit

Permalink
[DIRTY] commit while things aren't fully fixed for zach inspection
Browse files Browse the repository at this point in the history
  • Loading branch information
@alberthli
alberthli committed Jul 30, 2024
1 parent 1bcbb9d commit f11edc3
Show file tree
Hide file tree
Showing 3 changed files with 354 additions and 14 deletions.
329 changes: 315 additions & 14 deletions obelisk/cpp/zoo/sensing/zed2_sensors.h
View file
Original file line number Diff line number Diff line change
@@ -1,24 +1,325 @@
#ifndef OBELISK_ZED2_SENSORS_HPP
#define OBELISK_ZED2_SENSORS_HPP
#pragma once

#include <ament_index_cpp/get_package_share_directory.hpp>
#include <fstream>
#include <mutex>
#include <obelisk_sensor_msgs/msg/obk_image.hpp>
#include <rclcpp/callback_group.hpp>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <rclcpp/timer.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <set>
#include <sl/Camera.hpp>
#include <stdexcept>
#include <yaml-cpp/yaml.h>

class ObeliskZed2Sensors : public rclcpp::Node {
#include "obelisk_sensor.h"

class ObeliskZed2Sensors : public obelisk::ObeliskSensor {

public:
ObeliskZed2Sensors(const std::string& node_name) : Node(node_name) {
RCLCPP_INFO(this->get_logger(), "ObeliskZed2Sensors node created");
}
ObeliskZed2Sensors(const std::string& node_name) : rclcpp_lifecycle::LifecycleNode(node_name) {
// Declare parameters
this->declare_parameter("params_path_pkg", "");
this->declare_parameter("params_path", "");

// Get parameter values
std::string params_path_pkg = this->get_parameter("params_path_pkg").as_string();
std::string _params_path = this->get_parameter("params_path").as_string();

void dummy_yaml_action(const std::string& yaml_string) {
YAML::Node node = YAML::Load(yaml_string);
if (node["test_key"]) {
RCLCPP_INFO(this->get_logger(), "YAML test value: %s", node["test_key"].as<std::string>().c_str());
std::string params_path;
if (_params_path[0] == '/') {
params_path = _params_path;
} else if (params_path_pkg.empty()) {
std::string err_msg = "No package provided for ZED2 camera parameters!";
RCLCPP_ERROR(this->get_logger(), err_msg.c_str());
throw std::runtime_error(err_msg);
} else {
RCLCPP_INFO(this->get_logger(), "YAML test key not found");
std::string share_directory = ament_index_cpp::get_package_share_directory(params_path_pkg);
params_path = share_directory + "/" + _params_path;
}

this->set_camera_params(params_path);

// Register publisher for ZED2 cameras
pub_img_ = this->create_publisher<obelisk_sensor_msgs::msg::ObkImage>("pub_img", 10);
}
};

#endif // OBELISK_ZED2_SENSORS_HPP
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_configure(const rclcpp_lifecycle::State& prev_state) {
obelisk::ObeliskSensor::on_configure(prev_state);

// Initialize cameras
for (const auto& cam_param : cam_param_dicts_) {
int cam_index = cam_param.first;
const auto& cam_param_dict = cam_param.second;

// Initialize camera
sl::InitParameters init_params;
init_params.camera_resolution = resolution_;
init_params.camera_fps = fps_;
init_params.depth_mode = depth_ ? sl::DEPTH_MODE::ULTRA : sl::DEPTH_MODE::NONE;
init_params.coordinate_units = sl::UNIT::METER;
init_params.depth_minimum_distance = 0.3;
init_params.camera_image_flip = sl::FLIP_MODE::OFF;

// Set serial number
init_params.input.setFromSerialNumber(std::stoi(cam_param_dict.at("serial_number")));

// Open camera
sl::Camera camera;
sl::ERROR_CODE err = camera.open(init_params);
if (err != sl::ERROR_CODE::SUCCESS) {
RCLCPP_ERROR(this->get_logger(), "Failed to open camera with serial number %s: %s",
cam_param_dict.at("serial_number").c_str(), sl::toString(err).c_str());
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
}

RCLCPP_INFO(this->get_logger(), "Camera with serial number %s initialized!",
cam_param_dict.at("serial_number").c_str());

cams_[cam_index] = std::move(camera);
cam_polled_[cam_index] = false;
cam_images_[cam_index] = sl::Mat();

// Set runtime parameters
sl::RuntimeParameters rtp;
rtp.enable_depth = depth_;
cam_rt_params_[cam_index] = rtp;
}

// Create timers for each camera
for (const auto& cam_param : cam_param_dicts_) {
int cam_index = cam_param.first;

auto timer_callback = [this, cam_index]() { this->camera_callback(cam_index); };

double timer_period = 1.0 / static_cast<double>(fps_);
auto timer = this->create_wall_timer(std::chrono::duration<double>(timer_period), timer_callback);
registered_timers_[cam_index] = timer;
}

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

private:
// Private member variables
std::map<int, sl::Camera> cams_;
std::map<int, bool> cam_polled_;
std::map<int, sl::Mat> cam_images_;
std::map<int, sl::RuntimeParameters> cam_rt_params_;
std::map<int, std::vector<uint8_t>> frames_;
std::mutex lock_;

rclcpp::Publisher<obelisk_sensor_msgs::msg::ObkImage>::SharedPtr pub_img_;
sl::RESOLUTION resolution_;
int fps_;
bool depth_;
std::map<int, std::map<std::string, std::string>> cam_param_dicts_;

void set_camera_params(const std::string& params_path) {
// Check if the file has a .yaml or .yml extension
if (params_path.substr(params_path.find_last_of(".") + 1) != "yaml" &&
params_path.substr(params_path.find_last_of(".") + 1) != "yml") {
std::string err_msg = "Invalid parameters file: " + params_path +
"! ObeliskZed2Sensors expects a YAML file with extension .yaml or .yml.";
RCLCPP_ERROR(this->get_logger(), err_msg.c_str());
throw std::runtime_error(err_msg);
}

// Load the YAML file
YAML::Node config;
try {
config = YAML::LoadFile(params_path);
} catch (const YAML::Exception& e) {
std::string err_msg = "Could not load a configuration file at " + params_path + "! Error: " + e.what();
RCLCPP_ERROR(this->get_logger(), err_msg.c_str());
throw std::runtime_error(err_msg);
}

// Parse the camera parameters
for (const auto& cam : config) {
int cam_index = cam.first.as<int>();
const auto& cam_param_dict = cam.second;

// Check if cam_param_dict is a map
if (!cam_param_dict.IsMap()) {
std::string err_msg =
"Invalid camera parameters for camera " + std::to_string(cam_index) + " in " + params_path + "!";
RCLCPP_ERROR(this->get_logger(), err_msg.c_str());
throw std::runtime_error(err_msg);
}

// Check serial number
if (!cam_param_dict["serial_number"]) {
std::string err_msg =
"Serial number not provided for camera " + std::to_string(cam_index) + " in " + params_path + "!";
RCLCPP_ERROR(this->get_logger(), err_msg.c_str());
throw std::runtime_error(err_msg);
}
std::string serial_number = cam_param_dict["serial_number"].as<std::string>();
if (serial_number.length() != 8) {
std::string err_msg = "Invalid serial number for camera " + std::to_string(cam_index) + " in " +
params_path + "! Must be 8 digits.";
RCLCPP_ERROR(this->get_logger(), err_msg.c_str());
throw std::runtime_error(err_msg);
}

// Check resolution
std::string resolution =
cam_param_dict["resolution"] ? cam_param_dict["resolution"].as<std::string>() : "VGA";
std::transform(resolution.begin(), resolution.end(), resolution.begin(), ::tolower);
if (resolution == "vga") {
cam_param_dicts_[cam_index]["resolution"] = "VGA";
} else if (resolution == "720") {
cam_param_dicts_[cam_index]["resolution"] = "HD720";
} else if (resolution == "1080") {
cam_param_dicts_[cam_index]["resolution"] = "HD1080";
} else if (resolution == "2k") {
cam_param_dicts_[cam_index]["resolution"] = "HD2K";
} else {
std::string err_msg = "Invalid resolution for camera " + std::to_string(cam_index) + " in " +
params_path + "! Must be one of 'VGA', '720', '1080', '2K'.";
RCLCPP_ERROR(this->get_logger(), err_msg.c_str());
throw std::runtime_error(err_msg);
}

// Check fps
int fps = cam_param_dict["fps"] ? cam_param_dict["fps"].as<int>() : 0;
if (fps == 0) {
if (resolution == "vga")
fps = 100;
else if (resolution == "720")
fps = 60;
else if (resolution == "1080")
fps = 30;
else if (resolution == "2k")
fps = 15;
}
if (fps != 15 && fps != 30 && fps != 60 && fps != 100) {
std::string err_msg = "Invalid fps for camera " + std::to_string(cam_index) + " in " + params_path +
"! Must be one of 15, 30, 60, 100.";
RCLCPP_ERROR(this->get_logger(), err_msg.c_str());
throw std::runtime_error(err_msg);
}
cam_param_dicts_[cam_index]["fps"] = std::to_string(fps);

// Check depth
bool depth = cam_param_dict["depth"] ? cam_param_dict["depth"].as<bool>() : false;
cam_param_dicts_[cam_index]["depth"] = depth ? "true" : "false";

// Check side
std::string side = cam_param_dict["side"] ? cam_param_dict["side"].as<std::string>() : "left";
std::transform(side.begin(), side.end(), side.begin(), ::tolower);
if (side != "left" && side != "right") {
std::string err_msg = "Invalid side for camera " + std::to_string(cam_index) + " in " + params_path +
"! Must be one of 'left', 'right'.";
RCLCPP_ERROR(this->get_logger(), err_msg.c_str());
throw std::runtime_error(err_msg);
}
cam_param_dicts_[cam_index]["side"] = side;
}

// Check that all cameras have the same resolution, fps, and depth setting
std::set<std::string> resolutions, fpss, depths;
for (const auto& cam : cam_param_dicts_) {
resolutions.insert(cam.second.at("resolution"));
fpss.insert(cam.second.at("fps"));
depths.insert(cam.second.at("depth"));
}

if (resolutions.size() > 1) {
throw std::runtime_error("All cameras must have the same resolution!");
}
if (fpss.size() > 1) {
throw std::runtime_error("All cameras must have the same fps!");
}
if (depths.size() > 1) {
throw std::runtime_error("All cameras must have the same depth setting!");
}

resolution_ = sl::RESOLUTION::HD720; // Default value, you might want to set this based on the parsed resolution
fps_ = std::stoi(*fpss.begin());
depth_ = *depths.begin() == "true";

RCLCPP_INFO(this->get_logger(), "Camera parameters set successfully");
}

void camera_callback(int cam_index) {
std::lock_guard<std::mutex> lock(lock_);

sl::Camera& cam = cams_[cam_index];
sl::RuntimeParameters& rtps = cam_rt_params_[cam_index];
sl::Mat& cam_image = cam_images_[cam_index];
std::string side = cam_param_dicts_[cam_index]["side"];

if (cam.grab(rtps) == sl::ERROR_CODE::SUCCESS) {
if (depth_) {
if (side == "left") {
cam.retrieveImage(cam_image, sl::VIEW::DEPTH);
} else {
cam.retrieveImage(cam_image, sl::VIEW::DEPTH_RIGHT);
}
} else {
if (side == "left") {
cam.retrieveImage(cam_image, sl::VIEW::LEFT);
} else {
cam.retrieveImage(cam_image, sl::VIEW::RIGHT);
}
}

// Convert sl::Mat to std::vector<uint8_t>
size_t size = cam_image.getWidth() * cam_image.getHeight() * cam_image.getChannels();
frames_[cam_index].resize(size);
memcpy(frames_[cam_index].data(), cam_image.getPtr<sl::uchar1>(), size);

cam_polled_[cam_index] = true;
RCLCPP_INFO(this->get_logger(), "Polled camera %d!", cam_index);

check_and_publish_images();
}
}

void check_and_publish_images() {
if (std::all_of(cam_polled_.begin(), cam_polled_.end(), [](const auto& pair) { return pair.second; })) {
auto msg = std::make_unique<obelisk_sensor_msgs::msg::ObkImage>();
// Prepare the UInt8MultiArray
msg->y.layout.data_offset = 0;

// Calculate dimensions
int num_cameras = frames_.size();
int channels = depth_ ? 1 : 3;
std::array<int, 4> dims = {num_cameras, height_, width_, channels};

// Prepare dimension information
msg->y.layout.dim.resize(4);
int stride = 1;
for (int i = 3; i >= 0; --i) {
auto& dim = msg->y.layout.dim[i];
dim.label = (i == 0) ? "cameras" : (i == 1) ? "height" : (i == 2) ? "width" : "channels";
dim.size = dims[i];
dim.stride = stride;
stride *= dims[i];
}

// Combine images from all cameras into a single vector
std::vector<uint8_t> combined_data;
combined_data.reserve(num_cameras * height_ * width_ * channels);

for (const auto& frame : frames_) {
combined_data.insert(combined_data.end(), frame.second.begin(), frame.second.end());
}

// Assign the combined data to the message
msg->y.data = std::move(combined_data);

// Publish the message
pub_img_->publish(std::move(msg));

// Reset poll flags
for (auto& polled : cam_polled_) {
polled.second = false;
}
}
}
};
29 changes: 29 additions & 0 deletions obelisk_ws/src/cpp/obelisk_sensing_cpp/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,8 @@ endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)

# Get the Obelisk library
include(FetchContent)
Expand All @@ -18,6 +20,33 @@ FetchContent_Declare(
# Lets us Get Obelisk::Core and Obelisk::Zoo
FetchContent_MakeAvailable(Obelisk)

# zed2_sensors

# TODO(ahl): do I need all of this?
###############################################################################
option(LINK_SHARED_ZED "Link with the ZED SDK shared executable" ON)
find_package(ZED 4 REQUIRED)
find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED)
include_directories(${CUDA_INCLUDE_DIRS})
include_directories(${ZED_INCLUDE_DIRS})
link_directories(${ZED_LIBRARY_DIR})
link_directories(${CUDA_LIBRARY_DIRS})
###############################################################################

add_executable(zed2_sensor src/zed2_sensor.cpp)

# TODO(ahl): why do I need ZED_LIBRARIES here?
target_link_libraries(zed2_sensor
PUBLIC
Obelisk::Core
Obelisk::Zoo
${ZED_LIBRARIES})
ament_target_dependencies(zed2_sensor
PUBLIC
rclcpp
rclcpp_lifecycle)


if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
Expand Down
10 changes: 10 additions & 0 deletions obelisk_ws/src/cpp/obelisk_sensing_cpp/src/zed2_sensor.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
#include "rclcpp/rclcpp.hpp"

#include "obelisk_ros_utils.h"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "zed2_sensors.h"

int main(int argc, char* argv[]) {
obelisk::utils::SpinObelisk<ObeliskZed2Sensors, rclcpp::executors::MultiThreadedExecutor>(argc, argv,
"zed2_sensors");
}

0 comments on commit f11edc3

Please sign in to comment.