Skip to content
Merged
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
14 changes: 0 additions & 14 deletions docs/point_types.md
Original file line number Diff line number Diff line change
Expand Up @@ -31,20 +31,6 @@ These definitions can be found in `nebula_core_common/include/nebula_core_common
| `intensity` | `float` | | Intensity of the return as reported by the sensor. |
| `ring` | `uint16` | | Ring ID - only defined for rotational LiDARs. |

## [Deprecated] PointXYZICATR

| Field | Type | Units | Description |
| ------------- | -------- | --------- | ------------------------------------------------------- |
| `x` | `float` | `m` | Cartesian x coordinate. |
| `y` | `float` | `m` | The point's cartesian y coordinate. |
| `z` | `float` | `m` | Cartesian z coordinate. |
| padding | 4 bytes | | |
| `intensity` | `uint8` | | Intensity of the return as reported by the sensor. |
| `channel` | `uint16` | | The ID of the laser channel that produced the point. |
| `azimuth` | `float` | `degrees` | Azimuth in polar coordinates. |
| `timestamp` | `uint32` | `ns` | Time of detection relative to the pointcloud timestamp. |
| `return type` | `uint8` | | Return (echo) type. |

## [Deprecated] PointXYZIRADT

| Field | Type | Units | Description |
Expand Down
15 changes: 4 additions & 11 deletions src/nebula_continental/nebula_continental/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -25,8 +25,7 @@ target_link_libraries(
PUBLIC diagnostic_updater::diagnostic_updater
nebula_continental_decoders::nebula_continental_decoders
nebula_continental_hw_interfaces::nebula_continental_hw_interfaces
nebula_core_ros::nebula_core_ros
pcl_common)
nebula_core_ros::nebula_core_ros)
ament_target_dependencies(
continental_ars548_ros_wrapper
PUBLIC
Expand All @@ -35,7 +34,6 @@ ament_target_dependencies(
continental_srvs
diagnostic_msgs
geometry_msgs
pcl_conversions
nebula_msgs
radar_msgs
sensor_msgs
Expand Down Expand Up @@ -64,8 +62,7 @@ target_link_libraries(
continental_srr520_ros_wrapper
PUBLIC nebula_continental_decoders::nebula_continental_decoders
nebula_continental_hw_interfaces::nebula_continental_hw_interfaces
nebula_core_ros::nebula_core_ros
pcl_common)
nebula_core_ros::nebula_core_ros)

ament_target_dependencies(
continental_srr520_ros_wrapper
Expand All @@ -75,7 +72,6 @@ ament_target_dependencies(
diagnostic_msgs
geometry_msgs
nebula_msgs
pcl_conversions
radar_msgs
sensor_msgs
visualization_msgs
Expand All @@ -101,7 +97,6 @@ if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
find_package(ament_cmake_gtest REQUIRED)
find_package(rosbag2_cpp REQUIRED)
find_package(PCL REQUIRED COMPONENTS common)

ament_lint_auto_find_test_dependencies()

Expand All @@ -117,7 +112,7 @@ if(BUILD_TESTING)
PRIVATE tests include)
target_link_libraries(
continental_ros_decoder_test_ars548
PRIVATE nebula_core_common::nebula_core_common pcl_common
PRIVATE nebula_core_common::nebula_core_common
rosbag2_cpp::rosbag2_cpp diagnostic_updater::diagnostic_updater
nebula_continental_decoders::nebula_continental_decoders
nebula_core_ros::nebula_core_ros)
Expand All @@ -142,7 +137,7 @@ if(BUILD_TESTING)
PRIVATE tests include)
target_link_libraries(
continental_ros_decoder_test_srr520 nebula_core_common::nebula_core_common
pcl_common rosbag2_cpp::rosbag2_cpp diagnostic_updater::diagnostic_updater
rosbag2_cpp::rosbag2_cpp diagnostic_updater::diagnostic_updater
nebula_continental_decoders::nebula_continental_decoders
nebula_core_ros::nebula_core_ros)

Expand All @@ -169,7 +164,6 @@ ament_export_targets(export_continental_ars548_ros_wrapper)
ament_export_targets(export_continental_srr520_ros_wrapper)

ament_export_dependencies(
PCL
autoware_sensing_msgs
continental_msgs
continental_srvs
Expand All @@ -183,7 +177,6 @@ ament_export_dependencies(
nebula_continental_hw_interfaces
nebula_msgs
nebula_core_ros
pcl_conversions
radar_msgs
rclcpp_components
sensor_msgs
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -135,14 +135,14 @@ class ContinentalARS548DecoderWrapper
/// @brief Convert ARS548 detections to a pointcloud
/// @param msg The ARS548 detection list msg
/// @return Resulting detection pointcloud
pcl::PointCloud<nebula::drivers::continental_ars548::PointARS548Detection>::Ptr
drivers::PointCloud<nebula::drivers::continental_ars548::PointARS548Detection>
convert_to_pointcloud(const continental_msgs::msg::ContinentalArs548DetectionList & msg);

/// @brief Convert ARS548 objects to a pointcloud
/// @param msg The ARS548 object list msg
/// @return Resulting object pointcloud
pcl::PointCloud<nebula::drivers::continental_ars548::PointARS548Object>::Ptr
convert_to_pointcloud(const continental_msgs::msg::ContinentalArs548ObjectList & msg);
drivers::PointCloud<nebula::drivers::continental_ars548::PointARS548Object> convert_to_pointcloud(
const continental_msgs::msg::ContinentalArs548ObjectList & msg);

/// @brief Convert ARS548 detections to a standard RadarScan msg
/// @param msg The ARS548 detection list msg
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -98,14 +98,14 @@ class ContinentalSRR520DecoderWrapper
/// @brief Convert SRR520 detections to a pointcloud
/// @param msg The SRR520 detection list msg
/// @return Resulting detection pointcloud
pcl::PointCloud<nebula::drivers::continental_srr520::PointSRR520Detection>::Ptr
drivers::PointCloud<nebula::drivers::continental_srr520::PointSRR520Detection>
convert_to_pointcloud(const continental_msgs::msg::ContinentalSrr520DetectionList & msg);

/// @brief Convert SRR520 objects to a pointcloud
/// @param msg The SRR520 object list msg
/// @return Resulting object pointcloud
pcl::PointCloud<nebula::drivers::continental_srr520::PointSRR520Object>::Ptr
convert_to_pointcloud(const continental_msgs::msg::ContinentalSrr520ObjectList & msg);
drivers::PointCloud<nebula::drivers::continental_srr520::PointSRR520Object> convert_to_pointcloud(
const continental_msgs::msg::ContinentalSrr520ObjectList & msg);

/// @brief Convert SRR520 detections to a standard RadarScan msg
/// @param msg The SRR520 detection list msg
Expand Down
4 changes: 0 additions & 4 deletions src/nebula_continental/nebula_continental/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,6 @@
<buildtool_depend>autoware_cmake</buildtool_depend>
<buildtool_depend>ros_environment</buildtool_depend>
<buildtool_depend>ros_testing</buildtool_depend>
<build_depend>libpcl-all-dev</build_depend>
<build_export_depend>libpcl-all-dev</build_export_depend>

<depend>autoware_sensing_msgs</depend>
<depend>continental_msgs</depend>
Expand All @@ -31,7 +29,6 @@
<depend>nebula_core_common</depend>
<depend>nebula_core_ros</depend>
<depend>nebula_msgs</depend>
<depend>pcl_conversions</depend>
<depend>radar_msgs</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
Expand All @@ -43,7 +40,6 @@

<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>libpcl-all-dev</test_depend>
<test_depend>ros_testing</test_depend>
<test_depend>rosbag2_cpp</test_depend>

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,7 @@
#include "nebula_core_ros/sync_tooling/sync_tooling_worker.hpp"

#include <nebula_core_common/util/string_conversions.hpp>

#include <pcl_conversions/pcl_conversions.h>
#include <nebula_core_ros/point_cloud_conversions.hpp>

#include <algorithm>
#include <chrono>
Expand Down Expand Up @@ -204,9 +203,9 @@ void ContinentalARS548DecoderWrapper::detection_list_callback(
if (
detection_pointcloud_pub_->get_subscription_count() > 0 ||
detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) {
const auto detection_pointcloud_ptr = convert_to_pointcloud(*msg);
const auto detection_pointcloud = convert_to_pointcloud(*msg);
auto detection_pointcloud_msg_ptr = std::make_unique<sensor_msgs::msg::PointCloud2>();
pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr);
*detection_pointcloud_msg_ptr = nebula::ros::to_ros_msg(detection_pointcloud);

detection_pointcloud_msg_ptr->header = msg->header;
detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr));
Expand Down Expand Up @@ -245,9 +244,9 @@ void ContinentalARS548DecoderWrapper::object_list_callback(
if (
object_pointcloud_pub_->get_subscription_count() > 0 ||
object_pointcloud_pub_->get_intra_process_subscription_count() > 0) {
const auto object_pointcloud_ptr = convert_to_pointcloud(*msg);
const auto object_pointcloud = convert_to_pointcloud(*msg);
auto object_pointcloud_msg_ptr = std::make_unique<sensor_msgs::msg::PointCloud2>();
pcl::toROSMsg(*object_pointcloud_ptr, *object_pointcloud_msg_ptr);
*object_pointcloud_msg_ptr = nebula::ros::to_ros_msg(object_pointcloud);

object_pointcloud_msg_ptr->header = msg->header;
object_pointcloud_pub_->publish(std::move(object_pointcloud_msg_ptr));
Expand Down Expand Up @@ -731,15 +730,14 @@ ContinentalARS548DecoderWrapper::convert_to_autoware_radar_objects(
return autoware_objects;
}

pcl::PointCloud<nebula::drivers::continental_ars548::PointARS548Detection>::Ptr
drivers::PointCloud<nebula::drivers::continental_ars548::PointARS548Detection>
ContinentalARS548DecoderWrapper::convert_to_pointcloud(
const continental_msgs::msg::ContinentalArs548DetectionList & msg)
{
namespace ars548 = nebula::drivers::continental_ars548;

pcl::PointCloud<nebula::drivers::continental_ars548::PointARS548Detection>::Ptr output_pointcloud(
new pcl::PointCloud<nebula::drivers::continental_ars548::PointARS548Detection>);
output_pointcloud->reserve(msg.detections.size());
drivers::PointCloud<nebula::drivers::continental_ars548::PointARS548Detection> output_pointcloud;
output_pointcloud.reserve(msg.detections.size());

nebula::drivers::continental_ars548::PointARS548Detection point{};
for (const auto & detection : msg.detections) {
Expand Down Expand Up @@ -767,23 +765,20 @@ ContinentalARS548DecoderWrapper::convert_to_pointcloud(
point.object_id = detection.object_id;
point.ambiguity_flag = ars548::normalize_probability(detection.raw_ambiguity_flag);

output_pointcloud->points.emplace_back(point);
output_pointcloud.emplace_back(point);
}

output_pointcloud->height = 1;
output_pointcloud->width = output_pointcloud->points.size();
return output_pointcloud;
}

pcl::PointCloud<nebula::drivers::continental_ars548::PointARS548Object>::Ptr
drivers::PointCloud<nebula::drivers::continental_ars548::PointARS548Object>
ContinentalARS548DecoderWrapper::convert_to_pointcloud(
const continental_msgs::msg::ContinentalArs548ObjectList & msg)
{
namespace ars548 = nebula::drivers::continental_ars548;

pcl::PointCloud<nebula::drivers::continental_ars548::PointARS548Object>::Ptr output_pointcloud(
new pcl::PointCloud<nebula::drivers::continental_ars548::PointARS548Object>);
output_pointcloud->reserve(msg.objects.size());
drivers::PointCloud<nebula::drivers::continental_ars548::PointARS548Object> output_pointcloud;
output_pointcloud.reserve(msg.objects.size());

nebula::drivers::continental_ars548::PointARS548Object point{};
for (const auto & object : msg.objects) {
Expand Down Expand Up @@ -811,11 +806,9 @@ ContinentalARS548DecoderWrapper::convert_to_pointcloud(
point.shape_width_edge_mean = object.shape_width_edge_mean;
point.dynamics_orientation_rate_mean = object.orientation_rate_mean;

output_pointcloud->points.emplace_back(point);
output_pointcloud.emplace_back(point);
}

output_pointcloud->height = 1;
output_pointcloud->width = output_pointcloud->points.size();
return output_pointcloud;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,7 @@
#include "nebula_continental/continental_srr520_decoder_wrapper.hpp"

#include <nebula_core_common/util/string_conversions.hpp>

#include <pcl_conversions/pcl_conversions.h>
#include <nebula_core_ros/point_cloud_conversions.hpp>

#include <memory>
#include <unordered_set>
Expand Down Expand Up @@ -155,9 +154,9 @@ void ContinentalSRR520DecoderWrapper::near_detection_list_callback(
if (
near_detection_pointcloud_pub_->get_subscription_count() > 0 ||
near_detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) {
const auto detection_pointcloud_ptr = convert_to_pointcloud(*msg);
const auto detection_pointcloud = convert_to_pointcloud(*msg);
auto detection_pointcloud_msg_ptr = std::make_unique<sensor_msgs::msg::PointCloud2>();
pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr);
*detection_pointcloud_msg_ptr = nebula::ros::to_ros_msg(detection_pointcloud);

detection_pointcloud_msg_ptr->header = msg->header;
near_detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr));
Expand All @@ -184,9 +183,9 @@ void ContinentalSRR520DecoderWrapper::hrr_detection_list_callback(
if (
hrr_detection_pointcloud_pub_->get_subscription_count() > 0 ||
hrr_detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) {
const auto detection_pointcloud_ptr = convert_to_pointcloud(*msg);
const auto detection_pointcloud = convert_to_pointcloud(*msg);
auto detection_pointcloud_msg_ptr = std::make_unique<sensor_msgs::msg::PointCloud2>();
pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr);
*detection_pointcloud_msg_ptr = nebula::ros::to_ros_msg(detection_pointcloud);

detection_pointcloud_msg_ptr->header = msg->header;
hrr_detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr));
Expand All @@ -213,9 +212,9 @@ void ContinentalSRR520DecoderWrapper::object_list_callback(
if (
object_pointcloud_pub_->get_subscription_count() > 0 ||
object_pointcloud_pub_->get_intra_process_subscription_count() > 0) {
const auto object_pointcloud_ptr = convert_to_pointcloud(*msg);
const auto object_pointcloud = convert_to_pointcloud(*msg);
auto object_pointcloud_msg_ptr = std::make_unique<sensor_msgs::msg::PointCloud2>();
pcl::toROSMsg(*object_pointcloud_ptr, *object_pointcloud_msg_ptr);
*object_pointcloud_msg_ptr = nebula::ros::to_ros_msg(object_pointcloud);

object_pointcloud_msg_ptr->header = msg->header;
object_pointcloud_pub_->publish(std::move(object_pointcloud_msg_ptr));
Expand Down Expand Up @@ -249,13 +248,12 @@ void ContinentalSRR520DecoderWrapper::status_callback(
status_pub_->publish(std::move(status_msg_ptr));
}

pcl::PointCloud<nebula::drivers::continental_srr520::PointSRR520Detection>::Ptr
drivers::PointCloud<nebula::drivers::continental_srr520::PointSRR520Detection>
ContinentalSRR520DecoderWrapper::convert_to_pointcloud(
const continental_msgs::msg::ContinentalSrr520DetectionList & msg)
{
pcl::PointCloud<nebula::drivers::continental_srr520::PointSRR520Detection>::Ptr output_pointcloud(
new pcl::PointCloud<nebula::drivers::continental_srr520::PointSRR520Detection>);
output_pointcloud->reserve(msg.detections.size());
drivers::PointCloud<nebula::drivers::continental_srr520::PointSRR520Detection> output_pointcloud;
output_pointcloud.reserve(msg.detections.size());

nebula::drivers::continental_srr520::PointSRR520Detection point{};
for (const auto & detection : msg.detections) {
Expand All @@ -275,21 +273,18 @@ ContinentalSRR520DecoderWrapper::convert_to_pointcloud(
point.pdh04 = detection.pdh04;
point.pdh05 = detection.pdh05;

output_pointcloud->points.emplace_back(point);
output_pointcloud.emplace_back(point);
}

output_pointcloud->height = 1;
output_pointcloud->width = output_pointcloud->points.size();
return output_pointcloud;
}

pcl::PointCloud<nebula::drivers::continental_srr520::PointSRR520Object>::Ptr
drivers::PointCloud<nebula::drivers::continental_srr520::PointSRR520Object>
ContinentalSRR520DecoderWrapper::convert_to_pointcloud(
const continental_msgs::msg::ContinentalSrr520ObjectList & msg)
{
pcl::PointCloud<nebula::drivers::continental_srr520::PointSRR520Object>::Ptr output_pointcloud(
new pcl::PointCloud<nebula::drivers::continental_srr520::PointSRR520Object>);
output_pointcloud->reserve(msg.objects.size());
drivers::PointCloud<nebula::drivers::continental_srr520::PointSRR520Object> output_pointcloud;
output_pointcloud.reserve(msg.objects.size());

nebula::drivers::continental_srr520::PointSRR520Object point{};
for (const auto & object : msg.objects) {
Expand All @@ -312,11 +307,9 @@ ContinentalSRR520DecoderWrapper::convert_to_pointcloud(
point.dynamics_abs_acc_x = object.a_abs_x;
point.dynamics_abs_acc_y = object.a_abs_y;

output_pointcloud->points.emplace_back(point);
output_pointcloud.emplace_back(point);
}

output_pointcloud->height = 1;
output_pointcloud->width = output_pointcloud->points.size();
return output_pointcloud;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.20)
project(nebula_continental_common)

find_package(autoware_cmake REQUIRED)
find_package(Boost REQUIRED)
autoware_package()

add_library(nebula_continental_common INTERFACE)
Expand All @@ -10,14 +11,15 @@ target_include_directories(
nebula_continental_common
INTERFACE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_link_libraries(nebula_continental_common
INTERFACE nebula_core_common::nebula_core_common)
target_link_libraries(
nebula_continental_common
INTERFACE nebula_core_common::nebula_core_common Boost::headers)

install(TARGETS nebula_continental_common
EXPORT export_nebula_continental_common)
install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME})

ament_export_targets(export_nebula_continental_common)
ament_export_dependencies(nebula_core_common)
ament_export_dependencies(Boost nebula_core_common)

ament_package()
Loading
Loading