Updated ROS2 package

.gitignore

@@ -5,4 +5,4 @@ build
 *~
 \#*#
 .\#*
-
+.vscode/

.gitmodules

@@ -1,6 +0,0 @@
-[submodule "2ndparty/spencer_messages"]
-	path = 2ndparty/spencer_messages
-	url = https://github.com/spencer-project/spencer_messages.git
-[submodule "2ndparty/spencer_tracking_rviz_plugin"]
-	path = 2ndparty/spencer_tracking_rviz_plugin
-	url = https://github.com/srl-freiburg/spencer_tracking_rviz_plugin.git

3rdparty/libpedsim/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(pedsim)
 
 #------------------ Configuration ------------------#
@@ -15,27 +15,21 @@ if(SHALL_DEBUG)
   message("Debugging activated")
   add_definitions(-O0 -DDEBUG -ggdb -g3 -rdynamic)
 else(SHALL_DEBUG)
-  message("Debugging deactivated")
+  # message("Debugging deactivated")
   add_definitions(-Os)
 endif(SHALL_DEBUG)
 
 
-find_package(catkin REQUIRED COMPONENTS
-  roscpp
-)
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+
 find_package(Boost REQUIRED)
 
-catkin_package(
-  CATKIN_DEPENDS  roscpp
-  INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/include/
-  LIBRARIES pedsim
+set(dependencies
+  rclcpp
 )
 
-include_directories(
-  ${catkin_INCLUDE_DIRS}
-  ${PROJECT_SOURCE_DIR}/include/pedsim/
-  ${roscpp_INCLUDE_DIRS}
-)
+include_directories(include)
 
 set(SOURCES
   src/ped_agent.cpp
@@ -52,3 +46,20 @@ add_library(pedsim ${SOURCES})
 target_link_libraries(pedsim
   ${BOOST_LIBRARIES}
 )
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+install(TARGETS
+  pedsim
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_export_include_directories(include)
+ament_export_libraries(pedsim)
+ament_export_dependencies(${dependencies})
+ament_package()
+

3rdparty/libpedsim/include/pedsim/ped_agent.h

@@ -12,7 +12,7 @@
 #define LIBEXPORT
 #endif
 
-#include "ped_vector.h"
+#include "pedsim/ped_vector.h"
 
 #include <deque>
 #include <set>

3rdparty/libpedsim/include/pedsim/ped_includes.h

@@ -1,10 +1,10 @@
 #ifndef _ped_includes_h_
 #define _ped_includes_h_ 1
 
-#include "ped_agent.h"
-#include "ped_obstacle.h"
-#include "ped_scene.h"
-#include "ped_waypoint.h"
+#include "pedsim/ped_agent.h"
+#include "pedsim/ped_obstacle.h"
+#include "pedsim/ped_scene.h"
+#include "pedsim/ped_waypoint.h"
 
 namespace Ped {
 const double LIBEXPORT LIBPEDSIM_VERSION = 2.2;

3rdparty/libpedsim/include/pedsim/ped_obstacle.h

@@ -12,7 +12,7 @@
 #define LIBEXPORT
 #endif
 
-#include "ped_vector.h"
+#include "pedsim/ped_vector.h"
 
 namespace Ped {
 

3rdparty/libpedsim/include/pedsim/ped_vector.h

@@ -13,7 +13,7 @@
 #endif
 
 #include <cmath>
-#include "ped_angle.h"
+#include "pedsim/ped_angle.h"
 
 namespace Ped {
 /// Vector helper class. This is basically a struct with some related functions

3rdparty/libpedsim/include/pedsim/ped_waypoint.h

@@ -13,7 +13,7 @@
 #endif
 
 #include <cstddef>
-#include "ped_vector.h"
+#include "pedsim/ped_vector.h"
 
 using namespace std;
 

3rdparty/libpedsim/package.xml

@@ -1,14 +1,16 @@
 <?xml version="1.0"?>
-<package>
+<package format ="3">
   <name>pedsim</name>
   <version>0.2.0</version>
   <description>The pedsim package</description>
   <maintainer email="[email protected]">Billy Okal</maintainer>
   <maintainer email="[email protected]">Luigi Palmieri</maintainer>
   <license>BSD</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>roscpp</build_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
 
-  <run_depend>roscpp</run_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>

3rdparty/libpedsim/src/ped_agent.cpp

@@ -3,10 +3,10 @@
 // Copyright (c) 2003 - 20012 by Christian Gloor
 //
 
-#include "ped_agent.h"
-#include "ped_obstacle.h"
-#include "ped_scene.h"
-#include "ped_waypoint.h"
+#include "pedsim/ped_agent.h"
+#include "pedsim/ped_obstacle.h"
+#include "pedsim/ped_scene.h"
+#include "pedsim/ped_waypoint.h"
 
 #include <algorithm>
 #include <cmath>
@@ -52,12 +52,13 @@ Ped::Tagent::~Tagent() {}
 /// \warning Bad things will happen if the agent is not assigned to a scene. But
 /// usually, Tscene takes care of that.
 /// \param   *s A valid Tscene initialized earlier.
-void Ped::Tagent::assignScene(Ped::Tscene* sceneIn) { scene = sceneIn; }
+void Ped::Tagent::assignScene(Ped::Tscene *sceneIn) { scene = sceneIn; }
 
-void Ped::Tagent::removeAgentFromNeighbors(const Ped::Tagent* agentIn) {
+void Ped::Tagent::removeAgentFromNeighbors(const Ped::Tagent *agentIn) {
   // search agent in neighbors, and remove him
-  set<const Ped::Tagent*>::iterator foundNeighbor = neighbors.find(agentIn);
-  if (foundNeighbor != neighbors.end()) neighbors.erase(foundNeighbor);
+  set<const Ped::Tagent *>::iterator foundNeighbor = neighbors.find(agentIn);
+  if (foundNeighbor != neighbors.end())
+    neighbors.erase(foundNeighbor);
 }
 
 /// Sets the maximum velocity of an agent (vmax). Even if pushed by other
@@ -99,7 +100,7 @@ void Ped::Tagent::setForceFactorObstacle(double f) { forceFactorObstacle = f; }
 /// \return  Tvector: the calculated force
 Ped::Tvector Ped::Tagent::desiredForce() {
   // get destination
-  Twaypoint* waypoint = getCurrentWaypoint();
+  Twaypoint *waypoint = getCurrentWaypoint();
 
   // if there is no destination, don't move
   if (waypoint == NULL) {
@@ -140,9 +141,10 @@ Ped::Tvector Ped::Tagent::socialForce() const {
   const double n_prime = 3;
 
   Tvector force;
-  for (const Ped::Tagent* other : neighbors) {
+  for (const Ped::Tagent *other : neighbors) {
     // don't compute social force to yourself
-    if (other->id == id) continue;
+    if (other->id == id)
+      continue;
 
     // compute difference between both agents' positions
     Tvector diff = other->p - p;
@@ -193,10 +195,10 @@ Ped::Tvector Ped::Tagent::obstacleForce() const {
   Ped::Tvector minDiff;
   double minDistanceSquared = INFINITY;
 
-  for (const Tobstacle* obstacle : scene->obstacles) {
+  for (const Tobstacle *obstacle : scene->obstacles) {
     Ped::Tvector closestPoint = obstacle->closestPoint(p);
     Ped::Tvector diff = p - closestPoint;
-    double distanceSquared = diff.lengthSquared();  // use squared distance to
+    double distanceSquared = diff.lengthSquared(); // use squared distance to
     // avoid computing square
     // root
     if (distanceSquared < minDistanceSquared) {
@@ -229,8 +231,10 @@ void Ped::Tagent::computeForces() {
 
   // update forces
   desiredforce = desiredForce();
-  if (forceFactorSocial > 0) socialforce = socialForce();
-  if (forceFactorObstacle > 0) obstacleforce = obstacleForce();
+  if (forceFactorSocial > 0)
+    socialforce = socialForce();
+  if (forceFactorObstacle > 0)
+    obstacleforce = obstacleForce();
   myforce = myForce(desiredDirection);
 }
 
@@ -253,7 +257,8 @@ void Ped::Tagent::move(double stepSizeIn) {
 
   // don't exceed maximal speed
   double speed = v.length();
-  if (speed > vmax) v = v.normalized() * vmax;
+  if (speed > vmax)
+    v = v.normalized() * vmax;
 
   // internal position update = actual move
   p += stepSizeIn * v;

3rdparty/libpedsim/src/ped_angle.cpp

@@ -3,28 +3,28 @@
 // Copyright (c) 2003 - 2012 by Christian Gloor
 //
 
-#include "ped_angle.h"
+#include "pedsim/ped_angle.h"
 
-bool Ped::Tangle::operator==(const Ped::Tangle& otherIn) const {
+bool Ped::Tangle::operator==(const Ped::Tangle &otherIn) const {
   return value == otherIn.value;
 }
 
-bool Ped::Tangle::operator!=(const Ped::Tangle& otherIn) const {
+bool Ped::Tangle::operator!=(const Ped::Tangle &otherIn) const {
   return value != otherIn.value;
 }
 
-bool Ped::Tangle::operator<(const Ped::Tangle& otherIn) const {
+bool Ped::Tangle::operator<(const Ped::Tangle &otherIn) const {
   return value < otherIn.value;
 }
 
-bool Ped::Tangle::operator<=(const Ped::Tangle& otherIn) const {
+bool Ped::Tangle::operator<=(const Ped::Tangle &otherIn) const {
   return value <= otherIn.value;
 }
 
-bool Ped::Tangle::operator>(const Ped::Tangle& otherIn) const {
+bool Ped::Tangle::operator>(const Ped::Tangle &otherIn) const {
   return value > otherIn.value;
 }
 
-bool Ped::Tangle::operator>=(const Ped::Tangle& otherIn) const {
+bool Ped::Tangle::operator>=(const Ped::Tangle &otherIn) const {
   return value >= otherIn.value;
 }

3rdparty/libpedsim/src/ped_obstacle.cpp

@@ -3,7 +3,7 @@
 // Copyright (c) 2003 - 2012 by Christian Gloor
 //
 
-#include "ped_obstacle.h"
+#include "pedsim/ped_obstacle.h"
 
 #include <cmath>
 #include <vector>
@@ -42,7 +42,7 @@ Ped::Tobstacle::Tobstacle(double pax, double pay, double pbx, double pby) {
 /// \date    2013-08-02
 /// \param startIn The first corner of the obstacle.
 /// \param endIn The second corner of the obstacle.
-Ped::Tobstacle::Tobstacle(const Tvector& startIn, const Tvector& endIn) {
+Ped::Tobstacle::Tobstacle(const Tvector &startIn, const Tvector &endIn) {
   id = staticid++;
   ax = startIn.x;
   ay = startIn.y;
@@ -75,21 +75,21 @@ void Ped::Tobstacle::setPosition(double pax, double pay, double pbx,
   by = pby;
 }
 
-void Ped::Tobstacle::setPosition(const Tvector& startIn, const Tvector& endIn) {
+void Ped::Tobstacle::setPosition(const Tvector &startIn, const Tvector &endIn) {
   setPosition(startIn.x, startIn.y, endIn.x, endIn.y);
 }
 
-void Ped::Tobstacle::setStartPoint(const Tvector& startIn) {
+void Ped::Tobstacle::setStartPoint(const Tvector &startIn) {
   ax = startIn.x;
   ay = startIn.y;
 }
 
-void Ped::Tobstacle::setEndPoint(const Tvector& endIn) {
+void Ped::Tobstacle::setEndPoint(const Tvector &endIn) {
   bx = endIn.x;
   by = endIn.y;
 }
 
-Ped::Tvector Ped::Tobstacle::closestPoint(const Tvector& pointIn) const {
+Ped::Tvector Ped::Tobstacle::closestPoint(const Tvector &pointIn) const {
   Tvector startPoint(ax, ay);
   Tvector endPoint(bx, by);
   Tvector relativeEndPoint = endPoint - startPoint;
@@ -124,8 +124,8 @@ Ped::Tvector Ped::Tobstacle::closestPoint(double p1, double p2) const {
 /// \param   rotationCenterIn The point the obstacle will be rotated around.
 /// \param   angleIn The angle the obstacle will be rotated, where phi is given
 /// in radians
-void Ped::Tobstacle::rotate(const Ped::Tvector& rotationCenterIn,
-                            const Ped::Tangle& angleIn) {
+void Ped::Tobstacle::rotate(const Ped::Tvector &rotationCenterIn,
+                            const Ped::Tangle &angleIn) {
   double angleValue = angleIn.toRadian();
   double sinPhi = sin(angleValue);
   double cosPhi = cos(angleValue);