Program Listing for File node_canopen_402_driver.hpp

↰ Return to documentation for file (/tmp/ws/src/ros2_canopen/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp)

//    Copyright 2023 Christoph Hellmann Santos
//                          Vishnuprasad Prachandabhanu
//                          Lovro Ivanov
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.
//

#ifndef NODE_CANOPEN_402_DRIVER
#define NODE_CANOPEN_402_DRIVER

#include "canopen_402_driver/motor.hpp"
#include "canopen_base_driver/lely_driver_bridge.hpp"
#include "canopen_interfaces/srv/co_target_double.hpp"
#include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp"
#include "sensor_msgs/msg/joint_state.hpp"

namespace ros2_canopen
{
namespace node_interfaces
{

template <class NODETYPE>
class NodeCanopen402Driver : public NodeCanopenProxyDriver<NODETYPE>
{
  static_assert(
    std::is_base_of<rclcpp::Node, NODETYPE>::value ||
      std::is_base_of<rclcpp_lifecycle::LifecycleNode, NODETYPE>::value,
    "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode");

protected:
  std::shared_ptr<Motor402> motor_;
  rclcpp::TimerBase::SharedPtr timer_;
  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;
  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr publish_joint_state;
  double scale_pos_to_dev_;
  double scale_pos_from_dev_;
  double scale_vel_to_dev_;
  double scale_vel_from_dev_;
  ros2_canopen::State402::InternalState switching_state_;

  void publish();
  virtual void poll_timer_callback() override;
  void diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat) override;

public:
  NodeCanopen402Driver(NODETYPE * node);

  virtual void init(bool called_from_base) override;
  virtual void configure(bool called_from_base) override;
  virtual void activate(bool called_from_base) override;
  virtual void deactivate(bool called_from_base) override;
  virtual void add_to_master() override;

  virtual double get_speed() { return motor_->get_speed() * scale_vel_from_dev_; }

  virtual double get_position() { return motor_->get_position() * scale_pos_from_dev_; }

  virtual uint16_t get_mode() { return motor_->getMode(); }

  void handle_init(
    const std_srvs::srv::Trigger::Request::SharedPtr request,
    std_srvs::srv::Trigger::Response::SharedPtr response);

  bool init_motor();

  void handle_recover(
    const std_srvs::srv::Trigger::Request::SharedPtr request,
    std_srvs::srv::Trigger::Response::SharedPtr response);

  bool recover_motor();

  void handle_halt(
    const std_srvs::srv::Trigger::Request::SharedPtr request,
    std_srvs::srv::Trigger::Response::SharedPtr response);

  bool halt_motor();

  void handle_set_mode_position(
    const std_srvs::srv::Trigger::Request::SharedPtr request,
    std_srvs::srv::Trigger::Response::SharedPtr response);

  bool set_operation_mode(uint16_t mode);

  bool set_mode_position();

  void handle_set_mode_velocity(
    const std_srvs::srv::Trigger::Request::SharedPtr request,
    std_srvs::srv::Trigger::Response::SharedPtr response);

  bool set_mode_velocity();

  void handle_set_mode_cyclic_position(
    const std_srvs::srv::Trigger::Request::SharedPtr request,
    std_srvs::srv::Trigger::Response::SharedPtr response);

  void handle_set_mode_interpolated_position(
    const std_srvs::srv::Trigger::Request::SharedPtr request,
    std_srvs::srv::Trigger::Response::SharedPtr response);

  bool set_mode_interpolated_position();

  bool set_mode_cyclic_position();

  void handle_set_mode_cyclic_velocity(
    const std_srvs::srv::Trigger::Request::SharedPtr request,
    std_srvs::srv::Trigger::Response::SharedPtr response);

  bool set_mode_cyclic_velocity();

  void handle_set_mode_torque(
    const std_srvs::srv::Trigger::Request::SharedPtr request,
    std_srvs::srv::Trigger::Response::SharedPtr response);

  bool set_mode_torque();

  void handle_set_target(
    const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr request,
    canopen_interfaces::srv::COTargetDouble::Response::SharedPtr response);

  bool set_target(double target);
};
}  // namespace node_interfaces
}  // namespace ros2_canopen

#endif