diff --git a/cfg/FakeLaserGen.cfg b/cfg/FakeLaserGen.cfg
index c99496b3bfd10142f79db6fe5fe13542ddead930..a73f51c9fa9cba048f6e4adef4e05384c2754a02 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 c6b5b8f311ea1ed4133bc3fe74c4e3d418e59a16..6e8d1a5df8ce133027cca795cac4dd4dde21d899 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 98d62fafeb3ee0723b2e3f8c7697bc2d2c51fe54..4b46292c0ff03bb5efb5f7b5bed9d4189397147b 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 b43f16a40921f165e9c16fb0111b5e8690b54aaf..10b3fd1f63d3e96e8332303fbf863f113d1fcfdb 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");