Program Listing for File cia402_device_controller.hpp

↰ Return to documentation for file (/tmp/ws/src/ros2_canopen/canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_device_controller.hpp)

// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt)
//
// 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 CANOPEN_ROS2_CONTROLLERS__CANOPEN_CIA402_CONTROLLER_HPP_
#define CANOPEN_ROS2_CONTROLLERS__CANOPEN_CIA402_CONTROLLER_HPP_

#include "canopen_interfaces/srv/co_target_double.hpp"
#include "canopen_ros2_controllers/canopen_proxy_controller.hpp"

static constexpr int kLoopPeriodMS = 100;
static constexpr double kCommandValue = 1.0;

namespace
{
enum Cia402CommandInterfaces
{
  INIT_CMD = CommandInterfaces::LAST_COMMAND_AUX,
  INIT_FBK,
  HALT_CMD,
  HALT_FBK,
  RECOVER_CMD,
  RECOVER_FBK,
  POSITION_MODE_CMD,
  POSITION_MODE_FBK,
  VELOCITY_MODE_CMD,
  VELOCITY_MODE_FBK,
  CYCLIC_VELOCITY_MODE_CMD,
  CYCLIC_VELOCITY_MODE_FBK,
  CYCLIC_POSITION_MODE_CMD,
  CYCLIC_POSITION_MODE_FBK,
  INTERPOLATED_POSITION_MODE_CMD,
  INTERPOLATED_POSITION_MODE_FBK,
};

enum Cia402StateInterfaces
{
  FIRST_STATE = StateInterfaces::LAST_STATE_AUX,
};

}  // namespace

namespace canopen_ros2_controllers
{

class Cia402DeviceController : public canopen_ros2_controllers::CanopenProxyController
{
public:
  CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC
  Cia402DeviceController();

  CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC
  controller_interface::CallbackReturn on_init();

  CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC
  controller_interface::InterfaceConfiguration command_interface_configuration() const;

  CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC
  controller_interface::InterfaceConfiguration state_interface_configuration() const;

  CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC
  controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state);

  CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC
  controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state);

  CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC
  controller_interface::CallbackReturn on_deactivate(
    const rclcpp_lifecycle::State & previous_state);

  CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC
  controller_interface::return_type update(
    const rclcpp::Time & time, const rclcpp::Duration & period);

protected:
  inline rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr createTriggerSrv(
    const std::string & service, Cia402CommandInterfaces cmd, Cia402CommandInterfaces fbk)
  {
    // define service profile Qos
    auto service_profile = rclcpp::QoS(1);
    service_profile.keep_all();

    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr srv =
      get_node()->create_service<std_srvs::srv::Trigger>(
        service,
        [&, cmd, fbk](
          const std_srvs::srv::Trigger::Request::SharedPtr request,
          std_srvs::srv::Trigger::Response::SharedPtr response)
        {
          command_interfaces_[cmd].set_value(kCommandValue);

          while (std::isnan(command_interfaces_[fbk].get_value()) && rclcpp::ok())
          {
            std::this_thread::sleep_for(std::chrono::milliseconds(kLoopPeriodMS));
          }

          // report success
          response->success = static_cast<bool>(command_interfaces_[fbk].get_value());
          // reset to nan
          command_interfaces_[fbk].set_value(std::numeric_limits<double>::quiet_NaN());
          command_interfaces_[cmd].set_value(std::numeric_limits<double>::quiet_NaN());
        },
        service_profile);

    return srv;
  }

  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_init_service_;
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_halt_service_;
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_recover_service_;
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_set_mode_position_service_;
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_set_mode_torque_service_;
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_set_mode_velocity_service_;
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_set_mode_cyclic_velocity_service_;
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_set_mode_cyclic_position_service_;
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_set_mode_interpolated_position_service_;
  rclcpp::Service<canopen_interfaces::srv::COTargetDouble>::SharedPtr handle_set_target_service_;
};

}  // namespace canopen_ros2_controllers

#endif  // CANOPEN_ROS2_CONTROLLERS__CANOPEN_CIA402_CONTROLLER_HPP_