diff --git a/.gitignore b/.gitignore index dbe9c82..4a0e4ed 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ -.vscode/ \ No newline at end of file +.vscode/ +waypoint_manager/scripts/lib/__pycache__ diff --git a/waypoint_manager/scripts/lib/__pycache__/__init__.cpython-38.pyc b/waypoint_manager/scripts/lib/__pycache__/__init__.cpython-38.pyc deleted file mode 100644 index 0c2bd2a..0000000 --- a/waypoint_manager/scripts/lib/__pycache__/__init__.cpython-38.pyc +++ /dev/null Binary files differ diff --git a/waypoint_manager/scripts/lib/__pycache__/mymaplib.cpython-38.pyc b/waypoint_manager/scripts/lib/__pycache__/mymaplib.cpython-38.pyc deleted file mode 100644 index f4e3611..0000000 --- a/waypoint_manager/scripts/lib/__pycache__/mymaplib.cpython-38.pyc +++ /dev/null Binary files differ diff --git a/waypoint_manager/scripts/lib/__pycache__/waypointlib.cpython-38.pyc b/waypoint_manager/scripts/lib/__pycache__/waypointlib.cpython-38.pyc deleted file mode 100644 index 88c98f6..0000000 --- a/waypoint_manager/scripts/lib/__pycache__/waypointlib.cpython-38.pyc +++ /dev/null Binary files differ diff --git a/waypoint_nav/launch/waypoint_nav.launch b/waypoint_nav/launch/waypoint_nav.launch old mode 100644 new mode 100755 index db60758..4cd7678 --- a/waypoint_nav/launch/waypoint_nav.launch +++ b/waypoint_nav/launch/waypoint_nav.launch @@ -1,20 +1,22 @@ - - - - - - - - + + + + + + + + + - - + + + diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp old mode 100644 new mode 100755 index cfbe79a..38ee0a8 --- a/waypoint_nav/src/waypoint_nav.cpp +++ b/waypoint_nav/src/waypoint_nav.cpp @@ -79,6 +79,14 @@ } else { ROS_ERROR("waypoints file doesn't have name"); } + + //Scripts of "start from the middle" + //Edited mori 2022/10/19 + //############################################################# + StartFromTheMiddle = false; + private_nh.getParam("StartFromTheMiddle", StartFromTheMiddle); + //############################################################# + private_nh.param("min_dist_err", min_dist_err_, min_dist_err_); private_nh.param("min_yaw_err", min_yaw_err_, min_yaw_err_); @@ -89,7 +97,7 @@ cmd_vel_sub_ = nh.subscribe("cmd_vel", 1, &WaypointsNavigation::cmdVelCallback, this); wp_pub_ = nh.advertise("waypoints", 10); max_vel_pub_ = nh.advertise("max_vel", 5); - wp_num_pub_ = nh.advertise("waypoint_number", 5); + wp_num_pub_ = nh.advertise("waypoint_num", 5); clear_costmaps_srv_ = nh.serviceClient("/move_base/clear_costmaps"); } @@ -202,16 +210,72 @@ response.success = false; return false; } + //Scripts of "start from the middle" + //Edited mori 2022/10/19 + //############################################################# + wp_num_.data = 1; + init_waypoint_ = waypoints_.poses.begin(); + compare_waypoint_ = waypoints_.poses.begin(); + double min_dist_w = std::numeric_limits::max(); + if(StartFromTheMiddle){ + ROS_INFO("StartFromTheMiddle => True"); + tf::StampedTransform robot_init_pos = getRobotPosGL(); + double init_x = robot_init_pos.getOrigin().x(); + double init_y = robot_init_pos.getOrigin().y(); + double pre_waypoint_x; + double pre_waypoint_y; + double convert_x; + double init_yaw = tf::getYaw(robot_init_pos.getRotation()); + double square_x; + double square_y; + double dist_w; + double dir_w; + int min_waypoint = 0; + int i = 0; + while(compare_waypoint_ != waypoints_.poses.end() - 1){ + i++; + square_x = (compare_waypoint_ -> position.x - init_x) * (compare_waypoint_ -> position.x - init_x); + square_y = (compare_waypoint_ -> position.y - init_y) * (compare_waypoint_ -> position.y - init_y); + dist_w = sqrt(square_x + square_y); + if(i == 1){ + dir_w = atan2(compare_waypoint_ -> position.y, compare_waypoint_ -> position.x); + convert_x = (compare_waypoint_ -> position.x - init_x) * cos(-init_yaw) - (compare_waypoint_ -> position.y - init_y) * sin(-init_yaw); + if(abs(dir_w - init_yaw) < (90 / 180.0 * M_PI) && convert_x > 0){ + min_dist_w = dist_w; + init_waypoint_ = compare_waypoint_; + min_waypoint = 1; + } + }else if(min_dist_w > dist_w){ + dir_w = atan2(compare_waypoint_ -> position.y - pre_waypoint_y, compare_waypoint_ -> position.x- pre_waypoint_x); + convert_x = (compare_waypoint_ -> position.x - init_x) * cos(-init_yaw) - (compare_waypoint_ -> position.y - init_y) * sin(-init_yaw); + if(abs(dir_w - init_yaw) < (90 / 180.0 * M_PI) && convert_x > 0){ + min_dist_w = dist_w; + init_waypoint_ = compare_waypoint_; + min_waypoint = i; + } + } + pre_waypoint_x = compare_waypoint_ -> position.x; + pre_waypoint_y = compare_waypoint_ -> position.y; + compare_waypoint_++; + } + wp_num_.data = min_waypoint; + } std_srvs::Empty empty; while(!clear_costmaps_srv_.call(empty)) { ROS_WARN("Resend clear costmap service"); sleep(); } - current_waypoint_ = waypoints_.poses.begin(); - wp_num_.data = 1; - has_activate_ = true; - response.success = true; - return true; + if(min_dist_w == std::numeric_limits::max()){ + ROS_WARN("Could not find the closest appropriate waypoint. Please check the direction of the robot again."); + response.success = false; + return false; + }else{ + current_waypoint_ = init_waypoint_; + has_activate_ = true; + response.success = true; + return true; + } + //############################################################# } @@ -233,7 +297,6 @@ } - /* ++++++++++ Callback function for service of "stop_wp_nav" ++++++++++ */ @@ -304,7 +367,9 @@ double r, p ,y; m.getRPY(r, p, y); double diff_yaw = std::abs(y - target_yaw_); - return ((dist < min_dist_err_) && (diff_yaw < min_yaw_err_)); + if ((dist < min_dist_err_) && (diff_yaw < min_yaw_err_)) return true; + else if ((dist < min_dist_err_ * 2) && navigationFinished()) return true; + return false; } return dist < dist_err; } @@ -391,6 +456,7 @@ void run() { ROS_INFO("Waiting for waypoint navigation to start."); + int resend_goal; while(ros::ok()) { // has_activate_ is false, nothing to do if (!has_activate_) { @@ -412,11 +478,9 @@ ROS_INFO_STREAM("Go to waypoint " << wp_num_.data); ROS_INFO_STREAM("x: " << waypoint_list_[wp_num_.data-1].x << ", y: " << waypoint_list_[wp_num_.data-1].y); startNavigationGL(*current_waypoint_); - int resend_goal = 0; + resend_goal = 0; double start_nav_time = ros::Time::now().toSec(); - ROS_WARN("error"); dist_err_ = waypoint_list_[wp_num_.data-1].rad; - ROS_WARN("error"); try { // loop until reach waypoint while(!onNavigationPoint(current_waypoint_->position, dist_err_)) { @@ -446,6 +510,7 @@ // update current waypoint current_waypoint_++; wp_num_.data++; + wp_num_pub_.publish(wp_num_); } catch(const SwitchRunningStatus &e) { ROS_INFO_STREAM("Running status switched"); @@ -462,6 +527,15 @@ geometry_msgs::PoseArray waypoints_; visualization_msgs::MarkerArray marker_; std::vector waypoint_list_; + + //Scripts of "start from the middle" + //Edited mori 2022/10/19 + //############################################################# + bool StartFromTheMiddle; + std::vector::iterator init_waypoint_; + std::vector::iterator compare_waypoint_; + //############################################################# + std::vector::iterator current_waypoint_; std::vector::iterator finish_pose_; bool has_activate_, stopped_;