From 1995633d08555dceac2611b7e6a504aa640988c0 Mon Sep 17 00:00:00 2001 From: Fabian Aichele Date: Thu, 9 Nov 2017 12:21:32 +0100 Subject: [PATCH] Modify the main functions of the Gaden simulator and player to trigger individual iterations of the simulation and playback using a ROS service call, instead of stepping simulation/replay from the main loop directly. --- gaden_demo/demo/gaden_filament_simulator.rviz | 47 ++-- gaden_demo/launch/gaden_environment.launch | 34 +++ gaden_demo/launch/gaden_player_demo.launch | 5 +- gaden_demo/launch/gaden_simulator_demo.launch | 9 +- gaden_filament_simulator/CMakeLists.txt | 3 +- gaden_filament_simulator/CMakeLists.txt.user | 198 +++++++++++++++++ .../filament_simulator/filament_simulator.h | 16 +- gaden_filament_simulator/package.xml | 4 +- .../src/filament_simulator.cpp | 206 ++++++++++++++---- gaden_msgs/CMakeLists.txt | 30 +++ gaden_msgs/package.xml | 29 +++ .../srv/GasPosition.srv | 0 gaden_msgs/srv/SimulationIterationRequest.srv | 6 + .../srv/WindPosition.srv | 0 gaden_player/CMakeLists.txt | 21 +- gaden_player/package.xml | 4 + gaden_player/src/simulation_player.cpp | 136 ++++++++++-- gaden_player/src/simulation_player.h | 15 +- simulated_anemometer/src/fake_anemometer.cpp | 4 +- simulated_anemometer/src/fake_anemometer.h | 2 +- simulated_gas_sensor/src/fake_gas_sensor.cpp | 40 ++-- simulated_gas_sensor/src/fake_gas_sensor.h | 6 +- .../src/fake_gas_sensor_array.cpp | 8 +- .../src/fake_gas_sensor_array.h | 6 +- 24 files changed, 684 insertions(+), 145 deletions(-) create mode 100644 gaden_demo/launch/gaden_environment.launch create mode 100644 gaden_filament_simulator/CMakeLists.txt.user create mode 100644 gaden_msgs/CMakeLists.txt create mode 100644 gaden_msgs/package.xml rename {gaden_player => gaden_msgs}/srv/GasPosition.srv (100%) create mode 100644 gaden_msgs/srv/SimulationIterationRequest.srv rename {gaden_player => gaden_msgs}/srv/WindPosition.srv (100%) diff --git a/gaden_demo/demo/gaden_filament_simulator.rviz b/gaden_demo/demo/gaden_filament_simulator.rviz index ac9fc3a..0ea7ccb 100644 --- a/gaden_demo/demo/gaden_filament_simulator.rviz +++ b/gaden_demo/demo/gaden_filament_simulator.rviz @@ -1,11 +1,21 @@ Panels: - Class: rviz/Displays - Help Height: 81 + Help Height: 96 Name: Displays Property Tree Widget: - Expanded: ~ + Expanded: + - /Status1 + - /Grid1 + - /Map1 + - /Filaments_Dispersion1 + - /Environment_CAD1 + - /Gas_Dispersion1 + - /Gas_Dispersion1/Status1 + - /Gas_Sources1 + - /Gas_Sources1/Namespaces1 + - /Simulated_Sensors1 Splitter Ratio: 0.594443977 - Tree Height: 758 + Tree Height: 749 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -44,7 +54,7 @@ Visualization Manager: Z: 0 Plane: XY Plane Cell Count: 200 - Reference Frame: + Reference Frame: map Value: true - Class: rviz/Axes Enabled: false @@ -57,18 +67,17 @@ Visualization Manager: Class: rviz/Map Color Scheme: map Draw Behind: false - Enabled: true + Enabled: false Name: Map Topic: /map Unreliable: false - Use Timestamp: false - Value: true + Value: false - Class: rviz/Marker Enabled: true Marker Topic: /filament_visualization Name: Filaments_Dispersion Namespaces: - {} + filaments: true Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -84,7 +93,7 @@ Visualization Manager: Marker Topic: /Gas_Distribution Name: Gas_Dispersion Namespaces: - Gas_Dispersion: true + {} Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -100,7 +109,7 @@ Visualization Manager: Marker Topic: /Mox00/Sensor_display Name: Simulated_Sensors Namespaces: - sensor_visualization: true + {} Queue Size: 100 Value: true Enabled: true @@ -126,24 +135,24 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 18.3090649 + Distance: 18.1148739 Enable Stereo Rendering: Stereo Eye Separation: 0.0599999987 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.733855963 - Y: -2.98371601 - Z: -5.97928229e-05 + X: 2.21766186 + Y: 0.438620567 + Z: -5.78854742e-05 Focal Shape Fixed Size: true Focal Shape Size: 0.0500000007 Name: Current View Near Clip Distance: 0.00999999978 - Pitch: 1.18479669 + Pitch: 1.15479672 Target Frame: Value: XYOrbit (rviz) - Yaw: 4.72364616 + Yaw: 5.0386281 Saved: ~ Window Geometry: Displays: @@ -151,7 +160,7 @@ Window Geometry: Height: 1001 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000388fc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004300000388000000e300fffffffb00000014005200470042002d00430061006d006500720061020000006d0000003e000004840000034efb0000001800440065007000740068002d00430061006d0065007200610000000302000000c90000000000000000000000010000016300000388fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004300000388000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000022300fffffffb0000000800540069006d00650100000000000004500000000000000000000005200000038800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001c00000038efc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000400000038e0000010100fffffffb00000014005200470042002d00430061006d006500720061020000006d0000003e000004840000034efb0000001800440065007000740068002d00430061006d0065007200610000000302000000c90000000000000000000000010000016300000388fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004300000388000000c300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000023300fffffffb0000000800540069006d00650100000000000004500000000000000000000004ca0000038e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -161,5 +170,5 @@ Window Geometry: Views: collapsed: true Width: 1680 - X: 1680 - Y: 0 + X: 526 + Y: 45 diff --git a/gaden_demo/launch/gaden_environment.launch b/gaden_demo/launch/gaden_environment.launch new file mode 100644 index 0000000..d75f1bc --- /dev/null +++ b/gaden_demo/launch/gaden_environment.launch @@ -0,0 +1,34 @@ + + + + + + + ### NODE environment (for RVIZ visualization) ### + + + + # Plot source positions + + + + + + + + + + + + + + # Plot CAD models + + + + + + + + + diff --git a/gaden_demo/launch/gaden_player_demo.launch b/gaden_demo/launch/gaden_player_demo.launch index 197861c..669834e 100644 --- a/gaden_demo/launch/gaden_player_demo.launch +++ b/gaden_demo/launch/gaden_player_demo.launch @@ -38,9 +38,10 @@ ### NODE PLAYER ### #------------------- + - ###(Hz) Freq for loading the simulation log_files - ### Number of simulations to load [1-inf] (useful for multiple sources and gases) + ###(Hz) Freq for loading the simulation log_files + ### Number of simulations to load [1-inf] (useful for multiple sources and gases) # Data from the "filament_simulator" pkg # Multiple sources can be set here diff --git a/gaden_demo/launch/gaden_simulator_demo.launch b/gaden_demo/launch/gaden_simulator_demo.launch index 25e458f..f8c562e 100644 --- a/gaden_demo/launch/gaden_simulator_demo.launch +++ b/gaden_demo/launch/gaden_simulator_demo.launch @@ -37,12 +37,13 @@ ### NODE Filament Simulator ### ### [sec] Total time of the gas dispersion simulation + ### [sec] Time increment between snapshots. Set aprox = cell_size/max_wind_speed. ### Num of filaments released each second - ### [ppm] Gas concentration at the center of the 3D gaussian (filament) - ### [cm] Sigma of the filament at t=0-> 3DGaussian shape - ### [cm²/s] Growth ratio of the filament_std - ### [m] Range of the white noise added on each iteration + ### [ppm] Gas concentration at the center of the 3D gaussian (filament) + ### [cm] Sigma of the filament at t=0-> 3DGaussian shape + ### [cm²/s] Growth ratio of the filament_std + ### [m] Range of the white noise added on each iteration ### 0=Ethanol, 1=Methane, 2=Hydrogen, 6=Acetone ### [Kelvins] ### [Atm] diff --git a/gaden_filament_simulator/CMakeLists.txt b/gaden_filament_simulator/CMakeLists.txt index 4cfc018..975db65 100644 --- a/gaden_filament_simulator/CMakeLists.txt +++ b/gaden_filament_simulator/CMakeLists.txt @@ -8,13 +8,14 @@ find_package(catkin REQUIRED COMPONENTS std_msgs nav_msgs pcl_ros + gaden_msgs ) find_package(PCL REQUIRED) find_package(OpenCV REQUIRED) catkin_package( - DEPENDS roscpp visualization_msgs std_msgs nav_msgs + DEPENDS roscpp visualization_msgs std_msgs nav_msgs gaden_msgs ) include_directories(include) diff --git a/gaden_filament_simulator/CMakeLists.txt.user b/gaden_filament_simulator/CMakeLists.txt.user new file mode 100644 index 0000000..3671228 --- /dev/null +++ b/gaden_filament_simulator/CMakeLists.txt.user @@ -0,0 +1,198 @@ + + + + + + EnvironmentId + {61a07ff5-25ae-48eb-bcc6-e8a275e45be9} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + false + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + UTF-8 + false + 4 + false + 80 + true + true + 1 + true + false + 0 + true + true + 0 + 8 + true + 1 + true + true + true + false + + + + ProjectExplorer.Project.PluginSettings + + + + ProjectExplorer.Project.Target.0 + + Desktop + Desktop + {61da7bf3-decd-431f-880f-bd97db4ae447} + 0 + 0 + 0 + + + CMAKE_BUILD_TYPE:STRING= + CMAKE_CXX_COMPILER:FILEPATH=/usr/bin/c++ + + /home/faichele/src/gaden/build + + + + + all + + true + Make + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + + + all + + true + Make + + CMakeProjectManager.MakeStep + + 1 + Bereinigen + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Debug + Debug + CMakeProjectManager.CMakeBuildConfiguration + + 1 + + + 0 + Deployment + + ProjectExplorer.BuildSteps.Deploy + + 1 + Lokales Deployment + + ProjectExplorer.DefaultDeployConfiguration + + 1 + + + false + false + 1000 + + true + + false + false + false + false + true + 0.01 + 10 + true + 1 + 25 + + 1 + true + false + true + valgrind + + 0 + 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12 + 13 + 14 + + 2 + + + + %{buildDir} + Custom Executable + + ProjectExplorer.CustomExecutableRunConfiguration + 3768 + false + true + false + false + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 18 + + + Version + 18 + + diff --git a/gaden_filament_simulator/include/filament_simulator/filament_simulator.h b/gaden_filament_simulator/include/filament_simulator/filament_simulator.h index e9848b7..71188bc 100644 --- a/gaden_filament_simulator/include/filament_simulator/filament_simulator.h +++ b/gaden_filament_simulator/include/filament_simulator/filament_simulator.h @@ -13,6 +13,10 @@ #include #include +#include +#include +#include + // // Type definitions for a easier gaussian random number generation // @@ -86,7 +90,8 @@ class CFilamentSimulator double restuls_time_step; //(sec) Time increment between saving results - + void initAsyncSpinner(); + void shutdownAsyncSpinner(); protected: void loadNodeParameters(); @@ -96,12 +101,21 @@ class CFilamentSimulator int check_pose_with_environment(double pose_x, double pose_y, double pose_z); bool check_environment_for_obstacle(double start_x, double start_y, double start_z, double end_x, double end_y, double end_z); double random_number(double min_val, double max_val); + + bool requestSimulationStep(gaden_msgs::SimulationIterationRequest::Request&, gaden_msgs::SimulationIterationRequest::Response&); + + void rosLoop(); //Subscriptions & Publishers ros::Publisher marker_pub; //For visualization of the filaments! //Vars ros::NodeHandle n; + ros::ServiceServer simulationIterationService; + + boost::shared_ptr asyncSpinner; + boost::shared_ptr workerThread; + std::vector< std::vector< std::vector > > U, V, W, C, Env; std::vector filaments; visualization_msgs::Marker filament_marker; diff --git a/gaden_filament_simulator/package.xml b/gaden_filament_simulator/package.xml index 8263ce1..164c70b 100644 --- a/gaden_filament_simulator/package.xml +++ b/gaden_filament_simulator/package.xml @@ -16,12 +16,12 @@ visualization_msgs std_msgs nav_msgs - + gaden_msgs roscpp visualization_msgs std_msgs nav_msgs - + gaden_msgs diff --git a/gaden_filament_simulator/src/filament_simulator.cpp b/gaden_filament_simulator/src/filament_simulator.cpp index 64f6e58..0b092ee 100644 --- a/gaden_filament_simulator/src/filament_simulator.cpp +++ b/gaden_filament_simulator/src/filament_simulator.cpp @@ -43,11 +43,11 @@ CFilamentSimulator::CFilamentSimulator() //Init variables //----------------- - sim_time = 0.0; //Start at time = 0(sec) - current_wind_snapshot = 0; //Start with wind_iter= 0; - current_simulation_step = 0; //Start with iter= 0; + sim_time = 0.0; //Start at time = 0(sec) + current_wind_snapshot = 0; //Start with wind_iter= 0; + current_simulation_step = 0; //Start with iter= 0; last_saved_step = -1; - wind_notified = false; //To warm the user (only once) that no more wind data is found! + wind_notified = false; //To warm the user (only once) that no more wind data is found! //Init the Simulator initSimulator(); @@ -87,7 +87,7 @@ CFilamentSimulator::CFilamentSimulator() // Specific gravity is the ratio of the density of a substance to the density of a reference substance; equivalently, // it is the ratio of the mass of a substance to the mass of a reference substance for the same given volume. SpecificGravity[0] = 1.0378; //ethanol (heavier than air) - SpecificGravity[1] = 0.5537; //methane (lighter than air) + SpecificGravity[1] = 0.5537; //methane (lighter than air) SpecificGravity[2] = 0.0696; //hydrogen (lighter than air) SpecificGravity[6] = 1.4529; //acetone (heavier than air) @@ -240,6 +240,8 @@ void CFilamentSimulator::initSimulator() //3. Initialize the filaments vector to its max value (to avoid increasing the size at runtime) ROS_INFO("[filament] Initializing Filaments"); filaments.resize(total_number_filaments, CFilament(0.0, 0.0, 0.0, filament_initial_std)); + + simulationIterationService = n.advertiseService("request_simulation_iteration", &CFilamentSimulator::requestSimulationStep, this); } @@ -777,9 +779,11 @@ void CFilamentSimulator::save_state_to_file() //open file //--------- out = fopen( out_filemane.c_str(), "w" ); + ROS_INFO("[filament] Trying to open final output file: %s\n", out_filemane.c_str()); if( out == NULL ) - ROS_ERROR("Error in opening the final output files.\n"); - + { + ROS_ERROR("Error in opening the final output files.\n"); + } //Write header //-------------- fprintf(out, "env_min(m) %.4f %.4f %.4f\n",env_min_x, env_min_y, env_min_z); @@ -833,6 +837,76 @@ void CFilamentSimulator::save_state_to_file() last_saved_step++; } +bool CFilamentSimulator::requestSimulationStep(gaden_msgs::SimulationIterationRequest::Request& req, gaden_msgs::SimulationIterationRequest::Response& resp) +{ + ROS_INFO("Request for next simulation step received.\n"); + + ROS_INFO("[filament] Simulating step %i (sim_time = %.2f)", current_simulation_step, req.time_step); + + //0. Load wind snapshot (if necessary and availabe) + if ( floor(sim_time / windTime_step) != current_wind_snapshot) + read_wind_snapshot(floor(sim_time / windTime_step)); + + //1. Create new filaments close to the source location + // On each iteration num_filaments (See params) are created + add_new_filaments(cell_size); + + //2. Update Gas Concentration field + update_gas_concentration_from_filaments(); + + //3. Publish markers for RVIZ + publish_markers(); + + //4. Update filament locations + update_filaments_location(); + + //5. Save data (if necessary) + if ( (save_results == 1) && (floor(req.time_step / restuls_time_step) != last_saved_step) ) + save_state_to_file(); + + //4. Update Simulation state + sim_time = sim_time + req.time_step; //sec + current_simulation_step++; + + resp.current_simulation_step = current_simulation_step; + + return true; +} + +void CFilamentSimulator::initAsyncSpinner() +{ + asyncSpinner.reset(new ros::AsyncSpinner(2)); + + if (asyncSpinner->canStart()) + asyncSpinner->start(); + + workerThread.reset(new boost::thread(boost::bind(&CFilamentSimulator::rosLoop, this))); +} + +void CFilamentSimulator::shutdownAsyncSpinner() +{ + asyncSpinner->stop(); +} + +void CFilamentSimulator::rosLoop() +{ + //m_connectorThreadActive = true; + //m_runCondition.notify_all(); + + int iteration_counter = 0; + while (ros::ok()) + { + iteration_counter++; + + if (iteration_counter % 10 == 0) + std::cout << "rosLoop iteration = " << iteration_counter << std::endl; + + ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0.01)); + } + + //m_connectorThreadActive = false; +} + //================================ // // M A I N @@ -843,47 +917,91 @@ int main(int argc, char **argv) // Init ROS-NODE ros::init(argc, argv, "new_filament_simulator"); + ros::NodeHandle n; + + std::string single_stepping; + ros::param::param("~single_stepping", single_stepping, "no"); + //Create simulator obj and initialize it CFilamentSimulator sim; - // Initiate Random Number generator with current time srand(time(NULL)); - //-------------- - // LOOP - //-------------- - ros::Rate r(100); //Go as faster as you can! - while (ros::ok() && (sim.current_simulation_step("request_simulation_iteration"); + gaden_msgs::SimulationIterationRequest srv; + srv.request.time_step = sim.sim_time; + + if (client.call(srv)) + { + ROS_INFO("request_simulation_step answer: substeps = %ld",(long int) srv.response.num_sub_steps); + } + else + { + ROS_ERROR("Failed to call service request_simulation_step"); + } + + r.sleep(); + } + } + + + sim.shutdownAsyncSpinner(); + } } diff --git a/gaden_msgs/CMakeLists.txt b/gaden_msgs/CMakeLists.txt new file mode 100644 index 0000000..cb48e85 --- /dev/null +++ b/gaden_msgs/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 2.8.3) +project(gaden_msgs) + +find_package(catkin REQUIRED COMPONENTS + roscpp + visualization_msgs + std_msgs + nav_msgs + tf + message_generation +) + +add_service_files( + FILES + GasPosition.srv + WindPosition.srv + SimulationIterationRequest.srv +) + +generate_messages( + DEPENDENCIES + std_msgs +) + +catkin_package( + DEPENDS roscpp visualization_msgs std_msgs nav_msgs tf +) + +include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) + diff --git a/gaden_msgs/package.xml b/gaden_msgs/package.xml new file mode 100644 index 0000000..e040f6f --- /dev/null +++ b/gaden_msgs/package.xml @@ -0,0 +1,29 @@ + + gaden_msgs + 1.0.0 + ROS message and service definitions for Gaden. + Fabian Aichele + + GPLv3 + Fabian Aichele + + + catkin + + + roscpp + visualization_msgs + std_msgs + nav_msgs + tf + message_generation + + + roscpp + visualization_msgs + std_msgs + tf + nav_msgs + message_runtime + + diff --git a/gaden_player/srv/GasPosition.srv b/gaden_msgs/srv/GasPosition.srv similarity index 100% rename from gaden_player/srv/GasPosition.srv rename to gaden_msgs/srv/GasPosition.srv diff --git a/gaden_msgs/srv/SimulationIterationRequest.srv b/gaden_msgs/srv/SimulationIterationRequest.srv new file mode 100644 index 0000000..e7e12e5 --- /dev/null +++ b/gaden_msgs/srv/SimulationIterationRequest.srv @@ -0,0 +1,6 @@ +float64 time_step +int64 iteration_counter +--- +int64 num_sub_steps +int64 current_simulation_step +float64 iteration_duration diff --git a/gaden_player/srv/WindPosition.srv b/gaden_msgs/srv/WindPosition.srv similarity index 100% rename from gaden_player/srv/WindPosition.srv rename to gaden_msgs/srv/WindPosition.srv diff --git a/gaden_player/CMakeLists.txt b/gaden_player/CMakeLists.txt index 7a39666..992f91f 100644 --- a/gaden_player/CMakeLists.txt +++ b/gaden_player/CMakeLists.txt @@ -8,29 +8,18 @@ find_package(catkin REQUIRED COMPONENTS nav_msgs tf message_generation + gaden_msgs + olfaction_msgs ) -add_service_files( - FILES - GasPosition.srv - WindPosition.srv -) - -generate_messages( - DEPENDENCIES - std_msgs -) - - -FILE(GLOB_RECURSE MYFILES_CPP "src/*.cpp") catkin_package( - DEPENDS roscpp visualization_msgs std_msgs nav_msgs tf + DEPENDS roscpp visualization_msgs std_msgs nav_msgs tf gaden_msgs olfaction_msgs ) include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +set(MYFILES_CPP src/simulation_player.cpp) -add_executable(gaden_player ${MYFILES_CPP}) -#add_dependencies(gaden_player msgs_and_srvs_generate_messages_cpp) +add_executable(gaden_player ${MYFILES_CPP}) target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) diff --git a/gaden_player/package.xml b/gaden_player/package.xml index 76c3f87..71dd28b 100644 --- a/gaden_player/package.xml +++ b/gaden_player/package.xml @@ -17,6 +17,8 @@ nav_msgs tf message_generation + gaden_msgs + olfaction_msgs roscpp @@ -25,5 +27,7 @@ tf nav_msgs message_runtime + gaden_msgs + olfaction_msgs diff --git a/gaden_player/src/simulation_player.cpp b/gaden_player/src/simulation_player.cpp index 54e0a45..4166d4e 100644 --- a/gaden_player/src/simulation_player.cpp +++ b/gaden_player/src/simulation_player.cpp @@ -8,7 +8,7 @@ #include "simulation_player.h" //--------------- SERVICES CALLBACKS----------------------// -bool get_gas_value_srv(gaden_player::GasPosition::Request &req, gaden_player::GasPosition::Response &res) +bool get_gas_value_srv(gaden_msgs::GasPosition::Request &req, gaden_msgs::GasPosition::Response &res) { //ROS_INFO("[Player] Request for gas concentration at location [%.2f, %.2f, %.2f]m",req.x, req.y, req.z); @@ -40,26 +40,63 @@ bool get_gas_value_srv(gaden_player::GasPosition::Request &req, gaden_player::G } -bool get_wind_value_srv(gaden_player::WindPosition::Request &req, gaden_player::WindPosition::Response &res) +bool get_wind_value_srv(gaden_msgs::WindPosition::Request &req, gaden_msgs::WindPosition::Response &res) { //Since the wind fields are identical among different instances, return just the information from instance[0] player_instances[0].get_wind_value(req.x, req.y, req.z, res.u, res.v, res.w); return true; } +bool requestSimulationStep(gaden_msgs::SimulationIterationRequest::Request& req, gaden_msgs::SimulationIterationRequest::Response& resp) +{ + ROS_INFO("Single player iteration requested.\n"); + + + //Read Gas and Wind data from log_files + load_all_data_from_logfiles(req.iteration_counter); //On the first time, we configure gas type, source pos, etc. + display_current_gas_distribution(); //Rviz visualization + + time_last_loaded_file = ros::Time::now(); + + resp.num_sub_steps = 5; + resp.current_simulation_step = req.iteration_counter + 1; + resp.iteration_duration = 20.0; + return true; +} +void rosLoop() +{ + //m_connectorThreadActive = true; + //m_runCondition.notify_all(); + int iteration_counter = 0; + while (ros::ok()) + { + iteration_counter++; + + if (iteration_counter % 10 == 0) + std::cout << "rosLoop iteration = " << iteration_counter << std::endl; + + //std::cout << "Before callAvailable" << std::endl; + ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0.01)); + //std::cout << "After callAvailable" << std::endl; + } + + //m_connectorThreadActive = false; +} //------------------------ MAIN --------------------------// int main( int argc, char** argv ) { + boost::shared_ptr asyncSpinner; + ros::init(argc, argv, "simulation_player"); - ros::NodeHandle n; + ros::NodeHandle n; ros::NodeHandle pn("~"); //Read Node Parameters - loadNodeParameters(pn); - + loadNodeParameters(pn); + //Publishers marker_pub = n.advertise("Gas_Distribution", 1); @@ -69,7 +106,9 @@ int main( int argc, char** argv ) //Init variables init_all_simulation_instances(); - ros::Time time_last_loaded_file = ros::Time::now(); + + time_last_loaded_file = ros::Time::now(); + srand(time(NULL));// initialize random seed //Init Markers for RVIZ visualization @@ -84,28 +123,83 @@ int main( int argc, char** argv ) mkr_gas_points.scale.z = 0.025; mkr_gas_points.pose.orientation.w = 1.0; + std::string single_stepping; + ros::param::param("~single_stepping", single_stepping, "no"); - // Loop - ros::Rate r(100); //Set max rate at 100Hz (for handling services - Top Speed!!) + ROS_INFO("Single stepping requested: %s\n", single_stepping.c_str()); + int iteration_counter = initial_iteration; - while (ros::ok()) - { - if( (ros::Time::now() - time_last_loaded_file).toSec() >= 1/player_freq ) + + if (single_stepping.compare("no") == 0) + { + // Loop + ros::Rate r(100); //Set max rate at 100Hz (for handling services - Top Speed!!) + while (ros::ok()) + { + if( (ros::Time::now() - time_last_loaded_file).toSec() >= 1/player_freq ) + { + if (verbose) + ROS_INFO("[Player] Playing simulation iteration %i", iteration_counter); + //Read Gas and Wind data from log_files + load_all_data_from_logfiles(iteration_counter); //On the first time, we configure gas type, source pos, etc. + display_current_gas_distribution(); //Rviz visualization + iteration_counter++; + time_last_loaded_file = ros::Time::now(); + } + + //Attend service request at max rate! + //This allows sensors to have higher sampling rates than the simulation update + ros::spinOnce(); + r.sleep(); + } + } + else + { + ROS_INFO("Stepping player on request only.\n"); + ros::ServiceServer simulationIterationService = n.advertiseService("request_simulation_step", requestSimulationStep); + + asyncSpinner.reset(new ros::AsyncSpinner(2)); + + if (asyncSpinner->canStart()) + asyncSpinner->start(); + + workerThread.reset(new boost::thread(boost::bind(rosLoop))); + + ros::Rate r(5); + while (ros::ok()) { - if (verbose) - ROS_INFO("[Player] Playing simulation iteration %i", iteration_counter); - //Read Gas and Wind data from log_files - load_all_data_from_logfiles(iteration_counter); //On the first time, we configure gas type, source pos, etc. - display_current_gas_distribution(); //Rviz visualization + ROS_INFO("Idle loop in gaden_player\n"); iteration_counter++; - time_last_loaded_file = ros::Time::now(); + + if( (ros::Time::now() - time_last_loaded_file).toSec() >= 1/player_freq ) + { + // if (verbose) + ROS_INFO("[Player in ROS service mode] Playing simulation iteration %i", iteration_counter); + ros::ServiceClient client = n.serviceClient("request_simulation_step"); + gaden_msgs::SimulationIterationRequest srv; + srv.request.time_step = 50.0; + + if (client.call(srv)) + { + ROS_INFO("request_simulation_step answer: substeps = %ld",(long int) srv.response.num_sub_steps); + } + else + { + ROS_ERROR("Failed to call service request_simulation_step"); + } + } + + r.sleep(); } - //Attend service request at max rate! - //This allows sensors to have higher sampling rates than the simulation update - ros::spinOnce(); - r.sleep(); + std::cout << "Before stopping asyncSpinner" << std::endl; + if (asyncSpinner) + asyncSpinner->stop(); + + std::cout << "After stopping asyncSpinner" << std::endl; } + + return 0; } diff --git a/gaden_player/src/simulation_player.h b/gaden_player/src/simulation_player.h index 4af9b6d..bc3c33f 100644 --- a/gaden_player/src/simulation_player.h +++ b/gaden_player/src/simulation_player.h @@ -6,8 +6,9 @@ #include #include -#include -#include +#include +#include +#include #include #include @@ -18,6 +19,9 @@ #include #include +#include +#include + // CLASS for every simulation to run. If two gas sources are needed, just create 2 instances! class sim_obj { @@ -65,6 +69,13 @@ std::vector srv_response_gas_types; std::vector srv_response_gas_concs; int initial_iteration; +ros::Time time_last_loaded_file; + +bool requestSimulationStep(gaden_msgs::SimulationIterationRequest::Request&, gaden_msgs::SimulationIterationRequest::Response&); + +boost::shared_ptr workerThread; +void rosLoop(); + //Visualization ros::Publisher marker_pub; visualization_msgs::Marker mkr_gas_points; //We will create an array of particles according to cell concentration diff --git a/simulated_anemometer/src/fake_anemometer.cpp b/simulated_anemometer/src/fake_anemometer.cpp index f9740c8..cc92e6d 100644 --- a/simulated_anemometer/src/fake_anemometer.cpp +++ b/simulated_anemometer/src/fake_anemometer.cpp @@ -33,7 +33,7 @@ int main( int argc, char** argv ) //Service to request wind values to simulator - ros::ServiceClient client = n.serviceClient("/wind_value"); + ros::ServiceClient client = n.serviceClient("/wind_value"); tf::TransformListener tf_; @@ -117,7 +117,7 @@ int main( int argc, char** argv ) // Get Wind vectors (u,v,w) at current position // Service request to the simulator - gaden_player::WindPosition srv; + gaden_msgs::WindPosition srv; srv.request.x = x_pos; srv.request.y = y_pos; srv.request.z = z_pos; diff --git a/simulated_anemometer/src/fake_anemometer.h b/simulated_anemometer/src/fake_anemometer.h index b0190d4..0227879 100644 --- a/simulated_anemometer/src/fake_anemometer.h +++ b/simulated_anemometer/src/fake_anemometer.h @@ -6,7 +6,7 @@ //#include #include #include -#include +#include #include #include diff --git a/simulated_gas_sensor/src/fake_gas_sensor.cpp b/simulated_gas_sensor/src/fake_gas_sensor.cpp index 7791731..7075355 100644 --- a/simulated_gas_sensor/src/fake_gas_sensor.cpp +++ b/simulated_gas_sensor/src/fake_gas_sensor.cpp @@ -22,44 +22,44 @@ int main( int argc, char** argv ) //Publishers ros::Publisher sensor_read_pub = n.advertise("Sensor_reading", 500); - ros::Publisher marker_pub = n.advertise("Sensor_display", 100); + ros::Publisher marker_pub = n.advertise("Sensor_display", 100); //Service to request gas concentration - ros::ServiceClient client = n.serviceClient("/odor_value"); + ros::ServiceClient client = n.serviceClient("/odor_value"); //Init Visualization data (marker) //--------------------------------- // sensor = sphere // conector = stick from the floor to the sensor - visualization_msgs::Marker sensor,connector; - sensor.header.frame_id = input_fixed_frame.c_str(); - sensor.ns = "sensor_visualization"; + visualization_msgs::Marker sensor,connector; + sensor.header.frame_id = input_fixed_frame.c_str(); + sensor.ns = "sensor_visualization"; sensor.action = visualization_msgs::Marker::ADD; - sensor.type = visualization_msgs::Marker::SPHERE; + sensor.type = visualization_msgs::Marker::SPHERE; sensor.id = 0; sensor.scale.x = 0.1; sensor.scale.y = 0.1; sensor.scale.z = 0.1; sensor.color.r = 2.0f; - sensor.color.g = 1.0f; + sensor.color.g = 1.0f; sensor.color.a = 1.0; - connector.header.frame_id = input_fixed_frame.c_str(); - connector.ns = "sensor_visualization"; - connector.action = visualization_msgs::Marker::ADD; + connector.header.frame_id = input_fixed_frame.c_str(); + connector.ns = "sensor_visualization"; + connector.action = visualization_msgs::Marker::ADD; connector.type = visualization_msgs::Marker::CYLINDER; connector.id = 1; - connector.scale.x = 0.1; - connector.scale.y = 0.1; - connector.color.a = 1.0; - connector.color.r = 1.0f; - connector.color.b = 1.0f; - connector.color.g = 1.0f; + connector.scale.x = 0.1; + connector.scale.y = 0.1; + connector.color.a = 1.0; + connector.color.r = 1.0f; + connector.color.b = 1.0f; + connector.color.g = 1.0f; // Loop - tf::TransformListener listener; + tf::TransformListener listener; node_rate = 5; //Hz ros::Rate r(node_rate); first_reading = true; @@ -92,7 +92,7 @@ int main( int argc, char** argv ) // Get Gas concentration at current position (of each gas present) // Service request to the simulator - gaden_player::GasPosition srv; + gaden_msgs::GasPosition srv; srv.request.x = x_pos; srv.request.y = y_pos; srv.request.z = z_pos; @@ -216,7 +216,7 @@ int main( int argc, char** argv ) // Simulate MOX response: Sensitivity + Dynamic response // RS = R0*( A * conc^B ) // This method employes a curve fitting based on a line in the loglog scale to set the sensitivity -float simulate_mox_as_line_loglog(gaden_player::GasPositionResponse GT_gas_concentrations) +float simulate_mox_as_line_loglog(gaden_msgs::GasPositionResponse GT_gas_concentrations) { if (first_reading) { @@ -314,7 +314,7 @@ float simulate_mox_as_line_loglog(gaden_player::GasPositionResponse GT_gas_conce // Simulate PID response : Weighted Sum of all gases -float simulate_pid(gaden_player::GasPositionResponse GT_gas_concentrations) +float simulate_pid(gaden_msgs::GasPositionResponse GT_gas_concentrations) { //Handle multiple gases float accumulated_conc = 0.0; diff --git a/simulated_gas_sensor/src/fake_gas_sensor.h b/simulated_gas_sensor/src/fake_gas_sensor.h index a1179bf..641a665 100644 --- a/simulated_gas_sensor/src/fake_gas_sensor.h +++ b/simulated_gas_sensor/src/fake_gas_sensor.h @@ -5,7 +5,7 @@ #include #include #include -#include +#include #include #include @@ -62,8 +62,8 @@ int ch_id; //Chemical ID //functions: void loadNodeParameters(ros::NodeHandle private_nh); -float simulate_mox_as_line_loglog(gaden_player::GasPositionResponse GT_gas_concentrations); -float simulate_pid(gaden_player::GasPositionResponse GT_gas_concentrations); +float simulate_mox_as_line_loglog(gaden_msgs::GasPositionResponse GT_gas_concentrations); +float simulate_pid(gaden_msgs::GasPositionResponse GT_gas_concentrations); diff --git a/simulated_gas_sensor_array/src/fake_gas_sensor_array.cpp b/simulated_gas_sensor_array/src/fake_gas_sensor_array.cpp index 5070845..03e612a 100644 --- a/simulated_gas_sensor_array/src/fake_gas_sensor_array.cpp +++ b/simulated_gas_sensor_array/src/fake_gas_sensor_array.cpp @@ -24,7 +24,7 @@ int main( int argc, char** argv ) ros::Publisher enose_pub = n.advertise(topic_id, 500); //Service to request gas concentration - ros::ServiceClient client = n.serviceClient("/odor_value"); + ros::ServiceClient client = n.serviceClient("/odor_value"); //Configure sensor_array @@ -66,7 +66,7 @@ int main( int argc, char** argv ) // Get Gas concentration at current position (for each gas present) // Service request to the simulator - gaden_player::GasPosition srv; + gaden_msgs::GasPosition srv; srv.request.x = x_pos; srv.request.y = y_pos; srv.request.z = z_pos; @@ -187,7 +187,7 @@ int main( int argc, char** argv ) // Simulate MOX response: Sensitivity + Dynamic response // RS = R0*( A * conc^B ) // This method employes a curve fitting based on a line in the loglog scale to set the sensitivity -float simulate_mox_as_line_loglog(gaden_player::GasPositionResponse GT_gas_concentrations, int s_idx) +float simulate_mox_as_line_loglog(gaden_msgs::GasPositionResponse GT_gas_concentrations, int s_idx) { if (sensor_array[s_idx].first_reading) { @@ -285,7 +285,7 @@ float simulate_mox_as_line_loglog(gaden_player::GasPositionResponse GT_gas_conce // Simulate PID response : Weighted Sum of all gases -float simulate_pid(gaden_player::GasPositionResponse GT_gas_concentrations) +float simulate_pid(gaden_msgs::GasPositionResponse GT_gas_concentrations) { //Handle multiple gases float accumulated_conc = 0.0; diff --git a/simulated_gas_sensor_array/src/fake_gas_sensor_array.h b/simulated_gas_sensor_array/src/fake_gas_sensor_array.h index fbec93f..2ccfdf6 100644 --- a/simulated_gas_sensor_array/src/fake_gas_sensor_array.h +++ b/simulated_gas_sensor_array/src/fake_gas_sensor_array.h @@ -5,7 +5,7 @@ #include #include #include -#include +#include #include #include @@ -61,8 +61,8 @@ bool notified; //to notifiy about erros just once //functions: void loadNodeParameters(ros::NodeHandle private_nh); -float simulate_mox_as_line_loglog(gaden_player::GasPositionResponse GT_gas_concentrations, int s_idx); -float simulate_pid(gaden_player::GasPositionResponse GT_gas_concentrations); +float simulate_mox_as_line_loglog(gaden_msgs::GasPositionResponse GT_gas_concentrations, int s_idx); +float simulate_pid(gaden_msgs::GasPositionResponse GT_gas_concentrations);