From 5dfdb3a68e4707675269cf402fdf5ebef351bc44 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Andr=C3=A9s=20=C3=81lvarez=20Restrepo?= Date: Thu, 9 Jun 2022 13:18:58 -0500 Subject: [PATCH] Feature/return initial pose (#20) * Add return to initial pose param and logic * Format clang * Add readme feature * Fix log * Fix return statement * Deelete commented block --- README.md | 2 ++ explore/config/params.yaml | 1 + explore/include/explore/explore.h | 8 ++++-- explore/src/explore.cpp | 45 ++++++++++++++++++++++++++++--- 4 files changed, 51 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index ff101bf..5c2c1af 100644 --- a/README.md +++ b/README.md @@ -63,6 +63,8 @@ You can open rviz2 and add the exploration frontiers marker (topic is `explore/f #### 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`. +#### Returning to initial pose +The robot will return to its initial pose after exploration if you want by defining the parameter `return_to_init` to `True` when launching the node. #### 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 f51c5b6..04dcc64 100644 --- a/explore/config/params.yaml +++ b/explore/config/params.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: robot_base_frame: base_link + return_to_init: true costmap_topic: map costmap_updates_topic: map_updates visualize: true diff --git a/explore/include/explore/explore.h b/explore/include/explore/explore.h index 30f96a3..f677b62 100644 --- a/explore/include/explore/explore.h +++ b/explore/include/explore/explore.h @@ -80,7 +80,7 @@ public: ~Explore(); void start(); - void stop(); + void stop(bool finished_exploring = false); void resume(); using NavigationGoalHandle = @@ -121,7 +121,6 @@ private: // rclcpp::TimerBase::SharedPtr oneshot_; rclcpp::Subscription::SharedPtr resume_subscription_; - void resumeCallback(const std_msgs::msg::Bool::SharedPtr msg); std::vector frontier_blacklist_; @@ -130,11 +129,16 @@ private: rclcpp::Time last_progress_; size_t last_markers_count_; + geometry_msgs::msg::Pose initial_pose_; + void returnToInitialPose(void); + // parameters double planner_frequency_; double potential_scale_, orientation_scale_, gain_scale_; double progress_timeout_; bool visualize_; + bool return_to_init_; + std::string robot_base_frame_; }; } // namespace explore diff --git a/explore/src/explore.cpp b/explore/src/explore.cpp index 9398949..836377e 100644 --- a/explore/src/explore.cpp +++ b/explore/src/explore.cpp @@ -59,6 +59,7 @@ Explore::Explore() this->declare_parameter("orientation_scale", 0.0); this->declare_parameter("gain_scale", 1.0); this->declare_parameter("min_frontier_size", 0.5); + this->declare_parameter("return_to_init", false); this->get_parameter("planner_frequency", planner_frequency_); this->get_parameter("progress_timeout", timeout); @@ -67,6 +68,9 @@ Explore::Explore() this->get_parameter("orientation_scale", orientation_scale_); this->get_parameter("gain_scale", gain_scale_); this->get_parameter("min_frontier_size", min_frontier_size); + this->get_parameter("return_to_init", return_to_init_); + this->get_parameter("robot_base_frame", robot_base_frame_); + progress_timeout_ = timeout; move_base_client_ = rclcpp_action::create_client( @@ -93,6 +97,23 @@ Explore::Explore() move_base_client_->wait_for_action_server(); RCLCPP_INFO(logger_, "Connected to move_base nav2 server"); + if (return_to_init_) { + RCLCPP_INFO(logger_, "Getting initial pose of the robot"); + geometry_msgs::msg::TransformStamped transformStamped; + std::string map_frame = costmap_client_.getGlobalFrameID(); + try { + transformStamped = tf_buffer_.lookupTransform( + map_frame, robot_base_frame_, tf2::TimePointZero); + initial_pose_.position.x = transformStamped.transform.translation.x; + initial_pose_.position.y = transformStamped.transform.translation.y; + initial_pose_.orientation = transformStamped.transform.rotation; + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(logger_, "Couldn't find transform from %s to %s: %s", + map_frame.c_str(), robot_base_frame_.c_str(), ex.what()); + return_to_init_ = false; + } + } + exploring_timer_ = this->create_wall_timer( std::chrono::milliseconds((uint16_t)(1000.0 / planner_frequency_)), [this]() { makePlan(); }); @@ -219,7 +240,7 @@ void Explore::makePlan() if (frontiers.empty()) { RCLCPP_WARN(logger_, "No frontiers found, stopping."); - stop(); + stop(true); return; } @@ -236,7 +257,7 @@ void Explore::makePlan() }); if (frontier == frontiers.end()) { RCLCPP_WARN(logger_, "All frontiers traversed/tried out, stopping."); - stop(); + stop(true); return; } geometry_msgs::msg::Point target_position = frontier->centroid; @@ -288,6 +309,20 @@ void Explore::makePlan() move_base_client_->async_send_goal(goal, send_goal_options); } +void Explore::returnToInitialPose() +{ + RCLCPP_INFO(logger_, "Returning to initial pose."); + auto goal = nav2_msgs::action::NavigateToPose::Goal(); + goal.pose.pose.position = initial_pose_.position; + goal.pose.pose.orientation = initial_pose_.orientation; + goal.pose.header.frame_id = costmap_client_.getGlobalFrameID(); + goal.pose.header.stamp = this->now(); + + auto send_goal_options = + rclcpp_action::Client::SendGoalOptions(); + move_base_client_->async_send_goal(goal, send_goal_options); +} + bool Explore::goalOnBlacklist(const geometry_msgs::msg::Point& goal) { constexpr static size_t tolerace = 5; @@ -344,11 +379,15 @@ void Explore::start() RCLCPP_INFO(logger_, "Exploration started."); } -void Explore::stop() +void Explore::stop(bool finished_exploring) { RCLCPP_INFO(logger_, "Exploration stopped."); move_base_client_->async_cancel_all_goals(); exploring_timer_->cancel(); + + if (return_to_init_ && finished_exploring) { + returnToInitialPose(); + } } void Explore::resume()