Commit 9a13889e authored by Ely Repiso Polo's avatar Ely Repiso Polo
Browse files

ok, simulation iterative and get performances with executable

parent c783d779
......@@ -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"))
......@@ -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]
......
......@@ -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>
......
......@@ -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");
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment