void onMouseMove(float x,float y) { float offsetX = x - positiveX; float offsetY = y - positiveY; dragXs[switchPositive] = 0; dragYs[switchPositive] = 0; dragXs[switchPassive] = 0; dragYs[switchPassive] = 0; if(abs(offsetX) > abs(offsetY) && abs(offsetX)/abs(offsetY) > tan(3.14/6.0)) { if(offsetX >= 0) switchPassive = switchPositive + 1; else switchPassive = switchPositive - 1; if(!canSwitch(switchPositive,switchPassive)) return; if(switchPassive / NUM_COLS == switchPositive / NUM_COLS) { dragXs[switchPositive] = (abs(offsetX) > GRID_SIZE) ? GRID_SIZE*(offsetX/abs(offsetX)) : offsetX; dragXs[switchPassive] = -dragXs[switchPositive]; } } else if(abs(offsetX) < abs(offsetY) && abs(offsetY)/abs(offsetX) > tan(3.14/6.0)) { if(offsetY >= 0) switchPassive = switchPositive + NUM_COLS; else switchPassive = switchPositive - NUM_COLS; if(!canSwitch(switchPositive,switchPassive)) return; if(switchPassive % NUM_COLS == switchPositive % NUM_COLS) { dragYs[switchPositive] = (abs(offsetY) > GRID_SIZE) ? GRID_SIZE *(offsetY/abs(offsetY)): offsetY; dragYs[switchPassive] = -dragYs[switchPositive]; } } }
void onMouseUp(float x,float y) { printf("switchPositive:%d,switchPassive:%d\n",switchPositive,switchPassive); if( switchPositive == switchPassive) return; if(!canSwitch(switchPositive,switchPassive)) return; int temp = jewels[switchPositive]; jewels[switchPositive] = jewels[switchPassive]; jewels[switchPassive] = temp; if(!trySwitch(switchPositive,switchPassive)) { jewels[switchPassive] = jewels[switchPositive]; jewels[switchPositive] = temp; } memset(dragXs,0,num_jewels * sizeof(float)); memset(dragYs,0,num_jewels * sizeof(float)); }
bool prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list, const std::list<hardware_interface::ControllerInfo>& stop_list) { #if (hardware_interface_VERSION_MAJOR == 0) && (hardware_interface_VERSION_MINOR <= 9) printf("indigo hardware_interface_VERSION was found\n"); std::cout << "Start list" << std::endl; std::vector<std::pair<std::string, int> >newControl_map; std::map<std::string, int>::iterator it_modeMap; // for all controllers for (std::list<hardware_interface::ControllerInfo>::const_iterator it=start_list.begin(); it != start_list.end(); ++it) { // for each resource (joint) required by the controller std::cout << "\t"<< it->name << "; type "<< it->type << "; interface "<< it->hardware_interface << std::endl; for (std::set<std::string>::iterator it_set=it->resources.begin(); it_set != it->resources.end(); ++it_set) { if( (it_modeMap = controlModeMap.find(it->type)) != controlModeMap.end()) { std::cout << ' ' << *it_set << "; type code is "<< controlModeMap.at(it->type) << std::endl; } else { ROS_ERROR_NAMED(APPLICATION_NAME, "Desired controler type %s is not supported yet by this hardware interface", it->type.c_str()); return false; } newControl_map.push_back(std::make_pair<std::string, int>(*it_set, controlModeMap.at(it->type))); } } controlledJoints.insert(controlledJoints.end(), newControl_map.begin(), newControl_map.end()); // for all controllers std::cout << "Stop list" << std::endl; for (std::list<hardware_interface::ControllerInfo>::const_iterator it=stop_list.begin(); it != stop_list.end(); ++it) { // for each resource (joint) required by the controller std::cout << "\t"<< it->name << "; type "<< it->type << "; interface "<< it->hardware_interface << std::endl; for (std::set<std::string>::iterator it_set=it->resources.begin(); it_set != it->resources.end(); ++it_set) { // search for it into the current controlled joints list and remove it. for(int i = 0; i != controlledJoints.size(); i++) { std::cout << "i " << i << "; controlledJoint " << controlledJoints[i].first << "; *it_set " << *it_set << std::endl; if(controlledJoints[i].first == *it_set) { std::cout << "Removing joint " << controlledJoints[i].first << std::endl; controlledJoints.erase(controlledJoints.begin() + i); break; } } } } return canSwitch(start_list, stop_list); #else printf("jade onward hardware_interface_VERSION was found\n"); std::cout << "Start list" << std::endl; std::vector<std::pair<std::string, int> >newControl_map; std::map<std::string, int>::iterator it_modeMap; // for all controllers for (std::list<hardware_interface::ControllerInfo>::const_iterator it_control=start_list.begin(); it_control != start_list.end(); ++it_control) { for (std::vector<hardware_interface::InterfaceResources>::const_iterator it=it_control->claimed_resources.begin(); it != it_control->claimed_resources.end(); ++it) { // for each resource (joint) required by the controller std::cout << "\t"<< it_control->name << "; type "<< it_control->type << "; interface "<< it->hardware_interface << std::endl; for (std::set<std::string>::iterator it_set=it->resources.begin(); it_set != it->resources.end(); ++it_set) { if( (it_modeMap = controlModeMap.find(it_control->type)) != controlModeMap.end()) { std::cout << ' ' << *it_set << "; type code is "<< controlModeMap.at(it_control->type) << std::endl; } else { ROS_ERROR_NAMED(APPLICATION_NAME, "Desired controler type %s is not supported yet by this hardware interface", it_control->type.c_str()); return false; } newControl_map.push_back(std::make_pair<std::string, int>(*it_set, controlModeMap.at(it_control->type))); } } } controlledJoints.insert(controlledJoints.end(), newControl_map.begin(), newControl_map.end()); // for all controllers std::cout << "Stop list" << std::endl; for (std::list<hardware_interface::ControllerInfo>::const_iterator it_control=start_list.begin(); it_control != start_list.end(); ++it_control) { for (std::vector<hardware_interface::InterfaceResources>::const_iterator it=it_control->claimed_resources.begin(); it != it_control->claimed_resources.end(); ++it) { // for each resource (joint) required by the controller std::cout << "\t"<< it_control->name << "; type "<< it_control->type << "; interface "<< it->hardware_interface << std::endl; for (std::set<std::string>::iterator it_set=it->resources.begin(); it_set != it->resources.end(); ++it_set) { // search for it into the current controlled joints list and remove it. for(int i = 0; i != controlledJoints.size(); i++) { std::cout << "i " << i << "; controlledJoint " << controlledJoints[i].first << "; *it_set " << *it_set << std::endl; if(controlledJoints[i].first == *it_set) { std::cout << "Removing joint " << controlledJoints[i].first << std::endl; controlledJoints.erase(controlledJoints.begin() + i); break; } } } } } return true; #endif }