node_canopen_402_driver.hpp

Go to the documentation of this file.

//    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_enable_service;
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_disable_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_cyclic_torque_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_;
  double offset_pos_to_dev_;
  double offset_pos_from_dev_;
  ros2_canopen::State402::InternalState switching_state_;
  int homing_timeout_seconds_;
 
  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_ + offset_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);
 
  void handle_enable(
    const std_srvs::srv::Trigger::Request::SharedPtr request,
    std_srvs::srv::Trigger::Response::SharedPtr response);
 
  void handle_disable(
    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();
 
  bool set_operation_mode(uint16_t mode);
 
  void handle_set_mode_position(
    const std_srvs::srv::Trigger::Request::SharedPtr request,
    std_srvs::srv::Trigger::Response::SharedPtr response);
 
  void handle_set_mode_velocity(
    const std_srvs::srv::Trigger::Request::SharedPtr request,
    std_srvs::srv::Trigger::Response::SharedPtr response);
 
  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);
 
  void handle_set_mode_cyclic_velocity(
    const std_srvs::srv::Trigger::Request::SharedPtr request,
    std_srvs::srv::Trigger::Response::SharedPtr response);
 
  void handle_set_mode_torque(
    const std_srvs::srv::Trigger::Request::SharedPtr request,
    std_srvs::srv::Trigger::Response::SharedPtr response);
 
  void handle_set_mode_cyclic_torque(
    const std_srvs::srv::Trigger::Request::SharedPtr request,
    std_srvs::srv::Trigger::Response::SharedPtr response);
 
  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