int main(int argc, char** argv){ ros::init(argc, argv, "lift_test"); // figure out what kind of five we want bool left = false; bool right = true; if(argc > 1) { left = (strcmp(argv[1],"left") == 0) || (strcmp(argv[1],"double") == 0); right = (strcmp(argv[1],"right") == 0) || (strcmp(argv[1],"double") == 0); } RobotArm arm; Gripper gripper; gripper.close(left,right,true); while(ros::ok()) { float pre_five_r []= {-1.4204039529994779, 0.64014688694872024, 0.64722849826846929, -1.9254939970572147, 30.931365914354672, -0.52166442520665446, -16.642483924162743 }; float pre_five_l []= {1.544791237364544, 0.028669297805660576, -0.061946460502633194, -1.9322034349027488, -0.96915535302752587, -0.22688029069077575, -3.5411348633607669}; if(left) arm.startTrajectory(arm.arm_trajectoryPoint(pre_five_l,2.0,false),false); if(right) arm.startTrajectory(arm.arm_trajectoryPoint(pre_five_r,2.0,true),true); sleep(2.0); float five_r []= {-0.43912122996219438, -0.26865637196243625, -0.06073806015457861, -1.0597651429837187, 30.217764249732564, -0.098888248598895112, -17.139399300620212 }; float five_l []= {0.34795130919909756, -0.33483508677418122, 0.21450526015905091, -0.87161320330702452, -0.55300340950519367, -0.51138511922202357, -3.1737890509598912}; if(left) arm.startTrajectory(arm.arm_trajectoryPoint(five_l,1.25,false),false); if(right) arm.startTrajectory(arm.arm_trajectoryPoint(five_r,1.25,true),true); // now start looking for a slap during the move gripper.slap(left,right); //wait for a slap while(!gripper.slapDone(left,right) && ros::ok()) { ros::Duration(0.005).sleep(); } float post_five_r []= {-1.2271486279403441, 0.98994689398564029, 0.27937452657595019, -1.9855738422813785, 31.095246711685615, -0.75469820126008202, -17.206098475132887 }; float post_five_l []= {1.458651261930636, 1.0444005395208478, 0.081571109098415029, -2.1115743463069396, -1.3390296269894097, -0.16823026639652239, -3.5006715676681437}; if(left) arm.startTrajectory(arm.arm_trajectoryPoint(post_five_l,1.35,false),false); if(right) arm.startTrajectory(arm.arm_trajectoryPoint(post_five_r,1.35,true),true); sleep(2.0); } return 0; }
int main(int argc, char** argv){ ros::init(argc, argv, "gripper"); Gripper gripper; gripper.open(); ros::Duration(5.0).sleep(); // sleep for 5 seconds gripper.close(); return 0; }
int main(int argc, char** argv){ ros::init(argc, argv, "simple_gripper"); Gripper gripper; gripper.open(); gripper.close(); return 0; }
int main(int argc, char** argv){ ros::init(argc, argv, "simple_gripper"); //ros::NodeHandle nh; //Gripper gripper(nh); Gripper gripper; gripper.open(); gripper.close(); return 0; }
LRESULT CALLBACK Gripper::staticWinProc(HWND hwnd, UINT message, WPARAM wParam, LPARAM lParam) { Gripper *pDlgMoving = NULL; switch (message) { case WM_NCCREATE : pDlgMoving = reinterpret_cast<Gripper *>(reinterpret_cast<LPCREATESTRUCT>(lParam)->lpCreateParams); pDlgMoving->_hSelf = hwnd; ::SetWindowLongPtr(hwnd, GWLP_USERDATA, reinterpret_cast<LONG_PTR>(pDlgMoving)); return TRUE; default : pDlgMoving = reinterpret_cast<Gripper *>(::GetWindowLongPtr(hwnd, GWLP_USERDATA)); if (!pDlgMoving) return ::DefWindowProc(hwnd, message, wParam, lParam); return pDlgMoving->runProc(message, wParam, lParam); } }
LRESULT DockingManager::runProc(HWND hwnd, UINT message, WPARAM wParam, LPARAM lParam) { switch (message) { case WM_NCACTIVATE: { // activate/deactivate titlebar of toolbars for (size_t iCont = DOCKCONT_MAX, len = _vContainer.size(); iCont < len; ++iCont) { ::SendMessage(_vContainer[iCont]->getHSelf(), WM_NCACTIVATE, wParam, (LPARAM)-1); } if ((int)lParam != -1) { ::SendMessage(_hParent, WM_NCACTIVATE, wParam, (LPARAM)-1); } break; } case WM_MOVE: case WM_SIZE: { onSize(); break; } case WM_DESTROY: { // unregister window event hooking BEFORE EVERYTHING ELSE if (hWndServer == hwnd) { UnhookWindowsHookEx(gWinCallHook); gWinCallHook = NULL; hWndServer = NULL; } // destroy imagelist if it exists if (_hImageList != NULL) { ::ImageList_Destroy(_hImageList); } // destroy containers for (int i = _vContainer.size(); i > 0; i--) { _vContainer[i-1]->destroy(); delete _vContainer[i-1]; } CoUninitialize(); break; } case DMM_LBUTTONUP: //is this message still relevant? { if (::GetActiveWindow() != _hParent) break; // set respective activate state for (int i = 0; i < DOCKCONT_MAX; ++i) { _vContainer[i]->SetActive(IsChild(_vContainer[i]->getHSelf(), ::GetFocus())); } return TRUE; } case DMM_MOVE: { Gripper *pGripper = new Gripper; pGripper->init(_hInst, _hParent); pGripper->startGrip((DockingCont*)lParam, this); break; } case DMM_MOVE_SPLITTER: { int offset = wParam; for (int iCont = 0; iCont < DOCKCONT_MAX; ++iCont) { if (_vSplitter[iCont]->getHSelf() == (HWND)lParam) { switch (iCont) { case CONT_TOP: _dockData.rcRegion[iCont].bottom -= offset; if (_dockData.rcRegion[iCont].bottom < 0) { _dockData.rcRegion[iCont].bottom = 0; } if ((_rcWork.bottom < (-SPLITTER_WIDTH)) && (offset < 0)) { _dockData.rcRegion[iCont].bottom += offset; } break; case CONT_BOTTOM: _dockData.rcRegion[iCont].bottom += offset; if (_dockData.rcRegion[iCont].bottom < 0) { _dockData.rcRegion[iCont].bottom = 0; } if ((_rcWork.bottom < (-SPLITTER_WIDTH)) && (offset > 0)) { _dockData.rcRegion[iCont].bottom -= offset; } break; case CONT_LEFT: _dockData.rcRegion[iCont].right -= offset; if (_dockData.rcRegion[iCont].right < 0) { _dockData.rcRegion[iCont].right = 0; } if ((_rcWork.right < SPLITTER_WIDTH) && (offset < 0)) { _dockData.rcRegion[iCont].right += offset; } break; case CONT_RIGHT: _dockData.rcRegion[iCont].right += offset; if (_dockData.rcRegion[iCont].right < 0) { _dockData.rcRegion[iCont].right = 0; } if ((_rcWork.right < SPLITTER_WIDTH) && (offset > 0)) { _dockData.rcRegion[iCont].right -= offset; } break; } onSize(); break; } } break; } case DMM_DOCK: case DMM_FLOAT: { toggleActiveTb((DockingCont*)lParam, message); return FALSE; } case DMM_CLOSE: { tTbData TbData = *((DockingCont*)lParam)->getDataOfActiveTb(); return SendNotify(TbData.hClient, DMN_CLOSE); } case DMM_FLOATALL: { toggleVisTb((DockingCont*)lParam, DMM_FLOAT); return FALSE; } case DMM_DOCKALL: { toggleVisTb((DockingCont*)lParam, DMM_DOCK); return FALSE; } case DMM_GETIMAGELIST: { return (LPARAM)_hImageList; } case DMM_GETICONPOS: { for (UINT uImageCnt = 0, len = _vImageList.size(); uImageCnt < len; ++uImageCnt) { if ((HWND)lParam == _vImageList[uImageCnt]) { return uImageCnt; } } return -1; } default : break; } return ::DefWindowProc(_hSelf, message, wParam, lParam); }
int main(int argc, char **argv) { //initialize the ROS node ros::init(argc, argv, "manipulator"); ros::NodeHandle nh; ROS_INFO("Waiting to receive a point cloud to manipulate\n"); bool pickup_success; bool model_fit; int model_id; Gripper gripper; geometry_msgs::Point reset_posn; reset_posn.x = 0.6; reset_posn.y = -0.5; reset_posn.z = 0; geometry_msgs::Point red; red.x = 0.6; red.y = -0.5; red.z = -0.1; //Red bin geometry_msgs::Point blue; blue.x = 0.6; blue.y = -0.6; blue.z = -0.1; //Blue bin geometry_msgs::Point green; green.x = 0.6; green.y = -0.7; green.z = -0.1; //Green bin geometry_msgs::Point big, medium, small; big = red; medium = blue; small = green; // const geometry_msgs::Point yellow = {0.45, -0.5, 0}; //Yellow bin // const geometry_msgs::Point orange = {0.35, -0.5, 0}; //Orange bin int red_color = 0xff0000; const float RGB_RED = *reinterpret_cast<float*>(&red_color); int blue_color = 0xff; const float RGB_BLUE = *reinterpret_cast<float*>(&blue_color); int green_color = 0xff00; const float RGB_GREEN = *reinterpret_cast<float*>(&green_color); ros::Subscriber sub = nh.subscribe("pick_place",1,manipulate); ros::ServiceClient client_newpcd = nh.serviceClient<duplo_v0::Get_New_PCD>("get_new_pcd"); duplo_v0::Get_New_PCD srv_newpcd; //set service and action names const std::string OBJECT_DETECTION_SERVICE_NAME = "/object_detection"; const std::string COLLISION_PROCESSING_SERVICE_NAME = "/tabletop_collision_map_processing/tabletop_collision_map_processing"; const std::string PICKUP_ACTION_NAME = "/object_manipulator/object_manipulator_pickup"; //const std::string PLACE_ACTION_NAME = // "/object_manipulator/object_manipulator_place"; const std::string MODEL_FITTING_SERVICE_NAME = "/object_recognition"; //create service and action clients ros::ServiceClient object_detection_srv; ros::ServiceClient collision_processing_srv; actionlib::SimpleActionClient<object_manipulation_msgs::PickupAction> pickup_client(PICKUP_ACTION_NAME, true); // actionlib::SimpleActionClient<object_manipulation_msgs::PlaceAction> // place_client(PLACE_ACTION_NAME, true); ros::ServiceClient model_fitting_srv; //wait for detection client while ( !ros::service::waitForService(OBJECT_DETECTION_SERVICE_NAME, ros::Duration(2.0)) && nh.ok() ) { ROS_INFO("Waiting for object detection service to come up"); } if (!nh.ok()) exit(0); object_detection_srv = nh.serviceClient<tabletop_object_detector::TabletopDetection> (OBJECT_DETECTION_SERVICE_NAME, true); //wait for collision map processing client while ( !ros::service::waitForService(COLLISION_PROCESSING_SERVICE_NAME, ros::Duration(2.0)) && nh.ok() ) { ROS_INFO("Waiting for collision processing service to come up"); } if (!nh.ok()) exit(0); collision_processing_srv = nh.serviceClient <tabletop_collision_map_processing::TabletopCollisionMapProcessing> (COLLISION_PROCESSING_SERVICE_NAME, true); //wait for pickup client while(!pickup_client.waitForServer(ros::Duration(2.0)) && nh.ok()) { ROS_INFO_STREAM("Waiting for action client " << PICKUP_ACTION_NAME); } if (!nh.ok()) exit(0); //wait for place client /* while(!place_client.waitForServer(ros::Duration(2.0)) && nh.ok()) { ROS_INFO_STREAM("Waiting for action client " << PLACE_ACTION_NAME); } if (!nh.ok()) exit(0); */ /*while ( !ros::service::waitForService(MODEL_FITTING_SERVICE_NAME, ros::Duration(2.0)) && nh.ok() ) { ROS_INFO("Waiting for model fitting service to come up"); } if (!nh.ok()) exit(0); model_fitting_srv = nh.serviceClient<tabletop_object_detector::TabletopObjectRecognition> (MODEL_FITTING_SERVICE_NAME, true); */ while (ros::ok() && start_manipulation == false) { ros::spinOnce(); if (start_manipulation==true) { start_manipulation = false; //call the tabletop detection ROS_INFO("Calling tabletop detector"); tabletop_object_detector::TabletopDetection detection_call; //we want recognized database objects returned //set this to false if you are using the pipeline without the database detection_call.request.return_clusters = true; //we want the individual object point clouds returned as well detection_call.request.return_models = false; if (!object_detection_srv.call(detection_call)) { ROS_ERROR("Tabletop detection service failed"); return false; } if (detection_call.response.detection.result != detection_call.response.detection.SUCCESS) { ROS_ERROR("Tabletop detection returned error code %d", detection_call.response.detection.result); return false; } if (detection_call.response.detection.clusters.empty() && detection_call.response.detection.models.empty() ) { ROS_DEBUG("The tabletop detector detected the table, but found no objects"); //return false; } detection_call.response.detection.clusters.clear(); //call collision map processing ROS_INFO("Calling collision map processing"); tabletop_collision_map_processing::TabletopCollisionMapProcessing processing_call; //pass the result of the tabletop detection processing_call.request.detection_result = detection_call.response.detection; //ask for the exising map and collision models to be reset //processing_call.request.reset_static_map = true; processing_call.request.reset_collision_models = true; processing_call.request.reset_attached_models = true; //ask for a new static collision map to be taken with the laser //after the new models are added to the environment //processing_call.request.take_static_collision_map = false; //ask for the results to be returned in base link frame processing_call.request.desired_frame = "base_link"; if (!collision_processing_srv.call(processing_call)) { ROS_ERROR("Collision map processing service failed"); return false; } //the collision map processor returns instances of graspable objects if (processing_call.response.graspable_objects.empty()) { ROS_DEBUG("Collision map processing returned no graspable objects"); //return false; } //call object pickup ROS_INFO("Calling the pickup action"); object_manipulation_msgs::PickupGoal pickup_goal; //pass one of the graspable objects returned by the collission map processor /*********************************************************************/ /*********************************************************************/ /********** Changed *********/ //pickup_goal.target = processing_call.response.graspable_objects.at(0); object_manipulation_msgs::GraspableObject object; //object.reference_frame_id = "/openni_rgb_frame"; sensor_msgs::PointCloud goal_pcd; sensor_msgs::PointCloud2 pick_cluster; /*toROSMsg(to_pick,pick_cluster); tf::TransformListener listener; sensor_msgs::PointCloud2 transformed; pick_cluster.header.frame_id = "openni_rgb_optical_frame"; listener.waitForTransform("openni_rgb_optical_frame","base_link", pick_cluster.header.stamp,ros::Duration(2.0)); pcl_ros::transformPointCloud("base_link",pick_cluster,transformed,listener); */ if (sensor_msgs::convertPointCloud2ToPointCloud(to_pick,goal_pcd) == true) { ROS_INFO("Frame Id of input point cloud cluster is: %s\n", goal_pcd.header.frame_id.c_str()); ROS_INFO("Target frame id is: %s\n", detection_call.response.detection.table.pose.header.frame_id.c_str()); goal_pcd.header.frame_id = "base_link"; goal_pcd.header.stamp = ros::Time::now(); object.cluster = goal_pcd; object.reference_frame_id = "base_link"; pickup_goal.target = object; ROS_INFO("Set the goal target as a graspable object\n"); } else { ROS_ERROR("Conversion from pointcloud2 to pointcloud failed.\n"); return false; } /**** Fitting a model to goal_pcd ****/ /* tabletop_object_detector::TabletopObjectRecognition fitting_call; fitting_call.request.clusters.push_back(goal_pcd); fitting_call.request.num_models = 1; fitting_call.request.perform_fit_merge = false; if (!model_fitting_srv.call(fitting_call)) { ROS_ERROR("Model fitting service failed"); model_fit = false; //return false; } else { model_fit = true; model_id = fitting_call.response.cluster_model_indices[0]; } */ //pass the name that the object has in the collision environment //this name was also returned by the collision map processor //pickup_goal.collision_object_name = // processing_call.response.collision_object_names.at(0); /*********************************************************************/ /*********************************************************************/ //pass the collision name of the table, also returned by the collision //map processor pickup_goal.collision_support_surface_name = processing_call.response.collision_support_surface_name; /*********************************************************************/ /*********************************************************************/ /******* Added: allowing collisions with the table ******/ pickup_goal.allow_gripper_support_collision = true; /*********************************************************************/ /*********************************************************************/ //pick up the object with the right arm pickup_goal.arm_name = "right_arm"; //specify the desired distance between pre-grasp and final grasp //pickup_goal.desired_approach_distance = 0.1; //pickup_goal.min_approach_distance = 0.05; //we will be lifting the object along the "vertical" direction //which is along the z axis in the base_link frame geometry_msgs::Vector3Stamped direction; direction.header.stamp = ros::Time::now(); direction.header.frame_id = "base_link"; direction.vector.x = 0; direction.vector.y = 0; direction.vector.z = 1; pickup_goal.lift.direction = direction; //request a vertical lift of 10cm after grasping the object pickup_goal.lift.desired_distance = 0.15; pickup_goal.lift.min_distance = 0.1; //do not use tactile-based grasping or tactile-based lift pickup_goal.use_reactive_lift = false; pickup_goal.use_reactive_execution = false; //send the goal pickup_client.sendGoal(pickup_goal); while (!pickup_client.waitForResult(ros::Duration(5.0))) { ROS_INFO("Waiting for the pickup action..."); } object_manipulation_msgs::PickupResult pickup_result = *(pickup_client.getResult()); if (pickup_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED) { ROS_ERROR("The pickup action has failed with result code %d", pickup_result.manipulation_result.value); pickup_success = false; //return false; } else { ROS_INFO("The pickup action succeeded."); pickup_success= true; } if (pickup_success) { pcl::PointCloud<pcl::PointXYZRGB> temp; fromROSMsg(to_pick,temp); float longest_dim = find_longest_dim(temp); ROS_INFO("PP: Longest dimension of cluster = %f", longest_dim); if (longest_dim > 0.09) { //Place in size A bin ROS_INFO("PP: Placing in big bin"); move_arm(big); } else if (longest_dim >= 0.05 && longest_dim <= 0.08) { //Place in size B bin ROS_INFO("PP: Placing in medium bin"); move_arm(medium); } else if (longest_dim < 0.05) { //Place in size C bin ROS_INFO("PP: Placing in small bin"); move_arm(small); } else { move_arm(reset_posn); } gripper.open(); } else { move_arm(reset_posn); } RobotHead head; head.lookAt("base_link", 0.2, 0.0, 1.0); gripper.open(); ros::Duration(2).sleep(); // Ask for new PCD srv_newpcd.request.question = true; if (client_newpcd.call(srv_newpcd)) { ROS_INFO("Requesting for new point cloud."); } else { ROS_ERROR("Failed to call service get new pcd."); return 1; } } //ros::spin(); } return 0; }
int main() { Gripper gripper; pthread_t com=launch(&gripper,Com); //!<start communication pthread_t fault=launch(&gripper,Fault); //!<start fault monitoring //!Connect while(!gripper.isConnected()) { gripper.connect("192.168.1.11",502); usleep(sec); }cout << "connected" <<endl; //!Activation while(!gripper.isActivated()){ gripper.activate(); usleep(sec); }cout << "activated" <<endl<<endl; //============================================================================ // gripper do stuff here // refer to the interface documentation interface.pdf for available methods //============================================================================ //!Deactivate gripper.go(false); gripper.clear(); gripper.go(true); while(!gripper.isMoving()){ usleep(ms); }cout<<"moving back to full open"<<endl; while(gripper.isMoving()){ usleep(ms); }cout<<"position reached"<<endl; gripper.deactivate(); cout<<"deactivated"<<endl; gripper.disconnect(); cout<<"disconnected"<<endl; terminate(fault); terminate(com); return 0; }