diff --git a/include/fake_laser_gen_alg_node.h b/include/fake_laser_gen_alg_node.h
index f0a94e159d93e1917cd086223cb324705ad0ee2f..ee6bb76b228ed35b7945a92066f791ee94e132c0 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,corrido_obstacles_companion_2_paths,corrido_obstacles_companion_3_paths,corridor_obstacles_which_narrows_companion_1_paths, n_obstacles3, n_obstacles4, scene_3, scene_4, n_obstacles5, n_obstacles6};
+ enum laser_mode{Zero=0, one_obstacle, two_obstacles, n_obstacles, n_obstacles_2, scene, scene_2,corrido_obstacles_companion_2_paths,corrido_obstacles_companion_3_paths,corridor_obstacles_which_narrows_companion_1_paths, n_obstacles3, n_obstacles4, scene_3, scene_4, n_obstacles5, n_obstacles6, n_obstacles7, n_obstacles8};
private:
// [publisher attributes]
diff --git a/launch/fake_localization.launch b/launch/fake_localization.launch
index de959c577d0f8f5093f5db2d44a27b6483f2abcd..42e68ed772766a6e768b5aabab3cf3a20d062ef8 100644
--- a/launch/fake_localization.launch
+++ b/launch/fake_localization.launch
@@ -12,8 +12,8 @@
<param name="base_frame_id" value="/$(optenv ROBOT tibi)/base_footprint" />
<!-- parameters to change the robot initial position, if want the robot in -1 and -8=> delta_x=1.0 ; delta_y=8.0 -->
<param name="delta_x" value="1.0" /> <!-- Normal position (estrechez mitad camino) = delta_x=1.0 ; delta_y=2.0 ; delta_yaw=0.0 -->
- <param name="delta_y" value="1.5" /> <!-- GIRO ESQUINA position (estrechez mitad camino)= delta_x=1.0 ; delta_y=-18.0 ; delta_yaw=1.57 -->
- <param name="delta_yaw" value="0.0" />
+ <param name="delta_y" value="1.5" /> <!-- GIRO ESQUINA position (estrechez mitad camino)= delta_x=1.0 ; delta_y=-15.0 ; delta_yaw=1.57 -->
+ <param name="delta_yaw" value="0.0" /> <!-- Normal position2_act_ultima (estrechez mitad camino) = delta_x=1.0 ; delta_y=1.5 ; delta_yaw=0.0 -->
<remap from="/odom" to="/$(optenv ROBOT tibi)/segway/odom"/>
<remap from="base_pose_ground_truth" to="/$(optenv ROBOT tibi)/segway/odom"/>
</node>
diff --git a/launch/fake_localization2.launch b/launch/fake_localization2.launch
index cec0947f8155e012361559963842709bb8959be9..f641e879e147e0f1787912a44c6a0c2b77a72d37 100644
--- a/launch/fake_localization2.launch
+++ b/launch/fake_localization2.launch
@@ -12,8 +12,9 @@
<param name="base_frame_id" value="/$(optenv ROBOT dabo)/base_footprint" />
<!-- parameters to change the robot initial position, if want the robot in -1 and -8=> delta_x=1.0 ; delta_y=8.0 -->
<param name="delta_x" value="1.0" /> <!-- Normal position (estrechez mitad camino) = delta_x=1.0 ; delta_y=1.0 ; delta_yaw=0.0 -->
- <param name="delta_y" value="0.5" /> <!-- GIRO ESQUINA position (estrechez mitad camino)= delta_x=1.0 ; delta_y=-18.0 ; delta_yaw=1.57 -->
+ <param name="delta_y" value="0.5" /> <!-- GIRO ESQUINA position (estrechez mitad camino)= delta_x=0.0 ; delta_y=-15.0 ; delta_yaw=1.57 -->
<param name="delta_yaw" value="0.0" /> <!-- GIRO ESQUINA robot como persona => delta_x=0.0 ; delta_y=-15.0 ; delta_yaw=1.57 -->
+<!-- Normal position (estrechez mitad camino) = delta_x=1.0 ; delta_y=0.5 ; delta_yaw=0.0 -->
<remap from="/odom" to="/$(optenv ROBOT dabo)/segway/odom"/>
<remap from="base_pose_ground_truth" to="/$(optenv ROBOT dabo)/segway/odom"/>
</node>
diff --git a/src/fake_laser_gen_alg_node.cpp b/src/fake_laser_gen_alg_node.cpp
index 9978d9e72746568ede582e8c4bf0b7adaf18bf06..d1b27d44ae426850c1c0333f6205ac6f53fae3be 100644
--- a/src/fake_laser_gen_alg_node.cpp
+++ b/src/fake_laser_gen_alg_node.cpp
@@ -88,7 +88,9 @@ void FakeLaserGenAlgNode::node_config_update(Config &config, uint32_t level)
LaserScan_msg_.ranges.clear();
LaserScan_msg_.ranges.resize(1436,59.9);
//laser_mode_=FakeLaserGenAlgNode::n_obstacles5; //caso estrechez en medio del camino. Muy estrecho!
- // laser_mode_=FakeLaserGenAlgNode::n_obstacles3; //caso estrechez en medio del camino!
+ //laser_mode_=FakeLaserGenAlgNode::n_obstacles3; //caso estrechez en medio del camino! obstaculos a 2.2305 m entre ellos va con empty_map_whit_obstacles1
+ //laser_mode_=FakeLaserGenAlgNode::n_obstacles7; //caso estrechez en medio del camino! caso obstaculos a 1.94 m entre ellos. va con empty_map_whit_obstacles2
+ laser_mode_=FakeLaserGenAlgNode::n_obstacles8; //caso estrechez en medio del camino! caso obstaculos a 1.4965 m entre ellos. va con empty_map_whit_obstacles2
//laser_mode_=FakeLaserGenAlgNode::n_obstacles6; //caso estrechez en medio del camino!
//laser_mode_=FakeLaserGenAlgNode::scene_3; // caso pasillo, esquina. (Ancho)
//laser_mode_=FakeLaserGenAlgNode::scene_2; // Estrecho! caso pasillo, esquina.
@@ -282,7 +284,7 @@ void FakeLaserGenAlgNode::node_config_update(Config &config, uint32_t level)
}
break;
- case FakeLaserGenAlgNode::n_obstacles3 :
+ case FakeLaserGenAlgNode::n_obstacles3 : // caso obstaculos a 2.2305 m entre ellos. va con empty_map_whit_obstacles1
{
ROS_INFO("n obstacles 3");
/* LaserScan_msg_.ranges[100]= 4.0;
@@ -309,17 +311,93 @@ void FakeLaserGenAlgNode::node_config_update(Config &config, uint32_t level)
LaserScan_msg_.ranges[680-15]= 13.15;
LaserScan_msg_.ranges[680-10]= 13.15;
LaserScan_msg_.ranges[680-5]= 13.0;
- LaserScan_msg_.ranges[730+5]= 13.0;
- LaserScan_msg_.ranges[730+10]= 13.15;
- LaserScan_msg_.ranges[730+15]= 13.15;
+ LaserScan_msg_.ranges[730-5]= 13.0;
+ LaserScan_msg_.ranges[730]= 13.15;
+ LaserScan_msg_.ranges[730+5]= 13.15;
+ LaserScan_msg_.ranges[730+10]= 13.15;
+ LaserScan_msg_.ranges[730+15]= 13.15;
LaserScan_msg_.ranges[730+20]= 13.15;
- LaserScan_msg_.ranges[730+25]= 13.15;
- LaserScan_msg_.ranges[730+30]= 13.15;
LaserScan_msg_.ranges[1300+18]= 7.0;
}
break;
+ case FakeLaserGenAlgNode::n_obstacles7 : // caso obstaculos a 1.94 m entre ellos. va con empty_map_whit_obstacles2
+ {
+ 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[100]= 7.0;
+ LaserScan_msg_.ranges[100+40]= 7.0;
+ LaserScan_msg_.ranges[340]= 7.0;
+ //LaserScan_msg_.ranges[340+40]= 7.0;
+ // LaserScan_msg_.ranges[660]= 13.25;
+ LaserScan_msg_.ranges[680-20]= 13.0;
+ LaserScan_msg_.ranges[680-15]= 13.25;
+ LaserScan_msg_.ranges[680-10]= 13.25;
+ LaserScan_msg_.ranges[680-5]= 13.15;
+ LaserScan_msg_.ranges[680]= 13.15;
+ LaserScan_msg_.ranges[680+5]= 13.0;
+ LaserScan_msg_.ranges[730-5]= 13.0;
+ LaserScan_msg_.ranges[730]= 13.15;
+ LaserScan_msg_.ranges[730+5]= 13.15;
+ LaserScan_msg_.ranges[730+10]= 13.15;
+ LaserScan_msg_.ranges[730+15]= 13.15;
+ LaserScan_msg_.ranges[730+20]= 13.15;
+ LaserScan_msg_.ranges[1300+18]= 7.0;
+ }
+ break;
+
+ case FakeLaserGenAlgNode::n_obstacles8 : // caso obstaculos a 1.4965 m entre ellos. va con empty_map_whit_obstacles2
+ {
+ 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[100]= 7.0;
+ LaserScan_msg_.ranges[100+40]= 7.0;
+ LaserScan_msg_.ranges[340]= 7.0;
+ //LaserScan_msg_.ranges[340+40]= 7.0;
+ // LaserScan_msg_.ranges[660]= 13.25;
+ LaserScan_msg_.ranges[680-15]= 13.0;
+ LaserScan_msg_.ranges[680-10]= 13.25;
+ LaserScan_msg_.ranges[680-5]= 13.25;
+ LaserScan_msg_.ranges[680]= 13.15;
+ LaserScan_msg_.ranges[680+8]= 13.15; // 6.5 pasan...
+ // LaserScan_msg_.ranges[680+5]= 13.0;
+ LaserScan_msg_.ranges[730-3.5]= 13.15; //4.5 pasan...
+ LaserScan_msg_.ranges[730]= 13.15;
+ LaserScan_msg_.ranges[730+5]= 13.15;
+ LaserScan_msg_.ranges[730+10]= 13.15;
+ LaserScan_msg_.ranges[730+15]= 13.15;
+ LaserScan_msg_.ranges[730+20]= 13.15;
+ LaserScan_msg_.ranges[1300+18]= 7.0;
+ }
+ break;
+
+
+
case FakeLaserGenAlgNode::n_obstacles6 :
{
ROS_INFO("n obstacles 6");