Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include <ros/node_handle.h>
00019
00020 #include <cob_hand_bridge/InitFinger.h>
00021 #include <cob_hand_bridge/InitPins.h>
00022 #include <cob_hand_bridge/SetPWM.h>
00023 #include <cob_hand_bridge/UpdatePins.h>
00024 #include <cob_hand_bridge/Status.h>
00025 #include <std_msgs/UInt8.h>
00026 #include <std_srvs/Trigger.h>
00027
00028 #include <boost/thread/thread.hpp>
00029 #include <boost/scoped_ptr.hpp>
00030
00031 #include "hardware.h"
00032 #include "gpio.h"
00033 #include "sdhx.h"
00034
00035 ros::NodeHandle_<HandBridgeHardware> g_nh;
00036 cob_hand_bridge::Status g_status_msg;
00037
00038
00039
00040 boost::scoped_ptr<SDHX> g_sdhx;
00041
00042 using cob_hand_bridge::InitFinger;
00043
00044 InitFinger::Request g_init_req;
00045 std::string g_port;
00046
00047 bool initFinger(const InitFinger::Request & req){
00048 if(!g_sdhx){
00049 g_sdhx.reset(new SDHX);
00050 if(g_sdhx->init(req.port, req.min_pwm0, req.min_pwm1, req.max_pwm0, req.max_pwm1)){
00051 g_port = req.port;
00052 g_init_req = req;
00053 g_init_req.port = g_port.c_str();
00054 return true;
00055 }
00056 }
00057 return false;
00058 }
00059
00060 void handleInitFinger(const InitFinger::Request & req, InitFinger::Response & res){
00061 res.success = initFinger(req);
00062 }
00063 ros::ServiceServer<InitFinger::Request, InitFinger::Response> g_srv_init_finger("init_finger",&handleInitFinger);
00064
00065 using std_srvs::Trigger;
00066 void handleHaltFinger(const Trigger::Request & req, Trigger::Response & res){
00067 res.success = g_sdhx && g_sdhx->halt();
00068 }
00069 ros::ServiceServer<Trigger::Request, Trigger::Response> g_srv_halt_finger("halt",&handleHaltFinger);
00070
00071 void handleRecover(const Trigger::Request & req, Trigger::Response & res){
00072 if(g_sdhx) {
00073 if((g_status_msg.status & g_status_msg.MASK_ERROR) == 0 && g_status_msg.rc == 0) {
00074 res.success = true;
00075 }else if(g_sdhx->isInitialized()) {
00076 g_sdhx.reset();
00077 res.success = initFinger(g_init_req);
00078 if(res.success) g_status_msg.status &= ~g_status_msg.MASK_ERROR;
00079 g_status_msg.rc = 0;
00080 }else{
00081 res.message = "Not initialized";
00082 }
00083 }
00084 }
00085 ros::ServiceServer<Trigger::Request, Trigger::Response> g_srv_recover("recover",&handleRecover);
00086
00087 void handleJointCommand(const cob_hand_bridge::JointValues& j){
00088 if(g_sdhx) g_sdhx->move(j.position_cdeg, j.velocity_cdeg_s, j.current_100uA);
00089 }
00090 ros::Subscriber<cob_hand_bridge::JointValues> g_sub_command("command", handleJointCommand );
00091
00092
00093 GPIO g_gpio;
00094
00095 using cob_hand_bridge::InitPins;
00096
00097 void handleInitPins(const InitPins::Request & req, InitPins::Response & res){
00098 res.success = g_gpio.init();
00099
00100 for(size_t i = 0; i < req.input_pins_length; ++i)
00101 if(!g_gpio.setInput(req.input_pins[i]))
00102 res.success = false;
00103
00104 for(size_t i = 0; i < req.output_pins_length; ++i)
00105 if(!g_gpio.setOutput(req.output_pins[i]))
00106 res.success = false;
00107 }
00108 ros::ServiceServer<InitPins::Request, InitPins::Response> g_srv_init_pins("init_pins",&handleInitPins);
00109
00110 using cob_hand_bridge::UpdatePins;
00111 void handleUpdatePins(const UpdatePins::Request & req, UpdatePins::Response & res){
00112 res.success = g_gpio.setPins(req.set_pins) | g_gpio.clearPins(req.clear_pins);
00113 uint32_t state = g_gpio.getState();
00114 res.success = res.success && (state & req.set_pins) == req.set_pins && (state & req.clear_pins) == 0;
00115 }
00116 ros::ServiceServer<UpdatePins::Request, UpdatePins::Response> g_srv_update_pins("update_pins",&handleUpdatePins);
00117
00118 void handleClearPin(const std_msgs::UInt8& mgs){
00119 g_gpio.writePin(mgs.data, 0);
00120 }
00121 ros::Subscriber<std_msgs::UInt8> g_sub_clear_pin("clear_pin", handleClearPin );
00122
00123 void handleSetPin(const std_msgs::UInt8& mgs){
00124 g_gpio.writePin(mgs.data, 1);
00125 }
00126 ros::Subscriber<std_msgs::UInt8> g_sub_set_pin("set_pin", handleSetPin );
00127
00128 ros::Publisher g_pub("status", &g_status_msg);
00129
00130 using cob_hand_bridge::SetPWM;
00131
00132 void handleSetPWM(const SetPWM::Request & req, SetPWM::Response & res){
00133
00134 if(req.pins_length != req.levels_length){
00135 res.success = false;
00136 return;
00137 }
00138 res.success = true;
00139
00140 for(size_t i = 0; i < req.pins_length; ++i)
00141 if(!g_gpio.pwmPin(req.pins[i],req.levels[i]))
00142 res.success = false;
00143 }
00144 ros::ServiceServer<SetPWM::Request, SetPWM::Response> g_srv_set_pwm("set_pwm",&handleSetPWM);
00145
00146 void step(){
00147 g_status_msg.stamp = g_nh.now();
00148
00149 g_status_msg.status = g_status_msg.NOT_INITIALIZED;
00150 if(g_gpio.isInitialized()) g_status_msg.status |= g_status_msg.MASK_GPIO_READY;
00151
00152 cob_hand_bridge::JointValues &j= g_status_msg.joints;
00153
00154 if(g_sdhx && g_sdhx->isInitialized()){
00155 if(g_sdhx->getData(j.position_cdeg, j.velocity_cdeg_s, j.current_100uA, boost::chrono::seconds(1))) g_status_msg.status |= g_status_msg.MASK_FINGER_READY;
00156 else g_status_msg.status |= g_status_msg.MASK_ERROR;
00157 g_status_msg.rc = g_sdhx->getRC();
00158 }
00159
00160 g_status_msg.pins = g_gpio.getState();
00161
00162 g_pub.publish( &g_status_msg );
00163 g_nh.spinOnce();
00164
00165 if(g_sdhx) g_sdhx->poll();
00166
00167 ++g_status_msg.seq;
00168 }
00169
00170 int main(int argc, char** argv){
00171
00172 double rate = 20;
00173
00174 if(argc != 2 && argc != 3){
00175 std::cerr << "Usage: " << argv[0] << " port@baud [looprate]" << std::endl;
00176 exit(1);
00177 }
00178 if(argc == 3){
00179 rate = boost::lexical_cast<double>(argv[2]);
00180 if(rate <= 0.0){
00181 std::cerr << "Rate must be postive" << std::endl;
00182 exit(1);
00183 }
00184 }
00185
00186 std::string dev(argv[1]);
00187 g_nh.initNode(&dev[0]);
00188
00189 g_nh.advertiseService(g_srv_init_finger);
00190 g_nh.advertiseService(g_srv_halt_finger);
00191 g_nh.advertiseService(g_srv_recover);
00192 g_nh.subscribe(g_sub_command);
00193
00194 g_nh.advertiseService(g_srv_init_pins);
00195 g_nh.advertiseService(g_srv_set_pwm);
00196 g_nh.advertiseService(g_srv_update_pins);
00197 g_nh.subscribe(g_sub_clear_pin);
00198 g_nh.subscribe(g_sub_set_pin);
00199
00200 g_nh.advertise(g_pub);
00201
00202 boost::chrono::duration<double> double_interval(1.0/rate);
00203 boost::chrono::milliseconds interval = boost::chrono::duration_cast<boost::chrono::milliseconds>(double_interval);
00204 boost::chrono::steady_clock::time_point next = boost::chrono::steady_clock::now();
00205
00206 while(true) {
00207 step();
00208 next += interval;
00209 boost::this_thread::sleep_until(next);
00210 }
00211
00212 return 0;
00213 }