From 589775ee601543a0774ebda78d0ff371151feec3 Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sat, 17 Aug 2024 15:09:13 +0200 Subject: [PATCH 01/23] enable compilation with C++17 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6a3589e..8ba6737 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,7 +14,7 @@ catkin_package( people_msgs ) -set(CMAKE_CXX_FLAGS "-Wall -Wpedantic --std=c++14") +set(CMAKE_CXX_FLAGS "-Wall -Wpedantic --std=c++17") link_directories(${GAZEBO_LIBRARY_DIRS}) include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS}) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") From a21d063b58d99ccb0c5fa14e5d9142553948be00 Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sat, 17 Aug 2024 15:10:11 +0200 Subject: [PATCH 02/23] multi-inclusion guard changed to simple `pragma` --- src/PeopleExtraction.hpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/PeopleExtraction.hpp b/src/PeopleExtraction.hpp index 725491a..77e7c3b 100644 --- a/src/PeopleExtraction.hpp +++ b/src/PeopleExtraction.hpp @@ -5,8 +5,7 @@ * Author: rayvburn */ -#ifndef SRC_PEOPLEEXTRACTION_HPP_ -#define SRC_PEOPLEEXTRACTION_HPP_ +#pragma once #include #include @@ -96,5 +95,3 @@ class PeopleExtraction { // filtering detected obstacles - republishing ones indicating not-legs friend class LegsExtraction; }; - -#endif /* SRC_PEOPLEEXTRACTION_HPP_ */ From 46b97b6e40ef7101b1ab75a7200db0be859687a3 Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sat, 17 Aug 2024 15:10:57 +0200 Subject: [PATCH 03/23] `PeopleExtraction` - reworked static literal constants --- src/PeopleExtraction.cpp | 3 --- src/PeopleExtraction.hpp | 5 +++-- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/PeopleExtraction.cpp b/src/PeopleExtraction.cpp index 665c80c..ea6d56d 100644 --- a/src/PeopleExtraction.cpp +++ b/src/PeopleExtraction.cpp @@ -7,9 +7,6 @@ #include "PeopleExtraction.hpp" -constexpr char* PeopleExtraction::GAZEBO_FRAME_ID_DEFAULT; -constexpr char* PeopleExtraction::TARGET_FRAME_ID_DEFAULT; - #define DEBUG_TF PeopleExtraction::PeopleExtraction() : diff --git a/src/PeopleExtraction.hpp b/src/PeopleExtraction.hpp index 77e7c3b..0d222fe 100644 --- a/src/PeopleExtraction.hpp +++ b/src/PeopleExtraction.hpp @@ -23,6 +23,9 @@ class PeopleExtraction { public: + static constexpr auto GAZEBO_FRAME_ID_DEFAULT = "world"; + static constexpr auto TARGET_FRAME_ID_DEFAULT = "map"; + PeopleExtraction(); virtual ~PeopleExtraction() = default; @@ -66,8 +69,6 @@ class PeopleExtraction { void publishPeople(); void publishPeoplePositions(); - static constexpr char* GAZEBO_FRAME_ID_DEFAULT = "world"; - static constexpr char* TARGET_FRAME_ID_DEFAULT = "map"; ros::NodeHandle nh_; tf2_ros::Buffer tf_buffer_; From df7d464363f276a2de1831e87d5b29b0f8d5d389 Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sat, 17 Aug 2024 15:11:33 +0200 Subject: [PATCH 04/23] fixed `int` vs `size_t` type mismatches --- src/PeopleExtraction.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/PeopleExtraction.cpp b/src/PeopleExtraction.cpp index ea6d56d..a8e802c 100644 --- a/src/PeopleExtraction.cpp +++ b/src/PeopleExtraction.cpp @@ -43,7 +43,7 @@ void PeopleExtraction::gazeboModelStateCallback(const gazebo_msgs::ModelStates:: callback_counter_ = 0; // update internal database dynamically - int models_found = 0; + size_t models_found = 0; for (auto pattern : people_name_patterns_) { // evaluate, whether the object named with a given pattern exists in a simulation std::vector sim_names = findModels(msg->name, pattern); @@ -62,7 +62,7 @@ void PeopleExtraction::gazeboModelStateCallback(const gazebo_msgs::ModelStates:: } // possibly delete renamed/destroyed objects if (models_found < people_data_.size()) { - ROS_INFO("Seems that an object was deleted. Number of matching model names found: %d, size of database: %d", + ROS_INFO("Seems that an object was deleted. Number of matching model names found: %lu, size of database: %lu", models_found, people_data_.size()); deleteDestroyedPerson(msg->name); } From ffd8cd14f9a3ecfe1a5d409aff9cb1b15df66d4d Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sat, 17 Aug 2024 15:11:52 +0200 Subject: [PATCH 05/23] `printf` usage fix --- src/PeopleExtraction.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/PeopleExtraction.cpp b/src/PeopleExtraction.cpp index a8e802c..6a3cfc7 100644 --- a/src/PeopleExtraction.cpp +++ b/src/PeopleExtraction.cpp @@ -195,7 +195,7 @@ void PeopleExtraction::rotateTwist(geometry_msgs::TwistStamped &vel, const geome vel_v3.vector.y = vel.twist.linear.y; vel_v3.vector.z = vel.twist.linear.z; #ifdef DEBUG_TF - printf("[rotateTwist (1)] transform - raw tr: %2.3f, %2.3f, %2.3f | modded tr: %2.3f, %2.3f, %2.3f | rot: %2.3f, %2.3f, %2.3f\r\n", + printf("[rotateTwist (1)] transform - raw tr: %2.3f, %2.3f, %2.3f | modded tr: %2.3f, %2.3f, %2.3f | rot: %2.3f, %2.3f, %2.3f, %2.3f\r\n", transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z, From 43b1275668ea10b7a95cfe5a311aee3d62a1b14e Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sat, 17 Aug 2024 15:13:26 +0200 Subject: [PATCH 06/23] node configuration YAML file parameterized in the launch --- launch/people_extraction.launch | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/launch/people_extraction.launch b/launch/people_extraction.launch index 6d3b48b..f237c48 100644 --- a/launch/people_extraction.launch +++ b/launch/people_extraction.launch @@ -1,4 +1,6 @@ - + + + From 6fdf5bc8820529cc2870f559803e3edae409030d Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sat, 17 Aug 2024 15:14:37 +0200 Subject: [PATCH 07/23] updated contact info --- package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/package.xml b/package.xml index eb8e163..849d6c5 100755 --- a/package.xml +++ b/package.xml @@ -3,7 +3,8 @@ 1.0.0 people_extraction package - Jaroslaw Karwowski + Jarosław Karwowski + Jarosław Karwowski GPLv3 From 511ad6dd52fd11eed56fc1155c500374c0d12f61 Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sat, 17 Aug 2024 23:37:02 +0200 Subject: [PATCH 08/23] simplified data collection and processing, applied `people_msgs_utils::Person` to process data, reworked the launch file, general cleanup --- .gitignore | 3 +- CMakeLists.txt | 4 +- config/people.yaml | 17 +- launch/people_extraction.launch | 20 +- package.xml | 1 + src/PeopleExtraction.cpp | 426 +++++++++++++++----------------- src/PeopleExtraction.hpp | 67 +++-- src/Person.hpp | 30 --- 8 files changed, 267 insertions(+), 301 deletions(-) delete mode 100644 src/Person.hpp diff --git a/.gitignore b/.gitignore index 3759c54..079a737 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ src/actor_legs_extraction_node.cpp src/LegsExtraction.* launch/actor_legs_extraction.launch -config/legs.yaml \ No newline at end of file +config/legs.yaml +build/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 8ba6737..986b671 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,10 +1,11 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(people_extraction) find_package(catkin REQUIRED COMPONENTS gazebo_ros roscpp people_msgs + people_msgs_utils ) catkin_package( @@ -12,6 +13,7 @@ catkin_package( gazebo_ros roscpp people_msgs + people_msgs_utils ) set(CMAKE_CXX_FLAGS "-Wall -Wpedantic --std=c++17") diff --git a/config/people.yaml b/config/people.yaml index 5eb0c1d..5f82a31 100644 --- a/config/people.yaml +++ b/config/people.yaml @@ -1,8 +1,9 @@ -gazebo_frame: "world" -target_frame: "map" -callback_omits: 25 # each n-th simulator callback will be processed - -people_topic: "/people" -position_topic: "/people_measurements" - -model_name_patterns: ["person", "actor"] \ No newline at end of file +model_name_patterns: [ + "person", + "actor", + "Scrubs", + "PatientWheelChair", + "MaleVisitorSit", + "VisitorKidSit", + "FemaleVisitorSit" +] diff --git a/launch/people_extraction.launch b/launch/people_extraction.launch index f237c48..a0acac8 100644 --- a/launch/people_extraction.launch +++ b/launch/people_extraction.launch @@ -1,6 +1,22 @@ + + - - + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index 849d6c5..567c3bd 100755 --- a/package.xml +++ b/package.xml @@ -12,5 +12,6 @@ roscpp gazebo_ros people_msgs + people_msgs_utils diff --git a/src/PeopleExtraction.cpp b/src/PeopleExtraction.cpp index 6a3cfc7..b115335 100644 --- a/src/PeopleExtraction.cpp +++ b/src/PeopleExtraction.cpp @@ -7,280 +7,260 @@ #include "PeopleExtraction.hpp" -#define DEBUG_TF - -PeopleExtraction::PeopleExtraction() : - tf_listener_(tf_buffer_), - id_next_(0), - gazebo_tf_frame_(PeopleExtraction::GAZEBO_FRAME_ID_DEFAULT), - target_tf_frame_(PeopleExtraction::TARGET_FRAME_ID_DEFAULT), - callback_counter_(0), - callback_omits_(0), - seq_(0) +#include +#include + +#include + +PeopleExtraction::PeopleExtraction(): + tf_listener_(tf_buffer_) { - std::string people_topic(""); - std::string position_topic(""); - nh_.param("gazebo_frame", gazebo_tf_frame_, gazebo_tf_frame_); - nh_.param("target_frame", target_tf_frame_, target_tf_frame_); - nh_.param("callback_omits", callback_omits_, callback_omits_); - nh_.param("people_topic", people_topic, "/people_topic"); - nh_.param("position_topic", position_topic, "/position_topic"); + auto pub_frequency = nh_.param("pub_frequency", 30.0); + nh_.param("world_frame", world_tf_frame_, "world"); + nh_.param("target_frame", target_tf_frame_, "odom"); + + // model name check nh_.getParam("model_name_patterns", people_name_patterns_); if (!people_name_patterns_.size()) { ROS_ERROR("PeopleExtraction - model_name_patterns is empty! Class would be ill-formed"); return; } - sub_gazebo_ = nh_.subscribe("/gazebo/model_states", 1, &PeopleExtraction::gazeboModelStateCallback, this);; - pub_people_ = nh_.advertise(people_topic, 5); - pub_pos_ = nh_.advertise(position_topic, 5); + + // publishing frequency check + if (pub_frequency <= 0.0) { + ROS_ERROR( + "Simulator-based people data will not be published as wrong frequency was selected: %f Hz", + pub_frequency + ); + return; + } + + // create ROS interfaces + pub_people_ = nh_.advertise("/people", 5); + pub_pos_ = nh_.advertise("/people_measurements", 5); + sub_gazebo_ = nh_.subscribe( + "/gazebo/model_states", + 1, + &PeopleExtraction::gazeboModelStateCallback, + this + ); + + timer_pub_ = nh_.createTimer( + ros::Duration(1.0 / pub_frequency), + std::bind( + &PeopleExtraction::publish, + this + ) + ); } -void PeopleExtraction::gazeboModelStateCallback(const gazebo_msgs::ModelStates::ConstPtr &msg) { - if (callback_counter_++ != callback_omits_) { +void PeopleExtraction::gazeboModelStateCallback(const gazebo_msgs::ModelStates::ConstPtr& msg) { + if (msg->name.size() != msg->pose.size() || msg->name.size() != msg->twist.size()) { + ROS_ERROR("Vector sizes in the input message are not equal, cannot process further"); return; } std::lock_guard lock(mutex_); - callback_counter_ = 0; // update internal database dynamically size_t models_found = 0; - for (auto pattern : people_name_patterns_) { - // evaluate, whether the object named with a given pattern exists in a simulation - std::vector sim_names = findModels(msg->name, pattern); - models_found += sim_names.size(); - // iterate over actual names - for (auto sim_name : sim_names) { - // check whether the person already exists in the internal database - if (!doesPersonExist(sim_name)) { - Person bud; - bud.name = sim_name; - people_data_.push_back(bud); - people_name_id_[sim_name] = id_next_++; - ROS_INFO("Gazebo Callback - Added new person '%s' (ID: %d), detected from naming pattern: '%s'", sim_name.c_str(), people_name_id_[sim_name], pattern.c_str()); - } + size_t models_total = msg->name.size(); + for (size_t i = 0; i < models_total; i++) { + // investigated object - obtain only the name at this stage + const auto& name = msg->name.at(i); + // check whether a model with a given name is of our interest (according to the naming patterns) + bool pattern_matched = false; + std::string pattern; + std::tie(pattern_matched, pattern) = isMatching(name, people_name_patterns_); + if (!pattern_matched) { + continue; } - } - // possibly delete renamed/destroyed objects - if (models_found < people_data_.size()) { - ROS_INFO("Seems that an object was deleted. Number of matching model names found: %lu, size of database: %lu", - models_found, people_data_.size()); - deleteDestroyedPerson(msg->name); - } - // compute indexes of the `people_names` objects - for (auto&& person : people_data_) { - auto it = std::find(msg->name.begin(), msg->name.end(), person.name); - if (it != msg->name.end()) { - // found, compute index - int index = it - msg->name.begin(); - // retrieve meaningful data from corresponding vectors - if (index >= 0) { - // fill up raw (Gazebo) values at first - person.pose.header.frame_id = gazebo_tf_frame_; - person.pose.header.seq++; - person.pose.header.stamp = ros::Time::now(); - - person.pose.pose.position = msg->pose.at(index).position; - person.pose.pose.orientation = msg->pose.at(index).orientation; - - // copy header - person.vel.header = person.pose.header; - - person.vel.twist.linear = msg->twist.at(index).linear; - person.vel.twist.angular = msg->twist.at(index).angular; - // try to transform to another coordinate system - try { - geometry_msgs::TransformStamped transform = tf_buffer_.lookupTransform( - target_tf_frame_, - gazebo_tf_frame_, - ros::Time::now(), - ros::Duration(1.0) - ); - // transform pose - geometry_msgs::PoseStamped pose_backup = person.pose; - tf2::doTransform(person.pose, person.pose, transform); - #ifdef DEBUG_TF - printf("[transform pose (1)] before - position: %2.5f, %2.5f, %2.5f, orientation: %2.5f, %2.5f, %2.5f\r\n", - pose_backup.pose.position.x, - pose_backup.pose.position.y, - pose_backup.pose.position.z, - pose_backup.pose.orientation.x, - pose_backup.pose.orientation.y, - pose_backup.pose.orientation.z - ); - printf("[transform pose (2)] after - position: %2.5f, %2.5f, %2.5f, orientation: %2.5f, %2.5f, %2.5f\r\n", - person.pose.pose.position.x, - person.pose.pose.position.y, - person.pose.pose.position.z, - person.pose.pose.orientation.x, - person.pose.pose.orientation.y, - person.pose.pose.orientation.z - ); - #endif - // rotate velocity (in the local coordinate system) - rotateTwist(person.vel, transform); - // update header frame - person.pose.header.frame_id = target_tf_frame_; - person.vel.header.frame_id = target_tf_frame_; - #ifdef DEBUG_TF - printf("Assuming successful transformation - changed frame_id to TARGET: %s\r\n", - person.pose.header.frame_id.c_str()); - #endif - } catch (tf2::TransformException &e) { - ROS_WARN("exception: %s\r\nPerson %s: could not find TF from %s to %s", - e.what(), msg->name.at(index).c_str(), gazebo_tf_frame_.c_str(), target_tf_frame_.c_str()); - } - } + models_found++; + gazebo_msgs::ModelState model; + model.model_name = name; + model.pose = msg->pose.at(i); + model.twist = msg->twist.at(i); + + // check whether the person already exists in the internal database (only need ModelState update then) + if (doesPersonExist(name)) { + people_gazebo_[name].second = model; + continue; } + // new model - assign an ID and update its ModelState + people_gazebo_[name] = {people_gazebo_.size(), model}; + ROS_INFO( + "Tracking a new person '%s' (ID: '%lu'), detected from naming pattern: '%s'", + people_gazebo_[name].second.model_name.c_str(), + people_gazebo_[name].first, + pattern.c_str() + ); } - // publishing section - publishPeople(); - publishPeoplePositions(); + // possibly delete renamed/destroyed objects + if (models_found < people_gazebo_.size()) { + ROS_INFO( + "Seems that an object was deleted. Number of matching model names found: %lu, size of database: %lu", + models_found, + people_gazebo_.size() + ); + deleteDestroyedPerson(msg->name); + } } -bool PeopleExtraction::doesPersonExist(const std::string &name) const { - for (auto person : people_data_) { - if (person.name == name) { - return true; - } - } - return false; +bool PeopleExtraction::doesPersonExist(const std::string& name) const { + return people_gazebo_.find(name) != people_gazebo_.cend(); } -std::vector PeopleExtraction::findModels(const std::vector &model_names, std::string pattern) const { - std::vector db; - for (auto name : model_names) { - // find - if (name.find(pattern) != std::string::npos) { - // found - db.push_back(name); +std::tuple PeopleExtraction::isMatching(const std::string& model_name, std::vector& patterns) const { + for (const auto& pattern: patterns) { + // try to find + if (model_name.find(pattern) == std::string::npos) { + continue; } + return {true, pattern}; } - return db; + return {false, std::string()}; } -void PeopleExtraction::deleteDestroyedPerson(const std::vector &model_names) { - int num_deleted = 0; - auto i = std::begin(people_data_); +void PeopleExtraction::deleteDestroyedPerson(const std::vector& model_names) { + auto i = std::begin(people_gazebo_); // while is safer than for here - while (i != std::end(people_data_)) { - auto it = std::find(model_names.begin(), model_names.end(), i->name); + while (i != std::end(people_gazebo_)) { + // map and nested pair + auto model_name = i->second.second.model_name; + auto it = std::find(model_names.begin(), model_names.end(), model_name); if (it != model_names.end()) { // found i++; continue; } // delete current element - ROS_INFO("Deleting '%s' from database", i->name.c_str()); - // erase from map - num_deleted += people_name_id_.erase(i->name); - people_data_.erase(i); + ROS_INFO("Deleting '%s' from database", model_name.c_str()); + // erase from map; ref: https://stackoverflow.com/a/2874533 + i = people_gazebo_.erase(i); } - ROS_INFO("Deleted %d object(s)", num_deleted); } -void PeopleExtraction::rotateTwist(geometry_msgs::TwistStamped &vel, const geometry_msgs::TransformStamped &transform) { - // local coordinate system - consider rotation only - geometry_msgs::TransformStamped transform_local = transform; - transform_local.transform.translation.x = 0.0; - transform_local.transform.translation.y = 0.0; - transform_local.transform.translation.z = 0.0; - - // use msg type supported by tf2::doTransform - // http://docs.ros.org/en/kinetic/api/tf2_geometry_msgs/html/c++/namespacetf2.html#a82ca47c6f5b0360e6c5b250dca719a78 - geometry_msgs::Vector3Stamped vel_v3; - vel_v3.header = vel.header; - vel_v3.vector.x = vel.twist.linear.x; - vel_v3.vector.y = vel.twist.linear.y; - vel_v3.vector.z = vel.twist.linear.z; - #ifdef DEBUG_TF - printf("[rotateTwist (1)] transform - raw tr: %2.3f, %2.3f, %2.3f | modded tr: %2.3f, %2.3f, %2.3f | rot: %2.3f, %2.3f, %2.3f, %2.3f\r\n", - transform.transform.translation.x, - transform.transform.translation.y, - transform.transform.translation.z, - transform_local.transform.translation.x, - transform_local.transform.translation.y, - transform_local.transform.translation.z, - transform_local.transform.rotation.x, - transform_local.transform.rotation.y, - transform_local.transform.rotation.z, - transform_local.transform.rotation.w - ); +people_msgs_utils::People PeopleExtraction::gazeboModelsToPeople() const { + // transform localization data to the desired frame + geometry_msgs::TransformStamped tf_stamped; + // by default - lack of translation and rotation + tf_stamped.transform.rotation.w = 1.0; - tf2::Quaternion q_rot; - tf2::convert(transform_local.transform.rotation, q_rot); - tf2::Matrix3x3 mat(q_rot); - double roll, pitch, yaw; - mat.getRPY(roll, pitch, yaw); - printf("[rotateTwist (2)] transform - rot RPY: %2.3f, %2.3f, %2.3f\r\n", roll, pitch, yaw); - #endif - - geometry_msgs::Vector3Stamped vel_v3_modified; - tf2::doTransform(vel_v3, vel_v3_modified, transform_local); - #ifdef DEBUG_TF - printf("[rotateTwist (3)] vel_before: %2.3f, %2.3f, %2.3f | vel_after: %2.3f, %2.3f, %2.3f\r\n", - vel_v3.vector.x, - vel_v3.vector.y, - vel_v3.vector.z, - vel_v3_modified.vector.x, - vel_v3_modified.vector.y, - vel_v3_modified.vector.z - ); - #endif - - // copy meaningful values to the twist msg - vel.twist.linear.x = vel_v3_modified.vector.x; - vel.twist.linear.y = vel_v3_modified.vector.y; - vel.twist.linear.z = vel_v3_modified.vector.z; - // just to be on the safe side - vel.twist.angular.x = 0.0; - vel.twist.angular.y = 0.0; - vel.twist.angular.z = 0.0; + // obtain the transform to the desired frame + try { + tf_stamped = tf_buffer_.lookupTransform( + target_tf_frame_, + world_tf_frame_, + ros::Time(0) + ); + } catch (tf2::TransformException& ex) { + ROS_ERROR( + "Could not find a transform from the `%s` to `%s` frame! Exception details: `%s`", + world_tf_frame_.c_str(), + target_tf_frame_.c_str(), + ex.what() + ); + // avoid flooding console with this kind of errors (localization might not be operational yet) + std::this_thread::sleep_for(std::chrono::seconds(1)); + return people_msgs_utils::People(); + } + + // prepare output container + people_msgs_utils::People people; + + for (auto const& model_data: people_gazebo_) { + // retrieve from the value (key is not considered here) + auto id = model_data.second.first; + auto model = model_data.second.second; + + geometry_msgs::PoseWithCovariance pose; + pose.pose = model.pose; + pose.covariance.fill(0.0); // assuming ideal data + // convert data from twist to pose representation (contents are the same) + geometry_msgs::PoseWithCovariance velocity; + velocity.covariance.fill(0.0); // assuming ideal data + velocity.pose.position.x = model.twist.linear.x; + velocity.pose.position.y = model.twist.linear.y; + velocity.pose.position.z = model.twist.linear.z; + tf2::Quaternion angular_vel; + // NOTE: for a proper yaw angle, yaw, pitch and roll angles must be reordered (compared to the documentation) + angular_vel.setEuler( + model.twist.angular.x, + model.twist.angular.y, + model.twist.angular.z + ); + velocity.pose.orientation = tf2::toMsg(angular_vel); + + // create a Person instance (with geom. data in the source frame) + people_msgs_utils::Person p( + std::to_string(id), // avoid using 'string'-type names here + pose, + velocity, + 1.0, // complete reliability + false, + true, + id, + ros::Time::now().toNSec(), // track age = since startup as we have ideal data + std::string("") // group data not included + ); + // TODO: include group data (obtain from node parameters) + + // transform to the target frame + p.transform(tf_stamped); + // collect + people.push_back(p); + } + + return people; +} + +void PeopleExtraction::publish() { + std::lock_guard lock(mutex_); + + // check if there is something to publish + if (people_gazebo_.empty()) { + return; + } + + // Gazebo models to people representation + auto people = gazeboModelsToPeople(); + + publishPeople(people); + publishPeoplePositions(people); } -void PeopleExtraction::publishPeople() { +void PeopleExtraction::publishPeople(const people_msgs_utils::People& people) { people_msgs::People people_msg; - people_msgs::Person person_msg; - people_msg.header.frame_id = gazebo_tf_frame_; - people_msg.header.seq = seq_; + people_msg.header.frame_id = target_tf_frame_; people_msg.header.stamp = ros::Time::now(); - for (auto person : people_data_) { - // http://docs.ros.org/en/kinetic/api/people_msgs/html/msg/Person.html - person_msg.name = person.name; - person_msg.position = person.pose.pose.position; - person_msg.velocity.x = person.vel.twist.linear.x; - person_msg.velocity.y = person.vel.twist.linear.y; - person_msg.velocity.z = person.vel.twist.linear.z; - person_msg.reliability = 1.0; - people_msg.people.push_back(person_msg); + for (const auto& person: people) { + people_msg.people.push_back(person.toPersonStd()); } pub_people_.publish(people_msg); } -void PeopleExtraction::publishPeoplePositions() { - people_msgs::PositionMeasurementArray position_measurement_array_msg; - people_msgs::PositionMeasurement position_measurement_msg; +void PeopleExtraction::publishPeoplePositions(const people_msgs_utils::People& people) { + people_msgs::PositionMeasurementArray pos_msg_array; - position_measurement_array_msg.header.frame_id = gazebo_tf_frame_; - position_measurement_array_msg.header.seq = seq_; - position_measurement_array_msg.header.stamp = ros::Time::now(); + pos_msg_array.header.frame_id = target_tf_frame_; + pos_msg_array.header.stamp = ros::Time::now(); - position_measurement_msg.header = position_measurement_array_msg.header; + for (auto person: people) { + people_msgs::PositionMeasurement pos_msg; + pos_msg.header = pos_msg_array.header; - for (auto person : people_data_) { - // http://docs.ros.org/en/kinetic/api/people_msgs/html/msg/PositionMeasurement.html - position_measurement_msg.name = person.name; - position_measurement_msg.object_id = person.object_id; - position_measurement_msg.pos.x = person.pose.pose.position.x; - position_measurement_msg.pos.y = person.pose.pose.position.y; - position_measurement_msg.pos.z = person.pose.pose.position.z; - position_measurement_msg.reliability = 1.0; + pos_msg.name = person.getName(); + pos_msg.object_id = person.getName(); + pos_msg.pos.x = person.getPositionX(); + pos_msg.pos.y = person.getPositionY(); + pos_msg.pos.z = person.getPositionZ(); + pos_msg.reliability = person.getReliability(); // measurements are ideal, so covariance = 0 // FIXME: initialization? what is that for? - position_measurement_msg.initialization = 0; - position_measurement_array_msg.people.push_back(position_measurement_msg); + pos_msg.initialization = 0; + pos_msg_array.people.push_back(pos_msg); } - pub_pos_.publish(position_measurement_array_msg); + pub_pos_.publish(pos_msg_array); } diff --git a/src/PeopleExtraction.hpp b/src/PeopleExtraction.hpp index 0d222fe..ca808dc 100644 --- a/src/PeopleExtraction.hpp +++ b/src/PeopleExtraction.hpp @@ -8,24 +8,24 @@ #pragma once #include -#include -#include -#include + +#include #include #include #include + +#include +#include +#include +#include + #include #include -#include -#include -#include "Person.hpp" +#include class PeopleExtraction { public: - static constexpr auto GAZEBO_FRAME_ID_DEFAULT = "world"; - static constexpr auto TARGET_FRAME_ID_DEFAULT = "map"; - PeopleExtraction(); virtual ~PeopleExtraction() = default; @@ -33,41 +33,42 @@ class PeopleExtraction { /** * @brief * @note This is a high frequency topic - * @note TODO: message_filters, TimeSynchronizer? * @param msg */ - void gazeboModelStateCallback(const gazebo_msgs::ModelStates::ConstPtr &msg); + void gazeboModelStateCallback(const gazebo_msgs::ModelStates::ConstPtr& msg); /** * @brief Evaluates, whether the person exists in the internal database * @param name: name of the person that is searched for * @return */ - bool doesPersonExist(const std::string &name) const; + bool doesPersonExist(const std::string& name) const; /** - * @brief Finds models that meet the given pattern - * @param model_names - * @param pattern - * @return + * @brief Evaluates whether the given @ref model_name matches any pattern given by @ref patterns + * + * @param model_name + * @param patterns + * @return std::tuple */ - std::vector findModels(const std::vector &model_names, std::string pattern) const; + std::tuple isMatching(const std::string& model_name, std::vector& patterns) const; /** * @brief Get rid of the unnecessary model * @param model_names */ - void deleteDestroyedPerson(const std::vector &model_names); + void deleteDestroyedPerson(const std::vector& model_names); /** - * @brief Executes rotation of the `vel` vector according to the given `transform` - * @param vel: modified - rotated vector - * @param transform + * @brief Converts Gazebo ModelStates database to people representation in the @ref people_msgs_utils::People form + * + * @return people_msgs_utils::People */ - void rotateTwist(geometry_msgs::TwistStamped &vel, const geometry_msgs::TransformStamped &transform); + people_msgs_utils::People gazeboModelsToPeople() const; - void publishPeople(); - void publishPeoplePositions(); + void publish(); + void publishPeople(const people_msgs_utils::People& people); + void publishPeoplePositions(const people_msgs_utils::People& people); ros::NodeHandle nh_; @@ -79,20 +80,14 @@ class PeopleExtraction { ros::Publisher pub_people_; ros::Publisher pub_pos_; - std::vector people_data_; - /// @brief Algorithms looks for a model name that meets one of the given patterns + // key: name of the object in the simulation + // value: pair with object's ID and the Gazebo data structure + std::map> people_gazebo_; + /// @brief Algorithm looks for a model name that meets one of the given patterns std::vector people_name_patterns_; - /// @brief Stores name and ID correspondence of detected people - std::map people_name_id_; - int id_next_; - std::string gazebo_tf_frame_; + std::string world_tf_frame_; std::string target_tf_frame_; - int callback_counter_; - int callback_omits_; - unsigned long int seq_; - // possibly extend package features to: - // filtering detected obstacles - republishing ones indicating not-legs - friend class LegsExtraction; + ros::Timer timer_pub_; }; diff --git a/src/Person.hpp b/src/Person.hpp deleted file mode 100644 index 7ae781f..0000000 --- a/src/Person.hpp +++ /dev/null @@ -1,30 +0,0 @@ -/* - * Person.hpp - * - * Created on: Dec 29, 2020 - * Author: rayvburn - */ - -#ifndef SRC_PERSON_HPP_ -#define SRC_PERSON_HPP_ - -#include -#include -#include - -struct Person { - std::string name; - std::string object_id; - std::string frame_id; - geometry_msgs::PoseStamped pose; - geometry_msgs::TwistStamped vel; - double reliability; - double covariance[9]; - std::vector tagnames; - std::vector tags; - bool initialization; -}; - - - -#endif /* SRC_PERSON_HPP_ */ From 42a0be204af8ee15eabb4d420be857e5f7d3f022 Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sun, 18 Aug 2024 01:27:55 +0200 Subject: [PATCH 09/23] added parsing `LinkStates` as for some models the returned poses are zero (in contrast to their main links) --- config/people.yaml | 3 +- src/PeopleExtraction.cpp | 128 ++++++++++++++++++++++++++++++--------- src/PeopleExtraction.hpp | 45 +++++++++++--- 3 files changed, 138 insertions(+), 38 deletions(-) diff --git a/config/people.yaml b/config/people.yaml index 5f82a31..ca31cf1 100644 --- a/config/people.yaml +++ b/config/people.yaml @@ -1,6 +1,7 @@ model_name_patterns: [ - "person", "actor", +] +link_name_patterns: [ "Scrubs", "PatientWheelChair", "MaleVisitorSit", diff --git a/src/PeopleExtraction.cpp b/src/PeopleExtraction.cpp index b115335..e25012a 100644 --- a/src/PeopleExtraction.cpp +++ b/src/PeopleExtraction.cpp @@ -20,11 +20,17 @@ PeopleExtraction::PeopleExtraction(): nh_.param("target_frame", target_tf_frame_, "odom"); // model name check - nh_.getParam("model_name_patterns", people_name_patterns_); - if (!people_name_patterns_.size()) { + nh_.getParam("model_name_patterns", model_name_patterns_); + if (!model_name_patterns_.size()) { ROS_ERROR("PeopleExtraction - model_name_patterns is empty! Class would be ill-formed"); return; } + // link names check + nh_.getParam("link_name_patterns", link_name_patterns_); + if (!link_name_patterns_.size()) { + ROS_ERROR("PeopleExtraction - link_name_patterns is empty! Class would be ill-formed"); + return; + } // publishing frequency check if (pub_frequency <= 0.0) { @@ -38,13 +44,20 @@ PeopleExtraction::PeopleExtraction(): // create ROS interfaces pub_people_ = nh_.advertise("/people", 5); pub_pos_ = nh_.advertise("/people_measurements", 5); - sub_gazebo_ = nh_.subscribe( + sub_model_states_ = nh_.subscribe( "/gazebo/model_states", 1, &PeopleExtraction::gazeboModelStateCallback, this ); + sub_link_states_ = nh_.subscribe( + "/gazebo/link_states", + 1, + &PeopleExtraction::gazeboLinkStateCallback, + this + ); + timer_pub_ = nh_.createTimer( ros::Duration(1.0 / pub_frequency), std::bind( @@ -59,7 +72,7 @@ void PeopleExtraction::gazeboModelStateCallback(const gazebo_msgs::ModelStates:: ROS_ERROR("Vector sizes in the input message are not equal, cannot process further"); return; } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_ms_); // update internal database dynamically size_t models_found = 0; @@ -70,7 +83,7 @@ void PeopleExtraction::gazeboModelStateCallback(const gazebo_msgs::ModelStates:: // check whether a model with a given name is of our interest (according to the naming patterns) bool pattern_matched = false; std::string pattern; - std::tie(pattern_matched, pattern) = isMatching(name, people_name_patterns_); + std::tie(pattern_matched, pattern) = isMatching(name, model_name_patterns_); if (!pattern_matched) { continue; } @@ -82,36 +95,81 @@ void PeopleExtraction::gazeboModelStateCallback(const gazebo_msgs::ModelStates:: model.twist = msg->twist.at(i); // check whether the person already exists in the internal database (only need ModelState update then) - if (doesPersonExist(name)) { - people_gazebo_[name].second = model; + if (doesPersonExist(people_models_, name)) { + people_models_[name].second = model; continue; } // new model - assign an ID and update its ModelState - people_gazebo_[name] = {people_gazebo_.size(), model}; + people_models_[name] = {people_models_.size(), model}; ROS_INFO( - "Tracking a new person '%s' (ID: '%lu'), detected from naming pattern: '%s'", - people_gazebo_[name].second.model_name.c_str(), - people_gazebo_[name].first, + "Tracking a new person '%s' (ID: '%lu'), detected from naming pattern: '%s' (models)", + people_models_[name].second.model_name.c_str(), + people_models_[name].first, pattern.c_str() ); } // possibly delete renamed/destroyed objects - if (models_found < people_gazebo_.size()) { + deleteDestroyedPerson(people_models_, msg->name); +} + +void PeopleExtraction::gazeboLinkStateCallback(const gazebo_msgs::LinkStates::ConstPtr& msg) { + if (msg->name.size() != msg->pose.size() || msg->name.size() != msg->twist.size()) { + ROS_ERROR("Vector sizes in the input message are not equal, cannot process further"); + return; + } + std::lock_guard lock(mutex_ls_); + + // update internal database dynamically + size_t links_found = 0; + size_t links_total = msg->name.size(); + for (size_t i = 0; i < links_total; i++) { + // investigated object - obtain only the name at this stage + const auto& name = msg->name.at(i); + // check whether a model with a given name is of our interest (according to the naming patterns) + bool pattern_matched = false; + std::string pattern; + std::tie(pattern_matched, pattern) = isMatching(name, link_name_patterns_); + if (!pattern_matched) { + continue; + } + + links_found++; + gazebo_msgs::ModelState model; + model.model_name = name; + model.pose = msg->pose.at(i); + model.twist = msg->twist.at(i); + + // check whether the person already exists in the internal database (only need ModelState update then) + if (doesPersonExist(people_links_, name)) { + people_links_[name].second = model; + continue; + } + // new model - assign an ID and update its ModelState + people_links_[name] = {people_links_.size(), model}; ROS_INFO( - "Seems that an object was deleted. Number of matching model names found: %lu, size of database: %lu", - models_found, - people_gazebo_.size() + "Tracking a new person '%s' (ID: '%lu'), detected from naming pattern: '%s' (links)", + people_links_[name].second.model_name.c_str(), + people_links_[name].first, + pattern.c_str() ); - deleteDestroyedPerson(msg->name); } + + // possibly delete renamed/destroyed objects + deleteDestroyedPerson(people_links_, msg->name); } -bool PeopleExtraction::doesPersonExist(const std::string& name) const { - return people_gazebo_.find(name) != people_gazebo_.cend(); +bool PeopleExtraction::doesPersonExist( + const std::map>& people, + const std::string& name +) const { + return people.find(name) != people.cend(); } -std::tuple PeopleExtraction::isMatching(const std::string& model_name, std::vector& patterns) const { +std::tuple PeopleExtraction::isMatching( + const std::string& model_name, + std::vector& patterns +) const { for (const auto& pattern: patterns) { // try to find if (model_name.find(pattern) == std::string::npos) { @@ -122,10 +180,13 @@ std::tuple PeopleExtraction::isMatching(const std::string& mo return {false, std::string()}; } -void PeopleExtraction::deleteDestroyedPerson(const std::vector& model_names) { - auto i = std::begin(people_gazebo_); +void PeopleExtraction::deleteDestroyedPerson( + std::map>& people, + const std::vector& model_names +) const { + auto i = std::begin(people); // while is safer than for here - while (i != std::end(people_gazebo_)) { + while (i != std::end(people)) { // map and nested pair auto model_name = i->second.second.model_name; auto it = std::find(model_names.begin(), model_names.end(), model_name); @@ -137,11 +198,13 @@ void PeopleExtraction::deleteDestroyedPerson(const std::vector& mod // delete current element ROS_INFO("Deleting '%s' from database", model_name.c_str()); // erase from map; ref: https://stackoverflow.com/a/2874533 - i = people_gazebo_.erase(i); + i = people.erase(i); } } -people_msgs_utils::People PeopleExtraction::gazeboModelsToPeople() const { +people_msgs_utils::People PeopleExtraction::gazeboModelsToPeople( + const std::map>& people_models +) const { // transform localization data to the desired frame geometry_msgs::TransformStamped tf_stamped; // by default - lack of translation and rotation @@ -169,7 +232,7 @@ people_msgs_utils::People PeopleExtraction::gazeboModelsToPeople() const { // prepare output container people_msgs_utils::People people; - for (auto const& model_data: people_gazebo_) { + for (auto const& model_data: people_models) { // retrieve from the value (key is not considered here) auto id = model_data.second.first; auto model = model_data.second.second; @@ -216,15 +279,24 @@ people_msgs_utils::People PeopleExtraction::gazeboModelsToPeople() const { } void PeopleExtraction::publish() { - std::lock_guard lock(mutex_); + std::lock_guard lock_ms(mutex_ms_); + std::lock_guard lock_ls(mutex_ls_); + + // combine entities obtained from "models" and from "links" + std::map> people_gazebo; + // start with copying 1st and extend with the 2nd container + people_gazebo = people_models_; + for (auto const& [key, val]: people_links_) { + people_gazebo[key] = val; + } // check if there is something to publish - if (people_gazebo_.empty()) { + if (people_gazebo.empty()) { return; } // Gazebo models to people representation - auto people = gazeboModelsToPeople(); + auto people = gazeboModelsToPeople(people_gazebo); publishPeople(people); publishPeoplePositions(people); diff --git a/src/PeopleExtraction.hpp b/src/PeopleExtraction.hpp index ca808dc..d9de2fe 100644 --- a/src/PeopleExtraction.hpp +++ b/src/PeopleExtraction.hpp @@ -11,6 +11,8 @@ #include #include +#include +#include #include #include @@ -32,17 +34,27 @@ class PeopleExtraction { private: /** * @brief - * @note This is a high frequency topic + * @note Callback triggered by a high frequency Gazebo topic * @param msg */ void gazeboModelStateCallback(const gazebo_msgs::ModelStates::ConstPtr& msg); + /** + * @brief + * @note Callback triggered by a high frequency Gazebo topic + * @param msg + */ + void gazeboLinkStateCallback(const gazebo_msgs::LinkStates::ConstPtr& msg); + /** * @brief Evaluates, whether the person exists in the internal database * @param name: name of the person that is searched for * @return */ - bool doesPersonExist(const std::string& name) const; + bool doesPersonExist( + const std::map>& people, + const std::string& name + ) const; /** * @brief Evaluates whether the given @ref model_name matches any pattern given by @ref patterns @@ -55,16 +67,22 @@ class PeopleExtraction { /** * @brief Get rid of the unnecessary model - * @param model_names + * @param people people identified so far + * @param model_names names of entities recognized in the current step */ - void deleteDestroyedPerson(const std::vector& model_names); + void deleteDestroyedPerson( + std::map>& people, + const std::vector& model_names + ) const; /** * @brief Converts Gazebo ModelStates database to people representation in the @ref people_msgs_utils::People form * * @return people_msgs_utils::People */ - people_msgs_utils::People gazeboModelsToPeople() const; + people_msgs_utils::People gazeboModelsToPeople( + const std::map>& people_models + ) const; void publish(); void publishPeople(const people_msgs_utils::People& people); @@ -75,16 +93,25 @@ class PeopleExtraction { tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - std::mutex mutex_; - ros::Subscriber sub_gazebo_; + std::mutex mutex_ms_; + ros::Subscriber sub_model_states_; + std::mutex mutex_ls_; + ros::Subscriber sub_link_states_; + ros::Publisher pub_people_; ros::Publisher pub_pos_; + // NOTE: unfortunately, ROS Message Filters do not work with gazebo_msgs-based topics as those messages + // do not carry timestamp values + // // key: name of the object in the simulation // value: pair with object's ID and the Gazebo data structure - std::map> people_gazebo_; + std::map> people_models_; + std::map> people_links_; /// @brief Algorithm looks for a model name that meets one of the given patterns - std::vector people_name_patterns_; + std::vector model_name_patterns_; + /// @brief Same but for link states + std::vector link_name_patterns_; std::string world_tf_frame_; std::string target_tf_frame_; From 59d3c0f79929dd0dc965262a7e71a69bfd5c24a9 Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sun, 18 Aug 2024 11:31:09 +0200 Subject: [PATCH 10/23] parts common for `ModelStates` and `LinkStates` moved to `GazeboExtractor` template class --- src/GazeboExtractor.hpp | 164 ++++++++++++++++++++++++++++++++++++ src/PeopleExtraction.cpp | 177 +++++---------------------------------- src/PeopleExtraction.hpp | 70 +++------------- 3 files changed, 195 insertions(+), 216 deletions(-) create mode 100644 src/GazeboExtractor.hpp diff --git a/src/GazeboExtractor.hpp b/src/GazeboExtractor.hpp new file mode 100644 index 0000000..f221e95 --- /dev/null +++ b/src/GazeboExtractor.hpp @@ -0,0 +1,164 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +/** + * @brief Class that subscribes messages from a given topic and stores them for easy access (on demand) + * + * @tparam Tcb defines the type of the callback routine + * @tparam Tsub defines the type of the subscriber + * @tparam Tcontainer defines the type of the actual data storage + */ +template +class GazeboExtractor { +public: + GazeboExtractor( + ros::NodeHandle& nh, + const std::string& topic_name, + const std::vector& name_patterns, + std::atomic& id_ref + ): + name_patterns_(name_patterns), + id_ref_(id_ref) + { + sub_ = nh.subscribe( + topic_name, + 1, + &GazeboExtractor::callback, + this + ); + } + + std::map> getPeople() const { + std::lock_guard lock(mutex_); + return people_; + } + +protected: + /** + * @brief + * @note Callback triggered by a high frequency Gazebo topic + * @param msg + */ + void callback(const Tcb& msg) { + if (msg->name.size() != msg->pose.size() || msg->name.size() != msg->twist.size()) { + ROS_ERROR("Vector sizes in the input message are not equal, cannot process further"); + return; + } + std::lock_guard lock(mutex_); + + // update internal database dynamically + size_t models_found = 0; + size_t models_total = msg->name.size(); + for (size_t i = 0; i < models_total; i++) { + // investigated object - obtain only the name at this stage + const auto& name = msg->name.at(i); + // check whether a model with a given name is of our interest (according to the naming patterns) + bool pattern_matched = false; + std::string pattern; + + std::tie(pattern_matched, pattern) = isMatching(name, name_patterns_); + if (!pattern_matched) { + continue; + } + + models_found++; + Tcontainer model; + model.model_name = name; + model.pose = msg->pose.at(i); + model.twist = msg->twist.at(i); + + // check whether the person already exists in the internal database (only need ModelState update then) + if (doesPersonExist(name)) { + people_[name].second = model; + continue; + } + // new model - assign an ID and update its ModelState + people_[name] = {id_ref_.load(), model}; + // increment the reference ID counter + id_ref_++; + ROS_INFO( + "Tracking a new person '%s' (ID: '%lu'), detected from naming pattern: '%s'", + people_[name].second.model_name.c_str(), + people_[name].first, + pattern.c_str() + ); + } + + // possibly delete renamed/destroyed objects + deleteDestroyedPerson(msg->name); + } + + /** + * @brief Evaluates, whether the person exists in the internal database + * @param name: name of the person that is searched for + * @return + */ + bool doesPersonExist(const std::string& name) const { + return people_.find(name) != people_.cend(); + } + + /** + * @brief Evaluates whether the given @ref model_name matches any pattern given by @ref patterns + * + * @param model_name + * @param patterns + * @return std::tuple + */ + std::tuple isMatching( + const std::string& model_name, + std::vector& patterns + ) const { + for (const auto& pattern: patterns) { + // try to find + if (model_name.find(pattern) == std::string::npos) { + continue; + } + return {true, pattern}; + } + return {false, std::string()}; + } + + /** + * @brief Get rid of the unnecessary model + * @param people people identified so far + * @param model_names names of entities recognized in the current step + */ + void deleteDestroyedPerson(const std::vector& model_names) { + auto i = std::begin(people_); + // while is safer than for here + while (i != std::end(people_)) { + // map and nested pair + auto model_name = i->second.second.model_name; + auto it = std::find(model_names.begin(), model_names.end(), model_name); + if (it != model_names.end()) { + // found + i++; + continue; + } + // delete current element + ROS_INFO("Deleting '%s' from database", model_name.c_str()); + // erase from map; ref: https://stackoverflow.com/a/2874533 + i = people_.erase(i); + } + } + + ros::Subscriber sub_; + mutable std::mutex mutex_; + + /// @brief Algorithm looks for a model name that meets one of the given patterns + std::vector name_patterns_; + + // For assigning IDs between multiple classes + std::atomic& id_ref_; + + // key: name of the object in the simulation + // value: pair with object's ID and the Gazebo data structure + std::map> people_; +}; diff --git a/src/PeopleExtraction.cpp b/src/PeopleExtraction.cpp index e25012a..eb6825e 100644 --- a/src/PeopleExtraction.cpp +++ b/src/PeopleExtraction.cpp @@ -13,21 +13,23 @@ #include PeopleExtraction::PeopleExtraction(): + id_ref_(0), tf_listener_(tf_buffer_) { auto pub_frequency = nh_.param("pub_frequency", 30.0); nh_.param("world_frame", world_tf_frame_, "world"); nh_.param("target_frame", target_tf_frame_, "odom"); - // model name check - nh_.getParam("model_name_patterns", model_name_patterns_); - if (!model_name_patterns_.size()) { + // obtain model and link name patterns + std::vector model_name_patterns; + nh_.getParam("model_name_patterns", model_name_patterns); + if (!model_name_patterns.size()) { ROS_ERROR("PeopleExtraction - model_name_patterns is empty! Class would be ill-formed"); return; } - // link names check - nh_.getParam("link_name_patterns", link_name_patterns_); - if (!link_name_patterns_.size()) { + std::vector link_name_patterns; + nh_.getParam("link_name_patterns", link_name_patterns); + if (!link_name_patterns.size()) { ROS_ERROR("PeopleExtraction - link_name_patterns is empty! Class would be ill-formed"); return; } @@ -42,22 +44,23 @@ PeopleExtraction::PeopleExtraction(): } // create ROS interfaces - pub_people_ = nh_.advertise("/people", 5); - pub_pos_ = nh_.advertise("/people_measurements", 5); - sub_model_states_ = nh_.subscribe( + model_extractor_ = std::make_unique( + nh_, "/gazebo/model_states", - 1, - &PeopleExtraction::gazeboModelStateCallback, - this + model_name_patterns, + id_ref_ ); - sub_link_states_ = nh_.subscribe( + link_extractor_ = std::make_unique( + nh_, "/gazebo/link_states", - 1, - &PeopleExtraction::gazeboLinkStateCallback, - this + link_name_patterns, + id_ref_ ); + pub_people_ = nh_.advertise("/people", 5); + pub_pos_ = nh_.advertise("/people_measurements", 5); + timer_pub_ = nh_.createTimer( ros::Duration(1.0 / pub_frequency), std::bind( @@ -67,141 +70,6 @@ PeopleExtraction::PeopleExtraction(): ); } -void PeopleExtraction::gazeboModelStateCallback(const gazebo_msgs::ModelStates::ConstPtr& msg) { - if (msg->name.size() != msg->pose.size() || msg->name.size() != msg->twist.size()) { - ROS_ERROR("Vector sizes in the input message are not equal, cannot process further"); - return; - } - std::lock_guard lock(mutex_ms_); - - // update internal database dynamically - size_t models_found = 0; - size_t models_total = msg->name.size(); - for (size_t i = 0; i < models_total; i++) { - // investigated object - obtain only the name at this stage - const auto& name = msg->name.at(i); - // check whether a model with a given name is of our interest (according to the naming patterns) - bool pattern_matched = false; - std::string pattern; - std::tie(pattern_matched, pattern) = isMatching(name, model_name_patterns_); - if (!pattern_matched) { - continue; - } - - models_found++; - gazebo_msgs::ModelState model; - model.model_name = name; - model.pose = msg->pose.at(i); - model.twist = msg->twist.at(i); - - // check whether the person already exists in the internal database (only need ModelState update then) - if (doesPersonExist(people_models_, name)) { - people_models_[name].second = model; - continue; - } - // new model - assign an ID and update its ModelState - people_models_[name] = {people_models_.size(), model}; - ROS_INFO( - "Tracking a new person '%s' (ID: '%lu'), detected from naming pattern: '%s' (models)", - people_models_[name].second.model_name.c_str(), - people_models_[name].first, - pattern.c_str() - ); - } - - // possibly delete renamed/destroyed objects - deleteDestroyedPerson(people_models_, msg->name); -} - -void PeopleExtraction::gazeboLinkStateCallback(const gazebo_msgs::LinkStates::ConstPtr& msg) { - if (msg->name.size() != msg->pose.size() || msg->name.size() != msg->twist.size()) { - ROS_ERROR("Vector sizes in the input message are not equal, cannot process further"); - return; - } - std::lock_guard lock(mutex_ls_); - - // update internal database dynamically - size_t links_found = 0; - size_t links_total = msg->name.size(); - for (size_t i = 0; i < links_total; i++) { - // investigated object - obtain only the name at this stage - const auto& name = msg->name.at(i); - // check whether a model with a given name is of our interest (according to the naming patterns) - bool pattern_matched = false; - std::string pattern; - std::tie(pattern_matched, pattern) = isMatching(name, link_name_patterns_); - if (!pattern_matched) { - continue; - } - - links_found++; - gazebo_msgs::ModelState model; - model.model_name = name; - model.pose = msg->pose.at(i); - model.twist = msg->twist.at(i); - - // check whether the person already exists in the internal database (only need ModelState update then) - if (doesPersonExist(people_links_, name)) { - people_links_[name].second = model; - continue; - } - // new model - assign an ID and update its ModelState - people_links_[name] = {people_links_.size(), model}; - ROS_INFO( - "Tracking a new person '%s' (ID: '%lu'), detected from naming pattern: '%s' (links)", - people_links_[name].second.model_name.c_str(), - people_links_[name].first, - pattern.c_str() - ); - } - - // possibly delete renamed/destroyed objects - deleteDestroyedPerson(people_links_, msg->name); -} - -bool PeopleExtraction::doesPersonExist( - const std::map>& people, - const std::string& name -) const { - return people.find(name) != people.cend(); -} - -std::tuple PeopleExtraction::isMatching( - const std::string& model_name, - std::vector& patterns -) const { - for (const auto& pattern: patterns) { - // try to find - if (model_name.find(pattern) == std::string::npos) { - continue; - } - return {true, pattern}; - } - return {false, std::string()}; -} - -void PeopleExtraction::deleteDestroyedPerson( - std::map>& people, - const std::vector& model_names -) const { - auto i = std::begin(people); - // while is safer than for here - while (i != std::end(people)) { - // map and nested pair - auto model_name = i->second.second.model_name; - auto it = std::find(model_names.begin(), model_names.end(), model_name); - if (it != model_names.end()) { - // found - i++; - continue; - } - // delete current element - ROS_INFO("Deleting '%s' from database", model_name.c_str()); - // erase from map; ref: https://stackoverflow.com/a/2874533 - i = people.erase(i); - } -} - people_msgs_utils::People PeopleExtraction::gazeboModelsToPeople( const std::map>& people_models ) const { @@ -279,14 +147,11 @@ people_msgs_utils::People PeopleExtraction::gazeboModelsToPeople( } void PeopleExtraction::publish() { - std::lock_guard lock_ms(mutex_ms_); - std::lock_guard lock_ls(mutex_ls_); - // combine entities obtained from "models" and from "links" std::map> people_gazebo; // start with copying 1st and extend with the 2nd container - people_gazebo = people_models_; - for (auto const& [key, val]: people_links_) { + people_gazebo = model_extractor_->getPeople(); + for (auto const& [key, val]: link_extractor_->getPeople()) { people_gazebo[key] = val; } diff --git a/src/PeopleExtraction.hpp b/src/PeopleExtraction.hpp index d9de2fe..0366c02 100644 --- a/src/PeopleExtraction.hpp +++ b/src/PeopleExtraction.hpp @@ -26,55 +26,14 @@ #include +#include "GazeboExtractor.hpp" + class PeopleExtraction { public: PeopleExtraction(); virtual ~PeopleExtraction() = default; private: - /** - * @brief - * @note Callback triggered by a high frequency Gazebo topic - * @param msg - */ - void gazeboModelStateCallback(const gazebo_msgs::ModelStates::ConstPtr& msg); - - /** - * @brief - * @note Callback triggered by a high frequency Gazebo topic - * @param msg - */ - void gazeboLinkStateCallback(const gazebo_msgs::LinkStates::ConstPtr& msg); - - /** - * @brief Evaluates, whether the person exists in the internal database - * @param name: name of the person that is searched for - * @return - */ - bool doesPersonExist( - const std::map>& people, - const std::string& name - ) const; - - /** - * @brief Evaluates whether the given @ref model_name matches any pattern given by @ref patterns - * - * @param model_name - * @param patterns - * @return std::tuple - */ - std::tuple isMatching(const std::string& model_name, std::vector& patterns) const; - - /** - * @brief Get rid of the unnecessary model - * @param people people identified so far - * @param model_names names of entities recognized in the current step - */ - void deleteDestroyedPerson( - std::map>& people, - const std::vector& model_names - ) const; - /** * @brief Converts Gazebo ModelStates database to people representation in the @ref people_msgs_utils::People form * @@ -90,29 +49,20 @@ class PeopleExtraction { ros::NodeHandle nh_; + typedef GazeboExtractor GazeboModelExtractor; + typedef GazeboExtractor GazeboLinkExtractor; + // NOTE: unfortunately, ROS Message Filters do not work with gazebo_msgs-based topics as those messages + // do not carry timestamp values + std::unique_ptr model_extractor_; + std::unique_ptr link_extractor_; + std::atomic id_ref_; + tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - std::mutex mutex_ms_; - ros::Subscriber sub_model_states_; - std::mutex mutex_ls_; - ros::Subscriber sub_link_states_; - ros::Publisher pub_people_; ros::Publisher pub_pos_; - // NOTE: unfortunately, ROS Message Filters do not work with gazebo_msgs-based topics as those messages - // do not carry timestamp values - // - // key: name of the object in the simulation - // value: pair with object's ID and the Gazebo data structure - std::map> people_models_; - std::map> people_links_; - /// @brief Algorithm looks for a model name that meets one of the given patterns - std::vector model_name_patterns_; - /// @brief Same but for link states - std::vector link_name_patterns_; - std::string world_tf_frame_; std::string target_tf_frame_; From d8b97cc997637c8ed75583172c8b1d6cd0340cda Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sun, 18 Aug 2024 13:36:45 +0200 Subject: [PATCH 11/23] the main class extended with the `HuBeRo` agents extractor --- CMakeLists.txt | 4 + src/ActorLocalizationSubscriber.hpp | 65 +++++++++++++++ src/HuberoExtractor.cpp | 122 ++++++++++++++++++++++++++++ src/HuberoExtractor.hpp | 42 ++++++++++ src/PeopleExtraction.cpp | 86 +++++++++++++++++++- src/PeopleExtraction.hpp | 7 ++ 6 files changed, 325 insertions(+), 1 deletion(-) create mode 100644 src/ActorLocalizationSubscriber.hpp create mode 100644 src/HuberoExtractor.cpp create mode 100644 src/HuberoExtractor.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 986b671..d5a3b0f 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,6 +24,10 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") add_library(people_extraction src/PeopleExtraction.hpp src/PeopleExtraction.cpp + src/GazeboExtractor.hpp + src/ActorLocalizationSubscriber.hpp + src/HuberoExtractor.hpp + src/HuberoExtractor.cpp ) target_link_libraries(people_extraction diff --git a/src/ActorLocalizationSubscriber.hpp b/src/ActorLocalizationSubscriber.hpp new file mode 100644 index 0000000..785f764 --- /dev/null +++ b/src/ActorLocalizationSubscriber.hpp @@ -0,0 +1,65 @@ +#pragma once + +#include +#include + +#include +#include + +class ActorLocalizationSubscriber { +public: + /** + * @param nh node handle to use + * @param topic topic to subscribe odometry-localization messages + */ + ActorLocalizationSubscriber( + ros::NodeHandle& nh, + const size_t& actor_id, + const std::string& actor_name, + const std::string& topic + ): + id_(actor_id), + name_(actor_name), + topic_(topic) + { + sub_ = nh.subscribe( + topic_, + 10, + std::bind( + &ActorLocalizationSubscriber::callback, + this, + std::placeholders::_1 + ) + ); + } + + nav_msgs::Odometry getOdom() const { + std::lock_guard lock(mutex_); + return odom_; + } + + size_t getID() const { + return id_; + } + + std::string getName() const { + return name_; + } + + std::string getTopic() const { + return topic_; + } + +protected: + void callback(const nav_msgs::Odometry::ConstPtr& msg) { + std::lock_guard lock(mutex_); + odom_ = *msg; + } + + mutable std::mutex mutex_; + ros::Subscriber sub_; + nav_msgs::Odometry odom_; + size_t id_; + std::string name_; + std::string topic_; +}; diff --git a/src/HuberoExtractor.cpp b/src/HuberoExtractor.cpp new file mode 100644 index 0000000..c39f8b6 --- /dev/null +++ b/src/HuberoExtractor.cpp @@ -0,0 +1,122 @@ +#include "HuberoExtractor.hpp" + +#include + +HuberoExtractor::HuberoExtractor( + ros::NodeHandle& nh, + std::atomic& id_ref, + double discovery_frequency +): + nh_(nh), + id_ref_(id_ref) +{ + if (discovery_frequency > 0.0) { + timer_discovery_ = nh_.createTimer( + ros::Duration(1.0 / discovery_frequency), + std::bind( + &HuberoExtractor::refreshActorList, + this + ) + ); + } else { + ROS_WARN("Actor topics discovery will be performed only once!"); + refreshActorList(); + } +} + +void HuberoExtractor::refreshActorList() { + // Define the regex pattern that fits the parameter naming in the `hubero_ros` pkg launch - + // equivalent to `R"(/hubero_ros/.+/navigation/odometry_topic)"` + std::string prefix = "/hubero_ros/"; + std::string suffix = "/navigation/odometry_topic"; + + // Construct the regex pattern dynamically + auto regex_escape = [](const std::string& str) -> std::string { + static const std::regex re(R"([-[\]{}()*+?.,\^$|#\s])"); + return std::regex_replace(str, re, R"(\$&)"); + }; + std::string pattern_str = "^" + regex_escape(prefix) + "(.+)" + regex_escape(suffix) + "$"; + std::regex pattern(pattern_str); + + std::vector param_keys; + // access to, e.g., node handle, subscribers + std::lock_guard lock(mutex_); + nh_.getParamNames(param_keys); + + // stores IDs of actors and names of their localization-related topics + std::map param_keys_matched; + + // iterate through the vector and check for matches + for (const auto& key: param_keys) { + if (!std::regex_match(key, pattern)) { + continue; + } + // extract the substring with the actor ID by removing the prefix and suffix + std::string actor_name = key.substr( + prefix.size(), + key.size() - prefix.size() - suffix.size() + ); + // `key` defines the name of the parameter (key); topic name is stored as the value associated with that key + std::string topic; + if (!nh_.getParam(key, topic)) { + continue; + } + + // collect for further use + param_keys_matched[actor_name] = topic; + // assign an ID + detection_ids_[actor_name] = id_ref_.load(); + // increment the reference ID counter + id_ref_++; + } + + if (param_keys_matched.empty()) { + ROS_WARN_DELAYED_THROTTLE( + 30.0, + "Could not find any ROS Parameters that help finding HuBeRo actor topics. " + "Did you spawn actors using HuBeRo framework? " + "Also, note that this node does not work with bag files as ROS Parameter Server contents aren't captured." + ); + return; + } + + // check if already subscribed and (potentially) initiate subscribing the localization data + for (const auto& association: param_keys_matched) { + std::string actor_name = association.first; + std::string loc_topic = association.second; + bool already_handled = false; + for (const auto& sub: subscribers_) { + if (sub->getName() != actor_name) { + continue; + } + already_handled = true; + break; + } + if (already_handled) { + continue; + } + // obtain global ID of an actor + if (detection_ids_.find(actor_name) == detection_ids_.cend()) { + ROS_ERROR("Could not find an ID for an actor named `%s`", actor_name.c_str()); + continue; + } + auto actor_id = detection_ids_[actor_name]; + // create ActorLocalizationSubscriber + subscribers_.emplace_back( + std::make_unique( + nh_, + actor_id, + actor_name, + loc_topic + ) + ); + ROS_INFO( + "HuBeRo extractor discovered `%s`'s (ID=%lu) localization topic at `%s`!", + actor_name.c_str(), + actor_id, + loc_topic.c_str() + ); + } + + // TODO: handle deletion +} diff --git a/src/HuberoExtractor.hpp b/src/HuberoExtractor.hpp new file mode 100644 index 0000000..dece4b0 --- /dev/null +++ b/src/HuberoExtractor.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include "ActorLocalizationSubscriber.hpp" + +class HuberoExtractor { +public: + HuberoExtractor( + ros::NodeHandle& nh, + std::atomic& id_ref, + double discovery_frequency + ); + + /// Refreshes the list of relevant HuBeRo parameters and updates the list of @ref subscribers_ + void refreshActorList(); + + // Returns a list of data collecting instances + inline const std::vector>& get() const { + std::lock_guard lock(mutex_); + return subscribers_; + } + +protected: + ros::NodeHandle& nh_; + + // Vector of pointers due to the fact that stored objects contain non-copyable mutexes + std::vector> subscribers_; + + // For assigning IDs between multiple classes + std::atomic& id_ref_; + + std::map detection_ids_; + ros::Timer timer_discovery_; + mutable std::mutex mutex_; +}; diff --git a/src/PeopleExtraction.cpp b/src/PeopleExtraction.cpp index eb6825e..59396e4 100644 --- a/src/PeopleExtraction.cpp +++ b/src/PeopleExtraction.cpp @@ -16,6 +16,7 @@ PeopleExtraction::PeopleExtraction(): id_ref_(0), tf_listener_(tf_buffer_) { + auto discovery_frequency = nh_.param("discovery_frequency", 0.5); auto pub_frequency = nh_.param("pub_frequency", 30.0); nh_.param("world_frame", world_tf_frame_, "world"); nh_.param("target_frame", target_tf_frame_, "odom"); @@ -57,6 +58,11 @@ PeopleExtraction::PeopleExtraction(): link_name_patterns, id_ref_ ); + hubero_extractor_ = std::make_unique( + nh_, + id_ref_, + discovery_frequency + ); pub_people_ = nh_.advertise("/people", 5); pub_pos_ = nh_.advertise("/people_measurements", 5); @@ -146,6 +152,77 @@ people_msgs_utils::People PeopleExtraction::gazeboModelsToPeople( return people; } +people_msgs_utils::People PeopleExtraction::huberoActorsToPeople( + const std::vector>& actors +) const { + // prepare output container + people_msgs_utils::People people; + + // access to, e.g., TF buffer, subscribers + for (const auto& sub: actors) { + auto odom = sub->getOdom(); + + // transform localization data to the desired frame + geometry_msgs::TransformStamped tf_stamped; + // by default - lack of translation and rotation + tf_stamped.transform.rotation.w = 1.0; + + // obtain the transform to the desired frame + try { + tf_stamped = tf_buffer_.lookupTransform( + target_tf_frame_, + odom.header.frame_id, + ros::Time(0) + ); + } catch (tf2::TransformException& ex) { + ROS_ERROR( + "Could not transform `%s`'s pose! Exception details: `%s`", + sub->getName().c_str(), + ex.what() + ); + // avoid flooding console with this kind of errors (localization might not be operational yet) + std::this_thread::sleep_for(std::chrono::seconds(1)); + return people; + } + + // convert data from twist to pose representation (contents are the same) + geometry_msgs::PoseWithCovariance odom_vel; + odom_vel.covariance = odom.twist.covariance; + odom_vel.pose.position.x = odom.twist.twist.linear.x; + odom_vel.pose.position.y = odom.twist.twist.linear.y; + odom_vel.pose.position.z = odom.twist.twist.linear.z; + tf2::Quaternion angular_vel; + // NOTE: for a proper yaw angle, yaw, pitch and roll angles must be reordered (compared to the documentation) + angular_vel.setEuler( + odom.twist.twist.angular.x, + odom.twist.twist.angular.y, + odom.twist.twist.angular.z + ); + odom_vel.pose.orientation = tf2::toMsg(angular_vel); + + // create a Person instance (with geom. data in the source frame) + people_msgs_utils::Person p( + std::to_string(sub->getID()), // avoid using 'string'-type names here + odom.pose, + odom_vel, + 1.0, // complete reliability + false, + true, + sub->getID(), + ros::Time::now().toNSec(), // track age = since startup as we have ideal data + std::string("") // group data not included + ); + // TODO: include group data (e.g., detect "follow" task execution) + + // transform to the target frame + p.transform(tf_stamped); + + // add to the aggregated container + people.push_back(p); + } + return people; +} + void PeopleExtraction::publish() { // combine entities obtained from "models" and from "links" std::map> people_gazebo; @@ -155,13 +232,20 @@ void PeopleExtraction::publish() { people_gazebo[key] = val; } + // HuBeRo actors + const auto& people_hubero = hubero_extractor_->get(); + // check if there is something to publish - if (people_gazebo.empty()) { + if (people_gazebo.empty() && people_hubero.empty()) { return; } // Gazebo models to people representation auto people = gazeboModelsToPeople(people_gazebo); + // HuBeRo actors to people + for (const auto& person: huberoActorsToPeople(people_hubero)) { + people.push_back(person); + } publishPeople(people); publishPeoplePositions(people); diff --git a/src/PeopleExtraction.hpp b/src/PeopleExtraction.hpp index 0366c02..b9da449 100644 --- a/src/PeopleExtraction.hpp +++ b/src/PeopleExtraction.hpp @@ -27,6 +27,7 @@ #include #include "GazeboExtractor.hpp" +#include "HuberoExtractor.hpp" class PeopleExtraction { public: @@ -43,6 +44,10 @@ class PeopleExtraction { const std::map>& people_models ) const; + people_msgs_utils::People huberoActorsToPeople( + const std::vector>& actors + ) const; + void publish(); void publishPeople(const people_msgs_utils::People& people); void publishPeoplePositions(const people_msgs_utils::People& people); @@ -55,6 +60,8 @@ class PeopleExtraction { // do not carry timestamp values std::unique_ptr model_extractor_; std::unique_ptr link_extractor_; + // HuBeRo framework-specific extractor + std::unique_ptr hubero_extractor_; std::atomic id_ref_; tf2_ros::Buffer tf_buffer_; From 1b852d927018ce996198e9c28d9b5a8b4518afe7 Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sun, 18 Aug 2024 13:37:50 +0200 Subject: [PATCH 12/23] added note in the example YAML config --- config/people.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/config/people.yaml b/config/people.yaml index ca31cf1..b2b53b4 100644 --- a/config/people.yaml +++ b/config/people.yaml @@ -1,3 +1,4 @@ +# The example configuration fits https://github.com/aws-robotics/aws-robomaker-hospital-world model_name_patterns: [ "actor", ] From 53551bf68c542955bc4c346d7b0d9c5a2008c997 Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sun, 18 Aug 2024 13:38:43 +0200 Subject: [PATCH 13/23] added `inline` keyword in the `GazeboExtractor` --- src/GazeboExtractor.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/GazeboExtractor.hpp b/src/GazeboExtractor.hpp index f221e95..e822e0e 100644 --- a/src/GazeboExtractor.hpp +++ b/src/GazeboExtractor.hpp @@ -35,7 +35,7 @@ class GazeboExtractor { ); } - std::map> getPeople() const { + inline std::map> getPeople() const { std::lock_guard lock(mutex_); return people_; } From e5242907e9ad7134141230be3a623b48ad151d79 Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sun, 18 Aug 2024 13:40:15 +0200 Subject: [PATCH 14/23] added guards for valid TF frame names --- src/PeopleExtraction.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/PeopleExtraction.cpp b/src/PeopleExtraction.cpp index 59396e4..4d3031c 100644 --- a/src/PeopleExtraction.cpp +++ b/src/PeopleExtraction.cpp @@ -21,6 +21,16 @@ PeopleExtraction::PeopleExtraction(): nh_.param("world_frame", world_tf_frame_, "world"); nh_.param("target_frame", target_tf_frame_, "odom"); + // TF frame names validity check + if (world_tf_frame_.empty()) { + ROS_ERROR("Cannot run HuBeRo actors data publisher, as the `target_frame` parameter is empty!"); + return; + } + if (target_tf_frame_.empty()) { + ROS_ERROR("Cannot run HuBeRo actors data publisher, as the `target_frame` parameter is empty!"); + return; + } + // obtain model and link name patterns std::vector model_name_patterns; nh_.getParam("model_name_patterns", model_name_patterns); From e8ecb75147c0da2ad2d1fddea7a4aef480941b06 Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sun, 18 Aug 2024 13:41:38 +0200 Subject: [PATCH 15/23] `GazeboModelExtractor` and `GazeboLinkExtractor` created only when relevant naming patterns are properly defined --- src/PeopleExtraction.cpp | 81 +++++++++++++++++++++++++--------------- 1 file changed, 51 insertions(+), 30 deletions(-) diff --git a/src/PeopleExtraction.cpp b/src/PeopleExtraction.cpp index 4d3031c..d70c483 100644 --- a/src/PeopleExtraction.cpp +++ b/src/PeopleExtraction.cpp @@ -21,6 +21,24 @@ PeopleExtraction::PeopleExtraction(): nh_.param("world_frame", world_tf_frame_, "world"); nh_.param("target_frame", target_tf_frame_, "odom"); + // publishing frequency check + if (pub_frequency <= 0.0) { + ROS_ERROR( + "Simulator-based people data will not be published as wrong frequency was selected: %f Hz", + pub_frequency + ); + return; + } + // TF frame names validity check + if (world_tf_frame_.empty()) { + ROS_ERROR("Cannot run HuBeRo actors data publisher, as the `target_frame` parameter is empty!"); + return; + } + if (target_tf_frame_.empty()) { + ROS_ERROR("Cannot run HuBeRo actors data publisher, as the `target_frame` parameter is empty!"); + return; + } + // TF frame names validity check if (world_tf_frame_.empty()) { ROS_ERROR("Cannot run HuBeRo actors data publisher, as the `target_frame` parameter is empty!"); @@ -35,39 +53,31 @@ PeopleExtraction::PeopleExtraction(): std::vector model_name_patterns; nh_.getParam("model_name_patterns", model_name_patterns); if (!model_name_patterns.size()) { - ROS_ERROR("PeopleExtraction - model_name_patterns is empty! Class would be ill-formed"); - return; + ROS_WARN("Gazebo people model extractor will not be formed as the 'model_name_patterns' parameter is empty"); + } else { + // create a ROS interface for extracting model states + model_extractor_ = std::make_unique( + nh_, + "/gazebo/model_states", + model_name_patterns, + id_ref_ + ); } + std::vector link_name_patterns; nh_.getParam("link_name_patterns", link_name_patterns); if (!link_name_patterns.size()) { - ROS_ERROR("PeopleExtraction - link_name_patterns is empty! Class would be ill-formed"); - return; - } - - // publishing frequency check - if (pub_frequency <= 0.0) { - ROS_ERROR( - "Simulator-based people data will not be published as wrong frequency was selected: %f Hz", - pub_frequency + ROS_WARN("Gazebo people link extractor will not be formed as the 'link_name_patterns' parameter is empty"); + } else { + // create a ROS interface for extracting link states + link_extractor_ = std::make_unique( + nh_, + "/gazebo/link_states", + link_name_patterns, + id_ref_ ); - return; } - // create ROS interfaces - model_extractor_ = std::make_unique( - nh_, - "/gazebo/model_states", - model_name_patterns, - id_ref_ - ); - - link_extractor_ = std::make_unique( - nh_, - "/gazebo/link_states", - link_name_patterns, - id_ref_ - ); hubero_extractor_ = std::make_unique( nh_, id_ref_, @@ -234,12 +244,23 @@ people_msgs_utils::People PeopleExtraction::huberoActorsToPeople( } void PeopleExtraction::publish() { + // helper function + auto extendMap = []( + std::map>& dest, + const std::map>& src + ) { + for (auto const& [key, val]: src) { + dest[key] = val; + } + }; + // combine entities obtained from "models" and from "links" std::map> people_gazebo; - // start with copying 1st and extend with the 2nd container - people_gazebo = model_extractor_->getPeople(); - for (auto const& [key, val]: link_extractor_->getPeople()) { - people_gazebo[key] = val; + if (model_extractor_) { + extendMap(people_gazebo, model_extractor_->getPeople()); + } + if (link_extractor_) { + extendMap(people_gazebo, link_extractor_->getPeople()); } // HuBeRo actors From 1c42248832cc86e53058d63e892e8c0bf3eae0d0 Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sun, 18 Aug 2024 13:44:07 +0200 Subject: [PATCH 16/23] unified formatting --- src/GazeboExtractor.hpp | 210 ++++++++++++++++----------------- src/people_extraction_node.cpp | 13 +- 2 files changed, 110 insertions(+), 113 deletions(-) diff --git a/src/GazeboExtractor.hpp b/src/GazeboExtractor.hpp index e822e0e..27dc525 100644 --- a/src/GazeboExtractor.hpp +++ b/src/GazeboExtractor.hpp @@ -18,27 +18,27 @@ template class GazeboExtractor { public: - GazeboExtractor( - ros::NodeHandle& nh, - const std::string& topic_name, - const std::vector& name_patterns, - std::atomic& id_ref - ): - name_patterns_(name_patterns), - id_ref_(id_ref) - { - sub_ = nh.subscribe( - topic_name, - 1, - &GazeboExtractor::callback, - this - ); - } - - inline std::map> getPeople() const { - std::lock_guard lock(mutex_); - return people_; - } + GazeboExtractor( + ros::NodeHandle& nh, + const std::string& topic_name, + const std::vector& name_patterns, + std::atomic& id_ref + ): + name_patterns_(name_patterns), + id_ref_(id_ref) + { + sub_ = nh.subscribe( + topic_name, + 1, + &GazeboExtractor::callback, + this + ); + } + + inline std::map> getPeople() const { + std::lock_guard lock(mutex_); + return people_; + } protected: /** @@ -47,62 +47,62 @@ class GazeboExtractor { * @param msg */ void callback(const Tcb& msg) { - if (msg->name.size() != msg->pose.size() || msg->name.size() != msg->twist.size()) { - ROS_ERROR("Vector sizes in the input message are not equal, cannot process further"); - return; - } - std::lock_guard lock(mutex_); - - // update internal database dynamically - size_t models_found = 0; - size_t models_total = msg->name.size(); - for (size_t i = 0; i < models_total; i++) { - // investigated object - obtain only the name at this stage - const auto& name = msg->name.at(i); - // check whether a model with a given name is of our interest (according to the naming patterns) - bool pattern_matched = false; - std::string pattern; - - std::tie(pattern_matched, pattern) = isMatching(name, name_patterns_); - if (!pattern_matched) { - continue; - } - - models_found++; - Tcontainer model; - model.model_name = name; - model.pose = msg->pose.at(i); - model.twist = msg->twist.at(i); - - // check whether the person already exists in the internal database (only need ModelState update then) - if (doesPersonExist(name)) { - people_[name].second = model; - continue; - } - // new model - assign an ID and update its ModelState - people_[name] = {id_ref_.load(), model}; - // increment the reference ID counter - id_ref_++; - ROS_INFO( - "Tracking a new person '%s' (ID: '%lu'), detected from naming pattern: '%s'", - people_[name].second.model_name.c_str(), - people_[name].first, - pattern.c_str() - ); - } - - // possibly delete renamed/destroyed objects - deleteDestroyedPerson(msg->name); - } - - /** + if (msg->name.size() != msg->pose.size() || msg->name.size() != msg->twist.size()) { + ROS_ERROR("Vector sizes in the input message are not equal, cannot process further"); + return; + } + std::lock_guard lock(mutex_); + + // update internal database dynamically + size_t models_found = 0; + size_t models_total = msg->name.size(); + for (size_t i = 0; i < models_total; i++) { + // investigated object - obtain only the name at this stage + const auto& name = msg->name.at(i); + // check whether a model with a given name is of our interest (according to the naming patterns) + bool pattern_matched = false; + std::string pattern; + + std::tie(pattern_matched, pattern) = isMatching(name, name_patterns_); + if (!pattern_matched) { + continue; + } + + models_found++; + Tcontainer model; + model.model_name = name; + model.pose = msg->pose.at(i); + model.twist = msg->twist.at(i); + + // check whether the person already exists in the internal database (only need ModelState update then) + if (doesPersonExist(name)) { + people_[name].second = model; + continue; + } + // new model - assign an ID and update its ModelState + people_[name] = {id_ref_.load(), model}; + // increment the reference ID counter + id_ref_++; + ROS_INFO( + "Tracking a new person '%s' (ID: '%lu'), detected from naming pattern: '%s'", + people_[name].second.model_name.c_str(), + people_[name].first, + pattern.c_str() + ); + } + + // possibly delete renamed/destroyed objects + deleteDestroyedPerson(msg->name); + } + + /** * @brief Evaluates, whether the person exists in the internal database * @param name: name of the person that is searched for * @return */ bool doesPersonExist(const std::string& name) const { - return people_.find(name) != people_.cend(); - } + return people_.find(name) != people_.cend(); + } /** * @brief Evaluates whether the given @ref model_name matches any pattern given by @ref patterns @@ -112,18 +112,18 @@ class GazeboExtractor { * @return std::tuple */ std::tuple isMatching( - const std::string& model_name, - std::vector& patterns - ) const { - for (const auto& pattern: patterns) { - // try to find - if (model_name.find(pattern) == std::string::npos) { - continue; - } - return {true, pattern}; - } - return {false, std::string()}; - } + const std::string& model_name, + std::vector& patterns + ) const { + for (const auto& pattern: patterns) { + // try to find + if (model_name.find(pattern) == std::string::npos) { + continue; + } + return {true, pattern}; + } + return {false, std::string()}; + } /** * @brief Get rid of the unnecessary model @@ -131,32 +131,32 @@ class GazeboExtractor { * @param model_names names of entities recognized in the current step */ void deleteDestroyedPerson(const std::vector& model_names) { - auto i = std::begin(people_); - // while is safer than for here - while (i != std::end(people_)) { - // map and nested pair - auto model_name = i->second.second.model_name; - auto it = std::find(model_names.begin(), model_names.end(), model_name); - if (it != model_names.end()) { - // found - i++; - continue; - } - // delete current element - ROS_INFO("Deleting '%s' from database", model_name.c_str()); - // erase from map; ref: https://stackoverflow.com/a/2874533 - i = people_.erase(i); - } - } + auto i = std::begin(people_); + // while is safer than for here + while (i != std::end(people_)) { + // map and nested pair + auto model_name = i->second.second.model_name; + auto it = std::find(model_names.begin(), model_names.end(), model_name); + if (it != model_names.end()) { + // found + i++; + continue; + } + // delete current element + ROS_INFO("Deleting '%s' from database", model_name.c_str()); + // erase from map; ref: https://stackoverflow.com/a/2874533 + i = people_.erase(i); + } + } ros::Subscriber sub_; - mutable std::mutex mutex_; + mutable std::mutex mutex_; - /// @brief Algorithm looks for a model name that meets one of the given patterns + /// @brief Algorithm looks for a model name that meets one of the given patterns std::vector name_patterns_; - // For assigning IDs between multiple classes - std::atomic& id_ref_; + // For assigning IDs between multiple classes + std::atomic& id_ref_; // key: name of the object in the simulation // value: pair with object's ID and the Gazebo data structure diff --git a/src/people_extraction_node.cpp b/src/people_extraction_node.cpp index bb8aba0..69954fe 100644 --- a/src/people_extraction_node.cpp +++ b/src/people_extraction_node.cpp @@ -10,12 +10,9 @@ #include #include "PeopleExtraction.hpp" -int main(int argc, char** argv) -{ - - ros::init(argc, argv, "people_extraction"); - PeopleExtraction legs; - ros::spin(); - return 0; - +int main(int argc, char** argv) { + ros::init(argc, argv, "people_extraction"); + PeopleExtraction extraction; + ros::spin(); + return 0; } From bbc56805f88903bf7b24d9b1a5f4194b89d6f286 Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sun, 18 Aug 2024 15:02:25 +0200 Subject: [PATCH 17/23] additional TFs are set up to be applied for certain model classes in `GazeboExtractor` --- config/people.yaml | 15 +++++++++++++++ src/GazeboExtractor.hpp | 35 +++++++++++++++++++++++++++++++++++ src/PeopleExtraction.cpp | 39 +++++++++++++++++++++++++++++++++++++-- src/PeopleExtraction.hpp | 5 +++++ 4 files changed, 92 insertions(+), 2 deletions(-) diff --git a/config/people.yaml b/config/people.yaml index b2b53b4..5eb9f1f 100644 --- a/config/people.yaml +++ b/config/people.yaml @@ -2,6 +2,7 @@ model_name_patterns: [ "actor", ] + link_name_patterns: [ "Scrubs", "PatientWheelChair", @@ -9,3 +10,17 @@ link_name_patterns: [ "VisitorKidSit", "FemaleVisitorSit" ] + +# Additional transforms to be applied for some specific models +transforms: + # the key here must match the "name patterns" above + actor: [ + # transform is applied to elements one by one; + # the order of elements is: x, y, z, roll, pitch, yaw + +0.0, + +0.0, + -1.03, + -1.5708, + +0.0, + -1.5708 + ] diff --git a/src/GazeboExtractor.hpp b/src/GazeboExtractor.hpp index 27dc525..3f11c9a 100644 --- a/src/GazeboExtractor.hpp +++ b/src/GazeboExtractor.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -22,9 +23,11 @@ class GazeboExtractor { ros::NodeHandle& nh, const std::string& topic_name, const std::vector& name_patterns, + const std::map>& transforms, std::atomic& id_ref ): name_patterns_(name_patterns), + transforms_(transforms), id_ref_(id_ref) { sub_ = nh.subscribe( @@ -74,6 +77,37 @@ class GazeboExtractor { model.pose = msg->pose.at(i); model.twist = msg->twist.at(i); + // check whether additional transform is required for this entity + if (transforms_.find(pattern) != transforms_.cend()) { + auto tf_vector = transforms_[pattern]; + model.pose.position.x += tf_vector.at(0); + model.pose.position.y += tf_vector.at(1); + model.pose.position.z += tf_vector.at(2); + + // original quaternion to roll pitch yaw + tf::Quaternion q( + model.pose.orientation.x, + model.pose.orientation.y, + model.pose.orientation.z, + model.pose.orientation.w + ); + tf::Matrix3x3 m(q); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + // apply transformation as RPY + roll += tf_vector.at(3); + pitch += tf_vector.at(4); + yaw += tf_vector.at(5); + // convert back to quaternion + tf::Quaternion q_out; + q_out.setRPY(roll, pitch, yaw); + + model.pose.orientation.x = q_out.x(); + model.pose.orientation.y = q_out.y(); + model.pose.orientation.z = q_out.z(); + model.pose.orientation.w = q_out.w(); + } + // check whether the person already exists in the internal database (only need ModelState update then) if (doesPersonExist(name)) { people_[name].second = model; @@ -154,6 +188,7 @@ class GazeboExtractor { /// @brief Algorithm looks for a model name that meets one of the given patterns std::vector name_patterns_; + std::map> transforms_; // For assigning IDs between multiple classes std::atomic& id_ref_; diff --git a/src/PeopleExtraction.cpp b/src/PeopleExtraction.cpp index d70c483..d06ead4 100644 --- a/src/PeopleExtraction.cpp +++ b/src/PeopleExtraction.cpp @@ -51,7 +51,14 @@ PeopleExtraction::PeopleExtraction(): // obtain model and link name patterns std::vector model_name_patterns; + std::vector link_name_patterns; nh_.getParam("model_name_patterns", model_name_patterns); + nh_.getParam("link_name_patterns", link_name_patterns); + + // obtain additional transformations (patterns from above are needed) + auto models_transforms = discoverTransforms(model_name_patterns); + auto links_transforms = discoverTransforms(link_name_patterns); + if (!model_name_patterns.size()) { ROS_WARN("Gazebo people model extractor will not be formed as the 'model_name_patterns' parameter is empty"); } else { @@ -60,12 +67,11 @@ PeopleExtraction::PeopleExtraction(): nh_, "/gazebo/model_states", model_name_patterns, + models_transforms, id_ref_ ); } - std::vector link_name_patterns; - nh_.getParam("link_name_patterns", link_name_patterns); if (!link_name_patterns.size()) { ROS_WARN("Gazebo people link extractor will not be formed as the 'link_name_patterns' parameter is empty"); } else { @@ -74,6 +80,7 @@ PeopleExtraction::PeopleExtraction(): nh_, "/gazebo/link_states", link_name_patterns, + links_transforms, id_ref_ ); } @@ -96,6 +103,34 @@ PeopleExtraction::PeopleExtraction(): ); } +std::map> PeopleExtraction::discoverTransforms( + const std::vector& name_patterns +) const { + std::map> transforms; + for (const auto& pattern: name_patterns) { + std::string param = "transforms/" + pattern; + std::vector transform; + if (!nh_.getParam(param, transform)) { + continue; + } + if (transform.size() != 6) { + ROS_ERROR( + "Discovered additional transform for '%s' pattern but it has %lu elements, while 6 are required", + pattern.c_str(), + transform.size() + ); + continue; + } + transforms[pattern] = transform; + ROS_INFO( + "Discovered additional transform for '%s' pattern with %lu elements\r\n", + pattern.c_str(), + transform.size() + ); + } + return transforms; +} + people_msgs_utils::People PeopleExtraction::gazeboModelsToPeople( const std::map>& people_models ) const { diff --git a/src/PeopleExtraction.hpp b/src/PeopleExtraction.hpp index b9da449..7fab812 100644 --- a/src/PeopleExtraction.hpp +++ b/src/PeopleExtraction.hpp @@ -35,6 +35,11 @@ class PeopleExtraction { virtual ~PeopleExtraction() = default; private: + /// Searches through parameters to obtain a simple TF to be applied once reporting localization data + std::map> discoverTransforms( + const std::vector& name_patterns + ) const; + /** * @brief Converts Gazebo ModelStates database to people representation in the @ref people_msgs_utils::People form * From 8a7f0fcd486d2f84f38d55430aeb01a4c43407ed Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sun, 18 Aug 2024 15:05:53 +0200 Subject: [PATCH 18/23] updated README --- README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index c4e8190..e231d7a 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,5 @@ # people_extraction -ROS package that searches for certain model names at [`/gazebo/model_states`](http://docs.ros.org/en/kinetic/api/gazebo_msgs/html/msg/ModelStates.html) topic and republishes gathered data as [`people_msgs/People.msg`](http://docs.ros.org/en/api/people_msgs/html/msg/People.html) and [`people_msgs/PositionMeasurementArray.msg`](http://docs.ros.org/en/api/people_msgs/html/msg/PositionMeasurementArray.html) to ROS topics with a given names. -Tested under Ubuntu 16.04 and ROS Kinetic. +ROS package that searches for certain model names at [`/gazebo/model_states`](http://docs.ros.org/en/melodic/api/gazebo_msgs/html/msg/ModelStates.html) and certain link names at [`/gazebo/link_states`](http://docs.ros.org/en/melodic/api/gazebo_msgs/html/msg/LinkStates.html) topics and republishes gathered data as [`people_msgs/People.msg`](http://docs.ros.org/en/api/people_msgs/html/msg/People.html) and [`people_msgs/PositionMeasurementArray.msg`](http://docs.ros.org/en/api/people_msgs/html/msg/PositionMeasurementArray.html) to ROS topics with given names. + +Tested under Ubuntu 18.04 and ROS Melodic. From 425c1d722cc034ebcaa4af06c426108bf20865ea Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sun, 18 Aug 2024 20:02:11 +0200 Subject: [PATCH 19/23] `GazeboExtractor` - manual velocity calculation implemented (with simple filtering) --- CMakeLists.txt | 2 + package.xml | 1 + src/GazeboExtractor.hpp | 167 ++++++++++++++++++++++++++++++++------- src/PeopleExtraction.cpp | 16 +++- 4 files changed, 154 insertions(+), 32 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d5a3b0f..2632d26 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp people_msgs people_msgs_utils + angles ) catkin_package( @@ -14,6 +15,7 @@ catkin_package( roscpp people_msgs people_msgs_utils + angles ) set(CMAKE_CXX_FLAGS "-Wall -Wpedantic --std=c++17") diff --git a/package.xml b/package.xml index 567c3bd..8425465 100755 --- a/package.xml +++ b/package.xml @@ -13,5 +13,6 @@ gazebo_ros people_msgs people_msgs_utils + angles diff --git a/src/GazeboExtractor.hpp b/src/GazeboExtractor.hpp index 3f11c9a..bbc3548 100644 --- a/src/GazeboExtractor.hpp +++ b/src/GazeboExtractor.hpp @@ -3,10 +3,13 @@ #include #include +#include + #include #include #include #include +#include #include /** @@ -19,16 +22,25 @@ template class GazeboExtractor { public: + /// For velocity filtering, how much we trust new observations, i.e., how much we are relying on new readings + static constexpr auto COMPLEMENTARY_FILTER_INNOVATION_DEFAULT = 0.2; + static constexpr auto GAZEBO_CB_DIFF_TIME = 0.05; + GazeboExtractor( ros::NodeHandle& nh, const std::string& topic_name, const std::vector& name_patterns, const std::map>& transforms, - std::atomic& id_ref + std::atomic& id_ref, + double comp_filter_innovation = COMPLEMENTARY_FILTER_INNOVATION_DEFAULT, + double cb_time_diff = GAZEBO_CB_DIFF_TIME ): name_patterns_(name_patterns), transforms_(transforms), - id_ref_(id_ref) + id_ref_(id_ref), + time_last_update_(NAN), + comp_filter_innovation_(comp_filter_innovation), + cb_time_diff_(cb_time_diff) { sub_ = nh.subscribe( topic_name, @@ -50,10 +62,24 @@ class GazeboExtractor { * @param msg */ void callback(const Tcb& msg) { + if (std::isnan(time_last_update_)) { + // omit the first callback + time_last_update_ = ros::Time::now().toSec(); + } + if (msg->name.size() != msg->pose.size() || msg->name.size() != msg->twist.size()) { ROS_ERROR("Vector sizes in the input message are not equal, cannot process further"); return; } + + // check timing conditions (manual calculation of velocities) + double time_current = ros::Time::now().toSec(); + double time_diff = time_current - time_last_update_; + if (time_diff < cb_time_diff_) { + return; + } + time_last_update_ = time_current; + std::lock_guard lock(mutex_); // update internal database dynamically @@ -77,42 +103,28 @@ class GazeboExtractor { model.pose = msg->pose.at(i); model.twist = msg->twist.at(i); - // check whether additional transform is required for this entity + std::vector transform; if (transforms_.find(pattern) != transforms_.cend()) { - auto tf_vector = transforms_[pattern]; - model.pose.position.x += tf_vector.at(0); - model.pose.position.y += tf_vector.at(1); - model.pose.position.z += tf_vector.at(2); - - // original quaternion to roll pitch yaw - tf::Quaternion q( - model.pose.orientation.x, - model.pose.orientation.y, - model.pose.orientation.z, - model.pose.orientation.w - ); - tf::Matrix3x3 m(q); - double roll, pitch, yaw; - m.getRPY(roll, pitch, yaw); - // apply transformation as RPY - roll += tf_vector.at(3); - pitch += tf_vector.at(4); - yaw += tf_vector.at(5); - // convert back to quaternion - tf::Quaternion q_out; - q_out.setRPY(roll, pitch, yaw); - - model.pose.orientation.x = q_out.x(); - model.pose.orientation.y = q_out.y(); - model.pose.orientation.z = q_out.z(); - model.pose.orientation.w = q_out.w(); + transform = transforms_[pattern]; + } else { + transform = std::vector{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; } // check whether the person already exists in the internal database (only need ModelState update then) if (doesPersonExist(name)) { + // manually compute the new state a some Gazebo model might report wrong velocities + model = computeState( + people_[name].second, // previously saved "model" + model, + transform, + time_diff + ); people_[name].second = model; continue; } + + // special stage for a proper initialization + std::tie(model, std::ignore, std::ignore, std::ignore) = computeStateTransformed(model, transform); // new model - assign an ID and update its ModelState people_[name] = {id_ref_.load(), model}; // increment the reference ID counter @@ -129,6 +141,95 @@ class GazeboExtractor { deleteDestroyedPerson(msg->name); } + /// @brief Returns instance of desired type and R, P, Y angles + std::tuple computeStateTransformed( + const Tcontainer& state, + const std::vector& transform + ) const { + Tcontainer state_corrected; + // copy name first + state_corrected.model_name = state.model_name; + + state_corrected.pose.position.x = state.pose.position.x + transform.at(0); + state_corrected.pose.position.y = state.pose.position.y + transform.at(1); + state_corrected.pose.position.z = state.pose.position.z + transform.at(2); + // original quaternion to roll pitch yaw + tf::Quaternion q_raw( + state.pose.orientation.x, + state.pose.orientation.y, + state.pose.orientation.z, + state.pose.orientation.w + ); + tf::Matrix3x3 m_raw(q_raw); + double roll, pitch, yaw; + m_raw.getRPY(roll, pitch, yaw); + // apply transformation as RPY + roll = angles::normalize_angle(roll + transform.at(3)); + pitch = angles::normalize_angle(pitch + transform.at(4)); + yaw = angles::normalize_angle(yaw + transform.at(5)); + // convert back to quaternion - body orientation in the current step + tf::Quaternion q; + q.setRPY(roll, pitch, yaw); + + state_corrected.pose.orientation.x = q.x(); + state_corrected.pose.orientation.y = q.y(); + state_corrected.pose.orientation.z = q.z(); + state_corrected.pose.orientation.w = q.w(); + return {state_corrected, roll, pitch, yaw}; + } + + Tcontainer computeState( + const Tcontainer& state_prev, + const Tcontainer& state_curr_raw, + const std::vector& transform, + double time_diff + ) const { + // first, we need to apply the requested transformation to switch from `state_curr_raw` to `state_curr` + Tcontainer state_curr; + double roll_curr, pitch_curr, yaw_curr; + std::tie(state_curr, roll_curr, pitch_curr, yaw_curr) = computeStateTransformed(state_curr_raw, transform); + + // filtering factors + double ff_old = 1.0 - comp_filter_innovation_; + double ff_new = comp_filter_innovation_; + + // linear velocities (twists) + double tlx = ff_old * state_prev.twist.linear.x + ff_new * (state_curr.pose.position.x - state_prev.pose.position.x) / time_diff; + double tly = ff_old * state_prev.twist.linear.y + ff_new * (state_curr.pose.position.y - state_prev.pose.position.y) / time_diff; + double tlz = ff_old * state_prev.twist.linear.z + ff_new * (state_curr.pose.position.z - state_prev.pose.position.z) / time_diff; + + // body orientation in the previous step + tf::Quaternion q_prev( + state_prev.pose.orientation.x, + state_prev.pose.orientation.y, + state_prev.pose.orientation.z, + state_prev.pose.orientation.w + ); + tf::Matrix3x3 mat_q_prev(q_prev); + double roll_prev, pitch_prev, yaw_prev; + mat_q_prev.getRPY(roll_prev, pitch_prev, yaw_prev); + + // angular velocities + double tax = ff_old * state_prev.twist.angular.x + ff_new * (roll_curr - roll_prev) / time_diff; + double tay = ff_old * state_prev.twist.angular.y + ff_new * (pitch_curr - pitch_prev) / time_diff; + double taz = ff_old * state_prev.twist.angular.z + ff_new * (yaw_curr - yaw_prev) / time_diff; + + // updated state + Tcontainer state; + // copy name + state.model_name = state_curr_raw.model_name; + // pose is directly inherited from the simulator + state.pose = state_curr.pose; + // twist calculated "manually" + state.twist.linear.x = tlx; + state.twist.linear.y = tly; + state.twist.linear.z = tlz; + state.twist.angular.x = tax; + state.twist.angular.y = tay; + state.twist.angular.z = taz; + return state; + }; + /** * @brief Evaluates, whether the person exists in the internal database * @param name: name of the person that is searched for @@ -196,4 +297,10 @@ class GazeboExtractor { // key: name of the object in the simulation // value: pair with object's ID and the Gazebo data structure std::map> people_; + /// ROS time of the last update stage + double time_last_update_; + /// Innovation factor of the complementary filter for smoothing velocities + double comp_filter_innovation_; + /// How often (in ROS time), Gazebo callback should be processed + double cb_time_diff_; }; diff --git a/src/PeopleExtraction.cpp b/src/PeopleExtraction.cpp index d06ead4..19b5958 100644 --- a/src/PeopleExtraction.cpp +++ b/src/PeopleExtraction.cpp @@ -20,6 +20,14 @@ PeopleExtraction::PeopleExtraction(): auto pub_frequency = nh_.param("pub_frequency", 30.0); nh_.param("world_frame", world_tf_frame_, "world"); nh_.param("target_frame", target_tf_frame_, "odom"); + auto comp_filter_factor = nh_.param( + "velocity_filtering_factor", + GazeboModelExtractor::COMPLEMENTARY_FILTER_INNOVATION_DEFAULT + ); + auto gazebo_update_period = nh_.param( + "gazebo_cb_update_period", + GazeboModelExtractor::GAZEBO_CB_DIFF_TIME + ); // publishing frequency check if (pub_frequency <= 0.0) { @@ -68,7 +76,9 @@ PeopleExtraction::PeopleExtraction(): "/gazebo/model_states", model_name_patterns, models_transforms, - id_ref_ + id_ref_, + comp_filter_factor, + gazebo_update_period ); } @@ -81,7 +91,9 @@ PeopleExtraction::PeopleExtraction(): "/gazebo/link_states", link_name_patterns, links_transforms, - id_ref_ + id_ref_, + comp_filter_factor, + gazebo_update_period ); } From 985dfb3159c2e7f21f22fff8e719bb7a16f483f3 Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sat, 24 Aug 2024 15:01:28 +0200 Subject: [PATCH 20/23] `GazeboExtractor` - `isMatching` protected method transformed into static one named `doesNameMatchPatterns` --- src/GazeboExtractor.hpp | 45 ++++++++++++++++++++--------------------- 1 file changed, 22 insertions(+), 23 deletions(-) diff --git a/src/GazeboExtractor.hpp b/src/GazeboExtractor.hpp index bbc3548..969fdb8 100644 --- a/src/GazeboExtractor.hpp +++ b/src/GazeboExtractor.hpp @@ -55,6 +55,27 @@ class GazeboExtractor { return people_; } + /** + * @brief Evaluates whether the given @ref model_name matches any pattern given by @ref patterns + * + * @param model_name + * @param patterns + * @return std::tuple + */ + static std::tuple doesNameMatchPatterns( + const std::string& model_name, + const std::vector& patterns + ) { + for (const auto& pattern: patterns) { + // try to find + if (model_name.find(pattern) == std::string::npos) { + continue; + } + return {true, pattern}; + } + return {false, std::string()}; + } + protected: /** * @brief @@ -91,8 +112,7 @@ class GazeboExtractor { // check whether a model with a given name is of our interest (according to the naming patterns) bool pattern_matched = false; std::string pattern; - - std::tie(pattern_matched, pattern) = isMatching(name, name_patterns_); + std::tie(pattern_matched, pattern) = doesNameMatchPatterns(name, name_patterns_); if (!pattern_matched) { continue; } @@ -239,27 +259,6 @@ class GazeboExtractor { return people_.find(name) != people_.cend(); } - /** - * @brief Evaluates whether the given @ref model_name matches any pattern given by @ref patterns - * - * @param model_name - * @param patterns - * @return std::tuple - */ - std::tuple isMatching( - const std::string& model_name, - std::vector& patterns - ) const { - for (const auto& pattern: patterns) { - // try to find - if (model_name.find(pattern) == std::string::npos) { - continue; - } - return {true, pattern}; - } - return {false, std::string()}; - } - /** * @brief Get rid of the unnecessary model * @param people people identified so far From 8b4b1721107063be52b81442510ed5e4f93978d3 Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sat, 24 Aug 2024 16:19:03 +0200 Subject: [PATCH 21/23] information about people groups (F-formations) included in considerations --- config/people.yaml | 16 ++- src/PeopleExtraction.cpp | 248 +++++++++++++++++++++++++++++++++++---- src/PeopleExtraction.hpp | 22 +++- 3 files changed, 255 insertions(+), 31 deletions(-) diff --git a/config/people.yaml b/config/people.yaml index 5eb9f1f..a2f47a3 100644 --- a/config/people.yaml +++ b/config/people.yaml @@ -1,4 +1,4 @@ -# The example configuration fits https://github.com/aws-robotics/aws-robomaker-hospital-world +# The example configuration fits https://github.com/aws-robotics/aws-robomaker-hospital-world (with some modifications) model_name_patterns: [ "actor", ] @@ -8,7 +8,7 @@ link_name_patterns: [ "PatientWheelChair", "MaleVisitorSit", "VisitorKidSit", - "FemaleVisitorSit" + "FemaleVisitor", ] # Additional transforms to be applied for some specific models @@ -24,3 +24,15 @@ transforms: +0.0, -1.5708 ] + +groups: + # define names of groups so they can be read as separate ROS parameters (whole dicts cannot be passed as ROS params) + names: [ + "GroupDesk" + ] + # specify names of entities (values) involved in each group (keys) + GroupDesk: [ + "Scrubs_Group_01", + "Scrubs_Group_02", + "FemaleVisitor_Group_01" + ] diff --git a/src/PeopleExtraction.cpp b/src/PeopleExtraction.cpp index 19b5958..c896e34 100644 --- a/src/PeopleExtraction.cpp +++ b/src/PeopleExtraction.cpp @@ -67,6 +67,9 @@ PeopleExtraction::PeopleExtraction(): auto models_transforms = discoverTransforms(model_name_patterns); auto links_transforms = discoverTransforms(link_name_patterns); + // obtain group arrangements (relations between humans) + discoverGroupsConfiguration(); + if (!model_name_patterns.size()) { ROS_WARN("Gazebo people model extractor will not be formed as the 'model_name_patterns' parameter is empty"); } else { @@ -115,6 +118,44 @@ PeopleExtraction::PeopleExtraction(): ); } +void PeopleExtraction::discoverGroupsConfiguration() { + // for defining unique IDs of groups + static size_t group_id = 0; + + std::vector groups_names; + nh_.getParam("groups/names", groups_names); + for (const auto& gname: groups_names) { + std::vector group_entities; + nh_.getParam("groups/" + gname, group_entities); + + if (group_entities.empty()) { + ROS_WARN( + "Group with name '%s' cannot be properly loaded as it has no entities assigned", + gname.c_str() + ); + continue; + } + + // string composition is here only for debugging purposes + std::string entity_names; + // Add entities to the original map + for (const auto& ename: group_entities) { + entity_names += "'" + ename + "' "; + groups_arrangement_[group_id].push_back(ename); + } + + ROS_INFO( + "Discovered group with name '%s' - it has ID of '%lu' and %lu entities: %s", + gname.c_str(), + group_id, + group_entities.size(), + entity_names.c_str() + ); + + group_id++; + } +} + std::map> PeopleExtraction::discoverTransforms( const std::vector& name_patterns ) const { @@ -143,7 +184,7 @@ std::map> PeopleExtraction::discoverTransforms( return transforms; } -people_msgs_utils::People PeopleExtraction::gazeboModelsToPeople( +std::map PeopleExtraction::gazeboModelsToPeople( const std::map>& people_models ) const { // transform localization data to the desired frame @@ -167,16 +208,16 @@ people_msgs_utils::People PeopleExtraction::gazeboModelsToPeople( ); // avoid flooding console with this kind of errors (localization might not be operational yet) std::this_thread::sleep_for(std::chrono::seconds(1)); - return people_msgs_utils::People(); + return std::map(); } // prepare output container - people_msgs_utils::People people; + std::map people; - for (auto const& model_data: people_models) { + for (const auto& [model_name, model_data]: people_models) { // retrieve from the value (key is not considered here) - auto id = model_data.second.first; - auto model = model_data.second.second; + auto id = model_data.first; + auto model = model_data.second; geometry_msgs::PoseWithCovariance pose; pose.pose = model.pose; @@ -208,22 +249,20 @@ people_msgs_utils::People PeopleExtraction::gazeboModelsToPeople( ros::Time::now().toNSec(), // track age = since startup as we have ideal data std::string("") // group data not included ); - // TODO: include group data (obtain from node parameters) - // transform to the target frame p.transform(tf_stamped); - // collect - people.push_back(p); + // collect; note that the simulation model name is the key in this map + people.emplace(model_name, p); } return people; } -people_msgs_utils::People PeopleExtraction::huberoActorsToPeople( +std::map PeopleExtraction::huberoActorsToPeople( const std::vector>& actors ) const { // prepare output container - people_msgs_utils::People people; + std::map people; // access to, e.g., TF buffer, subscribers for (const auto& sub: actors) { @@ -284,12 +323,151 @@ people_msgs_utils::People PeopleExtraction::huberoActorsToPeople( // transform to the target frame p.transform(tf_stamped); - // add to the aggregated container - people.push_back(p); + // add to the aggregated container; note that the simulation model name is the key in this map + people.emplace(sub->getName(), p); } return people; } +std::vector> PeopleExtraction::createPeopleGroupAssociations( + const std::map& people +) { + const auto time_now = ros::Time::now().toNSec(); + + // duplicate the container at first, but it will have some info updated + std::map people_updated_with_groups = people; + + // groups container + std::vector groups; + + for (const auto& [group_id, group_members_names]: groups_arrangement_) { + // in this loop, we prepare arguments for group creation + std::map people_in_group; + std::vector member_ids_in_group; + + // first iteration - gather group members and their IDs + for (const auto& [person_full_name, person]: people) { + // check whether the person name is found within group members patterns + bool pattern_matched = false; + std::string pattern; + std::tie(pattern_matched, pattern) = link_extractor_->doesNameMatchPatterns(person_full_name, group_members_names); + if (!pattern_matched) { + continue; + } + people_in_group.emplace(person_full_name, person); + member_ids_in_group.push_back(person.getName()); + } + + if (people_in_group.empty()) { + continue; + } + + // second iteration - we need to properly assign group IDs to people + auto people_in_group_illformed = people_in_group; + people_in_group.clear(); + for (const auto& [person_full_name, person_wo_group]: people_in_group_illformed) { + // prepare ctor args + geometry_msgs::PoseWithCovariance pose; + pose.pose = person_wo_group.getPose(); + auto pose_covariance = person_wo_group.getCovariancePose(); + std::copy(pose_covariance.begin(), pose_covariance.end(), pose.covariance.begin()); + + geometry_msgs::PoseWithCovariance velocity; + velocity.pose = person_wo_group.getVelocity(); + auto velocity_covariance = person_wo_group.getCovarianceVelocity(); + std::copy(velocity_covariance.begin(), velocity_covariance.end(), velocity.covariance.begin()); + + // create correctly formed instance (with group) + people_msgs_utils::Person person_with_group( + person_wo_group.getName(), + pose, + velocity, + person_wo_group.getReliability(), + person_wo_group.isOccluded(), + person_wo_group.isMatched(), + person_wo_group.getDetectionID(), + person_wo_group.getTrackAge(), + std::to_string(group_id) + ); + people_in_group.emplace(person_full_name, person_with_group); + people_updated_with_groups.insert_or_assign(person_full_name, person_with_group); + } + + // third iteration - collect relations between members + const double RELATION_STRENGTH = 1.0; // hard coded with a maximum value due to the ideal data + std::vector> member_relations; + for (const auto& person_with_full_name_1: people_in_group) { + auto person1 = person_with_full_name_1.second; + for (const auto& person_with_full_name_2: people_in_group) { + auto person2 = person_with_full_name_2.second; + if (person1.getName() == person2.getName()) { + continue; + } + member_relations.push_back(std::make_tuple(person1.getName(), person2.getName(), RELATION_STRENGTH)); + } + } + + // fourth iteration - compute center of gravity of the group - mean of positions of the members + geometry_msgs::Point center_of_gravity; + for (const auto& person_with_full_name: people_in_group) { + auto person = person_with_full_name.second; + center_of_gravity.x += person.getPositionX(); + center_of_gravity.y += person.getPositionY(); + center_of_gravity.z += person.getPositionZ(); + } + center_of_gravity.x /= people_in_group.size(); + center_of_gravity.y /= people_in_group.size(); + center_of_gravity.z /= people_in_group.size(); + + // fifth interation: create a container with only Person objects + std::vector people_in_group_wo_full_names; + for (const auto& person_with_full_name: people_in_group) { + people_in_group_wo_full_names.push_back(person_with_full_name.second); + } + + // collect group in an aggregated container + groups.emplace_back( + std::to_string(group_id), + time_now, // time since the system startup as we have ideal data + people_in_group_wo_full_names, + member_ids_in_group, + member_relations, + center_of_gravity + ); + } + + // prepare associations so they can be easily used in publishing methods (there is some memory overhead + // due to duplicating groups) + std::vector> people_grouped; + + // NOTE: we cannot rely on 'people' container as its Person instances have not group names assigned + for (const auto& person_with_full_name: people_updated_with_groups) { + // obtain only the Person instance, ignore the full name + auto person = person_with_full_name.second; + if (!person.isAssignedToGroup()) { + // person without a group - create a dummy one + people_grouped.emplace_back(person, people_msgs_utils::Group()); + continue; + } + + // search for the correct group instance + bool group_found = false; + for (const auto& group: groups) { + if (person.getGroupName() != group.getName()) { + continue; + } + people_grouped.emplace_back(person, group); + group_found = true; + } + if (group_found) { + continue; + } + // a proper group was not found + people_grouped.emplace_back(person, people_msgs_utils::Group()); + } + return people_grouped; +} + void PeopleExtraction::publish() { // helper function auto extendMap = []( @@ -318,35 +496,55 @@ void PeopleExtraction::publish() { return; } + // another helper function + auto extendPeopleMap = []( + std::map& dest, + const std::map& src + ) { + // we cannot use `map[key] = value` as the `value` here does not have a default constructor + for (auto const& [key, val]: src) { + dest.emplace(key, val); + } + }; + + // aggregated container + std::map people; // Gazebo models to people representation - auto people = gazeboModelsToPeople(people_gazebo); + extendPeopleMap(people, gazeboModelsToPeople(people_gazebo)); // HuBeRo actors to people - for (const auto& person: huberoActorsToPeople(people_hubero)) { - people.push_back(person); - } + extendPeopleMap(people, huberoActorsToPeople(people_hubero)); + + // associate group data for each person + auto people_grouped = createPeopleGroupAssociations(people); - publishPeople(people); - publishPeoplePositions(people); + publishPeople(people_grouped); + publishPeoplePositions(people_grouped); } -void PeopleExtraction::publishPeople(const people_msgs_utils::People& people) { +void PeopleExtraction::publishPeople( + const std::vector>& people_grouped +) { people_msgs::People people_msg; people_msg.header.frame_id = target_tf_frame_; people_msg.header.stamp = ros::Time::now(); - for (const auto& person: people) { - people_msg.people.push_back(person.toPersonStd()); + for (const auto& [person, group]: people_grouped) { + people_msg.people.push_back(person.toPersonStd(group)); } pub_people_.publish(people_msg); } -void PeopleExtraction::publishPeoplePositions(const people_msgs_utils::People& people) { +void PeopleExtraction::publishPeoplePositions( + const std::vector>& people_grouped +) { people_msgs::PositionMeasurementArray pos_msg_array; pos_msg_array.header.frame_id = target_tf_frame_; pos_msg_array.header.stamp = ros::Time::now(); - for (auto person: people) { + for (auto person_with_group: people_grouped) { + auto person = person_with_group.first; + people_msgs::PositionMeasurement pos_msg; pos_msg.header = pos_msg_array.header; diff --git a/src/PeopleExtraction.hpp b/src/PeopleExtraction.hpp index 7fab812..a120d59 100644 --- a/src/PeopleExtraction.hpp +++ b/src/PeopleExtraction.hpp @@ -35,6 +35,13 @@ class PeopleExtraction { virtual ~PeopleExtraction() = default; private: + /** + * @brief Evaluates ROS parameters searching for human group definitions + * Prepares @ref member_groups_arrangement_ which is a map that eases filling the ROS messages later on. + * Specifically, additional computations are performed at the beginning to ease the access when publishing data. + */ + void discoverGroupsConfiguration(); + /// Searches through parameters to obtain a simple TF to be applied once reporting localization data std::map> discoverTransforms( const std::vector& name_patterns @@ -45,17 +52,21 @@ class PeopleExtraction { * * @return people_msgs_utils::People */ - people_msgs_utils::People gazeboModelsToPeople( + std::map gazeboModelsToPeople( const std::map>& people_models ) const; - people_msgs_utils::People huberoActorsToPeople( + std::map huberoActorsToPeople( const std::vector>& actors ) const; + std::vector> createPeopleGroupAssociations( + const std::map& people + ); + void publish(); - void publishPeople(const people_msgs_utils::People& people); - void publishPeoplePositions(const people_msgs_utils::People& people); + void publishPeople(const std::vector>& people_grouped); + void publishPeoplePositions(const std::vector>& people_grouped); ros::NodeHandle nh_; @@ -69,6 +80,9 @@ class PeopleExtraction { std::unique_ptr hubero_extractor_; std::atomic id_ref_; + // Grouping map: Group ID to entities in that group + std::map> groups_arrangement_; + tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; From f2bfb3ff5ba8ca37228a0299ce9e9d46109121db Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sun, 25 Aug 2024 13:45:01 +0200 Subject: [PATCH 22/23] `GazeboExtractor` - reworked data processing algorithm in model states callback to ensure consistent ID assignment order (according to provided naming patterns) --- src/GazeboExtractor.hpp | 112 ++++++++++++++++++++++------------------ 1 file changed, 62 insertions(+), 50 deletions(-) diff --git a/src/GazeboExtractor.hpp b/src/GazeboExtractor.hpp index 969fdb8..f9c5204 100644 --- a/src/GazeboExtractor.hpp +++ b/src/GazeboExtractor.hpp @@ -55,6 +55,20 @@ class GazeboExtractor { return people_; } + /** + * @brief Evaluates whether the given @ref model_name matches the given @ref pattern + */ + static bool doesNameMatchPattern( + const std::string& model_name, + const std::string& pattern + ) { + // try to find + if (model_name.find(pattern) == std::string::npos) { + return false; + } + return true; + } + /** * @brief Evaluates whether the given @ref model_name matches any pattern given by @ref patterns * @@ -67,8 +81,7 @@ class GazeboExtractor { const std::vector& patterns ) { for (const auto& pattern: patterns) { - // try to find - if (model_name.find(pattern) == std::string::npos) { + if (!doesNameMatchPattern(model_name, pattern)) { continue; } return {true, pattern}; @@ -106,57 +119,56 @@ class GazeboExtractor { // update internal database dynamically size_t models_found = 0; size_t models_total = msg->name.size(); - for (size_t i = 0; i < models_total; i++) { - // investigated object - obtain only the name at this stage - const auto& name = msg->name.at(i); - // check whether a model with a given name is of our interest (according to the naming patterns) - bool pattern_matched = false; - std::string pattern; - std::tie(pattern_matched, pattern) = doesNameMatchPatterns(name, name_patterns_); - if (!pattern_matched) { - continue; - } - - models_found++; - Tcontainer model; - model.model_name = name; - model.pose = msg->pose.at(i); - model.twist = msg->twist.at(i); - - std::vector transform; - if (transforms_.find(pattern) != transforms_.cend()) { - transform = transforms_[pattern]; - } else { - transform = std::vector{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - } - - // check whether the person already exists in the internal database (only need ModelState update then) - if (doesPersonExist(name)) { - // manually compute the new state a some Gazebo model might report wrong velocities - model = computeState( - people_[name].second, // previously saved "model" - model, - transform, - time_diff + // Iterate over the name patterns to ensure consistent ID assignment order + for (const auto& pattern: name_patterns_) { + for (size_t i = 0; i < models_total; i++) { + // investigated object - obtain only the name at this stage + const auto& name = msg->name.at(i); + // check whether a model with a given name is of our interest (according to the naming pattern) + if (!doesNameMatchPattern(name, pattern)) { + continue; + } + + models_found++; + Tcontainer model; + model.model_name = name; + model.pose = msg->pose.at(i); + model.twist = msg->twist.at(i); + + std::vector transform; + if (transforms_.find(pattern) != transforms_.cend()) { + transform = transforms_[pattern]; + } else { + transform = std::vector{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + } + + // check whether the person already exists in the internal database (only need ModelState update then) + if (doesPersonExist(name)) { + // manually compute the new state a some Gazebo model might report wrong velocities + model = computeState( + people_[name].second, // previously saved "model" + model, + transform, + time_diff + ); + people_[name].second = model; + continue; + } + + // special stage for a proper initialization + std::tie(model, std::ignore, std::ignore, std::ignore) = computeStateTransformed(model, transform); + // new model - assign an ID and update its ModelState + people_[name] = {id_ref_.load(), model}; + // increment the reference ID counter + id_ref_++; + ROS_INFO( + "Tracking a new person '%s' (ID: '%lu'), detected from naming pattern: '%s'", + people_[name].second.model_name.c_str(), + people_[name].first, + pattern.c_str() ); - people_[name].second = model; - continue; } - - // special stage for a proper initialization - std::tie(model, std::ignore, std::ignore, std::ignore) = computeStateTransformed(model, transform); - // new model - assign an ID and update its ModelState - people_[name] = {id_ref_.load(), model}; - // increment the reference ID counter - id_ref_++; - ROS_INFO( - "Tracking a new person '%s' (ID: '%lu'), detected from naming pattern: '%s'", - people_[name].second.model_name.c_str(), - people_[name].first, - pattern.c_str() - ); } - // possibly delete renamed/destroyed objects deleteDestroyedPerson(msg->name); } From 8004619340bf94fa74c3ac86e4e5afda3428845e Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sun, 25 Aug 2024 17:38:43 +0200 Subject: [PATCH 23/23] added methods to manually start processing callbacks in the `GazeboExtractor` class, added a thread in `PeopleExtraction` to ensure consistent ID assignment order (according to provided naming patterns) --- src/GazeboExtractor.hpp | 12 ++++++++++++ src/PeopleExtraction.cpp | 32 ++++++++++++++++++++++++++++++-- src/PeopleExtraction.hpp | 2 ++ 3 files changed, 44 insertions(+), 2 deletions(-) diff --git a/src/GazeboExtractor.hpp b/src/GazeboExtractor.hpp index f9c5204..12dff72 100644 --- a/src/GazeboExtractor.hpp +++ b/src/GazeboExtractor.hpp @@ -42,6 +42,7 @@ class GazeboExtractor { comp_filter_innovation_(comp_filter_innovation), cb_time_diff_(cb_time_diff) { + run_.store(false); sub_ = nh.subscribe( topic_name, 1, @@ -50,6 +51,11 @@ class GazeboExtractor { ); } + /// Call to this method enables processing the subscriber's callbacks + void run() { + run_.store(true); + } + inline std::map> getPeople() const { std::lock_guard lock(mutex_); return people_; @@ -101,6 +107,11 @@ class GazeboExtractor { time_last_update_ = ros::Time::now().toSec(); } + // make sure that callback processing should be performed + if (!run_.load()) { + return; + } + if (msg->name.size() != msg->pose.size() || msg->name.size() != msg->twist.size()) { ROS_ERROR("Vector sizes in the input message are not equal, cannot process further"); return; @@ -297,6 +308,7 @@ class GazeboExtractor { ros::Subscriber sub_; mutable std::mutex mutex_; + std::atomic run_; /// @brief Algorithm looks for a model name that meets one of the given patterns std::vector name_patterns_; diff --git a/src/PeopleExtraction.cpp b/src/PeopleExtraction.cpp index c896e34..c83055f 100644 --- a/src/PeopleExtraction.cpp +++ b/src/PeopleExtraction.cpp @@ -7,11 +7,10 @@ #include "PeopleExtraction.hpp" +#include #include #include -#include - PeopleExtraction::PeopleExtraction(): id_ref_(0), tf_listener_(tf_buffer_) @@ -116,6 +115,35 @@ PeopleExtraction::PeopleExtraction(): this ) ); + + topics_waiter_ = std::thread([this]() { + bool topics_available = false; + auto tm_ptr = ros::TopicManager::instance(); + while (!topics_available && ros::ok()) { + // check whether a topic is taken into consideration and published + bool models_topic_ok = !model_extractor_ || tm_ptr->getNumPublishers("/gazebo/model_states") > 0; + bool links_topic_ok = !link_extractor_ || tm_ptr->getNumPublishers("/gazebo/link_states") > 0; + topics_available = models_topic_ok && links_topic_ok; + if (!topics_available) { + ROS_INFO("Waiting for the topics to become available..."); + } + ros::Duration(0.5).sleep(); + } + + ROS_INFO("Preparing to enable callback processing..."); + ros::Duration(1.0).sleep(); + if (model_extractor_) { + model_extractor_->run(); + // With this delay we're making sure that the Gazebo callback related to models will be processed first, + // otherwise, the IDs of humans might not be consistent across system startups; + // Also, we assume that Gazebo operates at periods shorter than the delay time + ros::Duration(0.25).sleep(); + } + + if (link_extractor_) { + link_extractor_->run(); + } + }); } void PeopleExtraction::discoverGroupsConfiguration() { diff --git a/src/PeopleExtraction.hpp b/src/PeopleExtraction.hpp index a120d59..26121e3 100644 --- a/src/PeopleExtraction.hpp +++ b/src/PeopleExtraction.hpp @@ -25,6 +25,7 @@ #include #include +#include #include "GazeboExtractor.hpp" #include "HuberoExtractor.hpp" @@ -93,4 +94,5 @@ class PeopleExtraction { std::string target_tf_frame_; ros::Timer timer_pub_; + std::thread topics_waiter_; };