destroyed models deletion from db, WIP - tf from gazebo to target frame

config/people.yaml

@@ -1,6 +1,6 @@
-gazebo_tf_frame: "world"
+gazebo_frame: "world"
 target_frame: "map"
-callback_omits: 100 # each n-th simulator callback will be processed
+callback_omits: 25 # each n-th simulator callback will be processed
 
 people_topic: "/people"
 position_topic: "/people_measurements"

src/PeopleExtraction.cpp

@@ -8,18 +8,21 @@
 #include "PeopleExtraction.hpp"
 
 constexpr char* PeopleExtraction::GAZEBO_FRAME_ID_DEFAULT;
+constexpr char* PeopleExtraction::TARGET_FRAME_ID_DEFAULT;
 
 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)
 {
 	std::string people_topic("");
 	std::string position_topic("");
-	nh_.param<std::string>("gazebo_tf_frame", gazebo_tf_frame_, std::string(PeopleExtraction::GAZEBO_FRAME_ID_DEFAULT));
+	nh_.param<std::string>("gazebo_frame", gazebo_tf_frame_, gazebo_tf_frame_);
+	nh_.param<std::string>("target_frame", target_tf_frame_, target_tf_frame_);
 	nh_.param<int>("callback_omits", callback_omits_, callback_omits_);
 	nh_.param<std::string>("people_topic", people_topic, "/people_topic");
 	nh_.param<std::string>("position_topic", position_topic, "/position_topic");
@@ -41,12 +44,14 @@ void PeopleExtraction::gazeboModelStateCallback(const gazebo_msgs::ModelStates::
 	callback_counter_ = 0;
 
 	// update internal database dynamically
+	int models_found = 0;
 	for (auto pattern : people_name_patterns_) {
 		// evaluate, whether the object named with a given pattern exists in a simulation
 		std::vector<std::string> 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 exists in the internal database
+			// check whether the person already exists in the internal database
 			if (!doesPersonExist(sim_name)) {
 				Person bud;
 				bud.name = sim_name;
@@ -56,8 +61,12 @@ void PeopleExtraction::gazeboModelStateCallback(const gazebo_msgs::ModelStates::
 			}
 		}
 	}
-
-	// TODO: possibly delete renamed objects
+	// 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",
+			models_found, people_data_.size());
+		deleteDestroyedPerson(msg->name);
+	}
 
 	// compute indexes of the `people_names` objects
 	for (auto&& person : people_data_) {
@@ -67,6 +76,7 @@ void PeopleExtraction::gazeboModelStateCallback(const gazebo_msgs::ModelStates::
 			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();
@@ -79,6 +89,23 @@ void PeopleExtraction::gazeboModelStateCallback(const gazebo_msgs::ModelStates::
 
 				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 {
+					tf_buffer_.transform(person.pose, target_tf_frame_, ros::Duration(1.0));
+					/*
+					 * https://answers.ros.org/question/192273/how-to-implement-velocity-transformation/
+					auto tf_stamped = tf_buffer_.lookupTransform(target_tf_frame_, gazebo_tf_frame_, ros::Time::now());
+					tf_buffer_.transformtf_stamped.
+					tf_buffer_.transform(person.pose, target_tf_frame_, ros::Duration(1.0));
+					 */
+					// TODO: rotate twist
+					// ...
+					person.pose.header.frame_id = target_tf_frame_;
+					std::cout << "assuming successful transformation - changed frame_id to TARGET: " << person.pose.header.frame_id << std::endl;
+				} 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());
+				}
 			}
 		}
 	}
@@ -97,7 +124,7 @@ bool PeopleExtraction::doesPersonExist(const std::string &name) const {
 	return false;
 }
 
-std::vector<std::string> PeopleExtraction::findModels(const std::vector<std::string> model_names, std::string pattern) const {
+std::vector<std::string> PeopleExtraction::findModels(const std::vector<std::string> &model_names, std::string pattern) const {
 	std::vector<std::string> db;
 	for (auto name : model_names) {
 		// find
@@ -109,6 +136,26 @@ std::vector<std::string> PeopleExtraction::findModels(const std::vector<std::str
 	return db;
 }
 
+void PeopleExtraction::deleteDestroyedPerson(const std::vector<std::string> &model_names) {
+	int num_deleted = 0;
+	auto i = std::begin(people_data_);
+	// while is safer than for here
+	while (i != std::end(people_data_)) {
+		auto it = std::find(model_names.begin(), model_names.end(), i->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("Deleted %d object(s)", num_deleted);
+}
+
 void PeopleExtraction::publishPeople() {
 	people_msgs::People people_msg;
 	people_msgs::Person person_msg;

src/PeopleExtraction.hpp

@@ -49,12 +49,19 @@ class PeopleExtraction {
 	 * @param pattern
 	 * @return
 	 */
-	std::vector<std::string> findModels(const std::vector<std::string> model_names, std::string pattern) const;
+	std::vector<std::string> findModels(const std::vector<std::string> &model_names, std::string pattern) const;
+
+	/**
+	 * @brief Get rid of the unnecessary model
+	 * @param model_names
+	 */
+	void deleteDestroyedPerson(const std::vector<std::string> &model_names);
 
 	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_;
@@ -73,6 +80,7 @@ class PeopleExtraction {
 
 	int id_next_;
 	std::string gazebo_tf_frame_;
+	std::string target_tf_frame_;
 	int callback_counter_;
 	int callback_omits_;
 	unsigned long int seq_;