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
}
