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 00025 00026 #ifndef SCHUNK_CANOPEN_NODE_H_ 00027 #define SCHUNK_CANOPEN_NODE_H_ 00028 00029 #include <ros/ros.h> 00030 00031 #include "control_msgs/FollowJointTrajectoryAction.h" 00032 #include "actionlib/server/action_server.h" 00033 #include "actionlib/server/server_goal_handle.h" 00034 #include "std_srvs/Trigger.h" 00035 #include "schunk_canopen_driver/HomeAll.h" 00036 #include "schunk_canopen_driver/HomeWithIDs.h" 00037 #include "schunk_canopen_driver/HomeWithJointNames.h" 00038 00039 00040 #include "SchunkCanopenHardwareInterface.h" 00041 00042 #include <icl_hardware_canopen/CanOpenController.h> 00043 00044 using namespace icl_hardware; 00045 using namespace canopen_schunk; 00046 00047 class SchunkCanopenNode 00048 { 00049 public: 00050 SchunkCanopenNode(); 00051 00052 00053 private: 00054 ros::NodeHandle m_priv_nh; 00055 ros::NodeHandle m_pub_nh; 00056 00057 // Action interfaces for standalone mode (without ros_control) 00058 void goalCB(actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh); 00059 void cancelCB(actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh); 00063 void trajThread(actionlib::ServerGoalHandle< control_msgs::FollowJointTrajectoryAction >& gh); 00064 00065 00070 bool enableNodes(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& resp); 00071 00077 bool quickStopNodes(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& resp); 00078 00088 bool homeNodesCanIds(schunk_canopen_driver::HomeWithIDsRequest& req, 00089 schunk_canopen_driver::HomeWithIDsResponse& resp); 00090 00100 bool homeNodesJointNames(schunk_canopen_driver::HomeWithJointNamesRequest& req, 00101 schunk_canopen_driver::HomeWithJointNamesResponse& resp); 00102 00107 bool homeAllNodes(schunk_canopen_driver::HomeAllRequest& req, 00108 schunk_canopen_driver::HomeAllResponse& resp); 00109 00110 00117 bool closeBrakes (std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& resp); 00118 00125 bool initDevicesCb (std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& resp); 00126 00131 void initDevices(); 00132 00136 void rosControlLoop (); 00137 00138 00139 actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> m_action_server; 00140 ros::ServiceServer m_enable_service; 00141 ros::ServiceServer m_close_brakes_service; 00142 ros::ServiceServer m_quick_stop_service; 00143 ros::ServiceServer m_home_service_all; 00144 ros::ServiceServer m_home_service_joint_names; 00145 ros::ServiceServer m_home_service_canopen_ids; 00146 ros::ServiceServer m_init_service; 00147 00148 ros::Publisher m_joint_pub; 00149 ros::Publisher m_current_pub; 00150 00151 00152 CanOpenController::Ptr m_controller; 00153 00154 std::vector<DS402Group::Ptr> m_chain_handles; 00155 std::map<std::string, uint8_t> m_joint_name_mapping; 00156 bool m_has_goal; 00157 boost::thread m_traj_thread; 00158 boost::thread m_ros_control_thread; 00159 00160 bool m_use_ros_control; 00161 00162 boost::shared_ptr<SchunkCanopenHardwareInterface> m_hardware_interface; 00163 boost::shared_ptr<controller_manager::ControllerManager> m_controller_manager; 00164 00165 bool m_was_disabled; 00166 bool m_is_enabled; 00167 bool m_homing_active; 00168 00169 ds402::ProfilePositionModeConfiguration m_ppm_config; 00170 bool m_nodes_initialized; 00171 00172 std::string m_traj_controller_name; 00173 00174 }; 00175 00176 #endif /* SCHUNK_CANOPEN_NODE_H_ */