change rtk_hcp2,add localizer_pose. change points_filter_nan, test ok.

src/ros/catkin/src/pilottoros/package.xml

@@ -7,7 +7,7 @@
   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
   <!-- Example:  -->
   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="hans@todo.todo">hans</maintainer>
+  <maintainer email="yuchuli@catarc.ac.cn">yuchuli</maintainer>
 
 
   <!-- One license tag required, multiple allowed, one license per tag -->

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

@@ -24,19 +24,25 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr &input)
 
     pcl::fromROSMsg(*input, point_cloud);
 
-    int pointsize = point_cloud.width;
+    int pointsize = point_cloud.points.size();
     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.at(i).x;
-        y = point_cloud.at(i).y;
-        z = point_cloud.at(i).z;
+        x = point_cloud.points.at(i).x;
+        y =point_cloud.points.at(i).y;
+        z =point_cloud.points.at(i).z;
+
         if(std::isnan(x) ||(std::isnan(y))||std::isnan(z))
         {
             nskip++;
@@ -47,12 +53,18 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr &input)
             nskip++;
             continue;
         }
-        point_cloud_filter->points.push_back(point_cloud.at(i));
+
+        pcl::PointXYZI point;
+        point.x = x;
+        point.y = y;
+        point.z = z;
+        point.intensity = point_cloud.points.at(i).intensity;
+        point_cloud_filter->points.push_back(point);
         ++point_cloud_filter->width;
     }
 
     std::cout<<" skip nan point count: "<<nskip<<" out point count: "<<point_cloud_filter->width<<std::endl;
-     point_cloud_pub.publish(point_cloud);
+     point_cloud_pub.publish(point_cloud_filter);
 }
 
 

src/ros/catkin/src/rostopilot/package.xml

@@ -7,7 +7,7 @@
   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
   <!-- Example:  -->
   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="hans@todo.todo">hans</maintainer>
+  <maintainer email="yuchuli@catarc.ac.cn">yuchuli</maintainer>
 
 
   <!-- One license tag required, multiple allowed, one license per tag -->

src/ros/catkin/src/rtk_hcp2/package.xml

@@ -7,7 +7,7 @@
   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
   <!-- Example:  -->
   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="hans@todo.todo">hans</maintainer>
+  <maintainer email="yuchuli@catarc.ac.cn">yuchuli</maintainer>
 
 
   <!-- One license tag required, multiple allowed, one license per tag -->

src/ros/catkin/src/rtk_hcp2/src/main.cpp

@@ -33,6 +33,7 @@ static double ghead0 = 0;
 
 ros::Publisher pose_pub;
 ros::Publisher twist_pub;
+ros::Publisher localizer_pub;
 
 QString mstrHCP2;
 
@@ -311,6 +312,7 @@ void SerialGPSDecodeSen(QString strsen)
 
     pose_pub.publish(xPose);
     twist_pub.publish(xtwist);
+     localizer_pub.publish(xPose);
 
 //    std::cout<<"current pose"<<std::endl;
 
@@ -511,6 +513,7 @@ int main(int argc, char *argv[])
 
     pose_pub = private_nh.advertise<geometry_msgs::PoseStamped>(_pose_topic,10);
     twist_pub = private_nh.advertise<geometry_msgs::TwistStamped>(_twist_topic,10);
+    localizer_pub = private_nh.advertise<geometry_msgs::PoseStamped>("/localizer_pose",10);
 
 
 std::thread * pthread = new std::thread(ThreadRecvRTK);

src/ros/catkin/src/waypoint_planner/src/velocity_set/velocity_set.cpp

@@ -254,8 +254,8 @@ int detectStopObstacle(const pcl::PointCloud<pcl::PointXYZ>& points, const int c
   *obstacle_type = EObstacleType::NONE;
   std::cout<<" close "<<closest_waypoint<<std::endl<<" search distance: "<<STOP_SEARCH_DISTANCE<<std::endl;
   // start search from the closest waypoint
-  //for (int i = closest_waypoint; i < closest_waypoint + STOP_SEARCH_DISTANCE; i++)
-  for (int i = closest_waypoint; i < closest_waypoint + 300; i++)
+  for (int i = closest_waypoint; i < closest_waypoint + STOP_SEARCH_DISTANCE; i++)
+  //for (int i = closest_waypoint; i < closest_waypoint + 300; i++)
   {
     std::cout<<" i : "<<i<<std::endl;
     // reach the end of waypoints