diff --git a/src/tsukuba2022/config/waypoints/waypoints.yaml b/src/tsukuba2022/config/waypoints/waypoints.yaml
index ddac05b..33c7c83 100644
--- a/src/tsukuba2022/config/waypoints/waypoints.yaml
+++ b/src/tsukuba2022/config/waypoints/waypoints.yaml
@@ -1,19 +1,23 @@
waypoints:
-- point: {x: 3.82361, y: 0.164189, z: 0.00511169, vel: 1, rad: 1, stop: false}
-- point: {x: 11.1774, y: 0.0853908, z: -0.00192642, vel: 1, rad: 1, stop: false}
-- point: {x: 15.6239, y: 0.174851, z: 0.00251579, vel: 1, rad: 1, stop: false}
-- point: {x: 15.7556, y: 8.48258, z: 0.00242996, vel: 1, rad: 1, stop: false}
-- point: {x: 15.8543, y: 16.8748, z: 0.0023098, vel: 1, rad: 1, stop: false}
+- point: {x: 3.876438, y: 0.067258, z: 0.00511169, vel: 1, rad: 1, stop: false}
+- point: {x: 10.24828, y: 0.067258, z: -0.00192642, vel: 1, rad: 1, stop: false}
+- point: {x: 14.343374, y: 0.067258, z: 0.00251579, vel: 1, rad: 1, stop: false}
+- point: {x: 15.695671, y: 1.465396, z: 0.0, vel: 1.0, rad: 1.0, stop: false}
+- point: {x: 15.731122, y: 6.432358, z: 0.00242996, vel: 1, rad: 1, stop: false}
+- point: {x: 15.770729, y: 11.224717, z: 0.0, vel: 1, rad: 1, stop: false}
+- point: {x: 15.840833, y: 15.530818, z: 0.0023098, vel: 1, rad: 1, stop: false}
+- point: {x: 14.381574, y: 17.020638, z: 0.0, vel: 1, rad: 1, stop: false}
- point: {x: 9.36751, y: 17.0985, z: 0.00177956, vel: 1, rad: 1, stop: false}
-- point: {x: 2.68957, y: 17.1423, z: 0.000879288, vel: 1, rad: 1, stop: false}
+- point: {x: 4.640448, y: 17.165799, z: 0.000879288, vel: 1, rad: 1, stop: false}
+- point: {x: 3.318711, y: 15.828782, z: 0.0, vel: 1, rad: 1, stop: true}
- point: {x: 4.72936, y: 13.403, z: 0.00301743, vel: 1, rad: 1, stop: false}
- point: {x: 6.72777, y: 10.9204, z: 0.00507927, vel: 1, rad: 1, stop: false}
-- point: {x: 7.85304, y: 8.43392, z: 0.00030899, vel: 1, rad: 1, stop: false}
+- point: {x: 8.047505, y: 8.46548, z: 0.00030899, vel: 1, rad: 1, stop: false}
- point: {x: 9.32663, y: 6.47157, z: 0.00183296, vel: 1, rad: 1, stop: false}
-- point: {x: 11.2611, y: 3.52007, z: 0.00384521, vel: 1, rad: 1, stop: false}
-- point: {x: 11.2283, y: 0.737249, z: 0.00480747, vel: 1, rad: 1, stop: false}
+- point: {x: 11.210933, y: 3.520583, z: 0.00384521, vel: 1, rad: 1, stop: false}
+- point: {x: 10.872753, y: 0.465013, z: 0.00480747, vel: 1, rad: 1, stop: false}
finish_pose:
- header: {seq: 0, stamp: 113.132000000, frame_id: map}
- pose:
+ header: {seq: 0, stamp: 113.132, frame_id: map}
+ pose:
position: {x: 9.27349, y: 0.143879, z: 0}
- orientation: {x: 0, y: 0, z: 0.999983, w: 0.00578372}
+ orientation: {x: 0, y: 0, z: 0.999983, w: 0.00578372}
\ No newline at end of file
diff --git a/src/tsukuba2022/config/waypoints/waypoints_sim_nakaniwa.yaml b/src/tsukuba2022/config/waypoints/waypoints_sim_nakaniwa.yaml
index ddac05b..33c7c83 100644
--- a/src/tsukuba2022/config/waypoints/waypoints_sim_nakaniwa.yaml
+++ b/src/tsukuba2022/config/waypoints/waypoints_sim_nakaniwa.yaml
@@ -1,19 +1,23 @@
waypoints:
-- point: {x: 3.82361, y: 0.164189, z: 0.00511169, vel: 1, rad: 1, stop: false}
-- point: {x: 11.1774, y: 0.0853908, z: -0.00192642, vel: 1, rad: 1, stop: false}
-- point: {x: 15.6239, y: 0.174851, z: 0.00251579, vel: 1, rad: 1, stop: false}
-- point: {x: 15.7556, y: 8.48258, z: 0.00242996, vel: 1, rad: 1, stop: false}
-- point: {x: 15.8543, y: 16.8748, z: 0.0023098, vel: 1, rad: 1, stop: false}
+- point: {x: 3.876438, y: 0.067258, z: 0.00511169, vel: 1, rad: 1, stop: false}
+- point: {x: 10.24828, y: 0.067258, z: -0.00192642, vel: 1, rad: 1, stop: false}
+- point: {x: 14.343374, y: 0.067258, z: 0.00251579, vel: 1, rad: 1, stop: false}
+- point: {x: 15.695671, y: 1.465396, z: 0.0, vel: 1.0, rad: 1.0, stop: false}
+- point: {x: 15.731122, y: 6.432358, z: 0.00242996, vel: 1, rad: 1, stop: false}
+- point: {x: 15.770729, y: 11.224717, z: 0.0, vel: 1, rad: 1, stop: false}
+- point: {x: 15.840833, y: 15.530818, z: 0.0023098, vel: 1, rad: 1, stop: false}
+- point: {x: 14.381574, y: 17.020638, z: 0.0, vel: 1, rad: 1, stop: false}
- point: {x: 9.36751, y: 17.0985, z: 0.00177956, vel: 1, rad: 1, stop: false}
-- point: {x: 2.68957, y: 17.1423, z: 0.000879288, vel: 1, rad: 1, stop: false}
+- point: {x: 4.640448, y: 17.165799, z: 0.000879288, vel: 1, rad: 1, stop: false}
+- point: {x: 3.318711, y: 15.828782, z: 0.0, vel: 1, rad: 1, stop: true}
- point: {x: 4.72936, y: 13.403, z: 0.00301743, vel: 1, rad: 1, stop: false}
- point: {x: 6.72777, y: 10.9204, z: 0.00507927, vel: 1, rad: 1, stop: false}
-- point: {x: 7.85304, y: 8.43392, z: 0.00030899, vel: 1, rad: 1, stop: false}
+- point: {x: 8.047505, y: 8.46548, z: 0.00030899, vel: 1, rad: 1, stop: false}
- point: {x: 9.32663, y: 6.47157, z: 0.00183296, vel: 1, rad: 1, stop: false}
-- point: {x: 11.2611, y: 3.52007, z: 0.00384521, vel: 1, rad: 1, stop: false}
-- point: {x: 11.2283, y: 0.737249, z: 0.00480747, vel: 1, rad: 1, stop: false}
+- point: {x: 11.210933, y: 3.520583, z: 0.00384521, vel: 1, rad: 1, stop: false}
+- point: {x: 10.872753, y: 0.465013, z: 0.00480747, vel: 1, rad: 1, stop: false}
finish_pose:
- header: {seq: 0, stamp: 113.132000000, frame_id: map}
- pose:
+ header: {seq: 0, stamp: 113.132, frame_id: map}
+ pose:
position: {x: 9.27349, y: 0.143879, z: 0}
- orientation: {x: 0, y: 0, z: 0.999983, w: 0.00578372}
+ orientation: {x: 0, y: 0, z: 0.999983, w: 0.00578372}
\ No newline at end of file
diff --git a/src/tsukuba2022/launch/teb_local_planner_test.launch b/src/tsukuba2022/launch/teb_local_planner_test.launch
index 27664e6..93c2543 100644
--- a/src/tsukuba2022/launch/teb_local_planner_test.launch
+++ b/src/tsukuba2022/launch/teb_local_planner_test.launch
@@ -19,6 +19,7 @@
+
@@ -39,6 +40,7 @@
+
diff --git a/src/tsukuba2022/params/foot_print.yaml b/src/tsukuba2022/params/foot_print.yaml
index 9b61904..9f9bd76 100644
--- a/src/tsukuba2022/params/foot_print.yaml
+++ b/src/tsukuba2022/params/foot_print.yaml
@@ -1 +1 @@
-footprint: [ [0.25, 0.45], [0.25, -0.45], [-0.65, -0.45], [-0.65, 0.45] ]
+footprint: [ [0.25, 0.4], [0.25, -0.4], [-0.65, -0.4], [-0.65, 0.4] ]
diff --git a/src/tsukuba2022/params/global_costmap_params.yaml b/src/tsukuba2022/params/global_costmap_params.yaml
index 79eb8ec..69534a3 100644
--- a/src/tsukuba2022/params/global_costmap_params.yaml
+++ b/src/tsukuba2022/params/global_costmap_params.yaml
@@ -20,5 +20,5 @@
track_unknown_space: false
inflater_layer:
- inflation_radius: 1.5
- cost_scaling_factor: 2.0
+ inflation_radius: 2.0
+ cost_scaling_factor: 5.0
diff --git a/src/tsukuba2022/params/local_costmap_params.yaml b/src/tsukuba2022/params/local_costmap_params.yaml
index dae69df..2812066 100644
--- a/src/tsukuba2022/params/local_costmap_params.yaml
+++ b/src/tsukuba2022/params/local_costmap_params.yaml
@@ -4,9 +4,9 @@
update_frequency: 5.0
publish_frequency: 5.0
rolling_window: true
- resolution: 0.1
- width: 10.0
- height: 10.0
+ resolution: 0.05
+ width: 8
+ height: 8
always_send_full_costmap: true
transform_tolerance: 1.0
@@ -15,7 +15,7 @@
- {name: inflater_layer, type: "costmap_2d::InflationLayer"}
obstacle_layer:
- observation_sources: scan
+ observation_sources: scan
scan:
data_type: LaserScan
topic: /scan
@@ -30,7 +30,7 @@
observation_persistence: 0.0
inflater_layer:
- inflation_radius: 0.2
- cost_scaling_factor: 3.0
+ inflation_radius: 0.4
+ cost_scaling_factor: 5.0
# https://answers.ros.org/question/326867/local_costmap-not-showing-every-obstacle/
diff --git a/src/tsukuba2022/params/localization_params.yaml b/src/tsukuba2022/params/localization_params.yaml
index 58cc5f4..89604bb 100644
--- a/src/tsukuba2022/params/localization_params.yaml
+++ b/src/tsukuba2022/params/localization_params.yaml
@@ -1,15 +1,15 @@
odom_model_type: diff
-odom_alpha5: 0.1
transform_tolerance: 0.5 #0.2
gui_publish_rate: 10.0
laser_max_beams: 30
min_particles: 100
-max_particles: 5000
+max_particles: 4000
kld_err: 0.05
kld_z: 0.99
odom_alpha1: 0.2
odom_alpha2: 0.2
-odom_alpha4: 0.4
+odom_alpha3: 0.2
+odom_alpha4: 0.2
laser_z_hit: 0.5
laser_z_short: 0.05
laser_z_max: 0.05
@@ -21,6 +21,7 @@
laser_likelihood_max_dist: 2.0
update_min_d: 0.2
update_min_a: 0.5
+global_frame_id: map
odom_frame_id: odom
base_frame_od: base_footprint
resample_interval: 1
diff --git a/src/tsukuba2022/params/teb_local_planner_params.yaml b/src/tsukuba2022/params/teb_local_planner_params.yaml
index 3198655..e0531bc 100644
--- a/src/tsukuba2022/params/teb_local_planner_params.yaml
+++ b/src/tsukuba2022/params/teb_local_planner_params.yaml
@@ -7,29 +7,25 @@
# Trajectory
teb_autosize: True
- dt_ref: 0.5 #0.3
+ dt_ref: 0.5 #0.3 軌道の解像度[s]
dt_hysteresis: 0.1
global_plan_overwrite_orientation: True
- max_global_plan_lookahead_dist: 2.0
+ max_global_plan_lookahead_dist: 2.0 #仮想的なローカルゴールまでの距離
feasibility_check_no_poses: 5
# Robot
- max_vel_x: 1.6
+ max_vel_x: 1.2
max_vel_x_backwards: 0.2
- max_vel_theta: 0.4
+ max_vel_theta: 1.0
acc_lim_x: 2.0
acc_lim_theta: 1.0
min_turning_radius: 0.0
footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
- type: "polygon"
- # radius: 0.2 # for type "circular"
- # line_start: [-0.3, 0.0] # for type "line"
- # line_end: [0.3, 0.0] # for type "line"
- # front_offset: 0.2 # for type "two_circles"
- # front_radius: 0.2 # for type "two_circles"
- # rear_offset: 0.2 # for type "two_circles"
- # rear_radius: 0.2 # for type "two_circles"
- vertices: [ [0.25, 0.4], [0.25, -0.4], [-0.65, -0.4], [-0.65, 0.4] ] # for type "polygon"
+ # type: "polygon"
+ # vertices: [ [0.25, 0.4], [0.25, -0.4], [-0.65, -0.4], [-0.65, 0.4] ]
+ type: "line"
+ line_start: [-0.09, 0.0]
+ line_end: [-0.31, 0.0]
# GoalTolerance
xy_goal_tolerance: 0.3
@@ -38,41 +34,57 @@
# Obstacles
min_obstacle_dist: 0.4
- inflation_dist: 0.0 #0.6
- include_costmap_obstacles: True
- costmap_obstacles_behind_robot_dist: 1.0
- obstacle_poses_affected: 30
+ inflation_dist: 0.5 #0.6
+ include_dynamic_obstacles: False
+ include_costmap_obstacles: True # False -> 障害物割と無視
+ costmap_obstacles_behind_robot_dist: 0.5 #1.0
+ obstacle_poses_affected: 30 #30 # 障害物に対して考慮する軌道の数？
costmap_converter_plugin: ""
costmap_converter_spin_thread: True
costmap_converter_rate: 5
# Optimization
- no_inner_iterations: 4 #5
- no_outer_iterations: 3 #4
+ no_inner_iterations: 4 #5 反復計算の回数？
+ no_outer_iterations: 3 #4 no_inner_iterations x no_outer_iterations 回だけ計算をする？
optimization_activate: True
optimization_verbose: False
penalty_epsilon: 0.1
weight_max_vel_x: 1000 #2
weight_max_vel_theta: 1000 #1
- weight_acc_lim_x: 100 #1
- weight_acc_lim_theta: 100 #1
+ weight_acc_lim_x: 1 #100 #1
+ weight_acc_lim_theta: 1 #1
weight_kinematics_nh: 1000
weight_kinematics_forward_drive: 100 #1
weight_kinematics_turning_radius: 1
weight_optimaltime: 1
- weight_obstacle: 50
+ weight_shortest_path: 40
+ weight_obstacle: 40
+ weight_inflation: 0.1
weight_dynamic_obstacle: 10 # not in use yet
selection_alternative_time_cost: False # not in use yet
# Homotopy Class Planner
- enable_homotopy_class_planning: True
- enable_multithreading: True
+ enable_homotopy_class_planning: False #True #軌道の複数代替案を計算するかどうか？
+ enable_multithreading: False #True
simple_exploration: False
- max_number_classes: 3 #4
+ max_number_classes: 2 #4 軌道の代替案の計算個数
roadmap_graph_no_samples: 15
roadmap_graph_area_width: 5
h_signature_prescaler: 0.5
h_signature_threshold: 0.1
obstacle_keypoint_offset: 0.1
obstacle_heading_threshold: 0.45
- visualize_hc_graph: False
\ No newline at end of file
+ visualize_hc_graph: False
+
+
+ # https://mowito-navstack.readthedocs.io/en/latest/step_5c.html
+ # 処理を速くするために
+ # ・costmap_obstacles_behind_robot_dist -> 小さく
+ # ・obstacle_poses_affected -> 小さく
+ # ・footprint_model -> polygonを避ける
+ # ・dt_ref -> 大きく
+ # ・max_global_plan_lookahead_dist -> 小さく
+ # ・no_inner_iterations, no_outer_iterations -> 小さく
+ # ・weight_acc_lim -> 0.0に
+ # ・enable_homotopy_class_planning -> False ※影響大
+ # ・max_number_classes -> 小さく（2で十分？）
\ No newline at end of file
diff --git a/src/tsukuba2022/rviz_cfg/teb_nav.rviz b/src/tsukuba2022/rviz_cfg/teb_nav.rviz
index 2f214d7..a62c8a7 100644
--- a/src/tsukuba2022/rviz_cfg/teb_nav.rviz
+++ b/src/tsukuba2022/rviz_cfg/teb_nav.rviz
@@ -190,7 +190,7 @@
Topic: /move_base/TebLocalPlannerROS/global_plan
Unreliable: false
Value: true
- - Alpha: 0.5
+ - Alpha: 0.30000001192092896
Buffer Length: 1
Class: rviz/Path
Color: 255; 255; 0
@@ -199,7 +199,7 @@
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Billboards
- Line Width: 0.10000000149011612
+ Line Width: 0.07000000029802322
Name: Local Plan
Offset:
X: 0
@@ -296,16 +296,15 @@
Namespaces:
PointObstacles: true
RobotFootprintModel: true
- TebContainer: true
Queue Size: 100
Value: true
- Alpha: 1
- Arrow Length: 0.30000001192092896
+ Arrow Length: 0.5
Axes Length: 0.30000001192092896
Axes Radius: 0.009999999776482582
Class: rviz/PoseArray
- Color: 0; 0; 255
- Enabled: true
+ Color: 85; 0; 255
+ Enabled: false
Head Length: 0.07000000029802322
Head Radius: 0.029999999329447746
Name: Teb Poses
@@ -315,7 +314,7 @@
Shape: Arrow (Flat)
Topic: /move_base/TebLocalPlannerROS/teb_poses
Unreliable: false
- Value: true
+ Value: false
Enabled: true
Global Options:
Background Color: 48; 48; 48
@@ -344,7 +343,7 @@
Views:
Current:
Class: rviz/ThirdPersonFollower
- Distance: 29.42242431640625
+ Distance: 12.89369010925293
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
@@ -382,5 +381,5 @@
Views:
collapsed: true
Width: 1282
- X: 218
- Y: 21
+ X: 217
+ Y: 1
diff --git a/src/tsukuba2022/urdf/orange2022.gazebo b/src/tsukuba2022/urdf/orange2022.gazebo
index ae2b002..5bd5d41 100644
--- a/src/tsukuba2022/urdf/orange2022.gazebo
+++ b/src/tsukuba2022/urdf/orange2022.gazebo
@@ -20,10 +20,10 @@
30
left_wheel_hinge
right_wheel_hinge
- 0.670
- 0.192
- 1
- 10
+ 0.5672
+ 0.203
+ 2
+ 20
na
false
@@ -37,7 +37,7 @@
1000000.0
100.0
0.001
- 1.0
+ 3.0
@@ -47,7 +47,7 @@
1000000.0
100.0
0.001
- 1.0
+ 3.0
diff --git a/src/waypoint_navigation/.gitignore b/src/waypoint_navigation/.gitignore
new file mode 100644
index 0000000..dbe9c82
--- /dev/null
+++ b/src/waypoint_navigation/.gitignore
@@ -0,0 +1 @@
+.vscode/
\ No newline at end of file
diff --git a/src/waypoint_navigation/waypoint_nav/launch/waypoint_nav.launch b/src/waypoint_navigation/waypoint_nav/launch/waypoint_nav.launch
index eaa8101..e7b5ece 100644
--- a/src/waypoint_navigation/waypoint_nav/launch/waypoint_nav.launch
+++ b/src/waypoint_navigation/waypoint_nav/launch/waypoint_nav.launch
@@ -4,7 +4,8 @@
-
+
@@ -17,6 +18,7 @@
-
+
diff --git a/src/waypoint_navigation/waypoint_nav/src/velocity_controller.cpp b/src/waypoint_navigation/waypoint_nav/src/velocity_controller.cpp
index d064beb..53735d7 100644
--- a/src/waypoint_navigation/waypoint_nav/src/velocity_controller.cpp
+++ b/src/waypoint_navigation/waypoint_nav/src/velocity_controller.cpp
@@ -19,21 +19,19 @@
ROS_INFO("Waiting...");
}
+ ros::NodeHandle private_nh("~");
+ std::string max_vel_param = "/move_base/TrajectoryPlannerROS/max_vel_x";
+ std::string min_vel_param = "/move_base/TrajectoryPlannerROS/min_vel_x";
+ private_nh.param("max_vel_param", max_vel_param, max_vel_param);
+ private_nh.param("min_vel_param", min_vel_param, min_vel_param);
+
ros::NodeHandle nh;
max_vel_sub_ = nh.subscribe("/max_vel", 1, &VelocityController::maxVelCallback, this);
cmd_vel_sub_ = nh.subscribe("/cmd_vel_in", 10, &VelocityController::cmdVelCallback, this);
cmd_vel_pub_ = nh.advertise("/cmd_vel_out", 10);
- std::string planner = "base_local_planner/TrajectoryPlannerROS";
- std::string param_max = "/move_base/TrajectoryPlannerROS/max_vel_x";
- std::string param_min = "/move_base/TrajectoryPlannerROS/min_vel_x";
- nh.param("/move_base/base_local_planner", planner, planner);
- if (planner == "dwa_local_planner/DWAPlannerROS") {
- param_max = "/move_base/DWAPlannerROS/max_vel_trans";
- param_min = "/move_base/DWAPlannerROS/min_vel_trans";
- }
- nh.param(param_max, standard_vel_, standard_vel_);
- nh.param(param_min, min_vel_, min_vel_);
+ nh.param(max_vel_param, standard_vel_, standard_vel_);
+ nh.param(min_vel_param, min_vel_, min_vel_);
ROS_INFO_STREAM("Standard max vel: " << standard_vel_);
}
diff --git a/src/waypoint_navigation/waypoint_nav/src/waypoint_nav.cpp b/src/waypoint_navigation/waypoint_nav/src/waypoint_nav.cpp
index d251054..cb1f6e8 100644
--- a/src/waypoint_navigation/waypoint_nav/src/waypoint_nav.cpp
+++ b/src/waypoint_navigation/waypoint_nav/src/waypoint_nav.cpp
@@ -7,15 +7,12 @@
#include
#include
#include
+#include
#include
#include
#include
#include
-// #include
-// #include
-// #include
-
#include
#include // include Waypoint class
@@ -48,6 +45,7 @@
*/
WaypointsNavigation() :
has_activate_(false),
+ stopped_(false),
move_base_action_("move_base", true),
rate_(10),
last_moved_time_(0),
@@ -250,7 +248,7 @@
/*
- ++++++++++ Callback function for cmd_vel topic ++++++++++
+ ++++++++++ Callback function for /cmd_vel topic ++++++++++
*/
void cmdVelCallback(const geometry_msgs::Twist &msg)
{
@@ -283,7 +281,7 @@
/*
++++++++++ Check if robot reached current waypoint ++++++++++
*/
- bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err = 0.8)
+ bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err=1.0)
{
if (waypoint_list_[wp_num_.data-1].stop) {
return navigationFinished();
@@ -371,6 +369,7 @@
*/
void run()
{
+ ROS_INFO("Waiting for waypoint navigation to start.");
while(ros::ok()) {
// has_activate_ is false, nothing to do
if (!has_activate_) {
@@ -396,8 +395,10 @@
try {
// loop until reach waypoint
while(!onNavigationPoint(current_waypoint_->position, dist_err_)) {
- if (!has_activate_) {
- throw SwitchRunningStatus();
+ if (!has_activate_) throw SwitchRunningStatus();
+ if ((!waypoint_list_[wp_num_.data-1].stop) && (navigationFinished())) {
+ ROS_WARN("Reached default yaw_goal_torelance.");
+ break;
}
double time = ros::Time::now().toSec();
if (time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0) {
@@ -408,9 +409,7 @@
resend_goal++;
if (resend_goal == 3) {
ROS_WARN("Skip waypoint.");
- current_waypoint_++;
- wp_num_.data++;
- startNavigationGL(*current_waypoint_);
+ break;
}
start_nav_time = time;
}
@@ -444,12 +443,12 @@
std::vector waypoint_list_;
std::vector::iterator current_waypoint_;
std::vector::iterator finish_pose_;
- bool has_activate_;
+ bool has_activate_, stopped_;
std::string robot_frame_, world_frame_;
tf::TransformListener tf_listener_;
ros::Rate rate_;
ros::ServiceServer start_server_, stop_server_, resume_server_;
- ros::Subscriber cmd_vel_sub_;
+ ros::Subscriber cmd_vel_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_;