Skip to content
Snippets Groups Projects
Commit 1d7dc168 authored by Steven Macenski's avatar Steven Macenski
Browse files

remove old headers without plugins

parent 39c6b9cb
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 0 additions and 971 deletions
nav2_behavior_tree/include/nav2_behavior_tree/back_up_action.hpp deleted 100644 → 0
+ 0
− 64
View file @ 39c6b9cb
// Copyright (c) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__BACK_UP_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__BACK_UP_ACTION_HPP_
#include <string>
#include <memory>
#include <cmath>
#include "nav2_behavior_tree/bt_action_node.hpp"
#include "nav2_msgs/action/back_up.hpp"
namespace nav2_behavior_tree
{
class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
{
public:
BackUpAction(
const std::string & action_name,
const BT::NodeConfiguration & conf)
: BtActionNode<nav2_msgs::action::BackUp>(action_name, conf)
{
double dist;
getInput("backup_dist", dist);
double speed;
getInput("backup_speed", speed);
// silently fix, vector direction determined by distance sign
if (speed < 0.0) {
speed *= -1.0;
}
// Populate the input message
goal_.target.x = dist;
goal_.target.y = 0.0;
goal_.target.z = 0.0;
goal_.speed = speed;
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<double>("backup_dist", -0.15, "Distance to backup"),
BT::InputPort<double>("backup_speed", 0.025, "Speed at which to backup")
});
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__BACK_UP_ACTION_HPP_
nav2_behavior_tree/include/nav2_behavior_tree/clear_costmap_service.hpp deleted 100644 → 0
+ 0
− 41
View file @ 39c6b9cb
// Copyright (c) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__CLEAR_COSTMAP_SERVICE_HPP_
#define NAV2_BEHAVIOR_TREE__CLEAR_COSTMAP_SERVICE_HPP_
#include <string>
#include <memory>
#include <cmath>
#include "nav2_behavior_tree/bt_service_node.hpp"
#include "nav2_msgs/srv/clear_entire_costmap.hpp"
namespace nav2_behavior_tree
{
class ClearEntireCostmapService : public BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>
{
public:
ClearEntireCostmapService(
const std::string & service_node_name,
const BT::NodeConfiguration & conf)
: BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>(service_node_name, conf)
{
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__CLEAR_COSTMAP_SERVICE_HPP_
nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp deleted 100644 → 0
+ 0
− 70
View file @ 39c6b9cb
// Copyright (c) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__COMPUTE_PATH_TO_POSE_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__COMPUTE_PATH_TO_POSE_ACTION_HPP_
#include <memory>
#include <string>
#include "nav2_msgs/action/compute_path_to_pose.hpp"
#include "nav_msgs/msg/path.h"
#include "nav2_behavior_tree/bt_action_node.hpp"
namespace nav2_behavior_tree
{
class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePathToPose>
{
public:
ComputePathToPoseAction(
const std::string & action_name,
const BT::NodeConfiguration & conf)
: BtActionNode<nav2_msgs::action::ComputePathToPose>(action_name, conf)
{
}
void on_tick() override
{
getInput("goal", goal_.pose);
getInput("planner_property", goal_.planner_property);
}
void on_success() override
{
setOutput("path", result_.result->path);
if (first_time_) {
first_time_ = false;
} else {
config().blackboard->set("path_updated", true);
}
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
BT::InputPort<std::string>("planner_property", "")
});
}
private:
bool first_time_{true};
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__COMPUTE_PATH_TO_POSE_ACTION_HPP_
nav2_behavior_tree/include/nav2_behavior_tree/follow_path_action.hpp deleted 100644 → 0
+ 0
− 67
View file @ 39c6b9cb
// Copyright (c) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__FOLLOW_PATH_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__FOLLOW_PATH_ACTION_HPP_
#include <memory>
#include <string>
#include "nav2_msgs/action/follow_path.hpp"
#include "nav2_behavior_tree/bt_action_node.hpp"
namespace nav2_behavior_tree
{
class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
{
public:
FollowPathAction(
const std::string & action_name,
const BT::NodeConfiguration & conf)
: BtActionNode<nav2_msgs::action::FollowPath>(action_name, conf)
{
config().blackboard->set("path_updated", false);
}
void on_tick() override
{
getInput("path", goal_.path);
}
void on_server_timeout() override
{
// Check if the goal has been updated
if (config().blackboard->get<bool>("path_updated")) {
// Reset the flag in the blackboard
config().blackboard->set("path_updated", false);
// Grab the new goal and set the flag so that we send the new goal to
// the action server on the next loop iteration
getInput("path", goal_.path);
goal_updated_ = true;
}
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
});
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__FOLLOW_PATH_ACTION_HPP_
nav2_behavior_tree/include/nav2_behavior_tree/goal_reached_condition.hpp deleted 100644 → 0
+ 0
− 112
View file @ 39c6b9cb
// Copyright (c) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__GOAL_REACHED_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__GOAL_REACHED_CONDITION_HPP_
#include <string>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "behaviortree_cpp_v3/condition_node.h"
#include "nav2_util/robot_utils.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "tf2_ros/buffer.h"
namespace nav2_behavior_tree
{
class GoalReachedCondition : public BT::ConditionNode
{
public:
GoalReachedCondition(
const std::string & condition_name,
const BT::NodeConfiguration & conf)
: BT::ConditionNode(condition_name, conf), initialized_(false)
{
}
GoalReachedCondition() = delete;
~GoalReachedCondition()
{
cleanup();
}
BT::NodeStatus tick() override
{
if (!initialized_) {
initialize();
}
if (isGoalReached()) {
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
}
void initialize()
{
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
node_->get_parameter_or<double>("goal_reached_tol", goal_reached_tol_, 0.25);
tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
initialized_ = true;
}
bool
isGoalReached()
{
geometry_msgs::msg::PoseStamped current_pose;
if (!nav2_util::getCurrentPose(current_pose, *tf_)) {
RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
return false;
}
geometry_msgs::msg::PoseStamped goal;
getInput("goal", goal);
double dx = goal.pose.position.x - current_pose.pose.position.x;
double dy = goal.pose.position.y - current_pose.pose.position.y;
if ( (dx * dx + dy * dy) <= (goal_reached_tol_ * goal_reached_tol_) ) {
return true;
} else {
return false;
}
}
static BT::PortsList providedPorts()
{
return {
BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination")
};
}
protected:
void cleanup()
{
}
private:
rclcpp::Node::SharedPtr node_;
std::shared_ptr<tf2_ros::Buffer> tf_;
bool initialized_;
double goal_reached_tol_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__GOAL_REACHED_CONDITION_HPP_
nav2_behavior_tree/include/nav2_behavior_tree/initial_pose_received_condition.hpp deleted 100644 → 0
+ 0
− 28
View file @ 39c6b9cb
// Copyright (c) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__INITIAL_POSE_RECEIVED_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__INITIAL_POSE_RECEIVED_CONDITION_HPP_
#include "behaviortree_cpp_v3/behavior_tree.h"
namespace nav2_behavior_tree
{
BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node);
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__INITIAL_POSE_RECEIVED_CONDITION_HPP_
nav2_behavior_tree/include/nav2_behavior_tree/is_stuck_condition.hpp deleted 100644 → 0
+ 0
− 169
View file @ 39c6b9cb
// Copyright (c) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__IS_STUCK_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__IS_STUCK_CONDITION_HPP_
#include <string>
#include <chrono>
#include <cmath>
#include <atomic>
#include <memory>
#include <deque>
#include "rclcpp/rclcpp.hpp"
#include "behaviortree_cpp_v3/condition_node.h"
#include "nav_msgs/msg/odometry.hpp"
using namespace std::chrono_literals; // NOLINT
namespace nav2_behavior_tree
{
class IsStuckCondition : public BT::ConditionNode
{
public:
IsStuckCondition(
const std::string & condition_name,
const BT::NodeConfiguration & conf)
: BT::ConditionNode(condition_name, conf),
is_stuck_(false),
odom_history_size_(10),
current_accel_(0.0),
brake_accel_limit_(-10.0)
{
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
odom_sub_ = node_->create_subscription<nav_msgs::msg::Odometry>("odom",
rclcpp::SystemDefaultsQoS(),
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");
}
IsStuckCondition() = delete;
~IsStuckCondition()
{
RCLCPP_DEBUG(node_->get_logger(), "Shutting down IsStuckCondition BT node");
}
void onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg)
{
RCLCPP_INFO_ONCE(node_->get_logger(), "Got odometry");
while (odom_history_.size() >= odom_history_size_) {
odom_history_.pop_front();
}
odom_history_.push_back(*msg);
// TODO(orduno) #383 Move the state calculation and is stuck to robot class
updateStates();
}
BT::NodeStatus tick() override
{
// TODO(orduno) #383 Once check for is stuck and state calculations are moved to robot class
// this becomes
// if (robot_state_.isStuck()) {
if (is_stuck_) {
logStuck("Robot got stuck!");
return BT::NodeStatus::SUCCESS; // Successfully detected a stuck condition
}
logStuck("Robot is free");
return BT::NodeStatus::FAILURE; // Failed to detected a stuck condition
}
void logStuck(const std::string & msg) const
{
static std::string prev_msg;
if (msg == prev_msg) {
return;
}
RCLCPP_INFO(node_->get_logger(), msg);
prev_msg = msg;
}
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 curr_time = static_cast<double>(curr_odom.header.stamp.sec);
curr_time += (static_cast<double>(curr_odom.header.stamp.nanosec)) * 1e-9;
auto prev_odom = odom_history_.end()[-2];
double prev_time = static_cast<double>(prev_odom.header.stamp.sec);
prev_time += (static_cast<double>(prev_odom.header.stamp.nanosec)) * 1e-9;
double dt = curr_time - prev_time;
double vel_diff = static_cast<double>(
curr_odom.twist.twist.linear.x - prev_odom.twist.twist.linear.x);
current_accel_ = vel_diff / dt;
}
is_stuck_ = isStuck();
}
bool isStuck()
{
// TODO(orduno) #400 The robot getting stuck can result on different types of motion
// depending on the state prior to getting stuck (sudden change in accel, not moving at all,
// random oscillations, etc). For now, we only address the case where there is a sudden
// harsh deceleration. A better approach to capture all situations would be to do a forward
// simulation of the robot motion and compare it with the actual one.
// Detect if robot bumped into something by checking for abnormal deceleration
if (current_accel_ < brake_accel_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;
}
return false;
}
static BT::PortsList providedPorts() {return {};}
private:
// The node that will be used for any ROS operations
rclcpp::Node::SharedPtr node_;
std::atomic<bool> is_stuck_;
// Listen to odometry
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
// Store history of odometry measurements
std::deque<nav_msgs::msg::Odometry> odom_history_;
std::deque<nav_msgs::msg::Odometry>::size_type odom_history_size_;
// Calculated states
double current_accel_;
// Robot specific paramters
double brake_accel_limit_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__IS_STUCK_CONDITION_HPP_
nav2_behavior_tree/include/nav2_behavior_tree/navigate_to_pose_action.hpp deleted 100644 → 0
+ 0
− 70
View file @ 39c6b9cb
// Copyright (c) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__NAVIGATE_TO_POSE_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__NAVIGATE_TO_POSE_ACTION_HPP_
#include <memory>
#include <string>
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "nav2_behavior_tree/bt_action_node.hpp"
#include "nav2_behavior_tree/bt_conversions.hpp"
namespace nav2_behavior_tree
{
class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPose>
{
public:
NavigateToPoseAction(
const std::string & action_name,
const BT::NodeConfiguration & conf)
: BtActionNode<nav2_msgs::action::NavigateToPose>(action_name, conf)
{
}
void on_tick() override
{
// Use the position and orientation fields from the XML attributes to initialize the goal
geometry_msgs::msg::Point position;
geometry_msgs::msg::Quaternion orientation;
bool have_position = getInput("position", position);
bool have_orientation = getInput("orientation", orientation);
if (!have_position || !have_orientation) {
RCLCPP_ERROR(node_->get_logger(),
"NavigateToPoseAction: position or orientation not provided");
}
goal_.pose.pose.position = position;
goal_.pose.pose.orientation = orientation;
}
// Any BT node that accepts parameters must provide a requiredNodeParameters method
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<geometry_msgs::msg::Point>("position", "0;0;0", "Position"),
BT::InputPort<geometry_msgs::msg::Quaternion>("orientation", "0;0;0;0", "Orientation")
});
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__NAVIGATE_TO_POSE_ACTION_HPP_
nav2_behavior_tree/include/nav2_behavior_tree/random_crawl_action.hpp deleted 100644 → 0
+ 0
− 37
View file @ 39c6b9cb
// Copyright (c) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__RANDOM_CRAWL_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__RANDOM_CRAWL_ACTION_HPP_
#include <string>
#include "nav2_behavior_tree/bt_action_node.hpp"
#include "nav2_msgs/action/random_crawl.hpp"
namespace nav2_behavior_tree
{
class RandomCrawlAction : public BtActionNode<nav2_msgs::action::RandomCrawl>
{
public:
explicit RandomCrawlAction(const std::string & action_name)
: BtActionNode<nav2_msgs::action::RandomCrawl>(action_name)
{
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__RANDOM_CRAWL_ACTION_HPP_
nav2_behavior_tree/include/nav2_behavior_tree/rate_controller.hpp deleted 100644 → 0
+ 0
− 101
View file @ 39c6b9cb
// Copyright (c) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__RATE_CONTROLLER_HPP_
#define NAV2_BEHAVIOR_TREE__RATE_CONTROLLER_HPP_
#include <chrono>
#include <string>
#include "behaviortree_cpp_v3/decorator_node.h"
namespace nav2_behavior_tree
{
class RateController : public BT::DecoratorNode
{
public:
RateController(
const std::string & name,
const BT::NodeConfiguration & conf)
: BT::DecoratorNode(name, conf)
{
double hz = 1.0;
getInput("hz", hz);
period_ = 1.0 / hz;
}
// Any BT node that accepts parameters must provide a requiredNodeParameters method
static BT::PortsList providedPorts()
{
return {
BT::InputPort<double>("hz", 10.0, "Rate")
};
}
private:
BT::NodeStatus tick() override;
std::chrono::time_point<std::chrono::high_resolution_clock> start_;
double period_;
};
static bool first_time{false};
inline BT::NodeStatus RateController::tick()
{
if (status() == BT::NodeStatus::IDLE) {
// Reset the starting point since we're starting a new iteration of
// the rate controller (moving from IDLE to RUNNING)
start_ = std::chrono::high_resolution_clock::now();
first_time = true;
}
setStatus(BT::NodeStatus::RUNNING);
// Determine how long its been since we've started this iteration
auto now = std::chrono::high_resolution_clock::now();
auto elapsed = now - start_;
// Now, get that in seconds
typedef std::chrono::duration<float> float_seconds;
auto seconds = std::chrono::duration_cast<float_seconds>(elapsed);
// If we've exceed the specified period, execute the child node
if (first_time || seconds.count() >= period_) {
first_time = false;
const BT::NodeStatus child_state = child_node_->executeTick();
switch (child_state) {
case BT::NodeStatus::RUNNING:
return BT::NodeStatus::RUNNING;
case BT::NodeStatus::SUCCESS:
child_node_->setStatus(BT::NodeStatus::IDLE);
start_ = std::chrono::high_resolution_clock::now(); // Reset the timer
return BT::NodeStatus::SUCCESS;
case BT::NodeStatus::FAILURE:
default:
child_node_->setStatus(BT::NodeStatus::IDLE);
return BT::NodeStatus::FAILURE;
}
}
return status();
}
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__RATE_CONTROLLER_HPP_
nav2_behavior_tree/include/nav2_behavior_tree/recovery_node.hpp deleted 100644 → 0
+ 0
− 59
View file @ 39c6b9cb
// Copyright (c) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__RECOVERY_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__RECOVERY_NODE_HPP_
#include <string>
#include "behaviortree_cpp_v3/control_node.h"
namespace nav2_behavior_tree
{
/**
* @brief The RecoveryNode has only two children and returns SUCCESS if and only if the first child
* returns SUCCESS.
*
* - If the first child returns FAILURE, the second child will be executed. After that the first
* child is executed again if the second child returns SUCCESS.
*
* - If the first or second child returns RUNNING, this node returns RUNNING.
*
* - If the second child returns FAILURE, this control node will stop the loop and returns FAILURE.
*
*/
class RecoveryNode : public BT::ControlNode
{
public:
RecoveryNode(const std::string & name, const BT::NodeConfiguration & conf);
~RecoveryNode() override = default;
// Any BT node that accepts parameters must provide a requiredNodeParameters method
static BT::PortsList providedPorts()
{
return {
BT::InputPort<int>("number_of_retries", 1, "Number of retries")
};
}
private:
unsigned int current_child_idx_;
unsigned int number_of_retries_;
unsigned int retry_count_;
BT::NodeStatus tick() override;
void halt() override;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__RECOVERY_NODE_HPP_
nav2_behavior_tree/include/nav2_behavior_tree/reinitialize_global_localization_service.hpp deleted 100644 → 0
+ 0
− 41
View file @ 39c6b9cb
// Copyright (c) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
#define NAV2_BEHAVIOR_TREE__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
#include <string>
#include <memory>
#include <cmath>
#include "nav2_behavior_tree/bt_service_node.hpp"
#include "std_srvs/srv/empty.hpp"
namespace nav2_behavior_tree
{
class ReinitializeGlobalLocalizationService : public BtServiceNode<std_srvs::srv::Empty>
{
public:
ReinitializeGlobalLocalizationService(
const std::string & service_node_name,
const BT::NodeConfiguration & conf)
: BtServiceNode<std_srvs::srv::Empty>(service_node_name, conf)
{
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp deleted 100644 → 0
+ 0
− 54
View file @ 39c6b9cb
// Copyright (c) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__SPIN_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__SPIN_ACTION_HPP_
#include <string>
#include <memory>
#include <cmath>
#include "nav2_behavior_tree/bt_action_node.hpp"
#include "nav2_msgs/action/spin.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
namespace nav2_behavior_tree
{
class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
{
public:
SpinAction(
const std::string & action_name,
const BT::NodeConfiguration & conf)
: BtActionNode<nav2_msgs::action::Spin>(action_name, conf)
{
double dist;
getInput("spin_dist", dist);
goal_.target_yaw = dist;
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<double>("spin_dist", 1.57, "Spin distance")
});
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__SPIN_ACTION_HPP_
nav2_behavior_tree/include/nav2_behavior_tree/wait_action.hpp deleted 100644 → 0
+ 0
− 58
View file @ 39c6b9cb
// Copyright (c) 2018 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__WAIT_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__WAIT_ACTION_HPP_
#include <string>
#include <memory>
#include <cmath>
#include "nav2_behavior_tree/bt_action_node.hpp"
#include "nav2_msgs/action/wait.hpp"
namespace nav2_behavior_tree
{
class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
{
public:
WaitAction(
const std::string & action_name,
const BT::NodeConfiguration & conf)
: BtActionNode<nav2_msgs::action::Wait>(action_name, conf)
{
int duration;
getInput("wait_duration", duration);
if (duration <= 0) {
RCLCPP_WARN(node_->get_logger(), "Wait duration is negative or zero "
"(%i). Setting to positive.", duration);
duration *= -1;
}
goal_.time.sec = duration;
}
// Any BT node that accepts parameters must provide a requiredNodeParameters method
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<int>("wait_duration", 1, "Wait time")
});
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__WAIT_ACTION_HPP_
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment