diff --git a/.gitmodules b/.gitmodules index d025b6b..50965c3 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,3 @@ [submodule "rmw_zenoh"] path = rmw_zenoh url = git@github.com:ros2/rmw_zenoh.git -[submodule "ouster-ros"] - path = ouster-ros - url = git@github.com:ouster-lidar/ouster-ros.git diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..56cf85c --- /dev/null +++ b/Dockerfile @@ -0,0 +1,42 @@ +FROM ros:jazzy-ros-base + +ENV ROS_WS=/ros2 +WORKDIR $ROS_WS + +COPY rmw_zenoh src/rmw_zenoh +COPY meta src/meta + +RUN apt-get update &&\ + rosdep update &&\ + rosdep install --from-paths src --ignore-src -y &&\ + rm -rf /var/lib/apt/lists/* + +SHELL ["/bin/bash", "-c"] + +RUN source /opt/ros/$ROS_DISTRO/setup.bash &&\ + colcon build \ + --event-handlers console_direct+ \ + --parallel-workers $(nproc) \ + --cmake-args -DCMAKE_BUILD_TYPE=Release + +ARG INSTALL_DESKTOP=false + +RUN if [ "$INSTALL_DESKTOP" = "true" ]; then \ + apt-get update &&\ + apt-get install -y ros-${ROS_DISTRO}-desktop &&\ + rm -rf /var/lib/apt/lists/*; \ + fi + +COPY ouster_simulator src/ouster_simulator + +RUN source /opt/ros/$ROS_DISTRO/setup.bash &&\ + colcon build \ + --event-handlers console_direct+ \ + --parallel-workers $(nproc) \ + --cmake-args -DCMAKE_BUILD_TYPE=Release + +ENV ROS_DOMAIN_ID=10 +ENV RMW_IMPLEMENTATION=rmw_zenoh_cpp +COPY entrypoint.sh . +ENTRYPOINT ["./entrypoint.sh"] + diff --git a/Dockerfile.jazzy.ouster b/Dockerfile.jazzy.ouster deleted file mode 100644 index bfe0a3a..0000000 --- a/Dockerfile.jazzy.ouster +++ /dev/null @@ -1,21 +0,0 @@ -FROM jazzy:zenoh - -ENV RMW_IMPLEMENTATION=rmw_zenoh_cpp -WORKDIR $ROS_WS - -COPY ouster-ros src/ouster-ros - -RUN apt-get update &&\ - rosdep update &&\ - rosdep install --from-paths src --ignore-src -y &&\ - rm -rf /var/lib/apt/lists/* - -SHELL ["/bin/bash", "-c"] -RUN source install/setup.bash &&\ - colcon build \ - --event-handlers console_direct+ \ - --parallel-workers $(nproc) \ - --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_VIZ=OFF -DBUILD_OSF=OFF -DBUILD_EXAMPLES=OFF - -COPY entrypoint.ouster.sh entrypoint.sh -ENTRYPOINT ["./entrypoint.sh"] diff --git a/Dockerfile.jazzy.zenoh b/Dockerfile.jazzy.zenoh deleted file mode 100644 index 878c259..0000000 --- a/Dockerfile.jazzy.zenoh +++ /dev/null @@ -1,23 +0,0 @@ -FROM ros:jazzy-ros-base - -ENV ROS_WS=/ros2 -WORKDIR $ROS_WS - -COPY rmw_zenoh src/rmw_zenoh -COPY meta src/meta - -RUN apt-get update &&\ - rosdep update &&\ - rosdep install --from-paths src --ignore-src -y &&\ - rm -rf /var/lib/apt/lists/* - -SHELL ["/bin/bash", "-c"] -RUN source /opt/ros/$ROS_DISTRO/setup.bash &&\ - colcon build \ - --event-handlers console_direct+ \ - --parallel-workers $(nproc) \ - --cmake-args -DCMAKE_BUILD_TYPE=Release - -COPY entrypoint.ros2.sh entrypoint.sh -ENTRYPOINT ["./entrypoint.sh"] - diff --git a/entrypoint.ouster.sh b/entrypoint.ouster.sh deleted file mode 100755 index f3277c8..0000000 --- a/entrypoint.ouster.sh +++ /dev/null @@ -1,64 +0,0 @@ -#!/bin/bash -set -e - -# setup ros2 environment -source "/opt/ros/${ROS_DISTRO}/setup.bash" -[[ -f "${ROS_WS}/install/setup.bash" ]] && source "${ROS_WS}/install/setup.bash" - -# ROS information -echo 'ROS distro: ' $ROS_DISTRO - -if [ -n "$RMW_IMPLEMENTATION" ]; then - echo 'DDS middleware: ' $RMW_IMPLEMENTATION; -fi - -if [ -n "$ROS_DOMAIN_ID" ]; then - echo 'ROS Domain ID: ' $ROS_DOMAIN_ID; -fi - -echo "---------------------" -echo "Checking network settings" - -# Define the expected values -EXPECTED_RMEM_MAX=2147483647 -EXPECTED_IPFRAG_TIME=3 -EXPECTED_IPFRAG_HIGH_THRESH=1342177280 - -# Initialize a flag to track if any parameter is incorrectly set -ALL_VALID=true - -# Function to check and report parameter -check_parameter() { - PARAM_NAME=$1 - EXPECTED_VALUE=$2 - CURRENT_VALUE=$(sysctl -n $PARAM_NAME) - - if [ "$CURRENT_VALUE" -eq "$EXPECTED_VALUE" ]; then - echo "✔ $PARAM_NAME = $EXPECTED_VALUE" - else - echo "✘ $PARAM_NAME is incorrectly set to $CURRENT_VALUE (expected $EXPECTED_VALUE)" - echo "To set it correctly, run:" - echo " sudo sysctl -w $PARAM_NAME=$EXPECTED_VALUE" - echo "To make this change permanent, add the following line to /etc/sysctl.conf or a file in /etc/sysctl.d/:" - echo " $PARAM_NAME=$EXPECTED_VALUE" - echo - ALL_VALID=false - fi -} - -# Check each parameter -check_parameter net.core.rmem_max $EXPECTED_RMEM_MAX -check_parameter net.ipv4.ipfrag_time $EXPECTED_IPFRAG_TIME -check_parameter net.ipv4.ipfrag_high_thresh $EXPECTED_IPFRAG_HIGH_THRESH - -# Exit with an error code if any parameter is incorrect -if [ "$ALL_VALID" = false ]; then - echo "Error: One or more network parameters are incorrectly set." - exit 1 -fi - -echo "---------------------" - - -exec "$@" - diff --git a/entrypoint.ros2.sh b/entrypoint.sh similarity index 100% rename from entrypoint.ros2.sh rename to entrypoint.sh diff --git a/meta/package.xml b/meta/package.xml index 485e3c3..441d16d 100644 --- a/meta/package.xml +++ b/meta/package.xml @@ -11,7 +11,6 @@ ament_cmake demo_nodes_cpp - desktop foxglove_bridge diff --git a/ouster-ros b/ouster-ros deleted file mode 160000 index e4dd8bf..0000000 --- a/ouster-ros +++ /dev/null @@ -1 +0,0 @@ -Subproject commit e4dd8bf4bb1d66ec6272344cf0582d0c3f9d340e diff --git a/ouster_simulator/CMakeLists.txt b/ouster_simulator/CMakeLists.txt new file mode 100644 index 0000000..5ad36e1 --- /dev/null +++ b/ouster_simulator/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.8) +project(ouster_simulator) + +set(CMAKE_CXX_STANDARD 17) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) + +add_executable(ouster_publisher_node src/ouster_publisher_node.cpp) +ament_target_dependencies( + ouster_publisher_node + rclcpp + sensor_msgs +) + +add_executable(ouster_subscriber_node src/ouster_subscriber_node.cpp) +ament_target_dependencies( + ouster_subscriber_node + rclcpp + sensor_msgs +) + +install( + TARGETS ouster_publisher_node ouster_subscriber_node + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() + diff --git a/ouster_simulator/package.xml b/ouster_simulator/package.xml new file mode 100644 index 0000000..3ae6d41 --- /dev/null +++ b/ouster_simulator/package.xml @@ -0,0 +1,17 @@ + + + ouster_simulator + 0.0.1 + Example package that publishes/subscibes pointclouds in ouster format + Your Name + MIT + ament_cmake + rclcpp + sensor_msgs + rclcpp + sensor_msgs + + + ament_cmake + + diff --git a/ouster_simulator/src/ouster_publisher_node.cpp b/ouster_simulator/src/ouster_publisher_node.cpp new file mode 100644 index 0000000..19d1f95 --- /dev/null +++ b/ouster_simulator/src/ouster_publisher_node.cpp @@ -0,0 +1,198 @@ +#include +#include +#include +#include +#include + +// ROS 2 +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/msg/point_field.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" + +using namespace std::chrono_literals; + +/** + * @brief A ROS 2 node that publishes a 128×1024 point cloud using + * a custom layout matching the 'ouster_ros::Point' fields: + * + * x (float), y (float), z (float), + * intensity (float), + * t (uint32_t), + * reflectivity (uint16_t), + * ring (uint16_t), + * ambient (uint16_t), + * range (uint32_t). + * + * see https://github.com/ouster-lidar/ouster-ros/blob/ros2/ouster-ros/include/ouster_ros/os_point.h + */ +class OusterPublisher : public rclcpp::Node +{ +public: + OusterPublisher() + : Node("ouster_publisher") + { + publisher_ = this->create_publisher( + "/data", 10); + + timer_ = this->create_wall_timer( + 50ms, std::bind(&OusterPublisher::onTimer, this)); + + RCLCPP_INFO(this->get_logger(), "OusterPublisher node started."); + } + +private: + void onTimer() + { + auto cloud_msg = generatePointCloud(); + publisher_->publish(cloud_msg); + RCLCPP_INFO(this->get_logger(), "Published."); + } + + sensor_msgs::msg::PointCloud2 generatePointCloud() + { + // Dimensions + static constexpr uint32_t HEIGHT = 128; + static constexpr uint32_t WIDTH = 1024; + + // We have 9 fields, totalling 32 bytes per point: + // offset | name | type | bytes + // -------+---------------+-----------+------ + // 0 | x | float32 | 4 + // 4 | y | float32 | 4 + // 8 | z | float32 | 4 + // 12 | intensity | float32 | 4 + // 16 | t | uint32 | 4 + // 20 | reflectivity | uint16 | 2 + // 22 | ring | uint16 | 2 + // 24 | ambient | uint16 | 2 + // 26 | (padding) | uint16 | 2 (for alignment) + // 28 | range | uint32 | 4 + // + // total per point: 32 bytes + // + // Because we need alignment for the 'range' field, we introduce 2 bytes + // of padding after 'ambient' to make 'range' start at a multiple of 4. + + static float total_iterations = 0.0; + + sensor_msgs::msg::PointCloud2 cloud_msg; + cloud_msg.header.stamp = now(); + cloud_msg.header.frame_id = "os_lidar"; + cloud_msg.height = HEIGHT; + cloud_msg.width = WIDTH; + cloud_msg.is_bigendian = false; + cloud_msg.is_dense = false; + + cloud_msg.fields.resize(9); + + cloud_msg.fields[0].name = "x"; + cloud_msg.fields[0].offset = 0; + cloud_msg.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32; + cloud_msg.fields[0].count = 1; + + cloud_msg.fields[1].name = "y"; + cloud_msg.fields[1].offset = 4; + cloud_msg.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32; + cloud_msg.fields[1].count = 1; + + cloud_msg.fields[2].name = "z"; + cloud_msg.fields[2].offset = 8; + cloud_msg.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32; + cloud_msg.fields[2].count = 1; + + cloud_msg.fields[3].name = "intensity"; + cloud_msg.fields[3].offset = 12; + cloud_msg.fields[3].datatype = sensor_msgs::msg::PointField::FLOAT32; + cloud_msg.fields[3].count = 1; + + cloud_msg.fields[4].name = "t"; + cloud_msg.fields[4].offset = 16; + cloud_msg.fields[4].datatype = sensor_msgs::msg::PointField::UINT32; + cloud_msg.fields[4].count = 1; + + cloud_msg.fields[5].name = "reflectivity"; + cloud_msg.fields[5].offset = 20; + cloud_msg.fields[5].datatype = sensor_msgs::msg::PointField::UINT16; + cloud_msg.fields[5].count = 1; + + cloud_msg.fields[6].name = "ring"; + cloud_msg.fields[6].offset = 22; + cloud_msg.fields[6].datatype = sensor_msgs::msg::PointField::UINT16; + cloud_msg.fields[6].count = 1; + + cloud_msg.fields[7].name = "ambient"; + cloud_msg.fields[7].offset = 24; + cloud_msg.fields[7].datatype = sensor_msgs::msg::PointField::UINT16; + cloud_msg.fields[7].count = 1; + + cloud_msg.fields[8].name = "range"; + cloud_msg.fields[8].offset = 28; + cloud_msg.fields[8].datatype = sensor_msgs::msg::PointField::UINT32; + cloud_msg.fields[8].count = 1; + + cloud_msg.point_step = 32; + cloud_msg.row_step = cloud_msg.point_step * cloud_msg.width; + cloud_msg.data.resize(cloud_msg.row_step * cloud_msg.height); + + sensor_msgs::PointCloud2Iterator iter_x(cloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(cloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(cloud_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_intensity(cloud_msg, "intensity"); + sensor_msgs::PointCloud2Iterator iter_t(cloud_msg, "t"); + sensor_msgs::PointCloud2Iterator iter_reflectivity(cloud_msg, "reflectivity"); + sensor_msgs::PointCloud2Iterator iter_ring(cloud_msg, "ring"); + sensor_msgs::PointCloud2Iterator iter_ambient(cloud_msg, "ambient"); + sensor_msgs::PointCloud2Iterator iter_range(cloud_msg, "range"); + + // Fill the point cloud in row-major order + // Simple pattern: + // x = (float) col + // y = sin(col / 50.0 + row) + // z = (float) row + // intensity = row + col + // t = synthetic timestamp + // reflectivity = row + // ring = col + // ambient = row*col mod 65535 + // range = (col * 100 + row) + for (uint32_t row = 0; row < HEIGHT; ++row) + { + for (uint32_t col = 0; col < WIDTH; ++col) + { + *iter_x = static_cast(col); + *iter_y = std::sin(static_cast(col) / 50.0f + row); + *iter_z = static_cast(row); // static offset to slowly move cloud towards the sky + + *iter_intensity = static_cast(row + col); + + *iter_t = static_cast((row * 100000) + col); // pseudorandom + + *iter_reflectivity = static_cast(row); + *iter_ring = static_cast(col % 65535); + *iter_ambient = static_cast((row * col) % 65535); + + *iter_range = static_cast((col * 100) + row); + + ++iter_x; ++iter_y; ++iter_z; ++iter_intensity; + ++iter_t; + ++iter_reflectivity; ++iter_ring; ++iter_ambient; + ++iter_range; + total_iterations++; + } + } + + return cloud_msg; + } + + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/ouster_simulator/src/ouster_subscriber_node.cpp b/ouster_simulator/src/ouster_subscriber_node.cpp new file mode 100644 index 0000000..fe6bedb --- /dev/null +++ b/ouster_simulator/src/ouster_subscriber_node.cpp @@ -0,0 +1,46 @@ +#include +#include +#include +#include + +class OusterSubscriber : public rclcpp::Node +{ +public: + OusterSubscriber() + : Node("ouster_subscriber"), first_message_received_(false) + { + subscription_ = this->create_subscription("/data", 10, std::bind(&OusterSubscriber::topicCallback, this, std::placeholders::_1)); + } + +private: + void topicCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) + { + rclcpp::Time current_time = this->now(); + + if (first_message_received_) { + double delta_seconds = (current_time - prev_time_).seconds(); + if (delta_seconds > 0.0) { + double frequency = 1.0 / delta_seconds; + RCLCPP_INFO(this->get_logger(), "Got data at frequency: %.2f Hz", frequency); + } + } else { + first_message_received_ = true; + } + + prev_time_ = current_time; + } + + rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Time prev_time_; + bool first_message_received_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} + diff --git a/routerconfig.json5 b/routerconfig.json5 index 2958e98..ad2ec50 100644 --- a/routerconfig.json5 +++ b/routerconfig.json5 @@ -27,7 +27,7 @@ /// /// See https://docs.rs/zenoh/latest/zenoh/config/struct.EndPoint.html endpoints: [ - "tcp/11.11.11.2:7447" + "tcp/11.11.11.5:7447" // "/
" ],