Skip to content

Commit ce6b439

Browse files
ajtudelaajtudela
authored
Following (#5565)
* Following server Signed-off-by: Alberto Tudela <[email protected]> * feat: migrate to nav2_msgs for FollowObject action and remove opennav_following_msgs Signed-off-by: Alberto Tudela <[email protected]> * Fix mypy test Signed-off-by: Alberto Tudela <[email protected]> * Move FollowObject and FollowObjectCancel action nodes from bt package Signed-off-by: Alberto Tudela <[email protected]> * Update package and circle Signed-off-by: Alberto Tudela <[email protected]> * feat: add FollowObject action support in robot navigator and fix mypy Signed-off-by: Alberto Tudela <[email protected]> * Fix mypy issues Signed-off-by: Alberto Tudela <[email protected]> * feat: add follow_object action to bt_navigator parameters Signed-off-by: Alberto Tudela <[email protected]> * Update key and package Signed-off-by: Alberto Tudela <[email protected]> * Fix precommit Signed-off-by: Alberto Tudela <[email protected]> * Fix mypy? Signed-off-by: Alberto Tudela <[email protected]> * Fix mypy, round two Signed-off-by: Alberto Tudela <[email protected]> --------- Signed-off-by: Alberto Tudela <[email protected]>
1 parent ed0d59f commit ce6b439

File tree

25 files changed

+2835
-13
lines changed

25 files changed

+2835
-13
lines changed

‎.circleci/config.yml‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -33,12 +33,12 @@ _commands:
3333
- restore_cache:
3434
name: Restore Cache << parameters.key >>
3535
keys:
36-
- "<< parameters.key >>-v39\
36+
- "<< parameters.key >>-v40\
3737
-{{ arch }}\
3838
-{{ .Branch }}\
3939
-{{ .Environment.CIRCLE_PR_NUMBER }}\
4040
-{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}"
41-
- "<< parameters.key >>-v39\
41+
- "<< parameters.key >>-v40\
4242
-{{ arch }}\
4343
-main\
4444
-<no value>\
@@ -532,7 +532,7 @@ jobs:
532532

533533
_parameters:
534534
release_parameters: &release_parameters
535-
packages_skip_regex: "'(nav2_system_tests|nav2_smac_planner|nav2_mppi_controller|nav2_route|nav2_rviz_plugins|nav2_rotation_shim_controller|nav2_waypoint_follower|nav2_smoother|opennav_docking|nav2_behaviors|nav2_bringup|navigation2)'"
535+
packages_skip_regex: "'(nav2_system_tests|nav2_smac_planner|nav2_mppi_controller|nav2_route|nav2_rviz_plugins|nav2_rotation_shim_controller|nav2_waypoint_follower|nav2_smoother|opennav_docking|opennav_following|nav2_behaviors|nav2_bringup|navigation2)'"
536536

537537
workflows:
538538
version: 2

‎nav2_behavior_tree/CMakeLists.txt‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -261,6 +261,12 @@ list(APPEND plugin_libs nav2_compute_route_bt_node)
261261
add_library(nav2_toggle_collision_monitor_service_bt_node SHARED plugins/action/toggle_collision_monitor_service.cpp)
262262
list(APPEND plugin_libs nav2_toggle_collision_monitor_service_bt_node)
263263

264+
add_library(opennav_follow_action_bt_node SHARED plugins/action/follow_object_action.cpp)
265+
list(APPEND plugin_libs opennav_follow_action_bt_node)
266+
267+
add_library(opennav_follow_cancel_bt_node SHARED plugins/action/follow_object_cancel_node.cpp)
268+
list(APPEND plugin_libs opennav_follow_cancel_bt_node)
269+
264270
foreach(bt_plugin ${plugin_libs})
265271
target_include_directories(${bt_plugin}
266272
PRIVATE

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,7 @@ BtActionServer<ActionT, NodeT>::BtActionServer(
6767
"compute_path",
6868
"dock_robot",
6969
"drive_on_heading",
70+
"follow_object",
7071
"follow_path",
7172
"nav_thru_poses",
7273
"nav_to_pose",

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_object_action.hpp‎

Lines changed: 98 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,98 @@
1+
// Copyright (c) 2024 Open Navigation LLC
2+
// Copyright (c) 2024 Alberto J. Tudela Roldán
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
16+
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_OBJECT_ACTION_HPP_
17+
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_OBJECT_ACTION_HPP_
18+
19+
#include <string>
20+
#include <memory>
21+
#include <vector>
22+
23+
#include "nav2_behavior_tree/bt_action_node.hpp"
24+
#include "nav2_msgs/action/follow_object.hpp"
25+
26+
27+
namespace nav2_behavior_tree
28+
{
29+
30+
/**
31+
* @brief nav2_behavior_tree::BtActionNode class that wraps nav2_msgs/FollowObject
32+
*/
33+
class FollowObjectAction
34+
: public nav2_behavior_tree::BtActionNode<nav2_msgs::action::FollowObject>
35+
{
36+
using Action = nav2_msgs::action::FollowObject;
37+
using ActionResult = Action::Result;
38+
39+
public:
40+
/**
41+
* @brief A constructor for nav2_behavior_tree::FollowObjectAction
42+
* @param xml_tag_name Name for the XML tag for this node
43+
* @param action_name Action name this node creates a client for
44+
* @param conf BT node configuration
45+
*/
46+
FollowObjectAction(
47+
const std::string & xml_tag_name,
48+
const std::string & action_name,
49+
const BT::NodeConfiguration & conf);
50+
51+
/**
52+
* @brief Function to perform some user-defined operation on tick
53+
*/
54+
void on_tick() override;
55+
56+
/**
57+
* @brief Function to perform some user-defined operation upon successful completion of the action
58+
*/
59+
BT::NodeStatus on_success() override;
60+
61+
/**
62+
* @brief Function to perform some user-defined operation upon abortion of the action
63+
*/
64+
BT::NodeStatus on_aborted() override;
65+
66+
/**
67+
* @brief Function to perform some user-defined operation upon cancellation of the action
68+
*/
69+
BT::NodeStatus on_cancelled() override;
70+
71+
/**
72+
* @brief Creates list of BT ports
73+
* @return BT::PortsList Containing basic ports along with node-specific ports
74+
*/
75+
static BT::PortsList providedPorts()
76+
{
77+
return providedBasicPorts(
78+
{
79+
BT::InputPort<std::string>(
80+
"pose_topic", "dynamic_pose", "Topic to publish the pose of the object to follow"),
81+
BT::InputPort<std::string>(
82+
"tracked_frame", "Target frame to follow (Optional, used if pose_topic is not set)"),
83+
BT::InputPort<float>(
84+
"max_duration", 0.0, "The maximum duration to follow the object (Optional)"),
85+
86+
BT::OutputPort<ActionResult::_total_elapsed_time_type>(
87+
"total_elapsed_time", "Total elapsed time"),
88+
BT::OutputPort<ActionResult::_error_code_type>(
89+
"error_code_id", "Error code"),
90+
BT::OutputPort<std::string>(
91+
"error_msg", "Error message"),
92+
});
93+
}
94+
};
95+
96+
} // namespace nav2_behavior_tree
97+
98+
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_OBJECT_ACTION_HPP_

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_object_cancel_node.hpp‎

Lines changed: 61 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
// Copyright (c) 2024 Open Navigation LLC
2+
// Copyright (c) 2024 Alberto J. Tudela Roldán
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
16+
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_OBJECT_CANCEL_NODE_HPP_
17+
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_OBJECT_CANCEL_NODE_HPP_
18+
19+
#include <memory>
20+
#include <string>
21+
22+
#include "nav2_behavior_tree/bt_cancel_action_node.hpp"
23+
#include "nav2_ros_common/lifecycle_node.hpp"
24+
#include "nav2_msgs/action/follow_object.hpp"
25+
26+
namespace nav2_behavior_tree
27+
{
28+
29+
/**
30+
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::FollowObject
31+
*/
32+
class FollowObjectCancel
33+
: public nav2_behavior_tree::BtCancelActionNode<nav2_msgs::action::FollowObject>
34+
{
35+
public:
36+
/**
37+
* @brief A constructor for nav2_behavior_tree::FollowObject
38+
* @param xml_tag_name Name for the XML tag for this node
39+
* @param action_name Action name this node creates a client for
40+
* @param conf BT node configuration
41+
*/
42+
FollowObjectCancel(
43+
const std::string & xml_tag_name,
44+
const std::string & action_name,
45+
const BT::NodeConfiguration & conf);
46+
47+
/**
48+
* @brief Creates list of BT ports
49+
* @return BT::PortsList Containing basic ports along with node-specific ports
50+
*/
51+
static BT::PortsList providedPorts()
52+
{
53+
return providedBasicPorts(
54+
{
55+
});
56+
}
57+
};
58+
59+
} // namespace nav2_behavior_tree
60+
61+
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_OBJECT_CANCEL_NODE_HPP_

‎nav2_behavior_tree/plugins/action/follow_object_action.cpp‎

Lines changed: 82 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
1+
// Copyright (c) 2024 Open Navigation LLC
2+
// Copyright (c) 2024 Alberto J. Tudela Roldán
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
16+
#include <memory>
17+
#include <string>
18+
19+
#include "nav2_behavior_tree/plugins/action/follow_object_action.hpp"
20+
21+
namespace nav2_behavior_tree
22+
{
23+
24+
FollowObjectAction::FollowObjectAction(
25+
const std::string & xml_tag_name,
26+
const std::string & action_name,
27+
const BT::NodeConfiguration & conf)
28+
: BtActionNode<Action>(xml_tag_name, action_name, conf)
29+
{
30+
}
31+
32+
void FollowObjectAction::on_tick()
33+
{
34+
// Get core inputs about what to perform
35+
getInput("pose_topic", goal_.pose_topic);
36+
getInput("tracked_frame", goal_.tracked_frame);
37+
double max_duration;
38+
getInput("max_duration", max_duration);
39+
40+
// Populate the input message
41+
goal_.max_duration = rclcpp::Duration::from_seconds(max_duration);
42+
}
43+
44+
BT::NodeStatus FollowObjectAction::on_success()
45+
{
46+
setOutput("total_elapsed_time", result_.result->total_elapsed_time);
47+
setOutput("error_code_id", ActionResult::NONE);
48+
setOutput("error_msg", "");
49+
return BT::NodeStatus::SUCCESS;
50+
}
51+
52+
BT::NodeStatus FollowObjectAction::on_aborted()
53+
{
54+
setOutput("total_elapsed_time", result_.result->total_elapsed_time);
55+
setOutput("error_code_id", result_.result->error_code);
56+
setOutput("error_msg", result_.result->error_msg);
57+
return BT::NodeStatus::FAILURE;
58+
}
59+
60+
BT::NodeStatus FollowObjectAction::on_cancelled()
61+
{
62+
setOutput("total_elapsed_time", result_.result->total_elapsed_time);
63+
setOutput("error_code_id", ActionResult::NONE);
64+
setOutput("error_msg", "");
65+
return BT::NodeStatus::SUCCESS;
66+
}
67+
68+
} // namespace nav2_behavior_tree
69+
70+
#include "behaviortree_cpp/bt_factory.h"
71+
BT_REGISTER_NODES(factory)
72+
{
73+
BT::NodeBuilder builder =
74+
[](const std::string & name, const BT::NodeConfiguration & config)
75+
{
76+
return std::make_unique<nav2_behavior_tree::FollowObjectAction>(
77+
name, "follow_object", config);
78+
};
79+
80+
factory.registerBuilder<nav2_behavior_tree::FollowObjectAction>(
81+
"FollowObject", builder);
82+
}

‎nav2_behavior_tree/plugins/action/follow_object_cancel_node.cpp‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
// Copyright (c) 2024 Open Navigation LLC
2+
// Copyright (c) 2024 Alberto J. Tudela Roldán
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
16+
#include <string>
17+
#include <memory>
18+
19+
#include "nav2_behavior_tree/plugins/action/follow_object_cancel_node.hpp"
20+
21+
namespace nav2_behavior_tree
22+
{
23+
24+
FollowObjectCancel::FollowObjectCancel(
25+
const std::string & xml_tag_name,
26+
const std::string & action_name,
27+
const BT::NodeConfiguration & conf)
28+
: nav2_behavior_tree::BtCancelActionNode<nav2_msgs::action::FollowObject>(
29+
xml_tag_name, action_name, conf)
30+
{
31+
}
32+
33+
} // namespace nav2_behavior_tree
34+
35+
#include "behaviortree_cpp/bt_factory.h"
36+
BT_REGISTER_NODES(factory)
37+
{
38+
BT::NodeBuilder builder =
39+
[](const std::string & name, const BT::NodeConfiguration & config)
40+
{
41+
return std::make_unique<nav2_behavior_tree::FollowObjectCancel>(
42+
name, "follow_object", config);
43+
};
44+
45+
factory.registerBuilder<nav2_behavior_tree::FollowObjectCancel>(
46+
"CancelFollowObject", builder);
47+
}

‎nav2_behavior_tree/test/plugins/action/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -94,3 +94,7 @@ plugin_add_test(test_progress_checker_selector_node test_progress_checker_select
9494
plugin_add_test(test_toggle_collision_monitor_service
9595
test_toggle_collision_monitor_service.cpp
9696
nav2_toggle_collision_monitor_service_bt_node)
97+
98+
plugin_add_test(test_follow_object_action test_follow_object_action.cpp opennav_follow_action_bt_node)
99+
100+
plugin_add_test(test_follow_object_cancel_node test_follow_object_cancel_node.cpp opennav_follow_cancel_bt_node)

0 commit comments

Comments
 (0)