probar sim iterativo

launch/people_simulation.launch

@@ -14,7 +14,7 @@
       <!-- TODO change map with arg -->
       <remap from="~tracks"        to="/$(optenv ROBOT tibi)/tracks"/>
       <remap from="~tracksMarkers" to="/$(optenv ROBOT tibi)/tracksMarkers"/> 
-      <remap from="~init_simulations" to="/$(optenv ROBOT tibi)/init_simulations"/> 
+      <remap from="~init_simulations" to="/init_simulations"/> 
     </node>
 
 </launch>

src/people_simulation_alg_node.cpp

@@ -9,8 +9,7 @@ PeopleSimulationAlgNode::PeopleSimulationAlgNode(void) :
 {
   //init class attributes if necessary
   //this->loop_rate_ = 10;//in [Hz] Es de ubuntu 14.04
-  setRate(10);
-
+  setRate(10);//ROS_INFO("PeopleSimulationAlgNode::init_node");
   // [init publishers]
   this->tracksMarkers_publisher_ = this->public_node_handle_.advertise<visualization_msgs::MarkerArray>("tracksMarkers", 100);
   this->tracks_publisher_ = this->public_node_handle_.advertise<iri_perception_msgs::detectionArray>("tracks", 100);
@@ -102,26 +101,19 @@ void PeopleSimulationAlgNode::init_sim()
 
 void PeopleSimulationAlgNode::mainNodeThread(void)
 {
-
-//ROS_INFO(" PeopleSimulationAlgNode::mainNodeThread !!!");
-
+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();
   //for every new trajectory
 	iri_perception_msgs::detection person;
 
-
 	//scene update
     this->alg_.lock();
 // ------------------------------------------------------------------------
@@ -331,7 +323,6 @@ int main(int argc,char *argv[])
 }
 
 
-
 void PeopleSimulationAlgNode::init_simulations_mutex_enter(void)
 {
   pthread_mutex_lock(&this->init_simulations_mutex_);