This commit is contained in:
Ryoo
2020-09-21 14:24:59 +08:00
parent babfa8ff0c
commit dcc2e0b749

View File

@@ -195,6 +195,7 @@ namespace navi_multi_goals_pub_rviz_plugin {
// start to navigate, and only command the first goal
void MultiNaviGoalsPanel::startNavi() {
curGoalIdx_ = curGoalIdx_ % pose_array_.poses.size();
if (!pose_array_.poses.empty() && curGoalIdx_ < maxNumGoal_) {
geometry_msgs::PoseStamped goal;
goal.header = pose_array_.header;