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 cloi #22

Closed
wants to merge 5 commits into from
Closed
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
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