Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add unit tests for follow path, compute path to pose, and navigate to pose BT action nodes #1844

Merged
merged 3 commits into from
Jul 2, 2020
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
@@ -0,0 +1,55 @@
// 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__PLUGINS__ACTION__COMPUTE_PATH_TO_POSE_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_TO_POSE_ACTION_HPP_

#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 & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf);

void on_tick() override;

BT::NodeStatus on_success() override;

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_id", ""),
});
}

private:
bool first_time_{true};
};

} // namespace nav2_behavior_tree

#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_TO_POSE_ACTION_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,50 @@
// 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__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_

#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 & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf);

void on_tick() override;

void on_wait_for_result() override;

static BT::PortsList providedPorts()
{
return providedBasicPorts(
{
BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
BT::InputPort<std::string>("controller_id", ""),
});
}
};

} // namespace nav2_behavior_tree

#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,51 @@
// 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__PLUGINS__ACTION__NAVIGATE_TO_POSE_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_TO_POSE_ACTION_HPP_

#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"

namespace nav2_behavior_tree
{

class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPose>
{
public:
NavigateToPoseAction(
const std::string & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf);

void on_tick() override;

// Any BT node that accepts parameters must provide a requiredNodeParameters method
static BT::PortsList providedPorts()
{
return providedBasicPorts(
{
BT::InputPort<geometry_msgs::msg::Point>("position", "Position"),
BT::InputPort<geometry_msgs::msg::Quaternion>("orientation", "Orientation")
});
}
};

} // namespace nav2_behavior_tree

#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_TO_POSE_ACTION_HPP_
66 changes: 21 additions & 45 deletions nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,61 +12,39 @@
// 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"
#include "nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp"

namespace nav2_behavior_tree
{

class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePathToPose>
ComputePathToPoseAction::ComputePathToPoseAction(
const std::string & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf)
: BtActionNode<nav2_msgs::action::ComputePathToPose>(xml_tag_name, action_name, conf)
{
public:
ComputePathToPoseAction(
const std::string & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf)
: BtActionNode<nav2_msgs::action::ComputePathToPose>(xml_tag_name, action_name, conf)
{
}

void on_tick() override
{
getInput("goal", goal_.pose);
getInput("planner_id", goal_.planner_id);
}
}

BT::NodeStatus on_success() override
{
setOutput("path", result_.result->path);
void ComputePathToPoseAction::on_tick()
{
getInput("goal", goal_.pose);
getInput("planner_id", goal_.planner_id);
}

if (first_time_) {
first_time_ = false;
} else {
config().blackboard->set("path_updated", true);
}
return BT::NodeStatus::SUCCESS;
}
BT::NodeStatus ComputePathToPoseAction::on_success()
{
setOutput("path", result_.result->path);

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_id", ""),
});
if (first_time_) {
first_time_ = false;
} else {
config().blackboard->set("path_updated", true);
}

private:
bool first_time_{true};
};
return BT::NodeStatus::SUCCESS;
}

} // namespace nav2_behavior_tree

Expand All @@ -83,5 +61,3 @@ BT_REGISTER_NODES(factory)
factory.registerBuilder<nav2_behavior_tree::ComputePathToPoseAction>(
"ComputePathToPose", builder);
}

#endif // NAV2_BEHAVIOR_TREE__COMPUTE_PATH_TO_POSE_ACTION_HPP_
67 changes: 24 additions & 43 deletions nav2_behavior_tree/plugins/action/follow_path_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,59 +12,42 @@
// 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"
#include "nav2_behavior_tree/plugins/action/follow_path_action.hpp"

namespace nav2_behavior_tree
{

class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
FollowPathAction::FollowPathAction(
const std::string & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf)
: BtActionNode<nav2_msgs::action::FollowPath>(xml_tag_name, action_name, conf)
{
public:
FollowPathAction(
const std::string & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf)
: BtActionNode<nav2_msgs::action::FollowPath>(xml_tag_name, action_name, conf)
{
config().blackboard->set("path_updated", false);
}

void on_tick() override
{
getInput("path", goal_.path);
getInput("controller_id", goal_.controller_id);
}
config().blackboard->set("path_updated", false);
}

void on_wait_for_result() 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);
void FollowPathAction::on_tick()
{
getInput("path", goal_.path);
getInput("controller_id", goal_.controller_id);
}

// 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;
}
}
void FollowPathAction::on_wait_for_result()
{
// 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);

static BT::PortsList providedPorts()
{
return providedBasicPorts(
{
BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
BT::InputPort<std::string>("controller_id", ""),
});
// 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;
}
};
}

} // namespace nav2_behavior_tree

Expand All @@ -81,5 +64,3 @@ BT_REGISTER_NODES(factory)
factory.registerBuilder<nav2_behavior_tree::FollowPathAction>(
"FollowPath", builder);
}

#endif // NAV2_BEHAVIOR_TREE__FOLLOW_PATH_ACTION_HPP_
Loading