diff --git a/include/people_simulation_alg_node.h b/include/people_simulation_alg_node.h
index 3443b1b82d7885cd170f4ebd07e72ae0339fc045..653de2eae2c85a6337b4653a35a44ea120892dc0 100644
--- a/include/people_simulation_alg_node.h
+++ b/include/people_simulation_alg_node.h
@@ -94,7 +94,7 @@ class PeopleSimulationAlgNode : public algorithm_base::IriBaseAlgorithm<PeopleSi
     tf::TransformListener tf_listener_;
     std::string robot_;
 
-    
+    unsigned int id_person_to_approach_ros_;
 
   public:
    /**
diff --git a/launch/people_sim.launch b/launch/people_sim.launch
index 080ca27c6aea75273334f134269f5c42ca088e68..15f5e7fd65ae1ff74fe3a47b725460afdef6380d 100644
--- a/launch/people_sim.launch
+++ b/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="20"/>
+	    <param name="number_persons" value="40"/>
         <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" />
diff --git a/launch/people_simulation.launch b/launch/people_simulation.launch
index 9854cf8e3f172fc6b4a29d51bec0a8de363a1c0e..006886f032dba16bc06d0f89f1c4f93e456e26aa 100644
--- a/launch/people_simulation.launch
+++ b/launch/people_simulation.launch
@@ -7,7 +7,7 @@
           type="iri_people_simulation_approaching"
           name="people_simulation_approaching"
           output="screen">
-      <param name="number_persons" value="20"/>
+      <param name="number_persons" value="1"/>
       <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" />
diff --git a/map/empty_destinations.txt b/map/empty_destinations.txt
index 4d07d23d3b4843b88245310076d0cebb0099fad9..551e10c882251185fa96b71ed35ab7b78d8afc49 100644
--- a/map/empty_destinations.txt
+++ b/map/empty_destinations.txt
@@ -1,5 +1,7 @@
-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
+6
+0, 0, -25, 0.5, 5, 1,2,3,4,5
+1, 25, 25, 0.5, 5, 0,2,3,4,5
+2, 0, 25, 0.5, 5, 0,1,3,4,5
+3, 25, -25, 0.5, 5, 0,1,2,4,5
+4, 15, 25, 0.5, 5, 0,1,2,3,5
+5, 15, -25, 0.5, 5, 0,1,2,3,4
diff --git a/src/people_simulation_alg_node.cpp b/src/people_simulation_alg_node.cpp
index 106dec207239f81b90536d32d180b7802409981f..1236a5d53c7019b2860c79ba791dba5a2cdd184b 100644
--- a/src/people_simulation_alg_node.cpp
+++ b/src/people_simulation_alg_node.cpp
@@ -194,26 +194,36 @@ void PeopleSimulationAlgNode::mainNodeThread(void)
     for( std::list<Cperson_abstract*>::const_iterator iit = person_list->begin();
           iit!=person_list->end(); iit++ )
     {
-			track_marker_.id = (*iit)->get_id();
-			track_marker_.pose.position.x = (*iit)->get_current_pointV().x;
-			track_marker_.pose.position.y = (*iit)->get_current_pointV().y;
-			MarkerArray_msg_.markers.push_back(track_marker_);
-			id_marker_.id = (*iit)->get_id();
-			id_marker_.pose.position.x = (*iit)->get_current_pointV().x;
-			id_marker_.pose.position.y = (*iit)->get_current_pointV().y;
-			std::ostringstream target_id;
-			target_id << id_marker_.id;
-			id_marker_.text = target_id.str();
-			MarkerArray_msg_.markers.push_back(id_marker_);
-		}
+	track_marker_.id = (*iit)->get_id();
+	track_marker_.pose.position.x = (*iit)->get_current_pointV().x;
+	track_marker_.pose.position.y = (*iit)->get_current_pointV().y;
+	if(track_marker_.id==id_person_to_approach_ros_){
+	  track_marker_.color.r = 1.0;
+	  track_marker_.color.g = 0.0;
+	  track_marker_.color.b = 0.0;
+	}else{
+	  track_marker_.color.r = 0.0;
+	  track_marker_.color.g = 1.0;
+	  track_marker_.color.b = 0.0;
+	}
+
+	MarkerArray_msg_.markers.push_back(track_marker_);
+	id_marker_.id = (*iit)->get_id();
+	id_marker_.pose.position.x = (*iit)->get_current_pointV().x;
+	id_marker_.pose.position.y = (*iit)->get_current_pointV().y;
+	std::ostringstream target_id;
+	target_id << id_marker_.id;
+	id_marker_.text = target_id.str();
+	MarkerArray_msg_.markers.push_back(id_marker_);
+    }
     //draw robot if any
     if (  scene_.get_robot() != NULL )
     {
-		  track_marker_.id = 0;
+	track_marker_.id = 0;
     	track_marker_.ns = "robot";
-		  track_marker_.pose.position.x = scene_.get_robot()->get_current_pointV().x;
-		  track_marker_.pose.position.y = scene_.get_robot()->get_current_pointV().y;
-		  MarkerArray_msg_.markers.push_back(track_marker_);
+	track_marker_.pose.position.x = scene_.get_robot()->get_current_pointV().x;
+	track_marker_.pose.position.y = scene_.get_robot()->get_current_pointV().y;
+	MarkerArray_msg_.markers.push_back(track_marker_);
     	track_marker_.ns = "tracks";
     }
 
@@ -289,6 +299,7 @@ void PeopleSimulationAlgNode::node_config_update(Config &config, uint32_t level)
 	freeze_ = config.freeze;
 	scene_.set_number_virtual_people( n_persons_ );
   	scene_.set_id_person_to_apperoach(config.id_person_to_approach);
+	id_person_to_approach_ros_=config.id_person_to_approach;
  	
   this->alg_.unlock();
 }