From cc755cacac7acafdea0eda7c79b00ab7eda65b9d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Andr=C3=A9s=20=C3=81lvarez=20Restrepo?= Date: Thu, 9 Jun 2022 12:23:58 -0500 Subject: [PATCH] Feature:start stop (#18) * Add topic for stop/resume exploration + docs * Clang format --- README.md | 9 +++-- explore/config/params.yaml | 2 +- explore/config/params_costmap.yaml | 2 +- explore/include/explore/explore.h | 6 ++++ explore/src/costmap_client.cpp | 2 +- explore/src/explore.cpp | 56 +++++++++++++++++++++++------- 6 files changed, 60 insertions(+), 17 deletions(-) diff --git a/README.md b/README.md index eb9702c..ff101bf 100644 --- a/README.md +++ b/README.md @@ -49,7 +49,7 @@ Then just run the nav2 stack with slam: ``` export TURTLEBOT3_MODEL=waffle export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/${ROS_DISTRO}/share/turtlebot3_gazebo/models -ros2 launch nav2_bringup tb3_simulation_launch.py slam:=true +ros2 launch nav2_bringup tb3_simulation_launch.py slam:=True ``` And run this package with @@ -57,7 +57,12 @@ And run this package with ros2 launch explore_lite explore.launch.py ``` -You can open a rviz2 and add the exploration frontiers marker to see the algorithm working and choose a frontier to explore. +You can open rviz2 and add the exploration frontiers marker (topic is `explore/frontiers`) to see the algorithm working and the frontier chosen to explore. + +### Additional features +#### Stop/Resume exploration +By default the exploration node will start right away the frontier-based exploration algorithm. Alternatively, you can stop the exploration by publishing to a `False` to `explore/resume` topic. This will stop the exploration and the robot will stop moving. You can resume the exploration by publishing to `True` to `explore/resume`. + #### TB3 troubleshooting (with foxy) If you have trouble with TB3 in simulation, as we did, add these extra steps for configuring it. diff --git a/explore/config/params.yaml b/explore/config/params.yaml index 2b3b1d2..f51c5b6 100644 --- a/explore/config/params.yaml +++ b/explore/config/params.yaml @@ -4,7 +4,7 @@ costmap_topic: map costmap_updates_topic: map_updates visualize: true - planner_frequency: 0.25 + planner_frequency: 0.15 progress_timeout: 30.0 potential_scale: 3.0 orientation_scale: 0.0 diff --git a/explore/config/params_costmap.yaml b/explore/config/params_costmap.yaml index 515d622..fda4fbd 100644 --- a/explore/config/params_costmap.yaml +++ b/explore/config/params_costmap.yaml @@ -4,7 +4,7 @@ explore_node: costmap_topic: /global_costmap/costmap costmap_updates_topic: /global_costmap/costmap_updates visualize: true - planner_frequency: 0.25 + planner_frequency: 0.15 progress_timeout: 30.0 potential_scale: 3.0 orientation_scale: 0.0 diff --git a/explore/include/explore/explore.h b/explore/include/explore/explore.h index fc6665c..30f96a3 100644 --- a/explore/include/explore/explore.h +++ b/explore/include/explore/explore.h @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -80,6 +81,7 @@ public: void start(); void stop(); + void resume(); using NavigationGoalHandle = rclcpp_action::ClientGoalHandle; @@ -118,6 +120,10 @@ private: rclcpp::TimerBase::SharedPtr exploring_timer_; // rclcpp::TimerBase::SharedPtr oneshot_; + rclcpp::Subscription::SharedPtr resume_subscription_; + + void resumeCallback(const std_msgs::msg::Bool::SharedPtr msg); + std::vector frontier_blacklist_; geometry_msgs::msg::Point prev_goal_; double prev_distance_; diff --git a/explore/src/costmap_client.cpp b/explore/src/costmap_client.cpp index 9f632ab..ae2da83 100644 --- a/explore/src/costmap_client.cpp +++ b/explore/src/costmap_client.cpp @@ -36,11 +36,11 @@ *********************************************************************/ #include +#include #include #include #include -#include namespace explore { diff --git a/explore/src/explore.cpp b/explore/src/explore.cpp index d646f9c..9398949 100644 --- a/explore/src/explore.cpp +++ b/explore/src/explore.cpp @@ -69,7 +69,8 @@ Explore::Explore() this->get_parameter("min_frontier_size", min_frontier_size); progress_timeout_ = timeout; move_base_client_ = - rclcpp_action::create_client(this,ACTION_NAME); + rclcpp_action::create_client( + this, ACTION_NAME); search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(), potential_scale_, gain_scale_, @@ -77,11 +78,17 @@ Explore::Explore() if (visualize_) { marker_array_publisher_ = - this->create_publisher("frontier" + this->create_publisher("explore/" + "frontier" "s", 10); } + // Subscription to resume or stop exploration + resume_subscription_ = this->create_subscription( + "explore/resume", 10, + std::bind(&Explore::resumeCallback, this, std::placeholders::_1)); + RCLCPP_INFO(logger_, "Waiting to connect to move_base nav2 server"); move_base_client_->wait_for_action_server(); RCLCPP_INFO(logger_, "Connected to move_base nav2 server"); @@ -89,6 +96,8 @@ Explore::Explore() exploring_timer_ = this->create_wall_timer( std::chrono::milliseconds((uint16_t)(1000.0 / planner_frequency_)), [this]() { makePlan(); }); + // Start exploration right away + exploring_timer_->execute_callback(); } Explore::~Explore() @@ -96,6 +105,15 @@ Explore::~Explore() stop(); } +void Explore::resumeCallback(const std_msgs::msg::Bool::SharedPtr msg) +{ + if (msg->data) { + resume(); + } else { + stop(); + } +} + void Explore::visualizeFrontiers( const std::vector& frontiers) { @@ -132,13 +150,14 @@ void Explore::visualizeFrontiers( m.color.a = 255; // lives forever #ifdef ELOQUENT - m.lifetime = rclcpp::Duration(0); // deprecated in galactic warning + m.lifetime = rclcpp::Duration(0); // deprecated in galactic warning #elif DASHING - m.lifetime = rclcpp::Duration(0); // deprecated in galactic warning + m.lifetime = rclcpp::Duration(0); // deprecated in galactic warning #else - m.lifetime = rclcpp::Duration::from_seconds(0); // foxy onwards + m.lifetime = rclcpp::Duration::from_seconds(0); // foxy onwards #endif - // m.lifetime = rclcpp::Duration::from_nanoseconds(0); // suggested in galactic + // m.lifetime = rclcpp::Duration::from_nanoseconds(0); // suggested in + // galactic m.frame_locked = true; // weighted frontiers are always sorted @@ -199,6 +218,7 @@ void Explore::makePlan() } if (frontiers.empty()) { + RCLCPP_WARN(logger_, "No frontiers found, stopping."); stop(); return; } @@ -215,6 +235,7 @@ void Explore::makePlan() return goalOnBlacklist(f.centroid); }); if (frontier == frontiers.end()) { + RCLCPP_WARN(logger_, "All frontiers traversed/tried out, stopping."); stop(); return; } @@ -295,13 +316,15 @@ void Explore::reachedGoal(const NavigationGoalHandle::WrappedResult& result, RCLCPP_DEBUG(logger_, "Goal was aborted"); frontier_blacklist_.push_back(frontier_goal); RCLCPP_DEBUG(logger_, "Adding current goal to black list"); + // If it was aborted probably because we've found another frontier goal, + // so just return and don't make plan again return; case rclcpp_action::ResultCode::CANCELED: RCLCPP_DEBUG(logger_, "Goal was canceled"); - return; + break; default: RCLCPP_WARN(logger_, "Unknown result code from move base nav2"); - return; + break; } // find new goal immediately regardless of planning frequency. // execute via timer to prevent dead lock in move_base_client (this is @@ -311,9 +334,9 @@ void Explore::reachedGoal(const NavigationGoalHandle::WrappedResult& result, // ros::Duration(0, 0), [this](const ros::TimerEvent&) { makePlan(); }, // true); - // TODO: Implement this with ros2 timers? - // Because of the async nature of ros2 calls I think this is not needed. - // makePlan(); + // Because of the 1-thread-executor nature of ros2 I think timer is not + // needed. + makePlan(); } void Explore::start() @@ -323,9 +346,18 @@ void Explore::start() void Explore::stop() { + RCLCPP_INFO(logger_, "Exploration stopped."); move_base_client_->async_cancel_all_goals(); exploring_timer_->cancel(); - RCLCPP_INFO(logger_, "Exploration stopped."); +} + +void Explore::resume() +{ + RCLCPP_INFO(logger_, "Exploration resuming."); + // Reactivate the timer + exploring_timer_->reset(); + // Resume immediately + exploring_timer_->execute_callback(); } } // namespace explore