00001 #include "grasp_actions_alg.h" 00002 00003 GraspActionsAlgorithm::GraspActionsAlgorithm(void) 00004 { 00005 } 00006 00007 GraspActionsAlgorithm::~GraspActionsAlgorithm(void) 00008 { 00009 } 00010 00011 void GraspActionsAlgorithm::config_update(Config& new_cfg, uint32_t level) 00012 { 00013 this->lock(); 00014 00015 // save the current configuration 00016 this->config_=new_cfg; 00017 00018 this->unlock(); 00019 } 00020 00021 // GraspActionsAlgorithm Public API 00022 00024 //bool WamActions::check_box(double x, double y, double z){ 00025 // return (x > 0.33 && x < 0.80 && y > -0.4 && y < 0.4 && z > -0.36 && z < 0.8); 00026 //} 00027 // 00029 //bool WamActions::fit_box(double& x, double& y, double& z){ 00030 // 00031 // if(!check_box(x,y,z)){ 00032 // ROS_WARN("Grasping point %f %f %f outside working box [0.33, 0.8] [-0.35, 0.4] [-0.37, 0.8] fitting to box",x,y,z); 00033 // if(x<=0.33) x = 0.33; 00034 // else if(x>=0.8) x = 0.8; 00035 // if(y<=-0.4) y = -0.4; 00036 // else if(y>=0.4) y = 0.4; 00037 // if(z<=-0.36) z = -0.36; 00038 // else if(z>=0.8) z = 0.8; 00039 // ROS_WARN("fit with %f %f %f",x,y,z); 00040 // } 00041 // //TODO check if difference is too high instead of autorize move all the time 00042 // return true; 00043 //} 00044 00045 00046 //bool WamActions::take3D(geometry_msgs::Pose grasping_pose){ 00047 // 00048 // if(!fit_box(x,y,z)) 00049 // return false; 00050 // //execute move 00051 // 00061 // 00062 // //switch to camera coordinates 00063 // //transform to world coordinates 00064 // return true; 00065 //}