From abb747203dc8ec1d0fbf7896f8ba8fbc4255f7be Mon Sep 17 00:00:00 2001 From: "Orduno, Carlos A" Date: Tue, 11 Dec 2018 13:41:16 -0800 Subject: [PATCH] Pulling ROS node from BB and removing thread --- .../include/nav2_tasks/is_stuck_condition.hpp | 96 +++++++------------ 1 file changed, 37 insertions(+), 59 deletions(-) diff --git a/nav2_tasks/include/nav2_tasks/is_stuck_condition.hpp b/nav2_tasks/include/nav2_tasks/is_stuck_condition.hpp index e0d395dba86..97ad13e60ff 100644 --- a/nav2_tasks/include/nav2_tasks/is_stuck_condition.hpp +++ b/nav2_tasks/include/nav2_tasks/is_stuck_condition.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include #include @@ -32,40 +31,41 @@ using namespace std::chrono_literals; // NOLINT namespace nav2_tasks { -class IsStuckCondition : public BT::ConditionNode, public rclcpp::Node +class IsStuckCondition : public BT::ConditionNode { public: explicit IsStuckCondition(const std::string & condition_name) : BT::ConditionNode(condition_name), - Node("IsStuckCondition"), - workerThread_(nullptr), is_stuck_(false), - spinning_ok_(false), odom_history_size_(10), current_accel_(0.0), brake_accel_limit_(-10.0) { - RCLCPP_DEBUG(get_logger(), "Creating an IsStuckCondition BT node"); - - odom_sub_ = this->create_subscription("odom", - std::bind(&IsStuckCondition::onOdomReceived, this, std::placeholders::_1)); - - RCLCPP_INFO_ONCE(get_logger(), "Waiting on odometry"); - - startWorkerThread(); } IsStuckCondition() = delete; ~IsStuckCondition() { - RCLCPP_DEBUG(this->get_logger(), "Shutting down IsStuckCondition BT node"); - stopWorkerThread(); + RCLCPP_DEBUG(node_->get_logger(), "Shutting down IsStuckCondition BT node"); + } + + void onInit() override + { + node_ = blackboard()->template get("node"); + + odom_sub_ = node_->create_subscription("odom", + std::bind(&IsStuckCondition::onOdomReceived, this, std::placeholders::_1)); + + RCLCPP_DEBUG(node_->get_logger(), "Initialized an IsStuckCondition BT node"); + + RCLCPP_INFO_ONCE(node_->get_logger(), "Waiting on odometry"); + } void onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg) { - RCLCPP_INFO_ONCE(get_logger(), "Got odometry"); + RCLCPP_INFO_ONCE(node_->get_logger(), "Got odometry"); while (odom_history_.size() >= odom_history_size_) { odom_history_.pop_front(); @@ -75,7 +75,6 @@ class IsStuckCondition : public BT::ConditionNode, public rclcpp::Node // TODO(orduno) #383 Move the state calculation and is stuck to robot class updateStates(); - is_stuck_ = isStuck(); } BT::NodeStatus tick() override @@ -101,31 +100,30 @@ class IsStuckCondition : public BT::ConditionNode, public rclcpp::Node return; } - RCLCPP_INFO(get_logger(), msg); + RCLCPP_INFO(node_->get_logger(), msg); prev_msg = msg; } - void startWorkerThread() + void updateStates() { - spinning_ok_ = true; - workerThread_ = new std::thread(&IsStuckCondition::workerThread, this); - } + // Approximate acceleration + // TODO(orduno) #400 Smooth out velocity history for better accel approx. + if (odom_history_.size() > 2) { + auto curr_odom = odom_history_.end()[-1]; + double t2 = static_cast(curr_odom.header.stamp.sec); + t2 += (static_cast(curr_odom.header.stamp.nanosec)) * 1e-9; - void stopWorkerThread() - { - spinning_ok_ = false; - workerThread_->join(); - delete workerThread_; - workerThread_ = nullptr; - } + auto prev_odom = odom_history_.end()[-2]; + double t1 = static_cast(prev_odom.header.stamp.sec); + t1 += (static_cast(prev_odom.header.stamp.nanosec)) * 1e-9; - void workerThread() - { - while (spinning_ok_) { - // Spin the node to get messages from the subscriptions - rclcpp::spin_some(this->get_node_base_interface()); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + double dt = t2 - t1; + double vel_diff = static_cast( + curr_odom.twist.twist.linear.x - prev_odom.twist.twist.linear.x); + current_accel_ = vel_diff / dt; } + + is_stuck_ = isStuck(); } bool isStuck() @@ -138,7 +136,7 @@ class IsStuckCondition : public BT::ConditionNode, public rclcpp::Node // Detect if robot bumped into something by checking for abnormal deceleration if (current_accel_ < brake_accel_limit_) { - RCLCPP_DEBUG(get_logger(), "Current deceleration is beyond brake limit." + RCLCPP_DEBUG(node_->get_logger(), "Current deceleration is beyond brake limit." " brake limit: %.2f, current accel: %.2f", brake_accel_limit_, current_accel_); return true; @@ -147,35 +145,15 @@ class IsStuckCondition : public BT::ConditionNode, public rclcpp::Node return false; } - void updateStates() - { - // Approximate acceleration - // TODO(orduno) #400 Smooth out velocity history for better accel approx. - if (odom_history_.size() > 2) { - auto curr_odom = odom_history_.end()[-1]; - double t2 = static_cast(curr_odom.header.stamp.sec); - t2 += (static_cast(curr_odom.header.stamp.nanosec)) * 1e-9; - - auto prev_odom = odom_history_.end()[-2]; - double t1 = static_cast(prev_odom.header.stamp.sec); - t1 += (static_cast(prev_odom.header.stamp.nanosec)) * 1e-9; - - double dt = t2 - t1; - double vel_diff = static_cast( - curr_odom.twist.twist.linear.x - prev_odom.twist.twist.linear.x); - current_accel_ = vel_diff / dt; - } - } - void halt() override { } private: - // We handle the detection of the stuck condition on a separate thread - std::thread * workerThread_; + // The node that will be used for any ROS operations + rclcpp::Node::SharedPtr node_; + std::atomic is_stuck_; - std::atomic spinning_ok_; // Listen to odometry rclcpp::Subscription::SharedPtr odom_sub_;