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;
}
