00001 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- 00002 00003 // -- BEGIN LICENSE BLOCK ---------------------------------------------- 00004 // This file is part of the SCHUNK Canopen Driver suite. 00005 // 00006 // This program is free software licensed under the LGPL 00007 // (GNU LESSER GENERAL PUBLIC LICENSE Version 3). 00008 // You can find a copy of this license in LICENSE folder in the top 00009 // directory of the source code. 00010 // 00011 // © Copyright 2016 SCHUNK GmbH, Lauffen/Neckar Germany 00012 // © Copyright 2016 FZI Forschungszentrum Informatik, Karlsruhe, Germany 00013 // -- END LICENSE BLOCK ------------------------------------------------ 00014 00015 //---------------------------------------------------------------------- 00022 //---------------------------------------------------------------------- 00023 00024 #ifndef SCHUNK_CANOPEN_HARDWARE_INTERFACE_H_ 00025 #define SCHUNK_CANOPEN_HARDWARE_INTERFACE_H_ 00026 00027 #include <icl_hardware_canopen/CanOpenController.h> 00028 00029 #include <hardware_interface/joint_state_interface.h> 00030 #include <hardware_interface/joint_command_interface.h> 00031 #include <hardware_interface/robot_hw.h> 00032 #include <joint_limits_interface/joint_limits.h> 00033 #include <controller_manager/controller_manager.h> 00034 #include <ros/ros.h> 00035 #include <sensor_msgs/JointState.h> 00036 #include <joint_limits_interface/joint_limits.h> 00037 #include <joint_limits_interface/joint_limits_urdf.h> 00038 #include <joint_limits_interface/joint_limits_rosparam.h> 00039 #include <joint_limits_interface/joint_limits_interface.h> 00040 00041 using namespace icl_hardware; 00042 using namespace canopen_schunk; 00043 00048 class SchunkCanopenHardwareInterface : public hardware_interface::RobotHW 00049 { 00050 public: 00051 SchunkCanopenHardwareInterface (ros::NodeHandle& nh, boost::shared_ptr< CanOpenController >& canopen); 00052 00054 virtual void init(); 00055 00057 virtual void read(); 00058 00060 virtual void write(ros::Time time, ros::Duration period); 00061 00062 virtual bool prepareSwitch( 00063 const std::list<hardware_interface::ControllerInfo> &start_list, 00064 const std::list<hardware_interface::ControllerInfo> &stop_list); 00065 virtual void doSwitch(const std::list<hardware_interface::ControllerInfo>&start_list, 00066 const std::list<hardware_interface::ControllerInfo>&stop_list); 00067 00071 bool isFault() {return m_is_fault;} 00072 00076 sensor_msgs::JointState getJointMessage(); 00077 00078 protected: 00079 ros::NodeHandle m_node_handle; 00080 boost::shared_ptr<CanOpenController> m_canopen_controller; 00081 00082 // Interfaces 00083 hardware_interface::JointStateInterface m_joint_state_interface; 00084 hardware_interface::PositionJointInterface m_position_joint_interface; 00085 joint_limits_interface::PositionJointSoftLimitsInterface m_jnt_limits_interface; 00086 00087 size_t m_num_joints; 00088 00089 std::vector<uint8_t> m_node_ids; 00090 std::vector<std::string> m_joint_names; 00091 std::vector<double> m_joint_positions; 00092 std::vector<double> m_joint_velocity; 00093 std::vector<double> m_joint_effort; 00094 std::vector<double> m_joint_position_commands; 00095 std::vector<double> m_joint_position_commands_last; 00096 00097 std::vector<bool> m_nodes_in_fault; 00098 bool m_is_fault; 00099 00100 joint_limits_interface::JointLimits m_joint_limits; 00101 joint_limits_interface::SoftJointLimits m_joint_soft_limits; // only available through URDF, currently not used. 00102 }; 00103 00104 00105 00106 #endif /*SCHUNK_CANOPEN_HARDWARE_INTERFACE_H_*/