Add output topic with custom message

upo_laser_people_detector/CMakeLists.txt

@@ -12,6 +12,7 @@ find_package(rclcpp_components REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
+find_package(upo_laser_people_msgs REQUIRED)
 
 find_package(Python3 REQUIRED COMPONENTS Development.Embed NumPy)
 find_package(pybind11 REQUIRED)
@@ -36,6 +37,7 @@ ament_target_dependencies(lasermodelhost PUBLIC
 	std_msgs
 	sensor_msgs
 	visualization_msgs
+	upo_laser_people_msgs
 )
 
 rclcpp_components_register_node(lasermodelhost

upo_laser_people_detector/package.xml

@@ -14,6 +14,7 @@
 	<depend>std_msgs</depend>
 	<depend>sensor_msgs</depend>
 	<depend>visualization_msgs</depend>
+	<depend>upo_laser_people_msgs</depend>
 
 	<export>
 		<build_type>ament_cmake</build_type>

upo_laser_people_detector/src/lasermodelhost.cpp

@@ -10,11 +10,16 @@
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
+#include <upo_laser_people_msgs/msg/person_detection_list.hpp>
 
 #include <onnxruntime_cxx_api.h>
 
 namespace upo_laser_people_detector {
 
+using upo_laser_people_msgs::msg::PersonDetection;
+using upo_laser_people_msgs::msg::PersonDetectionList;
+using visualization_msgs::msg::MarkerArray;
+
 namespace {
 
 	visualization_msgs::msg::Marker makeEmptyMarker(std::string const& ref)
@@ -173,7 +178,8 @@ class LaserModelHost : public rclcpp::Node {
 	std::optional<LaserModel> m_model;
 
 	rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr m_sub{};
-	std::shared_ptr<rclcpp::Publisher<visualization_msgs::msg::MarkerArray>> m_pub{};
+	std::shared_ptr<rclcpp::Publisher<PersonDetectionList>> m_pubOutput{};
+	std::shared_ptr<rclcpp::Publisher<MarkerArray>> m_pubMarker{};
 
 	static void OrtRosLogging(
 		void* param, OrtLoggingLevel severity,
@@ -189,16 +195,37 @@ class LaserModelHost : public rclcpp::Node {
 	{
 		auto people = m_model->infer(msg);
 
-		auto out = std::make_unique<visualization_msgs::msg::MarkerArray>();
+		{
+			auto out = std::make_unique<PersonDetectionList>();
+			out->header = msg.header;
+
+			for (size_t i = 0; i < people.size(); i ++) {
+				auto& p = people[i];
 
-		out->markers.emplace_back(makeEmptyMarker(msg.header.frame_id));
+				PersonDetection det;
+				det.type = PersonDetection::TYPE_UNSPECIFIED;
+				det.score = p[0];
+				det.position.x = p[1];
+				det.position.y = p[2];
 
-		for (size_t i = 0; i < people.size(); i ++) {
-			auto& p = people[i];
-			out->markers.emplace_back(makePersonMarker(msg.header.frame_id, 1+i, p[1], p[2], 0.4f));
+				out->detections.emplace_back(std::move(det));
+			}
+
+			m_pubOutput->publish(std::move(out));
 		}
 
-		m_pub->publish(std::move(out));
+		{
+			auto out = std::make_unique<MarkerArray>();
+
+			out->markers.emplace_back(makeEmptyMarker(msg.header.frame_id));
+
+			for (size_t i = 0; i < people.size(); i ++) {
+				auto& p = people[i];
+				out->markers.emplace_back(makePersonMarker(msg.header.frame_id, 1+i, p[1], p[2], 0.4f));
+			}
+
+			m_pubMarker->publish(std::move(out));
+		}
 	}
 
 public:
@@ -208,8 +235,9 @@ class LaserModelHost : public rclcpp::Node {
 	{
 		auto model_file = declare_parameter<std::string>("model_file");
 
-		auto laser_topic = declare_parameter<std::string>("laser_topic", "/scanfront");
-		auto marker_topic = declare_parameter<std::string>("marker_topic", "detected_people");
+		auto laser_topic  = declare_parameter<std::string>("laser_topic",  "/scanfront");
+		auto output_topic = declare_parameter<std::string>("output_topic", "detected_people");
+		auto marker_topic = declare_parameter<std::string>("marker_topic", "detected_people_markers");
 
 		auto near   = declare_parameter<float>("scan_near",       0.02f);
 		auto far    = declare_parameter<float>("scan_far",        10.0f);
@@ -224,7 +252,8 @@ class LaserModelHost : public rclcpp::Node {
 			laser_topic, 10, std::bind(&LaserModelHost::onLaserScan, this, _1)
 		);
 
-		m_pub = create_publisher<visualization_msgs::msg::MarkerArray>(marker_topic, 10);
+		m_pubOutput = create_publisher<PersonDetectionList>(output_topic, 10);
+		m_pubMarker = create_publisher<MarkerArray>(marker_topic, 10);
 	}
 
 };

upo_laser_people_detector/src/lasermodelhost_peaks.cpp

@@ -12,6 +12,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
+#include <upo_laser_people_msgs/msg/person_detection_list.hpp>
 
 #include <pybind11/embed.h>
 #include <pybind11/numpy.h>
@@ -23,6 +24,10 @@ using namespace py::literals;
 
 namespace upo_laser_people_detector {
 
+using upo_laser_people_msgs::msg::PersonDetection;
+using upo_laser_people_msgs::msg::PersonDetectionList;
+using visualization_msgs::msg::MarkerArray;
+
 namespace {
 
 	static const char python_code[] =
@@ -250,7 +255,8 @@ class LaserModelHostPeaks : public rclcpp::Node {
 	std::optional<LaserModelPeaks> m_model;
 
 	rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr m_sub{};
-	std::shared_ptr<rclcpp::Publisher<visualization_msgs::msg::MarkerArray>> m_pub{};
+	std::shared_ptr<rclcpp::Publisher<PersonDetectionList>> m_pubOutput{};
+	std::shared_ptr<rclcpp::Publisher<MarkerArray>> m_pubMarker{};
 
 	void* m_hPythonDylib{};
 
@@ -268,16 +274,37 @@ class LaserModelHostPeaks : public rclcpp::Node {
 	{
 		auto people = m_model->infer(msg);
 
-		auto out = std::make_unique<visualization_msgs::msg::MarkerArray>();
+		{
+			auto out = std::make_unique<PersonDetectionList>();
+			out->header = msg.header;
+
+			for (size_t i = 0; i < people.size(); i ++) {
+				auto& p = people[i];
 
-		out->markers.emplace_back(makeEmptyMarker(msg.header.frame_id));
+				PersonDetection det;
+				det.type = PersonDetection::TYPE_UNSPECIFIED;
+				det.score = p[0];
+				det.position.x = p[1];
+				det.position.y = p[2];
 
-		for (size_t i = 0; i < people.size(); i ++) {
-			auto& p = people[i];
-			out->markers.emplace_back(makePersonMarker(msg.header.frame_id, 1+i, p[1], p[2], 0.4f));
+				out->detections.emplace_back(std::move(det));
+			}
+
+			m_pubOutput->publish(std::move(out));
 		}
 
-		m_pub->publish(std::move(out));
+		{
+			auto out = std::make_unique<MarkerArray>();
+
+			out->markers.emplace_back(makeEmptyMarker(msg.header.frame_id));
+
+			for (size_t i = 0; i < people.size(); i ++) {
+				auto& p = people[i];
+				out->markers.emplace_back(makePersonMarker(msg.header.frame_id, 1+i, p[1], p[2], 0.4f));
+			}
+
+			m_pubMarker->publish(std::move(out));
+		}
 	}
 
 public:
@@ -293,8 +320,9 @@ class LaserModelHostPeaks : public rclcpp::Node {
 
 		auto model_file = declare_parameter<std::string>("model_file");
 
-		auto laser_topic = declare_parameter<std::string>("laser_topic", "/scanfront");
-		auto marker_topic = declare_parameter<std::string>("marker_topic", "detected_people");
+		auto laser_topic  = declare_parameter<std::string>("laser_topic",  "/scanfront");
+		auto output_topic = declare_parameter<std::string>("output_topic", "detected_people");
+		auto marker_topic = declare_parameter<std::string>("marker_topic", "detected_people_markers");
 
 		auto near   = declare_parameter<float>("scan_near",       0.02f);
 		auto far    = declare_parameter<float>("scan_far",        10.0f);
@@ -310,7 +338,8 @@ class LaserModelHostPeaks : public rclcpp::Node {
 			laser_topic, 10, std::bind(&LaserModelHostPeaks::onLaserScan, this, _1)
 		);
 
-		m_pub = create_publisher<visualization_msgs::msg::MarkerArray>(marker_topic, 10);
+		m_pubOutput = create_publisher<PersonDetectionList>(output_topic, 10);
+		m_pubMarker = create_publisher<MarkerArray>(marker_topic, 10);
 	}
 
 	~LaserModelHostPeaks()

upo_laser_people_msgs/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.5)
+project(upo_laser_people_msgs)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+	add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+
+set(msg_files
+	"msg/PersonDetection.msg"
+	"msg/PersonDetectionList.msg"
+)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+	${msg_files}
+	DEPENDENCIES geometry_msgs std_msgs
+	ADD_LINTER_TESTS
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()

upo_laser_people_msgs/msg/PersonDetection.msg

@@ -0,0 +1,10 @@
+# Detection class
+uint32 type
+uint32 TYPE_UNSPECIFIED=0
+# Other types reserved for future use
+
+# Detection score [0, 1]
+float32 score
+
+# Only X and Y components used; Z is ignored
+geometry_msgs/Point position

upo_laser_people_msgs/msg/PersonDetectionList.msg

@@ -0,0 +1,4 @@
+std_msgs/Header header
+
+# List of detected people
+upo_laser_people_msgs/PersonDetection[] detections

upo_laser_people_msgs/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+	<name>upo_laser_people_msgs</name>
+	<version>0.1.0</version>
+	<description>Messages used by upo_laser_people_detector.</description>
+	<maintainer email="[email protected]">famozur</maintainer>
+	<license>MIT</license>
+
+	<buildtool_depend>ament_cmake</buildtool_depend>
+	<buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+	<depend>geometry_msgs</depend>
+	<depend>std_msgs</depend>
+
+	<exec_depend>rosidl_default_runtime</exec_depend>
+
+	<member_of_group>rosidl_interface_packages</member_of_group>
+
+	<export>
+		<build_type>ament_cmake</build_type>
+	</export>
+</package>