Fork: 0
tsukuba2022 / waypoint_navigation
Browse code
update
master
1 parent aa1c94f commit 196ae3a2974b5b22155e53c33d8767576045c6e5
@koki koki authored on 27 Oct 2022
Patch
Unified Split
Showing 1 changed file
View
120
■■■■■
 waypoint_nav/src/waypoint_nav.cpp
init_waypoint_ = waypoints_.poses.begin();
compare_waypoint_ = waypoints_.poses.begin();
double min_dist_w = std::numeric_limits<int>::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;
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();
}
if(min_dist_w == std::numeric_limits<int>::max()){
ROS_WARN("Could not find the closest appropriate waypoint. Please check the direction of the robot again.");
response.success = false;
return false;
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;
current_waypoint_ = init_waypoint_;
has_activate_ = true;
response.success = true;
return true;
}
//#############################################################
}
 
void run()
{
ROS_INFO("Waiting for waypoint navigation to start.");
int resend_goal;
double start_nav_time, time;
std_srvs::Empty empty;
while(ros::ok()) {
// has_activate_ is false, nothing to do
if (!has_activate_) {
sleep();
// go to fianal goal and finish process
if (current_waypoint_ == finish_pose_) {
ROS_INFO_STREAM("Go to final goal");
startNavigationGL(*current_waypoint_);
start_nav_time = ros::Time::now().toSec();
while(!onNavigationPoint(finish_pose_->position) && ros::ok()) {
sleep();
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);
startNavigationGL(*current_waypoint_);
start_nav_time = time;
}
sleep();
}
ROS_INFO("Final goal reached!!");
has_activate_ = false;
continue;
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_);
resend_goal = 0;
double start_nav_time = ros::Time::now().toSec();
start_nav_time = ros::Time::now().toSec();
dist_err_ = waypoint_list_[wp_num_.data-1].rad;
try {
// loop until reach waypoint
while(!onNavigationPoint(current_waypoint_->position, dist_err_)) {
if (!has_activate_) throw SwitchRunningStatus();
double time = ros::Time::now().toSec();
time = ros::Time::now().toSec();
if (time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0) {
ROS_WARN("Resend the navigation goal.");
std_srvs::Empty empty;
clear_costmaps_srv_.call(empty);
startNavigationGL(*current_waypoint_);
// resend_goal++;
//if (resend_goal == 3) {