From 2b265f8ed62ddf476c798db4a86fb38b811d8198 Mon Sep 17 00:00:00 2001 From: jginesclavero Date: Thu, 24 Sep 2020 09:45:42 +0000 Subject: [PATCH 1/4] Add mros_contingencies_sim --- mros_contingencies_sim/CMakeLists.txt | 131 ++++ .../battery_contingency_node.hpp | 49 ++ .../rviz_plugin/mros_contingencies_panel.hpp | 77 +++ mros_contingencies_sim/package.xml | 41 ++ .../plugins_description.xml | 9 + .../src/battery_contingency_node.cpp | 82 +++ .../rviz_plugin/mros_contingencies_panel.cpp | 160 +++++ pilot_urjc_bringup/CMakeLists.txt | 1 + .../turtlebot3_sim/nav2_turtlebot3_launch.py | 2 +- .../rviz/nav2_default_view.rviz | 591 ++++++++++++++++++ 10 files changed, 1142 insertions(+), 1 deletion(-) create mode 100644 mros_contingencies_sim/CMakeLists.txt create mode 100644 mros_contingencies_sim/include/mros_contingencies_sim/battery_contingency_node.hpp create mode 100644 mros_contingencies_sim/include/mros_contingencies_sim/rviz_plugin/mros_contingencies_panel.hpp create mode 100644 mros_contingencies_sim/package.xml create mode 100644 mros_contingencies_sim/plugins_description.xml create mode 100644 mros_contingencies_sim/src/battery_contingency_node.cpp create mode 100644 mros_contingencies_sim/src/rviz_plugin/mros_contingencies_panel.cpp create mode 100644 pilot_urjc_bringup/rviz/nav2_default_view.rviz diff --git a/mros_contingencies_sim/CMakeLists.txt b/mros_contingencies_sim/CMakeLists.txt new file mode 100644 index 0000000..addafc3 --- /dev/null +++ b/mros_contingencies_sim/CMakeLists.txt @@ -0,0 +1,131 @@ +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(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 +) + +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) + 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..6f129ad --- /dev/null +++ b/mros_contingencies_sim/include/mros_contingencies_sim/battery_contingency_node.hpp @@ -0,0 +1,49 @@ +// 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_CONTINGENCY_SIM__BATTERY_CONTINGENCY_HPP_ +#define MROS_CONTINGENCY_SIM__BATTERY_CONTINGENCY_HPP_ + +#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" + +namespace mros_contingencies_sim +{ + +class BatteryContingency: public rclcpp::Node +{ +public: + + BatteryContingency(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); + + rclcpp::Subscription::SharedPtr amcl_sub_; + rclcpp::Publisher::SharedPtr battery_pub_; + geometry_msgs::msg::Pose last_pose_; + float battery_consumption_, battery_level_; +}; + +} // namespace mros_contingencies_sim + +#endif // MROS_CONTINGENCY_SIM__BATTERY_CONTINGENCY_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..c9565bd --- /dev/null +++ b/mros_contingencies_sim/include/mros_contingencies_sim/rviz_plugin/mros_contingencies_panel.hpp @@ -0,0 +1,77 @@ +// 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" + +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_; + + QPushButton * laser_contingency_button_{nullptr}; + QStateMachine state_machine_; + + QState * initial_{nullptr}; + QState * idle_{nullptr}; + +}; + +} // 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..aed9cd2 --- /dev/null +++ b/mros_contingencies_sim/package.xml @@ -0,0 +1,41 @@ + + + + mros_contingencies_sim + 0.4.1 + Navigation 2 plugins for rviz + Michael Jeronimo + 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 + + 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..2d7e270 --- /dev/null +++ b/mros_contingencies_sim/src/battery_contingency_node.cpp @@ -0,0 +1,82 @@ +// 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 + +using namespace std::chrono_literals; +using std::placeholders::_1; + +namespace mros_contingencies_sim +{ + +BatteryContingency::BatteryContingency(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)); + + battery_level_ = 100.0; + battery_consumption_ = 2.0; +} + +float BatteryContingency::calculateDistance( + float current_x, float current_y, float old_x, float old_y) +{ + return sqrt((pow(current_x - old_x, 2) + pow(current_y - old_y, 2))); +} + +void BatteryContingency::setOldposition(geometry_msgs::msg::Pose current_pose) +{ + last_pose_ = current_pose; +} + +void BatteryContingency::amclCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) +{ + float current_x, current_y, distance; + 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_; + std_msgs::msg::Float64 battery_msg; + battery_msg.data = battery_level_; + battery_pub_->publish(battery_msg); +} + +} + +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; +} \ No newline at end of file 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..b61ab1e --- /dev/null +++ b/mros_contingencies_sim/src/rviz_plugin/mros_contingencies_panel.cpp @@ -0,0 +1,160 @@ +// 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 "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", "Deactive Laser Driver"); + 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", "Deactive Laser Driver"); + 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); +} + +MROSContingenciesPanel::~MROSContingenciesPanel() +{ +} + +void +MROSContingenciesPanel::onInitialize() +{ + auto node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); +} + +void +MROSContingenciesPanel::onStartup() +{ + std::cout << "onStartup" << std::endl; + auto request = std::make_shared(); + auto transition = lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE; + request->transition.id = transition; + std::chrono::seconds time_out = std::chrono::seconds(5); + if (!client_laser_->wait_for_service(time_out)) { + RCLCPP_ERROR( + client_node_->get_logger(), + "Service %s is not available.", + client_laser_->get_service_name()); + return; + } + // We send the request with the transition we want to invoke. + auto future_result = client_laser_->async_send_request(request); + // Let's wait until we have the answer from the node. + // If the request times out, we return an unknown state. + auto future_status = wait_for_result(future_result, time_out); + if (future_status != std::future_status::ready) { + RCLCPP_ERROR( + client_node_->get_logger(), "Server time out while getting current state for node %s", + managed_node_.c_str()); + return; + } + // We have an answer, let's print our success. + if (future_result.get()->success) { + RCLCPP_INFO( + client_node_->get_logger(), "Transition %d successfully triggered.", static_cast(transition)); + return; + } else { + RCLCPP_WARN( + client_node_->get_logger(), "Failed to trigger transition %u", static_cast(transition)); + return; + } +} + +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_urjc_bringup/CMakeLists.txt b/pilot_urjc_bringup/CMakeLists.txt index ae3c420..1ee6869 100644 --- a/pilot_urjc_bringup/CMakeLists.txt +++ b/pilot_urjc_bringup/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(rclpy REQUIRED) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) ament_export_dependencies(${dependencies}) diff --git a/pilot_urjc_bringup/launch/turtlebot3_sim/nav2_turtlebot3_launch.py b/pilot_urjc_bringup/launch/turtlebot3_sim/nav2_turtlebot3_launch.py index 45a14e5..ecabf48 100644 --- a/pilot_urjc_bringup/launch/turtlebot3_sim/nav2_turtlebot3_launch.py +++ b/pilot_urjc_bringup/launch/turtlebot3_sim/nav2_turtlebot3_launch.py @@ -91,7 +91,7 @@ def generate_launch_description(): declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', - default_value=os.path.join(nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz'), + default_value=os.path.join(pilot_dir, 'rviz', 'nav2_default_view.rviz'), description='Full path to the RVIZ config file to use') declare_use_rviz_cmd = DeclareLaunchArgument( diff --git a/pilot_urjc_bringup/rviz/nav2_default_view.rviz b/pilot_urjc_bringup/rviz/nav2_default_view.rviz new file mode 100644 index 0000000..36f0b59 --- /dev/null +++ b/pilot_urjc_bringup/rviz/nav2_default_view.rviz @@ -0,0 +1,591 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /TF1/Tree1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 600 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 + - Class: mros_contingencies_sim/MROS Contingencies Panel + Name: MROS Contingencies Panel +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: "" + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: false + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_footprint: + Value: true + base_link: + Value: true + base_scan: + Value: true + camera_depth_frame: + Value: true + camera_depth_optical_frame: + Value: true + camera_link: + Value: true + camera_rgb_frame: + Value: true + camera_rgb_optical_frame: + Value: true + caster_back_left_link: + Value: true + caster_back_right_link: + Value: true + imu_link: + Value: true + odom: + Value: true + wheel_left_link: + Value: true + wheel_right_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Arrow Length: 0.019999999552965164 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 180; 0 + Enabled: false + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Amcl Particle Swarm + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particlecloud + Value: false + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: false + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud + Color: 125; 125; 125 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Controller + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RealsenseCamera + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/image_raw + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: RealsenseDepthImage + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: false + Name: Realsense + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /waypoints + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mros_scan + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -1.6150002479553223 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 127.88431549072266 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: -0.044467076659202576 + Y: -0.38726311922073364 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 995 + Hide Left Dock: false + Hide Right Dock: true + MROS Contingencies Panel: + collapsed: false + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000038dfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000293000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000002d4000000ad000000ad00fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800fffffffb00000030004d0052004f005300200043006f006e00740069006e00670065006e0063006900650073002000500061006e0065006c0100000387000000410000004100ffffff000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005200000038d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RealsenseCamera: + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1680 + X: 0 + Y: 27 From 6bab9d0d6003f752e94608688961876b7a5c2345 Mon Sep 17 00:00:00 2001 From: jginesclavero Date: Tue, 29 Sep 2020 09:43:46 +0000 Subject: [PATCH 2/4] Transit to ErrorProcesing state --- laser_resender/src/laser_resender_node.cpp | 47 +++++++++++++++---- .../battery_contingency_node.hpp | 2 +- .../rviz_plugin/mros_contingencies_panel.hpp | 5 +- .../src/battery_contingency_node.cpp | 10 ++++ .../rviz_plugin/mros_contingencies_panel.cpp | 46 +++++------------- 5 files changed, 66 insertions(+), 44 deletions(-) diff --git a/laser_resender/src/laser_resender_node.cpp b/laser_resender/src/laser_resender_node.cpp index 9980725..f93b441 100644 --- a/laser_resender/src/laser_resender_node.cpp +++ b/laser_resender/src/laser_resender_node.cpp @@ -21,8 +21,9 @@ #include "sensor_msgs/msg/laser_scan.hpp" // Execute: -// ros2 lifecycle list /lifecycle_node_example -// ros2 lifecycle set /lifecycle_node_example configure +// 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; @@ -58,8 +59,13 @@ class LaserResender : public rclcpp_lifecycle::LifecycleNode 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) @@ -74,22 +80,47 @@ class LaserResender : public rclcpp_lifecycle::LifecycleNode 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); + if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + 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[]) 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 index 6f129ad..2d97743 100644 --- a/mros_contingencies_sim/include/mros_contingencies_sim/battery_contingency_node.hpp +++ b/mros_contingencies_sim/include/mros_contingencies_sim/battery_contingency_node.hpp @@ -35,7 +35,7 @@ class BatteryContingency: public rclcpp::Node void amclCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); float calculateDistance( float current_x, float current_y, float old_x, float old_y); - + void on_error(std::string msg); void setOldposition(geometry_msgs::msg::Pose current_pose); rclcpp::Subscription::SharedPtr amcl_sub_; 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 index c9565bd..6b89d76 100644 --- 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 @@ -31,6 +31,8 @@ #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 @@ -63,13 +65,14 @@ private Q_SLOTS: // 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 diff --git a/mros_contingencies_sim/src/battery_contingency_node.cpp b/mros_contingencies_sim/src/battery_contingency_node.cpp index 2d7e270..a8652ab 100644 --- a/mros_contingencies_sim/src/battery_contingency_node.cpp +++ b/mros_contingencies_sim/src/battery_contingency_node.cpp @@ -46,6 +46,12 @@ void BatteryContingency::setOldposition(geometry_msgs::msg::Pose current_pose) { last_pose_ = current_pose; } + +void BatteryContingency::on_error(std::string msg) +{ + RCLCPP_ERROR(get_logger(), "[%s] %s", get_name(), msg.c_str()); +} + void BatteryContingency::amclCallback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) @@ -62,9 +68,13 @@ void BatteryContingency::amclCallback( setOldposition(msg->pose.pose); battery_level_ = battery_level_ - distance * battery_consumption_; + if (battery_level_ < 0.0) + battery_level_ = 0.0; std_msgs::msg::Float64 battery_msg; battery_msg.data = battery_level_; battery_pub_->publish(battery_msg); + if (battery_level_ < 15.0) + on_error("Battery level lower than 15%"); } } diff --git a/mros_contingencies_sim/src/rviz_plugin/mros_contingencies_panel.cpp b/mros_contingencies_sim/src/rviz_plugin/mros_contingencies_panel.cpp index b61ab1e..c153fb2 100644 --- a/mros_contingencies_sim/src/rviz_plugin/mros_contingencies_panel.cpp +++ b/mros_contingencies_sim/src/rviz_plugin/mros_contingencies_panel.cpp @@ -59,14 +59,14 @@ MROSContingenciesPanel::MROSContingenciesPanel(QWidget * parent) initial_ = new QState(); initial_->setObjectName("initial"); - initial_->assignProperty(laser_contingency_button_, "text", "Deactive Laser Driver"); + 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", "Deactive Laser Driver"); + 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())); @@ -92,6 +92,10 @@ MROSContingenciesPanel::MROSContingenciesPanel(QWidget * parent) client_laser_ = client_node_->create_client( change_state_service_name); + pub_ = client_node_->create_publisher("/scan", rclcpp::SensorDataQoS()); + + num_injected_msgs_ = 10; + } MROSContingenciesPanel::~MROSContingenciesPanel() @@ -107,38 +111,12 @@ MROSContingenciesPanel::onInitialize() void MROSContingenciesPanel::onStartup() { - std::cout << "onStartup" << std::endl; - auto request = std::make_shared(); - auto transition = lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE; - request->transition.id = transition; - std::chrono::seconds time_out = std::chrono::seconds(5); - if (!client_laser_->wait_for_service(time_out)) { - RCLCPP_ERROR( - client_node_->get_logger(), - "Service %s is not available.", - client_laser_->get_service_name()); - return; - } - // We send the request with the transition we want to invoke. - auto future_result = client_laser_->async_send_request(request); - // Let's wait until we have the answer from the node. - // If the request times out, we return an unknown state. - auto future_status = wait_for_result(future_result, time_out); - if (future_status != std::future_status::ready) { - RCLCPP_ERROR( - client_node_->get_logger(), "Server time out while getting current state for node %s", - managed_node_.c_str()); - return; - } - // We have an answer, let's print our success. - if (future_result.get()->success) { - RCLCPP_INFO( - client_node_->get_logger(), "Transition %d successfully triggered.", static_cast(transition)); - return; - } else { - RCLCPP_WARN( - client_node_->get_logger(), "Failed to trigger transition %u", static_cast(transition)); - return; + int i = 0; + while (i < num_injected_msgs_) + { + sensor_msgs::msg::LaserScan msg; + pub_->publish(msg); + i++; } } From 40b72b59fc8ccec1787186104ccc5254a76ab98c Mon Sep 17 00:00:00 2001 From: jginesclavero Date: Tue, 29 Sep 2020 12:13:13 +0000 Subject: [PATCH 3/4] Add declare parameter --- laser_resender/src/laser_resender_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/laser_resender/src/laser_resender_node.cpp b/laser_resender/src/laser_resender_node.cpp index f93b441..5152c62 100644 --- a/laser_resender/src/laser_resender_node.cpp +++ b/laser_resender/src/laser_resender_node.cpp @@ -36,6 +36,7 @@ class LaserResender : public rclcpp_lifecycle::LifecycleNode 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)); From edb72c4d3c356c05131b46f343bdb9cac7bf921f Mon Sep 17 00:00:00 2001 From: jginesclavero Date: Tue, 29 Sep 2020 12:16:46 +0000 Subject: [PATCH 4/4] Add rules in pilot_modes --- dependencies.repos | 5 + .../metacontroller_sim.py | 103 +++++----------- .../simple_mode_manager.py | 113 ++++++++++++++++++ metacontroller_pilot/setup.py | 1 + pilot_urjc_bringup/params/pilot_modes.yaml | 21 ++-- 5 files changed, 164 insertions(+), 79 deletions(-) create mode 100644 metacontroller_pilot/metacontroller_pilot/simple_mode_manager.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/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/pilot_urjc_bringup/params/pilot_modes.yaml b/pilot_urjc_bringup/params/pilot_modes.yaml index 2f78038..fda161c 100644 --- a/pilot_urjc_bringup/params/pilot_modes.yaml +++ b/pilot_urjc_bringup/params/pilot_modes.yaml @@ -11,11 +11,11 @@ pilot: laser_resender modes: __DEFAULT__: - amcl: inactive - bt_navigator: inactive - controller_server: inactive - pointcloud_to_laser: inactive - laser_resender: inactive + amcl: active + bt_navigator: active + controller_server: active + pointcloud_to_laser: active + laser_resender: active NORMAL: amcl: active bt_navigator: active @@ -27,7 +27,7 @@ pilot: bt_navigator: active controller_server: active.DEGRADED pointcloud_to_laser: active - laser_resender: inactive + laser_resender: unconfigured PERFORMANCE: amcl: active bt_navigator: active.SIMPLE @@ -46,6 +46,11 @@ pilot: controller_server: active.SLOW pointcloud_to_laser: inactive laser_resender: active + rules: + degrade_from_NORMAL: + if_target: active.NORMAL + if_part: [laser_resender, unconfigured] + new_target: active.DEGRADED amcl: ros__parameters: @@ -78,7 +83,7 @@ laser_resender: modes: __DEFAULT__: ros__parameters: - laser_resender_test: "0" + node_name: "laser_resender" pointcloud_to_laser: ros__parameters: @@ -86,7 +91,7 @@ pointcloud_to_laser: modes: __DEFAULT__: ros__parameters: - pointcloud_to_laser_test: "0" + node_name: "pointcloud_to_laser" controller_server: ros__parameters: