change ros/catkin/rtk_hpc2,add localizer pub. and test veloctiy_set, know how.

src/ros/catkin/src/pilottoros/src/main.cpp

@@ -52,6 +52,7 @@ ros::Publisher odom_pub;
 ros::Publisher waypoints_pub;
 ros::Publisher pose_pub;
 ros::Publisher twist_pub;
+ros::Publisher localizer_pub;
 
 
 static iv::map::tracemap gtracemap;
@@ -181,6 +182,7 @@ if(_use_pilot_waypoints)
  //   std::cout<<" public pose"<<std::endl;
     pose_pub.publish(xPose);
     twist_pub.publish(xtwist);
+    localizer_pub.publish(xPose);
 }
 
 
@@ -363,6 +365,7 @@ int main(int argc, char *argv[])
         waypoints_pub = private_nh.advertise<autoware_msgs::Lane>(_waypoints_topic,10);
         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);
     }
 	
 

src/ros/catkin/src/rtk_hcp2_baselink/CMakeLists.txt

@@ -104,12 +104,12 @@ include_directories(
 )
 
 ## Declare a C++ executable
-add_executable(rtk_hcp2 src/main.cpp  src/gnss_coordinate_convert.cpp)
-target_link_libraries(rtk_hcp2 ${catkin_LIBRARIES}  ${OpenCV_LIBRARIES}    ${Protobuf_LIBRARIES}   Qt5Core Qt5SerialPort)
+add_executable(rtk_hcp2_baselink src/main.cpp  src/gnss_coordinate_convert.cpp)
+target_link_libraries(rtk_hcp2_baselink ${catkin_LIBRARIES}  ${OpenCV_LIBRARIES}    ${Protobuf_LIBRARIES}   Qt5Core Qt5SerialPort)
 
 
 install(
-  TARGETS rtk_hcp2
+  TARGETS rtk_hcp2_baselink
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

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

@@ -254,8 +254,10 @@ 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 + 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
     if (i >= static_cast<int>(lane.waypoints.size()))
       break;
@@ -267,6 +269,7 @@ int detectStopObstacle(const pcl::PointCloud<pcl::PointXYZ>& points, const int c
       stop_obstacle_waypoint = i;
       *obstacle_type = EObstacleType::STOPLINE;
       obstacle_points->setStopPoint(lane.waypoints.at(i).pose.pose.position); // for vizuialization
+      std::cout<<"breakl"<<std::endl;
       break;
     }
 
@@ -287,6 +290,7 @@ int detectStopObstacle(const pcl::PointCloud<pcl::PointXYZ>& points, const int c
     tf::Vector3 tf_waypoint = point2vector(waypoint);
     tf_waypoint.setZ(0);
 
+    std::cout<<" stop range "<<stop_range<<std::endl;
     int stop_point_count = 0;
     for (const auto& p : points)
     {
@@ -309,6 +313,7 @@ int detectStopObstacle(const pcl::PointCloud<pcl::PointXYZ>& points, const int c
     if (stop_point_count > points_threshold)
     {
       stop_obstacle_waypoint = i;
+      std::cout<<" obstacle on waypoints."<<std::endl;
       *obstacle_type = EObstacleType::ON_WAYPOINTS;
       break;
     }
@@ -328,6 +333,7 @@ int detectDecelerateObstacle(const pcl::PointCloud<pcl::PointXYZ>& points, const
 {
   int decelerate_obstacle_waypoint = -1;
   // start search from the closest waypoint
+  std::cout<<" close point index : "<<closest_waypoint<<" decdis: "<<DECELERATION_SEARCH_DISTANCE<<std::endl;
   for (int i = closest_waypoint; i < closest_waypoint + DECELERATION_SEARCH_DISTANCE; i++)
   {
     // reach the end of waypoints
@@ -377,11 +383,13 @@ EControl pointsDetection(const pcl::PointCloud<pcl::PointXYZ>& points, const int
                          const autoware_msgs::Lane& lane, const CrossWalk& crosswalk, const VelocitySetInfo& vs_info,
                          int* obstacle_waypoint, ObstaclePoints* obstacle_points)
 {
-  std::cout<<"points callback "<<std::endl;
+//  std::cout<<"points callback size "<<points.size()<<std::endl;
   // no input for detection || no closest waypoint
   if ((points.empty() == true && vs_info.getDetectionResultByOtherNodes() == -1) || closest_waypoint < 0)
     return EControl::KEEP;
 
+    std::cout<<"detect stop"<<std::endl;
+
 
   EObstacleType obstacle_type = EObstacleType::NONE;
   int stop_obstacle_waypoint =
@@ -389,8 +397,9 @@ EControl pointsDetection(const pcl::PointCloud<pcl::PointXYZ>& points, const int
                          vs_info.getPointsThreshold(), vs_info.getLocalizerPose(),
                          obstacle_points, &obstacle_type, vs_info.getDetectionResultByOtherNodes());
 
+  std::cout<<" stop waypoint "<<stop_obstacle_waypoint<<std::endl;
   // skip searching deceleration range
-  if (vs_info.getDecelerationRange() < 0.01)
+ if (vs_info.getDecelerationRange() < 0.01)
   {
     *obstacle_waypoint = stop_obstacle_waypoint;
     if (stop_obstacle_waypoint < 0)
@@ -408,6 +417,7 @@ EControl pointsDetection(const pcl::PointCloud<pcl::PointXYZ>& points, const int
                                vs_info.getPointsThreshold(), vs_info.getLocalizerPose(), obstacle_points);
 
   // stop obstacle was not found
+  //std::cout<<" dec : "<<decelerate_obstacle_waypoint<<std::endl;
   if (stop_obstacle_waypoint < 0)
   {
     *obstacle_waypoint = decelerate_obstacle_waypoint;
@@ -420,7 +430,8 @@ EControl pointsDetection(const pcl::PointCloud<pcl::PointXYZ>& points, const int
     *obstacle_waypoint = stop_obstacle_waypoint;
     return EControl::STOP;
   }
-
+ 
+   
   // about 5.0 meter
   double waypoint_interval =
       getPlaneDistance(lane.waypoints[0].pose.pose.position, lane.waypoints[1].pose.pose.position);
@@ -453,6 +464,13 @@ EControl obstacleDetection(int closest_waypoint, const autoware_msgs::Lane& lane
   static EControl prev_detection = EControl::KEEP;
   static int prev_obstacle_waypoint = -1;
 
+
+    if (detection_result == EControl::STOP)
+    {
+      std::cout<<"STOP STOP STOP"<<std::endl;
+    }
+
+
   // stop or decelerate because we found obstacles
   if (detection_result == EControl::STOP || detection_result == EControl::STOPLINE || detection_result == EControl::DECELERATE)
   {
@@ -478,6 +496,7 @@ EControl obstacleDetection(int closest_waypoint, const autoware_msgs::Lane& lane
 
   // there are no obstacles, so we move forward
   *obstacle_waypoint = -1;
+  //std::cout<<" ob point "<<*obstacle_waypoint <<std::endl;
   false_count = 0;
   prev_detection = EControl::KEEP;
   return detection_result;
@@ -488,6 +507,7 @@ void changeWaypoints(const VelocitySetInfo& vs_info, const EControl& detection_r
 {
   if (detection_result == EControl::STOP || detection_result == EControl::STOPLINE)
   {  // STOP for obstacle/stopline
+  std::cout<<"enter stop change . "<<std::endl;
     // stop_waypoint is about stop_distance meter away from obstacles/stoplines
     int stop_distance = (detection_result == EControl::STOP)
       ? vs_info.getStopDistanceObstacle() : vs_info.getStopDistanceStopline();
@@ -495,6 +515,7 @@ void changeWaypoints(const VelocitySetInfo& vs_info, const EControl& detection_r
       ? vs_info.getDecelerationObstacle() : vs_info.getDecelerationStopline();
     int stop_waypoint =
         calcWaypointIndexReverse(vs_path->getPrevWaypoints(), obstacle_waypoint, stop_distance);
+        std::cout<<" stop distance : "<<stop_distance<<"  deceleration: "<<deceleration<<"  stopwaypoint: "<<stop_waypoint<<std::endl;
     // change waypoints to stop by the stop_waypoint
     vs_path->changeWaypointsForStopping(stop_waypoint, obstacle_waypoint, closest_waypoint, deceleration);
     vs_path->avoidSuddenAcceleration(deceleration, closest_waypoint);

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

@@ -108,6 +108,8 @@ void VelocitySetInfo::pointsCallback(const sensor_msgs::PointCloud2ConstPtr &msg
     points_.push_back(v);
   }
 
+  std::cout<<"points size: "<<points_.size()<<std::endl;
+
 }
 
 void VelocitySetInfo::detectionCallback(const std_msgs::Int32 &msg)