fix message type conflict and function return type

README.md

@@ -43,21 +43,18 @@ The authors would like to thank Wilko Schwarting for sharing its Matlab implemen
 ## Software Requirements
 * ROS installation
 * Ubuntu
-* Pedestrian Simulator: pedsim_ros: https://github.com/bbrito/pedsim_ros
+* Pedestrian Simulator: pedsim_ros: https://github.com/weemma/pedsim_ros
     * Branch: four_persons
 
 ## Instalation instructions
-This set of instructions were only tested for Ubuntu16 with ROS-Kinetic. Additionally, we assume that you already have a complete ROS installation.
+This set of instructions were tested for Ubuntu16.04 with ROS-Kinetic and Ubuntu20.04 with ROS-Noetic. Additionally, we assume that you already have a complete ROS installation.
 * Please follow the following instructions:
 ```
-sudo apt-get install ros-kinetic-jackal-control ros-kinetic-jackal-gazebo ros-kinetic-jackal-simulator ros-kinetic-jackal-description ros-kinetic-jackal-desktop ros-kinetic-jackal-navigation ros-kinetic-jackal-viz
-sudo apt install ros-kinetic-people-msgs
-cd [workspace]/src
-git clone https://github.com/bbrito/pedsim_ros.git -b four_persons
-git clone https://github.com/spencer-project/spencer_messages.git
-git clone https://github.com/srl-freiburg/spencer_tracking_rviz_plugin.git
+sudo apt-get install ros-$ROS_DISTRO-jackal-control ros-$ROS_DISTRO-jackal-gazebo ros-$ROS_DISTRO-jackal-simulator ros-$ROS_DISTRO-jackal-description ros-$ROS_DISTRO-jackal-desktop ros-$ROS_DISTRO-jackal-navigation ros-$ROS_DISTRO-jackal-viz ros-$ROS_DISTRO-people-msgs
+cd [catkin_workspace]/src
+git clone --recursive https://github.com/weemma/pedsim_ros.git -b four_persons
 git clone https://github.com/tud-amr/amr-lmpcc.git
-cc ../
+cd ../
 rosdep install --from-paths src --ignore-src -r -y
 catkin_make
 source devel/setup.bash
@@ -77,7 +74,7 @@ Repository: https://github.com/bbrito/acado/blob/four_persons/examples/ocp/biore
 
         1. roslaunch lmpcc_obstacle_feed lmpcc_obstacle_feed.launch
 
-*Start lmpcc controller
+* Start lmpcc controller
 
         1. roslaunch lmpcc lmpcc.launch
 

lmpcc/config/lmpcc_config_parameter.yaml

@@ -5,7 +5,7 @@ gazebo_simulation: false
 # Debug modes
 activate_output: true
 activate_debug_output: false
-activate_timing_output: false
+activate_timing_output: true
 activate_visualization: true
 activate_feedback_message: true
 

lmpcc/include/lmpcc/lmpcc_controller.h

@@ -185,7 +185,7 @@ class LMPCC
     bool initialize();
 
 
-    bool initialize_visuals();
+    void initialize_visuals();
 
     /**
      * @brief StateCallBack: Get current state of the robot

lmpcc/src/lmpcc_controller.cpp

@@ -228,7 +228,7 @@ bool LMPCC::initialize()
 	}
 }
 
-bool LMPCC::initialize_visuals()
+void LMPCC::initialize_visuals()
 {
     //ROS_INFO_STREAM("initialize_visuals");
     robot_collision_space_pub_ = nh.advertise<visualization_msgs::MarkerArray>("/robot_collision_space", 100);
@@ -483,8 +483,9 @@ void LMPCC::controlLoop(const ros::TimerEvent &event)
         acadoVariables.u[3] = 0.0000001;           //slack variable
 
 		if(acadoVariables.x[3] > ss[2]) {
-
-            if((std::sqrt(std::pow(current_state_(0)-15,2)+std::pow(current_state_(1),2))<1) || (current_state_(0)>15)){
+            double goal_x = lmpcc_config_->ref_x_[lmpcc_config_->ref_x_.size() - 1];
+            double goal_y = lmpcc_config_->ref_y_[lmpcc_config_->ref_y_.size() - 1];
+            if(std::sqrt(std::pow(current_state_(0) - goal_x, 2) + std::pow(current_state_(1) - goal_y, 2)) < 3){
 		        goal_reached_ = true;
                 ROS_ERROR_STREAM("GOAL REACHED");
 
@@ -499,6 +500,18 @@ void LMPCC::controlLoop(const ros::TimerEvent &event)
                     acadoVariables.x[3] = referencePath.GetS0();
 
                     //ROS_ERROR_STREAM("LOOP STARTED");
+
+                    /** Set task flags and counters **/
+                    segment_counter = 0;
+                    goal_reached_ = false;
+
+                    if (lmpcc_config_->activate_debug_output_)
+                        referencePath.PrintLocalPath(ss,xx,yy);     // Print local reference path
+                    if (lmpcc_config_->activate_visualization_)
+                    {
+                        publishLocalRefPath();                      // Publish local reference path for visualization
+                        publishGlobalPlan();                        // Publish global reference path for visualization
+                    }
                 }
             } else{
 			    segment_counter++;
@@ -667,6 +680,8 @@ void LMPCC::controlLoop(const ros::TimerEvent &event)
 	}
 
     if(!enable_output_ || acado_getKKT() > lmpcc_config_->kkt_tolerance_) {
+        if (acado_getKKT() > lmpcc_config_->kkt_tolerance_)
+            ROS_INFO_STREAM("Failed to find solution! KKT value: " << acado_getKKT() << ", tolerance: " << lmpcc_config_->kkt_tolerance_);
 		publishZeroJointVelocity();
 	}
 	else {

lmpcc_obstacle_feed/config/lmpcc_obstacle_feed_config.yaml

@@ -5,6 +5,7 @@ obstacle_feed_mode: 4
 # Debugging info
 activate_debug_output: false
 activate_visualization: true
+activate_timing_output: true
 
 # Rate at which ellipse obstacles should be send out
 update_rate: 20
@@ -30,7 +31,7 @@ subscribe:
 # Detection criteria
 detected_obstacles:
     distance_threshold: 50
-    obstacle_number: 4
+    obstacle_number: 8
     min_obstacle_volume: 0.03
     max_obstacle_volume: 1.0
     obstacle_size: 0.3

lmpcc_obstacle_feed/include/lmpcc_obstacle_feed/lmpcc_obstacle_feed.h

@@ -154,7 +154,7 @@ class ObstacleFeed
     void clearDataMember();
     void detectionsCallback(const vision_msgs::Detection3DArray& objects);
     void pedestriansCallback(const spencer_tracking_msgs::TrackedPersons& person);
-    void PedsimCallback(const pedsim_msgs::TrackedPersons& person);
+    void PedsimCallback(const spencer_tracking_msgs::TrackedPersons& person);
     void optitrackCallback(const nav_msgs::Path& predicted_path);
     void updateObstacles(const ros::TimerEvent& event);
     void publishObstacles(const lmpcc_msgs::lmpcc_obstacle_array& obstacles);

lmpcc_obstacle_feed/include/lmpcc_obstacle_feed/lmpcc_obstacle_feed_configuration.h

@@ -78,6 +78,7 @@ class lmpcc_obstacle_feed_configuration
 
     bool activate_debug_output_;
     bool activate_visualization_;
+    bool activate_timing_output_;
 
     int update_rate_;
     int discretization_steps_;

lmpcc_obstacle_feed/src/lmpcc_obstacle_feed.cpp

@@ -395,10 +395,10 @@ void ObstacleFeed::detectionsCallback(const vision_msgs::Detection3DArray& objec
 
 }
 
-void ObstacleFeed::PedsimCallback(const pedsim_msgs::TrackedPersons& person)
+void ObstacleFeed::PedsimCallback(const spencer_tracking_msgs::TrackedPersons& person)
 {
     //ROS_INFO_STREAM("PEDSIM callback!");
-
+    auto start = std::chrono::steady_clock::now();
     double Xp, Yp;
     double q1, q2, q3, q4;
     double mag_q, distance;
@@ -616,6 +616,11 @@ void ObstacleFeed::PedsimCallback(const pedsim_msgs::TrackedPersons& person)
         visualizeObstacles(local_ellipses);
 
     }
+    auto end = std::chrono::steady_clock::now();
+    double dynamic_obs = double(std::chrono::duration_cast <std::chrono::microseconds> (end-start).count());
+
+    if (lmpcc_obstacle_feed_config_->activate_timing_output_)
+        ROS_INFO_STREAM("Dynamic obstacle solve time " << dynamic_obs  << " us");
 }
 
 void ObstacleFeed::pedestriansCallback(const spencer_tracking_msgs::TrackedPersons& person)

lmpcc_obstacle_feed/src/lmpcc_obstacle_feed_configuration.cpp

@@ -90,6 +90,13 @@ bool lmpcc_obstacle_feed_configuration::initialize()
         return false;
     }
 
+    // read parameter from parameter server if not set than terminate code, as this parameter is essential parameter
+    if (!nh.getParam ("activate_timing_output", activate_timing_output_) )
+    {
+        ROS_WARN(" Parameter 'activate_timing_output' not set on %s node " , ros::this_node::getName().c_str());
+        return false;
+    }
+
     // read parameter from parameter server if not set than terminate code, as this parameter is essential parameter
     if (!nh.getParam ("update_rate", update_rate_) )
     {

static_collision_avoidance/src/static_environment.cpp

@@ -314,10 +314,10 @@ void StaticEnvironment::ComputeCollisionFreeArea()
     }
     auto end = std::chrono::steady_clock::now();
 
-    te_collision_free_ = double(std::chrono::duration_cast <std::chrono::milliseconds> (end-start).count());
+    te_collision_free_ = double(std::chrono::duration_cast <std::chrono::microseconds> (end-start).count());
 
     if (activate_timing_output_)
-        ROS_INFO_STREAM("Free space solve time " << te_collision_free_  << " ms");
+        ROS_INFO_STREAM("Free space solve time " << te_collision_free_  << " us");
 }
 
 void StaticEnvironment::computeConstraint(int x_i, int y_i, double x_path, double y_path, double psi_path, int N)