Add mros_contingencies_sim

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;
@@ -35,6 +36,7 @@ class LaserResender : public rclcpp_lifecycle::LifecycleNode
   LaserResender()
   : rclcpp_lifecycle::LifecycleNode("laser_resender")
   {
+    declare_parameter("node_name");
     pub_ = create_publisher<sensor_msgs::msg::LaserScan>("/mros_scan", rclcpp::SensorDataQoS());
     sub_ = create_subscription<sensor_msgs::msg::LaserScan>
       ("/scan", rclcpp::SensorDataQoS(), std::bind(&LaserResender::scan_cb, this, _1));
@@ -58,8 +60,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 +81,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<sensor_msgs::msg::LaserScan>::SharedPtr pub_;
   rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr sub_;
+  bool all_zero_error_;
 };
 
 int main(int argc, char * argv[])

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()

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 on_error(std::string msg);
+  void setOldposition(geometry_msgs::msg::Pose current_pose);
+
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr amcl_sub_;
+  rclcpp::Publisher<std_msgs::msg::Float64>::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_

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 <QtWidgets>
+#include <QBasicTimer>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#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<rclcpp::Client<lifecycle_msgs::srv::ChangeState>> client_laser_;
+  rclcpp::Publisher<sensor_msgs::msg::LaserScan>::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_

mros_contingencies_sim/package.xml

@@ -0,0 +1,41 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>mros_contingencies_sim</name>
+  <version>0.4.1</version>
+  <description>Navigation 2 plugins for rviz</description>
+  <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>qtbase5-dev</build_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>nav2_util</depend>
+  <depend>nav2_lifecycle_manager</depend>
+  <depend>nav2_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>resource_retriever</depend>
+  <depend>rviz_common</depend>
+  <depend>rviz_default_plugins</depend>
+  <depend>rviz_ogre_vendor</depend>
+  <depend>rviz_rendering</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
+
+  <exec_depend>libqt5-core</exec_depend>
+  <exec_depend>libqt5-gui</exec_depend>
+  <exec_depend>libqt5-opengl</exec_depend>
+  <exec_depend>libqt5-widgets</exec_depend>
+
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>