Modernize and extend capabilities

.gitignore

@@ -1,4 +1,5 @@
 src/actor_legs_extraction_node.cpp
 src/LegsExtraction.*
 launch/actor_legs_extraction.launch
-config/legs.yaml
+config/legs.yaml
+build/

CMakeLists.txt

@@ -1,27 +1,35 @@
-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
+  angles
 )
 
 catkin_package(
 	CATKIN_DEPENDS 
 	gazebo_ros
 	roscpp 
     people_msgs
+	people_msgs_utils
+	angles
 )
 
-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}")
 
 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

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.

config/people.yaml

@@ -1,8 +1,38 @@
-gazebo_frame: "world"
-target_frame: "map"
-callback_omits: 25 # each n-th simulator callback will be processed
+# The example configuration fits https://github.com/aws-robotics/aws-robomaker-hospital-world (with some modifications)
+model_name_patterns: [
+  "actor",
+]
 
-people_topic: "/people"
-position_topic: "/people_measurements"
+link_name_patterns: [
+  "Scrubs",
+  "PatientWheelChair",
+  "MaleVisitorSit",
+  "VisitorKidSit",
+  "FemaleVisitor",
+]
 
-model_name_patterns: ["person", "actor"]
+# 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
+  ]
+
+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"
+  ]

launch/people_extraction.launch

@@ -1,4 +1,22 @@
 <launch>
-  <rosparam file="$(find people_extraction)/config/people.yaml" command="load" ns="people_extraction" />
-  <node name="people_extraction" pkg="people_extraction" type="people_extraction_node" output="screen" ns="people_extraction"/>
+  <arg name="world_frame" default="world"/>
+  <arg name="target_frame" default="odom"/>
+  <arg name="config_file" default="$(find people_extraction)/config/people.yaml"/>
+
+  <!-- output topics -->
+  <arg name="pub_frequency" default="10.0"/>
+  <arg name="people_topic" default="/people"/>
+  <arg name="position_topic" default="/people_pos"/>
+
+  <arg name="ns" default="people_extraction"/>
+  <rosparam file="$(arg config_file)" command="load" ns="$(arg ns)" />
+
+  <node name="people_extraction" pkg="people_extraction" type="people_extraction_node" output="screen" ns="$(arg ns)">
+    <remap from="/people" to="$(arg people_topic)"/>
+    <remap from="/people_measurements" to="$(arg position_topic)"/>
+    <param name="pub_frequency" value="$(arg pub_frequency)"/>
+
+    <param name="world_frame" value="$(arg world_frame)"/>
+    <param name="target_frame" value="$(arg target_frame)"/>
+  </node>
 </launch>

package.xml

@@ -3,13 +3,16 @@
 	<version>1.0.0</version>
 	<description>people_extraction package</description>
 
-	<maintainer email="[email protected]">Jaroslaw Karwowski</maintainer>
+	<author email="[email protected]">Jarosław Karwowski</author>
+	<maintainer email="[email protected]">Jarosław Karwowski</maintainer>
 
 	<license>GPLv3</license>
 
 	<buildtool_depend>catkin</buildtool_depend>
 	<depend>roscpp</depend>
 	<depend>gazebo_ros</depend>
 	<depend>people_msgs</depend>
+	<depend>people_msgs_utils</depend>
+	<depend>angles</depend>
 
 </package>

src/ActorLocalizationSubscriber.hpp

@@ -0,0 +1,65 @@
+#pragma once
+
+#include <ros/ros.h>
+#include <nav_msgs/Odometry.h>
+
+#include <mutex>
+#include <string>
+
+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<nav_msgs::Odometry>(
+			topic_,
+			10,
+			std::bind(
+				&ActorLocalizationSubscriber::callback,
+				this,
+				std::placeholders::_1
+			)
+		);
+	}
+
+	nav_msgs::Odometry getOdom() const {
+		std::lock_guard<std::mutex> 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<std::mutex> lock(mutex_);
+		odom_ = *msg;
+	}
+
+	mutable std::mutex mutex_;
+	ros::Subscriber sub_;
+	nav_msgs::Odometry odom_;
+	size_t id_;
+	std::string name_;
+	std::string topic_;
+};