git commit -m "first commit"

simulators/third_party/dynamic_rviz_config/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(dynamic_rviz_config)
+find_package(catkin REQUIRED COMPONENTS
+	roscpp
+	rospy
+)
+
+catkin_package(
+   CATKIN_DEPENDS urdf xacro
+)
+
+catkin_install_python(PROGRAMS
+  scripts/rviz_generate.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

simulators/third_party/dynamic_rviz_config/package.xml

@@ -0,0 +1,17 @@
+<package>
+  <name>dynamic_rviz_config</name>
+  <version>0.0.1</version>
+  <description> dynamic_rviz_config package</description>
+  <author>Winter</author>
+  <maintainer email="913982779@qq.com">Yang Haodong</maintainer>
+  <license>BSD</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>urdf</run_depend>
+  <run_depend>xacro</run_depend>
+
+</package>

simulators/third_party/dynamic_rviz_config/scripts/config.py

@@ -0,0 +1,166 @@
+#!/usr/bin/python
+#-*- coding: utf-8 -*-
+
+''' 
+******************************************************************************************
+*  Copyright (C) 2023 Yang Haodong, All Rights Reserved                                  *
+*                                                                                        *
+*  @brief    rviz basic pannel configure class.                                          *
+*  @author   Haodong Yang                                                                *
+*  @version  1.0.1                                                                       *
+*  @date     2022.07.06                                                                  *
+*  @license  GNU General Public License (GPL)                                            *
+******************************************************************************************
+'''
+import yaml
+
+class RVizConfig:
+    def __init__(self, rviz_name, path, base_file=None):
+        # Rviz visualization manager
+        self.VIZ_MAN = 'Visualization Manager'
+        # Rviz standard tools
+        self.STD_TOOLS = ['rviz/Interact', 'rviz/MoveCamera', 'rviz/Select', 'rviz/FocusCamera', 'rviz/Measure', 'rviz/PublishPoint']
+        # Rviz standard panels
+        self.STD_PANELS = ['rviz/Displays', 'rviz/Tool Properties']  
+        # Rviz file name
+        self.file_name = rviz_name
+        # Rviz cache
+        self.cache_path = path
+        # Rviz pre-configure
+        self.data = yaml.load(base_file) if base_file else {}
+
+    def __repr__(self):
+        return yaml.dump(self.data, default_flow_style=False)
+
+    # @breif: set Rviz configure path
+    #
+    # @param[in]: path  Rviz configure path
+    # @return: None
+    def setFileCache(self, path):
+        self.cache_path = path
+
+    # @breif: extract all configure data of `rviz Visualization Manager`
+    #
+    # @param[in]: None
+    # @return: all configure data of `rviz Visualization Manager`
+    def getVisualization(self):
+        if self.VIZ_MAN not in self.data:
+            self.data[self.VIZ_MAN] = {}
+        return self.data[self.VIZ_MAN]            
+
+    # @breif: set display format
+    # @param[in]: displays  display format
+    # @return: True if successful else False
+    def setDisplays(self, displays):
+        if 'Displays' not in self.getVisualization():
+            self.data[self.VIZ_MAN]['Displays'] = displays
+            print("Displays have been set successfully!")
+            return True
+        print("Displays setting failed! please call displays delete first!")
+        return False
+
+    # @breif: clear display
+    # @param[in]: None
+    # @return: True if successful else False
+    def delDisplays(self):
+        if 'Displays'  in self.getVisualization():
+            del self.data[self.VIZ_MAN]['Displays']
+            print("Displays have been deleted successfully!")
+            return True
+        print("Displays delete failed! please call displays setting first!")
+        return False  
+
+    # @breif: set Rviz standard tools
+    # @param[in]: None
+    # @return: True if successful else False
+    def setStdTools(self):
+        v = self.getVisualization()
+        if 'Tools' not in v:
+            v['Tools'] = []
+            for tool in self.STD_TOOLS:
+                v['Tools'].append({'Class': tool})
+            print("standard tools have been set successfully!")
+            return True
+        print("standard tools setting failed! please call standard tools delete first!")
+        return False
+
+    # @breif: clear Rviz standard tools
+    # @param[in]: None
+    # @return: True if successful else False
+    def delStdTools(self):
+        if 'Tools'  in self.getVisualization():
+            del self.data[self.VIZ_MAN]['Tools']
+            print("standard tools have been deleted successfully!")
+            return True
+        print("standard tools delete failed! please call standard tools setting first!")
+        return False  
+
+    # @breif: set Rviz standard panels
+    # @param[in]: None
+    # @return: True if successful else False
+    def setStdPanels(self):
+        if 'Panels' not in self.data:
+            self.data['Panels'] = []
+            for tool in self.STD_PANELS:
+                self.data['Panels'].append({'Class': tool, 'Name': tool.split('/')[-1]})
+                print("standard panels have been set successfully!")
+            return True
+        print("standard panels setting failed! please call standard panels delete first!")
+        return False
+
+    # @breif: clear Rviz standard panels
+    # @param[in]: None
+    # @return: True if successful else False
+    def delStdPanels(self):
+        if 'Panels'  in self.data:
+            del self.data['Panels']
+            print("standard panels have been deleted successfully!")
+            return True
+        print("standard panels delete failed! please call standard panels setting first!")
+        return False 
+
+    # @breif: set base coordinate
+    # @param[in]: frame base coordinate, default is `map`
+    # @return: None        
+    def setFixedFrame(self, frame='map'):
+        v = self.getVisualization()
+        if 'Global Options' not in v:
+            v['Global Options'] = {'Fixed Frame':frame}
+        else:
+            v['Global Options']['Fixed Frame'] = frame
+
+    # @breif: set the topics of Rviz standard tools
+    # @param[in]: name  Rviz standard tools classes
+    # @param[in]: topic  the topic of Rviz standard tools relatively
+    # @return: None
+    def setToolTopic(self, name, topic):
+        for m in self.data[self.VIZ_MAN]['Tools']:
+            if m.get('Class', '')==name:
+                m['Topic'] = topic
+
+    # @breif: append navigation tools and their topics
+    # @param[in]: None
+    # @return: None  
+    def appendToolGoal(self, topic):
+        v = self.getVisualization()
+        if 'Tools' not in v:
+            v['Tools'] = []
+        v['Tools'].append({'Class': 'rviz/SetGoal', 'Topic': topic})
+
+    # @breif: append initial Pose and its topic
+    # @param[in]: None
+    # @return: None 
+    def appendToolInit(self, topic):
+        v = self.getVisualization()
+        if 'Tools' not in v:
+            v['Tools'] = []
+        v['Tools'].append({'Class': 'rviz/SetInitialPose', 'Topic': topic})
+        
+    # @breif: run Rviz and apply user configure
+    # @param[in]: None
+    # @return: None  
+    def run(self):
+        with open( self.cache_path + self.file_name + '.rviz', "w") as fp:
+            fp.write(str(self))
+        import subprocess
+        subprocess.call(['rosrun','rviz','rviz', '-d', self.cache_path + self.file_name + '.rviz'])

simulators/third_party/dynamic_rviz_config/scripts/displays.py

@@ -0,0 +1,93 @@
+#!/usr/bin/python
+#-*- coding: utf-8 -*-
+
+''' 
+******************************************************************************************
+*  Copyright (C) 2023 Yang Haodong, All Rights Reserved                                  *
+*                                                                                        *
+*  @brief    rviz display pannel configure class.                                        *
+*  @author   Haodong Yang                                                                *
+*  @version  1.0.1                                                                       *
+*  @date     2022.07.06                                                                  *
+*  @license  GNU General Public License (GPL)                                            *
+******************************************************************************************
+'''
+
+class Displays(list):
+    def __init__(self):
+        None
+    
+    
+    def addDisplay(self, name, class_name, topic=None, color=None, fields={}, enabled=True):
+        d = {'Name': name, 'Class': class_name, 'Enabled': enabled}
+        if topic:
+            d['Topic'] = topic
+        if color:
+            d['Color'] = '%d; %d; %d'%color
+        d.update(fields)
+        self.append(d)
+
+    def addMap(self, topic='/map', name='Map', alpha=None, scheme=None):
+        fields = {}
+        if alpha:
+            fields['Alpha'] = alpha
+        if scheme:
+            fields['Color Scheme'] = scheme    
+        self.addDisplay(name, 'rviz/Map', topic, fields=fields)
+
+    def addGrid(self):
+        self.addDisplay('Grid', 'rviz/Grid')   
+
+    def addTf(self, scale=None):
+        fields = {}
+        if scale:
+            fields['Marker Scale'] = scale
+        self.addDisplay('TF', 'rviz/TF', fields=fields, enabled=False) 
+
+    def addGroup(self, name, displays):
+        self.addDisplay(name, 'rviz/Group', fields={'Displays': displays})
+        
+    def addModel(self, parameter='robot_description', tf_prefix=None):
+        fields = {'Robot Description': parameter}
+        if tf_prefix:
+            fields['TF Prefix'] = tf_prefix
+        self.addDisplay('RobotModel', 'rviz/RobotModel', fields=fields)
+        
+    def addLaserscan(self, topic='/base_scan', name=None, color=(46, 255, 0), size=0.1, alpha=None):
+        if name is None:
+            name = topic
+        fields = {'Size (m)': size, 'Style': 'Spheres', 'Color Transformer': 'FlatColor'}
+        if alpha:
+            fields['Alpha'] = alpha
+        self.addDisplay(name, 'rviz/LaserScan', topic, color, fields)
+
+    def addPoseArray(self, topic='/particlecloud', color=None):
+        self.addDisplay('AMCL Cloud', 'rviz/PoseArray', topic, color)
+        
+    def addFootprint(self, topic, color=(0,170,255)):
+        self.addDisplay('Robot Footprint', 'rviz/Polygon', topic, color)
+        
+    def addPath(self, topic, name, color=None):
+        self.addDisplay(name, 'rviz/Path', topic, color)
+
+    def addPose(self, topic, name='Current Goal', color=None, arrow_shape=None):
+        fields = {}
+        if arrow_shape:
+            fields['Head Length'] = arrow_shape[0]
+            fields['Head Radius'] = arrow_shape[1]
+            fields['Shaft Length'] = arrow_shape[2]
+            fields['Shaft Radius'] = arrow_shape[3]
+            fields['Shape'] = 'Arrow'
+        self.addDisplay(name, 'rviz/Pose', topic, color, fields)
+        
+    def addImage(self, topic="/camera/rgb/image_raw", name="Image"):
+        self.addDisplay(name, "rviz/Image", topic, enabled=False)
+        
+   
+
+import yaml
+
+def display_representer(dumper, data):
+    return dumper.represent_sequence(u'tag:yaml.org,2002:seq', list(data))
+
+yaml.add_representer(Displays, display_representer)

simulators/third_party/dynamic_rviz_config/scripts/rviz_generate.py

@@ -0,0 +1,73 @@
+#!/usr/bin/python
+#-*- coding: utf-8 -*-
+
+''' 
+******************************************************************************************
+*  Copyright (C) 2023 Yang Haodong, All Rights Reserved                                  *
+*                                                                                        *
+*  @brief    rviz basic pannel configure class.                                          *
+*  @author   Haodong Yang                                                                *
+*  @version  1.0.1                                                                       *
+*  @date     2022.07.06                                                                  *
+*  @license  GNU General Public License (GPL)                                            *
+******************************************************************************************
+'''
+import sys, os
+dir_path = os.path.dirname(os.path.realpath(__file__))
+sys.path.insert(0, dir_path)
+
+from config import RVizConfig
+from displays import Displays
+
+# the number of robots
+robot_num = int(sys.argv[1])
+
+r = RVizConfig("cache", os.path.split(os.path.realpath(__file__))[0] + "/../../../sim_env/rviz/")
+# set Rviz standard tools
+r.setStdTools()
+# visualize Rviz models
+d = Displays()
+# load map and grid
+d.addMap()
+d.addGrid()
+# set transform
+d.addTf()
+
+# single robot
+if robot_num == 1:
+    # append robots' model
+    d.addModel()
+    # append camera image
+    d.addImage()
+    # append laser
+    d.addLaserscan(topic="/scan", name="LaserScan", color=(255, 0, 0), size=0.03, alpha=1)
+    # append `initialpose` setting panel
+    r.appendToolInit('/initialpose')
+    # append `move_base_simple/goal` setting panel
+    r.appendToolGoal('/move_base_simple/goal')
+else:
+    # multi-robot
+    for i in range(robot_num):
+        meta_robot = Displays()
+        # append robots' model
+        meta_robot.addModel(parameter="robot" + str(i + 1) + "/robot_description", tf_prefix="robot" + str(i + 1))
+        # append camera image
+        meta_robot.addImage(topic="/robot" + str(i + 1) + "/camera/rgb/image_raw", name="Image" + str(i + 1))
+        # append laser
+        meta_robot.addLaserscan(topic="/robot" + str(i + 1) + "/scan", name="LaserScan" + str(i + 1), color=(255, 0, 0), size=0.03, alpha=1)
+        # append path
+        meta_robot.addPath(topic=None, name="Path", color=(32, 74, 135))
+        d.addGroup("Robot" + str(i + 1), meta_robot)
+        # append `initialpose` setting panel
+        r.appendToolInit('/robot' + str(i + 1) + '/initialpose')
+        # ppend `move_base_simple/goal` setting panel
+        r.appendToolGoal('/robot' + str(i + 1) +'/move_base_simple/goal')
+
+# 设置Rviz标准面板
+r.setStdPanels()
+# 设置Rviz模型显示
+r.setDisplays(d)
+# 设置Rviz基坐标系
+r.setFixedFrame()
+# 启动Rviz
+r.run()

simulators/third_party/dynamic_xml_config/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(dynamic_xml_config)
+find_package(catkin REQUIRED COMPONENTS
+    roscpp
+    rospy
+)
+
+catkin_package(
+    
+)
+
+catkin_install_python(PROGRAMS
+    plugins/obstacles_generate_ros.py
+    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

simulators/third_party/dynamic_xml_config/main_generate.py

@@ -0,0 +1,63 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+""" 
+******************************************************************************************
+*  Copyright (C) 2023 Yang Haodong, All Rights Reserved                                  *
+*                                                                                        *
+*  @brief    generate launch file dynamicly based on user configure.                     *
+*  @author   Haodong Yang                                                                *
+*  @version  1.0.3                                                                       *
+*  @date     2023.07.19                                                                  *
+*  @license  GNU General Public License (GPL)                                            *
+******************************************************************************************
+"""
+import xml.etree.ElementTree as ET
+from plugins import XMLGenerator, PedGenerator, RobotGenerator, ObstacleGenerator
+
+class MainGenerator(XMLGenerator):
+    def __init__(self, *plugins) -> None:
+        super().__init__()
+        self.main_path = self.root_path + "pedestrian_simulation/"
+        self.app_list = [app for app in plugins]
+    
+    def writeMainLaunch(self, path):
+        """
+        Create configure file of package `sim_env` dynamically.
+
+        Parameters
+        ----------
+        path: str
+            the path of file(.launch.xml) to write.
+        """
+        # root
+        launch = MainGenerator.createElement("launch")
+
+        # other applications
+        for app in self.app_list:
+            assert isinstance(app, XMLGenerator), "Expected type of app is XMLGenerator"
+            app_register = app.plugin()
+            for app_element in app_register:
+                launch.append(app_element)
+
+        # include
+        include = MainGenerator.createElement("include", props={"file": "$(find sim_env)/launch/config.launch"})
+        include.append(MainGenerator.createElement("arg", props={"name": "world", "value": "$(arg world_parameter)"}))
+        include.append(MainGenerator.createElement("arg", props={"name": "map", "value": self.user_cfg["map"]}))
+        include.append(MainGenerator.createElement("arg", props={"name": "robot_number", "value": str(len(self.user_cfg["robots_config"]))}))
+        include.append(MainGenerator.createElement("arg", props={"name": "rviz_file", "value": self.user_cfg["rviz_file"]}))
+
+        launch.append(include)
+        MainGenerator.indent(launch)
+
+        with open(path, "wb+") as f:
+            ET.ElementTree(launch).write(f, encoding="utf-8", xml_declaration=True)
+
+    def plugin(self):
+        pass
+
+# dynamic generator
+main_gen = MainGenerator(PedGenerator(),
+                         RobotGenerator(),
+                         ObstacleGenerator())
+main_gen.writeMainLaunch(main_gen.root_path + "sim_env/launch/main.launch")

simulators/third_party/dynamic_xml_config/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>dynamic_xml_config</name>
+  <version>0.2.0</version>
+  <description>
+    dynamic xml configure
+  </description>
+
+  <maintainer email="913982779@qq.com">Winter</maintainer>
+
+  <license>BSD</license>
+
+  <author email="913982779@qq.com">Winter</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+</package>

simulators/third_party/dynamic_xml_config/plugins/__init__.py

@@ -0,0 +1,6 @@
+from .xml_generate import XMLGenerator
+from .pedestrians_generate import PedGenerator
+from .robot_generate import RobotGenerator
+from .obstacles_generate import ObstacleGenerator
+
+__all__ = ["XMLGenerator", "PedGenerator", "RobotGenerator", "ObstacleGenerator"]

simulators/third_party/dynamic_xml_config/plugins/obstacles_generate.py

@@ -0,0 +1,191 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+""" 
+******************************************************************************************
+*  Copyright (C) 2023 Yang Haodong, All Rights Reserved                                  *
+*                                                                                        *
+*  @brief    generate launch file dynamicly based on user configure.                     *
+*  @author   Haodong Yang                                                                *
+*  @version  1.0.2                                                                       *
+*  @date     2023.03.15                                                                  *
+*  @license  GNU General Public License (GPL)                                            *
+******************************************************************************************
+"""
+import rospy, tf
+from gazebo_msgs.srv import SpawnModel
+from geometry_msgs.msg import Quaternion, Pose, Point
+import xml.etree.ElementTree as ET
+from .xml_generate import XMLGenerator
+
+
+class ObstacleGenerator(XMLGenerator):
+    def __init__(self) -> None:
+        super().__init__()
+        if "plugins" in self.user_cfg.keys() and "obstacles" in self.user_cfg["plugins"] \
+            and self.user_cfg["plugins"]["obstacles"]:
+            self.obs_cfg = ObstacleGenerator.yamlParser(self.root_path + "user_config/" + self.user_cfg["plugins"]["obstacles"])
+        else:
+            self.obs_cfg = None
+        
+        self.box_obs, self.cylinder_obs, self.sphere_obs = [], [], []
+    
+    def plugin(self):
+        """
+        Implement of obstacles application.
+        """
+        app_register = []
+        if not self.obs_cfg is None:
+            '''app register'''
+            # obstacles generation
+            obs_gen = ObstacleGenerator.createElement("node", props={"pkg": "dynamic_xml_config", "type": "obstacles_generate_ros.py",
+                "name": "obstacles_generate", "args": "user_config.yaml","output": "screen"})
+            app_register.append(obs_gen)
+        
+        return app_register
+
+    def spawn(self) -> None:
+        """
+        ROS service for spawning static obstacles in Gazebo world.
+        """
+        rospy.wait_for_service("gazebo/spawn_urdf_model")
+        proxy = rospy.ServiceProxy("gazebo/spawn_urdf_model", SpawnModel)
+
+        for obs in self.obs_cfg["obstacles"]:
+            pose = [float(i) for i in obs["pose"].split()]
+            x, y, z, r, p, yaw = pose
+            orient = tf.transformations.quaternion_from_euler(r, p, yaw)
+            orient = Quaternion(orient[0], orient[1], orient[2], orient[3])
+            params = obs["props"]
+            params["c"] = obs["color"]
+            urdf = self.constructURDF(obs["type"], **params)
+
+            if obs["type"] == "BOX":
+                z = z if z else obs["props"]["h"] / 2
+                pose = Pose(Point(x=x, y=y, z=z), orient)
+                proxy(obs["type"] + str(len(self.box_obs)), urdf, "", pose, "world")
+                self.box_obs.append(obs)
+            elif obs["type"] == "CYLINDER":
+                z = z if z else obs["props"]["h"] / 2
+                pose = Pose(Point(x=x, y=y, z=z), orient)
+                proxy(obs["type"] + str(len(self.cylinder_obs)), urdf, "", pose, "world")
+                self.cylinder_obs.append(obs)
+            elif obs["type"] == "SPHERE":
+                z = z if z else obs["props"]["r"]
+                pose = Pose(Point(x=x, y=y, z=z), orient)
+                proxy(obs["type"] + str(len(self.sphere_obs)), urdf, "", pose, "world")
+                self.sphere_obs.append(obs)
+
+    def constructURDF(self, model_type: str, **kwargs) -> str:
+        """
+        Construct URDF of specific type of obstacle.
+
+        Parameters
+        ----------
+        model_type: str
+            specific type of obstacle. Optional is `BOX`, `CYLINDER` and `SPHERE`
+        kwargs: dict
+            parameters of obstacle, such as mass, color, height, etc.
+
+        Return
+        ----------
+        urdf_str: str
+            URDF of obstacle
+        """
+        # parameters checking
+        if model_type.upper() == "BOX":
+            assert "m" in kwargs and \
+                "w" in kwargs and \
+                "d" in kwargs and \
+                "h" in kwargs and \
+                "c" in kwargs,    \
+                "Parameters of {} are `m`, `w`, `d`, `h`, `c` which mean mass, \
+                    width, depth, height and color, ".format(model_type)
+            ixx = (kwargs["m"] / 12.0) * (pow(kwargs["d"], 2) + pow(kwargs["h"], 2))
+            iyy = (kwargs["m"] / 12.0) * (pow(kwargs["w"], 2) + pow(kwargs["h"], 2))
+            izz = (kwargs["m"] / 12.0) * (pow(kwargs["w"], 2) + pow(kwargs["d"], 2))
+            geometry = ObstacleGenerator.createElement("geometry")
+            geometry.append(ObstacleGenerator.createElement(model_type.lower(),
+                props={"size": "{:f} {:f} {:f}".format(kwargs["w"], kwargs["d"], kwargs["h"])}))
+        elif model_type.upper() == "CYLINDER":
+            assert "m" in kwargs and \
+                "r" in kwargs and \
+                "h" in kwargs and \
+                "c" in kwargs,    \
+                "Parameters of {} are `m`, `r`, `h`, `c` which mean mass, \
+                    radius, height and color, ".format(model_type)
+            ixx = (kwargs["m"] / 12.0) * (3 * pow(kwargs["r"], 2) + pow(kwargs["h"], 2))
+            iyy = (kwargs["m"] / 12.0) * (3 * pow(kwargs["r"], 2) + pow(kwargs["h"], 2))
+            izz = (kwargs["m"] * pow(kwargs["r"], 2)) / 2.0
+            geometry = ObstacleGenerator.createElement("geometry")
+            geometry.append(ObstacleGenerator.createElement(model_type.lower(), 
+                props={"length": str(kwargs["h"]), "radius": str(kwargs["r"])}))
+        elif model_type.upper() == "SPHERE":
+            assert "m" in kwargs and \
+                "r" in kwargs and \
+                "c" in kwargs,    \
+                "Parameters of {} are `m`, `r`, `h`, `c` which mean mass, \
+                    radius and color, ".format(model_type)
+            ixx = (2 * kwargs["m"] * pow(kwargs["r"], 2)) / 5.0
+            iyy = (2 * kwargs["m"] * pow(kwargs["r"], 2)) / 5.0
+            izz = (2 * kwargs["m"] * pow(kwargs["r"], 2)) / 5.0
+            geometry = ObstacleGenerator.createElement("geometry")
+            geometry.append(ObstacleGenerator.createElement("sphere", props={"radius": str(kwargs["r"])}))
+        else:
+            raise NotImplementedError
+
+        # URDF generation dynamically
+        static_obs = ObstacleGenerator.createElement("robot", props={"name": model_type.lower()})
+        link = ObstacleGenerator.createElement("link", props={"name": model_type.lower() + "_link"})
+
+        inertial = ObstacleGenerator.createElement("inertial")
+        inertial.append(ObstacleGenerator.createElement("origin", props={"xyz": "0 0 0", "rpy": "0 0 0"}))
+        inertial.append(ObstacleGenerator.createElement("mass", props={"value": str(kwargs["m"])}))
+        inertial.append(ObstacleGenerator.createElement("inertia", props={"ixx": str(ixx), "ixy": "0.0", "ixz": "0.0",
+            "iyy": str(iyy), "iyz": "0.0", "izz": str(izz)}))
+
+        collision = ObstacleGenerator.createElement("collision")
+        collision.append(ObstacleGenerator.createElement("origin", props={"xyz": "0 0 0", "rpy": "0 0 0"}))
+        
+        collision.append(geometry)
+
+        visual = ObstacleGenerator.createElement("visual")
+        visual.append(ObstacleGenerator.createElement("origin", props={"xyz": "0 0 0", "rpy": "0 0 0"}))
+        visual.append(geometry)
+        r, g, b, a = ObstacleGenerator.color(kwargs["c"])
+        visual.append(ObstacleGenerator.createElement("color", props={"rgba": "{:f} {:f} {:f} {:f}".format(r, g, b, a)}))
+
+        link.append(inertial)
+        link.append(collision)
+        link.append(visual)
+
+        gazebo = ObstacleGenerator.createElement("gazebo", props={"reference": model_type.lower() + "_link"})
+        gazebo.append(ObstacleGenerator.createElement("kp", text=str(100000.0)))
+        gazebo.append(ObstacleGenerator.createElement("kd", text=str(100000.0)))
+        gazebo.append(ObstacleGenerator.createElement("mu1", text=str(10.0)))
+        gazebo.append(ObstacleGenerator.createElement("mu2", text=str(10.0)))
+        gazebo.append(ObstacleGenerator.createElement("material", text="Gazebo/" + kwargs["c"]))
+
+        static_obs.append(link)
+        static_obs.append(gazebo)
+
+        ObstacleGenerator.indent(static_obs)
+
+        return ET.tostring(static_obs).decode()
+
+    @staticmethod
+    def color(color_name):
+        if color_name == "Blue":
+            return (0, 0, 0.8, 1)
+        elif color_name == "Red":
+            return (0.8, 0, 0, 1)
+        elif color_name == "Green":
+            return (0, 0.8, 0, 1)
+        elif color_name == "Grey":
+            return (0.75, 0.75, 0.75, 1)
+        elif color_name == "White":
+            return (1.0, 1.0, 1.0, 1)
+        elif color_name == "Black":
+            return (0, 0, 0, 1)
+        else:
+            return (0.75, 0.75, 0.75, 1)

simulators/third_party/dynamic_xml_config/plugins/obstacles_generate_ros.py

@@ -0,0 +1,25 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+""" 
+******************************************************************************************
+*  Copyright (C) 2023 Yang Haodong, All Rights Reserved                                  *
+*                                                                                        *
+*  @brief    generate launch file dynamicly based on user configure.                     *
+*  @author   Haodong Yang                                                                *
+*  @version  1.0.2                                                                       *
+*  @date     2023.03.15                                                                  *
+*  @license  GNU General Public License (GPL)                                            *
+******************************************************************************************
+"""
+import rospy
+import sys, os
+sys.path.append(os.path.abspath(os.path.join(__file__, "../../")))
+
+from plugins import ObstacleGenerator
+
+if __name__ == "__main__":
+    obstacles = ObstacleGenerator()
+
+    rospy.init_node("spawn_obstacles")
+    obstacles.spawn()

simulators/third_party/dynamic_xml_config/plugins/pedestrians_generate.py

@@ -0,0 +1,185 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+""" 
+******************************************************************************************
+*  Copyright (C) 2023 Yang Haodong, All Rights Reserved                                  *
+*                                                                                        *
+*  @brief    generate launch file dynamicly based on user configure.                     *
+*  @author   Haodong Yang                                                                *
+*  @version  1.0.2                                                                       *
+*  @date     2023.03.15                                                                  *
+*  @license  GNU General Public License (GPL)                                            *
+******************************************************************************************
+"""
+import xml.etree.ElementTree as ET
+from .xml_generate import XMLGenerator
+
+
+class PedGenerator(XMLGenerator):
+    def __init__(self) -> None:
+        super().__init__()
+        if "plugins" in self.user_cfg.keys() and "pedestrians" in self.user_cfg["plugins"] \
+            and self.user_cfg["plugins"]["pedestrians"]:
+            self.ped_cfg = PedGenerator.yamlParser(self.root_path + "user_config/" + self.user_cfg["plugins"]["pedestrians"])
+        else:
+            self.ped_cfg = None
+    
+    def __str__(self) -> str:
+        return "Pedestrians Generator"
+
+    def plugin(self):
+        """
+        Implement of pedestrian application.
+        """
+        app_register = []
+        if not self.ped_cfg is None:
+            '''dynamic confiugre'''
+            ped_path = self.root_path + "sim_env/worlds/" + self.user_cfg["world"] + "_with_pedestrians.world"
+            self.writePedestrianWorld(ped_path)
+
+            '''app register'''
+            # world generation
+            ped_world = PedGenerator.createElement("arg", props={"name": "world_parameter", 
+                "value": self.user_cfg["world"] + "_with_pedestrians"})
+            app_register.append(ped_world)
+        else:
+            # world generation
+            ped_world = PedGenerator.createElement("arg", props={"name": "world_parameter", "value": self.user_cfg["world"]})
+            app_register.append(ped_world)
+        
+        return app_register
+
+    def writePedestrianWorld(self, path):
+        """
+        Create configure file to construct pedestrians environment.
+
+        Parameters
+        ----------
+        path: str
+            the path of file(.launch.xml) to write.
+        """
+
+        def createHuman(config, index):
+            """
+            Create human element of world file.
+
+            Parameters
+            ----------
+            config: dict
+                configure data structure.
+            index: int
+                human index
+
+            Return
+            ----------
+            human: ET.Element
+                human element ptr
+            """
+
+            def createCollision(name, scale, pose=None):
+                if pose:
+                    props = {"scale": " ".join("%s" % s for s in scale), "pose": " ".join("%s" % p for p in pose)}
+                else:
+                    props = {"scale": " ".join("%s" % s for s in scale)}
+                return PedGenerator.createElement("collision", text=name, props=props)
+
+            # configure
+            sfm = config["social_force"]
+            human = config["pedestrians"]["ped_property"][index]
+            upd_rate = config["pedestrians"]["update_rate"]
+
+            # root: actor
+            actor = PedGenerator.createElement("actor", props={"name": human["name"]})
+
+            # human initial pose
+            pose = PedGenerator.createElement("pose", text=human["pose"])
+
+            # human skin
+            skin = PedGenerator.createElement("skin")
+            skin.append(PedGenerator.createElement("filename", text="walk.dae"))
+            skin.append(PedGenerator.createElement("scale", text="1.0"))
+
+            animation = PedGenerator.createElement("animation", props={"name": "walking"})
+            animation.append(PedGenerator.createElement("filename", text="walk.dae"))
+            animation.append(PedGenerator.createElement("scale", text="1.0"))
+            animation.append(PedGenerator.createElement("interpolate_x", text="true"))
+
+            # plugin
+            if not index:
+                plugin_visual = PedGenerator.createElement("plugin", props={"name": "pedestrian_visual", "filename": "libPedestrianVisualPlugin.so"})
+                plugin_visual.append(PedGenerator.createElement("update_rate", text=str(upd_rate)))
+            else:
+                plugin_visual = None
+
+            plugin = PedGenerator.createElement("plugin", props={"name": human["name"] + "_plugin", "filename": "libPedestrianSFMPlugin.so"})
+            plugin.append(createCollision("LHipJoint_LeftUpLeg_collision", [0.01, 0.001, 0.001]))
+            plugin.append(createCollision("LeftUpLeg_LeftLeg_collision", [8.0, 8.0, 1.0]))
+            plugin.append(createCollision("LeftLeg_LeftFoot_collision", [10.0, 10.0, 1.5]))
+            plugin.append(createCollision("LeftFoot_LeftToeBase_collision", [4.0, 4.0, 1.5]))
+            plugin.append(createCollision("RHipJoint_RightUpLeg_collision", [0.01, 0.001, 0.001]))
+            plugin.append(createCollision("RightUpLeg_RightLeg_collision", [8.0, 8.0, 1.0]))
+            plugin.append(createCollision("RightLeg_RightFoot_collision", [10.0, 10.0, 1.5]))
+            plugin.append(createCollision("RightFoot_RightToeBase_collision", [4.0, 4.0, 1.5]))
+            plugin.append(createCollision("Spine_Spine1_collision", [0.01, 0.001, 0.001]))
+            plugin.append(createCollision("Neck_Neck1_collision", [0.01, 0.001, 0.001]))
+            plugin.append(createCollision("Neck1_Head_collision", [5.0, 5.0, 3.0]))
+            plugin.append(createCollision("LeftShoulder_LeftArm_collision", [0.01, 0.001, 0.001]))
+            plugin.append(createCollision("LeftArm_LeftForeArm_collision", [5.0, 5.0, 1.0]))
+            plugin.append(createCollision("LeftForeArm_LeftHand_collision", [5.0, 5.0, 1.0]))
+            plugin.append(createCollision("LeftFingerBase_LeftHandIndex1_collision", [4.0, 4.0, 3.0]))
+            plugin.append(createCollision("RightShoulder_RightArm_collision", [0.01, 0.001, 0.001]))
+            plugin.append(createCollision("RightArm_RightForeArm_collision", [5.0, 5.0, 1.0]))
+            plugin.append(createCollision("RightForeArm_RightHand_collision", [5.0, 5.0, 1.0]))
+            plugin.append(createCollision("RightFingerBase_RightHandIndex1_collision", [4.0, 4.0, 3.0]))
+            plugin.append(createCollision("LowerBack_Spine_collision", [12.0, 20.0, 5.0], [0.05, 0, 0, 0, -0.2, 0]))
+
+            plugin.append(PedGenerator.createElement("velocity", text=str(human["velocity"])))
+            plugin.append(PedGenerator.createElement("radius", text=str(human["radius"])))
+            plugin.append(PedGenerator.createElement("cycle", text=str(human["cycle"])))
+            plugin.append(PedGenerator.createElement("animation_factor", text=str(sfm["animation_factor"])))
+            plugin.append(PedGenerator.createElement("people_distance", text=str(sfm["people_distance"])))
+            plugin.append(PedGenerator.createElement("goal_weight", text=str(sfm["goal_weight"])))
+            plugin.append(PedGenerator.createElement("obstacle_weight", text=str(sfm["obstacle_weight"])))
+            plugin.append(PedGenerator.createElement("social_weight", text=str(sfm["social_weight"])))
+            plugin.append(PedGenerator.createElement("group_gaze_weight", text=str(sfm["group_gaze_weight"])))
+            plugin.append(PedGenerator.createElement("group_coh_weight", text=str(sfm["group_coh_weight"])))
+            plugin.append(PedGenerator.createElement("group_rep_weight", text=str(sfm["group_rep_weight"])))
+            
+            if "time_delay" in human.keys():
+                plugin.append(PedGenerator.createElement("time_delay", text=str(human["time_delay"])))
+
+            ignore_obstacles = PedGenerator.createElement("ignore_obstacles")
+            for model in human["ignore"].values():
+                ignore_obstacles.append(PedGenerator.createElement("model", text=model))
+
+            trajectory = PedGenerator.createElement("trajectory")
+            for goal in human["trajectory"].values():
+                trajectory.append(PedGenerator.createElement("goalpoint", text=goal))
+
+            plugin.append(ignore_obstacles)
+            plugin.append(trajectory)
+
+            actor.append(pose)
+            actor.append(skin)
+            actor.append(animation)
+            actor.append(plugin)
+
+            if not plugin_visual is None:
+                actor.append(plugin_visual)
+            
+            PedGenerator.indent(actor)
+
+            return actor
+
+        if not self.ped_cfg is None:
+            world_file = self.root_path + "sim_env/worlds/" + self.user_cfg["world"] + ".world"
+            tree = ET.parse(world_file)
+            world = tree.getroot().find("world")
+
+            human_num = len(self.ped_cfg["pedestrians"]["ped_property"])
+            for i in range(human_num):
+                world.append(createHuman(self.ped_cfg, i))
+
+            with open(path, "wb+") as f:
+                tree.write(f, encoding="utf-8", xml_declaration=True)

simulators/third_party/dynamic_xml_config/plugins/robot_generate.py

@@ -0,0 +1,124 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+""" 
+******************************************************************************************
+*  Copyright (C) 2023 Yang Haodong, All Rights Reserved                                  *
+*                                                                                        *
+*  @brief    generate launch file dynamicly based on user configure.                     *
+*  @author   Haodong Yang                                                                *
+*  @version  1.0.2                                                                       *
+*  @date     2023.07.19                                                                  *
+*  @license  GNU General Public License (GPL)                                            *
+******************************************************************************************
+"""
+import xml.etree.ElementTree as ET
+from .xml_generate import XMLGenerator
+
+
+class RobotGenerator(XMLGenerator):
+    def __init__(self) -> None:
+        super().__init__()
+    
+    def __str__(self) -> str:
+        return "Robots Generator"
+    
+    def plugin(self):
+        """
+        Implement of robots starting application.
+        """
+        app_register = []
+        self.writeRobotsXml(self.root_path + "sim_env/launch/include/robots/start_robots.launch.xml")
+        return app_register
+
+    def writeRobotsXml(self, path):
+        """
+        Create configure file to start robots dynamically.
+
+        Parameters
+        ----------
+        path: str
+            the path of file(.launch.xml) to write.
+        """
+
+        def getRobotArg(name: str, index: int = -1):
+            if index == -1:
+                e = RobotGenerator.createElement(
+                    "arg", props={"name": name, "value": "$(eval arg('robot' + str(arg('robot_number')) + '_" + name + "'))"}
+                )
+            else:
+                e = RobotGenerator.createElement(
+                    "arg",
+                    props={
+                        "name": "robot" + str(index + 1) + "_" + name,
+                        "value": self.user_cfg["robots_config"][index]["robot" + str(index + 1) + "_" + name],
+                    },
+                )
+            return e
+
+        # root
+        launch = RobotGenerator.createElement("launch")
+
+        # setting the number of robots
+        if "robots_config" in self.user_cfg.keys():
+            robots_num = len(self.user_cfg["robots_config"])
+        else:
+            robots_num = 0
+            raise ValueError("There is no robot!")
+
+        launch.append(RobotGenerator.createElement("arg", props={"name": "robot_number", "default": str(robots_num)}))
+
+        # setting the parameters of robots
+        for i in range(robots_num):
+            # robotic type
+            launch.append(getRobotArg("type", i))
+            # global planner
+            launch.append(getRobotArg("global_planner", i))
+            # local planner
+            launch.append(getRobotArg("local_planner", i))
+            # robotic pose
+            launch.append(getRobotArg("x_pos", i))
+            launch.append(getRobotArg("y_pos", i))
+            launch.append(getRobotArg("z_pos", i))
+            launch.append(getRobotArg("yaw", i))
+
+        # create starting node
+        include = RobotGenerator.createElement("include", props={"file": "$(find sim_env)/launch/app/environment_single.launch.xml"})
+        include.append(
+            RobotGenerator.createElement("arg", props={"name": "robot", "value": "$(eval arg('robot' + str(arg('robot_number')) + '_type'))"})
+        )
+        # planner
+        include.append(getRobotArg("global_planner"))
+        include.append(getRobotArg("local_planner"))
+        # namespace
+        include.append(RobotGenerator.createElement("arg", props={"name": "robot_namespace", "value": "robot$(arg robot_number)"}))
+        if robots_num > 1:
+            include.append(RobotGenerator.createElement("arg", props={"name": "start_ns", "value": "true"}))
+        else:
+            include.append(RobotGenerator.createElement("arg", props={"name": "start_ns", "value": "false"}))
+        # pose
+        include.append(
+            RobotGenerator.createElement("arg", props={"name": "robot_x", "value": "$(eval arg('robot' + str(arg('robot_number')) + '_x_pos'))"})
+        )
+        include.append(
+            RobotGenerator.createElement("arg", props={"name": "robot_y", "value": "$(eval arg('robot' + str(arg('robot_number')) + '_y_pos'))"})
+        )
+        include.append(
+            RobotGenerator.createElement("arg", props={"name": "robot_z", "value": "$(eval arg('robot' + str(arg('robot_number')) + '_z_pos'))"})
+        )
+        include.append(
+            RobotGenerator.createElement("arg", props={"name": "robot_yaw", "value": "$(eval arg('robot' + str(arg('robot_number')) + '_yaw'))"})
+        )
+
+        # recursive start
+        cycle = RobotGenerator.createElement(
+            "include", props={"file": "$(find sim_env)/launch/include/robots/start_robots.launch.xml", "if": "$(eval arg('robot_number') > 1)"}
+        )
+        cycle.append(RobotGenerator.createElement("arg", props={"name": "robot_number", "value": "$(eval arg('robot_number') - 1)"}))
+
+        launch.append(include)
+        launch.append(cycle)
+        RobotGenerator.indent(launch)
+
+        with open(path, "wb+") as f:
+            ET.ElementTree(launch).write(f, encoding="utf-8", xml_declaration=True)

simulators/third_party/dynamic_xml_config/plugins/xml_generate.py

@@ -0,0 +1,88 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+""" 
+******************************************************************************************
+*  Copyright (C) 2023 Yang Haodong, All Rights Reserved                                  *
+*                                                                                        *
+*  @brief    generate launch file dynamicly based on user configure.                     *
+*  @author   Haodong Yang                                                                *
+*  @version  1.0.2                                                                       *
+*  @date     2023.04.23                                                                  *
+*  @license  GNU General Public License (GPL)                                            *
+******************************************************************************************
+"""
+import yaml
+import xml.etree.ElementTree as ET
+import sys, os
+
+from abc import abstractmethod, ABC
+
+class XMLGenerator(ABC):
+    def __init__(self) -> None:
+        # get the root path of package(src layer)
+        self.root_path = os.path.split(os.path.realpath(__file__))[0] + "/../../../"
+        # user configure
+        self.user_cfg = XMLGenerator.yamlParser(self.root_path + "user_config/" + sys.argv[1])
+
+    @abstractmethod
+    def plugin(self):
+        """
+        [interface] Implement of specific application.
+
+        Return
+        ----------
+        app_register: list of ET.Element to register
+        """
+        pass
+
+    @staticmethod
+    def yamlParser(path: str) -> dict:
+        """
+        Parser user configure file(.yaml).
+
+        Parameters
+        ----------
+        path: str
+            the path of file(.yaml).
+
+        Return
+        ----------
+        data: dict
+            user configuer tree
+        """
+        with open(path, "r") as f:
+            data = yaml.load(f, Loader=yaml.FullLoader)
+        return data
+
+    @staticmethod
+    def indent(elem: ET.Element, level: int = 0) -> None:
+        """
+        Format the generated xml document.
+
+        Parameters
+        ----------
+        elem: ET.Element
+        level: int
+            indent level.
+        """
+        i = "\n" + level * "\t"
+        if len(elem):
+            if not elem.text or not elem.text.strip():
+                elem.text = i + "\t"
+            if not elem.tail or not elem.tail.strip():
+                elem.tail = i
+            for elem in elem:
+                XMLGenerator.indent(elem, level + 1)
+            if not elem.tail or not elem.tail.strip():
+                elem.tail = i
+        else:
+            if level and (not elem.tail or not elem.tail.strip()):
+                elem.tail = i
+
+    @staticmethod
+    def createElement(name: str, text: str = None, props: dict = {}) -> ET.Element:
+        e = ET.Element(name, attrib=props)
+        if not text is None:
+            e.text = text
+        return e

simulators/third_party/gazebo_plugins/pedestrian_sfm_plugin/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(gazebo_sfm_plugin)
+
+add_compile_options(-std=c++17)
+
+find_package(catkin REQUIRED COMPONENTS
+  gazebo_ros
+  roscpp
+  message_generation
+)
+
+find_package(Boost REQUIRED COMPONENTS thread)
+find_package(gazebo REQUIRED)
+
+add_service_files(
+  FILES
+  ped_state.srv
+)
+
+generate_messages(
+  DEPENDENCIES
+)
+
+include_directories(include)
+include_directories(SYSTEM
+  /usr/local/include  #to find lightsfm
+  ${Boost_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+  ${GAZEBO_INCLUDE_DIRS}
+)
+
+link_directories(
+  ${catkin_LIBRARY_DIRS}
+  ${GAZEBO_LIBRARY_DIRS}
+)
+
+
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES PedestrianSFMPlugin
+  CATKIN_DEPENDS gazebo_ros roscpp
+)
+
+
+add_library(PedestrianSFMPlugin src/pedestrian_sfm_plugin.cpp)
+add_dependencies(PedestrianSFMPlugin gazebo_sfm_plugin_generate_messages_cpp)
+target_link_libraries(PedestrianSFMPlugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) #${Boost_LIBRARIES
+
+install(TARGETS
+  PedestrianSFMPlugin
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+

simulators/third_party/gazebo_plugins/pedestrian_sfm_plugin/include/lightsfm/angle.hpp

@@ -0,0 +1,203 @@
+/***********************************************************************/
+/**                                                                    */
+/** angle.hpp                                                          */
+/**                                                                    */
+/** Copyright (c) 2016, Service Robotics Lab.                          */
+/**                     http://robotics.upo.es                         */
+/**                                                                    */
+/** All rights reserved.                                               */
+/**                                                                    */
+/** Authors:                                                           */
+/** Ignacio Perez-Hurtado (maintainer)                                 */
+/** Jesus Capitan                                                      */
+/** Fernando Caballero                                                 */
+/** Luis Merino                                                        */
+/**                                                                    */
+/** This software may be modified and distributed under the terms      */
+/** of the BSD license. See the LICENSE file for details.              */
+/**                                                                    */
+/** http://www.opensource.org/licenses/BSD-3-Clause                    */
+/**                                                                    */
+/***********************************************************************/
+
+#ifndef _ANGLE_HPP_
+#define _ANGLE_HPP_
+
+#include <iostream>
+#include <cmath>
+
+namespace utils
+{
+class Angle
+{
+public:
+  enum AngleRange
+  {
+    // [0, 2*pi) or [0°, 360°)
+    PositiveOnlyRange,
+    // (-pi, +pi] or (-180°, 180°]
+    PositiveNegativeRange
+  };
+
+  Angle() : value(0)
+  {
+  }
+  virtual ~Angle()
+  {
+  }
+
+  static Angle fromRadian(double value)
+  {
+    return Angle(value);
+  }
+
+  static Angle fromDegree(double value)
+  {
+    return Angle(value / 180 * M_PI);
+  }
+
+  double toRadian(AngleRange range = PositiveNegativeRange) const
+  {
+    if (range == PositiveNegativeRange)
+    {
+      return value;
+    }
+    else
+    {
+      return (value >= 0) ? value : (value + 2 * M_PI);
+    }
+  }
+
+  double cos() const
+  {
+    return std::cos(value);
+  }
+
+  double sin() const
+  {
+    return std::sin(value);
+  }
+
+  double toDegree(AngleRange range = PositiveNegativeRange) const
+  {
+    double degreeValue = value * 180 / M_PI;
+    if (range == PositiveNegativeRange)
+    {
+      return degreeValue;
+    }
+    else
+    {
+      return (degreeValue >= 0) ? degreeValue : (degreeValue + 360);
+    }
+  }
+
+  void setRadian(double value)
+  {
+    Angle::value = value;
+    normalize();
+  }
+
+  void setDegree(double value)
+  {
+    Angle::value = value / 180 * M_PI;
+    normalize();
+  }
+
+  int sign() const
+  {
+    if (value == 0)
+    {
+      return 0;
+    }
+    else if (value > 0)
+    {
+      return 1;
+    }
+    else
+    {
+      return -1;
+    }
+  }
+
+  Angle operator+(const Angle& other) const
+  {
+    return Angle(value + other.value);
+  }
+
+  Angle operator-(const Angle& other) const
+  {
+    return Angle(value - other.value);
+  }
+
+  Angle& operator+=(const Angle& other)
+  {
+    value += other.value;
+    normalize();
+    return *this;
+  }
+
+  Angle& operator-=(const Angle& other)
+  {
+    value -= other.value;
+    normalize();
+    return *this;
+  }
+
+  bool operator==(const Angle& other) const
+  {
+    return value == other.value;
+  }
+
+  bool operator!=(const Angle& other) const
+  {
+    return value != other.value;
+  }
+
+  bool operator<(const Angle& other) const
+  {
+    return value < other.value;
+  }
+
+  bool operator<=(const Angle& other) const
+  {
+    return value <= other.value;
+  }
+
+  bool operator>(const Angle& other) const
+  {
+    return value > other.value;
+  }
+
+  bool operator>=(const Angle& other) const
+  {
+    return value >= other.value;
+  }
+
+private:
+  Angle(double value)
+  {
+    Angle::value = value;
+    normalize();
+  }
+
+  void normalize()
+  {
+    while (value <= -M_PI)
+      value += 2 * M_PI;
+    while (value > M_PI)
+      value -= 2 * M_PI;
+  }
+
+  double value;
+};
+}  // namespace utils
+namespace std
+{
+inline ostream& operator<<(ostream& stream, const utils::Angle& alpha)
+{
+  stream << alpha.toRadian();
+  return stream;
+}
+}  // namespace std
+
+#endif

simulators/third_party/gazebo_plugins/pedestrian_sfm_plugin/include/lightsfm/map.hpp

@@ -0,0 +1,53 @@
+/***********************************************************************/
+/**                                                                    */
+/** map.hpp                                                            */
+/**                                                                    */
+/** Copyright (c) 2016, Service Robotics Lab.                          */
+/**                     http://robotics.upo.es                         */
+/**                                                                    */
+/** All rights reserved.                                               */
+/**                                                                    */
+/** Authors:                                                           */
+/** Ignacio Perez-Hurtado (maintainer)                                 */
+/** Jesus Capitan                                                      */
+/** Fernando Caballero                                                 */
+/** Luis Merino                                                        */
+/**                                                                    */
+/** This software may be modified and distributed under the terms      */
+/** of the BSD license. See the LICENSE file for details.              */
+/**                                                                    */
+/** http://www.opensource.org/licenses/BSD-3-Clause                    */
+/**                                                                    */
+/***********************************************************************/
+
+#ifndef _MAP_HPP_
+#define _MAP_HPP_
+
+#include "vector2d.hpp"
+
+namespace sfm
+{
+class Map
+{
+public:
+  struct Obstacle
+  {
+    Obstacle() : distance(-1)
+    {
+    }
+    double distance;
+    utils::Vector2d position;
+  };
+
+  Map()
+  {
+  }
+  virtual ~Map()
+  {
+  }
+  virtual const Obstacle& getNearestObstacle(const utils::Vector2d& x) = 0;
+  virtual bool isObstacle(const utils::Vector2d& x) const = 0;
+};
+}  // namespace sfm
+
+#endif

simulators/third_party/gazebo_plugins/pedestrian_sfm_plugin/include/lightsfm/sfm.hpp

@@ -0,0 +1,619 @@
+/***********************************************************************/
+/**                                                                    */
+/** sfm.hpp                                                            */
+/**                                                                    */
+/** Copyright (c) 2016, Service Robotics Lab.                          */
+/**                     http://robotics.upo.es                         */
+/**                                                                    */
+/** All rights reserved.                                               */
+/**                                                                    */
+/** Authors:                                                           */
+/** Ignacio Perez-Hurtado (maintainer)                                 */
+/** Jesus Capitan                                                      */
+/** Fernando Caballero                                                 */
+/** Luis Merino                                                        */
+/**                                                                    */
+/** This software may be modified and distributed under the terms      */
+/** of the BSD license. See the LICENSE file for details.              */
+/**                                                                    */
+/** http://www.opensource.org/licenses/BSD-3-Clause                    */
+/**                                                                    */
+/***********************************************************************/
+
+#ifndef _SFM_HPP_
+#define _SFM_HPP_
+
+#include "map.hpp"
+#include "vector2d.hpp"
+#include <cmath>
+#include <unordered_map>
+#include <vector>
+
+namespace sfm
+{
+struct Forces
+{
+  utils::Vector2d desiredForce;
+  utils::Vector2d obstacleForce;
+  utils::Vector2d socialForce;
+  utils::Vector2d groupGazeForce;
+  utils::Vector2d groupCoherenceForce;
+  utils::Vector2d groupRepulsionForce;
+  utils::Vector2d groupForce;
+  utils::Vector2d globalForce;
+  utils::Vector2d robotSocialForce;
+};
+
+struct Parameters
+{
+  Parameters()
+    : forceFactorDesired(2.0)
+    , forceFactorObstacle(10)
+    , forceSigmaObstacle(0.2)
+    , forceFactorSocial(2.1)
+    , forceFactorGroupGaze(3.0)
+    , forceFactorGroupCoherence(2.0)
+    , forceFactorGroupRepulsion(1.0)
+    , lambda(2.0)
+    , gamma(0.35)
+    , n(2.0)
+    , nPrime(3.0)
+    , relaxationTime(0.5)
+  {
+  }
+
+  double forceFactorDesired;
+  double forceFactorObstacle;
+  double forceSigmaObstacle;
+  double forceFactorSocial;
+  double forceFactorGroupGaze;
+  double forceFactorGroupCoherence;
+  double forceFactorGroupRepulsion;
+  double lambda;
+  double gamma;
+  double n;
+  double nPrime;
+  double relaxationTime;
+};
+
+struct Goal
+{
+  utils::Vector2d center;
+  double radius;
+};
+
+struct Agent
+{
+  Agent()
+    : desiredVelocity(0.6)
+    , radius(0.35)
+    , cyclicGoals(false)
+    , teleoperated(false)
+    , antimove(false)
+    , linearVelocity(0)
+    , angularVelocity(0)
+    , groupId(-1)
+  {
+  }
+
+  Agent(double linearVelocity, double angularVelocity)
+    : desiredVelocity(0.6)
+    , radius(0.35)
+    , cyclicGoals(false)
+    , teleoperated(true)
+    , antimove(false)
+    , linearVelocity(linearVelocity)
+    , angularVelocity(angularVelocity)
+    , groupId(-1)
+  {
+  }
+
+  Agent(const utils::Vector2d& position, const utils::Angle& yaw, double linearVelocity, double angularVelocity)
+    : position(position)
+    , yaw(yaw)
+    , desiredVelocity(0.6)
+    , radius(0.35)
+    , cyclicGoals(false)
+    , teleoperated(true)
+    , antimove(false)
+    , linearVelocity(linearVelocity)
+    , angularVelocity(angularVelocity)
+    , groupId(-1)
+  {
+  }
+
+  void move(double dt);  // only if teleoperated
+
+  utils::Vector2d position;
+  utils::Vector2d velocity;
+  utils::Angle yaw;
+
+  utils::Vector2d movement;
+
+  double desiredVelocity;
+  double radius;
+
+  std::list<Goal> goals;
+  bool cyclicGoals;
+
+  bool teleoperated;
+  bool antimove;
+  double linearVelocity;
+  double angularVelocity;
+
+  int groupId;
+
+  int id;
+
+  Forces forces;
+  Parameters params;
+  std::vector<utils::Vector2d> obstacles1;
+  std::vector<utils::Vector2d> obstacles2;
+};
+
+struct Group
+{
+  utils::Vector2d center;
+  std::vector<unsigned> agents;
+};
+
+class SocialForceModel
+{
+public:
+  SocialForceModel(SocialForceModel const&) = delete;
+  void operator=(SocialForceModel const&) = delete;
+  ~SocialForceModel()
+  {
+  }
+
+  static SocialForceModel& getInstance()
+  {
+    static SocialForceModel singleton;
+    return singleton;
+  }
+
+#define SFM SocialForceModel::getInstance()
+
+  std::vector<Agent>& computeForces(std::vector<Agent>& agents, Map* map = NULL) const;
+  void computeForces(Agent& me, std::vector<Agent>& agents, Map* map = NULL);
+  std::vector<Agent>& updatePosition(std::vector<Agent>& agents, double dt) const;
+  void updatePosition(Agent& me, double dt) const;
+
+private:
+#define PW(x) ((x) * (x))
+  SocialForceModel()
+  {
+  }
+  utils::Vector2d computeDesiredForce(Agent& agent) const;
+  void computeObstacleForce(Agent& agent, Map* map) const;
+  void computeSocialForce(unsigned index, std::vector<Agent>& agents) const;
+  void computeSocialForce(Agent& agent, std::vector<Agent>& agents) const;
+  void computeGroupForce(unsigned index, const utils::Vector2d& desiredDirection, std::vector<Agent>& agents,
+                         const std::unordered_map<int, Group>& groups) const;
+  void computeGroupForce(Agent& me, const utils::Vector2d& desiredDirection, std::vector<Agent>& agents,
+                         Group& group) const;
+};
+
+inline utils::Vector2d SocialForceModel::computeDesiredForce(Agent& agent) const
+{
+  utils::Vector2d desiredDirection;
+  if (!agent.goals.empty() && (agent.goals.front().center - agent.position).norm() > agent.goals.front().radius)
+  {
+    utils::Vector2d diff = agent.goals.front().center - agent.position;
+    desiredDirection = diff.normalized();
+    agent.forces.desiredForce = agent.params.forceFactorDesired *
+                                (desiredDirection * agent.desiredVelocity - agent.velocity) /
+                                agent.params.relaxationTime;
+    agent.antimove = false;
+  }
+  else
+  {
+    agent.forces.desiredForce = -agent.velocity / agent.params.relaxationTime;
+    agent.antimove = true;
+  }
+  return desiredDirection;
+}
+
+inline void SocialForceModel::computeObstacleForce(Agent& agent, Map* map) const
+{
+  // Initially, the obstacles were expected to be in robot local frame. We have
+  // replaced it to work in the same frame of the other forces.
+  //   if (agent.obstacles1.size() > 0 || agent.obstacles2.size() > 0) {
+  //     agent.forces.obstacleForce.set(0, 0);
+  //     for (unsigned i = 0; i < agent.obstacles1.size(); i++) {
+  //       double distance = agent.obstacles1[i].norm() - agent.radius;
+  //       agent.forces.obstacleForce +=
+  //           agent.params.forceFactorObstacle *
+  //           std::exp(-distance / agent.params.forceSigmaObstacle) *
+  //           (-agent.obstacles1[i]).normalized();
+  //     }
+  //     for (unsigned i = 0; i < agent.obstacles2.size(); i++) {
+  //       double distance = agent.obstacles2[i].norm() - agent.radius;
+  //       agent.forces.obstacleForce +=
+  //           agent.params.forceFactorObstacle *
+  //           std::exp(-distance / agent.params.forceSigmaObstacle) *
+  //           (-agent.obstacles2[i]).normalized();
+  //     }
+  //     agent.forces.obstacleForce /=
+  //         (double)(agent.obstacles1.size() + agent.obstacles2.size());
+  if (agent.obstacles1.size() > 0 || agent.obstacles2.size() > 0)
+  {
+    agent.forces.obstacleForce.set(0, 0);
+    for (unsigned i = 0; i < agent.obstacles1.size(); i++)
+    {
+      utils::Vector2d minDiff = agent.position - agent.obstacles1[i];
+      double distance = minDiff.norm() - agent.radius;
+      agent.forces.obstacleForce += agent.params.forceFactorObstacle *
+                                    std::exp(-distance / agent.params.forceSigmaObstacle) * minDiff.normalized();
+    }
+    for (unsigned i = 0; i < agent.obstacles2.size(); i++)
+    {
+      utils::Vector2d minDiff = agent.position - agent.obstacles2[i];
+      double distance = minDiff.norm() - agent.radius;
+      agent.forces.obstacleForce += agent.params.forceFactorObstacle *
+                                    std::exp(-distance / agent.params.forceSigmaObstacle) * minDiff.normalized();
+    }
+    agent.forces.obstacleForce /= (double)(agent.obstacles1.size() + agent.obstacles2.size());
+  }
+  else if (map != NULL)
+  {
+    const Map::Obstacle& obs = map->getNearestObstacle(agent.position);
+    utils::Vector2d minDiff = agent.position - obs.position;
+    double distance = minDiff.norm() - agent.radius;
+    agent.forces.obstacleForce =
+        agent.params.forceFactorObstacle * std::exp(-distance / agent.params.forceSigmaObstacle) * minDiff.normalized();
+  }
+  else
+  {
+    agent.forces.obstacleForce.set(0, 0);
+  }
+}
+
+inline void SocialForceModel::computeSocialForce(unsigned index, std::vector<Agent>& agents) const
+{
+  Agent& agent = agents[index];
+  agent.forces.socialForce.set(0, 0);
+  for (unsigned i = 0; i < agents.size(); i++)
+  {
+    if (i == index)
+    {
+      continue;
+    }
+    utils::Vector2d diff = agents[i].position - agent.position;
+    utils::Vector2d diffDirection = diff.normalized();
+    utils::Vector2d velDiff = agent.velocity - agents[i].velocity;
+    utils::Vector2d interactionVector = agent.params.lambda * velDiff + diffDirection;
+    double interactionLength = interactionVector.norm();
+    utils::Vector2d interactionDirection = interactionVector / interactionLength;
+    utils::Angle theta = interactionDirection.angleTo(diffDirection);
+    double B = agent.params.gamma * interactionLength;
+    double thetaRad = theta.toRadian();
+    double forceVelocityAmount = -std::exp(-diff.norm() / B - PW(agent.params.nPrime * B * thetaRad));
+    double forceAngleAmount = -theta.sign() * std::exp(-diff.norm() / B - PW(agent.params.n * B * thetaRad));
+    utils::Vector2d forceVelocity = forceVelocityAmount * interactionDirection;
+    utils::Vector2d forceAngle = forceAngleAmount * interactionDirection.leftNormalVector();
+    agent.forces.socialForce += agent.params.forceFactorSocial * (forceVelocity + forceAngle);
+    if (i == 0)
+    {
+      agent.forces.robotSocialForce = agent.params.forceFactorSocial * (forceVelocity + forceAngle);
+    }
+  }
+}
+
+inline void SocialForceModel::computeSocialForce(Agent& me, std::vector<Agent>& agents) const
+{
+  // Agent& agent = agents[index];
+  me.forces.socialForce.set(0, 0);
+  for (unsigned i = 0; i < agents.size(); i++)
+  {
+    if (agents[i].id == me.id)
+    {
+      continue;
+    }
+    utils::Vector2d diff = agents[i].position - me.position;
+    utils::Vector2d diffDirection = diff.normalized();
+    utils::Vector2d velDiff = me.velocity - agents[i].velocity;
+    utils::Vector2d interactionVector = me.params.lambda * velDiff + diffDirection;
+    double interactionLength = interactionVector.norm();
+    utils::Vector2d interactionDirection = interactionVector / interactionLength;
+    utils::Angle theta = interactionDirection.angleTo(diffDirection);
+    double B = me.params.gamma * interactionLength;
+    double thetaRad = theta.toRadian();
+    double forceVelocityAmount = -std::exp(-diff.norm() / B - PW(me.params.nPrime * B * thetaRad));
+    double forceAngleAmount = -theta.sign() * std::exp(-diff.norm() / B - PW(me.params.n * B * thetaRad));
+    utils::Vector2d forceVelocity = forceVelocityAmount * interactionDirection;
+    utils::Vector2d forceAngle = forceAngleAmount * interactionDirection.leftNormalVector();
+    me.forces.socialForce += me.params.forceFactorSocial * (forceVelocity + forceAngle);
+    // if (i == 0)
+    //{
+    //  agent.forces.robotSocialForce =
+    //      agent.params.forceFactorSocial * (forceVelocity + forceAngle);
+    //}
+  }
+}
+
+inline void SocialForceModel::computeGroupForce(unsigned index, const utils::Vector2d& desiredDirection,
+                                                std::vector<Agent>& agents,
+                                                const std::unordered_map<int, Group>& groups) const
+{
+  Agent& agent = agents[index];
+  agent.forces.groupForce.set(0, 0);
+  agent.forces.groupGazeForce.set(0, 0);
+  agent.forces.groupCoherenceForce.set(0, 0);
+  agent.forces.groupRepulsionForce.set(0, 0);
+  if (groups.count(agent.groupId) == 0 || groups.at(agent.groupId).agents.size() < 2)
+  {
+    return;
+  }
+  const Group& group = groups.at(agent.groupId);
+
+  // Gaze force
+  utils::Vector2d com = group.center;
+  com = (1 / (double)(group.agents.size() - 1)) * (group.agents.size() * com - agent.position);
+
+  utils::Vector2d relativeCom = com - agent.position;
+  utils::Angle visionAngle = utils::Angle::fromDegree(90);
+  double elementProduct = desiredDirection.dot(relativeCom);
+  utils::Angle comAngle =
+      utils::Angle::fromRadian(std::acos(elementProduct / (desiredDirection.norm() * relativeCom.norm())));
+  if (comAngle > visionAngle)
+  {
+#ifdef _PAPER_VERSION_
+    utils::Angle necessaryRotation = comAngle - visionAngle;
+    agent.forces.groupGazeForce = -necessaryRotation.toRadian() * desiredDirection;
+#else
+    double desiredDirectionSquared = desiredDirection.squaredNorm();
+    double desiredDirectionDistance = elementProduct / desiredDirectionSquared;
+    agent.forces.groupGazeForce = desiredDirectionDistance * desiredDirection;
+#endif
+    agent.forces.groupGazeForce *= agent.params.forceFactorGroupGaze;
+  }
+
+  // Coherence force
+  com = group.center;
+  relativeCom = com - agent.position;
+  double distance = relativeCom.norm();
+  double maxDistance = ((double)group.agents.size() - 1) / 2;
+#ifdef _PAPER_VERSION_
+  if (distance >= maxDistance)
+  {
+    agent.forces.groupCoherenceForce = relativeCom.normalized();
+    agent.forces.groupCoherenceForce *= agent.params.forceFactorGroupCoherence;
+  }
+#else
+  agent.forces.groupCoherenceForce = relativeCom;
+  double softenedFactor = agent.params.forceFactorGroupCoherence * (std::tanh(distance - maxDistance) + 1) / 2;
+  agent.forces.groupCoherenceForce *= softenedFactor;
+#endif
+
+  // Repulsion Force
+  for (unsigned i = 0; i < group.agents.size(); i++)
+  {
+    if (index == group.agents[i])
+    {
+      continue;
+    }
+    utils::Vector2d diff = agent.position - agents.at(group.agents[i]).position;
+    if (diff.norm() < agent.radius + agents.at(group.agents[i]).radius)
+    {
+      agent.forces.groupRepulsionForce += diff;
+    }
+  }
+  agent.forces.groupRepulsionForce *= agent.params.forceFactorGroupRepulsion;
+
+  // Group Force
+  agent.forces.groupForce =
+      agent.forces.groupGazeForce + agent.forces.groupCoherenceForce + agent.forces.groupRepulsionForce;
+}
+
+inline void SocialForceModel::computeGroupForce(Agent& me, const utils::Vector2d& desiredDirection,
+                                                std::vector<Agent>& agents, Group& group) const
+{
+  // Agent& agent = agents[index];
+  me.forces.groupForce.set(0, 0);
+  me.forces.groupGazeForce.set(0, 0);
+  me.forces.groupCoherenceForce.set(0, 0);
+  me.forces.groupRepulsionForce.set(0, 0);
+  if (group.agents.size() < 2)
+  {
+    return;
+  }
+
+  // Gaze force
+  utils::Vector2d com = group.center;
+  com = (1 / (double)(group.agents.size() - 1)) * (group.agents.size() * com - me.position);
+
+  utils::Vector2d relativeCom = com - me.position;
+  utils::Angle visionAngle = utils::Angle::fromDegree(90);
+  double elementProduct = desiredDirection.dot(relativeCom);
+  utils::Angle comAngle =
+      utils::Angle::fromRadian(std::acos(elementProduct / (desiredDirection.norm() * relativeCom.norm())));
+  if (comAngle > visionAngle)
+  {
+#ifdef _PAPER_VERSION_
+    utils::Angle necessaryRotation = comAngle - visionAngle;
+    me.forces.groupGazeForce = -necessaryRotation.toRadian() * desiredDirection;
+#else
+    double desiredDirectionSquared = desiredDirection.squaredNorm();
+    double desiredDirectionDistance = elementProduct / desiredDirectionSquared;
+    me.forces.groupGazeForce = desiredDirectionDistance * desiredDirection;
+#endif
+    me.forces.groupGazeForce *= me.params.forceFactorGroupGaze;
+  }
+
+  // Coherence force
+  com = group.center;
+  relativeCom = com - me.position;
+  double distance = relativeCom.norm();
+  double maxDistance = ((double)group.agents.size() - 1) / 2;
+#ifdef _PAPER_VERSION_
+  if (distance >= maxDistance)
+  {
+    me.forces.groupCoherenceForce = relativeCom.normalized();
+    me.forces.groupCoherenceForce *= me.params.forceFactorGroupCoherence;
+  }
+#else
+  me.forces.groupCoherenceForce = relativeCom;
+  double softenedFactor = me.params.forceFactorGroupCoherence * (std::tanh(distance - maxDistance) + 1) / 2;
+  me.forces.groupCoherenceForce *= softenedFactor;
+#endif
+
+  // Repulsion Force
+  // Index 0 -> me
+  for (unsigned i = 1; i < group.agents.size(); i++)
+  {
+    utils::Vector2d diff = me.position - agents.at(group.agents[i]).position;
+    if (diff.norm() < me.radius + agents.at(group.agents[i]).radius)
+    {
+      me.forces.groupRepulsionForce += diff;
+    }
+  }
+  me.forces.groupRepulsionForce *= me.params.forceFactorGroupRepulsion;
+
+  // Group Force
+  me.forces.groupForce = me.forces.groupGazeForce + me.forces.groupCoherenceForce + me.forces.groupRepulsionForce;
+}
+
+inline std::vector<Agent>& SocialForceModel::computeForces(std::vector<Agent>& agents, Map* map) const
+{
+  std::unordered_map<int, Group> groups;
+  for (unsigned i = 0; i < agents.size(); i++)
+  {
+    if (agents[i].groupId < 0)
+    {
+      continue;
+    }
+    groups[agents[i].groupId].agents.push_back(i);
+    groups[agents[i].groupId].center += agents[i].position;
+  }
+  for (auto it = groups.begin(); it != groups.end(); ++it)
+  {
+    it->second.center /= (double)(it->second.agents.size());
+  }
+
+  for (unsigned i = 0; i < agents.size(); i++)
+  {
+    utils::Vector2d desiredDirection = computeDesiredForce(agents[i]);
+    computeObstacleForce(agents[i], map);
+    computeSocialForce(i, agents);
+    computeGroupForce(i, desiredDirection, agents, groups);
+    agents[i].forces.globalForce = agents[i].forces.desiredForce + agents[i].forces.socialForce +
+                                   agents[i].forces.obstacleForce + agents[i].forces.groupForce;
+  }
+  return agents;
+}
+
+inline void SocialForceModel::computeForces(Agent& me, std::vector<Agent>& agents, Map* map)
+{
+  // form the group
+  Group mygroup;
+  if (me.groupId != -1)
+  {
+    mygroup.agents.push_back(me.id);
+    mygroup.center = me.position;
+    for (unsigned i = 0; i < agents.size(); i++)
+    {
+      if (agents[i].id == me.id)
+      {
+        continue;
+      }
+      if (agents[i].groupId == me.groupId)
+      {
+        mygroup.agents.push_back(i);
+        mygroup.center += agents[i].position;
+      }
+    }
+    mygroup.center /= (double)mygroup.agents.size();
+  }
+
+  // Compute agent's forces
+  utils::Vector2d desiredDirection = computeDesiredForce(me);
+  computeObstacleForce(me, map);
+  computeSocialForce(me, agents);
+  computeGroupForce(me, desiredDirection, agents, mygroup);
+  me.forces.globalForce =
+      me.forces.desiredForce + me.forces.socialForce + me.forces.obstacleForce + me.forces.groupForce;
+}
+
+inline void Agent::move(double dt)
+{
+  double imd = linearVelocity * dt;
+  utils::Vector2d inc(imd * std::cos(yaw.toRadian() + angularVelocity * dt * 0.5),
+                      imd * std::sin(yaw.toRadian() + angularVelocity * dt * 0.5));
+  yaw += utils::Angle::fromRadian(angularVelocity * dt);
+  position += inc;
+  velocity.set(linearVelocity * yaw.cos(), linearVelocity * yaw.sin());
+}
+
+inline std::vector<Agent>& SocialForceModel::updatePosition(std::vector<Agent>& agents, double dt) const
+{
+  for (unsigned i = 0; i < agents.size(); i++)
+  {
+    utils::Vector2d initPos = agents[i].position;
+    if (agents[i].teleoperated)
+    {
+      double imd = agents[i].linearVelocity * dt;
+      utils::Vector2d inc(imd * std::cos(agents[i].yaw.toRadian() + agents[i].angularVelocity * dt * 0.5),
+                          imd * std::sin(agents[i].yaw.toRadian() + agents[i].angularVelocity * dt * 0.5));
+      agents[i].yaw += utils::Angle::fromRadian(agents[i].angularVelocity * dt);
+      agents[i].position += inc;
+      agents[i].velocity.set(agents[i].linearVelocity * agents[i].yaw.cos(),
+                             agents[i].linearVelocity * agents[i].yaw.sin());
+    }
+    else
+    {
+      agents[i].velocity += agents[i].forces.globalForce * dt;
+      if (agents[i].velocity.norm() > agents[i].desiredVelocity)
+      {
+        agents[i].velocity.normalize();
+        agents[i].velocity *= agents[i].desiredVelocity;
+      }
+      agents[i].yaw = agents[i].velocity.angle();
+      agents[i].position += agents[i].velocity * dt;
+    }
+    agents[i].movement = agents[i].position - initPos;
+    if (!agents[i].goals.empty() &&
+        (agents[i].goals.front().center - agents[i].position).norm() <= agents[i].goals.front().radius)
+    {
+      Goal g = agents[i].goals.front();
+      agents[i].goals.pop_front();
+      if (agents[i].cyclicGoals)
+      {
+        agents[i].goals.push_back(g);
+      }
+    }
+  }
+  return agents;
+}
+
+inline void SocialForceModel::updatePosition(Agent& agent, double dt) const
+{
+  utils::Vector2d initPos = agent.position;
+  utils::Angle initYaw = agent.yaw;
+
+  agent.velocity += agent.forces.globalForce * dt;
+  if (agent.velocity.norm() > agent.desiredVelocity)
+  {
+    agent.velocity.normalize();
+    agent.velocity *= agent.desiredVelocity;
+  }
+  agent.yaw = agent.velocity.angle();
+  agent.position += agent.velocity * dt;
+
+  agent.linearVelocity = agent.velocity.norm();
+  agent.angularVelocity = (agent.yaw - initYaw).toRadian() / dt;
+
+  agent.movement = agent.position - initPos;
+  if (!agent.goals.empty() && (agent.goals.front().center - agent.position).norm() <= agent.goals.front().radius)
+  {
+    Goal g = agent.goals.front();
+    agent.goals.pop_front();
+    if (agent.cyclicGoals)
+    {
+      agent.goals.push_back(g);
+    }
+  }
+}
+
+}  // namespace sfm
+#endif

simulators/third_party/gazebo_plugins/pedestrian_sfm_plugin/include/lightsfm/vector2d.hpp

@@ -0,0 +1,258 @@
+/***********************************************************************/
+/**                                                                    */
+/** vector2d.hpp                                                       */
+/**                                                                    */
+/** Copyright (c) 2016, Service Robotics Lab.                          */
+/**                     http://robotics.upo.es                         */
+/**                                                                    */
+/** All rights reserved.                                               */
+/**                                                                    */
+/** Authors:                                                           */
+/** Ignacio Perez-Hurtado (maintainer)                                 */
+/** Jesus Capitan                                                      */
+/** Fernando Caballero                                                 */
+/** Luis Merino                                                        */
+/**                                                                    */
+/** This software may be modified and distributed under the terms      */
+/** of the BSD license. See the LICENSE file for details.              */
+/**                                                                    */
+/** http://www.opensource.org/licenses/BSD-3-Clause                    */
+/**                                                                    */
+/***********************************************************************/
+
+#ifndef _VECTOR2D_HPP_
+#define _VECTOR2D_HPP_
+
+#include <iostream>
+#include <cmath>
+// #include <geometry_msgs/Point.h>
+#include <boost/functional/hash.hpp>
+
+#include "angle.hpp"
+
+namespace utils
+{
+class Vector2d
+{
+public:
+  Vector2d() : x(0), y(0)
+  {
+  }
+  Vector2d(double x, double y) : x(x), y(y)
+  {
+  }
+  virtual ~Vector2d()
+  {
+  }
+  double operator()(int index) const
+  {
+    return index == 0 ? x : y;
+  }
+  double operator[](int index) const
+  {
+    return index == 0 ? x : y;
+  }
+  bool operator==(const Vector2d& other) const
+  {
+    return x == other.x && y == other.y;
+  }
+  bool operator<(const Vector2d& other) const
+  {
+    return x < other.x || (x == other.x && y < other.y);
+  }
+  double getX() const
+  {
+    return x;
+  }
+  double getY() const
+  {
+    return y;
+  }
+
+  /*geometry_msgs::Point toPoint() const
+  {
+    geometry_msgs::Point p;
+    p.x = x;
+    p.y = y;
+    p.z = 0;
+    return p;
+  }*/
+
+  Vector2d& set(double x, double y)
+  {
+    Vector2d::x = x;
+    Vector2d::y = y;
+    return *this;
+  }
+  Vector2d& setX(double x)
+  {
+    Vector2d::x = x;
+    return *this;
+  }
+
+  Vector2d& setY(double y)
+  {
+    Vector2d::y = y;
+    return *this;
+  }
+
+  Vector2d& incX(double inc_x)
+  {
+    x += inc_x;
+    return *this;
+  }
+
+  Vector2d& incY(double inc_y)
+  {
+    y += inc_y;
+    return *this;
+  }
+
+  Vector2d& inc(double inc_x, double inc_y)
+  {
+    x += inc_x;
+    y += inc_y;
+    return *this;
+  }
+
+  const Angle angle() const
+  {
+    return Angle::fromRadian(std::atan2(y, x));
+  }
+
+  Angle angleTo(const Vector2d& other) const
+  {
+    return other.angle() - angle();
+  }
+
+  double squaredNorm() const
+  {
+    return x * x + y * y;
+  }
+
+  double norm() const
+  {
+    return std::sqrt(squaredNorm());
+  }
+
+  double dot(const Vector2d& other) const
+  {
+    return x * other.x + y * other.y;
+  }
+
+  Vector2d& normalize()
+  {
+    double n = norm();
+    if (n > 0)
+    {
+      x /= n;
+      y /= n;
+    }
+    return *this;
+  }
+
+  Vector2d normalized() const
+  {
+    Vector2d v(*this);
+    v.normalize();
+    return v;
+  }
+
+  Vector2d& operator*=(double scalar)
+  {
+    x *= scalar;
+    y *= scalar;
+    return *this;
+  }
+  Vector2d operator*(double scalar) const
+  {
+    return Vector2d(x * scalar, y * scalar);
+  }
+
+  Vector2d& operator/=(double scalar)
+  {
+    x /= scalar;
+    y /= scalar;
+    return *this;
+  }
+  Vector2d operator/(double scalar) const
+  {
+    return Vector2d(x / scalar, y / scalar);
+  }
+
+  Vector2d leftNormalVector() const
+  {
+    return Vector2d(-y, x);
+  }
+
+  Vector2d rightNormalVector() const
+  {
+    return Vector2d(y, -x);
+  }
+
+  Vector2d& operator+=(const Vector2d& other)
+  {
+    set(x + other.x, y + other.y);
+    return *this;
+  }
+  Vector2d operator+(const Vector2d& other) const
+  {
+    return Vector2d(x + other.x, y + other.y);
+  }
+  Vector2d& operator-=(const Vector2d& other)
+  {
+    set(x - other.x, y - other.y);
+    return *this;
+  }
+  Vector2d operator-(const Vector2d& other) const
+  {
+    return Vector2d(x - other.x, y - other.y);
+  }
+  Vector2d operator-() const
+  {
+    return Vector2d(-x, -y);
+  }
+
+  static const Vector2d& Zero()
+  {
+    static Vector2d zero;
+    return zero;
+  }
+
+private:
+  double x;
+  double y;
+};
+}  // namespace utils
+
+inline utils::Vector2d operator*(double scalar, const utils::Vector2d& v)
+{
+  utils::Vector2d w(v);
+  w *= scalar;
+  return w;
+}
+
+namespace std
+{
+inline ostream& operator<<(ostream& stream, const utils::Vector2d& v)
+{
+  stream << "(" << v.getX() << "," << v.getY() << ")";
+  return stream;
+}
+
+template <>
+struct hash<utils::Vector2d>
+{
+  size_t operator()(const utils::Vector2d& v) const
+  {
+    using boost::hash_combine;
+    using boost::hash_value;
+    std::size_t seed = 0;
+    hash_combine(seed, hash_value(v[0]));
+    hash_combine(seed, hash_value(v[1]));
+    return seed;
+  }
+};
+}  // namespace std
+
+#endif

simulators/third_party/gazebo_plugins/pedestrian_sfm_plugin/include/pedestrian_sfm_plugin.h

@@ -0,0 +1,131 @@
+/***********************************************************
+ *
+ * @file: pedestrian_sfm_plugin.cpp
+ * @breif: Gazebo plugin for pedestrians using social force model
+ * @author: Yang Haodong
+ * @update: 2023-03-15
+ * @version: 1.1
+ *
+ * Copyright (c) 2023， Yang Haodong
+ * All rights reserved.
+ * --------------------------------------------------------
+ *
+ **********************************************************/
+#ifndef PEDESTRIANSFM_GAZEBO_PLUGIN_H
+#define PEDESTRIANSFM_GAZEBO_PLUGIN_H
+
+// C++
+#include <string>
+#include <vector>
+#include <algorithm>
+#include <ros/ros.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Twist.h>
+#include <tf2/utils.h>
+
+// Gazebo
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/util/system.hh>
+#include <gazebo/transport/transport.hh>
+
+// Social Force Model
+#include <lightsfm/sfm.hpp>
+
+// message
+#include <gazebo_sfm_plugin/ped_state.h>
+
+namespace gazebo
+{
+class GZ_PLUGIN_VISIBLE PedestrianSFMPlugin : public ModelPlugin
+{
+public:
+  /**
+   * @brief Construct a gazebo plugin
+   */
+  PedestrianSFMPlugin();
+
+  /**
+   * @brief De-Construct a gazebo plugin
+   */
+  ~PedestrianSFMPlugin();
+
+  /**
+   * @brief Load the actor plugin.
+   * @param _model  Pointer to the parent model.
+   * @param _sdf    Pointer to the plugin's SDF elements.
+   */
+  virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+
+  /**
+   * @brief Initialize the social force model.
+   */
+  virtual void Reset();
+
+private:
+  /**
+   * @brief Function that is called every update cycle.
+   * @param _info Timing information.
+   */
+  void OnUpdate(const common::UpdateInfo& _info);
+
+  bool OnStateCallBack(gazebo_sfm_plugin::ped_state::Request& req, gazebo_sfm_plugin::ped_state::Response& resp);
+
+  /**
+   * @brief Helper function to detect the closest obstacles.
+   */
+  void handleObstacles();
+
+  /**
+   * @brief Helper function to detect the nearby pedestrians (other actors).
+   */
+  void handlePedestrians();
+
+private:
+  // Gazebo ROS node
+  std::unique_ptr<ros::NodeHandle> node_;
+  // topic publisher
+  ros::Publisher pose_pub_;
+  ros::Publisher vel_pub_;
+  // pedestrian state server
+  ros::ServiceServer state_server_;
+  // this actor as a SFM agent
+  sfm::Agent sfm_actor_;
+  // names of the other models in my walking group.
+  std::vector<std::string> group_names_;
+  // vector of pedestrians detected.
+  std::vector<sfm::Agent> other_actors_;
+  // time delay
+  double time_delay_;
+  bool time_init_;
+  // Maximum distance to detect nearby pedestrians.
+  double people_dist_;
+  // initialized
+  bool pose_init_;
+  // last pose
+  double last_pose_x_, last_pose_y_;
+  // current state
+  double px_, py_, pz_, vx_, vy_, theta_;
+
+
+  // Pointer to the parent actor.
+  physics::ActorPtr actor_;
+  // Pointer to the world, for convenience.
+  physics::WorldPtr world_;
+  // Pointer to the sdf element.
+  sdf::ElementPtr sdf_;
+  // Velocity of the actor
+  ignition::math::Vector3d velocity_;
+  // List of connections
+  std::vector<event::ConnectionPtr> connections_;
+  // Time scaling factor. Used to coordinate translational motion with the actor's walking animation.
+  double animation_factor_ = 1.0;
+  // Time of the last update.
+  common::Time last_update_;
+  // List of models to ignore. Used for vector field
+  std::vector<std::string> ignore_models_;
+  // Custom trajectory info.
+  physics::TrajectoryInfoPtr trajectory_info_;
+};
+}  // namespace gazebo
+#endif

simulators/third_party/gazebo_plugins/pedestrian_sfm_plugin/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>gazebo_sfm_plugin</name>
+  <version>1.0.0</version>
+  <description>The gazebo_sfm_plugin package</description>
+
+  <maintainer email="913982779@qq.com">winter</maintainer>
+  <license>GPL3</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>lightsfm</build_depend>
+  <build_depend>gazebo_ros</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <build_export_depend>gazebo_ros</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+
+  <exec_depend>lightsfm</exec_depend>
+  <exec_depend>gazebo_ros</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+  <export>
+    <gazebo_ros plugin_path="${prefix}/../../lib" gazebo_media_path="${prefix}" />
+  </export>
+</package>

simulators/third_party/gazebo_plugins/pedestrian_sfm_plugin/src/pedestrian_sfm_plugin.cpp

@@ -0,0 +1,479 @@
+/***********************************************************
+ *
+ * @file: pedestrian_sfm_plugin.cpp
+ * @breif: Gazebo plugin for pedestrians using social force model
+ * @author: Yang Haodong
+ * @update: 2023-03-22
+ * @version: 2.0
+ *
+ * Copyright (c) 2023， Yang Haodong
+ * All rights reserved.
+ * --------------------------------------------------------
+ *
+ **********************************************************/
+#include <functional>
+#include <stdio.h>
+#include <string>
+
+#include <pedestrian_sfm_plugin.h>
+
+#define WALKING_ANIMATION "walking"
+
+using namespace gazebo;
+GZ_REGISTER_MODEL_PLUGIN(PedestrianSFMPlugin)
+
+/**
+ * @brief Construct a gazebo plugin
+ */
+PedestrianSFMPlugin::PedestrianSFMPlugin() : pose_init_(false), time_delay_(0.0), time_init_(false)
+{
+}
+
+/**
+ * @brief De-Construct a gazebo plugin
+ */
+PedestrianSFMPlugin::~PedestrianSFMPlugin()
+{
+  pose_pub_.shutdown();
+}
+
+/**
+ * @brief Load the actor plugin.
+ * @param _model  Pointer to the parent model.
+ * @param _sdf    Pointer to the plugin's SDF elements.
+ */
+void PedestrianSFMPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
+{
+  // gazebo model pointer
+  sdf_ = _sdf;
+  actor_ = boost::dynamic_pointer_cast<physics::Actor>(_model);
+  world_ = actor_->GetWorld();
+
+  // Create the ROS node
+  if (!ros::isInitialized())
+  {
+    int argc = 0;
+    char** argv = NULL;
+    ros::init(argc, argv, "gazebo_client", ros::init_options::NoSigintHandler);
+  }
+  node_.reset(new ros::NodeHandle("gazebo_client"));
+
+  // topic publisher
+  pose_pub_ = node_->advertise<geometry_msgs::PoseStamped>("/" + actor_->GetName() + "/pose", 10);
+  vel_pub_ = node_->advertise<geometry_msgs::Twist>("/" + actor_->GetName() + "/twist", 10);
+
+  // server
+  state_server_ = node_->advertiseService(actor_->GetName() + "_state", &PedestrianSFMPlugin::OnStateCallBack, this);
+
+  // Collision attribute configuration of pedestrian links
+  std::map<std::string, ignition::math::Vector3d> scales;
+  std::map<std::string, ignition::math::Pose3d> offsets;
+
+  // Read in the collision property
+  if (sdf_->HasElement("collision"))
+  {
+    auto elem = sdf_->GetElement("collision");
+    while (elem)
+    {
+      auto name = elem->Get<std::string>();
+
+      if (elem->HasAttribute("scale"))
+      {
+        auto scale = elem->Get<ignition::math::Vector3d>("scale");
+        scales[name] = scale;
+      }
+      if (elem->HasAttribute("pose"))
+      {
+        auto pose = elem->Get<ignition::math::Pose3d>("pose");
+        offsets[name] = pose;
+      }
+      elem = elem->GetNextElement("collision");
+    }
+  }
+  // Configure the links
+  for (const auto& link : actor_->GetLinks())
+  {
+    // Init the links, which in turn enables collisions
+    link->Init();
+
+    if (scales.empty())
+      continue;
+
+    // Process all the collisions in all the links
+    for (const auto& collision : link->GetCollisions())
+    {
+      auto name = collision->GetName();
+      // std::cout<<scales[name]<<std::endl;
+      if (scales.find(name) != scales.end())
+      {
+        auto boxShape = boost::dynamic_pointer_cast<gazebo::physics::BoxShape>(collision->GetShape());
+        // Make sure we have a box shape.
+        if (boxShape)
+          boxShape->SetSize(boxShape->Size() * scales[name]);
+      }
+
+      if (offsets.find(name) != offsets.end())
+      {
+        collision->SetInitialRelativePose(offsets[name] + collision->InitialRelativePose());
+      }
+    }
+  }
+
+  if (sdf_->HasElement("time_delay"))
+  {
+    time_delay_ = sdf_->GetElement("time_delay")->Get<double>();
+    // std::unique_ptr<std::thread> t_time_delay(new std::thread(&PedestrianSFMPlugin::timeDelay, this));
+    std::unique_ptr<std::thread> t_time_delay(new std::thread([this]() {
+      if (!time_init_)
+      {
+        sleep(time_delay_);
+        time_init_ = true;
+      }
+    }));
+
+    std::unique_ptr<std::thread> d_time_delay = std::move(t_time_delay);
+    d_time_delay->detach();
+  }
+  else
+    time_init_ = true;
+
+  // Bind the update callback function
+  connections_.push_back(
+      event::Events::ConnectWorldUpdateBegin(std::bind(&PedestrianSFMPlugin::OnUpdate, this, std::placeholders::_1)));
+
+  // Initialize the social force model.
+  this->Reset();
+}
+
+/**
+ * @brief Initialize the social force model.
+ */
+void PedestrianSFMPlugin::Reset()
+{
+  sfm_actor_.id = actor_->GetId();
+
+  // Read in the goals to reach
+  if (sdf_->HasElement("cycle"))
+    sfm_actor_.cyclicGoals = sdf_->GetElement("cycle")->Get<bool>();
+
+  if (sdf_->HasElement("trajectory"))
+  {
+    sdf::ElementPtr model_elem = sdf_->GetElement("trajectory")->GetElement("goalpoint");
+    while (model_elem)
+    {
+      ignition::math::Pose3d g = model_elem->Get<ignition::math::Pose3d>();
+      sfm::Goal goal;
+      goal.center.set(g.Pos().X(), g.Pos().Y());
+      goal.radius = 0.3;
+      sfm_actor_.goals.push_back(goal);
+      model_elem = model_elem->GetNextElement("goalpoint");
+    }
+  }
+
+  auto skel_anims = actor_->SkeletonAnimations();
+  if (skel_anims.find(WALKING_ANIMATION) == skel_anims.end())
+  {
+    gzerr << "Skeleton animation " << WALKING_ANIMATION << " not found.\n";
+  }
+  else
+  {
+    // Create custom trajectory
+    trajectory_info_.reset(new physics::TrajectoryInfo());
+    trajectory_info_->type = WALKING_ANIMATION;
+    trajectory_info_->duration = 1.0;
+    actor_->SetCustomTrajectory(trajectory_info_);
+  }
+
+  // Initialize sfm actor position
+  ignition::math::Vector3d pos = actor_->WorldPose().Pos();
+  ignition::math::Vector3d rpy = actor_->WorldPose().Rot().Euler();
+
+  sfm_actor_.position.set(pos.X(), pos.Y());
+  sfm_actor_.yaw = utils::Angle::fromRadian(rpy.Z());
+  ignition::math::Vector3d linvel = actor_->WorldLinearVel();
+  sfm_actor_.velocity.set(linvel.X(), linvel.Y());
+  sfm_actor_.linearVelocity = linvel.Length();
+  ignition::math::Vector3d angvel = actor_->WorldAngularVel();
+  sfm_actor_.angularVelocity = angvel.Z();
+
+  ignition::math::Pose3d actor_pose = actor_->WorldPose();
+  actor_pose.Rot() = ignition::math::Quaterniond(1.5707, 0, rpy.Z());
+  actor_->SetWorldPose(actor_pose);
+
+  // Read in the maximum velocity of the pedestrian
+  if (sdf_->HasElement("velocity"))
+    sfm_actor_.desiredVelocity = sdf_->Get<double>("velocity");
+  else
+    sfm_actor_.desiredVelocity = 0.8;
+
+  // Read in the algorithm weights
+  if (sdf_->HasElement("goal_weight"))
+    sfm_actor_.params.forceFactorDesired = sdf_->Get<double>("goal_weight");
+  if (sdf_->HasElement("obstacle_weight"))
+    sfm_actor_.params.forceFactorObstacle = sdf_->Get<double>("obstacle_weight");
+  if (sdf_->HasElement("social_weight"))
+    sfm_actor_.params.forceFactorSocial = sdf_->Get<double>("social_weight");
+  if (sdf_->HasElement("group_gaze_weight"))
+    sfm_actor_.params.forceFactorGroupGaze = sdf_->Get<double>("group_gaze_weight");
+  if (sdf_->HasElement("group_coh_weight"))
+    sfm_actor_.params.forceFactorGroupCoherence = sdf_->Get<double>("group_coh_weight");
+  if (sdf_->HasElement("group_rep_weight"))
+    sfm_actor_.params.forceFactorGroupRepulsion = sdf_->Get<double>("group_rep_weight");
+
+  // Read in the animation factor (applied in the OnUpdate function).
+  if (sdf_->HasElement("animation_factor"))
+    animation_factor_ = sdf_->Get<double>("animation_factor");
+  else
+    animation_factor_ = 4.5;
+
+  if (sdf_->HasElement("people_distance"))
+    people_dist_ = sdf_->Get<double>("people_distance");
+  else
+    people_dist_ = 5.0;
+
+  // Read in the pedestrians in your walking group
+  if (sdf_->HasElement("group"))
+  {
+    sfm_actor_.groupId = sfm_actor_.id;
+    sdf::ElementPtr model_elem = sdf_->GetElement("group")->GetElement("model");
+    while (model_elem)
+    {
+      group_names_.push_back(model_elem->Get<std::string>());
+      model_elem = model_elem->GetNextElement("model");
+    }
+    sfm_actor_.groupId = sfm_actor_.id;
+  }
+  else
+    sfm_actor_.groupId = -1;
+
+  // Read in the other obstacles to ignore
+  if (sdf_->HasElement("ignore_obstacles"))
+  {
+    sdf::ElementPtr model_elem = sdf_->GetElement("ignore_obstacles")->GetElement("model");
+    while (model_elem)
+    {
+      ignore_models_.push_back(model_elem->Get<std::string>());
+      model_elem = model_elem->GetNextElement("model");
+    }
+  }
+  // Add our own name to models we should ignore when avoiding obstacles.
+  ignore_models_.push_back(actor_->GetName());
+
+  // Add the other pedestrians to the ignored obstacles
+  for (unsigned int i = 0; i < world_->ModelCount(); ++i)
+  {
+    physics::ModelPtr model = world_->ModelByIndex(i);
+
+    if (model->GetId() != actor_->GetId() && ((int)model->GetType() == (int)actor_->GetType()))
+      ignore_models_.push_back(model->GetName());
+  }
+}
+
+/**
+ * @brief Helper function to detect the closest obstacles.
+ */
+void PedestrianSFMPlugin::handleObstacles()
+{
+  double min_dist = 10000.0;
+  ignition::math::Vector3d closest_obs;
+  sfm_actor_.obstacles1.clear();
+
+  for (unsigned int i = 0; i < world_->ModelCount(); ++i)
+  {
+    physics::ModelPtr model = world_->ModelByIndex(i);
+    if (std::find(ignore_models_.begin(), ignore_models_.end(), model->GetName()) == ignore_models_.end())
+    {
+      // simple method, suppose BBs are AABBs
+      ignition::math::Vector3d actorPos = actor_->WorldPose().Pos();
+      ignition::math::Vector3d modelPos = model->WorldPose().Pos();
+
+      // BB border
+      double max_x = model->BoundingBox().Max().X();
+      double min_x = model->BoundingBox().Min().X();
+      double max_y = model->BoundingBox().Max().Y();
+      double min_y = model->BoundingBox().Min().Y();
+      double max_z = model->BoundingBox().Max().Z();
+      double min_z = model->BoundingBox().Min().Z();
+
+      ignition::math::Vector3d closest_point;
+      double closest_weight = 0.8;
+      closest_point.X() =
+          ignition::math::clamp(closest_weight * actorPos.X() + (1 - closest_weight) * modelPos.X(), min_x, max_x);
+      closest_point.Y() =
+          ignition::math::clamp(closest_weight * actorPos.Y() + (1 - closest_weight) * modelPos.Y(), min_y, max_y);
+      closest_point.Z() =
+          ignition::math::clamp(closest_weight * actorPos.Z() + (1 - closest_weight) * modelPos.Z(), min_z, max_z);
+      ignition::math::Vector3d offset = closest_point - actorPos;
+      double model_dist = offset.Length();
+      if (model_dist < min_dist)
+      {
+        min_dist = model_dist;
+        closest_obs = closest_point;
+      }
+    }
+  }
+
+  // ROS_WARN("model: %s, min_dist: %.2f", closest_model->GetName().c_str(), min_dist);
+  utils::Vector2d ob(closest_obs.X(), closest_obs.Y());
+  sfm_actor_.obstacles1.push_back(ob);
+}
+
+/**
+ * @brief Helper function to detect the nearby pedestrians (other actors).
+ */
+void PedestrianSFMPlugin::handlePedestrians()
+{
+  other_actors_.clear();
+
+  for (unsigned int i = 0; i < world_->ModelCount(); ++i)
+  {
+    physics::ModelPtr model = world_->ModelByIndex(i);
+
+    if (model->GetId() != actor_->GetId() && ((int)model->GetType() == (int)actor_->GetType()))
+    {
+      ignition::math::Pose3d model_pose = model->WorldPose();
+      ignition::math::Vector3d pos = model_pose.Pos() - actor_->WorldPose().Pos();
+      if (pos.Length() < people_dist_)
+      {
+        sfm::Agent ped;
+        ped.id = model->GetId();
+        ped.position.set(model_pose.Pos().X(), model_pose.Pos().Y());
+        ignition::math::Vector3d rpy = model_pose.Rot().Euler();
+        ped.yaw = utils::Angle::fromRadian(rpy.Z());
+
+        ped.radius = sfm_actor_.radius;
+        ignition::math::Vector3d linvel = model->WorldLinearVel();
+        ped.velocity.set(linvel.X(), linvel.Y());
+        ped.linearVelocity = linvel.Length();
+        ignition::math::Vector3d angvel = model->WorldAngularVel();
+        ped.angularVelocity = angvel.Z();
+
+        // check if the ped belongs to my group
+        if (sfm_actor_.groupId != -1)
+        {
+          std::vector<std::string>::iterator it;
+          it = find(group_names_.begin(), group_names_.end(), model->GetName());
+          if (it != group_names_.end())
+            ped.groupId = sfm_actor_.groupId;
+          else
+            ped.groupId = -1;
+        }
+        other_actors_.push_back(ped);
+      }
+    }
+  }
+}
+
+/**
+ * @brief Function that is called every update cycle.
+ * @param _info Timing information.
+ */
+void PedestrianSFMPlugin::OnUpdate(const common::UpdateInfo& _info)
+{
+  if (time_init_)
+  {
+    if (!pose_init_)
+      last_update_ = _info.simTime;
+
+    // Time delta
+    double dt = (_info.simTime - last_update_).Double();
+
+    ignition::math::Pose3d actor_pose = actor_->WorldPose();
+
+    // update closest obstacle
+    this->handleObstacles();
+    // update pedestrian around
+    this->handlePedestrians();
+
+    // Compute Social Forces
+    sfm::SFM.computeForces(sfm_actor_, other_actors_);
+    // Update model
+    sfm::SFM.updatePosition(sfm_actor_, dt);
+
+    utils::Angle h = this->sfm_actor_.yaw;
+    utils::Angle add = utils::Angle::fromRadian(1.5707);
+    h = h + add;
+    double yaw = h.toRadian();
+
+    ignition::math::Vector3d rpy = actor_pose.Rot().Euler();
+    utils::Angle current = utils::Angle::fromRadian(rpy.Z());
+    double diff = (h - current).toRadian();
+    if (std::fabs(diff) > IGN_DTOR(10))
+    {
+      current = current + utils::Angle::fromRadian(diff * 0.005);
+      yaw = current.toRadian();
+    }
+
+    actor_pose.Pos().X(sfm_actor_.position.getX());
+    actor_pose.Pos().Y(sfm_actor_.position.getY());
+    actor_pose.Pos().Z(1.0);
+    actor_pose.Rot() = ignition::math::Quaterniond(1.5707, 0, yaw);
+
+    // Distance traveled is used to coordinate motion with the walking
+    double distance_traveled = (actor_pose.Pos() - actor_->WorldPose().Pos()).Length();
+
+    actor_->SetWorldPose(actor_pose);
+    actor_->SetScriptTime(actor_->ScriptTime() + distance_traveled * animation_factor_);
+
+    geometry_msgs::PoseStamped current_pose;
+    current_pose.header.frame_id = "map";
+    current_pose.header.stamp = ros::Time::now();
+    current_pose.pose.position.x = sfm_actor_.position.getX();
+    current_pose.pose.position.y = sfm_actor_.position.getY();
+    current_pose.pose.position.z = 1.0;
+    tf2::Quaternion q;
+    q.setRPY(0, 0, sfm_actor_.yaw.toRadian());
+    tf2::convert(q, current_pose.pose.orientation);
+
+    // set model velocity
+    geometry_msgs::Twist current_vel;
+    if (!pose_init_)
+    {
+      pose_init_ = true;
+      last_pose_x_ = current_pose.pose.position.x;
+      last_pose_y_ = current_pose.pose.position.y;
+      current_vel.linear.x = 0;
+      current_vel.linear.y = 0;
+    }
+    else
+    {
+      double dt = (_info.simTime - last_update_).Double();
+      double vx = (current_pose.pose.position.x - last_pose_x_) / dt;
+      double vy = (current_pose.pose.position.y - last_pose_y_) / dt;
+      last_pose_x_ = current_pose.pose.position.x;
+      last_pose_y_ = current_pose.pose.position.y;
+
+      current_vel.linear.x = vx;
+      current_vel.linear.y = vy;
+    }
+
+    pose_pub_.publish(current_pose);
+    vel_pub_.publish(current_vel);
+
+    // update
+    px_ = current_pose.pose.position.x;
+    py_ = current_pose.pose.position.y;
+    pz_ = current_pose.pose.position.z;
+    vx_ = current_vel.linear.x;
+    vy_ = current_vel.linear.y;
+    theta_ = yaw;
+    last_update_ = _info.simTime;
+  }
+}
+
+bool PedestrianSFMPlugin::OnStateCallBack(gazebo_sfm_plugin::ped_state::Request& req,
+                                          gazebo_sfm_plugin::ped_state::Response& resp)
+{
+  if (req.name == actor_->GetName())
+  {
+    resp.px = px_;
+    resp.py = py_;
+    resp.pz = pz_;
+    resp.vx = vx_;
+    resp.vy = vy_;
+    resp.theta = theta_;
+    return true;
+  }
+  else
+    return false;
+}

simulators/third_party/gazebo_plugins/pedestrian_sfm_plugin/srv/ped_state.srv

@@ -0,0 +1,10 @@
+# client
+string name
+---
+# server
+float64 px
+float64 py
+float64 pz
+float64 vx
+float64 vy
+float64 theta

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/gazebo_ped_visualizer_plugin/CMakeLists.txt

@@ -0,0 +1,48 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(gazebo_ped_visualizer_plugin)
+
+add_compile_options(-std=c++11)
+
+find_package(catkin REQUIRED COMPONENTS
+  gazebo_ros
+  roscpp
+  pedsim_msgs
+  std_msgs
+)
+
+find_package(Boost REQUIRED COMPONENTS thread)
+find_package(gazebo REQUIRED)
+
+
+include_directories(include)
+include_directories(SYSTEM
+  /usr/local/include  #to find lightsfm
+  ${Boost_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+  ${GAZEBO_INCLUDE_DIRS}
+)
+
+link_directories(
+  ${catkin_LIBRARY_DIRS}
+  ${GAZEBO_LIBRARY_DIRS}
+)
+
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES PedestrianVisualPlugin
+  CATKIN_DEPENDS gazebo_ros roscpp
+)
+
+add_library(PedestrianVisualPlugin src/pedestrian_visual_plugin.cpp)
+add_dependencies(PedestrianVisualPlugin ${catkin_EXPORTED_TARGETS})
+target_link_libraries(PedestrianVisualPlugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) #${Boost_LIBRARIES
+
+install(TARGETS
+  PedestrianVisualPlugin
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+
+

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/gazebo_ped_visualizer_plugin/include/pedestrian_visual_plugin.h

@@ -0,0 +1,93 @@
+/***********************************************************
+ *
+ * @file: pedestrian_sfm_plugin.cpp
+ * @breif: Gazebo plugin for pedestrians using social force model
+ * @author: Yang Haodong
+ * @update: 2023-03-15
+ * @version: 1.1
+ *
+ * Copyright (c) 2023， Yang Haodong
+ * All rights reserved.
+ * --------------------------------------------------------
+ *
+ **********************************************************/
+#ifndef PEDESTRIANVISUAL_GAZEBO_PLUGIN_H
+#define PEDESTRIANVISUAL_GAZEBO_PLUGIN_H
+
+// C++
+#include <string>
+#include <vector>
+#include <algorithm>
+#include <ros/ros.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Twist.h>
+#include <tf2/utils.h>
+
+// Gazebo
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/util/system.hh>
+#include <gazebo/transport/transport.hh>
+#include <gazebo_msgs/GetModelState.h>
+
+// message
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <pedsim_msgs/TrackedPersons.h>
+#include <pedsim_msgs/TrackedPerson.h>
+#include <gazebo_sfm_plugin/ped_state.h>
+
+namespace gazebo
+{
+class GZ_PLUGIN_VISIBLE PedestrianVisualPlugin : public ModelPlugin
+{
+public:
+  /**
+   * @brief Construct a gazebo plugin
+   */
+  PedestrianVisualPlugin();
+
+  /**
+   * @brief De-Construct a gazebo plugin
+   */
+  ~PedestrianVisualPlugin();
+
+  /**
+   * @brief Load the actor plugin.
+   * @param _model  Pointer to the parent model.
+   * @param _sdf    Pointer to the plugin's SDF elements.
+   */
+  virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+
+  /**
+   * @brief Publish pedestrians visualization information
+   */
+  void publishPedVisuals();
+
+private:
+  /**
+   * @brief Function that is called every update cycle.
+   * @param _info Timing information.
+   */
+  void OnUpdate(const common::UpdateInfo& _info);
+
+private:
+  // Gazebo ROS node
+  std::unique_ptr<ros::NodeHandle> node_;
+  // topic publisher
+  ros::Publisher ped_visual_pub_;
+  // Pointer to the parent actor.
+  physics::ActorPtr actor_;
+  // Pointer to the world, for convenience.
+  physics::WorldPtr world_;
+  // Pointer to the sdf element.
+  sdf::ElementPtr sdf_;
+  // List of connections
+  std::vector<event::ConnectionPtr> connections_;
+  // Update interval
+  size_t update_interval_;
+  // Update counter
+  size_t update_cnt_;
+};
+}  // namespace gazebo
+#endif

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/gazebo_ped_visualizer_plugin/package.xml

@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>gazebo_ped_visualizer_plugin</name>
+  <version>0.1.0</version>
+  <description>The pedsim_visualizer package</description>
+
+  <maintainer email="okal@cs.uni-freiburg.de">Billy Okal</maintainer>
+
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>pedsim_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>gazebo_ros</build_depend>
+
+
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>pedsim_msgs</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>visualization_msgs</build_export_depend>
+  <build_export_depend>gazebo_ros</build_export_depend>
+
+
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>pedsim_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
+  <exec_depend>gazebo_ros</exec_depend>
+
+
+</package>

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/gazebo_ped_visualizer_plugin/src/pedestrian_visual_plugin.cpp

@@ -0,0 +1,136 @@
+/***********************************************************
+ *
+ * @file: pedestrian_sfm_plugin.cpp
+ * @breif: Gazebo plugin for pedestrians using social force model
+ * @author: Yang Haodong
+ * @update: 2023-03-15
+ * @version: 1.1
+ *
+ * Copyright (c) 2023， Yang Haodong
+ * All rights reserved.
+ * --------------------------------------------------------
+ *
+ **********************************************************/
+#include <functional>
+#include <stdio.h>
+#include <string>
+
+#include <pedestrian_visual_plugin.h>
+
+using namespace gazebo;
+GZ_REGISTER_MODEL_PLUGIN(PedestrianVisualPlugin)
+
+/**
+ * @brief Construct a gazebo plugin
+ */
+PedestrianVisualPlugin::PedestrianVisualPlugin()
+{
+}
+
+/**
+ * @brief De-Construct a gazebo plugin
+ */
+PedestrianVisualPlugin::~PedestrianVisualPlugin()
+{
+  ped_visual_pub_.shutdown();
+}
+
+/**
+ * @brief Load the actor plugin.
+ * @param _model  Pointer to the parent model.
+ * @param _sdf    Pointer to the plugin's SDF elements.
+ */
+void PedestrianVisualPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
+{
+  // gazebo model pointer
+  sdf_ = _sdf;
+  actor_ = boost::dynamic_pointer_cast<physics::Actor>(_model);
+  world_ = actor_->GetWorld();
+
+  // Create the ROS node
+  if (!ros::isInitialized())
+  {
+    int argc = 0;
+    char** argv = NULL;
+    ros::init(argc, argv, "gazebo_client", ros::init_options::NoSigintHandler);
+  }
+  node_.reset(new ros::NodeHandle("gazebo_client"));
+
+  // topic publisher
+  ped_visual_pub_ = node_->advertise<pedsim_msgs::TrackedPersons>("/ped_visualization", 1);
+
+  // Bind the update callback function
+  connections_.push_back(event::Events::ConnectWorldUpdateBegin(
+      std::bind(&PedestrianVisualPlugin::OnUpdate, this, std::placeholders::_1)));
+
+  // Update rate setting
+  if (sdf_->HasElement("update_rate"))
+  {
+    auto update_rate = sdf_->Get<double>("update_rate");
+    update_interval_ = std::max(size_t(1), size_t(100 / update_rate));
+  }
+  else
+    update_interval_ = 10;
+
+  update_cnt_ = 0;
+}
+
+/**
+ * @brief Publish pedestrians visualization information
+ */
+void PedestrianVisualPlugin::publishPedVisuals()
+{
+  int human_id = 0;
+
+  pedsim_msgs::TrackedPersons tracked_people;
+  std_msgs::Header msg_header;
+  msg_header.stamp = ros::Time::now();
+  msg_header.frame_id = "map";
+  tracked_people.header = msg_header;
+
+  for (unsigned int i = 0; i < world_->ModelCount(); ++i)
+  {
+    physics::ModelPtr model = world_->ModelByIndex(i);
+    if (((int)model->GetType() == (int)actor_->GetType()))
+    {
+      human_id++;
+      pedsim_msgs::TrackedPerson person;
+      person.track_id = human_id;
+      person.is_occluded = false;
+      person.detection_id = human_id;
+
+      ros::ServiceClient state_client = node_->serviceClient<gazebo_sfm_plugin::ped_state>(model->GetName() + "_state");
+      gazebo_sfm_plugin::ped_state model_state;
+      model_state.request.name = model->GetName();
+      state_client.call(model_state);
+
+      geometry_msgs::PoseWithCovariance pose_with_cov;
+      pose_with_cov.pose.position.x = model_state.response.px;
+      pose_with_cov.pose.position.y = model_state.response.py;
+      pose_with_cov.pose.position.z = model_state.response.pz;
+      tf2::Quaternion q;
+      q.setRPY(0, 0, model_state.response.theta - 1.57);
+      tf2::convert(q, pose_with_cov.pose.orientation);
+      person.pose = pose_with_cov;
+
+      geometry_msgs::TwistWithCovariance twist_with_cov;
+      twist_with_cov.twist.linear.x = model_state.response.vx;
+      twist_with_cov.twist.linear.y = model_state.response.vy;
+      person.twist = twist_with_cov;
+
+      tracked_people.tracks.push_back(person);
+    }
+  }
+  ped_visual_pub_.publish(tracked_people);
+}
+
+/**
+ * @brief Function that is called every update cycle.
+ * @param _info Timing information.
+ */
+void PedestrianVisualPlugin::OnUpdate(const common::UpdateInfo& _info)
+{
+  update_cnt_ = update_cnt_ % update_interval_ + 1;
+  if (update_cnt_ == 1)
+    publishPedVisuals();
+}

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/CMakeLists.txt

@@ -0,0 +1,45 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(pedsim_msgs)
+
+# message and service dependencies
+set(MESSAGE_DEPENDENCIES
+  std_msgs
+  geometry_msgs
+  sensor_msgs
+  nav_msgs
+)
+
+find_package(catkin REQUIRED COMPONENTS message_generation ${MESSAGE_DEPENDENCIES})
+
+include_directories(${catkin_INCLUDE_DIRS})
+
+## Generate messages in the 'msg' folder
+add_message_files( DIRECTORY msg
+  FILES
+  AgentState.msg
+  AgentStates.msg
+  AgentGroup.msg
+  AgentGroups.msg
+  AgentForce.msg
+  LineObstacle.msg
+  LineObstacles.msg
+  TrackedPerson.msg
+  TrackedPersons.msg
+  TrackedGroup.msg
+  TrackedGroups.msg
+  SocialRelation.msg
+  SocialRelations.msg
+  SocialActivity.msg
+  SocialActivities.msg
+  Waypoint.msg
+  Waypoints.msg
+)
+
+# generate the messages
+generate_messages(DEPENDENCIES ${MESSAGE_DEPENDENCIES})
+
+
+#Declare package run-time dependencies
+catkin_package(
+    CATKIN_DEPENDS roscpp rospy message_runtime ${MESSAGE_DEPENDENCIES}
+)

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/msg/AgentForce.msg

@@ -0,0 +1,14 @@
+# Forces acting on an agent.
+
+# Basic SFM forces.
+geometry_msgs/Vector3 desired_force
+geometry_msgs/Vector3 obstacle_force
+geometry_msgs/Vector3 social_force
+
+# Additional Group Forces
+geometry_msgs/Vector3 group_coherence_force
+geometry_msgs/Vector3 group_gaze_force
+geometry_msgs/Vector3 group_repulsion_force
+
+# Extra stabilization/custom forces.
+geometry_msgs/Vector3 random_force

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/msg/AgentGroup.msg

@@ -0,0 +1,5 @@
+Header header
+uint16 group_id
+float64 age
+uint64[] members
+geometry_msgs/Pose center_of_mass

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/msg/AgentGroups.msg

@@ -0,0 +1,2 @@
+Header header
+pedsim_msgs/AgentGroup[] groups

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/msg/AgentState.msg

@@ -0,0 +1,21 @@
+Header header
+uint64 id
+uint16 type
+string social_state
+geometry_msgs/Pose pose
+geometry_msgs/Twist twist
+pedsim_msgs/AgentForce forces
+
+# Use sensors package to control observability
+
+# Social State string constants
+string      TYPE_STANDING = "standing"
+string      TYPE_INDIVIDUAL_MOVING = "individual_moving"
+string      TYPE_WAITING_IN_QUEUE = "waiting_in_queue"
+string      TYPE_GROUP_MOVING = "group_moving"
+
+
+# Agent types
+# 0, 1 -> ordinary agents
+# 2 -> Robot
+# 3 -> standing/elderly agents

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/msg/AgentStates.msg

@@ -0,0 +1,2 @@
+Header header
+pedsim_msgs/AgentState[] agent_states

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/msg/AllAgentsState.msg

@@ -0,0 +1,2 @@
+Header header
+pedsim_msgs/AgentState[] agent_states

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/msg/LineObstacle.msg

@@ -0,0 +1,4 @@
+# A line obstacle in the simulator.
+
+geometry_msgs/Point start
+geometry_msgs/Point end

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/msg/LineObstacles.msg

@@ -0,0 +1,4 @@
+# A collection of line obstacles.
+# No need to header since these are detemined at sim initiation time.
+Header header
+pedsim_msgs/LineObstacle[] obstacles

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/msg/SocialActivities.msg

@@ -0,0 +1,5 @@
+std_msgs/Header     header
+
+# All social activities that have been detected in the current time step,
+# within sensor range of the robot.
+SocialActivity[]    elements

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/msg/SocialActivity.msg

@@ -0,0 +1,22 @@
+string      type        # see constants below
+float32     confidence  # detection confidence
+
+uint64[]      track_ids   # IDs of all person tracks involved in the activity, might be one or multiple
+
+
+# Constants for social activity type (just examples at the moment)
+string      TYPE_SHOPPING = shopping
+string      TYPE_STANDING = standing
+string      TYPE_INDIVIDUAL_MOVING = individual_moving
+string      TYPE_WAITING_IN_QUEUE = waiting_in_queue
+string      TYPE_LOOKING_AT_INFORMATION_SCREEN = looking_at_information_screen
+string      TYPE_LOOKING_AT_KIOSK = looking_at_kiosk
+string      TYPE_GROUP_ASSEMBLING = group_assembling
+string      TYPE_GROUP_MOVING = group_moving
+string      TYPE_FLOW_WITH_ROBOT = flow
+string      TYPE_ANTIFLOW_AGAINST_ROBOT = antiflow
+string      TYPE_WAITING_FOR_OTHERS = waiting_for_others
+
+#string      TYPE_COMMUNICATING = communicating
+#string      TYPE_TAKING_PHOTOGRAPH = taking_photograph
+#string      TYPE_TALKING_ON_PHONE = talking_on_phone

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/msg/SocialRelation.msg

@@ -0,0 +1,12 @@
+string      type        # e.g. mother-son relationship, romantic relationship, etc.
+float32     strength    # relationship strength between 0.0 and 1.0
+
+uint32      track1_id
+uint32      track2_id
+
+
+# Constants for type (just examples at the moment)
+string      TYPE_SPATIAL  = "spatial"
+string      TYPE_ROMANTIC = "romantic"
+string      TYPE_PARENT_CHILD = "parent_child"
+string      TYPE_FRIENDSHIP = "friendship"

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/msg/SocialRelations.msg

@@ -0,0 +1,5 @@
+std_msgs/Header     header
+
+# All social relations of all tracks in the current time step.
+# There might be multiple social relations per pair of tracks.
+SocialRelation[]    elements

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/msg/TrackedGroup.msg

@@ -0,0 +1,10 @@
+# Message defining a tracked group
+#
+
+uint64      group_id        # unique identifier of the target, consistent over time
+
+duration    age             # age of the group
+
+geometry_msgs/PoseWithCovariance    centerOfGravity   # mean and covariance of the group (calculated from its person tracks)
+
+uint64[]    track_ids       # IDs of the tracked persons in this group. See srl_tracking_msgs/TrackedPersons

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/msg/TrackedGroups.msg

@@ -0,0 +1,5 @@
+# Message with all currently tracked groups
+#
+
+Header              header      # Header containing timestamp etc. of this message
+pedsim_msgs/TrackedGroup[]      groups      # All groups that are currently being tracked

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/msg/TrackedPerson.msg

@@ -0,0 +1,14 @@
+# Message defining a tracked person
+#
+
+uint64      track_id        # unique identifier of the target, consistent over time
+bool        is_occluded     # if the track is currently not observable in a physical way
+bool        is_matched      # if the track is currently matched by a detection
+uint64      detection_id    # id of the corresponding detection in the current cycle (undefined if occluded)
+duration    age             # age of the track
+
+# The following fields are extracted from the Kalman state x and its covariance C
+
+geometry_msgs/PoseWithCovariance       pose   # pose of the track (z value and orientation might not be set, check if corresponding variance on diagonal is > 99999)
+
+geometry_msgs/TwistWithCovariance   twist     # velocity of the track (z value and rotational velocities might not be set, check if corresponding variance on diagonal is > 99999)

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/msg/TrackedPersons.msg

@@ -0,0 +1,5 @@
+# Message with all currently tracked persons 
+#
+
+Header              header      # Header containing timestamp etc. of this message
+TrackedPerson[]     tracks      # All persons that are currently being tracked

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/msg/Waypoint.msg

@@ -0,0 +1,8 @@
+int8 BHV_SIMPLE = 0
+int8 BHV_SOURCE = 1
+int8 BHV_SINK = 2
+
+string name
+int8 behavior
+geometry_msgs/Point position
+float32 radius

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/msg/Waypoints.msg

@@ -0,0 +1,2 @@
+Header header
+pedsim_msgs/Waypoint[] waypoints

simulators/third_party/gazebo_plugins/pedestrian_visualizer_plugin/pedsim_msgs/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package>
+  <name>pedsim_msgs</name>
+  <version>0.0.2</version>
+  <description>The pedsim_msgs package: contains messages for pedsim</description>
+
+  <maintainer email="okal@cs.uni-freiburg.de">Billy Okal</maintainer>
+
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <run_depend>message_runtime</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+
+</package>

simulators/third_party/map_plugins/voronoi_layer/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(voronoi_layer)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++14)
+
+set(CMAKE_BUILD_TYPE Release)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror")
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  costmap_2d
+  dynamic_reconfigure
+  nav_msgs
+  roscpp
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES voronoi_layer
+  CATKIN_DEPENDS costmap_2d dynamic_reconfigure nav_msgs roscpp
+#  DEPENDS system_lib
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(voronoi_layer src/dynamicvoronoi.cpp src/voronoi_layer.cpp)
+target_link_libraries(voronoi_layer ${catkin_LIBRARIES})

simulators/third_party/map_plugins/voronoi_layer/costmap_plugins.xml

@@ -0,0 +1,7 @@
+<class_libraries>
+  <library path="lib/libvoronoi_layer">
+    <class type="costmap_2d::VoronoiLayer" base_class_type="costmap_2d::Layer">
+      <description>A costmap plugin for dynamic Voronoi.</description>
+    </class>
+  </library>
+</class_libraries>

simulators/third_party/map_plugins/voronoi_layer/include/bucketedqueue.h

@@ -0,0 +1,60 @@
+#ifndef _PRIORITYQUEUE2_H_
+#define _PRIORITYQUEUE2_H_
+
+#include <vector>
+#include <set>
+#include <queue>
+#include <assert.h>
+#include "point.h"
+#include <map>
+
+//! Priority queue for integer coordinates with squared distances as priority.
+/** A priority queue that uses buckets to group elements with the same priority.
+ *  The individual buckets are unsorted, which increases efficiency if these groups are large.
+ *  The elements are assumed to be integer coordinates, and the priorities are assumed
+ *  to be squared Euclidean distances (integers).
+ */
+
+template <typename T>
+class BucketPrioQueue {
+
+public:
+  //! Standard constructor
+  /** Standard constructor. When called for the first time it creates a look up table
+   *  that maps square distances to bucket numbers, which might take some time...
+   */
+  BucketPrioQueue();
+
+
+  void clear() {
+    buckets.clear();
+    count = 0;
+    nextPop = buckets.end();
+  }
+
+  //! Checks whether the Queue is empty
+  bool empty();
+  //! push an element
+  void push(int prio, T t);
+  //! return and pop the element with the lowest squared distance */
+  T pop();
+
+  int size() { return count; }
+  int getNumBuckets() { return buckets.size(); }
+
+  int getTopPriority(){
+    return nextPop->first;
+  }
+
+private:
+
+  int count;
+
+  typedef std::map< int, std::queue<T> > BucketType;
+  BucketType buckets;
+  typename BucketType::iterator nextPop;
+};
+
+#include "bucketedqueue.hxx"
+
+#endif

simulators/third_party/map_plugins/voronoi_layer/include/bucketedqueue.hxx

@@ -0,0 +1,38 @@
+#include "bucketedqueue.h"
+
+#include "limits.h"
+#include <stdio.h>
+#include <stdlib.h>
+
+template <class T>
+BucketPrioQueue<T>::BucketPrioQueue() {
+  clear();
+}
+
+template <class T>
+bool BucketPrioQueue<T>::empty() {
+  return (count==0);
+}
+
+
+template <class T>
+void BucketPrioQueue<T>::push(int prio, T t) {
+  buckets[prio].push(t);
+  if (nextPop == buckets.end() || prio < nextPop->first) nextPop = buckets.find(prio);
+  count++;
+}
+
+template <class T>
+T BucketPrioQueue<T>::pop() {
+  while (nextPop!=buckets.end() && nextPop->second.empty()) ++nextPop;
+
+  T p = nextPop->second.front();
+  nextPop->second.pop();
+  if (nextPop->second.empty()) {
+    typename BucketType::iterator it = nextPop;
+    nextPop++;
+    buckets.erase(it);
+  }
+  count--;
+  return p;
+}

simulators/third_party/map_plugins/voronoi_layer/include/dynamicvoronoi.h

@@ -0,0 +1,120 @@
+#ifndef _DYNAMICVORONOI_H_
+#define _DYNAMICVORONOI_H_
+
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <limits.h>
+#include <queue>
+
+#include "bucketedqueue.h"
+
+//! A DynamicVoronoi object computes and updates a distance map and Voronoi diagram.
+class DynamicVoronoi {
+
+public:
+
+  DynamicVoronoi();
+  ~DynamicVoronoi();
+
+  //! Initialization with an empty map
+  void initializeEmpty(int _sizeX, int _sizeY, bool initGridMap=true);
+  //! Initialization with a given binary map (false==free, true==occupied)
+  void initializeMap(int _sizeX, int _sizeY, bool** _gridMap);
+
+  //! add an obstacle at the specified cell coordinate
+  void occupyCell(int x, int y);
+  //! remove an obstacle at the specified cell coordinate
+  void clearCell(int x, int y);
+  //! remove old dynamic obstacles and add the new ones
+  void exchangeObstacles(std::vector<INTPOINT>& newObstacles);
+
+  //! update distance map and Voronoi diagram to reflect the changes
+  void update(bool updateRealDist=true);
+  //! prune the Voronoi diagram
+  void prune();
+  //! prune the Voronoi diagram by globally revisiting all Voronoi nodes. Takes more time but gives a more sparsely pruned Voronoi graph. You need to call this after every call to udpate()
+  void updateAlternativePrunedDiagram();
+  //! retrieve the alternatively pruned diagram. see updateAlternativePrunedDiagram()
+  int** alternativePrunedDiagram() const {
+    return alternativeDiagram;
+  };
+  //! retrieve the number of neighbors that are Voronoi nodes (4-connected)
+  int getNumVoronoiNeighborsAlternative(int x, int y) const;
+  //! returns whether the specified cell is part of the alternatively pruned diagram. See updateAlternativePrunedDiagram.
+  bool isVoronoiAlternative( int x, int y ) const;
+
+  //! returns the obstacle distance at the specified location
+  float getDistance( int x, int y ) const;
+  //! returns whether the specified cell is part of the (pruned) Voronoi graph
+  bool isVoronoi( int x, int y ) const;
+  //! checks whether the specficied location is occupied
+  bool isOccupied(int x, int y) const;
+  //! write the current distance map and voronoi diagram as ppm file
+  void visualize(const char* filename="result.ppm");
+
+  //! returns the horizontal size of the workspace/map
+  unsigned int getSizeX() const {return sizeX;}
+  //! returns the vertical size of the workspace/map
+  unsigned int getSizeY() const {return sizeY;}
+
+private:
+  struct dataCell {
+    float dist;
+    char voronoi;
+    char queueing;
+    int obstX;
+    int obstY;
+    bool needsRaise;
+    int sqdist;
+  };
+
+  typedef enum {voronoiKeep=-4, freeQueued = -3, voronoiRetry=-2, voronoiPrune=-1, free=0, occupied=1} State;
+  typedef enum {fwNotQueued=1, fwQueued=2, fwProcessed=3, bwQueued=4, bwProcessed=1} QueueingState;
+  typedef enum {invalidObstData = SHRT_MAX/2} ObstDataState;
+  typedef enum {pruned, keep, retry} markerMatchResult;
+
+
+
+  // methods
+  void setObstacle(int x, int y);
+  void removeObstacle(int x, int y);
+  inline void checkVoro(int x, int y, int nx, int ny, dataCell& c, dataCell& nc);
+  void recheckVoro();
+  void commitAndColorize(bool updateRealDist=true);
+  inline void reviveVoroNeighbors(int &x, int &y);
+
+  inline bool isOccupied(int &x, int &y, dataCell &c);
+  inline markerMatchResult markerMatch(int x, int y);
+  inline bool markerMatchAlternative(int x, int y);
+  inline int getVoronoiPruneValence(int x, int y);
+
+  // queues
+
+  BucketPrioQueue<INTPOINT> open;
+  std::queue<INTPOINT> pruneQueue;
+  BucketPrioQueue<INTPOINT> sortedPruneQueue;
+
+  std::vector<INTPOINT> removeList;
+  std::vector<INTPOINT> addList;
+  std::vector<INTPOINT> lastObstacles;
+
+  // maps
+  int sizeY;
+  int sizeX;
+  dataCell** data;
+  bool** gridMap;
+  bool allocatedGridMap;
+
+  // parameters
+  int padding;
+  double doubleThreshold;
+
+  double sqrt2;
+
+  //  dataCell** getData(){ return data; }
+  int** alternativeDiagram;
+};
+
+
+#endif

simulators/third_party/map_plugins/voronoi_layer/include/point.h

@@ -0,0 +1,14 @@
+#ifndef _VOROPOINT_H_
+#define _VOROPOINT_H_
+
+#define INTPOINT IntPoint
+
+/*! A light-weight integer point with fields x,y */
+class IntPoint {
+public:
+  IntPoint() : x(0), y(0) {}
+  IntPoint(int _x, int _y) : x(_x), y(_y) {}
+  int x,y;
+};
+
+#endif

simulators/third_party/map_plugins/voronoi_layer/include/voronoi_layer.h

@@ -0,0 +1,76 @@
+/******************************************************************************
+ * Copyright (c) 2023, NKU Mobile & Flying Robotics Lab
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * 3. Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS'
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *****************************************************************************/
+
+#pragma once
+
+#include <memory>
+
+#include <boost/thread.hpp>
+
+#include "costmap_2d/GenericPluginConfig.h"
+#include "costmap_2d/cost_values.h"
+#include "costmap_2d/layer.h"
+#include "costmap_2d/layered_costmap.h"
+#include "dynamic_reconfigure/server.h"
+#include "dynamicvoronoi.h"
+#include "nav_msgs/OccupancyGrid.h"
+#include "ros/ros.h"
+
+namespace costmap_2d
+{
+class VoronoiLayer : public Layer
+{
+public:
+  VoronoiLayer() = default;
+  virtual ~VoronoiLayer() = default;
+
+  void onInitialize() override;
+  void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x,
+                    double* max_y) override;
+  void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override;
+  const DynamicVoronoi& getVoronoi() const;
+  boost::mutex& getMutex();
+
+private:
+  void publishVoronoiGrid(const costmap_2d::Costmap2D& master_grid);
+  void outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value);
+
+  void reconfigureCB(const costmap_2d::GenericPluginConfig& config, uint32_t level);
+  std::unique_ptr<dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>> dsrv_ = nullptr;
+  ros::Publisher voronoi_grid_pub_;
+
+  DynamicVoronoi voronoi_;
+  unsigned int last_size_x_ = 0;
+  unsigned int last_size_y_ = 0;
+  boost::mutex mutex_;
+};
+
+}  // namespace costmap_2d

simulators/third_party/map_plugins/voronoi_layer/package.xml

@@ -0,0 +1,69 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>voronoi_layer</name>
+  <version>0.0.0</version>
+  <description>The voronoi_layer package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="jwen@todo.todo">jwen</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/voronoi_layer</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>costmap_2d</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_export_depend>costmap_2d</build_export_depend>
+  <build_export_depend>dynamic_reconfigure</build_export_depend>
+  <build_export_depend>nav_msgs</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <exec_depend>costmap_2d</exec_depend>
+  <exec_depend>dynamic_reconfigure</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+
+  <export>
+    <costmap_2d plugin="${prefix}/costmap_plugins.xml" />
+  </export>
+
+</package>

simulators/third_party/map_plugins/voronoi_layer/src/dynamicvoronoi.cpp

@@ -0,0 +1,720 @@
+#include "dynamicvoronoi.h"
+
+#include <math.h>
+#include <iostream>
+
+DynamicVoronoi::DynamicVoronoi() {
+  sqrt2 = sqrt(2.0);
+  data = NULL;
+  gridMap = NULL;
+  alternativeDiagram = NULL;
+  allocatedGridMap = false;
+}
+
+DynamicVoronoi::~DynamicVoronoi() {
+  if (data) {
+    for (int x=0; x<sizeX; x++) delete[] data[x];
+    delete[] data;
+  }
+  if (allocatedGridMap && gridMap) {
+    for (int x=0; x<sizeX; x++) delete[] gridMap[x];
+    delete[] gridMap;
+  }
+}
+
+void DynamicVoronoi::initializeEmpty(int _sizeX, int _sizeY, bool initGridMap) {
+  if (data) {
+    for (int x=0; x<sizeX; x++) delete[] data[x];
+    delete[] data;
+    data = NULL;
+  }
+  if(alternativeDiagram){
+    for (int x=0; x<sizeX; x++) delete[] alternativeDiagram[x];
+    delete[] alternativeDiagram;
+    alternativeDiagram = NULL;
+  }
+  if (initGridMap) {
+    if (allocatedGridMap && gridMap) {
+      for (int x=0; x<sizeX; x++) delete[] gridMap[x];
+      delete[] gridMap;
+      gridMap = NULL;
+      allocatedGridMap = false;
+    }
+  }
+
+
+  sizeX = _sizeX;
+  sizeY = _sizeY;
+  data = new dataCell*[sizeX];
+  for (int x=0; x<sizeX; x++) data[x] = new dataCell[sizeY];
+
+  if (initGridMap) {
+    gridMap = new bool*[sizeX];
+    for (int x=0; x<sizeX; x++) gridMap[x] = new bool[sizeY];
+    allocatedGridMap = true;
+  }
+
+  dataCell c;
+  c.dist = INFINITY;
+  c.sqdist = INT_MAX;
+  c.obstX = invalidObstData;
+  c.obstY = invalidObstData;
+  c.voronoi = free;
+  c.queueing = fwNotQueued;
+  c.needsRaise = false;
+
+  for (int x=0; x<sizeX; x++)
+    for (int y=0; y<sizeY; y++) data[x][y] = c;
+
+  if (initGridMap) {
+    for (int x=0; x<sizeX; x++)
+      for (int y=0; y<sizeY; y++) gridMap[x][y] = 0;
+  }
+}
+
+void DynamicVoronoi::initializeMap(int _sizeX, int _sizeY, bool** _gridMap) {
+  gridMap = _gridMap;
+  initializeEmpty(_sizeX, _sizeY, false);
+
+  for (int x=0; x<sizeX; x++) {
+    for (int y=0; y<sizeY; y++) {
+      if (gridMap[x][y]) {
+        dataCell c = data[x][y];
+        if (!isOccupied(x,y,c)) {
+
+          bool isSurrounded = true;
+          for (int dx=-1; dx<=1; dx++) {
+            int nx = x+dx;
+            if (nx<=0 || nx>=sizeX-1) continue;
+            for (int dy=-1; dy<=1; dy++) {
+              if (dx==0 && dy==0) continue;
+              int ny = y+dy;
+              if (ny<=0 || ny>=sizeY-1) continue;
+
+              if (!gridMap[nx][ny]) {
+                isSurrounded = false;
+                break;
+              }
+            }
+          }
+          if (isSurrounded) {
+            c.obstX = x;
+            c.obstY = y;
+            c.sqdist = 0;
+            c.dist=0;
+            c.voronoi=occupied;
+            c.queueing = fwProcessed;
+            data[x][y] = c;
+          } else setObstacle(x,y);
+        }
+      }
+    }
+  }
+}
+
+void DynamicVoronoi::occupyCell(int x, int y) {
+  gridMap[x][y] = 1;
+  setObstacle(x,y);
+}
+void DynamicVoronoi::clearCell(int x, int y) {
+  gridMap[x][y] = 0;
+  removeObstacle(x,y);
+}
+
+void DynamicVoronoi::setObstacle(int x, int y) {
+  dataCell c = data[x][y];
+  if(isOccupied(x,y,c)) return;
+
+  addList.push_back(INTPOINT(x,y));
+  c.obstX = x;
+  c.obstY = y;
+  data[x][y] = c;
+}
+
+void DynamicVoronoi::removeObstacle(int x, int y) {
+  dataCell c = data[x][y];
+  if(isOccupied(x,y,c) == false) return;
+
+  removeList.push_back(INTPOINT(x,y));
+  c.obstX = invalidObstData;
+  c.obstY  = invalidObstData;
+  c.queueing = bwQueued;
+  data[x][y] = c;
+}
+
+void DynamicVoronoi::exchangeObstacles(std::vector<INTPOINT>& points) {
+
+  for (unsigned int i=0; i<lastObstacles.size(); i++) {
+    int x = lastObstacles[i].x;
+    int y = lastObstacles[i].y;
+
+    bool v = gridMap[x][y];
+    if (v) continue;
+    removeObstacle(x,y);
+  }
+
+  lastObstacles.clear();
+
+  for (unsigned int i=0; i<points.size(); i++) {
+    int x = points[i].x;
+    int y = points[i].y;
+    bool v = gridMap[x][y];
+    if (v) continue;
+    setObstacle(x,y);
+    lastObstacles.push_back(points[i]);
+  }
+}
+
+void DynamicVoronoi::update(bool updateRealDist) {
+
+  commitAndColorize(updateRealDist);
+
+  while (!open.empty()) {
+    INTPOINT p = open.pop();
+    int x = p.x;
+    int y = p.y;
+    dataCell c = data[x][y];
+
+    if(c.queueing==fwProcessed) continue;
+
+    if (c.needsRaise) {
+      // RAISE
+      for (int dx=-1; dx<=1; dx++) {
+        int nx = x+dx;
+        if (nx<=0 || nx>=sizeX-1) continue;
+        for (int dy=-1; dy<=1; dy++) {
+          if (dx==0 && dy==0) continue;
+          int ny = y+dy;
+          if (ny<=0 || ny>=sizeY-1) continue;
+          dataCell nc = data[nx][ny];
+          if (nc.obstX!=invalidObstData && !nc.needsRaise) {
+            if(!isOccupied(nc.obstX,nc.obstY,data[nc.obstX][nc.obstY])) {
+              open.push(nc.sqdist, INTPOINT(nx,ny));
+              nc.queueing = fwQueued;
+              nc.needsRaise = true;
+              nc.obstX = invalidObstData;
+              nc.obstY = invalidObstData;
+              if (updateRealDist) nc.dist = INFINITY;
+              nc.sqdist = INT_MAX;
+              data[nx][ny] = nc;
+            } else {
+              if(nc.queueing != fwQueued){
+                open.push(nc.sqdist, INTPOINT(nx,ny));
+                nc.queueing = fwQueued;
+                data[nx][ny] = nc;
+              }
+            }
+          }
+        }
+      }
+      c.needsRaise = false;
+      c.queueing = bwProcessed;
+      data[x][y] = c;
+    }
+    else if (c.obstX != invalidObstData && isOccupied(c.obstX,c.obstY,data[c.obstX][c.obstY])) {
+
+      // LOWER
+      c.queueing = fwProcessed;
+      c.voronoi = occupied;
+
+      for (int dx=-1; dx<=1; dx++) {
+        int nx = x+dx;
+        if (nx<=0 || nx>=sizeX-1) continue;
+        for (int dy=-1; dy<=1; dy++) {
+          if (dx==0 && dy==0) continue;
+          int ny = y+dy;
+          if (ny<=0 || ny>=sizeY-1) continue;
+          dataCell nc = data[nx][ny];
+          if(!nc.needsRaise) {
+            int distx = nx-c.obstX;
+            int disty = ny-c.obstY;
+            int newSqDistance = distx*distx + disty*disty;
+            bool overwrite =  (newSqDistance < nc.sqdist);
+            if(!overwrite && newSqDistance==nc.sqdist) {
+              if (nc.obstX == invalidObstData || isOccupied(nc.obstX,nc.obstY,data[nc.obstX][nc.obstY])==false) overwrite = true;
+            }
+            if (overwrite) {
+              open.push(newSqDistance, INTPOINT(nx,ny));
+              nc.queueing = fwQueued;
+              if (updateRealDist) {
+                nc.dist = sqrt((double) newSqDistance);
+              }
+              nc.sqdist = newSqDistance;
+              nc.obstX = c.obstX;
+              nc.obstY = c.obstY;
+            } else {
+              checkVoro(x,y,nx,ny,c,nc);
+            }
+            data[nx][ny] = nc;
+          }
+        }
+      }
+    }
+    data[x][y] = c;
+  }
+}
+
+float DynamicVoronoi::getDistance( int x, int y ) const {
+  if( (x>0) && (x<sizeX) && (y>0) && (y<sizeY)) return data[x][y].dist;
+  else return -INFINITY;
+}
+
+bool DynamicVoronoi::isVoronoi( int x, int y ) const {
+  dataCell c = data[x][y];
+  return (c.voronoi==free || c.voronoi==voronoiKeep);
+}
+
+bool DynamicVoronoi::isVoronoiAlternative(int x, int y) const {
+  int v = alternativeDiagram[x][y];
+  return (v == free || v == voronoiKeep);
+}
+
+void DynamicVoronoi::commitAndColorize(bool updateRealDist) {
+  // ADD NEW OBSTACLES
+  for (unsigned int i=0; i<addList.size(); i++) {
+    INTPOINT p = addList[i];
+    int x = p.x;
+    int y = p.y;
+    dataCell c = data[x][y];
+
+    if(c.queueing != fwQueued){
+      if (updateRealDist) c.dist = 0;
+      c.sqdist = 0;
+      c.obstX = x;
+      c.obstY = y;
+      c.queueing = fwQueued;
+      c.voronoi = occupied;
+      data[x][y] = c;
+      open.push(0, INTPOINT(x,y));
+    }
+  }
+
+  // REMOVE OLD OBSTACLES
+  for (unsigned int i=0; i<removeList.size(); i++) {
+    INTPOINT p = removeList[i];
+    int x = p.x;
+    int y = p.y;
+    dataCell c = data[x][y];
+
+    if (isOccupied(x,y,c)==true) continue; // obstacle was removed and reinserted
+    open.push(0, INTPOINT(x,y));
+    if (updateRealDist) c.dist  = INFINITY;
+    c.sqdist = INT_MAX;
+    c.needsRaise = true;
+    data[x][y] = c;
+  }
+  removeList.clear();
+  addList.clear();
+}
+
+
+void DynamicVoronoi::checkVoro(int x, int y, int nx, int ny, dataCell& c, dataCell& nc) {
+
+  if ((c.sqdist>1 || nc.sqdist>1) && nc.obstX!=invalidObstData) {
+    if (abs(c.obstX-nc.obstX) > 1 || abs(c.obstY-nc.obstY) > 1) {
+      //compute dist from x,y to obstacle of nx,ny
+      int dxy_x = x-nc.obstX;
+      int dxy_y = y-nc.obstY;
+      int sqdxy = dxy_x*dxy_x + dxy_y*dxy_y;
+      int stability_xy = sqdxy - c.sqdist;
+      if (sqdxy - c.sqdist<0) return;
+
+      //compute dist from nx,ny to obstacle of x,y
+      int dnxy_x = nx - c.obstX;
+      int dnxy_y = ny - c.obstY;
+      int sqdnxy = dnxy_x*dnxy_x + dnxy_y*dnxy_y;
+      int stability_nxy = sqdnxy - nc.sqdist;
+      if (sqdnxy - nc.sqdist <0) return;
+
+      //which cell is added to the Voronoi diagram?
+      if(stability_xy <= stability_nxy && c.sqdist>2) {
+        if (c.voronoi != free) {
+          c.voronoi = free;
+          reviveVoroNeighbors(x,y);
+          pruneQueue.push(INTPOINT(x,y));
+        }
+      }
+      if(stability_nxy <= stability_xy && nc.sqdist>2) {
+        if (nc.voronoi != free) {
+          nc.voronoi = free;
+          reviveVoroNeighbors(nx,ny);
+          pruneQueue.push(INTPOINT(nx,ny));
+        }
+      }
+    }
+  }
+}
+
+
+void DynamicVoronoi::reviveVoroNeighbors(int &x, int &y) {
+  for (int dx=-1; dx<=1; dx++) {
+    int nx = x+dx;
+    if (nx<=0 || nx>=sizeX-1) continue;
+    for (int dy=-1; dy<=1; dy++) {
+      if (dx==0 && dy==0) continue;
+      int ny = y+dy;
+      if (ny<=0 || ny>=sizeY-1) continue;
+      dataCell nc = data[nx][ny];
+      if (nc.sqdist != INT_MAX && !nc.needsRaise && (nc.voronoi == voronoiKeep || nc.voronoi == voronoiPrune)) {
+        nc.voronoi = free;
+        data[nx][ny] = nc;
+        pruneQueue.push(INTPOINT(nx,ny));
+      }
+    }
+  }
+}
+
+
+bool DynamicVoronoi::isOccupied(int x, int y) const {
+  dataCell c = data[x][y];
+  return (c.obstX==x && c.obstY==y);
+}
+
+bool DynamicVoronoi::isOccupied(int &x, int &y, dataCell &c) {
+  return (c.obstX==x && c.obstY==y);
+}
+
+void DynamicVoronoi::visualize(const char *filename) {
+  // write ppm files
+
+  FILE* F = fopen(filename, "w");
+  if (!F) {
+    std::cerr << "could not open 'result.pgm' for writing!\n";
+    return;
+  }
+  fprintf(F, "P6\n#\n");
+  fprintf(F, "%d %d\n255\n", sizeX, sizeY);
+
+  for(int y = sizeY-1; y >=0; y--){
+    for(int x = 0; x<sizeX; x++){
+      unsigned char c = 0;
+      if (alternativeDiagram!=NULL && (alternativeDiagram[x][y] == free || alternativeDiagram[x][y]==voronoiKeep)) {
+        fputc( 255, F );
+        fputc( 0, F );
+        fputc( 0, F );
+      } else if(isVoronoi(x,y)){
+        fputc( 0, F );
+        fputc( 0, F );
+        fputc( 255, F );
+      } else if (data[x][y].sqdist==0) {
+        fputc( 0, F );
+        fputc( 0, F );
+        fputc( 0, F );
+      } else {
+        float f = 80+(sqrt(data[x][y].sqdist)*10);
+        if (f>255) f=255;
+        if (f<0) f=0;
+        c = (unsigned char)f;
+        fputc( c, F );
+        fputc( c, F );
+        fputc( c, F );
+      }
+    }
+  }
+  fclose(F);
+}
+
+
+void DynamicVoronoi::prune() {
+  // filler
+  while(!pruneQueue.empty()) {
+    INTPOINT p = pruneQueue.front();
+    pruneQueue.pop();
+    int x = p.x;
+    int y = p.y;
+
+    if (data[x][y].voronoi==occupied) continue;
+    if (data[x][y].voronoi==freeQueued) continue;
+
+    data[x][y].voronoi = freeQueued;
+    sortedPruneQueue.push(data[x][y].sqdist, p);
+
+    /* tl t tr
+       l c r
+       bl b br */
+
+    dataCell tr,tl,br,bl;
+    tr = data[x+1][y+1];
+    tl = data[x-1][y+1];
+    br = data[x+1][y-1];
+    bl = data[x-1][y-1];
+
+    dataCell r,b,t,l;
+    r = data[x+1][y];
+    l = data[x-1][y];
+    t = data[x][y+1];
+    b = data[x][y-1];
+
+    if (x+2<sizeX && r.voronoi==occupied) {
+      // fill to the right
+      if (tr.voronoi!=occupied && br.voronoi!=occupied && data[x+2][y].voronoi!=occupied) {
+        r.voronoi = freeQueued;
+        sortedPruneQueue.push(r.sqdist, INTPOINT(x+1,y));
+        data[x+1][y] = r;
+      }
+    }
+    if (x-2>=0 && l.voronoi==occupied) {
+      // fill to the left
+      if (tl.voronoi!=occupied && bl.voronoi!=occupied && data[x-2][y].voronoi!=occupied) {
+        l.voronoi = freeQueued;
+        sortedPruneQueue.push(l.sqdist, INTPOINT(x-1,y));
+        data[x-1][y] = l;
+      }
+    }
+    if (y+2<sizeY && t.voronoi==occupied) {
+      // fill to the top
+      if (tr.voronoi!=occupied && tl.voronoi!=occupied && data[x][y+2].voronoi!=occupied) {
+        t.voronoi = freeQueued;
+        sortedPruneQueue.push(t.sqdist, INTPOINT(x,y+1));
+        data[x][y+1] = t;
+      }
+    }
+    if (y-2>=0 && b.voronoi==occupied) {
+      // fill to the bottom
+      if (br.voronoi!=occupied && bl.voronoi!=occupied && data[x][y-2].voronoi!=occupied) {
+        b.voronoi = freeQueued;
+        sortedPruneQueue.push(b.sqdist, INTPOINT(x,y-1));
+        data[x][y-1] = b;
+      }
+    }
+  }
+
+
+  while(!sortedPruneQueue.empty()) {
+    INTPOINT p = sortedPruneQueue.pop();
+    dataCell c = data[p.x][p.y];
+    int v = c.voronoi;
+    if (v!=freeQueued && v!=voronoiRetry) { // || v>free || v==voronoiPrune || v==voronoiKeep) {
+      //      assert(v!=retry);
+      continue;
+    }
+
+    markerMatchResult r = markerMatch(p.x,p.y);
+    if (r==pruned) c.voronoi = voronoiPrune;
+    else if (r==keep) c.voronoi = voronoiKeep;
+    else { // r==retry
+      c.voronoi = voronoiRetry;
+      //      printf("RETRY %d %d\n", x, sizeY-1-y);
+      pruneQueue.push(p);
+    }
+    data[p.x][p.y] = c;
+
+    if (sortedPruneQueue.empty()) {
+      while (!pruneQueue.empty()) {
+        INTPOINT p = pruneQueue.front();
+        pruneQueue.pop();
+        sortedPruneQueue.push(data[p.x][p.y].sqdist, p);
+      }
+    }
+  }
+  //  printf("match: %d\nnomat: %d\n", matchCount, noMatchCount);
+}
+
+void DynamicVoronoi::updateAlternativePrunedDiagram() {
+
+  if(alternativeDiagram==NULL){
+    alternativeDiagram = new int*[sizeX];
+    for(int x=0; x<sizeX; x++){
+      alternativeDiagram[x] = new int[sizeY];
+    }
+  }
+
+
+  std::queue<INTPOINT> end_cells;
+  BucketPrioQueue<INTPOINT> sortedPruneQueue;
+  for(int x=1; x<sizeX-1; x++){
+    for(int y=1; y<sizeY-1; y++){
+      dataCell& c = data[x][y];
+	alternativeDiagram[x][y] = c.voronoi;
+	if(c.voronoi <=free){
+	  sortedPruneQueue.push(c.sqdist, INTPOINT(x,y));
+	  end_cells.push(INTPOINT(x, y));
+	}
+    }
+  }
+
+  for(int x=1; x<sizeX-1; x++){
+    for(int y=1; y<sizeY-1; y++){
+      if( getNumVoronoiNeighborsAlternative(x, y) >= 3){
+	alternativeDiagram[x][y] = voronoiKeep;
+	sortedPruneQueue.push(data[x][y].sqdist, INTPOINT(x,y));
+	end_cells.push(INTPOINT(x, y));
+      }
+    }
+  }
+
+  for(int x=1; x<sizeX-1; x++){
+    for(int y=1; y<sizeY-1; y++){
+      if( getNumVoronoiNeighborsAlternative(x, y) >= 3){
+	alternativeDiagram[x][y] = voronoiKeep;
+	sortedPruneQueue.push(data[x][y].sqdist, INTPOINT(x,y));
+	end_cells.push(INTPOINT(x, y));
+      }
+    }
+  }
+
+
+  while (!sortedPruneQueue.empty()) {
+    INTPOINT p = sortedPruneQueue.pop();
+
+    if (markerMatchAlternative(p.x, p.y)) {
+      alternativeDiagram[p.x][p.y]=voronoiPrune;
+    } else {
+  alternativeDiagram[p.x][p.y]=voronoiKeep;
+    }
+  }
+
+  // //delete worms
+  while (!end_cells.empty()) {
+    INTPOINT p = end_cells.front();
+    end_cells.pop();
+
+    if (isVoronoiAlternative(p.x,p.y) && getNumVoronoiNeighborsAlternative(p.x, p.y) == 1) {
+      alternativeDiagram[p.x][p.y] = voronoiPrune;
+
+      for (int dx = -1; dx <= 1; ++dx) {
+        for (int dy = -1; dy <= 1; ++dy) {
+          if (!(dx || dy) || (dx && dy)) {
+            continue;
+          }
+          int nx = p.x + dx;
+          int ny = p.y + dy;
+          if (nx < 0 || nx >= sizeX || ny < 0 || ny >= sizeY) {
+            continue;
+          }
+          if (isVoronoiAlternative(nx,ny)) {
+            if (getNumVoronoiNeighborsAlternative(nx, ny) == 1) {
+              end_cells.push(INTPOINT(nx, ny));
+            }
+          }
+        }
+      }
+    }
+  }
+}
+
+bool DynamicVoronoi::markerMatchAlternative(int x, int y) {
+// prune if this returns true
+
+  bool f[8];
+
+  int nx, ny;
+  int dx, dy;
+
+  int i = 0;
+//  int obstacleCount=0;
+  int voroCount = 0;
+  for (dy = 1; dy >= -1; dy--) {
+    ny = y + dy;
+    for (dx = -1; dx <= 1; dx++) {
+      if (dx || dy) {
+        nx = x + dx;
+        int v = alternativeDiagram[nx][ny];
+        bool b = (v <= free && v != voronoiPrune);
+        //	if (v==occupied) obstacleCount++;
+        f[i] = b;
+        if (v <= free && !(dx && dy))
+          voroCount++;
+        i++;
+      }
+    }
+  }
+
+  /*
+   * 5 6 7
+   * 3   4
+   * 0 1 2
+   */
+
+  {
+    //connected horizontal or vertically to only one cell
+    if (voroCount == 1 && (f[1] || f[3] || f[4] || f[6])) {
+      return false;
+    }
+
+    // 4-connected
+    if ((!f[0] && f[1] && f[3]) || (!f[2] && f[1] && f[4]) || (!f[5] && f[3] && f[6]) || (!f[7] && f[6] && f[4]))
+      return false;
+
+    if ((f[3] && f[4] && !f[1] && !f[6]) || (f[1] && f[6] && !f[3] && !f[4]))
+      return false;
+
+  }
+  return true;
+}
+
+int DynamicVoronoi::getNumVoronoiNeighborsAlternative(int x, int y) const {
+  int count = 0;
+  for (int dx = -1; dx <= 1; dx++) {
+    for (int dy = -1; dy <= 1; dy++) {
+      if ((dx == 0 && dy == 0) || (dx != 0 && dy != 0)) {
+        continue;
+      }
+
+      int nx = x + dx;
+      int ny = y + dy;
+      if (nx < 0 || nx >= sizeX || ny < 0 || ny >= sizeY) {
+        continue;
+      }
+      if (alternativeDiagram[nx][ny]==free || alternativeDiagram[nx][ny]==voronoiKeep) {
+        count++;
+      }
+    }
+  }
+  return count;
+}
+
+
+
+DynamicVoronoi::markerMatchResult DynamicVoronoi::markerMatch(int x, int y) {
+  // implementation of connectivity patterns
+  bool f[8];
+
+  int nx, ny;
+  int dx, dy;
+
+  int i=0;
+  int count=0;
+  //  int obstacleCount=0;
+  int voroCount=0;
+  int voroCountFour=0;
+
+  for (dy=1; dy>=-1; dy--) {
+    ny = y+dy;
+    for (dx=-1; dx<=1; dx++) {
+      if (dx || dy) {
+        nx = x+dx;
+        dataCell nc = data[nx][ny];
+        int v = nc.voronoi;
+        bool b = (v<=free && v!=voronoiPrune);
+        //	if (v==occupied) obstacleCount++;
+        f[i] = b;
+        if (b) {
+          voroCount++;
+          if (!(dx && dy)) voroCountFour++;
+        }
+        if (b && !(dx && dy) ) count++;
+        //	if (v<=free && !(dx && dy)) voroCount++;
+        i++;
+      }
+    }
+  }
+  if (voroCount<3 && voroCountFour==1 && (f[1] || f[3] || f[4] || f[6])) {
+    //    assert(voroCount<2);
+    //    if (voroCount>=2) printf("voro>2 %d %d\n", x, y);
+    return keep;
+  }
+
+  // 4-connected
+  if ((!f[0] && f[1] && f[3]) || (!f[2] && f[1] && f[4]) || (!f[5] && f[3] && f[6]) || (!f[7] && f[6] && f[4])) return keep;
+  if ((f[3] && f[4] && !f[1] && !f[6]) || (f[1] && f[6] && !f[3] && !f[4])) return keep;
+
+
+
+  // keep voro cells inside of blocks and retry later
+  if (voroCount>=5 && voroCountFour>=3 && data[x][y].voronoi!=voronoiRetry) {
+    return retry;
+  }
+
+  return pruned;
+}

simulators/third_party/map_plugins/voronoi_layer/src/voronoi_layer.cpp

@@ -0,0 +1,204 @@
+/******************************************************************************
+ * Copyright (c) 2023, NKU Mobile & Flying Robotics Lab
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * 3. Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS'
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *****************************************************************************/
+
+#include "voronoi_layer.h"
+
+#include <chrono>  // NOLINT
+
+#include "pluginlib/class_list_macros.h"
+
+PLUGINLIB_EXPORT_CLASS(costmap_2d::VoronoiLayer, costmap_2d::Layer)
+
+namespace costmap_2d
+{
+void VoronoiLayer::onInitialize()
+{
+  ros::NodeHandle nh("~/" + name_);
+  current_ = true;
+
+  voronoi_grid_pub_ = nh.advertise<nav_msgs::OccupancyGrid>("voronoi_grid", 1);
+
+  dsrv_ = std::make_unique<dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>>(nh);
+  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb =
+      boost::bind(&VoronoiLayer::reconfigureCB, this, _1, _2);
+  dsrv_->setCallback(cb);
+}
+
+void VoronoiLayer::reconfigureCB(const costmap_2d::GenericPluginConfig& config, uint32_t level)
+{
+  enabled_ = config.enabled;
+}
+
+const DynamicVoronoi& VoronoiLayer::getVoronoi() const
+{
+  return voronoi_;
+}
+
+boost::mutex& VoronoiLayer::getMutex()
+{
+  return mutex_;
+}
+
+void VoronoiLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
+                                double* max_x, double* max_y)
+{
+  if (!enabled_)
+  {
+    return;
+  }
+}
+
+void VoronoiLayer::outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value)
+{
+  unsigned char* pc = costarr;
+  for (int i = 0; i < nx; i++)
+  {
+    *pc++ = value;
+  }
+  pc = costarr + (ny - 1) * nx;
+  for (int i = 0; i < nx; i++)
+  {
+    *pc++ = value;
+  }
+  pc = costarr;
+  for (int i = 0; i < ny; i++, pc += nx)
+  {
+    *pc = value;
+  }
+  pc = costarr + nx - 1;
+  for (int i = 0; i < ny; i++, pc += nx)
+  {
+    *pc = value;
+  }
+}
+
+void VoronoiLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+{
+  if (!enabled_)
+  {
+    return;
+  }
+
+  boost::unique_lock<boost::mutex> lock(mutex_);
+
+  unsigned int size_x = master_grid.getSizeInCellsX();
+  unsigned int size_y = master_grid.getSizeInCellsY();
+  outlineMap(master_grid.getCharMap(), size_x, size_y, costmap_2d::LETHAL_OBSTACLE);
+
+  if (last_size_x_ != size_x || last_size_y_ != size_y)
+  {
+    voronoi_.initializeEmpty(size_x, size_y);
+
+    last_size_x_ = size_x;
+    last_size_y_ = size_y;
+  }
+
+  std::vector<IntPoint> new_free_cells, new_occupied_cells;
+  for (unsigned int j = 0; j < size_y; ++j)
+  {
+    for (unsigned int i = 0; i < size_x; ++i)
+    {
+      if (voronoi_.isOccupied(i, j) && master_grid.getCost(i, j) == costmap_2d::FREE_SPACE)
+      {
+        new_free_cells.push_back(IntPoint(i, j));
+      }
+
+      if (!voronoi_.isOccupied(i, j) && master_grid.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE)
+      {
+        new_occupied_cells.push_back(IntPoint(i, j));
+      }
+    }
+  }
+
+  for (size_t i = 0; i < new_free_cells.size(); ++i)
+  {
+    voronoi_.clearCell(new_free_cells[i].x, new_free_cells[i].y);
+  }
+
+  for (size_t i = 0; i < new_occupied_cells.size(); ++i)
+  {
+    voronoi_.occupyCell(new_occupied_cells[i].x, new_occupied_cells[i].y);
+  }
+
+  // start timing
+  const auto start_timestamp = std::chrono::system_clock::now();
+
+  voronoi_.update();
+  voronoi_.prune();
+
+  // end timing
+  const auto end_timestamp = std::chrono::system_clock::now();
+  const std::chrono::duration<double> diff = end_timestamp - start_timestamp;
+  ROS_DEBUG("Runtime=%.3fms.", diff.count() * 1e3);
+
+  publishVoronoiGrid(master_grid);
+}
+
+void VoronoiLayer::publishVoronoiGrid(const costmap_2d::Costmap2D& master_grid)
+{
+  unsigned int nx = master_grid.getSizeInCellsX();
+  unsigned int ny = master_grid.getSizeInCellsY();
+
+  double resolution = master_grid.getResolution();
+  nav_msgs::OccupancyGrid grid;
+  // Publish Whole Grid
+  grid.header.frame_id = "map";
+  grid.header.stamp = ros::Time::now();
+  grid.info.resolution = resolution;
+
+  grid.info.width = nx;
+  grid.info.height = ny;
+
+  grid.info.origin.position.x = master_grid.getOriginX();
+  grid.info.origin.position.y = master_grid.getOriginY();
+  grid.info.origin.position.z = 0.0;
+  grid.info.origin.orientation.w = 1.0;
+
+  grid.data.resize(nx * ny);
+
+  for (unsigned int x = 0; x < nx; x++)
+  {
+    for (unsigned int y = 0; y < ny; y++)
+    {
+      if (voronoi_.isVoronoi(x, y))
+      {
+        grid.data[x + y * nx] = 128;
+      }
+      else
+      {
+        grid.data[x + y * nx] = 0;
+      }
+    }
+  }
+  voronoi_grid_pub_.publish(grid);
+}
+
+}  // namespace costmap_2d

simulators/third_party/rviz_plugins/spencer_messages/.gitignore

@@ -0,0 +1,29 @@
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app

simulators/third_party/rviz_plugins/spencer_messages/LICENSE

@@ -0,0 +1,23 @@
+Copyright (c) 2016, spencer.eu
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

simulators/third_party/rviz_plugins/spencer_messages/README.md

@@ -0,0 +1 @@
+# spencer_messages

simulators/third_party/rviz_plugins/spencer_messages/spencer_control_msgs/CHANGELOG.rst

@@ -0,0 +1,35 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package spencer_control_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.8 (2017-09-22)
+------------------
+* Merge pull request `#41 <https://github.com/LCAS/spencer_people_tracking/issues/41>`_ from LCAS/master
+  1.0.7
+* Contributors: Timm Linder
+
+1.0.7 (2017-06-09)
+------------------
+
+1.0.6 (2017-06-09)
+------------------
+
+1.0.5 (2017-05-12)
+------------------
+
+1.0.4 (2017-05-12)
+------------------
+
+1.0.3 (2017-05-11)
+------------------
+
+1.0.2 (2017-05-09)
+------------------
+* added missing roslib deps
+* Contributors: Marc Hanheide
+
+1.0.1 (2017-05-09)
+------------------
+* homogenised all version strings to 1.0.0
+* Adding missing spencer_control_msgs package
+* Contributors: Marc Hanheide, Timm Linder

simulators/third_party/rviz_plugins/spencer_messages/spencer_control_msgs/CMakeLists.txt

@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(spencer_control_msgs)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  roslib
+  std_msgs
+  message_generation
+)
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+# Generate messages in the 'msg' folder
+add_message_files(
+  FILES
+  ComponentStatus.msg
+)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+   DEPENDENCIES
+   std_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  CATKIN_DEPENDS roscpp std_msgs message_runtime
+)

simulators/third_party/rviz_plugins/spencer_messages/spencer_control_msgs/LICENSE

@@ -0,0 +1,29 @@
+Software License Agreement (BSD License)
+
+Copyright (c) 2013-2015 Timm Linder, Social Robotics Laboratory, University of Freiburg
+Copyright (c) 2014-2015 Stefan Breuers, Lucas Beyer, Computer Vision Group, RWTH Aachen University
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its contributors
+  may be used to endorse or promote products derived from this software
+  without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE

simulators/third_party/rviz_plugins/spencer_messages/spencer_control_msgs/msg/ComponentStatus.msg

@@ -0,0 +1,3 @@
+string name
+bool alive
+string detail

simulators/third_party/rviz_plugins/spencer_messages/spencer_control_msgs/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<package>
+
+  <name>spencer_control_msgs</name>
+  <version>1.0.8</version>
+  <description>Messages used for controlling the SPENCER robot platform.</description>
+
+  <maintainer email="linder@cs.uni-freiburg.de">Timm Linder</maintainer>
+  <author email="linder@cs.uni-freiburg.de">Timm Linder</author>
+  
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  
+  <run_depend>message_runtime</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+<build_depend>roslib</build_depend><run_depend>roslib</run_depend></package>

simulators/third_party/rviz_plugins/spencer_messages/spencer_human_attribute_msgs/CHANGELOG.rst

@@ -0,0 +1,39 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package spencer_human_attribute_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.8 (2017-09-22)
+------------------
+* Merge pull request `#2 <https://github.com/LCAS/spencer_people_tracking/issues/2>`_ from spencer-project/master
+  Integrate multiple fixes from upstream
+* Specify correct license in package.xmls
+* Merge pull request `#41 <https://github.com/LCAS/spencer_people_tracking/issues/41>`_ from LCAS/master
+  1.0.7
+* Contributors: Marc Hanheide, Timm Linder
+
+1.0.7 (2017-06-09)
+------------------
+
+1.0.6 (2017-06-09)
+------------------
+
+1.0.5 (2017-05-12)
+------------------
+
+1.0.4 (2017-05-12)
+------------------
+
+1.0.3 (2017-05-11)
+------------------
+
+1.0.2 (2017-05-09)
+------------------
+* added missing roslib deps
+* Contributors: Marc Hanheide
+
+1.0.1 (2017-05-09)
+------------------
+* homogenised all version strings to 1.0.0
+* Updated licenses
+* Adding message definitions, RViz plugins and mock scripts
+* Contributors: Marc Hanheide, Timm Linder

simulators/third_party/rviz_plugins/spencer_messages/spencer_human_attribute_msgs/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(spencer_human_attribute_msgs)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  roslib
+  std_msgs
+  message_generation
+)
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+# Generate messages in the 'msg' folder
+add_message_files(
+   FILES
+   CategoricalAttribute.msg
+   ScalarAttribute.msg
+   HumanAttributes.msg
+)
+
+# Generate services in the 'srv' folder
+#add_service_files(
+#  FILES
+#)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+   DEPENDENCIES
+   std_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  CATKIN_DEPENDS roscpp std_msgs message_runtime 
+)

simulators/third_party/rviz_plugins/spencer_messages/spencer_human_attribute_msgs/LICENSE

@@ -0,0 +1,28 @@
+Software License Agreement (BSD License)
+
+Copyright (c) 2014-2015 Timm Linder, Social Robotics Laboratory, University of Freiburg
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its contributors
+  may be used to endorse or promote products derived from this software
+  without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE

simulators/third_party/rviz_plugins/spencer_messages/spencer_human_attribute_msgs/msg/CategoricalAttribute.msg

@@ -0,0 +1,32 @@
+uint64                  subject_id              # either an observation ID or a track ID (if information has been integrated over time); should be encoded in topic name
+string                  type                    # type of the attribute, see constants below
+
+string[]                values                  # values, each value also should have a confidence, highest-confidence attribute should come first
+float32[]               confidences             # corresponding confidences should sum up to 1.0, highest confidence comes first
+
+
+##################################################
+### Constants for categorical attribute types. ###
+##################################################
+
+string      GENDER        = gender
+string      AGE_GROUP     = age_group
+
+###################################################
+### Constants for categorical attribute values. ###
+###################################################
+
+string      GENDER_MALE             = male
+string      GENDER_FEMALE           = female
+
+# Age groups are based upon the categories from the "Images Of Groups" RGB database
+string      AGE_GROUP_0_TO_2        = 0-2
+string      AGE_GROUP_3_TO_7        = 3-7
+string      AGE_GROUP_8_TO_12       = 8-12
+string      AGE_GROUP_13_TO_19      = 13-19
+string      AGE_GROUP_20_TO_36      = 20-36
+string      AGE_GROUP_37_TO_65      = 37-65
+string      AGE_GROUP_66_TO_99      = 66-99
+
+
+

simulators/third_party/rviz_plugins/spencer_messages/spencer_human_attribute_msgs/msg/HumanAttributes.msg

@@ -0,0 +1,5 @@
+std_msgs/Header header
+
+# One entry per attribute type per person
+CategoricalAttribute[]   categoricalAttributes
+ScalarAttribute[]       scalarAttributes

simulators/third_party/rviz_plugins/spencer_messages/spencer_human_attribute_msgs/msg/ScalarAttribute.msg

@@ -0,0 +1,12 @@
+uint64                  subject_id              # either an observation ID or a track ID (if information has been integrated over time); should be encoded in topic name
+string                  type                    # type of the attribute, see constants below
+
+float32[]               values                  # values, each value also should have a confidence; highest-confidence value comes first!
+float32[]               confidences             # corresponding confidences should sum up to 1.0
+
+
+###########################################
+### Constants for scalar attribute types. #
+###########################################
+
+string      PERSON_HEIGHT        = person_height

simulators/third_party/rviz_plugins/spencer_messages/spencer_human_attribute_msgs/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<package>
+
+  <name>spencer_human_attribute_msgs</name>
+  <version>1.0.8</version>
+  <description>Messages used for Human Attribute Recognition</description>
+
+  <maintainer email="linder@informatik.uni-freiburg.de">Timm Linder</maintainer>
+  <author email="linder@informatik.uni-freiburg.de">Timm Linder</author>
+  
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  
+  <run_depend>message_runtime</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+<build_depend>roslib</build_depend><run_depend>roslib</run_depend></package>

simulators/third_party/rviz_plugins/spencer_messages/spencer_social_relation_msgs/CHANGELOG.rst

@@ -0,0 +1,40 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package spencer_social_relation_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.8 (2017-09-22)
+------------------
+* Merge pull request `#2 <https://github.com/LCAS/spencer_people_tracking/issues/2>`_ from spencer-project/master
+  Integrate multiple fixes from upstream
+* Specify correct license in package.xmls
+* Merge pull request `#41 <https://github.com/LCAS/spencer_people_tracking/issues/41>`_ from LCAS/master
+  1.0.7
+* Contributors: Marc Hanheide, Timm Linder
+
+1.0.7 (2017-06-09)
+------------------
+
+1.0.6 (2017-06-09)
+------------------
+
+1.0.5 (2017-05-12)
+------------------
+
+1.0.4 (2017-05-12)
+------------------
+
+1.0.3 (2017-05-11)
+------------------
+
+1.0.2 (2017-05-09)
+------------------
+* added missing roslib deps
+* Contributors: Marc Hanheide
+
+1.0.1 (2017-05-09)
+------------------
+* homogenised all version strings to 1.0.0
+* Updating lots of utility packages to latest version from SPENCER repo. Licenses updated.
+* Updated licenses
+* Adding message definitions, RViz plugins and mock scripts
+* Contributors: Marc Hanheide, Timm Linder

simulators/third_party/rviz_plugins/spencer_messages/spencer_social_relation_msgs/CMakeLists.txt

@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(spencer_social_relation_msgs)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  roslib
+  std_msgs
+  sensor_msgs
+  nav_msgs
+  geometry_msgs
+  message_generation
+)
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+# Generate messages in the 'msg' folder
+add_message_files(
+   FILES
+   SocialRelation.msg
+   SocialRelations.msg
+   SocialActivity.msg
+   SocialActivities.msg
+)
+
+# Generate services in the 'srv' folder
+#add_service_files(
+#  FILES
+#)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+   DEPENDENCIES
+   geometry_msgs
+   sensor_msgs
+   nav_msgs
+   std_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  CATKIN_DEPENDS roscpp std_msgs sensor_msgs nav_msgs geometry_msgs message_runtime 
+)

simulators/third_party/rviz_plugins/spencer_messages/spencer_social_relation_msgs/LICENSE

@@ -0,0 +1,28 @@
+Software License Agreement (BSD License)
+
+Copyright (c) 2014-2015 Timm Linder, Billy Okal, Social Robotics Laboratory, University of Freiburg
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its contributors
+  may be used to endorse or promote products derived from this software
+  without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE

simulators/third_party/rviz_plugins/spencer_messages/spencer_social_relation_msgs/msg/SocialActivities.msg

@@ -0,0 +1,5 @@
+std_msgs/Header     header
+
+# All social activities that have been detected in the current time step,
+# within sensor range of the robot.
+SocialActivity[]    elements

simulators/third_party/rviz_plugins/spencer_messages/spencer_social_relation_msgs/msg/SocialActivity.msg

@@ -0,0 +1,24 @@
+string      type        # see constants below
+float32     confidence  # detection confidence
+
+uint64[]      track_ids   # IDs of all person tracks involved in the activity, might be one or multiple
+
+
+# Constants for social activity type (just examples at the moment)
+string      TYPE_SHOPPING = shopping
+string      TYPE_STANDING = standing
+string      TYPE_INDIVIDUAL_MOVING = individual_moving
+string      TYPE_WAITING_IN_QUEUE = waiting_in_queue
+string      TYPE_LOOKING_AT_INFORMATION_SCREEN = looking_at_information_screen
+string      TYPE_LOOKING_AT_KIOSK = looking_at_kiosk
+string      TYPE_GROUP_ASSEMBLING = group_assembling
+string      TYPE_GROUP_MOVING = group_moving
+string      TYPE_FLOW_WITH_ROBOT = flow
+string      TYPE_ANTIFLOW_AGAINST_ROBOT = antiflow
+string      TYPE_WAITING_FOR_OTHERS = waiting_for_others
+string      TYPE_LOOKING_FOR_HELP = looking_for_help
+
+
+#string      TYPE_COMMUNICATING = communicating
+#string      TYPE_TAKING_PHOTOGRAPH = taking_photograph
+#string      TYPE_TALKING_ON_PHONE = talking_on_phone

simulators/third_party/rviz_plugins/spencer_messages/spencer_social_relation_msgs/msg/SocialRelation.msg

@@ -0,0 +1,12 @@
+string      type        # e.g. mother-son relationship, romantic relationship, etc.
+float32     strength    # relationship strength between 0.0 and 1.0
+
+uint32      track1_id
+uint32      track2_id
+
+
+# Constants for type (just examples at the moment)
+string      TYPE_SPATIAL  = "spatial"
+string      TYPE_ROMANTIC = "romantic"
+string      TYPE_PARENT_CHILD = "parent_child"
+string      TYPE_FRIENDSHIP = "friendship"

simulators/third_party/rviz_plugins/spencer_messages/spencer_social_relation_msgs/msg/SocialRelations.msg

@@ -0,0 +1,5 @@
+std_msgs/Header     header
+
+# All social relations of all tracks in the current time step.
+# There might be multiple social relations per pair of tracks.
+SocialRelation[]    elements

simulators/third_party/rviz_plugins/spencer_messages/spencer_social_relation_msgs/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package>
+
+  <name>spencer_social_relation_msgs</name>
+  <version>1.0.8</version>
+  <description>Messages used for Social Activity Detection and Social Relation Analysis</description>
+
+  <maintainer email="linder@informatik.uni-freiburg.de">Timm Linder</maintainer>
+  <license>BSD</license>
+  
+  <author email="linder@informatik.uni-freiburg.de">Timm Linder</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  
+  <run_depend>message_runtime</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+
+<build_depend>roslib</build_depend><run_depend>roslib</run_depend></package>

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/CHANGELOG.rst

@@ -0,0 +1,41 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package spencer_tracking_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.8 (2017-09-22)
+------------------
+* Merge pull request `#2 <https://github.com/LCAS/spencer_people_tracking/issues/2>`_ from spencer-project/master
+  Integrate multiple fixes from upstream
+* Specify correct license in package.xmls
+* Merge pull request `#41 <https://github.com/LCAS/spencer_people_tracking/issues/41>`_ from LCAS/master
+  1.0.7
+* Contributors: Marc Hanheide, Timm Linder
+
+1.0.7 (2017-06-09)
+------------------
+
+1.0.6 (2017-06-09)
+------------------
+
+1.0.5 (2017-05-12)
+------------------
+
+1.0.4 (2017-05-12)
+------------------
+
+1.0.3 (2017-05-11)
+------------------
+
+1.0.2 (2017-05-09)
+------------------
+* added missing roslib deps
+* Contributors: Marc Hanheide
+
+1.0.1 (2017-05-09)
+------------------
+* homogenised all version strings to 1.0.0
+* Updating lots of utility packages to latest version from SPENCER repo. Licenses updated.
+* Updated licenses
+* Adding is_matched field in TrackedPerson message to distinguish between misdetections and physical occlusions
+* Adding message definitions, RViz plugins and mock scripts
+* Contributors: Marc Hanheide, Timm Linder

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/CMakeLists.txt

@@ -0,0 +1,52 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(spencer_tracking_msgs)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  roslib
+  std_msgs
+  geometry_msgs
+  message_generation
+)
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+# Generate messages in the 'msg' folder
+add_message_files(
+   FILES
+   DetectedPerson.msg
+   DetectedPersons.msg
+   CompositeDetectedPerson.msg
+   CompositeDetectedPersons.msg
+   TrackedPerson.msg
+   TrackedPersons.msg
+   TrackedPerson2d.msg
+   TrackedPersons2d.msg
+   TrackedGroup.msg
+   TrackedGroups.msg
+   ImmDebugInfo.msg
+   ImmDebugInfos.msg
+   TrackingTimingMetrics.msg
+)
+
+# Generate services in the 'srv' folder
+add_service_files(
+  FILES
+  GetPersonTrajectories.srv
+)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+   DEPENDENCIES
+   geometry_msgs
+   std_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  CATKIN_DEPENDS roscpp std_msgs geometry_msgs message_runtime 
+)

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/LICENSE

@@ -0,0 +1,29 @@
+Software License Agreement (BSD License)
+
+Copyright (c) 2013-2015 Timm Linder, Social Robotics Laboratory, University of Freiburg
+Copyright (c) 2014-2015 Stefan Breuers, Lucas Beyer, Computer Vision Group, RWTH Aachen University
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its contributors
+  may be used to endorse or promote products derived from this software
+  without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/CompositeDetectedPerson.msg

@@ -0,0 +1,20 @@
+# Specifies which detected persons have been merged into a composite detection by the spencer_detected_person_association module.
+
+# TODO: Do we need a composite person-specific timestamp (or even a full message header including frame ID)?
+# Having a separate timestamp per person could be useful if the timestamps of the merged DetectedPersons messages vary a lot,
+# and some persons are only seen by a single sensor (so averaging over all input timestamps would have a detrimental effect). 
+
+uint64      composite_detection_id          # ID of the fused detection
+
+float64     mean_confidence                 # mean of the confidences of the original detections
+float64     max_confidence                  # maximum confidence of original detections
+float64     min_confidence                  # minimum confidence of original detections
+
+
+geometry_msgs/PoseWithCovariance    pose    # Merged 3D pose (position + orientation) of the detection center
+                                            # check covariance to see which dimensions are actually set!
+                                            # unset dimensions shall have a covariance > 9999
+
+DetectedPerson[] original_detections        # The original detections from individual sensor-specific detectors that have been combined into a composite detection
+                                            # We are copying the entire DetectedPersons messages, *with poses transformed into the target frame*, such that subscribers
+                                            # do not have to subscribe to all the original DetectedPersons topics.

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/CompositeDetectedPersons.msg

@@ -0,0 +1,8 @@
+# Message specifying which original detected persons (from all kinds of sensors) have been merged into one fused detection before being processed by the person tracker, in the current time step.
+#
+# This information is processed by the spencer_detected_person_association module, such that other perception components can associate their results (e.g. person age, gender)
+# with a particular spencer_tracking_msgs/TrackedPerson (which contains a reference to a composite person detection ID).
+
+Header                      header          # Header timestamp is set to the *latest* timestamp of all fused DetectedPerson messages.
+                                            # Before fusion, all detections are transformed into a common coordinate frame (usually base_footprint).
+CompositeDetectedPerson[]   elements        # Fused (merged) detected persons

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/DetectedPerson.msg

@@ -0,0 +1,23 @@
+# Message describing a detection of a person
+#
+
+# Unique id of the detection, monotonically increasing over time
+uint64          detection_id
+
+# (Pseudo-)probabilistic value between 0.0 and 1.0 describing the detector's confidence in the detection
+float64         confidence
+
+# 3D pose (position + orientation) of the *center* of the detection
+# check covariance to see which dimensions are actually set! unset dimensions shall have a covariance > 9999
+geometry_msgs/PoseWithCovariance    pose    
+
+# Sensor modality / detector type, see example constants below. 
+# used e.g. later in tracking to check which tracks have been visually confirmed
+string          modality            
+
+                                    
+string          MODALITY_GENERIC_LASER_2D = laser2d
+string          MODALITY_GENERIC_LASER_3D = laser3d
+string          MODALITY_GENERIC_MONOCULAR_VISION = mono
+string          MODALITY_GENERIC_STEREO_VISION = stereo
+string          MODALITY_GENERIC_RGBD = rgbd

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/DetectedPersons.msg

@@ -0,0 +1,5 @@
+# Message with all currently detected persons
+#
+
+Header                  header          # Header containing timestamp etc. of this message
+DetectedPerson[]        detections      # All persons that are currently being detected

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/ImmDebugInfo.msg

@@ -0,0 +1,10 @@
+# Message for passing debug information of filter performance
+#
+
+uint64      track_id        # unique identifier of the target, consistent over time
+float64      innovation      # innovation of prediction and associated observation
+float64      CpXX            # variance of prediction acc. to x
+float64      CpYY            # variance of prediction acc. to y
+float64      CXX             # variance of state acc. to x
+float64      CYY             # variance of state acc. to y
+float64[]    modeProbabilities# array containing mode probabilities

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/ImmDebugInfos.msg

@@ -0,0 +1,5 @@
+# Message with all debug infos per frame
+#
+
+Header              header      # Header containing timestamp etc. of this message
+ImmDebugInfo[]      infos      # All persons that are currently being tracked

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/PersonTrajectory.msg

@@ -0,0 +1,10 @@
+# Message defining the trajectory of a tracked person.
+#
+# The distinction between track and trajectory is that, depending on the
+# implementation of the tracker, a single track (i.e. tracked person) might
+# change the trajectory if at some point a new trajectory "fits" that track (person)
+# better.
+#
+
+uint64                   track_id   # Unique identifier of the tracked person.
+PersonTrajectoryEntry[]  trajectory # All states of the last T frames of the most likely trajectory of that tracked person.

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/PersonTrajectoryEntry.msg

@@ -0,0 +1,11 @@
+# Message defining an entry of a person trajectory.
+#
+
+time     stamp           # age of the track
+bool     is_occluded     # if the track is currently not matched by a detection
+uint64   detection_id    # id of the corresponding detection in the current cycle (undefined if occluded)
+
+# The following fields are extracted from the Kalman state x and its covariance C
+
+geometry_msgs/PoseWithCovariance    pose   # pose of the track (z value and orientation might not be set, check if corresponding variance on diagonal is > 99999)
+geometry_msgs/TwistWithCovariance   twist  # velocity of the track (z value and rotational velocities might not be set, check if corresponding variance on diagonal is > 99999)

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/TrackedGroup.msg

@@ -0,0 +1,10 @@
+# Message defining a tracked group
+#
+
+uint64      group_id        # unique identifier of the target, consistent over time
+
+duration    age             # age of the group
+
+geometry_msgs/PoseWithCovariance    centerOfGravity   # mean and covariance of the group (calculated from its person tracks)
+
+uint64[]    track_ids       # IDs of the tracked persons in this group. See srl_tracking_msgs/TrackedPersons

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/TrackedGroups.msg

@@ -0,0 +1,5 @@
+# Message with all currently tracked groups 
+#
+
+Header              header      # Header containing timestamp etc. of this message
+TrackedGroup[]      groups      # All groups that are currently being tracked

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/TrackedPerson.msg

@@ -0,0 +1,14 @@
+# Message defining a tracked person
+#
+
+uint64      track_id        # unique identifier of the target, consistent over time
+bool        is_occluded     # if the track is currently not observable in a physical way
+bool        is_matched      # if the track is currently matched by a detection
+uint64      detection_id    # id of the corresponding detection in the current cycle (undefined if occluded)
+duration    age             # age of the track
+
+# The following fields are extracted from the Kalman state x and its covariance C
+
+geometry_msgs/PoseWithCovariance       pose   # pose of the track (z value and orientation might not be set, check if corresponding variance on diagonal is > 99999)
+
+geometry_msgs/TwistWithCovariance   twist     # velocity of the track (z value and rotational velocities might not be set, check if corresponding variance on diagonal is > 99999)

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/TrackedPerson2d.msg

@@ -0,0 +1,10 @@
+# Message defining a 2d image bbox of a tracked person
+#
+
+uint64      track_id        # unique identifier of the target, consistent over time
+float32     person_height   # 3d height of person in m
+int32       x               # top left corner x of 2d image bbox
+int32       y               # top left corner y of 2d image bbox
+uint32      w               # width of 2d image bbox
+uint32      h               # height of 2d image bbox
+float32     depth           # distance from the camera in m