00001 00002 #ifndef CANOPEN_MOTOR_NODE_ROBOT_LAYER_H_ 00003 #define CANOPEN_MOTOR_NODE_ROBOT_LAYER_H_ 00004 00005 #include <boost/unordered_map.hpp> 00006 #include <hardware_interface/joint_command_interface.h> 00007 #include <hardware_interface/joint_state_interface.h> 00008 #include <joint_limits_interface/joint_limits_interface.h> 00009 #include <hardware_interface/robot_hw.h> 00010 #include <urdf/urdfdom_compatibility.h> 00011 #include <urdf/model.h> 00012 #include <canopen_402/base.h> 00013 #include <canopen_motor_node/handle_layer_base.h> 00014 00015 00016 namespace canopen { 00017 00018 class RobotLayer : public LayerGroupNoDiag<HandleLayerBase>, public hardware_interface::RobotHW{ 00019 hardware_interface::JointStateInterface state_interface_; 00020 hardware_interface::PositionJointInterface pos_interface_; 00021 hardware_interface::VelocityJointInterface vel_interface_; 00022 hardware_interface::EffortJointInterface eff_interface_; 00023 00024 joint_limits_interface::PositionJointSoftLimitsInterface pos_soft_limits_interface_; 00025 joint_limits_interface::PositionJointSaturationInterface pos_saturation_interface_; 00026 joint_limits_interface::VelocityJointSoftLimitsInterface vel_soft_limits_interface_; 00027 joint_limits_interface::VelocityJointSaturationInterface vel_saturation_interface_; 00028 joint_limits_interface::EffortJointSoftLimitsInterface eff_soft_limits_interface_; 00029 joint_limits_interface::EffortJointSaturationInterface eff_saturation_interface_; 00030 00031 ros::NodeHandle nh_; 00032 urdf::Model urdf_; 00033 00034 typedef boost::unordered_map< std::string, boost::shared_ptr<HandleLayerBase> > HandleMap; 00035 HandleMap handles_; 00036 struct SwitchData { 00037 boost::shared_ptr<HandleLayerBase> handle; 00038 canopen::MotorBase::OperationMode mode; 00039 bool enforce_limits; 00040 }; 00041 typedef std::vector<SwitchData> SwitchContainer; 00042 typedef boost::unordered_map<std::string, SwitchContainer> SwitchMap; 00043 SwitchMap switch_map_; 00044 00045 boost::atomic<bool> first_init_; 00046 00047 void stopControllers(const std::vector<std::string> controllers); 00048 public: 00049 void add(const std::string &name, boost::shared_ptr<HandleLayerBase> handle); 00050 RobotLayer(ros::NodeHandle nh); 00051 urdf::JointConstSharedPtr getJoint(const std::string &n) const { return urdf_.getJoint(n); } 00052 00053 virtual void handleInit(canopen::LayerStatus &status); 00054 void enforce(const ros::Duration &period, bool reset); 00055 virtual bool prepareSwitch(const std::list<hardware_interface::ControllerInfo> &start_list, const std::list<hardware_interface::ControllerInfo> &stop_list); 00056 virtual void doSwitch(const std::list<hardware_interface::ControllerInfo> &start_list, const std::list<hardware_interface::ControllerInfo> &stop_list); 00057 }; 00058 00059 } // namespace canopen 00060 00061 #endif