Fork: 0
tsukuba2022 / waypoint_navigation
Browse code
fix
master
1 parent d0a6a96 commit d6cfe37fa232604dedbbb62e361de34275d23706
@koki koki authored on 20 Oct 2022
Patch
Unified Split
Showing 2 changed files
View
24
■■■■■
 waypoint_nav/launch/waypoint_nav.launch 100644 → 100755
<?xml version="1.0"?>
 
<launch>
<arg name="waypoints_file" default="$(find waypoint_nav)/waypoints_cfg/waypoints.yaml"/>
<arg name="cmd_vel_topic_in" default="/move_base/cmd_vel"/>
<arg name="cmd_vel_topic_out" default="/wp_nav/cmd_vel"/>
<arg name="robot_frame" default="base_link"/>
<arg name="max_vel_param" default="/move_base/TrajectoryPlannerROS/max_vel_x"/>
<arg name="min_vel_param" default="/move_base/TrajectoryPlannerROS/min_vel_x"/>
<arg name="min_dist_err" default="0.3"/>
<arg name="min_yaw_err" default="0.3"/>
<arg name="waypoints_file" default="$(find waypoint_nav)/waypoints_cfg/waypoints.yaml"/>
<arg name="StartFromTheMiddle" default="false"/>
<arg name="cmd_vel_topic_in" default="/move_base/cmd_vel"/>
<arg name="cmd_vel_topic_out" default="/wp_nav/cmd_vel"/>
<arg name="robot_frame" default="base_link"/>
<arg name="max_vel_param" default="/move_base/TrajectoryPlannerROS/max_vel_x"/>
<arg name="min_vel_param" default="/move_base/TrajectoryPlannerROS/min_vel_x"/>
<arg name="min_dist_err" default="0.3"/>
<arg name="min_yaw_err" default="0.3"/>
 
 
<node name="waypoint_nav" pkg="waypoint_nav" type="waypoint_nav" output="screen">
<remap from="/cmd_vel" to="$(arg cmd_vel_topic_out)"/>
<param name="filename" value="$(arg waypoints_file)"/>
<param name="robot_frame" value="$(arg robot_frame)"/>
<param name="filename" value="$(arg waypoints_file)"/>
<param name="StartFromTheMiddle" value="$(arg StartFromTheMiddle)"/>
<param name="robot_frame" value="$(arg robot_frame)"/>
</node>
 
 
<node name="velocity_controller" pkg="waypoint_nav" type="velocity_controller" output="screen">
View
98
■■■■■
 waypoint_nav/src/waypoint_nav.cpp 100644 → 100755
wp_num_.data = 1;
} 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_);
 
ros::NodeHandle nh;
ROS_WARN("Waypoint navigation is already started");
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<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;
}
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<int>::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;
}
//#############################################################
}
 
 
 
response.success = true;
}
return true;
}
 
 
 
/*
++++++++++ Callback function for service of "stop_wp_nav" ++++++++++
double r, p ,y;
m.getRPY(r, p, y);
double diff_yaw = std::abs(y - target_yaw_);
if ((dist < min_dist_err_) && (diff_yaw < min_yaw_err_)) return true;
else if ((dist < min_dist_err_ + 0.3) && navigationFinished()) return true;
else return false;
else if ((dist < min_dist_err_ * 2) && navigationFinished()) return true;
return false;
}
return dist < dist_err;
}
 
*/
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_) {
sleep();
// go to current waypoint
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();
dist_err_ = waypoint_list_[wp_num_.data-1].rad;
try {
// loop until reach waypoint
}
// update current waypoint
current_waypoint_++;
wp_num_.data++;
wp_num_pub_.publish(wp_num_);
 
} catch(const SwitchRunningStatus &e) {
ROS_INFO_STREAM("Running status switched");
}
actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_action_;
geometry_msgs::PoseArray waypoints_;
visualization_msgs::MarkerArray marker_;
std::vector<Waypoint> waypoint_list_;
//Scripts of "start from the middle"
//Edited mori 2022/10/19
//#############################################################
bool StartFromTheMiddle;
std::vector<geometry_msgs::Pose>::iterator init_waypoint_;
std::vector<geometry_msgs::Pose>::iterator compare_waypoint_;
//#############################################################
std::vector<geometry_msgs::Pose>::iterator current_waypoint_;
std::vector<geometry_msgs::Pose>::iterator finish_pose_;
bool has_activate_, stopped_;
std::string robot_frame_, world_frame_;