diff --git a/planner_cspace/cfg/Planner3D.cfg b/planner_cspace/cfg/Planner3D.cfg index 47b7727f..31749447 100755 --- a/planner_cspace/cfg/Planner3D.cfg +++ b/planner_cspace/cfg/Planner3D.cfg @@ -48,5 +48,6 @@ gen.add("fast_map_update", bool_t, 0, "", False) gen.add("max_retry_num", int_t, 0, "", -1, -1, 100) gen.add("keep_a_part_of_previous_path", bool_t, 0, "If true, a part of the previous path is preserved to avoid radical path changes.", False) gen.add("dist_stop_to_previous_path", double_t, 0, "Valid only when keep_a_part_of_previous_path is true. This should be the same as dist_stop parameter of trajectory_tracker.", 0.1, 0.0, 1.0) +gen.add("trigger_plan_by_costmap_update", bool_t, 0, "", False) exit(gen.generate(PACKAGE, "planner_cspace", "Planner3D")) diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp index 7f264b34..1bb22ff5 100644 --- a/planner_cspace/src/planner_3d.cpp +++ b/planner_cspace/src/planner_3d.cpp @@ -174,6 +174,7 @@ class Planner3dNode bool retain_last_error_status_; int num_cost_estim_task_; bool keep_a_part_of_previous_path_; + bool trigger_plan_by_costmap_update_; JumpDetector jump_; std::string robot_frame_; @@ -222,6 +223,7 @@ class Planner3dNode int prev_map_update_y_max_; nav_msgs::Path previous_path_; StartPosePredictor start_pose_predictor_; + ros::Timer no_map_update_timer_; bool cbForget(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& res) @@ -721,12 +723,8 @@ class Planner3dNode previous_path_ = path; } - void cbMapUpdate(const costmap_cspace_msgs::CSpace3DUpdate::ConstPtr msg) + void applyCostmapUpdate(const costmap_cspace_msgs::CSpace3DUpdate::ConstPtr msg) { - if (!has_map_) - return; - ROS_DEBUG("Map updated"); - const auto ts_cm_init_start = boost::chrono::high_resolution_clock::now(); const ros::Time now = ros::Time::now(); @@ -907,6 +905,41 @@ class Planner3dNode 0.0, "second")); publishDebug(); + return; + } + + void cbNoMapUpdateTimer(const ros::TimerEvent& e) + { + planPath(e.current_real); + no_map_update_timer_ = + nh_.createTimer(costmap_watchdog_, &Planner3dNode::cbNoMapUpdateTimer, this, true); + } + void cbMapUpdate(const costmap_cspace_msgs::CSpace3DUpdate::ConstPtr msg) + { + if (!has_map_) + return; + ROS_DEBUG("Map updated"); + if (trigger_plan_by_costmap_update_) + { + no_map_update_timer_.stop(); + updateStart(); + if (jump_.detectJump()) + { + bbf_costmap_.clear(); + // Robot pose jumped. + } + applyCostmapUpdate(msg); + planPath(last_costmap_); + if (costmap_watchdog_ > ros::Duration(0)) + { + no_map_update_timer_ = + nh_.createTimer(costmap_watchdog_, &Planner3dNode::cbNoMapUpdateTimer, this, true); + } + } + else + { + applyCostmapUpdate(msg); + } } void cbMap(const costmap_cspace_msgs::CSpace3D::ConstPtr& msg) { @@ -1363,6 +1396,7 @@ class Planner3dNode sw_wait_ = config.sw_wait; } start_pose_predictor_.setConfig(start_pose_predictor_config); + trigger_plan_by_costmap_update_ = config.trigger_plan_by_costmap_update; } GridAstarModel3D::Vec pathPose2Grid(const geometry_msgs::PoseStamped& pose) const @@ -1418,6 +1452,7 @@ class Planner3dNode { // robot has arrived at the switchback point is_path_switchback_ = false; + return; } } } @@ -1429,201 +1464,211 @@ class Planner3dNode } } - void spin() + void planPath(const ros::Time& now) { - ROS_DEBUG("Initialized"); - - ros::Time next_replan_time = ros::Time::now(); - - while (ros::ok()) + ROS_INFO("replan called"); + if (has_map_ && !cost_estim_cache_created_ && has_goal_) + { + createCostEstimCache(); + } + bool has_costmap(false); + if (costmap_watchdog_ > ros::Duration(0)) { - waitUntil(next_replan_time); + const ros::Duration costmap_delay = now - last_costmap_; + metrics_.data.push_back(neonavigation_metrics_msgs::metric( + "costmap_delay", + costmap_delay.toSec(), + "second")); + if (costmap_delay > costmap_watchdog_) + { + ROS_WARN_THROTTLE(1.0, + "Navigation is stopping since the costmap is too old (costmap: %0.3f)", + last_costmap_.toSec()); + status_.error = planner_cspace_msgs::PlannerStatus::DATA_MISSING; + publishEmptyPath(); + } + else + { + has_costmap = true; + } + } + else + { + metrics_.data.push_back(neonavigation_metrics_msgs::metric( + "costmap_delay", + -1.0, + "second")); + has_costmap = true; + } - const ros::Time now = ros::Time::now(); - next_replan_time = now; + if (has_map_ && has_goal_ && has_start_ && has_costmap) + { + ROS_INFO("Generating path"); + if (act_->isActive()) + { + move_base_msgs::MoveBaseFeedback feedback; + feedback.base_position = start_; + act_->publishFeedback(feedback); + } - if (has_map_ && !cost_estim_cache_created_ && has_goal_) + if (act_tolerant_->isActive()) { - createCostEstimCache(); + planner_cspace_msgs::MoveWithToleranceFeedback feedback; + feedback.base_position = start_; + act_tolerant_->publishFeedback(feedback); } - bool has_costmap(false); - if (costmap_watchdog_ > ros::Duration(0)) + + is_path_switchback_ = false; + if (status_.status == planner_cspace_msgs::PlannerStatus::FINISHING) { - const ros::Duration costmap_delay = now - last_costmap_; - metrics_.data.push_back(neonavigation_metrics_msgs::metric( - "costmap_delay", - costmap_delay.toSec(), - "second")); - if (costmap_delay > costmap_watchdog_) + const float yaw_s = tf2::getYaw(start_.pose.orientation); + float yaw_g = tf2::getYaw(goal_.pose.orientation); + if (force_goal_orientation_) + yaw_g = tf2::getYaw(goal_raw_.pose.orientation); + + float yaw_diff = yaw_s - yaw_g; + if (yaw_diff > M_PI) + yaw_diff -= M_PI * 2.0; + else if (yaw_diff < -M_PI) + yaw_diff += M_PI * 2.0; + if (std::abs(yaw_diff) < + (act_tolerant_->isActive() ? goal_tolerant_->goal_tolerance_ang_finish : goal_tolerance_ang_finish_)) { - ROS_WARN_THROTTLE(1.0, - "Navigation is stopping since the costmap is too old (costmap: %0.3f)", - last_costmap_.toSec()); - status_.error = planner_cspace_msgs::PlannerStatus::DATA_MISSING; - publishEmptyPath(); + status_.status = planner_cspace_msgs::PlannerStatus::DONE; + has_goal_ = false; + // Don't publish empty path here in order a path follower + // to minimize the error to the desired final pose + ROS_INFO("Path plan finished"); + + if (act_->isActive()) + act_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached."); + if (act_tolerant_->isActive()) + act_tolerant_->setSucceeded(planner_cspace_msgs::MoveWithToleranceResult(), "Goal reached."); } else { - has_costmap = true; + publishFinishPath(); } } else { - metrics_.data.push_back(neonavigation_metrics_msgs::metric( - "costmap_delay", - -1.0, - "second")); - has_costmap = true; - } - - bool is_path_switchback = false; - if (has_map_ && has_goal_ && has_start_ && has_costmap) - { - if (act_->isActive()) + bool skip_path_planning = false; + if (escaping_) { - move_base_msgs::MoveBaseFeedback feedback; - feedback.base_position = start_; - act_->publishFeedback(feedback); + status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND; } - - if (act_tolerant_->isActive()) + else if (max_retry_num_ != -1 && cnt_stuck_ > max_retry_num_) { - planner_cspace_msgs::MoveWithToleranceFeedback feedback; - feedback.base_position = start_; - act_tolerant_->publishFeedback(feedback); - } + status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND; + status_.status = planner_cspace_msgs::PlannerStatus::DONE; + has_goal_ = false; - if (status_.status == planner_cspace_msgs::PlannerStatus::FINISHING) + publishEmptyPath(); + // next_replan_time += ros::Duration(1.0 / freq_); + ROS_ERROR("Exceeded max_retry_num:%d", max_retry_num_); + + if (act_->isActive()) + act_->setAborted( + move_base_msgs::MoveBaseResult(), "Goal is in Rock"); + if (act_tolerant_->isActive()) + act_tolerant_->setAborted( + planner_cspace_msgs::MoveWithToleranceResult(), "Goal is in Rock"); + return; + } + else if (!cost_estim_cache_created_) { - const float yaw_s = tf2::getYaw(start_.pose.orientation); - float yaw_g = tf2::getYaw(goal_.pose.orientation); - if (force_goal_orientation_) - yaw_g = tf2::getYaw(goal_raw_.pose.orientation); - - float yaw_diff = yaw_s - yaw_g; - if (yaw_diff > M_PI) - yaw_diff -= M_PI * 2.0; - else if (yaw_diff < -M_PI) - yaw_diff += M_PI * 2.0; - if (std::abs(yaw_diff) < - (act_tolerant_->isActive() ? goal_tolerant_->goal_tolerance_ang_finish : goal_tolerance_ang_finish_)) + skip_path_planning = true; + if (is_start_occupied_) { - status_.status = planner_cspace_msgs::PlannerStatus::DONE; - has_goal_ = false; - // Don't publish empty path here in order a path follower - // to minimize the error to the desired final pose - ROS_INFO("Path plan finished"); - - if (act_->isActive()) - act_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached."); - if (act_tolerant_->isActive()) - act_tolerant_->setSucceeded(planner_cspace_msgs::MoveWithToleranceResult(), "Goal reached."); + status_.error = planner_cspace_msgs::PlannerStatus::IN_ROCK; } else { - publishFinishPath(); + status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND; } } else { - bool skip_path_planning = false; - if (escaping_) - { - status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND; - } - else if (max_retry_num_ != -1 && cnt_stuck_ > max_retry_num_) - { - status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND; - status_.status = planner_cspace_msgs::PlannerStatus::DONE; - has_goal_ = false; - - publishEmptyPath(); - next_replan_time += ros::Duration(1.0 / freq_); - ROS_ERROR("Exceeded max_retry_num:%d", max_retry_num_); - - if (act_->isActive()) - act_->setAborted( - move_base_msgs::MoveBaseResult(), "Goal is in Rock"); - if (act_tolerant_->isActive()) - act_tolerant_->setAborted( - planner_cspace_msgs::MoveWithToleranceResult(), "Goal is in Rock"); - - continue; - } - else if (!cost_estim_cache_created_) - { - skip_path_planning = true; - if (is_start_occupied_) - { - status_.error = planner_cspace_msgs::PlannerStatus::IN_ROCK; - } - else - { - status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND; - } - } - else - { - status_.error = planner_cspace_msgs::PlannerStatus::GOING_WELL; - } + status_.error = planner_cspace_msgs::PlannerStatus::GOING_WELL; + } - if (skip_path_planning) - { - publishEmptyPath(); - } - else + if (skip_path_planning) + { + publishEmptyPath(); + } + else + { + nav_msgs::Path path; + path.header = map_header_; + path.header.stamp = now; + makePlan(start_.pose, goal_.pose, path, true); + publishPath(path); + if ((sw_wait_ > 0.0) && !keep_a_part_of_previous_path_) { - nav_msgs::Path path; - path.header = map_header_; - path.header.stamp = now; - makePlan(start_.pose, goal_.pose, path, true); - publishPath(path); - if ((sw_wait_ > 0.0) && !keep_a_part_of_previous_path_) - { - const int sw_index = getSwitchIndex(path); - is_path_switchback = (sw_index >= 0); - if (is_path_switchback) - sw_pos_ = path.poses[sw_index]; - } + const int sw_index = getSwitchIndex(path); + is_path_switchback_ = (sw_index >= 0); + if (is_path_switchback_) + sw_pos_ = path.poses[sw_index]; } } } - else if (!has_goal_) - { - if (!retain_last_error_status_) - status_.error = planner_cspace_msgs::PlannerStatus::GOING_WELL; - publishEmptyPath(); - } - status_.header.stamp = now; - pub_status_.publish(status_); - diag_updater_.force_update(); + } + else if (!has_goal_) + { + ROS_INFO("No goal"); + if (!retain_last_error_status_) + status_.error = planner_cspace_msgs::PlannerStatus::GOING_WELL; + publishEmptyPath(); + } + status_.header.stamp = now; + pub_status_.publish(status_); + diag_updater_.force_update(); - metrics_.header.stamp = now; - metrics_.data.push_back(neonavigation_metrics_msgs::metric( - "stuck_cnt", - cnt_stuck_, - "count")); - metrics_.data.push_back(neonavigation_metrics_msgs::metric( - "error", - status_.error, - "enum")); - metrics_.data.push_back(neonavigation_metrics_msgs::metric( - "status", - status_.status, - "enum")); - pub_metrics_.publish(metrics_); - metrics_.data.clear(); + metrics_.header.stamp = now; + metrics_.data.push_back(neonavigation_metrics_msgs::metric( + "stuck_cnt", + cnt_stuck_, + "count")); + metrics_.data.push_back(neonavigation_metrics_msgs::metric( + "error", + status_.error, + "enum")); + metrics_.data.push_back(neonavigation_metrics_msgs::metric( + "status", + status_.status, + "enum")); + pub_metrics_.publish(metrics_); + metrics_.data.clear(); + } + + void spin() + { + ROS_DEBUG("Initialized"); - if (is_path_switchback) + ros::Time next_replan_time = ros::Time::now(); + ros::Rate r(100); + while (ros::ok()) + { + if (trigger_plan_by_costmap_update_) { - next_replan_time += ros::Duration(sw_wait_); - ROS_INFO("Planned path has switchback. Planner will stop until: %f at the latest.", next_replan_time.toSec()); + ros::spinOnce(); + r.sleep(); } else { - next_replan_time += ros::Duration(1.0 / freq_); + waitUntil(next_replan_time); + const ros::Time now = ros::Time::now(); + planPath(now); + if (is_path_switchback_) + { + next_replan_time = now + ros::Duration(sw_wait_); + ROS_INFO("Planned path has switchback. Planner will stop until: %f at the latest.", next_replan_time.toSec()); + } + else + { + next_replan_time = now + ros::Duration(1.0 / freq_); + } } - is_path_switchback_ = is_path_switchback; } } diff --git a/planner_cspace/test/src/test_dynamic_parameter_change.cpp b/planner_cspace/test/src/test_dynamic_parameter_change.cpp index aed50e65..a8ae1f73 100644 --- a/planner_cspace/test/src/test_dynamic_parameter_change.cpp +++ b/planner_cspace/test/src/test_dynamic_parameter_change.cpp @@ -87,6 +87,8 @@ class DynamicParameterChangeTest void cbPath(const nav_msgs::Path::ConstPtr& msg) { path_ = msg; + ++path_received_count_; + last_path_received_time_ = ros::Time::now(); } move_base_msgs::MoveBaseGoal CreateGoalInFree() @@ -191,6 +193,47 @@ class DynamicParameterChangeTest pub_map_overlay_.publish(map_overlay_); } + double getPathHz(const ros::Duration costmap_publishing_interval) + { + publishMapAndRobot(2.55, 0.45, M_PI); + ros::Duration(0.3).sleep(); + move_base_->sendGoal(CreateGoalInFree()); + while (ros::ok() && (move_base_->getState() != actionlib::SimpleClientGoalState::ACTIVE)) + { + ros::spinOnce(); + } + + last_path_received_time_ = ros::Time(); + publishMapAndRobot(2.55, 0.45, M_PI); + ros::Time last_costmap_publishing_time = ros::Time::now(); + ros::Rate r(100); + while (ros::ok() && (last_path_received_time_ == ros::Time())) + { + if ((ros::Time::now() - last_costmap_publishing_time) > costmap_publishing_interval) + { + publishMapAndRobot(2.55, 0.45, M_PI); + last_costmap_publishing_time = ros::Time::now(); + }; + ros::spinOnce(); + r.sleep(); + } + const ros::Time initial_path_received_time_ = last_path_received_time_; + const int prev_path_received_count = path_received_count_; + while (ros::ok() && (path_received_count_ < prev_path_received_count + 10)) + { + if ((ros::Time::now() - last_costmap_publishing_time) > costmap_publishing_interval) + { + publishMapAndRobot(2.55, 0.45, M_PI); + last_costmap_publishing_time = ros::Time::now(); + } + ros::spinOnce(); + r.sleep(); + } + const double elapesed_time_sec = (last_path_received_time_ - initial_path_received_time_).toSec(); + const double hz = (path_received_count_ - prev_path_received_count) / elapesed_time_sec; + return hz; + } + tf2_ros::TransformBroadcaster tfb_; ros::Subscriber sub_path_; nav_msgs::Path::ConstPtr path_; @@ -199,6 +242,8 @@ class DynamicParameterChangeTest ros::Publisher pub_odom_; nav_msgs::OccupancyGrid map_overlay_; planner_cspace::Planner3DConfig default_config_; + int path_received_count_; + ros::Time last_path_received_time_; }; TEST_F(DynamicParameterChangeTest, DisableCurves) @@ -268,6 +313,30 @@ TEST_F(DynamicParameterChangeTest, StartPosePrediction) EXPECT_TRUE(comparePath(short_path, *path_)); } +TEST_F(DynamicParameterChangeTest, TriggerPlanByCostmapUpdate) +{ + publishMapAndRobot(2.55, 0.45, M_PI); + ros::Duration(0.5).sleep(); + sendGoalAndWaitForPath(); + + const double hz = getPathHz(ros::Duration(0.1)); + EXPECT_LT(3.0, hz); + EXPECT_LT(hz, 5.0); + + planner_cspace::Planner3DConfig config = default_config_; + config.trigger_plan_by_costmap_update = true; + config.costmap_watchdog = 0.5; + ASSERT_TRUE(planner_3d_client_->setConfiguration(config)); + + const double hz2 = getPathHz(ros::Duration(0.1)); + EXPECT_LT(9.0, hz2); + EXPECT_LT(hz2, 10.0); + + const double hz3 = getPathHz(ros::Duration(100)); + EXPECT_LT(1.0, hz3); + EXPECT_LT(hz3, 3.0); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv);