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 namespace shadowhandRosLib 00045 { 00046 enum HandType 00047 { 00048 UNKNOWN, 00049 CAN, 00050 ETHERCAT 00051 }; 00052 } 00053 00054 namespace shadowrobot 00055 { 00060 class HandCommander 00061 { 00062 public: 00063 HandCommander(const std::string& ns = ""); 00064 00066 ~HandCommander(); 00067 00068 void sendCommands(std::vector<sr_robot_msgs::joint> joint_vector); 00069 00078 std::string get_controller_state_topic(std::string joint_name); 00079 00087 std::pair<double, double> get_min_max(std::string joint_name); 00088 00095 std::vector<std::string> get_all_joints(); 00096 00097 private: 00099 NodeHandle node_; 00100 00102 std::map<std::string, boost::shared_ptr<urdf::Joint> > all_joints; 00103 00105 Publisher sr_hand_target_pub; 00107 boost::ptr_map<std::string,Publisher> sr_hand_target_pub_map; 00108 00110 std::map<std::string, std::string> sr_hand_sub_topics; 00111 00112 shadowhandRosLib::HandType hand_type; 00113 bool ethercat_controllers_found; 00114 00119 void initializeEthercatHand(); 00120 00121 static const double TIMEOUT_TO_DETECT_CONTROLLER_MANAGER; 00122 }; // end class ShadowhandSubscriber 00123 00124 } // end namespace 00125 00126 /* For the emacs weenies in the crowd. 00127 Local Variables: 00128 c-basic-offset: 2 00129 End: 00130 */