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;
}
