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

Humble sync 2: Nov 8 #3274

Merged
merged 38 commits into from
Nov 8, 2022
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
38 commits
Select commit Hold shift + click to select a range
dd7a9e6
standalone assisted teleop (#2904)
jwallace42 Aug 25, 2022
a395e09
Add the support of range sensors to Collision Monitor (#3099)
AlexeyMerzlyakov Aug 26, 2022
a0ccc20
Fix #3152: Costmap extend did not include Y component (#3153)
SteveMacenski Aug 26, 2022
1d65e3f
missing nodes added to nav2_tree_nodes.xml (#3155)
enesbedir1 Aug 27, 2022
cd86153
Change deprecated ceres function (#3158)
Tobias-Fischer Aug 30, 2022
804af4f
remove camera_rgb_joint since child frame does not exist (#3162)
ipa-kut Aug 31, 2022
86211fe
bugfix (#3109) deadlock when costmap receives new map (#3145)
daisukes Aug 31, 2022
e251231
simple command costmap api - first few functions (#3159)
stevedanomodolor Aug 31, 2022
c93f4c5
Fix missing dependency on nav2_collision_monitor (#3175)
fantalukas Sep 6, 2022
0587cab
fixed start (#3168)
jwallace42 Sep 6, 2022
92e846e
Fix velocities comparison for rotation at place case (#3177)
AlexeyMerzlyakov Sep 9, 2022
84c2683
set a empty path on halt (#3178)
jwallace42 Sep 9, 2022
f1c3390
simple command costmap api - update few functions (#3169)
JacksonK9 Sep 12, 2022
7fe15cf
clear names for bt nodes (#3183)
jwallace42 Sep 12, 2022
7037c38
[Smac] check if a node exists before creating (#3195)
SteveMacenski Sep 16, 2022
2a42a1b
fixing benchmarkign for planners (#3202)
SteveMacenski Sep 19, 2022
46f6b19
[Smac] Robin hood data structure improves performance by 10-15%! (#3201)
SteveMacenski Sep 20, 2022
6c0e429
[RPP] Add parameter to enable/disable collision detection (#3204)
fantalukas Sep 21, 2022
f8e79d6
Update waffle.model
SteveMacenski Sep 21, 2022
461513f
add benchmark launch file + instructions (#3218)
SteveMacenski Sep 22, 2022
ec641a6
removing hypotf from smac planner heuristic computation (#3217)
SteveMacenski Sep 22, 2022
c4f0eb9
complete smac planner tolerances (#3219)
SteveMacenski Sep 23, 2022
9a6413c
Disable Output Buffering (#3220)
ruffsl Sep 23, 2022
5803674
fix majority of python linting errors introduced in python costmap AP…
SteveMacenski Sep 23, 2022
4525aa0
Assisted teleop simple commander (#3198)
jwallace42 Sep 26, 2022
e1c429a
Costmap Filter enabling service (#3229)
AlexeyMerzlyakov Oct 5, 2022
54ea117
Add binary flip costmap filter (#3228)
AlexeyMerzlyakov Oct 10, 2022
332aea2
Update waffle.model
SteveMacenski Oct 11, 2022
dd99474
Update waffle.model
SteveMacenski Oct 11, 2022
d290bcb
Update test_actions.cpp
SteveMacenski Oct 11, 2022
bd86155
odom alpha restriction to avoid overflow caused by user-misconfigurat…
Cryst4L9527 Oct 11, 2022
28474a9
Update controller server goal checker (#3240)
NicolasRochaPacheco Oct 12, 2022
079a717
map-size restriction to avoid overflow and nullptr caused by user-mis…
Cryst4L9527 Oct 14, 2022
4964294
Add Path Smoothers Benchmarking suite (#3236)
AlexeyMerzlyakov Oct 14, 2022
9e08e29
Fix typo (#3262)
woawo1213 Nov 1, 2022
49449b6
Adding new Nav2 Smoother: Savitzky-Golay Smoother (#3264)
SteveMacenski Nov 3, 2022
90c4ac1
Added Line Iterator (#3197)
afifswaidan Nov 7, 2022
3a8c7d0
bumping to 1.1.3 for release
SteveMacenski Nov 8, 2022
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
7 changes: 4 additions & 3 deletions .circleci/config.yml
Original file line number Diff line number Diff line change
Expand Up @@ -33,12 +33,12 @@ _commands:
- restore_cache:
name: Restore Cache << parameters.key >>
keys:
- "<< parameters.key >>-v11\
- "<< parameters.key >>-v12\
-{{ arch }}\
-{{ .Branch }}\
-{{ .Environment.CIRCLE_PR_NUMBER }}\
-{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}"
- "<< parameters.key >>-v11\
- "<< parameters.key >>-v12\
-{{ arch }}\
-main\
-<no value>\
Expand All @@ -58,7 +58,7 @@ _commands:
steps:
- save_cache:
name: Save Cache << parameters.key >>
key: "<< parameters.key >>-v11\
key: "<< parameters.key >>-v12\
-{{ arch }}\
-{{ .Branch }}\
-{{ .Environment.CIRCLE_PR_NUMBER }}\
Expand Down Expand Up @@ -447,6 +447,7 @@ _environments:
RCUTILS_LOGGING_BUFFERED_STREAM: "0"
RCUTILS_LOGGING_USE_STDOUT: "0"
DEBIAN_FRONTEND: "noninteractive"
PYTHONUNBUFFERED: "1"

executors:
release_exec:
Expand Down
1 change: 1 addition & 0 deletions Dockerfile
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@ RUN echo '\
APT::Install-Recommends "0";\n\
APT::Install-Suggests "0";\n\
' > /etc/apt/apt.conf.d/01norecommend
ENV PYTHONUNBUFFERED 1

# install CI dependencies
ARG RTI_NC_LICENSE_ACCEPTED=yes
Expand Down
2 changes: 1 addition & 1 deletion nav2_amcl/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_amcl</name>
<version>1.1.2</version>
<version>1.1.3</version>
<description>
<p>
amcl is a probabilistic localization system for a robot moving in
Expand Down
35 changes: 35 additions & 0 deletions nav2_amcl/src/amcl_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1158,18 +1158,53 @@ AmclNode::dynamicParametersCallback(
if (param_type == ParameterType::PARAMETER_DOUBLE) {
if (param_name == "alpha1") {
alpha1_ = parameter.as_double();
//alpha restricted to be non-negative
if (alpha1_ < 0.0) {
RCLCPP_WARN(
get_logger(), "You've set alpha1 to be negative,"
" this isn't allowed, so the alpha1 will be set to be zero.");
alpha1_ = 0.0;
}
reinit_odom = true;
} else if (param_name == "alpha2") {
alpha2_ = parameter.as_double();
//alpha restricted to be non-negative
if (alpha2_ < 0.0) {
RCLCPP_WARN(
get_logger(), "You've set alpha2 to be negative,"
" this isn't allowed, so the alpha2 will be set to be zero.");
alpha2_ = 0.0;
}
reinit_odom = true;
} else if (param_name == "alpha3") {
alpha3_ = parameter.as_double();
//alpha restricted to be non-negative
if (alpha3_ < 0.0) {
RCLCPP_WARN(
get_logger(), "You've set alpha3 to be negative,"
" this isn't allowed, so the alpha3 will be set to be zero.");
alpha3_ = 0.0;
}
reinit_odom = true;
} else if (param_name == "alpha4") {
alpha4_ = parameter.as_double();
//alpha restricted to be non-negative
if (alpha4_ < 0.0) {
RCLCPP_WARN(
get_logger(), "You've set alpha4 to be negative,"
" this isn't allowed, so the alpha4 will be set to be zero.");
alpha4_ = 0.0;
}
reinit_odom = true;
} else if (param_name == "alpha5") {
alpha5_ = parameter.as_double();
//alpha restricted to be non-negative
if (alpha5_ < 0.0) {
RCLCPP_WARN(
get_logger(), "You've set alpha5 to be negative,"
" this isn't allowed, so the alpha5 will be set to be zero.");
alpha5_ = 0.0;
}
reinit_odom = true;
} else if (param_name == "beam_skip_distance") {
beam_skip_distance_ = parameter.as_double();
Expand Down
6 changes: 6 additions & 0 deletions nav2_behavior_tree/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,9 @@ list(APPEND plugin_libs nav2_spin_cancel_bt_node)
add_library(nav2_back_up_cancel_bt_node SHARED plugins/action/back_up_cancel_node.cpp)
list(APPEND plugin_libs nav2_back_up_cancel_bt_node)

add_library(nav2_assisted_teleop_cancel_bt_node SHARED plugins/action/assisted_teleop_cancel_node.cpp)
list(APPEND plugin_libs nav2_assisted_teleop_cancel_bt_node)

add_library(nav2_drive_on_heading_cancel_bt_node SHARED plugins/action/drive_on_heading_cancel_node.cpp)
list(APPEND plugin_libs nav2_drive_on_heading_cancel_bt_node)

Expand All @@ -90,6 +93,9 @@ list(APPEND plugin_libs nav2_spin_action_bt_node)
add_library(nav2_wait_action_bt_node SHARED plugins/action/wait_action.cpp)
list(APPEND plugin_libs nav2_wait_action_bt_node)

add_library(nav2_assisted_teleop_action_bt_node SHARED plugins/action/assisted_teleop_action.cpp)
list(APPEND plugin_libs nav2_assisted_teleop_action_bt_node)

add_library(nav2_clear_costmap_service_bt_node SHARED plugins/action/clear_costmap_service.cpp)
list(APPEND plugin_libs nav2_clear_costmap_service_bt_node)

Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,70 @@
// Copyright (c) 2022 Joshua Wallace
//
// 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__ASSISTED_TELEOP_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_ACTION_HPP_

#include <string>

#include "nav2_behavior_tree/bt_action_node.hpp"
#include "nav2_msgs/action/assisted_teleop.hpp"

namespace nav2_behavior_tree
{

/**
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::AssistedTeleop
*/
class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTeleop>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::nav2_msgs::action::AssistedTeleop
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
AssistedTeleopAction(
const std::string & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf);

/**
* @brief Function to perform some user-defined operation on tick
*/
void on_tick() override;

BT::NodeStatus on_aborted() override;

/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts()
{
return providedBasicPorts(
{
BT::InputPort<double>("time_allowance", 10.0, "Allowed time for running assisted teleop"),
BT::InputPort<bool>("is_recovery", false, "If true the recovery count will be incremented")
});
}

private:
bool is_recovery_;
};

} // namespace nav2_behavior_tree

#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_ACTION_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,59 @@
// Copyright (c) 2022 Joshua Wallace
//
// 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__ASSISTED_TELEOP_CANCEL_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_CANCEL_NODE_HPP_

#include <memory>
#include <string>

#include "nav2_msgs/action/assisted_teleop.hpp"

#include "nav2_behavior_tree/bt_cancel_action_node.hpp"

namespace nav2_behavior_tree
{

/**
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::BackUp
*/
class AssistedTeleopCancel : public BtCancelActionNode<nav2_msgs::action::AssistedTeleop>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::BackUpAction
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
AssistedTeleopCancel(
const std::string & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf);

/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts()
{
return providedBasicPorts(
{
});
}
};

} // namespace nav2_behavior_tree

#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_CANCEL_NODE_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,11 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
*/
BT::NodeStatus on_cancelled() override;

/**
* \brief Override required by the a BT action. Cancel the action and set the path output
*/
void halt() override;

/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
Expand Down
26 changes: 25 additions & 1 deletion nav2_behavior_tree/nav2_tree_nodes.xml
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@
<input_port name="server_timeout">Server timeout</input_port>
</Action>

<Action ID="DriveOnHeadingCancel">
<Action ID="CancelDriveOnHeading">
<input_port name="service_name">Service name to cancel the drive on heading behavior</input_port>
<input_port name="server_timeout">Server timeout</input_port>
</Action>
Expand All @@ -43,6 +43,11 @@
<input_port name="server_timeout">Server timeout</input_port>
</Action>

<Action ID="CancelAssistedTeleop">
<input_port name="service_name">Service name to cancel the assisted teleop behavior</input_port>
<input_port name="server_timeout">Server timeout</input_port>
</Action>

<Action ID="CancelWait">
<input_port name="service_name">Service name to cancel the wait behavior</input_port>
<input_port name="server_timeout">Server timeout</input_port>
Expand All @@ -53,6 +58,18 @@
<input_port name="server_timeout">Server timeout</input_port>
</Action>

<Action ID="ClearCostmapExceptRegion">
<input_port name="service_name">Service name</input_port>
<input_port name="server_timeout">Server timeout</input_port>
<input_port name="reset_distance">Distance from the robot above which obstacles are cleared</input_port>
</Action>

<Action ID="ClearCostmapAroundRobot">
<input_port name="service_name">Service name</input_port>
<input_port name="server_timeout">Server timeout</input_port>
<input_port name="reset_distance">Distance from the robot under which obstacles are cleared</input_port>
</Action>

<Action ID="ComputePathToPose">
<input_port name="goal">Destination to plan to</input_port>
<input_port name="start">Start pose of the path if overriding current robot pose</input_port>
Expand Down Expand Up @@ -164,6 +181,13 @@
<input_port name="server_timeout">Server timeout</input_port>
</Action>

<Action ID="AssistedTeleop">
<input_port name="time_allowance">Allowed time for spinning</input_port>
<input_port name="is_recovery">If true recovery count will be incremented</input_port>
<input_port name="server_name">Service name</input_port>
<input_port name="server_timeout">Server timeout</input_port>
</Action>

<!-- ############################### CONDITION NODES ############################## -->
<Condition ID="GoalReached">
<input_port name="goal">Destination</input_port>
Expand Down
2 changes: 1 addition & 1 deletion nav2_behavior_tree/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_behavior_tree</name>
<version>1.1.2</version>
<version>1.1.3</version>
<description>TODO</description>
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
<maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
Expand Down
62 changes: 62 additions & 0 deletions nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,62 @@
// Copyright (c) 2022 Joshua Wallace
//
// 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.

#include <string>
#include <memory>

#include "nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp"

namespace nav2_behavior_tree
{

AssistedTeleopAction::AssistedTeleopAction(
const std::string & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf)
: BtActionNode<nav2_msgs::action::AssistedTeleop>(xml_tag_name, action_name, conf)
{
double time_allowance;
getInput("time_allowance", time_allowance);
getInput("is_recovery", is_recovery_);

// Populate the input message
goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
}

void AssistedTeleopAction::on_tick()
{
if (is_recovery_) {
increment_recovery_count();
}
}

BT::NodeStatus AssistedTeleopAction::on_aborted()
{
return is_recovery_ ? BT::NodeStatus::FAILURE : BT::NodeStatus::SUCCESS;
}

} // namespace nav2_behavior_tree

#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
BT::NodeBuilder builder =
[](const std::string & name, const BT::NodeConfiguration & config)
{
return std::make_unique<nav2_behavior_tree::AssistedTeleopAction>(
name, "assisted_teleop", config);
};

factory.registerBuilder<nav2_behavior_tree::AssistedTeleopAction>("AssistedTeleop", builder);
}
Loading