diff --git a/README.md b/README.md
index b65b1c4..b9b5b75 100644
--- a/README.md
+++ b/README.md
@@ -122,7 +122,7 @@ This pilot has been tested on different platforms. Above we show how to run the
1. **Launch turtlebot3 world in gazebo sim**
```console
- export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:[ros2_ws]/src/turtlebot3/turtlebot3_simulations/turtlebot3_gazebo/models
+ export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:[ros2_ws]/src/turtlebot3/turtlebot3_simulations/turtlebot3_gazebo/models:[ros2_ws]/src/Pilot-URJC/pilot_urjc_bringup/worlds/models
export TURTLEBOT3_MODEL=${TB3_MODEL}
ros2 launch pilot_urjc_bringup tb3_sim_launch.py
```
diff --git a/dependencies.repos b/dependencies.repos
index 167a673..61adfc4 100644
--- a/dependencies.repos
+++ b/dependencies.repos
@@ -14,3 +14,8 @@ repositories:
url: https://github.com/MROS-RobMoSys-ITP/pointcloud_to_laserscan
version: managed_node
+ utils/system_modes:
+ type: git
+ url: https://github.com/micro-ROS/system_modes
+ version: feature/rules
+
diff --git a/laser_resender/CMakeLists.txt b/laser_resender/CMakeLists.txt
index 437d442..87f303b 100644
--- a/laser_resender/CMakeLists.txt
+++ b/laser_resender/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.5)
project(laser_resender)
set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_BUILD_TYPE DEBUG)
+#set(CMAKE_BUILD_TYPE DEBUG)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
@@ -22,6 +22,12 @@ set(dependencies
add_executable(laser_resender_node src/laser_resender_node.cpp)
ament_target_dependencies(laser_resender_node ${dependencies})
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ find_package(ament_lint_common REQUIRED)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
install(TARGETS
laser_resender_node
ARCHIVE DESTINATION lib
diff --git a/laser_resender/package.xml b/laser_resender/package.xml
index 5af7478..bab04ce 100644
--- a/laser_resender/package.xml
+++ b/laser_resender/package.xml
@@ -18,6 +18,9 @@
sensor_msgs
lifecycle_msgs
+ ament_lint_auto
+ ament_lint_common
+
ament_cmake
diff --git a/laser_resender/src/laser_resender_node.cpp b/laser_resender/src/laser_resender_node.cpp
index 9980725..b3936e5 100644
--- a/laser_resender/src/laser_resender_node.cpp
+++ b/laser_resender/src/laser_resender_node.cpp
@@ -20,24 +20,26 @@
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
-// Execute:
-// ros2 lifecycle list /lifecycle_node_example
-// ros2 lifecycle set /lifecycle_node_example configure
+// Execute:
+// ros2 lifecycle list /laser_resender
+// ros2 lifecycle get /laser_resender
+// ros2 lifecycle set /laser_resender configure
using rcl_interfaces::msg::ParameterType;
using std::placeholders::_1;
using namespace std::placeholders;
-
class LaserResender : public rclcpp_lifecycle::LifecycleNode
{
public:
LaserResender()
: rclcpp_lifecycle::LifecycleNode("laser_resender")
{
+ declare_parameter("node_name");
pub_ = create_publisher("/mros_scan", rclcpp::SensorDataQoS());
- sub_ = create_subscription
- ("/scan", rclcpp::SensorDataQoS(), std::bind(&LaserResender::scan_cb, this, _1));
+ sub_ = create_subscription(
+ "/scan",
+ rclcpp::SensorDataQoS(), std::bind(&LaserResender::scan_cb, this, _1));
}
using CallbackReturnT =
@@ -45,51 +47,94 @@ class LaserResender : public rclcpp_lifecycle::LifecycleNode
CallbackReturnT on_configure(const rclcpp_lifecycle::State & state)
{
- RCLCPP_INFO(get_logger(), "[%s] Configuring from [%s] state...", get_name(), state.label().c_str());
+ RCLCPP_INFO(
+ get_logger(), "[%s] Configuring from [%s] state...",
+ get_name(),
+ state.label().c_str());
return CallbackReturnT::SUCCESS;
}
- CallbackReturnT on_activate(const rclcpp_lifecycle::State & state)
+ CallbackReturnT on_activate(const rclcpp_lifecycle::State & state)
{
- RCLCPP_INFO(get_logger(), "[%s] Activating from [%s] state...", get_name(), state.label().c_str());
+ RCLCPP_INFO(
+ get_logger(), "[%s] Activating from [%s] state...",
+ get_name(),
+ state.label().c_str());
pub_->on_activate();
return CallbackReturnT::SUCCESS;
}
- CallbackReturnT on_deactivate(const rclcpp_lifecycle::State & state)
+ CallbackReturnT on_deactivate(const rclcpp_lifecycle::State & state)
{
- RCLCPP_INFO(get_logger(), "[%s] Deactivating from [%s] state...", get_name(), state.label().c_str());
- return CallbackReturnT::SUCCESS;
+ if (all_zero_error_) {
+ return CallbackReturnT::ERROR;
+ } else {
+ RCLCPP_INFO(
+ get_logger(), "[%s] Deactivating from [%s] state...",
+ get_name(),
+ state.label().c_str());
+ return CallbackReturnT::SUCCESS;
+ }
}
- CallbackReturnT on_cleanup(const rclcpp_lifecycle::State & state)
+ CallbackReturnT on_cleanup(const rclcpp_lifecycle::State & state)
{
- RCLCPP_INFO(get_logger(), "[%s] Cleanning Up from [%s] state...", get_name(), state.label().c_str());
+ RCLCPP_INFO(
+ get_logger(), "[%s] Cleanning Up from [%s] state...",
+ get_name(),
+ state.label().c_str());
return CallbackReturnT::SUCCESS;
}
- CallbackReturnT on_shutdown(const rclcpp_lifecycle::State & state)
+ CallbackReturnT on_shutdown(const rclcpp_lifecycle::State & state)
{
- RCLCPP_INFO(get_logger(), "[%s] Shutting Down from [%s] state...", get_name(), state.label().c_str());
+ RCLCPP_INFO(
+ get_logger(), "[%s] Shutting Down from [%s] state...",
+ get_name(),
+ state.label().c_str());
return CallbackReturnT::SUCCESS;
}
- CallbackReturnT on_error(const rclcpp_lifecycle::State & state)
+ CallbackReturnT on_error(const rclcpp_lifecycle::State & state)
{
- RCLCPP_INFO(get_logger(), "[%s] Shutting Down from [%s] state...", get_name(), state.label().c_str());
+ RCLCPP_ERROR(
+ get_logger(), "[%s] Error processing from [%s] state...",
+ get_name(),
+ state.label().c_str());
return CallbackReturnT::SUCCESS;
}
void scan_cb(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan)
{
if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
- pub_->publish(*laser_scan);
+ all_zero_error_ = true;
+ for (auto range : laser_scan->ranges) {
+ if (range != 0.0) {
+ all_zero_error_ = false;
+ break;
+ }
+ }
+
+ if (!all_zero_error_) {
+ pub_->publish(*laser_scan);
+ } else {
+ RCLCPP_WARN(
+ get_logger(),
+ "[%s] ALL-ZEROS. It has to go to error processing state", get_name());
+ trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE);
+ }
}
+
+ /* if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+ {
+ trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+ } */
}
private:
rclcpp_lifecycle::LifecyclePublisher::SharedPtr pub_;
rclcpp::Subscription::SharedPtr sub_;
+ bool all_zero_error_;
};
int main(int argc, char * argv[])
@@ -101,4 +146,4 @@ int main(int argc, char * argv[])
rclcpp::shutdown();
return 0;
-}
\ No newline at end of file
+}
diff --git a/metacontroller_pilot/metacontroller_pilot/metacontroller_sim.py b/metacontroller_pilot/metacontroller_pilot/metacontroller_sim.py
index c6d880f..d393199 100644
--- a/metacontroller_pilot/metacontroller_pilot/metacontroller_sim.py
+++ b/metacontroller_pilot/metacontroller_pilot/metacontroller_sim.py
@@ -1,38 +1,20 @@
#!/usr/bin/env python
-# Software License Agreement (BSD License)
+# Copyright 2020 Intelligent Robotics Lab
#
-# Copyright (c) 2020, Intelligent Robotics Core S.L.
-# All rights reserved.
+# 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
#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
+# http://www.apache.org/licenses/LICENSE-2.0
#
-# * Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# * Redistributions in binary form must reproduce the above
-# copyright notice, this list of conditions and the following
-# disclaimer in the documentation and/or other materials provided
-# with the distribution.
-# * Neither the name of Willow Garage, Inc. nor the names of its
-# contributors may be used to endorse or promote products derived
-# from this software without specific prior written permission.
+# 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.
#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-# Author: Lorena Bajo Rebollo - lorena.bajo@urjc.es
+# Author: jginesclavero - jonatan.gines@urjc.es
import argparse
import functools
@@ -47,66 +29,45 @@
from rqt_gui_py.plugin import Plugin
from std_msgs.msg import Float32, Header
from system_modes.srv import ChangeMode
-
+from rcl_interfaces.msg import Log
class Metacontroller(Node):
- def __init__(self, node_name, mode_name):
+ def __init__(self):
super().__init__('metacontroller')
+ self.rosout_sub_ = self.create_subscription(
+ Log,
+ "/rosout",
+ self.rosout_cb, 1)
+ self.current_mode = 'NORMAL'
+ def change_mode(self, node_name, mode_name):
cli = self.create_client(ChangeMode, '/'+node_name+'/change_mode')
while not cli.wait_for_service(timeout_sec=1.0):
print('service not available, waiting again...')
req = ChangeMode.Request()
+ req.node_name = node_name
req.mode_name = mode_name
future = cli.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result() is not None:
self.get_logger().info('Mode change completed')
- sys.exit()
else:
self.get_logger().error('Exception while calling service: %r' % future.exception())
-def main(args=None):
- print ("------------------------------")
- print ("Specify the option number:")
- print ("------------------------------")
- print (" 0) Normal")
- print (" 1) Degraded (Navigate with pointcloud)")
- print (" 2) Performance")
- print (" 3) Energy saving")
- print (" 4) Slow")
- print ("------------------------------")
-
- option = input()
- if option == "0":
- print ("Normal")
- mode_name = 'NORMAL'
- elif option == "1":
- print ("Degraded (Navigate with pointcloud).")
- mode_name = 'DEGRADED'
- elif option == "2":
- print ("Performance")
- mode_name = 'PERFORMANCE'
- elif option == "3":
- print ("Energy saving.")
- mode_name = 'ENERGY_SAVING'
- elif option == "4":
- print ("Slow.")
- mode_name = 'SLOW'
- else:
- print("Invalid option.")
- sys.exit()
+ def rosout_cb(self, msg):
+ if msg.level == 40 and msg.function == "on_error":
+ if msg.name == "battery_contingency_sim" and self.current_mode == 'NORMAL':
+ self.current_mode = 'ENERGY_SAVING'
+ self.get_logger().info('Battery low detected, solving contingency...')
+ self.change_mode("pilot", self.current_mode)
+def main(args=None):
rclpy.init(args=args)
- node_name = "pilot"
- node = Metacontroller(node_name, mode_name)
-
- try:
- rclpy.spin(node)
- except KeyboardInterrupt:
- pass
-
- node.destroy_node()
+ node = Metacontroller()
+ node.change_mode("pilot", '__DEFAULT__')
+ node.change_mode("pilot", 'NORMAL')
+ rclpy.spin(node)
+ node.destroy()
rclpy.shutdown()
if __name__ == '__main__':
diff --git a/metacontroller_pilot/metacontroller_pilot/simple_mode_manager.py b/metacontroller_pilot/metacontroller_pilot/simple_mode_manager.py
new file mode 100644
index 0000000..c6d880f
--- /dev/null
+++ b/metacontroller_pilot/metacontroller_pilot/simple_mode_manager.py
@@ -0,0 +1,113 @@
+#!/usr/bin/env python
+
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2020, Intelligent Robotics Core S.L.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of Willow Garage, Inc. nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: Lorena Bajo Rebollo - lorena.bajo@urjc.es
+
+import argparse
+import functools
+import re
+import time
+import sys
+
+import rclpy
+import rclpy.logging
+from rclpy.node import Node
+
+from rqt_gui_py.plugin import Plugin
+from std_msgs.msg import Float32, Header
+from system_modes.srv import ChangeMode
+
+
+class Metacontroller(Node):
+ def __init__(self, node_name, mode_name):
+ super().__init__('metacontroller')
+ cli = self.create_client(ChangeMode, '/'+node_name+'/change_mode')
+ while not cli.wait_for_service(timeout_sec=1.0):
+ print('service not available, waiting again...')
+ req = ChangeMode.Request()
+ req.mode_name = mode_name
+
+ future = cli.call_async(req)
+ rclpy.spin_until_future_complete(self, future)
+ if future.result() is not None:
+ self.get_logger().info('Mode change completed')
+ sys.exit()
+ else:
+ self.get_logger().error('Exception while calling service: %r' % future.exception())
+
+def main(args=None):
+ print ("------------------------------")
+ print ("Specify the option number:")
+ print ("------------------------------")
+ print (" 0) Normal")
+ print (" 1) Degraded (Navigate with pointcloud)")
+ print (" 2) Performance")
+ print (" 3) Energy saving")
+ print (" 4) Slow")
+ print ("------------------------------")
+
+ option = input()
+ if option == "0":
+ print ("Normal")
+ mode_name = 'NORMAL'
+ elif option == "1":
+ print ("Degraded (Navigate with pointcloud).")
+ mode_name = 'DEGRADED'
+ elif option == "2":
+ print ("Performance")
+ mode_name = 'PERFORMANCE'
+ elif option == "3":
+ print ("Energy saving.")
+ mode_name = 'ENERGY_SAVING'
+ elif option == "4":
+ print ("Slow.")
+ mode_name = 'SLOW'
+ else:
+ print("Invalid option.")
+ sys.exit()
+
+ rclpy.init(args=args)
+ node_name = "pilot"
+ node = Metacontroller(node_name, mode_name)
+
+ try:
+ rclpy.spin(node)
+ except KeyboardInterrupt:
+ pass
+
+ node.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/metacontroller_pilot/setup.py b/metacontroller_pilot/setup.py
index f4e1f8b..b84b629 100644
--- a/metacontroller_pilot/setup.py
+++ b/metacontroller_pilot/setup.py
@@ -32,6 +32,7 @@
entry_points={
'console_scripts': [
'metacontroller = metacontroller_pilot.metacontroller_sim:main',
+ 'mode_manager = metacontroller_pilot.simple_mode_manager:main',
],
},
)
diff --git a/mros_contingencies_sim/CMakeLists.txt b/mros_contingencies_sim/CMakeLists.txt
new file mode 100644
index 0000000..63d6a88
--- /dev/null
+++ b/mros_contingencies_sim/CMakeLists.txt
@@ -0,0 +1,134 @@
+cmake_minimum_required(VERSION 3.5)
+project(mros_contingencies_sim)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+# Qt5 boilerplate options from http://doc.qt.io/qt-5/cmake-manual.html
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+set(CMAKE_AUTOMOC ON)
+
+find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(nav2_lifecycle_manager REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(rviz_common REQUIRED)
+find_package(rviz_default_plugins REQUIRED)
+find_package(rviz_ogre_vendor REQUIRED)
+find_package(rviz_rendering REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(diagnostic_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+
+set(mros_contingencies_panel_headers_to_moc
+ include/mros_contingencies_sim/rviz_plugin/mros_contingencies_panel.hpp
+)
+
+include_directories(
+ include
+)
+
+set(library_name ${PROJECT_NAME})
+
+add_library(${library_name} SHARED
+ src/rviz_plugin/mros_contingencies_panel.cpp
+ ${mros_contingencies_panel_headers_to_moc}
+)
+
+set(dependencies
+ geometry_msgs
+ nav2_util
+ nav2_lifecycle_manager
+ nav2_msgs
+ nav_msgs
+ pluginlib
+ Qt5
+ rclcpp
+ rclcpp_lifecycle
+ rviz_common
+ rviz_default_plugins
+ rviz_ogre_vendor
+ rviz_rendering
+ std_msgs
+ tf2_geometry_msgs
+ diagnostic_msgs
+)
+
+ament_target_dependencies(${library_name}
+ ${dependencies}
+)
+
+target_include_directories(${library_name} PUBLIC
+ ${Qt5Widgets_INCLUDE_DIRS}
+ ${OGRE_INCLUDE_DIRS}
+)
+
+target_link_libraries(${library_name}
+ rviz_common::rviz_common
+)
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+# TODO: Make this specific to this project (not rviz default plugins)
+target_compile_definitions(${library_name} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY")
+
+# prevent pluginlib from using boost
+target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+pluginlib_export_plugin_description_file(rviz_common plugins_description.xml)
+
+
+add_executable(battery_contingency_sim_node src/battery_contingency_node.cpp)
+ament_target_dependencies(battery_contingency_sim_node ${dependencies})
+
+install(
+ TARGETS ${library_name}
+ EXPORT ${library_name}
+ ARCHIVE DESTINATION lib
+ LIBRARY DESTINATION lib
+ RUNTIME DESTINATION bin
+ INCLUDES DESTINATION include
+)
+
+install(TARGETS
+ battery_contingency_sim_node
+ DESTINATION lib/${PROJECT_NAME}
+ INCLUDES DESTINATION include
+)
+
+install(
+ DIRECTORY include/
+ DESTINATION include/
+)
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ find_package(ament_lint_common REQUIRED)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(include)
+#ament_export_targets(${library_name} HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+ Qt5
+ rviz_common
+ geometry_msgs
+ map_msgs
+ nav_msgs
+ rclcpp
+)
+
+ament_package()
diff --git a/mros_contingencies_sim/include/mros_contingencies_sim/battery_contingency_node.hpp b/mros_contingencies_sim/include/mros_contingencies_sim/battery_contingency_node.hpp
new file mode 100644
index 0000000..2e5e45f
--- /dev/null
+++ b/mros_contingencies_sim/include/mros_contingencies_sim/battery_contingency_node.hpp
@@ -0,0 +1,51 @@
+// Copyright (c) 2020 Intelligent Robotics Lab - Rey Juan Carlos University
+//
+// 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 MROS_CONTINGENCIES_SIM__BATTERY_CONTINGENCY_NODE_HPP_
+#define MROS_CONTINGENCIES_SIM__BATTERY_CONTINGENCY_NODE_HPP_
+
+#include
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "std_msgs/msg/float64.hpp"
+#include "diagnostic_msgs/msg/diagnostic_array.hpp"
+
+namespace mros_contingencies_sim
+{
+
+class BatteryContingency : public rclcpp::Node
+{
+public:
+ explicit BatteryContingency(const std::string & name);
+
+private:
+ void amclCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
+ float calculateDistance(
+ float current_x, float current_y, float old_x, float old_y);
+ void setOldposition(geometry_msgs::msg::Pose current_pose);
+ void publish_diagnostic(std::string value);
+
+ rclcpp::Subscription::SharedPtr amcl_sub_;
+ rclcpp::Publisher::SharedPtr battery_pub_;
+ rclcpp::Publisher::SharedPtr diagnostics_pub_;
+ geometry_msgs::msg::Pose last_pose_;
+ float battery_level_;
+ const float BATTERY_CONSUMPTION = 2.0;
+};
+
+} // namespace mros_contingencies_sim
+
+#endif // MROS_CONTINGENCIES_SIM__BATTERY_CONTINGENCY_NODE_HPP_
diff --git a/mros_contingencies_sim/include/mros_contingencies_sim/rviz_plugin/mros_contingencies_panel.hpp b/mros_contingencies_sim/include/mros_contingencies_sim/rviz_plugin/mros_contingencies_panel.hpp
new file mode 100644
index 0000000..6b89d76
--- /dev/null
+++ b/mros_contingencies_sim/include/mros_contingencies_sim/rviz_plugin/mros_contingencies_panel.hpp
@@ -0,0 +1,80 @@
+// Copyright (c) 2020 Intelligent Robotics Lab - Rey Juan Carlos University
+//
+// 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 MROS_CONTINGENCIES_SIM__RVIZ_PLUGIN__MROS_CONTINGENCIES_PANEL_HPP_
+#define MROS_CONTINGENCIES_SIM__RVIZ_PLUGIN__MROS_CONTINGENCIES_PANEL_HPP_
+
+#include
+#include
+
+#include
+#include
+#include
+
+#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "rviz_common/panel.hpp"
+
+#include "lifecycle_msgs/msg/state.hpp"
+#include "lifecycle_msgs/msg/transition.hpp"
+#include "lifecycle_msgs/srv/change_state.hpp"
+
+#include "sensor_msgs/msg/laser_scan.hpp"
+
+class QPushButton;
+
+namespace nav2_rviz_plugins
+{
+
+/// Panel to interface to the nav2 stack
+class MROSContingenciesPanel : public rviz_common::Panel
+{
+ Q_OBJECT
+
+public:
+ explicit MROSContingenciesPanel(QWidget * parent = 0);
+ virtual ~MROSContingenciesPanel();
+
+ void onInitialize() override;
+
+ /// Load and save configuration data
+ void load(const rviz_common::Config & config) override;
+ void save(rviz_common::Config config) const override;
+
+private Q_SLOTS:
+ void onStartup();
+
+private:
+ // The (non-spinning) client node used to invoke the action client
+ rclcpp::Node::SharedPtr client_node_;
+ std::string managed_node_;
+ // A timer used to check on the completion status of the action
+ QBasicTimer timer_;
+
+ // The client used to control the laser driver
+ std::shared_ptr> client_laser_;
+ rclcpp::Publisher::SharedPtr pub_;
+
+ QPushButton * laser_contingency_button_{nullptr};
+ QStateMachine state_machine_;
+
+ QState * initial_{nullptr};
+ QState * idle_{nullptr};
+ int num_injected_msgs_;
+};
+
+} // namespace nav2_rviz_plugins
+
+#endif // MROS_CONTINGENCIES_SIM__RVIZ_PLUGIN__MROS_CONTINGENCIES_PANEL_HPP_
diff --git a/mros_contingencies_sim/package.xml b/mros_contingencies_sim/package.xml
new file mode 100644
index 0000000..e63aaf7
--- /dev/null
+++ b/mros_contingencies_sim/package.xml
@@ -0,0 +1,41 @@
+
+
+
+ mros_contingencies_sim
+ 0.4.1
+ MROS contingencies sim
+ jginesclavero
+ Apache-2.0
+
+ ament_cmake
+ qtbase5-dev
+
+ geometry_msgs
+ nav2_util
+ nav2_lifecycle_manager
+ nav2_msgs
+ nav_msgs
+ pluginlib
+ rclcpp
+ rclcpp_lifecycle
+ resource_retriever
+ rviz_common
+ rviz_default_plugins
+ rviz_ogre_vendor
+ rviz_rendering
+ std_msgs
+ tf2_geometry_msgs
+ visualization_msgs
+ diagnostic_msgs
+ libqt5-core
+ libqt5-gui
+ libqt5-opengl
+ libqt5-widgets
+
+ ament_lint_common
+ ament_lint_auto
+
+
+ ament_cmake
+
+
diff --git a/mros_contingencies_sim/plugins_description.xml b/mros_contingencies_sim/plugins_description.xml
new file mode 100644
index 0000000..18c3a39
--- /dev/null
+++ b/mros_contingencies_sim/plugins_description.xml
@@ -0,0 +1,9 @@
+
+
+
+ The MROS Contingencies rviz panel.
+
+
+
diff --git a/mros_contingencies_sim/src/battery_contingency_node.cpp b/mros_contingencies_sim/src/battery_contingency_node.cpp
new file mode 100644
index 0000000..5e58153
--- /dev/null
+++ b/mros_contingencies_sim/src/battery_contingency_node.cpp
@@ -0,0 +1,109 @@
+// Copyright (c) 2020 Intelligent Robotics Lab - Rey Juan Carlos University
+//
+// 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
+
+#include
+#include
+#include
+
+using namespace std::chrono_literals;
+using std::placeholders::_1;
+
+namespace mros_contingencies_sim
+{
+
+BatteryContingency::BatteryContingency(const std::string & name)
+: Node(name)
+{
+ amcl_sub_ = create_subscription(
+ "/amcl_pose", rclcpp::QoS(10), std::bind(
+ &BatteryContingency::amclCallback,
+ this,
+ _1));
+ battery_pub_ = create_publisher(
+ "/robot/battery",
+ rclcpp::QoS(10));
+ diagnostics_pub_ = create_publisher(
+ "/metacontroller/diagnostics",
+ rclcpp::QoS(10));
+ battery_level_ = 100.0;
+}
+
+float BatteryContingency::calculateDistance(
+ float current_x, float current_y, float old_x, float old_y)
+{
+ double diff_x = current_x - old_x;
+ double diff_y = current_y - old_y;
+ return sqrt(diff_x * diff_x + diff_y * diff_y);
+}
+
+void BatteryContingency::setOldposition(geometry_msgs::msg::Pose current_pose)
+{
+ last_pose_ = current_pose;
+}
+
+void BatteryContingency::publish_diagnostic(std::string value)
+{
+ diagnostic_msgs::msg::DiagnosticArray diagnostic_msg;
+ std::vector diag_status;
+ std::vector key_values;
+ diagnostic_msgs::msg::DiagnosticStatus status_msg;
+ diagnostic_msg.header.stamp = now();
+
+ status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+ diagnostic_msgs::msg::KeyValue key_value;
+ key_value.key = "energy";
+ key_value.value = value;
+ key_values.push_back(key_value);
+ status_msg.values = key_values;
+ status_msg.message = "QA status";
+ diag_status.push_back(status_msg);
+ diagnostic_msg.status = diag_status;
+ diagnostics_pub_->publish(diagnostic_msg);
+}
+
+void BatteryContingency::amclCallback(
+ const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
+{
+ float current_x, current_y, distance = 0.0;
+ current_x = msg->pose.pose.position.x;
+ current_y = msg->pose.pose.position.y;
+ if (last_pose_.position.x != 0.0 && last_pose_.position.x != 0.0) {
+ distance = calculateDistance(
+ current_x, current_y, last_pose_.position.x, last_pose_.position.y);
+ setOldposition(msg->pose.pose);
+ } else {
+ setOldposition(msg->pose.pose);
+ }
+
+ battery_level_ = battery_level_ - distance * BATTERY_CONSUMPTION;
+ if (battery_level_ < 0.0) {
+ battery_level_ = 0.0;
+ }
+ publish_diagnostic(std::to_string(battery_level_));
+}
+
+} // namespace mros_contingencies_sim
+
+int main(int argc, char * argv[])
+{
+ rclcpp::init(argc, argv);
+
+ auto node =
+ std::make_shared("battery_contingency_node");
+ rclcpp::spin(node);
+ rclcpp::shutdown();
+ return 0;
+}
diff --git a/mros_contingencies_sim/src/rviz_plugin/mros_contingencies_panel.cpp b/mros_contingencies_sim/src/rviz_plugin/mros_contingencies_panel.cpp
new file mode 100644
index 0000000..f7f5ca4
--- /dev/null
+++ b/mros_contingencies_sim/src/rviz_plugin/mros_contingencies_panel.cpp
@@ -0,0 +1,145 @@
+// Copyright (c) 2020 Intelligent Robotics Lab - Rey Juan Carlos University
+//
+// 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 "mros_contingencies_sim/rviz_plugin/mros_contingencies_panel.hpp"
+
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include "rviz_common/display_context.hpp"
+
+using namespace std::chrono_literals;
+
+namespace nav2_rviz_plugins
+{
+
+template
+std::future_status
+wait_for_result(
+ FutureT & future,
+ WaitTimeT time_to_wait)
+{
+ auto end = std::chrono::steady_clock::now() + time_to_wait;
+ std::chrono::milliseconds wait_period(100);
+ std::future_status status = std::future_status::timeout;
+ do {
+ auto now = std::chrono::steady_clock::now();
+ auto time_left = end - now;
+ if (time_left <= std::chrono::seconds(0)) {break;}
+ status = future.wait_for((time_left < wait_period) ? time_left : wait_period);
+ } while (rclcpp::ok() && status != std::future_status::ready);
+ return status;
+}
+
+MROSContingenciesPanel::MROSContingenciesPanel(QWidget * parent)
+: Panel(parent), managed_node_("laser_resender")
+{
+ // Create the control button and its tooltip
+
+ laser_contingency_button_ = new QPushButton;
+
+ // Create the state machine used to present the proper control button states in the UI
+
+ const char * laser_contingency_msg = "Configure and activate all nav2 lifecycle nodes";
+
+ initial_ = new QState();
+ initial_->setObjectName("initial");
+ initial_->assignProperty(
+ laser_contingency_button_,
+ "text",
+ "Inject all-zero laser msg in the system");
+ initial_->assignProperty(laser_contingency_button_, "toolTip", laser_contingency_msg);
+ initial_->assignProperty(laser_contingency_button_, "enabled", true);
+
+ // State entered when navigate_to_pose action is not active
+ idle_ = new QState();
+ idle_->setObjectName("idle");
+ idle_->assignProperty(
+ laser_contingency_button_,
+ "text",
+ "Inject all-zero laser msg in the system");
+ idle_->assignProperty(laser_contingency_button_, "enabled", false);
+
+ QObject::connect(initial_, SIGNAL(exited()), this, SLOT(onStartup()));
+ // Start/Reset button click transitions
+ initial_->addTransition(laser_contingency_button_, SIGNAL(clicked()), idle_);
+
+ state_machine_.addState(initial_);
+ state_machine_.addState(idle_);
+ state_machine_.setInitialState(initial_);
+
+ state_machine_.start();
+
+ // Lay out the items in the panel
+ QVBoxLayout * main_layout = new QVBoxLayout;
+ main_layout->addWidget(laser_contingency_button_);
+
+ main_layout->setContentsMargins(10, 10, 10, 10);
+ setLayout(main_layout);
+
+ client_node_ = std::make_shared("_");
+
+ std::string change_state_service_name = managed_node_ + "/change_state";
+
+ client_laser_ = client_node_->create_client(
+ change_state_service_name);
+ pub_ = client_node_->create_publisher(
+ "/scan",
+ rclcpp::SensorDataQoS());
+
+ num_injected_msgs_ = 10;
+}
+
+MROSContingenciesPanel::~MROSContingenciesPanel()
+{
+}
+
+void
+MROSContingenciesPanel::onInitialize()
+{
+ auto node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
+}
+
+void
+MROSContingenciesPanel::onStartup()
+{
+ int i = 0;
+ while (i < num_injected_msgs_) {
+ sensor_msgs::msg::LaserScan msg;
+ pub_->publish(msg);
+ i++;
+ }
+}
+
+void
+MROSContingenciesPanel::save(rviz_common::Config config) const
+{
+ Panel::save(config);
+}
+
+void
+MROSContingenciesPanel::load(const rviz_common::Config & config)
+{
+ Panel::load(config);
+}
+
+} // namespace nav2_rviz_plugins
+
+#include // NOLINT
+PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::MROSContingenciesPanel, rviz_common::Panel)
diff --git a/pilot_behavior/CMakeLists.txt b/pilot_behavior/CMakeLists.txt
index 029ef33..bb3f042 100644
--- a/pilot_behavior/CMakeLists.txt
+++ b/pilot_behavior/CMakeLists.txt
@@ -30,35 +30,11 @@ set(dependencies
behaviortree_cpp_v3
)
-add_library(pilot_behavior_get_order_bt_node SHARED
- src/behavior_tree_nodes/GetOrder.cpp
-)
-list(APPEND plugin_libs pilot_behavior_get_order_bt_node)
-
-add_library(pilot_behavior_navigate_to_barman_bt_node SHARED
- src/behavior_tree_nodes/NavigateToBarman.cpp
-)
-list(APPEND plugin_libs pilot_behavior_navigate_to_barman_bt_node)
-
-add_library(pilot_behavior_interact_with_barman_bt_node SHARED
- src/behavior_tree_nodes/InteractWithBarman.cpp
-)
-list(APPEND plugin_libs pilot_behavior_interact_with_barman_bt_node)
-
-add_library(pilot_behavior_navigate_to_client_bt_node SHARED
- src/behavior_tree_nodes/NavigateToClient.cpp
-)
-list(APPEND plugin_libs pilot_behavior_navigate_to_client_bt_node)
-
-add_library(pilot_behavior_deliver_order_bt_node SHARED
- src/behavior_tree_nodes/DeliverOrder.cpp
-)
-list(APPEND plugin_libs pilot_behavior_deliver_order_bt_node)
-add_library(pilot_behavior_check_order_bt_node SHARED
- src/behavior_tree_nodes/CheckOrder.cpp
+add_library(pilot_behavior_navigate_to_wp_bt_node SHARED
+ src/behavior_tree_nodes/NavigateToWp.cpp
)
-list(APPEND plugin_libs pilot_behavior_check_order_bt_node)
+list(APPEND plugin_libs pilot_behavior_navigate_to_wp_bt_node)
foreach(bt_plugin ${plugin_libs})
ament_target_dependencies(${bt_plugin} ${dependencies})
diff --git a/pilot_behavior/behavior_trees/bt.xml b/pilot_behavior/behavior_trees/bt.xml
index 1bc6e0d..e357848 100644
--- a/pilot_behavior/behavior_trees/bt.xml
+++ b/pilot_behavior/behavior_trees/bt.xml
@@ -1,12 +1,15 @@
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
-
\ No newline at end of file
+
\ No newline at end of file
diff --git a/pilot_behavior/include/pilot_behavior/behavior_tree_nodes/CheckOrder.hpp b/pilot_behavior/include/pilot_behavior/behavior_tree_nodes/CheckOrder.hpp
deleted file mode 100644
index eacb20e..0000000
--- a/pilot_behavior/include/pilot_behavior/behavior_tree_nodes/CheckOrder.hpp
+++ /dev/null
@@ -1,50 +0,0 @@
-// Copyright 2020 Intelligent Robotics Lab
-//
-// 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 PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__CHECKORDER_HPP_
-#define PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__CHECKORDER_HPP_
-
-#include
-#include
-
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
-
-#include "rclcpp/rclcpp.hpp"
-
-namespace pilot_behavior
-{
-
-class CheckOrder : public BT::ActionNodeBase
-{
-public:
- explicit CheckOrder(
- const std::string & xml_tag_name,
- const BT::NodeConfiguration & conf);
-
- void halt() {}
- BT::NodeStatus tick() override;
-
- static BT::PortsList providedPorts()
- {
- return BT::PortsList({});
- }
-
-protected:
- rclcpp::Node::SharedPtr node_;
-};
-
-} // namespace pilot_behavior
-
-#endif // PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__CHECKORDER_HPP_
diff --git a/pilot_behavior/include/pilot_behavior/behavior_tree_nodes/DeliverOrder.hpp b/pilot_behavior/include/pilot_behavior/behavior_tree_nodes/DeliverOrder.hpp
deleted file mode 100644
index e24774f..0000000
--- a/pilot_behavior/include/pilot_behavior/behavior_tree_nodes/DeliverOrder.hpp
+++ /dev/null
@@ -1,50 +0,0 @@
-// Copyright 2020 Intelligent Robotics Lab
-//
-// 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 PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__DELIVERORDER_HPP_
-#define PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__DELIVERORDER_HPP_
-
-#include
-#include
-
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
-
-#include "rclcpp/rclcpp.hpp"
-
-namespace pilot_behavior
-{
-
-class DeliverOrder : public BT::ActionNodeBase
-{
-public:
- explicit DeliverOrder(
- const std::string & xml_tag_name,
- const BT::NodeConfiguration & conf);
-
- void halt() {}
- BT::NodeStatus tick() override;
-
- static BT::PortsList providedPorts()
- {
- return BT::PortsList({});
- }
-
-protected:
- rclcpp::Node::SharedPtr node_;
-};
-
-} // namespace pilot_behavior
-
-#endif // PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__DELIVERORDER_HPP_
diff --git a/pilot_behavior/include/pilot_behavior/behavior_tree_nodes/GetOrder.hpp b/pilot_behavior/include/pilot_behavior/behavior_tree_nodes/GetOrder.hpp
deleted file mode 100644
index 08b371c..0000000
--- a/pilot_behavior/include/pilot_behavior/behavior_tree_nodes/GetOrder.hpp
+++ /dev/null
@@ -1,53 +0,0 @@
-// Copyright 2020 Intelligent Robotics Lab
-//
-// 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 PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__GETORDER_HPP_
-#define PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__GETORDER_HPP_
-
-#include
-#include
-
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
-
-#include "ros2_knowledge_graph/GraphNode.hpp"
-
-#include "rclcpp/rclcpp.hpp"
-
-namespace pilot_behavior
-{
-
-class GetOrder : public BT::ActionNodeBase
-{
-public:
- explicit GetOrder(
- const std::string & xml_tag_name,
- const BT::NodeConfiguration & conf);
-
- void halt() {}
- BT::NodeStatus tick() override;
-
- static BT::PortsList providedPorts()
- {
- return BT::PortsList({});
- }
-
-protected:
- rclcpp::Node::SharedPtr node_;
- std::shared_ptr graph_;
-};
-
-} // namespace pilot_behavior
-
-#endif // PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__GETORDER_HPP_
diff --git a/pilot_behavior/include/pilot_behavior/behavior_tree_nodes/InteractWithBarman.hpp b/pilot_behavior/include/pilot_behavior/behavior_tree_nodes/InteractWithBarman.hpp
deleted file mode 100644
index acedc12..0000000
--- a/pilot_behavior/include/pilot_behavior/behavior_tree_nodes/InteractWithBarman.hpp
+++ /dev/null
@@ -1,53 +0,0 @@
-// Copyright 2020 Intelligent Robotics Lab
-//
-// 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 PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__INTERACTWITHBARMAN_HPP_
-#define PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__INTERACTWITHBARMAN_HPP_
-
-#include
-#include
-
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
-
-#include "ros2_knowledge_graph/GraphNode.hpp"
-
-#include "rclcpp/rclcpp.hpp"
-
-namespace pilot_behavior
-{
-
-class InteractWithBarman : public BT::ActionNodeBase
-{
-public:
- explicit InteractWithBarman(
- const std::string & xml_tag_name,
- const BT::NodeConfiguration & conf);
-
- void halt() {}
- BT::NodeStatus tick() override;
-
- static BT::PortsList providedPorts()
- {
- return BT::PortsList({});
- }
-
-protected:
- rclcpp::Node::SharedPtr node_;
- std::shared_ptr graph_;
-};
-
-} // namespace pilot_behavior
-
-#endif // PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__INTERACTWITHBARMAN_HPP_
diff --git a/pilot_behavior/include/pilot_behavior/behavior_tree_nodes/NavigateToClient.hpp b/pilot_behavior/include/pilot_behavior/behavior_tree_nodes/NavigateToClient.hpp
deleted file mode 100644
index 080906d..0000000
--- a/pilot_behavior/include/pilot_behavior/behavior_tree_nodes/NavigateToClient.hpp
+++ /dev/null
@@ -1,50 +0,0 @@
-// Copyright 2020 Intelligent Robotics Lab
-//
-// 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 PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__NAVIGATETOCLIENT_HPP_
-#define PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__NAVIGATETOCLIENT_HPP_
-
-#include
-#include
-
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
-
-#include "rclcpp/rclcpp.hpp"
-
-namespace pilot_behavior
-{
-
-class NavigateToClient : public BT::ActionNodeBase
-{
-public:
- explicit NavigateToClient(
- const std::string & xml_tag_name,
- const BT::NodeConfiguration & conf);
-
- void halt() {}
- BT::NodeStatus tick() override;
-
- static BT::PortsList providedPorts()
- {
- return BT::PortsList({});
- }
-
-protected:
- rclcpp::Node::SharedPtr node_;
-};
-
-} // namespace pilot_behavior
-
-#endif // PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__NAVIGATETOCLIENT_HPP_
diff --git a/pilot_behavior/include/pilot_behavior/behavior_tree_nodes/NavigateToBarman.hpp b/pilot_behavior/include/pilot_behavior/behavior_tree_nodes/NavigateToWp.hpp
similarity index 69%
rename from pilot_behavior/include/pilot_behavior/behavior_tree_nodes/NavigateToBarman.hpp
rename to pilot_behavior/include/pilot_behavior/behavior_tree_nodes/NavigateToWp.hpp
index af032a5..82b1530 100644
--- a/pilot_behavior/include/pilot_behavior/behavior_tree_nodes/NavigateToBarman.hpp
+++ b/pilot_behavior/include/pilot_behavior/behavior_tree_nodes/NavigateToWp.hpp
@@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__NAVIGATETOBARMAN_HPP_
-#define PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__NAVIGATETOBARMAN_HPP_
+#ifndef PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__NAVIGATETOWP_HPP_
+#define PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__NAVIGATETOWP_HPP_
#include
#include
@@ -24,16 +24,15 @@
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "pilot_behavior/BTActionNode.hpp"
-
#include "rclcpp/rclcpp.hpp"
namespace pilot_behavior
{
-class NavigateToBarman : public BtActionNode
+class NavigateToWp : public BtActionNode
{
public:
- explicit NavigateToBarman(
+ explicit NavigateToWp(
const std::string & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf);
@@ -43,10 +42,16 @@ class NavigateToBarman : public BtActionNode
static BT::PortsList providedPorts()
{
- return BT::PortsList({});
+ return {
+ BT::InputPort("server_name", "Action server name"),
+ BT::InputPort("goal")
+ };
}
+private:
+ rclcpp::Node::SharedPtr node_;
+ geometry_msgs::msg::Pose wp_;
};
} // namespace pilot_behavior
-#endif // PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__NAVIGATETOBARMAN_HPP_
+#endif // PILOT_BEHAVIOR__BEHAVIOR_TREE_NODES__NAVIGATETOWP_HPP_
diff --git a/pilot_behavior/launch/pilot_urjc_launch.py b/pilot_behavior/launch/pilot_urjc_launch.py
index f85b5ed..1ef8f70 100644
--- a/pilot_behavior/launch/pilot_urjc_launch.py
+++ b/pilot_behavior/launch/pilot_urjc_launch.py
@@ -24,16 +24,6 @@ def generate_launch_description():
output='screen',
parameters=[])
- rqt_bica_graph_cmd = Node(
- package='rqt_gui',
- node_executable='rqt_gui',
- node_name='bica_rqt_graph',
- output='screen',
- parameters=[{'-s': 'bica_rqt_graph'}])
-
ld = LaunchDescription()
-
ld.add_action(pilot_behavior_cmd)
- ld.add_action(rqt_bica_graph_cmd)
-
return ld
diff --git a/pilot_behavior/src/behavior_tree_nodes/CheckOrder.cpp b/pilot_behavior/src/behavior_tree_nodes/CheckOrder.cpp
deleted file mode 100644
index 6a24ea4..0000000
--- a/pilot_behavior/src/behavior_tree_nodes/CheckOrder.cpp
+++ /dev/null
@@ -1,45 +0,0 @@
-// Copyright 2020 Intelligent Robotics Lab
-//
-// 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
-#include
-
-#include "pilot_behavior/behavior_tree_nodes/CheckOrder.hpp"
-
-namespace pilot_behavior
-{
-
-CheckOrder::CheckOrder(
- const std::string & xml_tag_name,
- const BT::NodeConfiguration & conf)
-: BT::ActionNodeBase(xml_tag_name, conf)
-{
- node_ = config().blackboard->get("node");
-}
-
-BT::NodeStatus
-CheckOrder::tick()
-{
- RCLCPP_INFO(node_->get_logger(), "CheckOrder");
-
- return BT::NodeStatus::SUCCESS;
-}
-
-} // namespace pilot_behavior
-
-#include "behaviortree_cpp_v3/bt_factory.h"
-BT_REGISTER_NODES(factory)
-{
- factory.registerNodeType("CheckOrder");
-}
diff --git a/pilot_behavior/src/behavior_tree_nodes/DeliverOrder.cpp b/pilot_behavior/src/behavior_tree_nodes/DeliverOrder.cpp
deleted file mode 100644
index 98db39b..0000000
--- a/pilot_behavior/src/behavior_tree_nodes/DeliverOrder.cpp
+++ /dev/null
@@ -1,45 +0,0 @@
-// Copyright 2020 Intelligent Robotics Lab
-//
-// 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
-#include
-
-#include "pilot_behavior/behavior_tree_nodes/DeliverOrder.hpp"
-
-namespace pilot_behavior
-{
-
-DeliverOrder::DeliverOrder(
- const std::string & xml_tag_name,
- const BT::NodeConfiguration & conf)
-: BT::ActionNodeBase(xml_tag_name, conf)
-{
- node_ = config().blackboard->get("node");
-}
-
-BT::NodeStatus
-DeliverOrder::tick()
-{
- RCLCPP_INFO(node_->get_logger(), "DeliverOrder");
-
- return BT::NodeStatus::SUCCESS;
-}
-
-} // namespace pilot_behavior
-
-#include "behaviortree_cpp_v3/bt_factory.h"
-BT_REGISTER_NODES(factory)
-{
- factory.registerNodeType("DeliverOrder");
-}
diff --git a/pilot_behavior/src/behavior_tree_nodes/GetOrder.cpp b/pilot_behavior/src/behavior_tree_nodes/GetOrder.cpp
deleted file mode 100644
index 955a4f0..0000000
--- a/pilot_behavior/src/behavior_tree_nodes/GetOrder.cpp
+++ /dev/null
@@ -1,57 +0,0 @@
-// Copyright 2020 Intelligent Robotics Lab
-//
-// 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
-#include
-
-#include "pilot_behavior/behavior_tree_nodes/GetOrder.hpp"
-
-namespace pilot_behavior
-{
-
-GetOrder::GetOrder(
- const std::string & xml_tag_name,
- const BT::NodeConfiguration & conf)
-: BT::ActionNodeBase(xml_tag_name, conf)
-{
- node_ = config().blackboard->get("node");
- //graph_ = std::make_shared(xml_tag_name + node_->get_name());
- //graph_->start();
- graph_ = config().blackboard->get>("pilot_graph");
-}
-
-BT::NodeStatus
-GetOrder::tick()
-{
- graph_->add_node(ros2_knowledge_graph::Node{"sonny", "robot"});
- graph_->add_node(ros2_knowledge_graph::Node{"table_1", "table"});
- graph_->add_node(ros2_knowledge_graph::Node{"bar", "table"});
- graph_->add_node(ros2_knowledge_graph::Node{"water", "object"});
- graph_->add_node(ros2_knowledge_graph::Node{"toastie", "object"});
- graph_->add_node(ros2_knowledge_graph::Node{"mango_juice", "object"});
-
- graph_->add_edge(ros2_knowledge_graph::Edge{"wants", "symbolic", "table_1", "water"});
- graph_->add_edge(ros2_knowledge_graph::Edge{"wants", "symbolic", "table_1", "toastie"});
- graph_->add_edge(ros2_knowledge_graph::Edge{"wants", "symbolic", "table_1", "mango_juice"});
-
- return BT::NodeStatus::SUCCESS;
-}
-
-} // namespace pilot_behavior
-
-#include "behaviortree_cpp_v3/bt_factory.h"
-BT_REGISTER_NODES(factory)
-{
- factory.registerNodeType("GetOrder");
-}
diff --git a/pilot_behavior/src/behavior_tree_nodes/InteractWithBarman.cpp b/pilot_behavior/src/behavior_tree_nodes/InteractWithBarman.cpp
deleted file mode 100644
index 23bfa02..0000000
--- a/pilot_behavior/src/behavior_tree_nodes/InteractWithBarman.cpp
+++ /dev/null
@@ -1,72 +0,0 @@
-// Copyright 2020 Intelligent Robotics Lab
-//
-// 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
-#include
-
-#include "pilot_behavior/behavior_tree_nodes/InteractWithBarman.hpp"
-
-namespace pilot_behavior
-{
-
-InteractWithBarman::InteractWithBarman(
- const std::string & xml_tag_name,
- const BT::NodeConfiguration & conf)
-: BT::ActionNodeBase(xml_tag_name, conf)
-{
- node_ = config().blackboard->get("node");
- //graph_ = std::make_shared(xml_tag_name + node_->get_name());
- //graph_->start();
- graph_ = config().blackboard->get>("pilot_graph");
-}
-
-BT::NodeStatus
-InteractWithBarman::tick()
-{
- // TODO(fmrico): Decir la comanda al barman
- std::string text_to_say = "The table_1 wants: ";
-
- if (graph_->get_num_nodes() == 0)
- {
- RCLCPP_WARN(node_->get_logger(), "0 nodes received");
- return BT::NodeStatus::RUNNING;
- }
-
- RCLCPP_INFO(
- node_->get_logger(), "Nodes table_1 [%d]", graph_->get_node_names_by_id(
- "table_1").size());
- RCLCPP_INFO(
- node_->get_logger(), "Edges in table_1 [%d]", graph_->get_edges_from_node(
- "table_1").size());
-
- auto edges = graph_->get_edges_from_node_by_data("table_1", "wants", "symbolic");
-
- for (auto & edge : edges) {
- text_to_say += edge.target + " ,";
- }
-
- RCLCPP_INFO(node_->get_logger(), "%s", text_to_say.c_str());
-
- config().blackboard->set("text", text_to_say);
-
- return BT::NodeStatus::SUCCESS;
-}
-
-} // namespace pilot_behavior
-
-#include "behaviortree_cpp_v3/bt_factory.h"
-BT_REGISTER_NODES(factory)
-{
- factory.registerNodeType("InteractWithBarman");
-}
diff --git a/pilot_behavior/src/behavior_tree_nodes/NavigateToClient.cpp b/pilot_behavior/src/behavior_tree_nodes/NavigateToClient.cpp
deleted file mode 100644
index 40b5360..0000000
--- a/pilot_behavior/src/behavior_tree_nodes/NavigateToClient.cpp
+++ /dev/null
@@ -1,45 +0,0 @@
-// Copyright 2020 Intelligent Robotics Lab
-//
-// 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
-#include
-
-#include "pilot_behavior/behavior_tree_nodes/NavigateToClient.hpp"
-
-namespace pilot_behavior
-{
-
-NavigateToClient::NavigateToClient(
- const std::string & xml_tag_name,
- const BT::NodeConfiguration & conf)
-: BT::ActionNodeBase(xml_tag_name, conf)
-{
- node_ = config().blackboard->get("node");
-}
-
-BT::NodeStatus
-NavigateToClient::tick()
-{
- RCLCPP_INFO(node_->get_logger(), "NavigateToClient");
-
- return BT::NodeStatus::SUCCESS;
-}
-
-} // namespace pilot_behavior
-
-#include "behaviortree_cpp_v3/bt_factory.h"
-BT_REGISTER_NODES(factory)
-{
- factory.registerNodeType("NavigateToClient");
-}
diff --git a/pilot_behavior/src/behavior_tree_nodes/NavigateToBarman.cpp b/pilot_behavior/src/behavior_tree_nodes/NavigateToWp.cpp
similarity index 63%
rename from pilot_behavior/src/behavior_tree_nodes/NavigateToBarman.cpp
rename to pilot_behavior/src/behavior_tree_nodes/NavigateToWp.cpp
index c02e91c..2dca9a6 100644
--- a/pilot_behavior/src/behavior_tree_nodes/NavigateToBarman.cpp
+++ b/pilot_behavior/src/behavior_tree_nodes/NavigateToWp.cpp
@@ -15,33 +15,36 @@
#include
#include
-#include "pilot_behavior/behavior_tree_nodes/NavigateToBarman.hpp"
+#include "pilot_behavior/behavior_tree_nodes/NavigateToWp.hpp"
namespace pilot_behavior
{
-NavigateToBarman::NavigateToBarman(
+NavigateToWp::NavigateToWp(
const std::string & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf)
: BtActionNode(xml_tag_name, action_name, conf)
{
+ node_ = config().blackboard->get("node");
+
}
void
-NavigateToBarman::on_tick()
+NavigateToWp::on_tick()
{
- geometry_msgs::msg::PoseStamped goal;
-
- goal.pose.position.x = -0.5;
- goal.pose.position.y = -0.5;
- goal.header.frame_id = "map";
-
- goal_.pose = goal;
+ auto wp_map =
+ config().blackboard->get>("wp_map");
+
+ auto res = getInput("goal").value();
+ wp_ = wp_map[res];
+ RCLCPP_INFO(node_->get_logger(), "Navigating to... [%s -- %f %f]",
+ res.c_str(), wp_.position.x, wp_.position.y);
+ goal_.pose.pose = wp_;
}
BT::NodeStatus
-NavigateToBarman::on_success()
+NavigateToWp::on_success()
{
return BT::NodeStatus::SUCCESS;
}
@@ -54,10 +57,10 @@ BT_REGISTER_NODES(factory)
BT::NodeBuilder builder =
[](const std::string & name, const BT::NodeConfiguration & config)
{
- return std::make_unique(
+ return std::make_unique(
name, "navigate_to_pose", config);
};
- factory.registerBuilder(
- "NavigateToBarman", builder);
+ factory.registerBuilder(
+ "NavigateToWp", builder);
}
diff --git a/pilot_behavior/src/pilot_behavior.cpp b/pilot_behavior/src/pilot_behavior.cpp
index 686ab29..ec7c5f1 100644
--- a/pilot_behavior/src/pilot_behavior.cpp
+++ b/pilot_behavior/src/pilot_behavior.cpp
@@ -30,6 +30,7 @@
#include "ament_index_cpp/get_package_share_directory.hpp"
#include "ros2_knowledge_graph/GraphNode.hpp"
+#include "nav2_util/geometry_utils.hpp"
#include "rclcpp/rclcpp.hpp"
@@ -42,19 +43,52 @@ int main(int argc, char ** argv)
BT::BehaviorTreeFactory factory;
BT::SharedLibrary loader;
- factory.registerFromPlugin(loader.getOSName("pilot_behavior_deliver_order_bt_node"));
- factory.registerFromPlugin(loader.getOSName("pilot_behavior_interact_with_barman_bt_node"));
- factory.registerFromPlugin(loader.getOSName("pilot_behavior_get_order_bt_node"));
- factory.registerFromPlugin(loader.getOSName("pilot_behavior_navigate_to_barman_bt_node"));
- factory.registerFromPlugin(loader.getOSName("pilot_behavior_navigate_to_client_bt_node"));
- factory.registerFromPlugin(loader.getOSName("pilot_behavior_check_order_bt_node"));
-
+ factory.registerFromPlugin(loader.getOSName("pilot_behavior_navigate_to_wp_bt_node"));
+
auto blackboard = BT::Blackboard::create();
auto node = rclcpp::Node::make_shared("pilot_node");
auto graph = std::make_shared("pilot_graph");
graph->start();
blackboard->set("node", node);
- blackboard->set("pilot_graph", graph);
+ blackboard->set("pilot_graph", graph);
+
+ std::unordered_map wp_map;
+ geometry_msgs::msg::Pose wp;
+ wp.position.x = 1.0;
+ wp.position.y = -1.0;
+ wp.orientation = nav2_util::geometry_utils::orientationAroundZAxis(M_PI_2);
+ wp_map.insert(std::pair("wp_1", wp));
+ wp.position.x = -1.0;
+ wp.position.y = 1.0;
+ wp.orientation = nav2_util::geometry_utils::orientationAroundZAxis(M_PI);
+ wp_map.insert(std::pair("wp_2", wp));
+
+ wp.position.x = -3.5;
+ wp.position.y = 1.0;
+ wp.orientation = nav2_util::geometry_utils::orientationAroundZAxis(M_PI);
+ wp_map.insert(std::pair("wp_3", wp));
+
+ wp.position.x = -6.25;
+ wp.position.y = 2.66;
+ wp.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI_2);
+ wp_map.insert(std::pair("wp_4", wp));
+
+ wp.position.x = -6.40;
+ wp.position.y = -2.81;
+ wp.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI_2);
+ wp_map.insert(std::pair("wp_5", wp));
+
+ wp.position.x = -6.25;
+ wp.position.y = 2.66;
+ wp.orientation = nav2_util::geometry_utils::orientationAroundZAxis(M_PI_2);
+ wp_map.insert(std::pair("wp_6", wp));
+
+ wp.position.x = -1.0;
+ wp.position.y = 1.0;
+ wp.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.0);
+ wp_map.insert(std::pair