git commit -m "first commit"

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>