Cloned existing controller and renamed it

balance_controller/CMakeLists.txt

@@ -0,0 +1,69 @@
+cmake_minimum_required(VERSION 3.5)
+project(balance_controller)
+
+# 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)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(angles REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(control_msgs REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(realtime_tools REQUIRED)
+find_package(trajectory_msgs REQUIRED)
+find_package(control_toolbox REQUIRED)
+
+add_library(balance_controller SHARED
+        src/balance_controller.cpp
+        )
+target_include_directories(balance_controller PUBLIC include)
+ament_target_dependencies(balance_controller
+        angles
+        builtin_interfaces
+        controller_interface
+        control_msgs
+        hardware_interface
+        pluginlib
+        rclcpp
+        rclcpp_lifecycle
+        realtime_tools
+        trajectory_msgs
+        control_toolbox
+        )
+
+pluginlib_export_plugin_description_file(controller_interface balance_plugin.xml)
+
+install(DIRECTORY include/
+        DESTINATION include
+        )
+
+install(TARGETS balance_controller
+        RUNTIME DESTINATION bin
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        )
+
+ament_export_dependencies(
+        controller_interface
+        control_msgs
+        hardware_interface
+        rclcpp
+        rclcpp_lifecycle
+        trajectory_msgs
+)
+ament_export_include_directories(
+        include
+)
+ament_export_libraries(
+        balance_controller
+)
+ament_package()

balance_controller/README.md

@@ -0,0 +1,5 @@
+# balance_controllers package
+
+The package implements controllers to balance a ball on a 3D plane.
+
+For detailed documentation check the `docs` folder or [ros2_control documentation](https://ros-controls.github.io/control.ros.org/).

balance_controller/balance_plugin.xml

@@ -0,0 +1,7 @@
+<library path="balance_controller">
+  <class name="balance_controller/BalanceController" type="balance_controller::BalanceController" base_class_type="controller_interface::ControllerInterface">
+  <description>
+    The balance controller adjusts the orientation of a 2d plane such that the object stays in the middle
+  </description>
+  </class>
+</library>

balance_controller/include/balance_controller/balance_controller.hpp

@@ -0,0 +1,249 @@
+// Copyright (c) 2021 ros2_control Development Team
+//
+// 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.
+
+#pragma once
+
+#include <chrono>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "control_msgs/action/follow_joint_trajectory.hpp"
+#include "control_msgs/msg/joint_trajectory_controller_state.hpp"
+#include "control_toolbox/pid.hpp"
+#include "controller_interface/controller_interface.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "tolerances.hpp"
+#include "visibility_control.h"
+#include "rclcpp/duration.hpp"
+#include "rclcpp/subscription.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp_action/server.hpp"
+#include "rclcpp_action/types.hpp"
+#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "realtime_tools/realtime_buffer.h"
+#include "realtime_tools/realtime_publisher.h"
+#include "realtime_tools/realtime_server_goal_handle.h"
+#include "trajectory_msgs/msg/joint_trajectory.hpp"
+#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
+
+using namespace std::chrono_literals;  // NOLINT
+
+namespace rclcpp_action
+{
+template <typename ActionT>
+class ServerGoalHandle;
+}  // namespace rclcpp_action
+namespace rclcpp_lifecycle
+{
+class State;
+}  // namespace rclcpp_lifecycle
+
+namespace balance_controller
+{
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+class Trajectory;
+
+class BalanceController : public controller_interface::ControllerInterface
+{
+public:
+  BALANCE_CONTROLLER_PUBLIC
+  BalanceController();
+
+  BALANCE_CONTROLLER_PUBLIC
+  controller_interface::return_type init(const std::string & controller_name) override;
+
+  /**
+   * @brief command_interface_configuration This controller requires the position command
+   * interfaces for the controlled joints
+   */
+  BALANCE_CONTROLLER_PUBLIC
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  /**
+   * @brief command_interface_configuration This controller requires the position and velocity
+   * state interfaces for the controlled joints
+   */
+  BALANCE_CONTROLLER_PUBLIC
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  BALANCE_CONTROLLER_PUBLIC
+  controller_interface::return_type update() override;
+
+  BALANCE_CONTROLLER_PUBLIC
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  BALANCE_CONTROLLER_PUBLIC
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  BALANCE_CONTROLLER_PUBLIC
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  BALANCE_CONTROLLER_PUBLIC
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  BALANCE_CONTROLLER_PUBLIC
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_error(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  BALANCE_CONTROLLER_PUBLIC
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+protected:
+  std::vector<std::string> joint_names_;
+  std::vector<std::string> command_interface_types_;
+  std::vector<std::string> state_interface_types_;
+
+  // To reduce number of variables and to make the code shorter the interfaces are ordered in types
+  // as the following constants
+  const std::vector<std::string> allowed_interface_types_ = {
+    hardware_interface::HW_IF_POSITION,
+    hardware_interface::HW_IF_VELOCITY,
+    hardware_interface::HW_IF_ACCELERATION,
+    hardware_interface::HW_IF_EFFORT,
+  };
+
+  // Parameters for some special cases, e.g. hydraulics powered robots
+  /// Run he controller in open-loop, i.e., read hardware states only when starting controller.
+  /// This is useful when robot is not exactly following the commanded trajectory.
+  bool open_loop_control_ = false;
+  trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_;
+
+  // The interfaces are defined as the types in 'allowed_interface_types_' member.
+  // For convenience, for each type the interfaces are ordered so that i-th position
+  // matches i-th index in joint_names_
+  template <typename T>
+  using InterfaceReferences = std::vector<std::vector<std::reference_wrapper<T>>>;
+
+  InterfaceReferences<hardware_interface::LoanedCommandInterface> joint_command_interface_;
+  InterfaceReferences<hardware_interface::LoanedStateInterface> joint_state_interface_;
+
+  bool has_velocity_state_interface_ = false;
+  bool has_acceleration_state_interface_ = false;
+  bool has_position_command_interface_ = false;
+  bool has_velocity_command_interface_ = false;
+  bool has_acceleration_command_interface_ = false;
+  bool has_effort_command_interface_ = false;
+
+  /// If true, a velocity feedforward term plus corrective PID term is used
+  bool use_closed_loop_pid_adapter = false;
+  std::vector<std::unique_ptr<control_toolbox::Pid>> pids_;
+  std::chrono::steady_clock::time_point last_update_time_;
+  std::vector<double> velocity_ff_;
+
+  // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported
+  bool subscriber_is_active_ = false;
+  rclcpp::Subscription<trajectory_msgs::msg::JointTrajectory>::SharedPtr joint_command_subscriber_ =
+    nullptr;
+
+  std::shared_ptr<Trajectory> * traj_point_active_ptr_ = nullptr;
+  std::shared_ptr<Trajectory> traj_external_point_ptr_ = nullptr;
+  std::shared_ptr<Trajectory> traj_home_point_ptr_ = nullptr;
+  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> traj_msg_home_ptr_ = nullptr;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectory>>
+    traj_msg_external_point_ptr_;
+
+  // The controller should be in halted state after creation otherwise memory corruption
+  // TODO(anyone): Is the variable relevant, since we are using lifecycle?
+  bool is_halted_ = true;
+
+  using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState;
+  using StatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>;
+  using StatePublisherPtr = std::unique_ptr<StatePublisher>;
+  rclcpp::Publisher<ControllerStateMsg>::SharedPtr publisher_;
+  StatePublisherPtr state_publisher_;
+
+  rclcpp::Duration state_publisher_period_ = rclcpp::Duration(20ms);
+  rclcpp::Time last_state_publish_time_;
+
+  using FollowJTrajAction = control_msgs::action::FollowJointTrajectory;
+  using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle<FollowJTrajAction>;
+  using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
+  using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer<RealtimeGoalHandlePtr>;
+
+  rclcpp_action::Server<FollowJTrajAction>::SharedPtr action_server_;
+  bool allow_partial_joints_goal_ = false;
+  RealtimeGoalHandleBuffer rt_active_goal_;  ///< Currently active action goal, if any.
+  rclcpp::TimerBase::SharedPtr goal_handle_timer_;
+  rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms);
+
+  // callbacks for action_server_
+  BALANCE_CONTROLLER_PUBLIC
+  rclcpp_action::GoalResponse goal_callback(
+    const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const FollowJTrajAction::Goal> goal);
+  BALANCE_CONTROLLER_PUBLIC
+  rclcpp_action::CancelResponse cancel_callback(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle);
+  BALANCE_CONTROLLER_PUBLIC
+  void feedback_setup_callback(
+    std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle);
+
+  // fill trajectory_msg so it matches joints controlled by this controller
+  // positions set to current position, velocities, accelerations and efforts to 0.0
+  BALANCE_CONTROLLER_PUBLIC
+  void fill_partial_goal(
+    std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg) const;
+  // sorts the joints of the incoming message to our local order
+  BALANCE_CONTROLLER_PUBLIC
+  //void sort_to_local_joint_order(
+  //  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg);
+  BALANCE_CONTROLLER_PUBLIC
+  bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const;
+  BALANCE_CONTROLLER_PUBLIC
+  void add_new_trajectory_msg(
+    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg);
+  BALANCE_CONTROLLER_PUBLIC
+  bool validate_trajectory_point_field(
+    size_t joint_names_size, const std::vector<double> & vector_field,
+    const std::string & string_for_vector_field, size_t i, bool allow_empty) const;
+
+  SegmentTolerances default_tolerances_;
+
+  BALANCE_CONTROLLER_PUBLIC
+  void preempt_active_goal();
+  BALANCE_CONTROLLER_PUBLIC
+  void set_hold_position();
+
+  BALANCE_CONTROLLER_PUBLIC
+  bool reset();
+
+  using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint;
+  BALANCE_CONTROLLER_PUBLIC
+  void publish_state(
+    const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state,
+    const JointTrajectoryPoint & state_error);
+
+  void read_state_from_hardware(JointTrajectoryPoint & state);
+
+  bool read_state_from_command_interfaces(JointTrajectoryPoint & state);
+
+private:
+  bool contains_interface_type(
+    const std::vector<std::string> & interface_type_list, const std::string & interface_type);
+
+  void resize_joint_trajectory_point(
+    trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
+};
+
+}  // namespace joint_trajectory_controller