diff --git a/config/sim_viz.rviz b/config/sim_viz.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..fe0606d417f06fdc60b51fe7e89132e0e531855e
--- /dev/null
+++ b/config/sim_viz.rviz
@@ -0,0 +1,128 @@
+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
diff --git a/include/people_simulation_alg_node.h b/include/people_simulation_alg_node.h
index 7afe371b0d88738f945f62d56b08e1c719ea228e..3443b1b82d7885cd170f4ebd07e72ae0339fc045 100644
--- a/include/people_simulation_alg_node.h
+++ b/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
diff --git a/launch/people_sim.launch b/launch/people_sim.launch
index 86dca540679c3324e929949abfce62d5add820c8..080ca27c6aea75273334f134269f5c42ca088e68 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="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" />
diff --git a/launch/people_simulation.launch b/launch/people_simulation.launch
index 523b302bcc46207bad72c14a44c05aad49398d76..9854cf8e3f172fc6b4a29d51bec0a8de363a1c0e 100644
--- a/launch/people_simulation.launch
+++ b/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>
diff --git a/map/empty_destinations.txt b/map/empty_destinations.txt
new file mode 100644
index 0000000000000000000000000000000000000000..4d07d23d3b4843b88245310076d0cebb0099fad9
--- /dev/null
+++ b/map/empty_destinations.txt
@@ -0,0 +1,5 @@
+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
diff --git a/src/people_simulation_alg_node.cpp b/src/people_simulation_alg_node.cpp
index 36cb24da3361c8a6e3b3a9a6918664209e8bbb3a..106dec207239f81b90536d32d180b7802409981f 100644
--- a/src/people_simulation_alg_node.cpp
+++ b/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();