ok, simulation iterative and get performances with executable

config/sim_viz.rviz

+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 775
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /iri_people_simulation_approaching/tracksMarkers
+      Name: MarkerArray
+      Namespaces:
+        ids: true
+        robot: true
+        tracks: true
+      Queue Size: 100
+      Value: true
+    - Alpha: 0.7
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 34.7855
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.785398
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785398
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1056
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007430000003efc0100000002fb0000000800540069006d0065010000000000000743000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004be0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1859
+  X: 61
+  Y: 24

include/people_simulation_alg_node.h

@@ -34,11 +34,13 @@
 #include <visualization_msgs/MarkerArray.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <iri_perception_msgs/detectionArray.h>
-#include <iri_perception_msgs/InitialiceSim.h>
+#include <actionlib/client/simple_action_client.h>
 
 // [service client headers]
 #include <std_srvs/Empty.h>
-
+#include <iri_perception_msgs/InitialiceSim.h>
+#include <actionlib/client/terminal_state.h>
+#include <iri_perception_msgs/restartSim.h>
 // [action server client headers]
 
 /**
@@ -92,6 +94,8 @@ class PeopleSimulationAlgNode : public algorithm_base::IriBaseAlgorithm<PeopleSi
     tf::TransformListener tf_listener_;
     std::string robot_;
 
+    
+
   public:
    /**
     * \brief Constructor

launch/people_sim.launch

@@ -16,7 +16,7 @@
 		type="iri_people_simulation_approaching"
 		name="iri_people_simulation_approaching"
 		output="screen">
-	    <param name="number_persons" value="5"/>
+	    <param name="number_persons" value="20"/>
         <param name="vis_mode" value="True"/>
         <param name="robot" type="string" value="$(optenv ROBOT tibi)" />
         <param name="destination_map_path" value="$(find iri_people_simulation_approaching)/map/4_destinations.txt" />

launch/people_simulation.launch

@@ -7,13 +7,14 @@
           type="iri_people_simulation_approaching"
           name="people_simulation_approaching"
           output="screen">
-      <param name="number_persons" value="5"/>
+      <param name="number_persons" value="20"/>
       <param name="vis_mode"       value="True"/>
       <param name="robot"          value="$(optenv ROBOT tibi)" />
       <param name="destination_map_path" value="$(find iri_akp_local_planner_approaching)/maps/$(arg map)_destinations.txt" />
       <!-- TODO change map with arg -->
       <remap from="~tracks"        to="/$(optenv ROBOT tibi)/mht/tracks"/>
-      <remap from="~tracksMarkers" to="/$(optenv ROBOT tibi)/mht/tracksMarkers"/>
+      <remap from="~tracksMarkers" to="/$(optenv ROBOT tibi)/mht/tracksMarkers"/> 
+      <remap from="~init_simulations" to="/$(optenv ROBOT tibi)/init_simulations"/> 
     </node>
 
 </launch>

map/empty_destinations.txt

+4
+0, 0, -15, 0.5, 1, 1
+1, 15, 15, 0.5, 1, 0
+2, 0, 15, 0.5, 1, 3
+3, 15, -15, 0.5, 1, 2

src/people_simulation_alg_node.cpp

@@ -99,6 +99,19 @@ void PeopleSimulationAlgNode::init_sim()
 
 void PeopleSimulationAlgNode::mainNodeThread(void)
 {
+
+//ROS_INFO(" PeopleSimulationAlgNode::mainNodeThread !!!");
+
+
+//ros::init(argc, argv, "init_simulations");
+// ros::NodeHandle nh("~");
+// PoseServer server;
+// ros::ServiceServer service = nh.advertiseService("/epsilon/get_pose", server.compute_Pose);
+
+
+
+
+
   // [fill msg structures]
   //peopleTrackingArray_msg_.header.seq = seq_cont_; //autocompeted by ros.... lol
   peopleTrackingArray_msg_.header.stamp = ros::Time::now();
@@ -217,6 +230,11 @@ void PeopleSimulationAlgNode::mainNodeThread(void)
 
   // [publish messages]
   this->tracks_publisher_.publish(this->peopleTrackingArray_msg_);
+
+
+ //ros::spinOnce();
+
+
 }
 
 /*  [subscriber callbacks] */
@@ -301,7 +319,7 @@ void PeopleSimulationAlgNode::init_simulations_mutex_exit(void)
 
 bool PeopleSimulationAlgNode::init_simulationsCallback(iri_perception_msgs::InitialiceSim::Request &req, iri_perception_msgs::InitialiceSim::Response &res)
 {
-  //ROS_INFO("dabo [People_companion] AkpLocalPlanner::init_simulationsCallback: New Request Received!");
+  ROS_INFO("tibi [People_companion] PeopleSimulationAlgNode::init_simulationsCallback: New Request Received!");
 
   //use appropiate mutex to shared variables if necessary
   //this->alg_.lock();
@@ -335,12 +353,18 @@ bool PeopleSimulationAlgNode::init_simulationsCallback(iri_perception_msgs::Init
       break;
   
     }
-    scene_.set_actual_simulation_case(Actual_case_ROS_);
-    Spoint person_goal_init_point=Spoint(req.init.initial_position[2].x,req.init.initial_position[2].y,ros::Time::now().toSec()); 
-
-    ROS_INFO("NodoPruebaBacioAlgNode::init_simulationsCallback: person_goal_init_point.x=%f; person_goal_init_point.y=%f",person_goal_init_point.x, person_goal_init_point.y);
-    scene_.set_person_goal_init_poin(person_goal_init_point);
 
+   ROS_INFO(" [PERSONS] 1");
+   scene_.set_actual_simulation_case(Actual_case_ROS_);
+   if(Action_ROS_==Cscene_sim::START){
+     ROS_INFO(" [PERSONS] 2");
+     Spoint person_goal_init_point=Spoint(req.init.initial_position[2].x,req.init.initial_position[2].y,ros::Time::now().toSec()); 
+     ROS_INFO(" [PERSONS] 3");
+     ROS_INFO("NodoPruebaBacioAlgNode::init_simulationsCallback: person_goal_init_point.x=%f; person_goal_init_point.y=%f",person_goal_init_point.x, person_goal_init_point.y);
+     scene_.set_person_goal_init_poin(person_goal_init_point);
+   }
+
+ ROS_INFO(" [PERSONS] 4");
 
  //ROS_INFO("AkpLocalPlanner::init_simulationsCallback: Init person goal Spoint; orientation=%f",req.init.orientation[2]);
  //   person_goal_init_point.print();