From 9a13889e451d10873b150fad73561f3c99f09d43 Mon Sep 17 00:00:00 2001
From: Ely Repiso Polo <erepiso@iri.upc.edu>
Date: Thu, 2 May 2019 14:46:54 +0000
Subject: [PATCH] ok, simulation iterative and get performances with executable
---
cfg/FakeLaserGen.cfg | 2 +-
include/fake_laser_gen_alg_node.h | 2 +-
launch/fake_laser_gen.launch | 2 +-
src/fake_laser_gen_alg_node.cpp | 158 ++++++++++++++++++++++++++++++
4 files changed, 161 insertions(+), 3 deletions(-)
diff --git a/cfg/FakeLaserGen.cfg b/cfg/FakeLaserGen.cfg
index c99496b..a73f51c 100755
--- a/cfg/FakeLaserGen.cfg
+++ b/cfg/FakeLaserGen.cfg
@@ -39,6 +39,6 @@ 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("laser_mode", int_t, 0, "#0-circle, #1-Single obstacle #2-Two obstacles, #3-multiple obstacles, #4-multiple obstacles2, #5-scene, #6-scene_2", 0, 0, 6)
+gen.add("laser_mode", int_t, 0, "#0-circle, #1-Single obstacle #2-Two obstacles, #3-multiple obstacles, #4-multiple obstacles2, #5-scene, #6-scene_2 #7-multiple obstacles3", 0, 0, 10)
exit(gen.generate(PACKAGE, "FakeLaserGenAlgorithm", "FakeLaserGen"))
diff --git a/include/fake_laser_gen_alg_node.h b/include/fake_laser_gen_alg_node.h
index c6b5b8f..6e8d1a5 100644
--- a/include/fake_laser_gen_alg_node.h
+++ b/include/fake_laser_gen_alg_node.h
@@ -41,7 +41,7 @@
*/
class FakeLaserGenAlgNode : public algorithm_base::IriBaseAlgorithm<FakeLaserGenAlgorithm>
{
- enum laser_mode{Zero=0, one_obstacle, two_obstacles, n_obstacles, n_obstacles_2, scene, scene_2};
+ enum laser_mode{Zero=0, one_obstacle, two_obstacles, n_obstacles, n_obstacles_2, scene, scene_2, n_obstacles_3, n_obstacles_4, n_obstacles_5};
private:
// [publisher attributes]
diff --git a/launch/fake_laser_gen.launch b/launch/fake_laser_gen.launch
index 98d62fa..4b46292 100644
--- a/launch/fake_laser_gen.launch
+++ b/launch/fake_laser_gen.launch
@@ -5,7 +5,7 @@
type="fake_laser_gen"
name="$(optenv ROBOT tibi)_fake_laser_gen"
output="screen">
- <param name="laser_mode" value="0" />
+ <param name="laser_mode" value="9" />
<remap from="~scan" to="/$(optenv ROBOT tibi)/sensors/front_laser_scan"/>
</node>
diff --git a/src/fake_laser_gen_alg_node.cpp b/src/fake_laser_gen_alg_node.cpp
index b43f16a..10b3fd1 100644
--- a/src/fake_laser_gen_alg_node.cpp
+++ b/src/fake_laser_gen_alg_node.cpp
@@ -87,6 +87,9 @@ void FakeLaserGenAlgNode::node_config_update(Config &config, uint32_t level)
//plot fixed map
LaserScan_msg_.ranges.clear();
LaserScan_msg_.ranges.resize(1436,59.9);
+
+// NOTE TO MARTA: the good scenarios are: n obstacles 3 (num 7); n obstacles 4 (num 8); n obstacles 5 (num 9); (Also if you want, you can include more scerarios, but save these ones.
+
switch(laser_mode_)
{
case FakeLaserGenAlgNode::one_obstacle :
@@ -166,6 +169,161 @@ void FakeLaserGenAlgNode::node_config_update(Config &config, uint32_t level)
for (unsigned int n = 1260; n<1400; ++n)
LaserScan_msg_.ranges[n] = -1.0 / sin(LaserScan_msg_.angle_increment * (double) n);
break;
+
+ case FakeLaserGenAlgNode::n_obstacles_3 : // 7!?
+ ROS_INFO("n obstacles 3");
+ LaserScan_msg_.ranges[100]= 4.0;
+ LaserScan_msg_.ranges[100+40]= 4.0;
+ LaserScan_msg_.ranges[340]= 4.0;
+ LaserScan_msg_.ranges[340+40]= 4.0;
+ LaserScan_msg_.ranges[580]= 4.0;
+ LaserScan_msg_.ranges[580+40]= 4.0;
+ LaserScan_msg_.ranges[820]= 4.0;
+ LaserScan_msg_.ranges[820+40]= 4.0;
+ LaserScan_msg_.ranges[1060]= 4.0;
+ LaserScan_msg_.ranges[1060+40]= 4.0;
+ LaserScan_msg_.ranges[1300]= 4.0;
+ LaserScan_msg_.ranges[1300+40]= 4.0;
+ LaserScan_msg_.ranges[1]= 10.0;
+ LaserScan_msg_.ranges[241]= 10.0;
+ LaserScan_msg_.ranges[481]= 10.0;
+ LaserScan_msg_.ranges[721]= 10.0;
+ LaserScan_msg_.ranges[961]= 10.0;
+ LaserScan_msg_.ranges[1201]= 10.0;
+ LaserScan_msg_.ranges[1+20]= 10.0;
+ LaserScan_msg_.ranges[241+20]= 10.0;
+ LaserScan_msg_.ranges[481+20]= 10.0;
+ LaserScan_msg_.ranges[721+20]= 10.0;
+ LaserScan_msg_.ranges[961+20]= 10.0;
+ LaserScan_msg_.ranges[1201+20]= 10.0;
+ break;
+
+ case FakeLaserGenAlgNode::n_obstacles_4 : // 8!?
+ ROS_INFO("n obstacles 4");
+ LaserScan_msg_.ranges[100]= 4.0;
+ LaserScan_msg_.ranges[100+40]= 4.0;
+ LaserScan_msg_.ranges[340]= 4.0;
+ LaserScan_msg_.ranges[340+40]= 4.0;
+ LaserScan_msg_.ranges[580]= 4.0;
+ LaserScan_msg_.ranges[580+40]= 4.0;
+ LaserScan_msg_.ranges[820]= 4.0;
+ LaserScan_msg_.ranges[820+40]= 4.0;
+ LaserScan_msg_.ranges[1060]= 4.0;
+ LaserScan_msg_.ranges[1060+40]= 4.0;
+ LaserScan_msg_.ranges[1300]= 4.0;
+ LaserScan_msg_.ranges[1300+40]= 4.0;
+
+ LaserScan_msg_.ranges[1]= 10.0;
+ LaserScan_msg_.ranges[1+20]= 10.0;
+ LaserScan_msg_.ranges[121]= 10.0;
+ LaserScan_msg_.ranges[121+20]= 10.0;
+ LaserScan_msg_.ranges[241]= 10.0;
+ LaserScan_msg_.ranges[241+20]= 10.0;
+ LaserScan_msg_.ranges[361]= 10.0;
+ LaserScan_msg_.ranges[361+20]= 10.0;
+ LaserScan_msg_.ranges[481]= 10.0;
+ LaserScan_msg_.ranges[481+20]= 10.0;
+ LaserScan_msg_.ranges[601]= 10.0;
+ LaserScan_msg_.ranges[601+20]= 10.0;
+ LaserScan_msg_.ranges[721]= 10.0;
+ LaserScan_msg_.ranges[721+20]= 10.0;
+ LaserScan_msg_.ranges[841]= 10.0;
+ LaserScan_msg_.ranges[841+20]= 10.0;
+ LaserScan_msg_.ranges[961]= 10.0;
+ LaserScan_msg_.ranges[961+20]= 10.0;
+ LaserScan_msg_.ranges[1081]= 10.0;
+ LaserScan_msg_.ranges[1081+20]= 10.0;
+ LaserScan_msg_.ranges[1201]= 10.0;
+ LaserScan_msg_.ranges[1201+20]= 10.0;
+ break;
+
+ case FakeLaserGenAlgNode::n_obstacles_5 : // 8!?
+ ROS_INFO("n obstacles 5");
+ LaserScan_msg_.ranges[100]= 4.0;
+ LaserScan_msg_.ranges[100+40]= 4.0;
+ LaserScan_msg_.ranges[340]= 4.0;
+ LaserScan_msg_.ranges[340+40]= 4.0;
+ LaserScan_msg_.ranges[580]= 4.0;
+ LaserScan_msg_.ranges[580+40]= 4.0;
+ LaserScan_msg_.ranges[820]= 4.0;
+ LaserScan_msg_.ranges[820+40]= 4.0;
+ LaserScan_msg_.ranges[1060]= 4.0;
+ LaserScan_msg_.ranges[1060+40]= 4.0;
+ LaserScan_msg_.ranges[1300]= 4.0;
+ LaserScan_msg_.ranges[1300+40]= 4.0;
+
+ LaserScan_msg_.ranges[1]= 10.0;
+ LaserScan_msg_.ranges[1+20]= 10.0;
+ LaserScan_msg_.ranges[121]= 10.0;
+ LaserScan_msg_.ranges[121+20]= 10.0;
+ LaserScan_msg_.ranges[241]= 10.0;
+ LaserScan_msg_.ranges[241+20]= 10.0;
+ LaserScan_msg_.ranges[361]= 10.0;
+ LaserScan_msg_.ranges[361+20]= 10.0;
+ LaserScan_msg_.ranges[481]= 10.0;
+ LaserScan_msg_.ranges[481+20]= 10.0;
+ LaserScan_msg_.ranges[601]= 10.0;
+ LaserScan_msg_.ranges[601+20]= 10.0;
+ LaserScan_msg_.ranges[721]= 10.0;
+ LaserScan_msg_.ranges[721+20]= 10.0;
+ LaserScan_msg_.ranges[841]= 10.0;
+ LaserScan_msg_.ranges[841+20]= 10.0;
+ LaserScan_msg_.ranges[961]= 10.0;
+ LaserScan_msg_.ranges[961+20]= 10.0;
+ LaserScan_msg_.ranges[1081]= 10.0;
+ LaserScan_msg_.ranges[1081+20]= 10.0;
+ LaserScan_msg_.ranges[1201]= 10.0;
+ LaserScan_msg_.ranges[1201+20]= 10.0;
+
+
+ LaserScan_msg_.ranges[50]= 15.0;
+ LaserScan_msg_.ranges[50+10]= 15.0;
+ // LaserScan_msg_.ranges[150]= 15.0;
+ // LaserScan_msg_.ranges[150+10]= 15.0;
+ LaserScan_msg_.ranges[200]= 15.0;
+ LaserScan_msg_.ranges[200+10]= 15.0;
+ // LaserScan_msg_.ranges[250]= 15.0;
+ // LaserScan_msg_.ranges[250+10]= 15.0;
+ LaserScan_msg_.ranges[300]= 15.0;
+ LaserScan_msg_.ranges[300+10]= 15.0;
+ // LaserScan_msg_.ranges[350]= 15.0;
+ // LaserScan_msg_.ranges[350+10]= 10.0;
+ LaserScan_msg_.ranges[400]= 15.0;
+ LaserScan_msg_.ranges[400+10]= 15.0;
+ // LaserScan_msg_.ranges[450]= 15.0;
+ // LaserScan_msg_.ranges[450+10]= 15.0;
+ LaserScan_msg_.ranges[500]= 15.0;
+ LaserScan_msg_.ranges[500+10]= 15.0;
+ // LaserScan_msg_.ranges[550]= 15.0;
+ // LaserScan_msg_.ranges[550+10]= 15.0;
+ LaserScan_msg_.ranges[600]= 15.0;
+ LaserScan_msg_.ranges[600+10]= 15.0;
+
+ // LaserScan_msg_.ranges[650]= 15.0;
+ // LaserScan_msg_.ranges[650+10]= 15.0;
+ LaserScan_msg_.ranges[700]= 15.0;
+ LaserScan_msg_.ranges[700+10]= 15.0;
+ // LaserScan_msg_.ranges[750]= 15.0;
+ // LaserScan_msg_.ranges[750+10]= 15.0;
+ LaserScan_msg_.ranges[800]= 15.0;
+ LaserScan_msg_.ranges[800+10]= 15.0;
+ // LaserScan_msg_.ranges[850]= 15.0;
+ // LaserScan_msg_.ranges[850+10]= 15.0;
+ LaserScan_msg_.ranges[900]= 15.0;
+ LaserScan_msg_.ranges[900+10]= 15.0;
+ // LaserScan_msg_.ranges[950]= 15.0;
+ // LaserScan_msg_.ranges[950+10]= 15.0;
+ LaserScan_msg_.ranges[1000]= 15.0;
+ LaserScan_msg_.ranges[1000+10]= 15.0;
+ // LaserScan_msg_.ranges[1050]= 15.0;
+ // LaserScan_msg_.ranges[1050+10]= 15.0;
+ LaserScan_msg_.ranges[1100]= 15.0;
+ LaserScan_msg_.ranges[1100+10]= 15.0;
+ // LaserScan_msg_.ranges[1150]= 15.0;
+ // LaserScan_msg_.ranges[1150+10]= 15.0;
+
+ break;
+
case FakeLaserGenAlgNode::Zero :
default :
ROS_INFO("zero");
--
GitLab