00001 00030 #pragma once 00031 00032 #include <ros/ros.h> 00033 #include <vector> 00034 #include <string> 00035 #include <map> 00036 00037 #include <boost/smart_ptr.hpp> 00038 #include <boost/ptr_container/ptr_map.hpp> 00039 #include <sr_robot_msgs/joint.h> 00040 #include <urdf/model.h> 00041 00042 using namespace ros; 00043 00044 00045 namespace shadowhandRosLib 00046 { 00047 enum HandType 00048 { 00049 UNKNOWN, 00050 CAN, 00051 ETHERCAT 00052 }; 00053 } 00054 00055 namespace shadowrobot 00056 { 00057 00062 class HandCommander 00063 { 00064 public: 00065 HandCommander(); 00066 00068 ~HandCommander(); 00069 00070 void sendCommands(std::vector<sr_robot_msgs::joint> joint_vector); 00071 00080 std::string get_controller_state_topic(std::string joint_name); 00081 00089 std::pair<double, double> get_min_max(std::string joint_name); 00090 00097 std::vector<std::string> get_all_joints(); 00098 00099 private: 00101 NodeHandle node_; 00102 00104 std::map<std::string, boost::shared_ptr<urdf::Joint> > all_joints; 00105 00107 Publisher sr_hand_target_pub; 00109 boost::ptr_map<std::string,Publisher> sr_hand_target_pub_map; 00110 00112 std::map<std::string, std::string> sr_hand_sub_topics; 00113 00114 shadowhandRosLib::HandType hand_type; 00115 bool ethercat_controllers_found; 00116 00121 void initializeEthercatHand(); 00122 00123 static const double TIMEOUT_TO_DETECT_CONTROLLER_MANAGER; 00124 }; // end class ShadowhandSubscriber 00125 00126 } // end namespace 00127 00128 /* For the emacs weenies in the crowd. 00129 Local Variables: 00130 c-basic-offset: 2 00131 End: 00132 */