change points_filter_nan, add yawrotate. change tool_trace2vectormap, fix area.

src/ros/catkin/src/points_filter_nan/launch/points_filter_nan.launch

@@ -0,0 +1,12 @@
+
+<launch>
+  <node pkg="points_filter_nan" type="points_filter_nan" name="points_filter_nan"  output="screen">
+    <param name="points_topic" value="/points_raw"/>
+    <param name="sensor_topic" value="/points_raw_sensor"/>
+    <param name="rotate" value="0"/>  
+  </node>
+
+</launch>
+
+
+

src/ros/catkin/src/points_filter_nan/src/main.cpp

@@ -13,6 +13,9 @@ static std::string _point_topic = "/points_raw";
 ros::Publisher  point_cloud_pub;
 ros::Subscriber points_sub;
 
+static  double yawrotate = 0;
+
+
 
 
 static void points_callback(const sensor_msgs::PointCloud2::ConstPtr &input)
@@ -24,25 +27,20 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr &input)
 
     pcl::fromROSMsg(*input, point_cloud);
 
-    int pointsize = point_cloud.points.size();
+    int pointsize = point_cloud.width;
     int i;
     int nskip = 0;
 
         pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_filter(
                 new pcl::PointCloud<pcl::PointXYZI>());
     point_cloud_filter->header = point_cloud.header;
-
-    point_cloud_filter->width = 0;
-    point_cloud_filter->height = 1;
-    
-    std::cout<<" point width "<<point_cloud.width<<"  size: "<<point_cloud.points.size()<<std::endl;
     for(i=0;i<pointsize;i++)
     {
         double x,y,z;
-        x = point_cloud.points.at(i).x;
-        y =point_cloud.points.at(i).y;
-        z =point_cloud.points.at(i).z;
-
+        x = point_cloud.at(i).x;
+        y = point_cloud.at(i).y;
+        z = point_cloud.at(i).z;
+        point_cloud.points.at(i);
         if(std::isnan(x) ||(std::isnan(y))||std::isnan(z))
         {
             nskip++;
@@ -55,6 +53,13 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr &input)
         }
 
         pcl::PointXYZI point;
+        if(yawrotate != 0)
+        {
+                double s = x*cos(yawrotate*M_PI/180.0) - y*sin(yawrotate*M_PI/180.0);
+                double t = x*sin(yawrotate*M_PI/180.0)+y*cos(yawrotate*M_PI/180.0);
+                x = s;
+                y= t;
+        }
         point.x = x;
         point.y = y;
         point.z = z;
@@ -64,7 +69,7 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr &input)
     }
 
     std::cout<<" skip nan point count: "<<nskip<<" out point count: "<<point_cloud_filter->width<<std::endl;
-     point_cloud_pub.publish(point_cloud_filter);
+     point_cloud_pub.publish(point_cloud);
 }
 
 
@@ -78,8 +83,10 @@ int main(int argc, char *argv[])
 	    
     private_nh.getParam("points_topic",_point_topic);
     private_nh.getParam("sensor_topic",_point_sensor_topic);
+    private_nh.getParam("rotate",yawrotate);
     std::cout<<"_point_topic is "<<_point_topic<<std::endl;
 	std::cout<<"_sensor_topic : "<<_point_sensor_topic<<std::endl;
+    std::cout<<" yaw rotate: "<<yawrotate<<std::endl;
 
     point_cloud_pub = private_nh.advertise<sensor_msgs::PointCloud2>(
                 _point_topic, 10);

src/tool/tool_trace2vectormap/trace2vectormap.cpp

@@ -262,12 +262,70 @@ int trace2vectormap::convert()
         xvectorline[i + npointsize-1].blid = xvectorline[i-1 + npointsize -1].lid;
     }
 
+    int nlid =  (npointsize-1)*2;
+    int naid = 1;
+    for(i=1;i<npointsize;i++)
+    {
+
+        int p1id,p2id,p3id,p4id;
+        p4id =  xvectorpoint[i-1+npointsize*1].pid;
+        p3id = xvectorpoint[i+npointsize*1].pid;
+        p2id = xvectorpoint[i+npointsize*2].pid;
+        p1id = xvectorpoint[i-1+npointsize*2].pid;
+
+
+            iv::vectormap::area xarea;
+            xarea.aid = naid;
+        iv::vectormap::line xline;
+        xline.lid = nlid;
+        xline.bpid = p4id;
+        xline.fpid = p3id;
+        xline.blid = 0;// nlid+3;
+        xline.flid = nlid+1;
+        xvectorline.push_back(xline);
+        nlid++;
+        xarea.slid = xline.lid;
+        xline.lid = nlid;
+        xline.bpid = p3id;
+        xline.fpid = p2id;
+        xline.blid = nlid-1;
+        xline.flid = nlid+1;
+        xvectorline.push_back(xline);
+        nlid++;
+        xline.lid = nlid;
+        xline.bpid = p2id;
+        xline.fpid = p1id;
+        xline.blid = nlid-1;
+        xline.flid = nlid+1;
+        xvectorline.push_back(xline);
+        nlid++;
+        xline.lid = nlid;
+        xline.bpid = p1id;
+        xline.fpid = p4id;
+        xline.blid = nlid-1;
+        xline.flid =0;//nlid-3;
+        xvectorline.push_back(xline);
+        nlid++;
+        xarea.elid = xline.lid;
+        xvectorarea.push_back(xarea);
+
+        naid++;
+        iv::vectormap::wayarea xwayarea;
+        xwayarea.waid = i;
+        xwayarea.aid = xarea.aid;
+        xvectorwayarea.push_back(xwayarea);
+
+    }
+
+    /*
     for(i=1;i<npointsize;i++)
     {
         iv::vectormap::line xline;
         xline.lid = i + (npointsize-1)*2;
-        xline.bpid = xvectorpoint[i+npointsize*2].pid;
-        xline.fpid = xvectorpoint[i-1+npointsize*2].pid;
+//        xline.bpid = xvectorpoint[i+npointsize*2].pid;
+//        xline.fpid = xvectorpoint[i-1+npointsize*2].pid;
+        xline.bpid = xvectorpoint[i+npointsize*0].pid;
+        xline.fpid = xvectorpoint[i-1+npointsize*0].pid;
         xline.blid = 0;
         xline.flid = 0;
         xvectorline.push_back(xline);
@@ -283,7 +341,9 @@ int trace2vectormap::convert()
     {
         iv::vectormap::area xarea;
         xarea.aid = i+1;
-        xarea.slid = xvectorline[i+(npointsize-1)].lid;
+        xarea.slid = xvectorline[i+(npointsize-1)*1].lid;
+        xarea.elid = xvectorline[i+(npointsize-1)*2].lid;
+        xarea.slid = xvectorline[i+(npointsize-1)*1].lid;
         xarea.elid = xvectorline[i+(npointsize-1)*2].lid;
         xvectorarea.push_back(xarea);
         iv::vectormap::wayarea xwayarea;
@@ -291,6 +351,7 @@ int trace2vectormap::convert()
         xwayarea.aid = xarea.aid;
         xvectorwayarea.push_back(xwayarea);
     }
+    */
 
     if(bcircle)
     {