iterative simulations (needs a little more) and performances(done), and controler real(done).

cfg/PeopleSimulation.cfg

@@ -40,6 +40,8 @@ gen = ParameterGenerator()
 #       Name                       Type       Reconfiguration level            Description                       Default   Min   Max
 #gen.add("velocity_scale_factor",  double_t,  0,                               "Maximum velocity scale factor",  0.5,      0.0,  1.0)
 gen.add("robot",   str_t,   0,    "robot name",   "unknown")
+gen.add("id_person_to_approach",       int_t,      0,   " id of the person to approach",    1,  0,  1)
+
 gen.add("simulation_mode",       int_t,      0,                        "Person simulation mode: 0 for normal mode: constant number of persons; 1 for people density growing",    0,  0,  1)
 gen.add("vis_mode",              bool_t,      0,                           "Visualization markers are enabled or not",   False)
 gen.add("freeze",              bool_t,      0,                  "Freezes persons to publish the same scene over time",   False)

include/people_simulation_alg_node.h

@@ -34,8 +34,7 @@
 #include <visualization_msgs/MarkerArray.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <iri_perception_msgs/detectionArray.h>
-
-
+#include <iri_perception_msgs/InitialiceSim.h>
 
 // [service client headers]
 #include <std_srvs/Empty.h>
@@ -68,6 +67,15 @@ class PeopleSimulationAlgNode : public algorithm_base::IriBaseAlgorithm<PeopleSi
     void reset_mutex_enter(void);
     void reset_mutex_exit(void);
 
+    // server to initialice person positions.
+    ros::ServiceServer init_simulations_server_;
+    bool init_simulationsCallback(iri_perception_msgs::InitialiceSim::Request &req, iri_perception_msgs::InitialiceSim::Response &res);
+    pthread_mutex_t init_simulations_mutex_;
+    void init_simulations_mutex_enter(void);
+    void init_simulations_mutex_exit(void);
+
+     Cscene_sim::action_modeScene Action_ROS_;
+     Cscene_sim::simulation_caseScene Actual_case_ROS_;
     // [client attributes]
 
     // [action server attributes]

launch/people_sim.launch

@@ -7,26 +7,26 @@
  	<node name="map_server" 
 		pkg="map_server" 
 		type="map_server" 
-		args="$(find iri_akp_local_planner)/maps/empty.yaml">
+		args="$(find iri_akp_local_planner_approaching)/maps/empty.yaml">
 			<param name="frame_id" value="/map" />			
 	</node>  
 
 
-  <node pkg="iri_people_simulation"
-		type="iri_people_simulation"
-		name="people_simulation"
+  <node pkg="iri_people_simulation_approaching"
+		type="iri_people_simulation_approaching"
+		name="iri_people_simulation_approaching"
 		output="screen">
-	    <param name="number_persons" value="40"/>
+	    <param name="number_persons" value="5"/>
         <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)/map/4_destinations.txt" />
+        <param name="destination_map_path" value="$(find iri_people_simulation_approaching)/map/4_destinations.txt" />
 
   </node>
 
   <node pkg ="rviz" 
         name="rviz" 
         type="rviz" 
-        args="-d $(find iri_people_simulation)/config/sim_vis.rviz" />
+        args="-d $(find iri_people_simulation_approaching)/config/sim_vis.rviz" />
 
 
 </launch>

launch/people_simulation.launch

@@ -7,7 +7,7 @@
           type="iri_people_simulation_approaching"
           name="people_simulation_approaching"
           output="screen">
-      <param name="number_persons" value="1"/>
+      <param name="number_persons" value="5"/>
       <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" />

map/4_destinations.txt

 4
-0, -8.0, -8.0, 0.25, 3, 1, 2, 3
-1, 4.0, -4.0, 0.25, 3, 0, 2, 3
-2, -4.0, 4.0, 0.25, 3, 0, 1, 3
-3, 8.0, 8.0, 0.25, 3, 0, 1, 2
+0, -20.0, -8.0, 0.25, 3, 1, 2, 3
+1, 29.0, -4.0, 0.25, 3, 0, 2, 3
+2, -4.0, 29.0, 0.25, 3, 0, 1, 3
+3, 20.0, 8.0, 0.25, 3, 0, 1, 2

src/people_simulation_alg_node.cpp

@@ -19,7 +19,9 @@ PeopleSimulationAlgNode::PeopleSimulationAlgNode(void) :
   // [init clients]
   
   // [init action servers]
-  
+    // [init services] => My cervice reset cscene sim, persons to initial pose
+  this->init_simulations_server_ = this->public_node_handle_.advertiseService("init_simulations", &PeopleSimulationAlgNode::init_simulationsCallback, this);
+
   // [init action clients]
   
   init_sim();
@@ -132,7 +134,8 @@ void PeopleSimulationAlgNode::mainNodeThread(void)
 
 	std::vector<SdetectionObservation> obs_scene;
 	obs_scene.push_back( SdetectionObservation(0, ros::Time::now().toSec() ));//void observation, just for the timestamp
-	scene_.update_scene( obs_scene );
+	bool no_comp=false;
+	scene_.update_scene( obs_scene , no_comp);
 
   //publish data
   const std::list<Cperson_abstract *>* person_list = scene_.get_scene( );
@@ -262,14 +265,13 @@ void PeopleSimulationAlgNode::node_config_update(Config &config, uint32_t level)
   this->alg_.lock();
 
 	//ROS_INFO("         *******  algorithm config update  *******\n\n");
-  simulation_mode_ = (PeopleSimulationAlgNode::simulation_mode) config.simulation_mode;
-  vis_mode_ = config.vis_mode;
+  	simulation_mode_ = (PeopleSimulationAlgNode::simulation_mode) config.simulation_mode;
+  	vis_mode_ = config.vis_mode;
 	n_persons_ = config.number_persons;
 	freeze_ = config.freeze;
 	scene_.set_number_virtual_people( n_persons_ );
- 
- 
-	
+  	scene_.set_id_person_to_apperoach(config.id_person_to_approach);
+ 	
   this->alg_.unlock();
 }
 
@@ -282,3 +284,78 @@ int main(int argc,char *argv[])
 {
   return algorithm_base::main<PeopleSimulationAlgNode>(argc, argv, "people_simulation_alg_node");
 }
+
+
+
+void PeopleSimulationAlgNode::init_simulations_mutex_enter(void)
+{
+  pthread_mutex_lock(&this->init_simulations_mutex_);
+}
+
+void PeopleSimulationAlgNode::init_simulations_mutex_exit(void)
+{
+  pthread_mutex_unlock(&this->init_simulations_mutex_);
+}
+
+/* [service callback] => restart persons to initial position */
+
+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!");
+
+  //use appropiate mutex to shared variables if necessary
+  //this->alg_.lock();
+  //this->init_simulations_mutex_enter(); //
+   switch(req.init.state){
+
+     case 0:
+          Action_ROS_=Cscene_sim::START;
+      break;
+      case 1:
+          Action_ROS_=Cscene_sim::ITER;
+      break;
+  
+    }
+    scene_.set_state_Action_person_companion(Action_ROS_);
+   // this->scene_.get_akp_for_person_companion()->set_state_Action(Action_ROS_); // We do not have companion.
+
+   switch(req.init.act_case){
+
+	case 0:
+          ROS_INFO(" [PERSONS] AkpLocalPlanner::computeVelocityCommands: case0");
+         Actual_case_ROS_=Cscene_sim::case0;
+      break;
+      case 1:
+            ROS_INFO(" [PERSONS] AkpLocalPlanner::computeVelocityCommands: case1");
+           Actual_case_ROS_=Cscene_sim::case1;
+      break;
+      case 2:
+            ROS_INFO(" [PERSONS] AkpLocalPlanner::computeVelocityCommands: case2");
+           Actual_case_ROS_=Cscene_sim::case2;
+      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("AkpLocalPlanner::init_simulationsCallback: Init person goal Spoint; orientation=%f",req.init.orientation[2]);
+ //   person_goal_init_point.print();
+
+  // + generar el mensaje a enviar al server.
+  
+
+  //ROS_INFO("NodoPruebaBacioAlgNode::init_simulationsCallback: Processing New Request!");
+  //do operations with req and output on res
+  //res.data2 = req.data1 + my_var;
+
+  //unlock previously blocked shared variables
+  //this->init_simulations_mutex_exit();
+  //this->alg_.unlock();
+
+  return true;
+}
+