diff --git a/cfg/PeopleSimulation.cfg b/cfg/PeopleSimulation.cfg
index e61bd3fb0b4266e7935f71b0f97d890e6c315cee..f279d73751420fc798544967bd71ba2b9bc601e8 100755
--- a/cfg/PeopleSimulation.cfg
+++ b/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)
diff --git a/include/people_simulation_alg_node.h b/include/people_simulation_alg_node.h
index 44b547005fc2e57635d7327ca85fcfc9154a43da..7afe371b0d88738f945f62d56b08e1c719ea228e 100644
--- a/include/people_simulation_alg_node.h
+++ b/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]
diff --git a/launch/people_sim.launch b/launch/people_sim.launch
index 3ece09e6d54214aaca6a12082c1c251204ed64c5..86dca540679c3324e929949abfce62d5add820c8 100644
--- a/launch/people_sim.launch
+++ b/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>
diff --git a/launch/people_simulation.launch b/launch/people_simulation.launch
index 508ea7c9369c7385f556bf4d0a38ae6a7918a739..523b302bcc46207bad72c14a44c05aad49398d76 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="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" />
diff --git a/map/4_destinations.txt b/map/4_destinations.txt
index 0c07b631ef44270895d15dc4d999e1b6eeb3e4dd..e0ce4de037d77582a6b14eac2f483a2161c13844 100644
--- a/map/4_destinations.txt
+++ b/map/4_destinations.txt
@@ -1,5 +1,5 @@
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
diff --git a/src/people_simulation_alg_node.cpp b/src/people_simulation_alg_node.cpp
index 0cbe244d3ea4d3e870de570a2aff19e04aa6d1dc..36cb24da3361c8a6e3b3a9a6918664209e8bbb3a 100644
--- a/src/people_simulation_alg_node.cpp
+++ b/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;
+}
+