diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp index 2195d1f..b019047 100755 --- a/waypoint_nav/src/waypoint_nav.cpp +++ b/waypoint_nav/src/waypoint_nav.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include // include Waypoint class @@ -95,6 +96,7 @@ stop_server_ = nh.advertiseService("stop_wp_nav", &WaypointsNavigation::stopNavigationCallback, this); resume_server_ = nh.advertiseService("resume_nav", &WaypointsNavigation::resumeNavigationCallback, this); cmd_vel_sub_ = nh.subscribe("cmd_vel", 1, &WaypointsNavigation::cmdVelCallback, this); + scan_sub_ = nh.subscribe("scan", 1, &WaypointsNavigation::laserscan_callback, this); wp_pub_ = nh.advertise("waypoints", 10); max_vel_pub_ = nh.advertise("max_vel", 5); wp_num_pub_ = nh.advertise("waypoint_num", 5); @@ -343,6 +345,109 @@ /* + ++++++++++ Callback function for /scan topic ++++++++++ + */ + void laserscan_callback(const sensor_msgs::LaserScan &msg) + { + laserscan = msg; + } + + + + /* + ++++++++++ Search for a goal without obstacles ++++++++++ + */ + void sendNoObstacleGoal(float first_thresh, float after_second_thresh, float goal_thresh, const geometry_msgs::Pose &dest) + { + if(laserscan.ranges.size() > 0){ + move_base_msgs::MoveBaseGoal move_base_goal; + tf::StampedTransform robot_pos = getRobotPosGL(); + geometry_msgs::Pose mid_waypoint; + double start_nav_time, time; + float sum_x, sum_y; + float x[laserscan.ranges.size()]; + float y[laserscan.ranges.size()]; + float waypoint_x = dest.position.x - robot_pos.getOrigin().x(); + float waypoint_y = dest.position.y - robot_pos.getOrigin().y(); + float waypoint_x_update = dest.position.x - robot_pos.getOrigin().x(); + float waypoint_y_update = dest.position.y - robot_pos.getOrigin().y(); + float r = std::sqrt(std::pow(dest.position.x - robot_pos.getOrigin().x(), 2) + std::pow(dest.position.y - robot_pos.getOrigin().y(), 2)); + int j = 0; + int sum_num = 0; + bool pass_bool = false; + for(int i = 0; i < laserscan.ranges.size(); i++){ + if(!(laserscan.ranges[i] < laserscan.range_min || laserscan.ranges[i] > laserscan.range_max || std::isnan(laserscan.ranges[i]))){ + float theta = laserscan.angle_min + i * laserscan.angle_increment; + x[j] = laserscan.ranges[i] * cosf(theta); + y[j] = laserscan.ranges[i] * sinf(theta); + j++; + } + } + float dist_waypoint[j]; + for(int i = 0;;i++){ + sum_x = 0; + sum_y = 0; + sum_num = 0; + for(int ii = 0; ii < j; ii++){ + dist_waypoint[ii] = std::sqrt(std::pow(x[ii] - waypoint_x_update, 2) + std::pow(y[ii] - waypoint_y_update, 2)); + if(i == 0 && dist_waypoint[ii] < first_thresh){ + sum_x += x[ii]; + sum_y += y[ii]; + sum_num++; + } + } + int size = sizeof(dist_waypoint) / sizeof(*dist_waypoint); + float min = *std::min_element(dist_waypoint, dist_waypoint + size); + if((i == 0 && min > first_thresh) || (i == 0 && std::sqrt(std::pow(waypoint_x_update, 2) + std::pow(waypoint_y_update, 2)) < first_thresh)){ + pass_bool = true; + break; + }else if(i == 0 && min < first_thresh){ + ROS_WARN("There is an obstacle near waypoint."); + ROS_WARN("Waypoint x: %f, y: %f", waypoint_x_update, waypoint_y_update); + ROS_WARN("Obstacle x: %f, y: %f", sum_x / sum_num, sum_y / sum_num); + }else if(waypoint_x * waypoint_x_update < 0){ + waypoint_x_update = waypoint_x; + waypoint_y_update = waypoint_y; + break; + }else if(i > 0 && min > after_second_thresh){ + break; + } + waypoint_x = waypoint_x_update; + waypoint_y = waypoint_y_update; + waypoint_x_update = (dest.position.x - robot_pos.getOrigin().x()) * (r - 0.5 * (i + 1)) / r; + waypoint_y_update = (dest.position.y - robot_pos.getOrigin().y()) * (r - 0.5 * (i + 1)) / r; + } + if(!pass_bool){ + std_srvs::Empty empty; + mid_waypoint.position.x = waypoint_x_update + robot_pos.getOrigin().x(); + mid_waypoint.position.y = waypoint_y_update + robot_pos.getOrigin().y(); + mid_waypoint.orientation = dest.orientation; + move_base_goal.target_pose.header.stamp = ros::Time::now(); + move_base_goal.target_pose.header.frame_id = world_frame_; + move_base_goal.target_pose.pose.orientation = mid_waypoint.orientation; + move_base_goal.target_pose.pose.position = mid_waypoint.position; + ROS_WARN("Send goal x: %f, y: %f", waypoint_x_update, waypoint_y_update); + move_base_action_.sendGoal(move_base_goal); + start_nav_time = ros::Time::now().toSec(); + while(!onNavigationPoint(mid_waypoint.position, goal_thresh)){ + time = ros::Time::now().toSec(); + if(time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0) { + ROS_WARN("Resend the navigation goal."); + clear_costmaps_srv_.call(empty); + move_base_goal.target_pose.header.stamp = ros::Time::now(); + move_base_action_.sendGoal(move_base_goal); + start_nav_time = time; + } + sleep(); + } + ROS_WARN("Reach the goal without obstacle."); + } + } + } + + + + /* ++++++++++ Check if robot reached the goal sent to move_base ++++++++++ */ bool navigationFinished() @@ -427,6 +532,13 @@ void startNavigationGL(const geometry_msgs::Pose &dest) { setMaxVel(); + bool FindGoalWithoutObstacle = true; + float first_thresh = 0.5; + float after_second_thresh = 0.7; + float goal_thresh = 0.5; + if(FindGoalWithoutObstacle){ + sendNoObstacleGoal(first_thresh, after_second_thresh, goal_thresh, dest); + } move_base_msgs::MoveBaseGoal move_base_goal; move_base_goal.target_pose.header.stamp = ros::Time::now(); move_base_goal.target_pose.header.frame_id = world_frame_; @@ -560,12 +672,13 @@ tf::TransformListener tf_listener_; ros::Rate rate_; ros::ServiceServer start_server_, stop_server_, resume_server_; - ros::Subscriber cmd_vel_sub_, move_base_status_sub_; + ros::Subscriber cmd_vel_sub_,scan_sub_, move_base_status_sub_; ros::Publisher wp_pub_, max_vel_pub_, wp_num_pub_; ros::ServiceClient clear_costmaps_srv_; double last_moved_time_, dist_err_, min_dist_err_, target_yaw_, min_yaw_err_; std_msgs::UInt16 wp_num_; std_msgs::Float32 max_vel_msg_; + sensor_msgs::LaserScan laserscan; };