Merge pull request #138 from CPFL/mod_vscan

Mod vscan
autowarefoundation · Nov 19, 2015 · 10758a2 · 10758a2
2 parents f8699d4 + 831ee5f
commit 10758a2
Show file tree

Hide file tree

Showing 2 changed files with 73 additions and 18 deletions.
diff --git a/ros/src/sensing/fusion/packages/points2image/nodes/points2vscan/mainwindow.cpp b/ros/src/sensing/fusion/packages/points2image/nodes/points2vscan/mainwindow.cpp
@@ -1,23 +1,23 @@
 #include "mainwindow.h"
 #include "ui_mainwindow.h"
 
-#define BEAMNUM 1440
-#define STEP 0.1
-#define MINFLOOR -3.0
-#define MAXFLOOR -1.3
-#define MAXCEILING 6.0
-#define MINCEILING -0.5
-#define ROADSLOPMINHEIGHT 80.0
-#define ROADSLOPMAXHEIGHT 30.0
-#define ROTATION 3
-#define OBSTACLEMINHEIGHT 1
-#define MAXBACKDISTANCE 1
-#define PASSHEIGHT 3
-
-#define MAXRANGE 80.0
-#define MINRANGE 3
-#define GRIDSIZE 10.0
-#define IMAGESIZE 1000.0
+static int BEAMNUM;
+static double STEP;
+static double MINFLOOR;
+static double MAXFLOOR;
+static double MAXCEILING;
+static double MINCEILING;
+static double ROADSLOPMINHEIGHT;
+static double ROADSLOPMAXHEIGHT;
+static double ROTATION;
+static double OBSTACLEMINHEIGHT;
+static double MAXBACKDISTANCE;
+static double PASSHEIGHT;
+
+static double MAXRANGE;
+static double MINRANGE;
+static double GRIDSIZE;
+static double IMAGESIZE;
 
 MainWindow::MainWindow(QWidget *parent) :
     QMainWindow(parent)
@@ -33,6 +33,25 @@ MainWindow::MainWindow(QWidget *parent) :
     vsros=new ROSPub<sensor_msgs::PointCloud2>("/vscan_points",1000);
     scanros=new ROSPub<sensor_msgs::LaserScan>("/scan",1000);
 
+    ros::NodeHandle private_nh("~");
+    private_nh.param("BEAMNUM", BEAMNUM, 1440);
+    private_nh.param("STEP", STEP, 0.1);
+    private_nh.param("MINFLOOR", MINFLOOR, -3.0);
+    private_nh.param("MAXFLOOR", MAXFLOOR, -1.3);
+    private_nh.param("MAXCEILING", MAXCEILING, 6.0);
+    private_nh.param("MINCEILING", MINCEILING, -0.5);
+    private_nh.param("ROADSLOPMINHEIGHT", ROADSLOPMINHEIGHT, 80.0);
+    private_nh.param("ROADSLOPMAXHEIGHT", ROADSLOPMAXHEIGHT, 30.0);
+    private_nh.param("ROTATION", ROTATION, (double)3);
+    private_nh.param("OBSTACLEMINHEIGHT", OBSTACLEMINHEIGHT, (double)1);
+    private_nh.param("MAXBACKDISTANCE", MAXBACKDISTANCE, (double)1);
+    private_nh.param("PASSHEIGHT", PASSHEIGHT, (double)2);
+
+    private_nh.param("MAXRANGE", MAXRANGE, 80.0);
+    private_nh.param("MINRANGE", MINRANGE, (double)3);
+    private_nh.param("GRIDSIZE", GRIDSIZE, 10.0);
+    private_nh.param("IMAGESIZE", IMAGESIZE, 1000.0);
+
 #ifdef DEBUG_GUI
     double PI=3.141592654;
     double density=2*PI/BEAMNUM;

diff --git a/ros/src/util/packages/runtime_manager/scripts/vscan.launch b/ros/src/util/packages/runtime_manager/scripts/vscan.launch
@@ -1,5 +1,41 @@
 <launch>
-  <node pkg="points2image" type="points2vscan" name="points2vscan" output="screen" />
+    <!-- parameters for points2vscan -->
+    <arg name="beam_num" default="1440"/>
+    <arg name="step" default="0.1"/>
+    <arg name="min_floor" default="-3.0"/>
+    <arg name="max_floor" default="-1.3"/>
+    <arg name="max_ceiling" default="6.0" />
+    <arg name="min_ceiling" default="-0.5" />
+    <arg name="road_slop_min_height" default="80.0" />
+    <arg name="road_slop_max_height" default="30.0" />
+    <arg name="rotation" default="3" />
+    <arg name="obstacle_min_height" default="1" />
+    <arg name="max_back_distance" default="1" />
+    <arg name="pass_height" default="2" />
+    <arg name="max_range" default="80.0" />
+    <arg name="min_range" default="3" />
+    <arg name="grid_size" default="10.0" />
+    <arg name="image_size" default="1000.0" />
+
+  <node pkg="points2image" type="points2vscan" name="points2vscan" output="screen" >
+      <param name="BEAMNUM" type="int" value="$(arg beam_num)" />
+      <param name="STEP" type="double" value="$(arg step)" />
+      <param name="MINFLOOR" type="double" value="$(arg min_floor)" />
+      <param name="MAXFLOOR" type="double" value="$(arg max_floor)" />
+      <param name="MAXCEILING" type="double" value="$(arg max_ceiling)" />
+      <param name="MINCEILING" type="double" value="$(arg min_ceiling)" />
+      <param name="ROADSLOPMINHEIGHT" type="double" value="$(arg road_slop_min_height)" />
+      <param name="ROADSLOPMAXHEIGHT" type="double" value="$(arg road_slop_max_height)" />
+      <param name="ROTATION" type="double" value="$(arg rotation)" />
+      <param name="OBSTACLEMINHEIGHT" type="double" value="$(arg obstacle_min_height)" />
+      <param name="MAXBACKDISTANCE" type="double" value="$(arg max_back_distance)" />
+      <param name="PASSHEIGHT" type="double" value="$(arg pass_height)" />
+      <param name="MAXRANGE" type="double" value="$(arg max_range)" />
+      <param name="MINRANGE" type="double" value="$(arg min_range)" />
+      <param name="GRIDSIZE" type="double" value="$(arg grid_size)" />
+      <param name="IMAGESIZE" type="double" value="$(arg image_size)" />
+  </node>
+
   <node pkg="points2image" type="vscan2image" name="vscan2image" output="screen" />
   <node pkg="points2image" type="vscan2linelist" name="vscan2linelist" output="screen" />
 </launch>