Minimal implementation of realtime control demo

pendulum_control/CMakeLists.txt

@@ -0,0 +1,38 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+project(pendulum_control)
+
+if(APPLE OR WIN32)
+  message(STATUS "The pendulum_control demo is only supported on Linux. Package will not be built.")
+  return()
+endif()
+
+find_package(ament_cmake REQUIRED)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra")
+
+find_package(rclcpp REQUIRED)
+find_package(rmw_implementation REQUIRED)
+find_package(pendulum_msgs REQUIRED)
+find_package(rttest)
+
+if(NOT rttest_FOUND)
+  message(STATUS "rttest not found. pendulum_control package will not be built.")
+  return()
+endif()
+
+add_executable_for_each_rmw_implementations(pendulum_demo
+  src/pendulum_demo.cpp
+  TARGET_DEPENDENCIES
+  "rclcpp"
+  "pendulum_msgs"
+  "rttest"
+  INSTALL
+)
+
+if(AMENT_ENABLE_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

pendulum_control/README.md

@@ -0,0 +1,65 @@
+## Build instructions
+
+If you haven't already, clone [rttest](https://github.com/jacquelinekay/rttest) into your ament workspace.
+
+Build:
+
+```
+./src/ament/ament_tools/scripts/ament.py build --build-tests --symlink-install --only pendulum_control
+```
+
+Run:
+
+```
+. install/setup.bash
+./build/pendulum_control/pendulum_demo[__rmw_opensplice/__rmw_connext]
+```
+The demo should work with both middleware implementations.
+
+A few command line arguments related to real-time performance profiling are provided by rttest.
+See https://github.com/ros2/rttest/blob/master/README.md for more information.
+
+## Running with real-time performance
+
+The demo will print out its performance statistics continuously and at the end of the program.
+
+Example final output:
+```
+rttest statistics:
+  - Minor pagefaults: 0
+  - Major pagefaults: 0
+  Latency (time after deadline was missed):
+    - Min: 2414 ns
+    - Max: 45949 ns
+    - Mean: 8712.12 ns
+    - Standard deviation: 1438.74
+```
+
+Ideally you want to see 0 minor or major pagefaults and an average latency of less than 30,000 nanosceonds
+(3% of the 1 millisecond update period).
+
+If you see pagefaults, you may not have permission to lock memory via `mlockall`.
+You can run the program as sudo, or you can adjust the system limits for memory locking.
+
+Add to `/etc/security/limits.conf`:
+```
+<user>    -   memlock   <limit in kB>
+```
+
+A limit of `-1` is unlimited.
+If you choose this, you may need to accompany it with `ulimit -l unlimited` after editing the file.
+
+After saving the file, log out and log back in.
+
+If you see a high mean latency in the results, you may need to adjust the maximum priority for processes.
+
+Add to `/etc/security/limits.conf`:
+```
+<user>    -   rtprio   <maximum priority>
+```
+
+The range of the priority is 0-99.
+However, do NOT set the limit to 99 because then your processes could interfere with important system processes that run at the top priority (e.g. watchdog).
+This demo will attempt to run the control loop at priority 98.
+
+With these settings you can get decent performance even if you don't have the RT_PREEMPT kernel installed, but you will likely see an unacceptably large maximum latency and periodic latency spikes.

pendulum_control/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>pendulum_control</name>
+  <version>0.0.0</version>
+  <description>Demonstrates ROS 2's realtime capabilities with a simulated inverted pendulum.</description>
+  <maintainer email="[email protected]">Jackie Kay</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>rmw_implementation</build_depend>
+  <build_depend>pendulum_msgs</build_depend>
+  <build_depend>rttest</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>rmw_implementation</exec_depend>
+  <exec_depend>pendulum_msgs</exec_depend>
+  <exec_depend>rttest</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

pendulum_control/src/pendulum_controller.hpp

@@ -0,0 +1,136 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// 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 PENDULUM_DEMO_PENDULUM_CONTROLLER_HPP_
+#define PENDULUM_DEMO_PENDULUM_CONTROLLER_HPP_
+
+#include <chrono>
+
+#include <pendulum_msgs/msg/joint_command.hpp>
+#include <pendulum_msgs/msg/joint_state.hpp>
+
+#ifndef PI
+#define PI 3.14159265359
+#endif
+
+namespace pendulum_control
+{
+
+struct PIDProperties
+{
+  // Properties of a PID controller
+  double p = 1;
+  double i = 0;
+  double d = 0;
+  double command = PI / 2;
+};
+
+class PendulumController
+{
+public:
+  PendulumController(std::chrono::nanoseconds period, PIDProperties pid)
+  : publish_period_(period), pid_(pid),
+    command_message_(std::make_shared<pendulum_msgs::msg::JointCommand>()),
+    message_ready_(false)
+  {
+    command_message_->position = pid_.command;
+    dt_ = publish_period_.count() / (1000.0 * 1000.0 * 1000.0);
+    if (isnan(dt_) || dt_ == 0) {
+      throw std::runtime_error("Invalid dt_ calculated in PendulumController constructor");
+    }
+  }
+
+  // Calculate new command based on new sensor state and PID controller properties
+  void on_sensor_message(const pendulum_msgs::msg::JointState::SharedPtr msg)
+  {
+    ++messages_received;
+
+    if (isnan(msg->position)) {
+      throw std::runtime_error("Sensor value was NaN in on_sensor_message callback");
+    }
+    double error = pid_.command - msg->position;
+    double p_gain = pid_.p * error;
+    i_gain_ = pid_.i * (i_gain_ + error * dt_);
+    double d_gain = pid_.d * (error - last_error_) / dt_;
+    last_error_ = error;
+
+    // TODO consider filtering the PID output
+    command_message_->position = msg->position + p_gain + i_gain_ + d_gain;
+    // limits
+    if (command_message_->position > PI) {
+      command_message_->position = PI;
+    } else if (command_message_->position < 0) {
+      command_message_->position = 0;
+    }
+
+    if (isnan(command_message_->position)) {
+      throw std::runtime_error("Resulting command was NaN in on_sensor_message callback");
+    }
+    message_ready_ = true;
+  }
+
+  const pendulum_msgs::msg::JointCommand::SharedPtr get_next_command_message()
+  {
+    return command_message_;
+  }
+
+  bool next_message_ready() const
+  {
+    return message_ready_;
+  }
+
+  std::chrono::nanoseconds get_publish_period() const
+  {
+    return publish_period_;
+  }
+
+  void set_pid_properties(const PIDProperties & properties)
+  {
+    pid_ = properties;
+  }
+
+  const PIDProperties & get_pid_properties() const
+  {
+    return pid_;
+  }
+
+  void set_command(double command)
+  {
+    pid_.command = command;
+  }
+
+  double get_command() const
+  {
+    return pid_.command;
+  }
+
+  // gather statistics
+  size_t messages_received = 0;
+
+private:
+  // controller should publish less frequently than the motor
+  std::chrono::nanoseconds publish_period_;
+  PIDProperties pid_;
+  pendulum_msgs::msg::JointCommand::SharedPtr command_message_;
+  bool message_ready_;
+
+  // state for PID controller
+  double last_error_ = 0;
+  double i_gain_ = 0;
+  double dt_;
+};
+
+}  /* namespace pendulum_demo */
+
+#endif  /* PENDULUM_DEMO_PENDULUM_CONTROLLER_HPP_ */