int main(int argc, char **argv) { int i = 0;/// ros::init(argc, argv, "velocity_set"); ros::NodeHandle nh; ros::NodeHandle private_nh("~"); ros::Subscriber localizer_sub = nh.subscribe("localizer_pose", 1, LocalizerCallback); ros::Subscriber control_pose_sub = nh.subscribe("control_pose", 1, ControlCallback); ros::Subscriber odometry_subscriber = nh.subscribe("odom_pose", 10, OdometryPoseCallback); ros::Subscriber vscan_sub = nh.subscribe("vscan_points", 1, VscanCallback); ros::Subscriber base_waypoint_sub = nh.subscribe("base_waypoints", 1, BaseWaypointCallback); ros::Subscriber obj_pose_sub = nh.subscribe("obj_pose", 1, ObjPoseCallback); ros::Subscriber estimated_vel_sub = nh.subscribe("estimated_vel_mps", 1, EstimatedVelCallback); ros::Subscriber config_sub = nh.subscribe("config/velocity_set", 10, ConfigCallback); //------------------ Vector Map ----------------------// ros::Subscriber sub_dtlane = nh.subscribe("vector_map_info/cross_walk", 1, &CrossWalk::CrossWalkCallback, &vmap); ros::Subscriber sub_area = nh.subscribe("vector_map_info/area_class", 1, &CrossWalk::AreaclassCallback, &vmap); ros::Subscriber sub_line = nh.subscribe("vector_map_info/line_class", 1, &CrossWalk::LineclassCallback, &vmap); ros::Subscriber sub_point = nh.subscribe("vector_map_info/point_class", 1, &CrossWalk::PointclassCallback, &vmap); //----------------------------------------------------// _range_pub = nh.advertise<visualization_msgs::MarkerArray>("detection_range", 0); _sound_pub = nh.advertise<std_msgs::String>("sound_player", 10); _temporal_waypoints_pub = nh.advertise<waypoint_follower::lane>("temporal_waypoints", 1000, true); static ros::Publisher closest_waypoint_pub; closest_waypoint_pub = nh.advertise<std_msgs::Int32>("closest_waypoint", 1000); _obstacle_pub = nh.advertise<visualization_msgs::Marker>("obstacle", 0); private_nh.param<bool>("sim_mode", g_sim_mode, false); ROS_INFO_STREAM("sim_mode : " << g_sim_mode); ros::Rate loop_rate(LOOP_RATE); while (ros::ok()) { ros::spinOnce(); if (vmap.loaded_all && !vmap.set_points) vmap.setCrossWalkPoints(); if (_pose_flag == false || _path_flag == false) { std::cout << "\rtopic waiting \rtopic waiting"; for (int j = 0; j < i; j++) {std::cout << ".";} i++; i = i%10; std::cout << std::flush; loop_rate.sleep(); continue; } _closest_waypoint = getClosestWaypoint(_path_change.getCurrentWaypoints(), _current_pose.pose); closest_waypoint_pub.publish(_closest_waypoint); vmap.setDetectionWaypoint(FindCrossWalk()); EControl detection_result = ObstacleDetection(); ChangeWaypoint(detection_result); loop_rate.sleep(); } return 0; }
int main(int argc, char **argv) { int i = 0; /// ros::init(argc, argv, "velocity_set"); ros::NodeHandle nh; ros::NodeHandle private_nh("~"); ros::Subscriber localizer_sub = nh.subscribe("localizer_pose", 1, localizerCallback); ros::Subscriber control_pose_sub = nh.subscribe("current_pose", 1, controlCallback); ros::Subscriber vscan_sub = nh.subscribe("vscan_points", 1, vscanCallback); ros::Subscriber base_waypoint_sub = nh.subscribe("base_waypoints", 1, baseWaypointCallback); ros::Subscriber obj_pose_sub = nh.subscribe("obj_pose", 1, objPoseCallback); ros::Subscriber current_vel_sub = nh.subscribe("current_velocity", 1, currentVelCallback); ros::Subscriber config_sub = nh.subscribe("config/velocity_set", 10, configCallback); //------------------ Vector Map ----------------------// ros::Subscriber sub_dtlane = nh.subscribe("vector_map_info/cross_walk", 1, &CrossWalk::crossWalkCallback, &vmap); ros::Subscriber sub_area = nh.subscribe("vector_map_info/area_class", 1, &CrossWalk::areaclassCallback, &vmap); ros::Subscriber sub_line = nh.subscribe("vector_map_info/line_class", 1, &CrossWalk::lineclassCallback, &vmap); ros::Subscriber sub_point = nh.subscribe("vector_map_info/point_class", 1, &CrossWalk::pointclassCallback, &vmap); //----------------------------------------------------// g_range_pub = nh.advertise<visualization_msgs::MarkerArray>("detection_range", 0); g_sound_pub = nh.advertise<std_msgs::String>("sound_player", 10); g_temporal_waypoints_pub = nh.advertise<waypoint_follower::lane>("temporal_waypoints", 1000, true); ros::Publisher closest_waypoint_pub; closest_waypoint_pub = nh.advertise<std_msgs::Int32>("closest_waypoint", 1000); g_obstacle_pub = nh.advertise<visualization_msgs::Marker>("obstacle", 0); ros::Rate loop_rate(LOOP_RATE); while (ros::ok()) { ros::spinOnce(); if (vmap.loaded_all && !vmap.set_points) vmap.setCrossWalkPoints(); if (g_pose_flag == false || g_path_flag == false) { std::cout << "\rtopic waiting \rtopic waiting"; for (int j = 0; j < i; j++) { std::cout << "."; } i++; i = i % 10; std::cout << std::flush; loop_rate.sleep(); continue; } g_closest_waypoint = getClosestWaypoint(g_path_change.getCurrentWaypoints(), g_control_pose.pose); std_msgs::Int32 closest_waypoint; closest_waypoint.data = g_closest_waypoint; closest_waypoint_pub.publish(closest_waypoint); vmap.setDetectionWaypoint(findCrossWalk()); EControl detection_result = obstacleDetection(); changeWaypoint(detection_result); g_vscan.clear(); loop_rate.sleep(); } return 0; }