1
0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 18:10:47 +02:00

Added HardwareInterface

This commit is contained in:
Felix Mauch
2019-04-11 14:05:51 +02:00
parent 7b88b47aa1
commit 153585ad9d
7 changed files with 326 additions and 19 deletions

View File

@@ -0,0 +1,72 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// 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.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-11
*
*/
//----------------------------------------------------------------------
#ifndef UR_DRIVER_HARDWARE_INTERFACE_H_INCLUDED
#define UR_DRIVER_HARDWARE_INTERFACE_H_INCLUDED
#include <hardware_interface/robot_hw.h>
#include <hardware_interface/force_torque_sensor_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <algorithm>
#include "ur_rtde_driver/ur/ur_driver.h"
namespace ur_driver
{
class HardwareInterface : public hardware_interface::RobotHW
{
public:
HardwareInterface();
virtual ~HardwareInterface() = default;
virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) override;
virtual void read(const ros::Time& time, const ros::Duration& period) override;
virtual void write(const ros::Time& time, const ros::Duration& period) override;
virtual bool prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list) override;
virtual void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list) override;
const uint32_t getControlFrequency() const;
protected:
std::unique_ptr<UrDriver> ur_driver_;
hardware_interface::JointStateInterface js_interface_;
hardware_interface::PositionJointInterface pj_interface_;
// hardware_interface::VelocityJointInterface vj_interface_;
vector6d_t joint_position_command_;
// std::vector<double> joint_velocity_command_;
vector6d_t joint_positions_;
vector6d_t joint_velocities_;
vector6d_t joint_efforts_;
std::vector<std::string> joint_names_;
};
} // namespace ur_driver
#endif // ifndef UR_DRIVER_HARDWARE_INTERFACE_H_INCLUDED