dynamixel_hardware_interface.cpp#

Class implementation of the hardware interface for the Dynamixel motor. More...
Namespaces#

Name
dynamixel_hardware_interface
Detailed Description#

Class implementation of the hardware interface for the Dynamixel motor.
Author: Masaya Kataoka (ms.kataoka@gmail.com)
Version: 0.1
Date: 2021-05-01
Source code#

// Copyright (c) 2021 OUXT Polaris
//
// 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.

#include <dynamixel_hardware_interface/dynamixel_hardware_interface.hpp>
#include <memory>
#include <string>
#include <vector>

namespace dynamixel_hardware_interface
{
#if defined(GALACTIC) || defined(HUMBLE)
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
DynamixelHardwareInterface::on_init(const hardware_interface::HardwareInfo & info)
#else
hardware_interface::return_type DynamixelHardwareInterface::configure(
  const hardware_interface::HardwareInfo & info)
#endif
{
#if defined(GALACTIC) || defined(HUMBLE)
  if (
    SystemInterface::on_init(info) !=
    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) {
    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
  }
#else
  if (configure_default(info) != hardware_interface::return_type::OK) {
    return hardware_interface::return_type::ERROR;
  }
#endif
  RCLCPP_INFO_STREAM(
    rclcpp::get_logger("dynamixel_hardware_interface"), "configure hardware " << info.name);
  for (const auto & hardware_parameter : info_.hardware_parameters) {
    RCLCPP_INFO_STREAM(
      rclcpp::get_logger("dynamixel_hardware_interface"),
      "hardware parameter : " << hardware_parameter.first << " = " << hardware_parameter.second);
  }
  port_name_ = getHardwareParameter<std::string>("port_name");
  baudrate_ = getHardwareParameter<int>("baudrate");
  RCLCPP_INFO(rclcpp::get_logger("dynamixel_hardware_interface"), "initialize port handler");
  port_handler_ = std::shared_ptr<dynamixel::PortHandler>(
    dynamixel::PortHandler::getPortHandler(port_name_.c_str()));
  port_handler_->setBaudRate(baudrate_);
  RCLCPP_INFO(rclcpp::get_logger("dynamixel_hardware_interface"), "initialize packet handler");
  packet_handler_ = std::shared_ptr<dynamixel::PacketHandler>(
    dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION));
  if (!getHardwareParameter<bool>("enable_dummy")) {
    RCLCPP_INFO_STREAM(
      rclcpp::get_logger("dynamixel_hardware_interface"),
      "serial port : " << port_handler_->getPortName());
    RCLCPP_INFO_STREAM(
      rclcpp::get_logger("dynamixel_hardware_interface"),
      "baudrate : " << port_handler_->getBaudRate());
    if (port_handler_->openPort()) {
      RCLCPP_INFO(rclcpp::get_logger("dynamixel_hardware_interface"), "open serial port succeed");
    } else {
      RCLCPP_ERROR(rclcpp::get_logger("dynamixel_hardware_interface"), "open serial port failed");
#if defined(GALACTIC) || defined(HUMBLE)
      return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
#else
      return hardware_interface::return_type::ERROR;
#endif
    }
  }
  RCLCPP_INFO(rclcpp::get_logger("dynamixel_hardware_interface"), "configure each motors");
  for (const auto & joint : info.joints) {
    std::shared_ptr<MotorBase> motor;
    try {
      motor = constructMotorInstance(joint);
    } catch (const std::runtime_error & e) {
      RCLCPP_ERROR_STREAM(rclcpp::get_logger("dynamixel_hardware_interface"), e.what());
#if defined(GALACTIC) || defined(HUMBLE)
      return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
#else
      return hardware_interface::return_type::ERROR;
#endif
    }
    const auto result = motor->configure();
    if (!result.success) {
      RCLCPP_ERROR_STREAM(rclcpp::get_logger("dynamixel_hardware_interface"), result.description);
#if defined(GALACTIC) || defined(HUMBLE)
      return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
#else
      return hardware_interface::return_type::ERROR;
#endif
    }
    motors_.emplace_back(motor);
  }
#if defined(GALACTIC) || defined(HUMBLE)
  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
#else
  return hardware_interface::return_type::OK;
#endif
}

std::vector<hardware_interface::StateInterface>
DynamixelHardwareInterface::export_state_interfaces()
{
  std::vector<hardware_interface::StateInterface> state_interfaces = {};
  for (const auto & motor : motors_) {
    motor->appendStateInterfaces(state_interfaces);
  }
  RCLCPP_INFO_STREAM(
    rclcpp::get_logger("dynamixel_hardware_interface"), state_interfaces.size()
                                                          << " state interfaces exported.");
  return state_interfaces;
}

std::vector<hardware_interface::CommandInterface>
DynamixelHardwareInterface::export_command_interfaces()
{
  std::vector<hardware_interface::CommandInterface> command_interfaces = {};
  for (const auto & motor : motors_) {
    motor->appendCommandInterfaces(command_interfaces);
  }
  RCLCPP_INFO_STREAM(
    rclcpp::get_logger("dynamixel_hardware_interface"), command_interfaces.size()
                                                          << " command interfaces exported.");
  return command_interfaces;
}

SupportedMotors DynamixelHardwareInterface::strToSupportMotorsEnum(
  const std::string & motor_type) const
{
  if (motor_type == "XW540-T260") {
    return SupportedMotors::XW540_T260;
  }
  return SupportedMotors::INVALID;
}

std::shared_ptr<MotorBase> DynamixelHardwareInterface::constructMotorInstance(
  const hardware_interface::ComponentInfo & info) const
{
  RCLCPP_INFO_STREAM(
    rclcpp::get_logger("dynamixel_hardware_interface"),
    "constructing motor instance : " << info.name);
  for (const auto & parameter : info.parameters) {
    RCLCPP_INFO_STREAM(
      rclcpp::get_logger("dynamixel_hardware_interface"),
      "parameter " << parameter.first << " : " << parameter.second);
  }
  if (info.type == "joint") {
    const auto motor_type = strToSupportMotorsEnum(getParameter<std::string>("motor_type", info));
    if (motor_type == SupportedMotors::INVALID) {
      throw std::runtime_error("failed to construct motor instance, motor type is invalid");
    }
    const auto id = static_cast<uint8_t>(getParameter<int>("id", info));

    const auto max_joint_limit = getParameter<double>("max_joint_limit", info);
    const auto min_joint_limit = getParameter<double>("min_joint_limit", info);

    switch (motor_type) {
      case SupportedMotors::XW540_T260:
        return std::make_shared<motors::XW540_T260>(
          info.name, getHardwareParameter<bool>("enable_dummy"), baudrate_, id, max_joint_limit,
          min_joint_limit, port_handler_, packet_handler_);
        break;
      default:
        break;
    }
  }
  throw std::runtime_error("failed to construct motor instance");
}

// #ifndef GALACTIC
// hardware_interface::return_type DynamixelHardwareInterface::start()
// {
//   status_ = hardware_interface::status::STARTED;
//   return hardware_interface::return_type::OK;
// }

// hardware_interface::return_type DynamixelHardwareInterface::stop()
// {
//   return hardware_interface::return_type::OK;
// }
// #endif

hardware_interface::return_type DynamixelHardwareInterface::read(
  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
  for (const auto & motor : motors_) {
    if (motor->operationSupports(Operation::PRESENT_POSITION)) {
      const auto result = motor->updateJointPosition();
      if (!result.success) {
        RCLCPP_ERROR_STREAM(rclcpp::get_logger("dynamixel_hardware_interface"), result.description);
        return hardware_interface::return_type::ERROR;
      }
    }
    if (motor->operationSupports(Operation::PRESENT_SPEED)) {
      const auto result = motor->updateJointVelocity();
      if (!result.success) {
        RCLCPP_ERROR_STREAM(rclcpp::get_logger("dynamixel_hardware_interface"), result.description);
        return hardware_interface::return_type::ERROR;
      }
    }
    if (motor->operationSupports(Operation::PRESENT_TEMPERATURE)) {
      const auto result = motor->updatePresentTemperature();
      if (!result.success) {
        RCLCPP_ERROR_STREAM(rclcpp::get_logger("dynamixel_hardware_interface"), result.description);
        return hardware_interface::return_type::ERROR;
      }
    }
  }
  return hardware_interface::return_type::OK;
}

hardware_interface::return_type DynamixelHardwareInterface::write(
  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
  for (const auto & motor : motors_) {
    if (motor->operationSupports(Operation::GOAL_POSITION)) {
      motor->setCurrentGoalPosition();
    }
  }
  return hardware_interface::return_type::OK;
}
}  // namespace dynamixel_hardware_interface

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(
  dynamixel_hardware_interface::DynamixelHardwareInterface, hardware_interface::SystemInterface)
Updated on 17 July 2023 at 01:42:11 UTC