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)
{
ros::init(argc, argv, "collision_avoid");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
ros::Subscriber twist_sub = nh.subscribe("twist_raw", 1, TwistCmdCallback);
ros::Subscriber vscan_sub = nh.subscribe("vscan_points", 1, VscanCallback);
ros::Subscriber ndt_sub = nh.subscribe("control_pose", 1, NDTCallback);
ros::Subscriber waypoint_sub = nh.subscribe("safety_waypoint", 1, WaypointCallback);
_twist_pub = nh.advertise<geometry_msgs::TwistStamped>("twist_cmd", 1000);
_vis_pub = nh.advertise<visualization_msgs::Marker>("obstaclewaypoint_mark", 0);
_range_pub = nh.advertise<visualization_msgs::Marker>("detection_range", 0);
_sound_pub = nh.advertise<std_msgs::String>("sound_player", 10);
private_nh.getParam("detection_range", _detection_range);
std::cout << "detection_range : " << _detection_range << std::endl;
private_nh.getParam("threshold_points", _threshold_points);
std::cout << "threshold_points : " << _threshold_points << std::endl;
private_nh.getParam("stop_interval", _stop_interval);
std::cout << "stop_interval : " << _stop_interval << std::endl;
private_nh.getParam("detection_height_top", _detection_height_top);
std::cout << "detection_height_top : " << _detection_height_top << std::endl;
private_nh.getParam("detection_height_bottom", _detection_height_bottom);
std::cout << "detection_height_bottom : " << _detection_height_bottom << std::endl;
private_nh.getParam("current_pose_topic", _current_pose_topic);
std::cout << "current_pose_topic : " << _current_pose_topic << std::endl;
ros::Rate loop_rate(LOOP_RATE);
while (ros::ok()) {
ros::spinOnce();
if (_pose_flag == false || _path_flag == false) {
std::cout << "topic waiting..." << std::endl;
loop_rate.sleep();
continue;
}
bool detection_result = ObstacleDetection();
std::cout << "detection result : ";
if(detection_result == false)
std::cout << "false";
else
std::cout << "true";
std::cout << std::endl;
std::cout << "obstacle waypoint : "<< _obstacle_waypoint << std::endl;
if (_twist_flag == true) {
geometry_msgs::TwistStamped twist;
if (detection_result == true) {
//decelerate
std::cout << "twist deceleration..." << std::endl;
twist.twist.linear.x = Decelerate();
twist.twist.angular.z = _current_twist.twist.angular.z;
} else {
//through
std::cout << "twist through" << std::endl;
twist.twist = _current_twist.twist;
}
std::cout << "twist.linear.x = " << twist.twist.linear.x << std::endl;
std::cout << "twist.angular.z = " << twist.twist.angular.z << std::endl;
std::cout << std::endl;
twist.header.stamp = _current_twist.header.stamp;
_twist_pub.publish(twist);
} else {
std::cout << "no twist topic" << std::endl;
}
loop_rate.sleep();
}
return 0;
}