00001 #define DEBUG_DRAW_TABLE_MARKERS
00002 #define DEBUG_DRAW_TABLETOP_OBJECTS
00003
00004
00005 #define END_EFFECTOR_OFFSET 0.175 //0.193
00006
00007 #include <plane_extractor.h>
00008 #include <move_omni_base.h>
00009
00010 #include <book_stacking_msgs/DragRequest.h>
00011 #include <book_stacking_msgs/NavWaypoints.h>
00012 #include <book_stacking_msgs/ObjectInfos.h>
00013 #include <book_stacking_msgs/ObjectInfo.h>
00014
00015
00016 #include <ros/ros.h>
00017 #include <actionlib/client/simple_action_client.h>
00018 #include <geometry_msgs/PoseStamped.h>
00019 #include <geometry_msgs/Vector3Stamped.h>
00020 #include <geometry_msgs/Quaternion.h>
00021 #include <sensor_msgs/PointCloud2.h>
00022 #include <sensor_msgs/JointState.h>
00023 #include <pr2_controllers_msgs/SingleJointPositionAction.h>
00024 #include <pr2_controllers_msgs/PointHeadAction.h>
00025 #include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
00026 #include <std_msgs/String.h>
00027 #include <kinematics_msgs/GetKinematicSolverInfo.h>
00028 #include <kinematics_msgs/GetPositionFK.h>
00029 #include <kinematics_msgs/GetConstraintAwarePositionIK.h>
00030 #include <arm_navigation_msgs/SetPlanningSceneDiff.h>
00031 #include <arm_navigation_msgs/CollisionOperation.h>
00032 #include <arm_navigation_msgs/MoveArmAction.h>
00033 #include <arm_navigation_msgs/utils.h>
00034 #include <arm_navigation_msgs/MoveArmGoal.h>
00035 #include <pr2_controllers_msgs/JointTrajectoryGoal.h>
00036 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00037 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00038 #include <pr2_msgs/PressureState.h>
00039 #include <pr2_msgs/LaserScannerSignal.h>
00040 #include <laser_assembler/AssembleScans.h>
00041 #include <move_base_msgs/MoveBaseAction.h>
00042 #include <tabletop_collision_map_processing/TabletopCollisionMapProcessing.h>
00043 #include <object_manipulation_msgs/PickupAction.h>
00044 #include <object_manipulation_msgs/PlaceAction.h>
00045
00046
00047
00048 #include <pcl_ros/filters/passthrough.h>
00049 #include <pcl/segmentation/sac_segmentation.h>
00050 #include <pcl/sample_consensus/method_types.h>
00051 #include <pcl/sample_consensus/model_types.h>
00052 #include <pcl/surface/convex_hull.h>
00053 #include <pcl/filters/extract_indices.h>
00054 #include <pcl/filters/voxel_grid.h>
00055 #include <pcl_ros/transforms.h>
00056 #include <pcl/filters/statistical_outlier_removal.h>
00057
00058
00059 #include <tf/transform_listener.h>
00060
00061
00062 #include <string.h>
00063 #include <math.h>
00064 #include <vector>
00065 #include <iostream>
00066 #include <fstream>
00067 #include <stdio.h>
00068
00069
00070 #include <visualization_msgs/Marker.h>
00071 #include <visualization_msgs/MarkerArray.h>
00072
00073 struct arm_config_7DOF
00074 {
00075 double angles[7];
00076 };
00077 struct DragActionTemplate
00078 {
00079 arm_config_7DOF q_0;
00080 arm_config_7DOF q_1;
00081 arm_config_7DOF q_2;
00082 double dist;
00083 };
00084
00085 static const std::string COLLISION_PROCESSING_SERVICE_NAME = "/tabletop_collision_map_processing/tabletop_collision_map_processing";
00086 static const std::string PICKUP_ACTION_NAME = "/object_manipulator/object_manipulator_pickup";
00087 static const std::string PLACE_ACTION_NAME = "/object_manipulator/object_manipulator_place";
00088 static const std::string TORSO_LIFT_LINK_STR = "torso_lift_link";
00089 static const std::string R_WRIST_ROLL_LINK_STR = "r_wrist_roll_link";
00090 static const std::string RIGHT_ARM_IK_NAME = "pr2_right_arm_kinematics/get_constraint_aware_ik";
00091 static const std::string RIGHT_ARM_FK_NAME = "pr2_right_arm_kinematics/get_fk";
00092 static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
00093 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction> TrajClient;
00094 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction> PointHeadClient;
00095 typedef pcl::PointCloud<pcl::PointXYZ> XYZPointCloud;
00096 typedef pcl::PointXYZ Point;
00097 typedef pcl::KdTree<Point>::Ptr KdTreePtr;
00098 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::SingleJointPositionAction> TorsoClient;
00099 typedef actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> ArmActionClient;
00100 typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
00101 typedef actionlib::SimpleActionClient<object_manipulation_msgs::PickupAction> PickupClient;
00102 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> GripperClient;
00103
00104 class book_stacking
00105 {
00106
00107 private:
00108 ros::NodeHandle root_handle_;
00109 ros::ServiceClient ik_client_right;
00110 ros::ServiceClient fk_client_right;
00111 ros::ServiceClient laser_assembler_client;
00112 ros::ServiceClient collision_processing_srv;
00113 ros::Subscriber point_cloud_sub_;
00114 ros::Subscriber r_fingertip_sub_;
00115 ros::Subscriber l_fingertip_sub_;
00116 ros::Subscriber tilt_timing_sub_;
00117 ros::Publisher tilting_pt_cloud_pub_;
00118 PointHeadClient* point_head_client_;
00119 TorsoClient *torso_client_;
00120 TrajClient *traj_right_arm_client_;
00121 ArmActionClient *move_right_arm_client_;
00122 ArmActionClient *move_left_arm_client_;
00123 MoveBaseClient *move_base_client_;
00124 MoveBaseClient *omnix_move_base_client_;
00125 PickupClient *pickup_client_;
00126 GripperClient *right_gripper_client_;
00127 GripperClient *left_gripper_client_;
00128 tf::TransformListener tf_listener;
00129
00130 int filter_outliers_meank;
00131 int min_plane_inliers_;
00132 int max_sac_iterations_;
00133 int max_planes_;
00134 bool filter_outliers;
00135 bool downsample_cloud;
00136 bool concave_hull_mode_;
00137 bool use_normal_seg_;
00138 bool use_omp_;
00139 bool filter_spatial;
00140 double plane_distance_thresh_;
00141 double normal_search_radius_;
00142 double downsample_grid_size_;
00143 double sac_probability_;
00144 double filter_outliers_stddev_thresh;
00145 double filter_spatial_xmin, filter_spatial_xmax;
00146 double filter_spatial_ymin, filter_spatial_ymax;
00147 double filter_spatial_zmin, filter_spatial_zmax;
00148 double table_obj_detector_upper_z,table_obj_detector_lower_z;
00149 double optimal_workspace_wrt_torso_x, optimal_workspace_wrt_torso_y, optimal_workspace_wrt_torso_z;
00150 double optimal_workspace_wrt_torso_x_grasping, optimal_workspace_wrt_torso_y_grasping, optimal_workspace_wrt_torso_z_grasping;
00151 double init_torso_position;
00152 double predrag_dist;
00153 double pregrasp_dist;
00154 double preplace_dist;
00155 double pregrasp_dist_vertical;
00156 double diff_drag_force;
00157
00158 kinematics_msgs::GetKinematicSolverInfo::Response ik_solver_info;
00159 kinematics_msgs::GetKinematicSolverInfo::Response fk_solver_info;
00160 pr2_controllers_msgs::JointTrajectoryGoal rightArmHomingTrajectory;
00161 std::vector<short int> right_arm_l_finger_tip_nominals;
00162 std::vector<short int> right_arm_r_finger_tip_nominals;
00163 std::string workspace_frame;
00164 std::string base_frame_tf;
00165 bool left_arm_fingertips_sensing;
00166 bool right_arm_fingertips_sensing;
00167 bool right_arm_fingertips_contacted;
00168 bool right_arm_fingertips_nominals;
00169 bool right_arm_end_eff_goal_resulted;
00170 bool robot_initialized;
00171 bool latest_pt_cloud_ready;
00172 bool test_arms;
00173 bool enable_nav;
00174 bool got_init_table;
00175 XYZPointCloud latest_cloud;
00176 double tilt_period;
00177 double FINGERTIP_CONTACT_THRESHOLD;
00178 double nav_waypoint_offset;
00179 pr2_msgs::LaserScannerSignal tilt_minima_msg, tilt_maxima_msg;
00180 std::ofstream logFile;
00181
00182 book_stacking_msgs::PlaneInfo init_table_plane_info;
00183 pcl::PointCloud<Point> init_table_hull;
00184 geometry_msgs::Point32 init_table_centroid;
00185 pcl::PointCloud<Point> nav_waypoints;
00186 book_stacking_msgs::NavWaypoints nav_waypoint_goals;
00187 book_stacking_msgs::ObjectInfos all_objects_wrt_odom_combined;
00188 std::vector<double> all_objects_x_wrt_odom_combined;
00189 std::vector<double> all_objects_y_wrt_odom_combined;
00190 std::vector<double> all_objects_z_wrt_odom_combined;
00191 double grasp_offset_from_com;
00192 public:
00193 ros::NodeHandle n_;
00194 ros::Publisher filtered_cloud_pub_;
00195 ros::Publisher plane_marker_pub_;
00196 ros::Publisher obj_marker_pub_;
00197 ros::Publisher vis_pub_;
00198 ros::Subscriber command_subscriber_;
00199
00200
00201 book_stacking():
00202 n_("~")
00203 {
00204 robot_initialized=false;
00205 LoadParameters();
00206 InitializeRobot();
00207
00208 if(test_arms)
00209 {
00210 TestArm();
00211 HomeRightArm();
00212 }
00213
00214 robot_initialized=true;
00215
00216
00217
00218 }
00219
00220 void commandCallback (const std_msgs::String::ConstPtr& msg)
00221 {
00222 bool stop_when_contact=true;
00223 pr2_controllers_msgs::JointTrajectoryGoal output_traj;
00224 double path_distance,dt;
00225 double speed=0.15;
00226 arm_config_7DOF q_solution;
00227 std::cout<<"Command: "<<msg->data<<std::endl;
00228 char key0 = msg->data.c_str()[0]; kinematics_msgs::GetKinematicSolverInfo::Response response;
00229 std::vector<arm_config_7DOF> qs;
00230 std::vector<double> ts;
00231 move_base_msgs::MoveBaseGoal goal;
00232 arm_config_7DOF q_current;
00233 double odom_x,odom_y,yaw;
00234
00235 switch (key0)
00236 {
00237 case 'b':
00238
00239 if(msg->data.length()>2)
00240 {
00241 char key2 = msg->data.c_str()[2];
00242 switch (key2)
00243 {
00244 case 'a':
00245 TestArm();
00246 break;
00247 case 'b':
00248 HomeRightArm();
00249 break;
00250 case 'c':
00251 got_init_table=getInitTable(false);
00252 break;
00253 case 'd':
00254 plan0();
00255 break;
00256 case 'e':
00257 HomeRightArm();
00258 break;
00259 case 'f':
00260 ExploreLinearMoveAction(0.0,0.15,0.0,GetCurrentRightArmJointAngles(),true,path_distance,q_solution);
00261 dt=path_distance/speed;
00262 qs.push_back(q_solution);
00263 ts.push_back(dt);
00264 output_traj=createRightArmTrajectoryFromAngles(qs,ts);
00265 SendRightEndEffectorTrajectory(output_traj,stop_when_contact);
00266 break;
00267 case 'g':
00268 ExploreLinearMoveAction(0.0,-0.15,0.0,GetCurrentRightArmJointAngles(),true,path_distance,q_solution);
00269 dt=path_distance/speed;
00270 qs.push_back(q_solution);
00271 ts.push_back(dt);
00272 output_traj=createRightArmTrajectoryFromAngles(qs,ts);
00273 SendRightEndEffectorTrajectory(output_traj,stop_when_contact);
00274 break;
00275 case 'h':
00276 ExploreLinearMoveAction(0.15,0.0,0.0,GetCurrentRightArmJointAngles(),true,path_distance,q_solution);
00277 dt=path_distance/speed;
00278 qs.push_back(q_solution);
00279 ts.push_back(dt);
00280 output_traj=createRightArmTrajectoryFromAngles(qs,ts);
00281 SendRightEndEffectorTrajectory(output_traj,stop_when_contact);
00282 break;
00283 case 'i':
00284 ExploreLinearMoveAction(-0.15,0.0,0.0,GetCurrentRightArmJointAngles(),true,path_distance,q_solution);
00285 dt=path_distance/speed;
00286 qs.push_back(q_solution);
00287 ts.push_back(dt);
00288 output_traj=createRightArmTrajectoryFromAngles(qs,ts);
00289 SendRightEndEffectorTrajectory(output_traj,stop_when_contact);
00290 break;
00291 case 'j':
00292 ExploreLinearMoveAction(0.0,0.0,0.15,GetCurrentRightArmJointAngles(),true,path_distance,q_solution);
00293 dt=path_distance/speed;
00294 qs.push_back(q_solution);
00295 ts.push_back(dt);
00296 output_traj=createRightArmTrajectoryFromAngles(qs,ts);
00297 SendRightEndEffectorTrajectory(output_traj,stop_when_contact);
00298 break;
00299 case 'k':
00300 ExploreLinearMoveAction(0.0,0.0,-0.15,GetCurrentRightArmJointAngles(),true,path_distance,q_solution);
00301 dt=path_distance/speed;
00302 qs.push_back(q_solution);
00303 ts.push_back(dt);
00304 output_traj=createRightArmTrajectoryFromAngles(qs,ts);
00305 SendRightEndEffectorTrajectory(output_traj,stop_when_contact);
00306 break;
00307 case 'l':
00308 goal.target_pose.header.frame_id="base_link";
00309 goal.target_pose.pose.position.x = 0.25;
00310 goal.target_pose.pose.position.y = 0.0;
00311 goal.target_pose.pose.position.z = 0.0;
00312 goal.target_pose.pose.orientation.x = 0.0;
00313 goal.target_pose.pose.orientation.y = 0.0;
00314 goal.target_pose.pose.orientation.z = 0.0;
00315 goal.target_pose.pose.orientation.w = 1.0;
00316 goal.target_pose.header.stamp=ros::Time::now();
00317 OmnixMoveBasePosition(goal);
00318 break;
00319 case 'm':
00320 goal.target_pose.header.frame_id="base_link";
00321 goal.target_pose.pose.position.x = -0.25;
00322 goal.target_pose.pose.position.y = 0.0;
00323 goal.target_pose.pose.position.z = 0.0;
00324 goal.target_pose.pose.orientation.x = 0.0;
00325 goal.target_pose.pose.orientation.y = 0.0;
00326 goal.target_pose.pose.orientation.z = 0.0;
00327 goal.target_pose.pose.orientation.w = 1.0;
00328 goal.target_pose.header.stamp=ros::Time::now();
00329 OmnixMoveBasePosition(goal);
00330 break;
00331 case 'n':
00332 goal.target_pose.header.frame_id="base_link";
00333 goal.target_pose.pose.position.x = 0.0;
00334 goal.target_pose.pose.position.y = 0.25;
00335 goal.target_pose.pose.position.z = 0.0;
00336 goal.target_pose.pose.orientation.x = 0.0;
00337 goal.target_pose.pose.orientation.y = 0.0;
00338 goal.target_pose.pose.orientation.z = 0.0;
00339 goal.target_pose.pose.orientation.w = 1.0;
00340 goal.target_pose.header.stamp=ros::Time::now();
00341 OmnixMoveBasePosition(goal);
00342 break;
00343 case 'o':
00344 goal.target_pose.header.frame_id="base_link";
00345 goal.target_pose.pose.position.x = 0.0;
00346 goal.target_pose.pose.position.y = -0.25;
00347 goal.target_pose.pose.position.z = 0.0;
00348 goal.target_pose.pose.orientation.x = 0.0;
00349 goal.target_pose.pose.orientation.y = 0.0;
00350 goal.target_pose.pose.orientation.z = 0.0;
00351 goal.target_pose.pose.orientation.w = 1.0;
00352 goal.target_pose.header.stamp=ros::Time::now();
00353 OmnixMoveBasePosition(goal);
00354 break;
00355 case 'p':
00356 log_IK();
00357 break;
00358 case 'r':
00359 q_current=GetCurrentRightArmJointAngles();
00360 for(unsigned int i=0;i<7;i++)
00361 {
00362 std::cout<<"angles["<<i<<"]: "<<q_current.angles[i]<<std::endl;
00363 }
00364 case 's':
00365 getOdomPose(odom_x,odom_y,yaw,ros::Time::now());
00366 break;
00367 case 'q':
00368 OpenRightGripper();
00369 break;
00370 case 't':
00371 CloseRightGripper();
00372 break;
00373 case 'u':
00374 OpenLeftGripper();
00375 break;
00376 case 'w':
00377 CloseLeftGripper();
00378 break;
00379 case 'v':
00380 if(checkIfRightGripperHoldsObject())
00381 std::cout<<"HAND FULL"<<std::endl;
00382 else
00383 std::cout<<"HAND EMPTY"<<std::endl;
00384 break;
00385 case'y':
00386 GetTiltingPointCloud(true);
00387 break;
00388 default:
00389 break;
00390 }
00391 }
00392 default:
00393 break;
00394 }
00395 }
00396
00397
00398
00399 void drawHull(pcl::PointCloud<Point> hull_pts,const ros::Publisher& plane_pub, float r, float g, float b)
00400 {
00401 visualization_msgs::MarkerArray markers;
00402 visualization_msgs::Marker marker;
00403 marker.header.frame_id="odom_combined";
00404 marker.header.stamp=ros::Time::now();
00405 marker.ns = "nav_waypoints";
00406 marker.id =5;
00407 marker.type = visualization_msgs::Marker::LINE_STRIP;
00408 marker.action = visualization_msgs::Marker::ADD;
00409 marker.color.r = r;
00410 marker.color.g = g;
00411 marker.color.b = b;
00412 marker.color.a = 0.5;
00413 marker.scale.x = 0.05;
00414 marker.scale.y = 0.05;
00415 marker.scale.z = 0.05;
00416 marker.pose.position.x = 0.0;
00417 marker.pose.position.y = 0.0;
00418 marker.pose.position.z = 0.0;
00419 marker.pose.orientation.x = 0.0;
00420 marker.pose.orientation.y = 0.0;
00421 marker.pose.orientation.z = 0.0;
00422 marker.pose.orientation.w = 1.0;
00423 marker.lifetime = ros::Duration(60.0*30.0);
00424
00425 for(size_t j = 0; j < hull_pts.points.size(); j++)
00426 {
00427 geometry_msgs::Point pt;
00428 pt.x = hull_pts.points[j].x;
00429 pt.y = hull_pts.points[j].y;
00430 pt.z = hull_pts.points[j].z;
00431 marker.points.push_back(pt);
00432 }
00433
00434 geometry_msgs::Point pt;
00435 pt.x = hull_pts.points[0].x;
00436 pt.y = hull_pts.points[0].y;
00437 pt.z = hull_pts.points[0].z;
00438 marker.points.push_back(pt);
00439
00440 markers.markers.push_back(marker);
00441 plane_pub.publish(markers);
00442 return;
00443 }
00444
00445 bool getInitTable(bool refreshScan)
00446 {
00447 ros::Rate loop_rate(10);
00448 latest_pt_cloud_ready=false;
00449 GetTiltingPointCloud(refreshScan);
00450 while (!latest_pt_cloud_ready)
00451 {
00452 ros::spinOnce();
00453 loop_rate.sleep();
00454 }
00455 if(getTablePlane(latest_cloud,init_table_plane_info,true))
00456 {
00457 std::cout<<"frame_id: "<<init_table_plane_info.header.frame_id<<std::endl;
00458 std::cout<<"hull frame_id: "<<init_table_plane_info.hull.header.frame_id<<std::endl;
00459
00460 XYZPointCloud table_hull_wrt_base_link;
00461 pcl::fromROSMsg(init_table_plane_info.hull,table_hull_wrt_base_link);
00462 tf::StampedTransform transf;
00463 try{
00464 tf_listener.waitForTransform("odom_combined", init_table_plane_info.header.frame_id,
00465 init_table_plane_info.hull.header.stamp, ros::Duration(2.0));
00466 tf_listener.lookupTransform("odom_combined", init_table_plane_info.header.frame_id,
00467 init_table_plane_info.hull.header.stamp, transf);
00468 }
00469 catch(tf::TransformException ex)
00470 {
00471 ROS_ERROR("getInitTable TF error:%s", ex.what());
00472 return false;
00473 }
00474 tf::Vector3 v3 = transf.getOrigin();
00475 tf::Quaternion quat = transf.getRotation();
00476 Eigen::Quaternionf rot(quat.w(), quat.x(), quat.y(), quat.z());
00477 Eigen::Vector3f offset(v3.x(), v3.y(), v3.z());
00478 pcl::transformPointCloud(table_hull_wrt_base_link,init_table_hull,offset,rot);
00479 init_table_hull.header = table_hull_wrt_base_link.header;
00480 init_table_hull.header.frame_id = "odom_combined";
00481
00482 init_table_centroid = calcCentroid(init_table_hull);
00483 std::cout<<"CENTROID | x: "<<init_table_centroid.x<<" y: "<<init_table_centroid.y<< " z: "<<init_table_centroid.z<<std::endl;
00484 nav_waypoints.clear();
00485 for(unsigned int i=0;i<init_table_hull.points.size();i++)
00486 {
00487 geometry_msgs::Point32 p;
00488 p.x = init_table_hull.points[i].x;
00489 p.y = init_table_hull.points[i].y;
00490 p.z = init_table_hull.points[i].z;
00491
00492 geometry_msgs::Point32 v;
00493 v.x=p.x-init_table_centroid.x;
00494 v.y=p.y-init_table_centroid.y;
00495 double angle=atan2(v.y,v.x);
00496 Point p_new;
00497 p_new.x=p.x+nav_waypoint_offset*cos(angle);
00498 p_new.y=p.y+nav_waypoint_offset*sin(angle);
00499 p_new.z=0.0;
00500 nav_waypoints.push_back(p_new);
00501
00502
00503
00504 move_base_msgs::MoveBaseGoal goal;
00505 goal.target_pose.header.frame_id="odom_combined";
00506 goal.target_pose.pose.position.x = p_new.x;
00507 goal.target_pose.pose.position.y = p_new.y;
00508 goal.target_pose.pose.position.z = 0.0;
00509 geometry_msgs::Quaternion quat=tf::createQuaternionMsgFromYaw(atan2(-v.y,-v.x));
00510 goal.target_pose.pose.orientation = quat;
00511 goal.target_pose.header.stamp=ros::Time::now();
00512 nav_waypoint_goals.nav_goals.push_back(goal);
00513
00514
00515
00516
00517
00518
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539 }
00540 drawHull(nav_waypoints,plane_marker_pub_,0.0,0.0,1.0);
00541
00542 std::cout<<"NAV_WAYPOInts size: "<<nav_waypoints.size()<<std::endl;
00543 return true;
00544 }
00545 else
00546 {
00547 return false;
00548 }
00549 }
00550
00551
00552
00553 double calculateHowMuchToPush (book_stacking_msgs::ObjectInfo objInfo)
00554 {
00555
00556 return 0.1;
00557 }
00558
00559 double getEuclidianDist(double x1, double y1, double x2, double y2)
00560 {
00561 double result=sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2));
00562
00563 return result;
00564 }
00565
00566 void ExecuteNavWaypoints(book_stacking_msgs::NavWaypoints goals)
00567 {
00568 for(unsigned int i=0;i<goals.nav_goals.size();i++)
00569 { std::cout<<"-Goal #"<<i<<std::endl;
00570 std::cout<<"x: "<<goals.nav_goals[i].target_pose.pose.position.x<<std::endl;
00571 std::cout<<"y: "<<goals.nav_goals[i].target_pose.pose.position.y<<std::endl;
00572 OmnixMoveBasePosition(goals.nav_goals[i]);
00573 }
00574 }
00575
00576 void TestCollision()
00577 {
00578
00579 std::cout<<"init_table_hull.points.size(): "<<init_table_hull.points.size()<<std::endl;
00580 std::cout<<"nav_waypoints.points.size(): "<<nav_waypoints.points.size()<<std::endl;
00581 Point p_new;
00582 p_new.x=init_table_centroid.x;
00583 p_new.y=init_table_centroid.y;
00584 p_new.z=0.0;
00585
00586 if(pcl::isPointIn2DPolygon(p_new,init_table_hull))
00587 {
00588 std::cout<<"IN POLYGON"<<std::endl;
00589 }
00590 else
00591 {
00592 std::cout<<"NOT IN POLYGON"<<std::endl;
00593 }
00594 return;
00595 }
00596
00597 double DetermineDragDistance(book_stacking_msgs::ObjectInfo objInfo)
00598 {
00599 if(!got_init_table)
00600 {
00601 ROS_WARN("No initial table!");
00602 return -1.0;
00603 }
00604
00605 geometry_msgs::PointStamped obj_centroid_wrt_odom_combined;
00606 if(objInfo.header.frame_id.compare("odom_combined") == 0)
00607 {
00608
00609
00610 obj_centroid_wrt_odom_combined.point.x=objInfo.centroid.x;
00611 obj_centroid_wrt_odom_combined.point.y=objInfo.centroid.y;
00612 obj_centroid_wrt_odom_combined.point.z=objInfo.centroid.z;
00613 }
00614 else
00615 {
00616
00617 geometry_msgs::PointStamped input_point;
00618 input_point.header.frame_id=objInfo.header.frame_id;
00619 input_point.point.x=objInfo.centroid.x;
00620 input_point.point.y=objInfo.centroid.y;
00621 input_point.point.z=objInfo.centroid.z;
00622 tf_listener.transformPoint("odom_combined", input_point, obj_centroid_wrt_odom_combined);
00623 }
00624
00625 double odom_x,odom_y,yaw;
00626 getOdomPose(odom_x,odom_y,yaw,ros::Time::now());
00627
00628 Point p_robot(odom_x,odom_y,0.0);
00629 Point p_obj(obj_centroid_wrt_odom_combined.point.x,obj_centroid_wrt_odom_combined.point.y,0.0);
00630 double d_robot_obj=getEuclidianDist(p_robot.x,p_robot.y,p_obj.x,p_obj.y);
00631 double res=0.01;
00632
00633 geometry_msgs::Point32 v;
00634 v.x=p_robot.x-p_obj.x;
00635 v.y=p_robot.y-p_obj.y;
00636
00637 double angle=yaw+M_PI;
00638
00639
00640
00641
00642
00643
00644 for(double d=0.0;d<d_robot_obj;d=d+res)
00645 {
00646 Point p_new;
00647 p_new.x=p_obj.x+d*cos(angle);
00648 p_new.y=p_obj.y+d*sin(angle);
00649 p_new.z=0.0;
00650
00651
00652
00653
00654 if(!pcl::isPointIn2DPolygon(p_new,init_table_hull))
00655 {
00656
00657 return d;
00658 }
00659 else
00660 {
00661
00662 }
00663
00664 }
00665
00666 return -1.0;
00667 }
00668
00669 bool NavigateToObject(book_stacking_msgs::ObjectInfo objInfo)
00670 {
00671 geometry_msgs::PointStamped input_point;
00672 geometry_msgs::PointStamped obj_centroid_wrt_odom_combined;
00673 input_point.header.frame_id=objInfo.header.frame_id;
00674 input_point.point.x=objInfo.centroid.x;
00675 input_point.point.y=objInfo.centroid.y;
00676 input_point.point.z=objInfo.centroid.z;
00677 tf_listener.transformPoint("odom_combined", input_point, obj_centroid_wrt_odom_combined);
00678
00679 double odom_x,odom_y,yaw;
00680 getOdomPose(odom_x,odom_y,yaw,ros::Time::now());
00681
00682 double d_robot_min=999.0;
00683 double d_object_min=999.0;
00684 unsigned int min_robot_index=0;
00685 unsigned int min_object_index=0;
00686
00687 for(unsigned int i=0;i<nav_waypoint_goals.nav_goals.size();i++)
00688 {
00689 double d_robot=getEuclidianDist(nav_waypoint_goals.nav_goals[i].target_pose.pose.position.x, nav_waypoint_goals.nav_goals[i].target_pose.pose.position.y, odom_x,odom_y);
00690 double d_object=getEuclidianDist(nav_waypoint_goals.nav_goals[i].target_pose.pose.position.x, nav_waypoint_goals.nav_goals[i].target_pose.pose.position.y ,obj_centroid_wrt_odom_combined.point.x, obj_centroid_wrt_odom_combined.point.y);
00691 if(d_robot<d_robot_min)
00692 {
00693 d_robot_min=d_robot;
00694 min_robot_index=i;
00695 }
00696 if(d_object<d_object_min)
00697 {
00698 d_object_min=d_object;
00699 min_object_index=i;
00700 }
00701 }
00702
00703 std::cout<<"Closest_robot| x: "<<nav_waypoint_goals.nav_goals[min_robot_index].target_pose.pose.position.x <<
00704 " y: "<< nav_waypoint_goals.nav_goals[min_robot_index].target_pose.pose.position.y <<std::endl;
00705 std::cout<<"min_robot_index: "<<min_robot_index<<std::endl;
00706
00707 std::cout<<"Closest_object| x: "<<nav_waypoint_goals.nav_goals[min_object_index].target_pose.pose.position.x <<
00708 " y: "<< nav_waypoint_goals.nav_goals[min_object_index].target_pose.pose.position.y <<std::endl;
00709 std::cout<<"min_object_index: "<<min_object_index<<std::endl;
00710
00711 if(min_robot_index==min_object_index)
00712 return true;
00713
00714
00715 book_stacking_msgs::NavWaypoints clockwise_goals;
00716 book_stacking_msgs::NavWaypoints c_clockwise_goals;
00717
00718 int waypoint_size=nav_waypoint_goals.nav_goals.size();
00719 std::cout<<"Waypoint_size: "<<waypoint_size<<std::endl;
00720
00721 bool clockwise_found=false;
00722 bool c_clockwise_found=false;
00723 int clockwise_index;
00724 int c_clockwise_index;
00725 for(int i=0;i<waypoint_size;i++)
00726 {
00727
00728 if(!clockwise_found)
00729 {
00730 clockwise_index=min_robot_index+i;
00731 if(clockwise_index>=waypoint_size)
00732 {
00733 clockwise_index=clockwise_index-waypoint_size;
00734 }
00735 else if(clockwise_index<0)
00736 {
00737 clockwise_index=clockwise_index+waypoint_size;
00738 }
00739 clockwise_goals.nav_goals.push_back(nav_waypoint_goals.nav_goals[clockwise_index]);
00740 if(clockwise_index==min_object_index)
00741 {
00742 clockwise_found=true;
00743 }
00744 }
00745
00746 if(!c_clockwise_found)
00747 {
00748 c_clockwise_index=min_robot_index-i;
00749 if(c_clockwise_index>=waypoint_size)
00750 {
00751 c_clockwise_index=c_clockwise_index-waypoint_size;
00752 }
00753 else if(c_clockwise_index<0)
00754 {
00755 c_clockwise_index=c_clockwise_index+waypoint_size;
00756 }
00757 c_clockwise_goals.nav_goals.push_back(nav_waypoint_goals.nav_goals[c_clockwise_index]);
00758 if(c_clockwise_index==min_object_index)
00759 {
00760 c_clockwise_found=true;
00761 }
00762 }
00763
00764
00765
00766
00767
00768 if(clockwise_found && c_clockwise_found)
00769 break;
00770 }
00771
00772 std::cout<<"clockwise_goals size: "<<clockwise_goals.nav_goals.size()<<std::endl;
00773 std::cout<<"c_clockwise_goals size: "<<c_clockwise_goals.nav_goals.size()<<std::endl;
00774
00775 if(clockwise_goals.nav_goals.size()<c_clockwise_goals.nav_goals.size())
00776 {
00777 ExecuteNavWaypoints(clockwise_goals);
00778 }
00779 else
00780 {
00781 ExecuteNavWaypoints(c_clockwise_goals);
00782 }
00783 return true;
00784 }
00785
00786 bool getOdomPose(double &x,double &y,double &yaw, const ros::Time& t)
00787 {
00788 tf_listener.waitForTransform("odom_combined", "base_link", ros::Time::now(), ros::Duration(2.0));
00789
00790
00791 tf::Stamped<tf::Pose> ident (btTransform(tf::createQuaternionFromRPY(0,0,0),
00792 btVector3(0,0,0)), t, "base_link");
00793 tf::Stamped<btTransform> odom_pose;
00794 try
00795 {
00796 tf_listener.transformPose("odom_combined", ident, odom_pose);
00797 }
00798 catch(tf::TransformException e)
00799 {
00800 ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
00801 return false;
00802 }
00803 yaw = tf::getYaw(odom_pose.getRotation());
00804 x=odom_pose.getOrigin().x();
00805 y=odom_pose.getOrigin().y();
00806
00807
00808 return true;
00809 }
00810
00811
00812 unsigned int ScanAndGetTableAndObjects(book_stacking_msgs::PlaneInfo &latest_table_plane_info, book_stacking_msgs::ObjectInfos &allObjectInfos,bool refreshScan)
00813 {
00814 ros::Rate rate10(10);
00815 while(true)
00816 {
00817 ros::spinOnce();
00818 latest_pt_cloud_ready=false;
00819 GetTiltingPointCloud(refreshScan);
00820 while (!latest_pt_cloud_ready)
00821 {
00822 ros::spinOnce();
00823 rate10.sleep();
00824 }
00825 bool got_current_table=getTablePlane(latest_cloud,latest_table_plane_info,false);
00826 if(got_current_table)
00827 break;
00828 }
00829 allObjectInfos = getObjectsOverPlane(latest_table_plane_info, latest_cloud, table_obj_detector_lower_z, table_obj_detector_upper_z);
00830 #ifdef DEBUG_DRAW_TABLETOP_OBJECTS
00831 drawObjectPrisms(allObjectInfos,obj_marker_pub_,latest_table_plane_info,0.0f,1.0f,0.0f);
00832 #endif
00833 ROS_INFO("# OF OBJS: %d",(int)(allObjectInfos.objects.size()));
00834 return (allObjectInfos.objects.size());
00835 }
00836
00837 void SaveAllObjectsInOdomCombined(book_stacking_msgs::ObjectInfos allObjectInfos)
00838 {
00839 all_objects_x_wrt_odom_combined.clear();
00840 all_objects_y_wrt_odom_combined.clear();
00841 all_objects_z_wrt_odom_combined.clear();
00842 for(unsigned int i=0; i<allObjectInfos.objects.size();i++)
00843 {
00844 int cloud_size=allObjectInfos.objects[i].cloud.data.size();
00845 std::cout<<"OBJ "<<i<<" : "<<cloud_size<<" pts"<<std::endl;
00846
00847 geometry_msgs::PointStamped input_point;
00848 geometry_msgs::PointStamped obj_centroid_wrt_odom_combined;
00849 input_point.header.frame_id=allObjectInfos.objects[i].header.frame_id;
00850 input_point.point.x=allObjectInfos.objects[i].centroid.x;
00851 input_point.point.y=allObjectInfos.objects[i].centroid.y;
00852 input_point.point.z=allObjectInfos.objects[i].centroid.z;
00853 tf_listener.transformPoint("odom_combined", input_point, obj_centroid_wrt_odom_combined);
00854 all_objects_x_wrt_odom_combined.push_back(obj_centroid_wrt_odom_combined.point.x);
00855 all_objects_y_wrt_odom_combined.push_back(obj_centroid_wrt_odom_combined.point.y);
00856 all_objects_z_wrt_odom_combined.push_back(obj_centroid_wrt_odom_combined.point.z);
00857 }
00858
00859 return;
00860 }
00861
00862
00863
00864 bool TrackObject(int object_index, book_stacking_msgs::ObjectInfos allObjectInfos, double max_matching_dist)
00865 {
00866 int min_ind=99;
00867 double min_dist=999999.9;
00868 double bestx=0.0;
00869 double besty=0.0;
00870 double bestz=0.0;
00871 for(int j=0;j<allObjectInfos.objects.size();j++)
00872 {
00873 book_stacking_msgs::ObjectInfo objInfo=allObjectInfos.objects[j];
00874 geometry_msgs::PointStamped obj_centroid_wrt_odom_combined;
00875 if(objInfo.header.frame_id.compare("odom_combined") == 0)
00876 {
00877 obj_centroid_wrt_odom_combined.point.x=objInfo.centroid.x;
00878 obj_centroid_wrt_odom_combined.point.y=objInfo.centroid.y;
00879 obj_centroid_wrt_odom_combined.point.z=objInfo.centroid.z;
00880 }
00881 else
00882 {
00883 geometry_msgs::PointStamped input_point;
00884 input_point.header.frame_id=objInfo.header.frame_id;
00885 input_point.point.x=objInfo.centroid.x;
00886 input_point.point.y=objInfo.centroid.y;
00887 input_point.point.z=objInfo.centroid.z;
00888 tf_listener.transformPoint("odom_combined", input_point, obj_centroid_wrt_odom_combined);
00889 }
00890 double dist=getEuclidianDist(obj_centroid_wrt_odom_combined.point.x, obj_centroid_wrt_odom_combined.point.y, all_objects_x_wrt_odom_combined[object_index], all_objects_y_wrt_odom_combined[object_index]);
00891 if(dist<min_dist)
00892 {
00893 min_dist=dist;
00894 min_ind=j;
00895 bestx=obj_centroid_wrt_odom_combined.point.x;
00896 besty=obj_centroid_wrt_odom_combined.point.y;
00897 bestz=obj_centroid_wrt_odom_combined.point.z;
00898 }
00899 }
00900 if(min_dist<max_matching_dist)
00901 {
00902 all_objects_x_wrt_odom_combined[object_index]=bestx;
00903 all_objects_y_wrt_odom_combined[object_index]=besty;
00904 all_objects_z_wrt_odom_combined[object_index]=bestz;
00905 return true;
00906 }
00907 return false;
00908 }
00909
00910 bool PickUpBook(int object_index)
00911 {
00912 book_stacking_msgs::PlaneInfo latest_table_plane_info;
00913 book_stacking_msgs::ObjectInfos allObjectInfos;
00914
00915 bool drag_overall_success=false;
00916 while(!drag_overall_success)
00917 {
00918 book_stacking_msgs::ObjectInfo book_info;
00919 book_info.header.frame_id="odom_combined";
00920 book_info.centroid.x=all_objects_x_wrt_odom_combined[object_index];
00921 book_info.centroid.y=all_objects_y_wrt_odom_combined[object_index];
00922 book_info.centroid.z=all_objects_z_wrt_odom_combined[object_index];
00923
00924 if(enable_nav)
00925 {
00926 NavigateToObject(book_info);
00927
00928 }
00929
00930 double drag_dist=DetermineDragDistance(book_info);
00931 std::cout<<"drag_dist: "<<drag_dist<<std::endl;
00932
00933 if(drag_dist<0.11 && drag_dist>=0.0)
00934 {
00935 std::cout<<"Book is on the edge!"<<std::endl;
00936 drag_overall_success=true;
00937 }
00938
00939
00940
00941 bool partial_success=false;
00942 if(test_arms && !drag_overall_success)
00943 {
00944 double effective_desired_dist;
00945
00946 if(drag_dist>0.055)
00947 {
00948 effective_desired_dist=drag_dist-0.055;
00949 }
00950 else
00951 {
00952 effective_desired_dist=drag_dist-0.02;
00953 }
00954
00955 book_stacking_msgs::DragRequest drag_req;
00956
00957 drag_req.object=book_info;
00958 drag_req.dir.header.frame_id="torso_lift_link";
00959 drag_req.dir.header.stamp=ros::Time::now();
00960 drag_req.dir.vector.x=-1.0;
00961 drag_req.dir.vector.y=0.0;
00962 drag_req.dir.vector.z=0.0;
00963 drag_req.dist=effective_desired_dist;
00964 double output_dist;
00965 partial_success=DragObject(drag_req,output_dist);
00966 if(partial_success)
00967 {
00968 HomeRightArm();
00969
00970 }
00971 }
00972
00973 while(!drag_overall_success)
00974 {
00975 unsigned int object_count=ScanAndGetTableAndObjects(latest_table_plane_info, allObjectInfos,true);
00976 if(object_count > 0)
00977 {
00978 double max_tracking_dist;
00979 if(partial_success)
00980 max_tracking_dist=0.5;
00981 else
00982 max_tracking_dist=0.2;
00983
00984 if(TrackObject(object_index,allObjectInfos,max_tracking_dist))
00985 break;
00986 }
00987 }
00988
00989
00990 }
00991
00992
00993
00994 bool grasp_overall_success=false;
00995 while(!grasp_overall_success)
00996 {
00997
00998 if(test_arms)
00999 {
01000 double output_dist;
01001 if(GraspObject(object_index, allObjectInfos,output_dist))
01002 {
01003 HomeRightArm();
01004 if(checkIfRightGripperHoldsObject())
01005 {
01006 grasp_overall_success=true;
01007 }
01008 }
01009
01010 if(!grasp_overall_success)
01011 {
01012 while(true)
01013 {
01014 unsigned int object_count=ScanAndGetTableAndObjects(latest_table_plane_info, allObjectInfos,true);
01015 if(object_count > 0)
01016 {
01017 double max_tracking_dist=0.22;
01018 if(TrackObject(object_index,allObjectInfos,max_tracking_dist))
01019 break;
01020 }
01021 }
01022 }
01023 }
01024
01025 }
01026
01027
01028
01029
01030
01031 return true;
01032 }
01033
01034
01035
01036 bool plan0()
01037 {
01038 got_init_table=getInitTable(false);
01039 if(!got_init_table)
01040 {
01041 ROS_WARN("No initial table!");
01042 return false;
01043 }
01044
01045 book_stacking_msgs::PlaneInfo latest_table_plane_info;
01046 book_stacking_msgs::ObjectInfos allObjectInfos;
01047 unsigned int object_count=ScanAndGetTableAndObjects(latest_table_plane_info, allObjectInfos,false);
01048 if(object_count < 1)
01049 return false;
01050
01051 SaveAllObjectsInOdomCombined(allObjectInfos);
01052
01053 int target_object_index=0;
01054
01055
01056
01057 PickUpBook(target_object_index);
01058
01059
01060
01061 double output_dist;
01062 bool place_overall_success=false;
01063 while(!place_overall_success)
01064 {
01065
01066 while(true)
01067 {
01068 unsigned int object_count= ScanAndGetTableAndObjects(latest_table_plane_info, allObjectInfos, true);
01069 if(object_count > 0)
01070 {
01071 break;
01072 }
01073 }
01074 SaveAllObjectsInOdomCombined(allObjectInfos);
01075
01076
01077 int host_object_index=getTallestObjectIndex(allObjectInfos);
01078 place_overall_success=PlaceObject(host_object_index,output_dist);
01079 }
01080
01081
01082 std::cout<<"PLAN SUCCEEDED!"<<std::endl;
01083
01084
01085
01086
01087
01088
01089 return false;
01090
01091 }
01092
01093 int getTallestObjectIndex(book_stacking_msgs::ObjectInfos allObjectInfos)
01094 {
01095 double max_height=-999.0;
01096 int max_height_index=0;
01097 for(unsigned int i=0; i<allObjectInfos.objects.size();i++)
01098 {
01099 geometry_msgs::PointStamped input_point;
01100 geometry_msgs::PointStamped obj_centroid_wrt_odom_combined;
01101 input_point.header.frame_id=allObjectInfos.objects[i].header.frame_id;
01102 input_point.point.x=allObjectInfos.objects[i].centroid.x;
01103 input_point.point.y=allObjectInfos.objects[i].centroid.y;
01104 input_point.point.z=allObjectInfos.objects[i].centroid.z;
01105 tf_listener.transformPoint("odom_combined", input_point, obj_centroid_wrt_odom_combined);
01106 double h=obj_centroid_wrt_odom_combined.point.z;
01107 if(h>max_height)
01108 {
01109 max_height=h;
01110 max_height_index=i;
01111 }
01112 }
01113 return max_height_index;
01114 }
01115
01116
01117 int getShortestObjectIndex(book_stacking_msgs::ObjectInfos allObjectInfos)
01118 {
01119 double min_height=999.0;
01120 int min_height_index=0;
01121 double diff_with_second=0.0;
01122
01123 for(unsigned int i=0; i<allObjectInfos.objects.size();i++)
01124 {
01125 geometry_msgs::PointStamped input_point;
01126 geometry_msgs::PointStamped obj_centroid_wrt_odom_combined;
01127 input_point.header.frame_id=allObjectInfos.objects[i].header.frame_id;
01128 input_point.point.x=allObjectInfos.objects[i].centroid.x;
01129 input_point.point.y=allObjectInfos.objects[i].centroid.y;
01130 input_point.point.z=allObjectInfos.objects[i].centroid.z;
01131 tf_listener.transformPoint("odom_combined", input_point, obj_centroid_wrt_odom_combined);
01132 double h=obj_centroid_wrt_odom_combined.point.z;
01133 if(h<min_height)
01134 {
01135 min_height=h;
01136 min_height_index=i;
01137 diff_with_second=min_height-h;
01138 }
01139 }
01140
01141 if(diff_with_second<0.01)
01142 {
01143 return 0;
01144 }
01145 else
01146 {
01147 return min_height_index;
01148 }
01149 }
01150
01151
01152 void HomeRightArm()
01153 {
01154 traj_right_arm_client_->sendGoal(rightArmHomingTrajectory);
01155 traj_right_arm_client_->waitForResult();
01156 return;
01157 }
01158
01159 bool MoveBasePosition(move_base_msgs::MoveBaseGoal goal)
01160 {
01161 ROS_INFO("Sending move_base goal");
01162 move_base_client_->sendGoal(goal);
01163 move_base_client_->waitForResult(ros::Duration(3.0));
01164 if(move_base_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
01165 return true;
01166 else
01167 return false;
01168 }
01169
01170
01171
01172 bool OmnixAdjustBaseForOptimalWorkspaceForGrasping(geometry_msgs::PoseStamped queried_pose_wrt_torso_lift_link)
01173 {
01174 geometry_msgs::Vector3 optimal_offset;
01175 optimal_offset.x=-optimal_workspace_wrt_torso_x_grasping+ queried_pose_wrt_torso_lift_link.pose.position.x;
01176 optimal_offset.y=-optimal_workspace_wrt_torso_y_grasping+queried_pose_wrt_torso_lift_link.pose.position.y;
01177 optimal_offset.z=-optimal_workspace_wrt_torso_z_grasping+queried_pose_wrt_torso_lift_link.pose.position.z;
01178
01179
01180
01181
01182
01183 move_base_msgs::MoveBaseGoal goal;
01184 goal.target_pose.header.frame_id=TORSO_LIFT_LINK_STR;
01185 goal.target_pose.pose.position.x = optimal_offset.x;
01186 goal.target_pose.pose.position.y = optimal_offset.y;
01187 goal.target_pose.pose.position.z = 0.0;
01188 goal.target_pose.pose.orientation.x = 0.0;
01189 goal.target_pose.pose.orientation.y = 0.0;
01190 goal.target_pose.pose.orientation.z = 0.0;
01191 goal.target_pose.pose.orientation.w = 1.0;
01192 goal.target_pose.header.stamp=ros::Time::now();
01193 OmnixMoveBasePosition(goal);
01194 return true;
01195 }
01196
01197 bool OmnixAdjustBaseForOptimalWorkspace(geometry_msgs::PoseStamped queried_pose_wrt_torso_lift_link)
01198 {
01199 geometry_msgs::Vector3 optimal_offset;
01200 optimal_offset.x=-optimal_workspace_wrt_torso_x+queried_pose_wrt_torso_lift_link.pose.position.x;
01201 optimal_offset.y=-optimal_workspace_wrt_torso_y+queried_pose_wrt_torso_lift_link.pose.position.y;
01202 optimal_offset.z=-optimal_workspace_wrt_torso_z+queried_pose_wrt_torso_lift_link.pose.position.z;
01203
01204
01205
01206
01207
01208 move_base_msgs::MoveBaseGoal goal;
01209 goal.target_pose.header.frame_id=TORSO_LIFT_LINK_STR;
01210 goal.target_pose.pose.position.x = optimal_offset.x;
01211 goal.target_pose.pose.position.y = optimal_offset.y;
01212 goal.target_pose.pose.position.z = 0.0;
01213 goal.target_pose.pose.orientation.x = 0.0;
01214 goal.target_pose.pose.orientation.y = 0.0;
01215 goal.target_pose.pose.orientation.z = 0.0;
01216 goal.target_pose.pose.orientation.w = 1.0;
01217 goal.target_pose.header.stamp=ros::Time::now();
01218 OmnixMoveBasePosition(goal);
01219 return true;
01220 }
01221
01222
01223 bool OmnixMoveBasePosition(move_base_msgs::MoveBaseGoal goal)
01224 {
01225 move_base_msgs::MoveBaseGoal output_goal;
01226 geometry_msgs::PoseStamped output_pose_stamped;
01227
01228 if (goal.target_pose.header.frame_id.compare("odom_combined") == 0)
01229 {
01230 omnix_move_base_client_->sendGoal(goal);
01231 }
01232 else
01233 {
01234
01235 tf::StampedTransform transf;
01236 try{
01237 tf_listener.waitForTransform("odom_combined", goal.target_pose.header.frame_id,
01238 goal.target_pose.header.stamp, ros::Duration(2.0));
01239 tf_listener.lookupTransform("odom_combined", goal.target_pose.header.frame_id,
01240 goal.target_pose.header.stamp, transf);
01241 }
01242 catch(tf::TransformException ex)
01243 {
01244 ROS_ERROR("can't transform to torso_lift_link:%s", ex.what());
01245 return false;
01246 }
01247
01248 tf_listener.transformPose("odom_combined", goal.target_pose, output_pose_stamped);
01249 output_goal.target_pose=output_pose_stamped;
01250
01251
01252 omnix_move_base_client_->sendGoal(output_goal);
01253 }
01254
01255 omnix_move_base_client_->waitForResult(ros::Duration(20.0));
01256 if(omnix_move_base_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
01257 {
01258 ROS_INFO("Omni move_base Goal Succeeded");
01259 return true;
01260 }
01261 else
01262 {
01263 ROS_INFO("Omni move_base Goal Aborted");
01264 return false;
01265 }
01266 }
01267
01268 void log_IK()
01269 {
01270 logFile.open("./result.txt");
01271 logFile.precision(3);
01272 arm_config_7DOF q_seed=GetExampleRightArmJointAngles();
01273 arm_config_7DOF q_solution;
01274 geometry_msgs::PoseStamped target_pose;
01275 double x_res=0.04;
01276 double y_res=0.04;
01277 double z_res=0.04;
01278 for(double ix=0.1;ix<0.9;ix=ix+x_res)
01279 {
01280 std::cout<<(double)ix<<std::endl;
01281 for(double iy=-1.0;iy<0.5;iy=iy+y_res)
01282 {
01283 for(double iz=-0.8;iz<0.5;iz=iz+z_res)
01284 {
01285 geometry_msgs::PoseStamped tpose;
01286 tpose.header.frame_id=TORSO_LIFT_LINK_STR;
01287 tpose.pose.orientation=tf::createQuaternionMsgFromRollPitchYaw(M_PI/2,M_PI/2,0.0);
01288 tpose.pose.position.x=ix;
01289 tpose.pose.position.y=iy;
01290 tpose.pose.position.z=iz;
01291 bool ik_result=run_ik(tpose,q_seed,q_solution,R_WRIST_ROLL_LINK_STR,ik_solver_info);
01292 logFile<<(double)ix<<"\t"<<(double)iy<<"\t"<<(double)iz<<"\t"<<ik_result<<"\n";
01293 }
01294 }
01295 }
01296 logFile.close();
01297 ROS_INFO("Log IK finished");
01298 }
01299
01300
01301 bool callQueryFK(kinematics_msgs::GetKinematicSolverInfo::Response &response)
01302 {
01303 ros::NodeHandle z;
01304 ros::service::waitForService("pr2_right_arm_kinematics/get_fk_solver_info");
01305 ros::ServiceClient query_client = z.serviceClient<kinematics_msgs::GetKinematicSolverInfo>
01306 ("pr2_right_arm_kinematics/get_fk_solver_info");
01307 kinematics_msgs::GetKinematicSolverInfo::Request request;
01308 if(query_client.call(request,response))
01309 {
01310 for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
01311 {
01312 ROS_DEBUG("Joint: %d %s", i,response.kinematic_solver_info.joint_names[i].c_str());
01313 }
01314 }
01315 else
01316 {
01317 ROS_ERROR("Could not call query service");
01318 return false;
01319 }
01320
01321 return true;
01322 }
01323
01324
01325
01326 bool callQueryIK(kinematics_msgs::GetKinematicSolverInfo::Response &response)
01327 {
01328 ros::NodeHandle z;
01329 ros::service::waitForService("pr2_right_arm_kinematics/get_ik_solver_info");
01330 ros::ServiceClient query_client = z.serviceClient<kinematics_msgs::GetKinematicSolverInfo>("pr2_right_arm_kinematics/get_ik_solver_info");
01331 kinematics_msgs::GetKinematicSolverInfo::Request request;
01332 if(query_client.call(request,response))
01333 {
01334 for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
01335 {
01336 ROS_INFO("Joint: %d %s",i,response.kinematic_solver_info.joint_names[i].c_str());
01337 }
01338 return true;
01339 }
01340 else
01341 {
01342 ROS_ERROR("Could not call query service");
01343 return false;
01344 }
01345
01346
01347
01348 }
01349
01350
01351 void InitializeRobot()
01352 {
01353 ik_client_right = root_handle_.serviceClient<kinematics_msgs::GetConstraintAwarePositionIK>(RIGHT_ARM_IK_NAME);
01354 fk_client_right = root_handle_.serviceClient<kinematics_msgs::GetPositionFK>(RIGHT_ARM_FK_NAME);
01355
01356 while (!ros::service::waitForService(COLLISION_PROCESSING_SERVICE_NAME, ros::Duration(5.0)) && root_handle_.ok() )
01357 {
01358 ROS_INFO("Waiting for collision processing service to come up");
01359 }
01360 collision_processing_srv = root_handle_.serviceClient<tabletop_collision_map_processing::TabletopCollisionMapProcessing>
01361 (COLLISION_PROCESSING_SERVICE_NAME, true);
01362
01363
01364 point_cloud_sub_=n_.subscribe("/assembled_pt_cloud2_self_filtered",1,&book_stacking::KinectCallback,this);
01365 filtered_cloud_pub_ = n_.advertise<sensor_msgs::PointCloud2>("filtered_cloud",1);
01366 plane_marker_pub_ = n_.advertise<visualization_msgs::MarkerArray>("akans_plane_marker_array",1);
01367 obj_marker_pub_ = n_.advertise<visualization_msgs::Marker>("obj_markers",1);
01368 vis_pub_ = n_.advertise<visualization_msgs::Marker>("visualization_marker",1);
01369 command_subscriber_=n_.subscribe<std_msgs::String>("/command_generator_PR2_topic",1,&book_stacking::commandCallback, this);
01370 r_fingertip_sub_=n_.subscribe("/pressure/r_gripper_motor",1, &book_stacking::FingertipRightCallback,this);
01371 l_fingertip_sub_=n_.subscribe("/pressure/l_gripper_motor",1, &book_stacking::FingertipLeftCallback,this);
01372 tilt_timing_sub_=n_.subscribe("/laser_tilt_controller/laser_scanner_signal",2, &book_stacking::TiltTimingCallback,this);
01373 traj_right_arm_client_=new TrajClient("r_arm_controller/joint_trajectory_action", true);
01374 while(!traj_right_arm_client_->waitForServer(ros::Duration(5.0)))
01375 {
01376 ROS_INFO("Waiting for the joint_trajectory_action server");
01377 }
01378 torso_client_ = new TorsoClient("torso_controller/position_joint_action", true);
01379 while(!torso_client_->waitForServer(ros::Duration(5.0)))
01380 {
01381 ROS_INFO("Waiting for the torso action server to come up");
01382 }
01383
01384 point_head_client_ = new PointHeadClient("/head_traj_controller/point_head_action", true);
01385 while(!point_head_client_->waitForServer(ros::Duration(5.0)))
01386 {
01387 ROS_INFO("Waiting for the point_head_action server to come up");
01388 }
01389
01390 move_right_arm_client_ = new ArmActionClient("move_right_arm",true);
01391 while(!move_right_arm_client_->waitForServer(ros::Duration(5.0)))
01392 {
01393 ROS_INFO("Waiting for the move_right_arm server to come up");
01394 }
01395
01396 move_left_arm_client_ = new ArmActionClient("move_left_arm",true);
01397 while(!move_left_arm_client_->waitForServer(ros::Duration(5.0)))
01398 {
01399 ROS_INFO("Waiting for the move_left_arm server to come up");
01400 }
01401
01402 move_base_client_ = new MoveBaseClient("move_base_local",true);
01403 while(!move_base_client_->waitForServer(ros::Duration(5.0)))
01404 {
01405 ROS_INFO("Waiting for the move_base server to come up");
01406 }
01407
01408 omnix_move_base_client_ = new MoveBaseClient("move_omni_base",true);
01409 while(!move_base_client_->waitForServer(ros::Duration(5.0)))
01410 {
01411 ROS_INFO("Waiting for the move_omni_base server to come up");
01412 }
01413 pickup_client_=new PickupClient("/object_manipulator/object_manipulator_pickup", true);
01414 while(!pickup_client_->waitForServer(ros::Duration(5.0)))
01415 {
01416 ROS_INFO("Waiting for the PickupClient server to come up");
01417 }
01418 right_gripper_client_=new GripperClient("r_gripper_sensor_controller/gripper_action", true);
01419 while(!right_gripper_client_->waitForServer(ros::Duration(5.0)))
01420 {
01421 ROS_INFO("Waiting for the r_gripper_controller/gripper_action action server to come up");
01422 }
01423 left_gripper_client_=new GripperClient("l_gripper_sensor_controller/gripper_action", true);
01424 while(!left_gripper_client_->waitForServer(ros::Duration(5.0)))
01425 {
01426 ROS_INFO("Waiting for the l_gripper_controller/gripper_action action server to come up");
01427 }
01428
01429 laser_assembler_client = root_handle_.serviceClient<laser_assembler::AssembleScans>("assemble_scans");
01430 tilting_pt_cloud_pub_ = n_.advertise<sensor_msgs::PointCloud> ("/assembled_pt_cloud_raw", 5);
01431
01432 callQueryIK(ik_solver_info);
01433 callQueryFK(fk_solver_info);
01434
01435 moveTorsoToPosition(init_torso_position);
01436 lookAt("base_link", 1.0, 0.0, 0.0);
01437 OpenRightGripper();
01438
01439 rightArmHomingTrajectory=createRightArmHomingTrajectory();
01440 got_init_table=false;
01441 right_arm_fingertips_sensing=true;
01442 right_arm_fingertips_nominals=true;
01443 right_arm_fingertips_contacted=false;
01444 right_arm_end_eff_goal_resulted=false;
01445 left_arm_fingertips_sensing=false;
01446 FINGERTIP_CONTACT_THRESHOLD=150.0;
01447
01448 ROS_INFO("Subs/Pubs Initialized..");
01449 }
01450
01451
01452 pr2_controllers_msgs::JointTrajectoryGoal createRightArmTrajectoryFromAngles(std::vector<arm_config_7DOF> qs, std::vector<double> ts)
01453 {
01454 pr2_controllers_msgs::JointTrajectoryGoal goal;
01455 goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
01456 goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
01457 goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
01458 goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
01459 goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
01460 goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
01461 goal.trajectory.joint_names.push_back("r_wrist_roll_joint");
01462 goal.trajectory.points.resize(qs.size());
01463
01464 for(unsigned int ind=0;ind<qs.size();ind++)
01465 {
01466 goal.trajectory.points[ind].positions.resize(7);
01467 goal.trajectory.points[ind].positions[0] =qs[ind].angles[0];
01468 goal.trajectory.points[ind].positions[1] =qs[ind].angles[1];
01469 goal.trajectory.points[ind].positions[2] =qs[ind].angles[2];
01470 goal.trajectory.points[ind].positions[3] =qs[ind].angles[3];
01471 goal.trajectory.points[ind].positions[4] =qs[ind].angles[4];
01472 goal.trajectory.points[ind].positions[5] =qs[ind].angles[5];
01473 goal.trajectory.points[ind].positions[6] =qs[ind].angles[6];
01474 goal.trajectory.points[ind].velocities.resize(7);
01475 for (size_t j = 0; j < 7; ++j)
01476 {
01477 goal.trajectory.points[ind].velocities[j] = 0.0;
01478 }
01479 goal.trajectory.points[ind].time_from_start = ros::Duration(ts[ind]);
01480 }
01481 return goal;
01482 }
01483
01484
01485
01486
01487
01488 pr2_controllers_msgs::JointTrajectoryGoal createRightArmHomingTrajectory()
01489 {
01490 pr2_controllers_msgs::JointTrajectoryGoal goal;
01491 goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
01492 goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
01493 goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
01494 goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
01495 goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
01496 goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
01497 goal.trajectory.joint_names.push_back("r_wrist_roll_joint");
01498 goal.trajectory.points.resize(2);
01499
01500 int ind = 0;
01501
01502
01503 goal.trajectory.points[ind].positions.resize(7);
01504
01505 goal.trajectory.points[ind].positions[0] = -0.66;
01506 goal.trajectory.points[ind].positions[1] = -0.173;
01507 goal.trajectory.points[ind].positions[2] = -0.70;
01508 goal.trajectory.points[ind].positions[3] = -1.52;
01509 goal.trajectory.points[ind].positions[4] = 4.64;
01510 goal.trajectory.points[ind].positions[5] = -1.52;
01511 goal.trajectory.points[ind].positions[6] = -9.32;
01512
01513 goal.trajectory.points[ind].velocities.resize(7);
01514 for (size_t j = 0; j < 7; ++j)
01515 {
01516 goal.trajectory.points[ind].velocities[j] = 0.0;
01517 }
01518 goal.trajectory.points[ind].time_from_start = ros::Duration(3.0);
01519
01520
01521
01522
01523 ind += 1;
01524 goal.trajectory.points[ind].positions.resize(7);
01525 goal.trajectory.points[ind].positions[0] = -1.367;
01526 goal.trajectory.points[ind].positions[1] = -0.173;
01527 goal.trajectory.points[ind].positions[2] = 0.0;
01528 goal.trajectory.points[ind].positions[3] = -1.52;
01529 goal.trajectory.points[ind].positions[4] = 4.64;
01530 goal.trajectory.points[ind].positions[5] = -1.52;
01531 goal.trajectory.points[ind].positions[6] = -9.32;
01532 goal.trajectory.points[ind].velocities.resize(7);
01533 for (size_t j = 0; j < 7; ++j)
01534 {
01535 goal.trajectory.points[ind].velocities[j] = 0.0;
01536 }
01537 goal.trajectory.points[ind].time_from_start = ros::Duration(5.5);
01538
01539 return goal;
01540 }
01541
01542
01543 arm_config_7DOF GetExampleRightArmJointAngles()
01544 {
01545 arm_config_7DOF q_current;
01546 int i;
01547 for(i=0; i<7; i++)
01548 {
01549 q_current.angles[i] = (ik_solver_info.kinematic_solver_info.limits[i].min_position + ik_solver_info.kinematic_solver_info.limits[i].max_position)/2.0;
01550 }
01551 return q_current;
01552 }
01553
01554 arm_config_7DOF GetCurrentRightArmJointAngles()
01555 {
01556 arm_config_7DOF q_current;
01557 int i;
01558
01559 pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr state_msg =
01560 ros::topic::waitForMessage<pr2_controllers_msgs::JointTrajectoryControllerState>
01561 ("r_arm_controller/state");
01562
01563
01564
01565 for(i=0; i<7; i++)
01566 {
01567 q_current.angles[i] = state_msg->actual.positions[i];
01568 }
01569
01570
01571
01572
01573
01574
01575
01576 return q_current;
01577 }
01578
01579 bool run_fk(arm_config_7DOF q, geometry_msgs::PoseStamped &solution, kinematics_msgs::GetKinematicSolverInfo::Response fk_solver_info)
01580 {
01581 kinematics_msgs::GetPositionFK::Request fk_request;
01582 kinematics_msgs::GetPositionFK::Response fk_response;
01583 fk_request.header.frame_id = "torso_lift_link";
01584 fk_request.fk_link_names.resize(1);
01585 fk_request.fk_link_names[0] = "r_wrist_roll_link";
01586 fk_request.robot_state.joint_state.position.resize(fk_solver_info.kinematic_solver_info.joint_names.size());
01587 fk_request.robot_state.joint_state.name = fk_solver_info.kinematic_solver_info.joint_names;
01588
01589 for(unsigned int i=0;i<fk_solver_info.kinematic_solver_info.joint_names.size(); i++)
01590 {
01591 fk_request.robot_state.joint_state.position[i] = q.angles[i];
01592 }
01593
01594 if(fk_client_right.call(fk_request, fk_response))
01595 {
01596 if(fk_response.error_code.val == fk_response.error_code.SUCCESS)
01597 {
01598
01599
01600
01601
01602
01603
01604
01605
01606
01607
01608
01609
01610 solution=fk_response.pose_stamped[0];
01611
01612 }
01613 else
01614 {
01615 ROS_ERROR("Forward kinematics failed");
01616 }
01617 }
01618 else
01619 {
01620 ROS_ERROR("Forward kinematics service call failed");
01621 }
01622 return true;
01623 }
01624
01625
01626 bool run_ik(geometry_msgs::PoseStamped pose, arm_config_7DOF q_seed, arm_config_7DOF &q_solution, std::string link_name,kinematics_msgs::GetKinematicSolverInfo::Response ik_solver_info)
01627 {
01628 kinematics_msgs::GetConstraintAwarePositionIK::Request ik_request;
01629 kinematics_msgs::GetConstraintAwarePositionIK::Response ik_response;
01630 ik_request.timeout = ros::Duration(5.0);
01631 ik_request.ik_request.ik_seed_state.joint_state.name = ik_solver_info.kinematic_solver_info.joint_names;
01632 ik_request.ik_request.ik_link_name = link_name;
01633 ik_request.ik_request.pose_stamped = pose;
01634 ik_request.ik_request.ik_seed_state.joint_state.position.resize(ik_solver_info.kinematic_solver_info.joint_names.size());
01635
01636 for(unsigned int i=0; i< ik_solver_info.kinematic_solver_info.joint_names.size(); i++)
01637 {
01638 ik_request.ik_request.ik_seed_state.joint_state.position[i] = q_seed.angles[i];
01639 }
01640
01641
01642 ROS_INFO("IK request: %0.3f %0.3f %0.3f", pose.pose.position.x, pose.pose.position.y, pose.pose.position.z);
01643 bool ik_service_call = ik_client_right.call(ik_request,ik_response);
01644 if(!ik_service_call)
01645 {
01646 ROS_ERROR("IK service call failed!");
01647 return false;
01648 }
01649 if(ik_response.error_code.val == ik_response.error_code.SUCCESS)
01650 {
01651 for(int i=0; i<7; i++)
01652 {
01653 q_solution.angles[i] = ik_response.solution.joint_state.position[i];
01654 }
01655
01656
01657
01658
01659 ROS_INFO("IK YES");
01660 return true;
01661 }
01662 else
01663 {
01664 ROS_ERROR("IK NO");
01665 return false;
01666 }
01667 return true;
01668 }
01669
01670 bool checkIfRightGripperHoldsObject()
01671 {
01672 FINGERTIP_CONTACT_THRESHOLD=600;
01673 right_arm_fingertips_sensing=true;
01674
01675
01676 ros::Rate loop_rate(5);
01677 ros::spinOnce();
01678 loop_rate.sleep();
01679 ros::spinOnce();
01680 if(right_arm_fingertips_contacted)
01681 {
01682 right_arm_fingertips_contacted=false;
01683 right_arm_fingertips_sensing=false;
01684 FINGERTIP_CONTACT_THRESHOLD=150.0;
01685 return true;
01686 }
01687 else
01688 {
01689 right_arm_fingertips_sensing=false;
01690 FINGERTIP_CONTACT_THRESHOLD=150.0;
01691 return false;
01692 }
01693
01694 }
01695
01696 bool ExploreLinearMoveAction(double dx,double dy,double dz, arm_config_7DOF q_seed, bool move_as_much_you_can, double &path_distance, arm_config_7DOF &q_solution)
01697 {
01698
01699
01700
01701
01702
01703
01704 geometry_msgs::PoseStamped current_pose;
01705 run_fk(q_seed,current_pose,fk_solver_info);
01706 geometry_msgs::PoseStamped target_pose=current_pose;
01707 target_pose.pose.position.x=current_pose.pose.position.x+dx;
01708 target_pose.pose.position.y=current_pose.pose.position.y+dy;
01709 target_pose.pose.position.z=current_pose.pose.position.z+dz;
01710 std::string link_name="r_wrist_roll_link";
01711
01712 bool ik_result=run_ik(target_pose,GetExampleRightArmJointAngles(),q_solution,link_name,ik_solver_info);
01713 if(!ik_result)
01714 {
01715 if(!move_as_much_you_can)
01716 {
01717 return false;
01718 }
01719 const int num_inter_checks=5;
01720 for(int i=num_inter_checks-1;i>0;i--)
01721 {
01722 double path_ratio=((double)(i))/num_inter_checks;
01723 target_pose.pose.position.x=current_pose.pose.position.x+dx*path_ratio;
01724 target_pose.pose.position.y=current_pose.pose.position.y+dy*path_ratio;
01725 target_pose.pose.position.z=current_pose.pose.position.z+dz*path_ratio;
01726 ik_result=run_ik(target_pose,GetExampleRightArmJointAngles(),q_solution,link_name,ik_solver_info);
01727 if(ik_result)
01728 {
01729 path_distance=sqrt(dx*dx+dy*dy+dz*dz)*path_ratio;
01730 break;
01731 }
01732 }
01733 }
01734 else
01735 {
01736 path_distance=sqrt(dx*dx+dy*dy+dz*dz);
01737 }
01738
01739
01740
01741
01742
01743
01744
01745
01746 return ik_result;
01747 }
01748
01749
01750 bool SendRightEndEffectorTrajectory(pr2_controllers_msgs::JointTrajectoryGoal traj, bool stop_when_contact)
01751 {
01752
01753 if(stop_when_contact)
01754 {
01755
01756 right_arm_fingertips_sensing=true;
01757 traj_right_arm_client_->sendGoal(traj);
01758 while(true)
01759 {
01760 if(right_arm_fingertips_contacted)
01761 {
01762 std::cout<<"R FINGER CONTACT!"<<std::endl;
01763 right_arm_fingertips_contacted=false;
01764 traj_right_arm_client_->cancelAllGoals();
01765 right_arm_fingertips_sensing=false;
01766 return false;
01767 }
01768 actionlib::SimpleClientGoalState state = traj_right_arm_client_->getState();
01769 if(state==actionlib::SimpleClientGoalState::SUCCEEDED ||
01770 state==actionlib::SimpleClientGoalState::ABORTED ||
01771 state==actionlib::SimpleClientGoalState::REJECTED ||
01772 state==actionlib::SimpleClientGoalState::LOST)
01773 {
01774 right_arm_fingertips_sensing=false;
01775 return true;
01776 }
01777 ros::spinOnce();
01778 }
01779 }
01780 else
01781 {
01782 traj_right_arm_client_->sendGoal(traj);
01783 traj_right_arm_client_->waitForResult();
01784 actionlib::SimpleClientGoalState state = traj_right_arm_client_->getState();
01785 if(state==actionlib::SimpleClientGoalState::SUCCEEDED)
01786 {
01787 return true;
01788 }
01789 else
01790 {
01791 return false;
01792 }
01793 }
01794 }
01795
01796
01797 void TiltTimingCallback(const pr2_msgs::LaserScannerSignal::ConstPtr& msg)
01798 {
01799 if(msg->signal==1)
01800 {
01801 tilt_minima_msg=*msg;
01802 }
01803 else
01804 {
01805 tilt_maxima_msg=*msg;
01806 }
01807 return;
01808 }
01809
01810 void SendLeftGripperCommand(double pos, double effort)
01811 {
01812 pr2_controllers_msgs::Pr2GripperCommandGoal open;
01813 open.command.position = pos;
01814 open.command.max_effort = effort;
01815 left_gripper_client_->sendGoal(open);
01816 left_gripper_client_->waitForResult();
01817 if(left_gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
01818 ROS_INFO("The gripper command succeeded!");
01819 else
01820 ROS_INFO("The gripper failed to open.");
01821 }
01822
01823 void OpenLeftGripper()
01824 {
01825 SendLeftGripperCommand(0.08,-1.0);
01826 }
01827
01828 void CloseLeftGripper()
01829 {
01830 SendLeftGripperCommand(0.0,50.0);
01831 }
01832
01833 void SendRightGripperCommand(double pos, double effort)
01834 {
01835 pr2_controllers_msgs::Pr2GripperCommandGoal open;
01836 open.command.position = pos;
01837 open.command.max_effort = effort;
01838 right_gripper_client_->sendGoal(open);
01839 right_gripper_client_->waitForResult();
01840 if(right_gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
01841 ROS_INFO("The gripper command succeeded!");
01842 else
01843 ROS_INFO("The gripper failed to open.");
01844 }
01845
01846 void OpenRightGripper()
01847 {
01848 SendRightGripperCommand(0.08,-1.0);
01849 }
01850
01851 void CloseRightGripper()
01852 {
01853 SendRightGripperCommand(0.0,50.0);
01854 }
01855
01856
01857 void FingertipLeftCallback(const pr2_msgs::PressureState::ConstPtr& msg)
01858 {
01859
01860 if(left_arm_fingertips_sensing)
01861 {
01862 ROS_INFO("L");
01863
01864 }
01865
01866 }
01867
01868 void FingertipRightCallback(const pr2_msgs::PressureState::ConstPtr& msg)
01869 {
01870 if(right_arm_fingertips_sensing)
01871 {
01872
01873 if(right_arm_fingertips_nominals)
01874 {
01875 right_arm_fingertips_nominals=false;
01876 right_arm_fingertips_sensing=false;
01877 right_arm_l_finger_tip_nominals=msg->l_finger_tip;
01878 right_arm_r_finger_tip_nominals=msg->r_finger_tip;
01879 }
01880 else
01881 {
01882 std::vector<short int> l_finger_tip_arr=msg->l_finger_tip;
01883 std::vector<short int> r_finger_tip_arr=msg->r_finger_tip;
01884
01885 int counter=0;
01886 for(unsigned int i=0;i<l_finger_tip_arr.size();i++)
01887 {
01888 int diff_l=l_finger_tip_arr[i]-right_arm_l_finger_tip_nominals[i];
01889 int diff_r=r_finger_tip_arr[i]-right_arm_r_finger_tip_nominals[i];
01890
01891
01892 if(diff_r>FINGERTIP_CONTACT_THRESHOLD)
01893 {
01894 counter++;
01895 }
01896 if(diff_l>FINGERTIP_CONTACT_THRESHOLD)
01897 {
01898 counter++;
01899 }
01900 if(counter>2)
01901 {
01902 right_arm_fingertips_contacted=true;
01903 }
01904
01905 }
01906
01907 }
01908
01909 }
01910 }
01911
01912 bool GetTiltingPointCloud(bool refreshScan)
01913 {
01914 ros::Time t_minima=tilt_minima_msg.header.stamp;
01915 ros::Time t_maxima=tilt_maxima_msg.header.stamp;
01916 if(refreshScan)
01917 {
01918 ros::Duration dt_latest_msg;
01919
01920 if(t_minima>t_maxima)
01921 {
01922 dt_latest_msg=ros::Time::now()-t_minima;
01923 }
01924 else
01925 {
01926 dt_latest_msg=ros::Time::now()-t_maxima;
01927 }
01928 ros::Duration wait_sec=ros::Duration(tilt_period)-dt_latest_msg+ros::Duration(0.5);
01929 if(wait_sec.toSec()>ros::Duration(tilt_period).toSec())
01930 {
01931 wait_sec=ros::Duration(tilt_period);
01932 }
01933 std::cout<<"Waiting for "<<wait_sec.toSec()<<" s."<<std::endl;
01934
01935
01936 ros::Rate loop_rate(10);
01937 int counter_max=(int)(wait_sec.toSec()*10)+2;
01938 int counter=0;
01939 while (counter<counter_max)
01940 {
01941 ros::spinOnce();
01942 loop_rate.sleep();
01943 counter++;
01944 }
01945 t_minima=tilt_minima_msg.header.stamp;
01946 t_maxima=tilt_maxima_msg.header.stamp;
01947
01948 }
01949
01950
01951
01952
01953
01954
01955
01956
01957
01958 laser_assembler::AssembleScans srv;
01959 if(t_minima>t_maxima)
01960 {
01961 srv.request.begin=t_maxima;
01962 srv.request.end =t_minima;
01963 }
01964 else
01965 {
01966 srv.request.begin=t_minima;
01967 srv.request.end=t_maxima;
01968 }
01969
01970 if (laser_assembler_client.call(srv))
01971 {
01972 printf("Assembled cloud: %u points\n", srv.response.cloud.points.size());
01973
01974 tilting_pt_cloud_pub_.publish(srv.response.cloud);
01975
01976 }
01977 else
01978 printf("Service call failed\n");
01979
01980 return true;
01981 }
01982
01983 void moveTorsoToPosition(double d)
01984 {
01985 pr2_controllers_msgs::SingleJointPositionGoal q;
01986 q.position = d;
01987 q.min_duration = ros::Duration(2.0);
01988 q.max_velocity = 1.0;
01989 torso_client_->sendGoal(q);
01990 torso_client_->waitForResult();
01991 }
01992
01993
01994 void LoadParameters()
01995 {
01996
01997 n_.param("filter_spatial",filter_spatial,true);
01998 n_.param("filter_spatial_zmax",filter_spatial_zmax,3.0);
01999 n_.param("filter_spatial_zmin",filter_spatial_zmin,0.05);
02000 n_.param("filter_spatial_ymax",filter_spatial_ymax,10.0);
02001 n_.param("filter_spatial_ymin",filter_spatial_ymin,-10.0);
02002 n_.param("filter_spatial_xmax",filter_spatial_xmax,10.0);
02003 n_.param("filter_spatial_xmin",filter_spatial_xmin,-10.0);
02004
02005
02006 n_.param("filter_outliers",filter_outliers,true);
02007 n_.param("filter_outliers_meank",filter_outliers_meank,50);
02008 n_.param("filter_outliers_stddev_thresh", filter_outliers_stddev_thresh,1.0);
02009
02010
02011 n_.param("downsample_cloud",downsample_cloud,true);
02012 n_.param("downsample_grid_size",downsample_grid_size_,0.01);
02013
02014
02015 n_.param("max_planes",max_planes_,4);
02016 n_.param("use_omp",use_omp_,false);
02017 n_.param("plane_distance_thresh",plane_distance_thresh_,0.03);
02018 n_.param("normal_search_radius",normal_search_radius_,0.1);
02019 n_.param("min_plane_inliers",min_plane_inliers_,1000);
02020 n_.param("max_sac_iterations",max_sac_iterations_,1000);
02021 n_.param("sac_probability",sac_probability_,0.99);
02022 n_.param("concave_hull_mode",concave_hull_mode_,false);
02023 n_.param("use_normal_seg",use_normal_seg_,false);
02024 n_.param("base_frame_tf", base_frame_tf,std::string("/base_link"));
02025 n_.param("tilt_period", tilt_period, 15.0);
02026
02027 n_.param("table_obj_detector_lower_z",table_obj_detector_lower_z,-0.2);
02028 n_.param("table_obj_detector_upper_z",table_obj_detector_upper_z,-0.015);
02029 n_.param("test_arms",test_arms,true);
02030 n_.param("enable_nav",enable_nav,true);
02031
02032 n_.param("optimal_workspace_wrt_torso_x",optimal_workspace_wrt_torso_x,0.6);
02033 n_.param("optimal_workspace_wrt_torso_y",optimal_workspace_wrt_torso_y,-0.188);
02034 n_.param("optimal_workspace_wrt_torso_z",optimal_workspace_wrt_torso_z,-0.28);
02035 n_.param("optimal_workspace_wrt_torso_x_grasping",optimal_workspace_wrt_torso_x_grasping,0.6);
02036 n_.param("optimal_workspace_wrt_torso_y_grasping",optimal_workspace_wrt_torso_y_grasping, -0.188);
02037 n_.param("optimal_workspace_wrt_torso_z_grasping",optimal_workspace_wrt_torso_z_grasping , -0.28);
02038 n_.param("init_torso_position",init_torso_position,0.3);
02039 n_.param("predrag_dist",predrag_dist,0.07);
02040 n_.param("pregrasp_dist",pregrasp_dist,0.1);
02041 n_.param("preplace_dist",preplace_dist,0.1);
02042 n_.param("pregrasp_dist_vertical",pregrasp_dist_vertical,0.2);
02043 n_.param("diff_drag_force",diff_drag_force,600.0);
02044 n_.param("nav_waypoint_offset",nav_waypoint_offset,0.75);
02045 n_.param("grasp_offset_from_com",grasp_offset_from_com,0.05);
02046
02047
02048 }
02049
02050 void lookAt(std::string frame_id, double x, double y, double z)
02051 {
02052
02053 pr2_controllers_msgs::PointHeadGoal goal;
02054
02055
02056 geometry_msgs::PointStamped point;
02057 point.header.frame_id = frame_id;
02058 point.point.x = x; point.point.y = y; point.point.z = z;
02059 goal.target = point;
02060
02061
02062
02063 goal.pointing_frame = "high_def_frame";
02064
02065
02066 goal.min_duration = ros::Duration(0.5);
02067
02068
02069 goal.max_velocity = 1.0;
02070
02071
02072 point_head_client_->sendGoal(goal);
02073
02074
02075 point_head_client_->waitForResult(ros::Duration(2));
02076 }
02077
02078 void shakeHead(int n)
02079 {
02080 int count = 0;
02081 while (ros::ok() && ++count <= n )
02082 {
02083
02084 lookAt("base_link", 5.0, 1.0, 1.2);
02085
02086
02087 lookAt("base_link", 5.0, -1.0, 1.2);
02088 }
02089 }
02090
02091 bool ExecuteDragAction(geometry_msgs::Vector3Stamped desired_dir,double desired_dist,DragActionTemplate drag_act)
02092 {
02093 OpenRightGripper();
02094
02095 double speed=0.12;
02096 std::vector<arm_config_7DOF> qs;
02097 std::vector<double> ts;
02098 qs.push_back(drag_act.q_0);
02099 ts.push_back(3.0);
02100 pr2_controllers_msgs::JointTrajectoryGoal traj0=createRightArmTrajectoryFromAngles(qs,ts);
02101 SendRightEndEffectorTrajectory(traj0,false);
02102
02103 double path_distance;
02104 arm_config_7DOF q_1,q_2;
02105
02106 ExploreLinearMoveAction(0.0,0.0,-(predrag_dist),GetCurrentRightArmJointAngles(),true,path_distance,q_1);
02107 qs.clear();
02108 ts.clear();
02109 ts.push_back(2.0);
02110
02111 qs.push_back(drag_act.q_1);
02112 pr2_controllers_msgs::JointTrajectoryGoal traj1=createRightArmTrajectoryFromAngles(qs,ts);
02113
02114 FINGERTIP_CONTACT_THRESHOLD=diff_drag_force;
02115 SendRightEndEffectorTrajectory(traj1,true);
02116 FINGERTIP_CONTACT_THRESHOLD=150.0;
02117
02118
02119
02120
02121
02122
02123
02124
02125
02126
02127
02128
02129
02130
02131
02132
02133 qs.clear();
02134 ts.clear();
02135 ts.push_back(3.0);
02136 qs.push_back(drag_act.q_2);
02137 pr2_controllers_msgs::JointTrajectoryGoal traj2=createRightArmTrajectoryFromAngles(qs,ts);
02138 SendRightEndEffectorTrajectory(traj2,false);
02139
02140
02141
02142
02143
02144
02145
02146
02147
02148
02149
02150
02151
02152
02153
02154
02155
02156
02157
02158
02159
02160
02161
02162
02163
02164
02165 return true;
02166 }
02167
02168 bool ExploreDragAction(book_stacking_msgs::ObjectInfo objInfo,geometry_msgs::Vector3Stamped desired_dir,double desired_dist, double &output_dist,DragActionTemplate &drag_act, geometry_msgs::PoseStamped &queried_pose_wrt_torso_lift_link)
02169 {
02170 geometry_msgs::PointStamped input_point;
02171 geometry_msgs::PointStamped obj_centroid_wrt_torso_lift_link;
02172 input_point.header.frame_id=objInfo.header.frame_id;
02173 input_point.point.x=objInfo.centroid.x;
02174 input_point.point.y=objInfo.centroid.y;
02175 input_point.point.z=objInfo.centroid.z;
02176 tf_listener.transformPoint(TORSO_LIFT_LINK_STR, input_point, obj_centroid_wrt_torso_lift_link);
02177
02178 geometry_msgs::PoseStamped tpose;
02179 tpose.header.frame_id=TORSO_LIFT_LINK_STR;
02180 tpose.pose.orientation=tf::createQuaternionMsgFromRollPitchYaw(M_PI/2,M_PI/2,0.0);
02181 tpose.pose.position.x=obj_centroid_wrt_torso_lift_link.point.x;
02182 tpose.pose.position.y=obj_centroid_wrt_torso_lift_link.point.y;
02183 tpose.pose.position.z=obj_centroid_wrt_torso_lift_link.point.z+END_EFFECTOR_OFFSET;
02184
02185 queried_pose_wrt_torso_lift_link=tpose;
02186
02187 std::cout<<"q_1 :Drag pose"<<std::endl;
02188 arm_config_7DOF q_1;
02189 if (run_ik(tpose,GetExampleRightArmJointAngles(),q_1,R_WRIST_ROLL_LINK_STR,ik_solver_info))
02190 {
02191 arm_config_7DOF q_2;
02192 std::cout<<"q_2 :After Drag pose"<<std::endl;
02193 ExploreLinearMoveAction(desired_dir.vector.x*desired_dist,desired_dir.vector.y*desired_dist,desired_dir.vector.z*desired_dist,q_1,true,output_dist,q_2);
02194
02195 tpose.pose.position.z=obj_centroid_wrt_torso_lift_link.point.z+(END_EFFECTOR_OFFSET+predrag_dist);
02196 arm_config_7DOF q_0;
02197 std::cout<<"q_0 :Pre-drag pose"<<std::endl;
02198 if(run_ik(tpose,GetExampleRightArmJointAngles(),q_0,R_WRIST_ROLL_LINK_STR,ik_solver_info))
02199 {
02200 drag_act.q_0=q_0;
02201 drag_act.q_1=q_1;
02202 drag_act.q_2=q_2;
02203 drag_act.dist=output_dist;
02204 return true;
02205 }
02206 else
02207 {
02208 return false;
02209 }
02210
02211 }
02212 else
02213 {
02214 return false;
02215 }
02216 }
02217
02218 bool ExecutePlaceAction(DragActionTemplate drag_act)
02219 {
02220
02221 std::vector<arm_config_7DOF> qs;
02222 std::vector<double> ts;
02223 qs.push_back(drag_act.q_0);
02224 ts.push_back(3.0);
02225 pr2_controllers_msgs::JointTrajectoryGoal traj0=createRightArmTrajectoryFromAngles(qs,ts);
02226 SendRightEndEffectorTrajectory(traj0,false);
02227
02228 double path_distance;
02229 arm_config_7DOF q_1,q_2;
02230
02231
02232
02233
02234
02235
02236
02237
02238
02239 OpenRightGripper();
02240
02241 qs.clear();
02242 ts.clear();
02243 ts.push_back(0.5);
02244 qs.push_back(drag_act.q_2);
02245 pr2_controllers_msgs::JointTrajectoryGoal traj2=createRightArmTrajectoryFromAngles(qs,ts);
02246 SendRightEndEffectorTrajectory(traj2,false);
02247
02248 move_base_msgs::MoveBaseGoal goal;
02249 goal.target_pose.header.frame_id="base_link";
02250 goal.target_pose.pose.position.x = -0.1;
02251 goal.target_pose.pose.position.y = 0.0;
02252 goal.target_pose.pose.position.z = 0.0;
02253 goal.target_pose.pose.orientation.x = 0.0;
02254 goal.target_pose.pose.orientation.y = 0.0;
02255 goal.target_pose.pose.orientation.z = 0.0;
02256 goal.target_pose.pose.orientation.w = 1.0;
02257 goal.target_pose.header.stamp=ros::Time::now();
02258 OmnixMoveBasePosition(goal);
02259
02260 HomeRightArm();
02261
02262 return true;
02263 }
02264
02265 bool ExecuteGraspAction(DragActionTemplate drag_act)
02266 {
02267 OpenRightGripper();
02268
02269 std::vector<arm_config_7DOF> qs;
02270 std::vector<double> ts;
02271 qs.push_back(drag_act.q_0);
02272 ts.push_back(3.0);
02273 pr2_controllers_msgs::JointTrajectoryGoal traj0=createRightArmTrajectoryFromAngles(qs,ts);
02274 SendRightEndEffectorTrajectory(traj0,false);
02275
02276 double path_distance;
02277 arm_config_7DOF q_1,q_2;
02278
02279
02280 qs.clear();
02281 ts.clear();
02282 ts.push_back(3.0);
02283 qs.push_back(drag_act.q_1);
02284 pr2_controllers_msgs::JointTrajectoryGoal traj1=createRightArmTrajectoryFromAngles(qs,ts);
02285
02286 FINGERTIP_CONTACT_THRESHOLD=100.0;
02287 SendRightEndEffectorTrajectory(traj1,true);
02288 FINGERTIP_CONTACT_THRESHOLD=150.0;
02289
02290
02291
02292
02293
02294
02295
02296
02297
02298
02299
02300
02301
02302
02303
02304
02305
02306 SendRightGripperCommand(0.02,50.0);
02307
02308
02309
02310
02311
02312
02313
02314
02315
02316
02317 qs.clear();
02318 ts.clear();
02319 ts.push_back(3.0);
02320 qs.push_back(drag_act.q_2);
02321 pr2_controllers_msgs::JointTrajectoryGoal traj2=createRightArmTrajectoryFromAngles(qs,ts);
02322 SendRightEndEffectorTrajectory(traj2,false);
02323
02324 return true;
02325 }
02326
02327 bool ExplorePlaceAction(book_stacking_msgs::ObjectInfo objInfo,double &output_dist, DragActionTemplate &grasp_act, geometry_msgs::PoseStamped &queried_pose_wrt_torso_lift_link)
02328 {
02329 geometry_msgs::PointStamped input_point;
02330 geometry_msgs::PointStamped obj_centroid_wrt_torso_lift_link;
02331 input_point.header.frame_id=objInfo.header.frame_id;
02332 input_point.point.x=objInfo.centroid.x;
02333 input_point.point.y=objInfo.centroid.y;
02334 input_point.point.z=objInfo.centroid.z;
02335 tf_listener.transformPoint(TORSO_LIFT_LINK_STR, input_point, obj_centroid_wrt_torso_lift_link);
02336
02337 geometry_msgs::PoseStamped tpose;
02338 tpose.header.frame_id=TORSO_LIFT_LINK_STR;
02339 tpose.pose.orientation=tf::createQuaternionMsgFromRollPitchYaw(M_PI/2,M_PI/4,0.0);
02340 tpose.pose.position.x=obj_centroid_wrt_torso_lift_link.point.x-(END_EFFECTOR_OFFSET+0.06);
02341 tpose.pose.position.y=obj_centroid_wrt_torso_lift_link.point.y;
02342 tpose.pose.position.z=obj_centroid_wrt_torso_lift_link.point.z+(0.04);
02343 queried_pose_wrt_torso_lift_link=tpose;
02344
02345 std::cout<<"q_1 :Place pose"<<std::endl;
02346 arm_config_7DOF q_1;
02347 if (run_ik(tpose,GetExampleRightArmJointAngles(),q_1,R_WRIST_ROLL_LINK_STR,ik_solver_info))
02348 {
02349 arm_config_7DOF q_2;
02350 std::cout<<"q_2 :After Place pose"<<std::endl;
02351 ExploreLinearMoveAction(-0.10,0.0,0.10,q_1,true,output_dist,q_2);
02352
02353 tpose.pose.position.z=obj_centroid_wrt_torso_lift_link.point.z+(0.04+preplace_dist);
02354 arm_config_7DOF q_0;
02355 std::cout<<"q_0 :Pre-place pose"<<std::endl;
02356 if(run_ik(tpose,GetExampleRightArmJointAngles(),q_0,R_WRIST_ROLL_LINK_STR,ik_solver_info))
02357 {
02358 grasp_act.q_0=q_0;
02359 grasp_act.q_1=q_1;
02360 grasp_act.q_2=q_2;
02361 grasp_act.dist=output_dist;
02362 return true;
02363 }
02364 else
02365 {
02366 return false;
02367 }
02368 }
02369 else
02370 {
02371 return false;
02372 }
02373
02374 return false;
02375 }
02376
02377 bool ExploreGraspAction(book_stacking_msgs::ObjectInfo objInfo,double &output_dist, DragActionTemplate &grasp_act, geometry_msgs::PoseStamped &queried_pose_wrt_torso_lift_link)
02378 {
02379 geometry_msgs::PointStamped input_point;
02380 geometry_msgs::PointStamped obj_centroid_wrt_torso_lift_link;
02381 input_point.header.frame_id=objInfo.header.frame_id;
02382 input_point.point.x=objInfo.centroid.x;
02383 input_point.point.y=objInfo.centroid.y;
02384 input_point.point.z=objInfo.centroid.z;
02385 tf_listener.transformPoint(TORSO_LIFT_LINK_STR, input_point, obj_centroid_wrt_torso_lift_link);
02386
02387 geometry_msgs::PoseStamped tpose;
02388 tpose.header.frame_id=TORSO_LIFT_LINK_STR;
02389 tpose.pose.orientation=tf::createQuaternionMsgFromRollPitchYaw(M_PI/2,0.0,0.0);
02390 tpose.pose.position.x=obj_centroid_wrt_torso_lift_link.point.x-(END_EFFECTOR_OFFSET+grasp_offset_from_com);
02391 tpose.pose.position.y=obj_centroid_wrt_torso_lift_link.point.y;
02392 tpose.pose.position.z=obj_centroid_wrt_torso_lift_link.point.z-0.02;
02393 queried_pose_wrt_torso_lift_link=tpose;
02394
02395 std::cout<<"q_1 :Grasp pose"<<std::endl;
02396 arm_config_7DOF q_1;
02397 if (run_ik(tpose,GetExampleRightArmJointAngles(),q_1,R_WRIST_ROLL_LINK_STR,ik_solver_info))
02398 {
02399 arm_config_7DOF q_2;
02400 std::cout<<"q_2 :After Grasp pose"<<std::endl;
02401 ExploreLinearMoveAction(0.0,0.0,pregrasp_dist_vertical,q_1,true,output_dist,q_2);
02402
02403 tpose.pose.position.x=obj_centroid_wrt_torso_lift_link.point.x-(END_EFFECTOR_OFFSET+ grasp_offset_from_com+pregrasp_dist);
02404 arm_config_7DOF q_0;
02405 std::cout<<"q_0 :Pre-drag pose"<<std::endl;
02406 if(run_ik(tpose,GetExampleRightArmJointAngles(),q_0,R_WRIST_ROLL_LINK_STR,ik_solver_info))
02407 {
02408 grasp_act.q_0=q_0;
02409 grasp_act.q_1=q_1;
02410 grasp_act.q_2=q_2;
02411 grasp_act.dist=output_dist;
02412 return true;
02413 }
02414 else
02415 {
02416 return false;
02417 }
02418 }
02419 else
02420 {
02421 return false;
02422 }
02423
02424 return false;
02425 }
02426 bool PlaceObject(int object_index,double &output_dist)
02427 {
02428
02429 book_stacking_msgs::ObjectInfo book_info;
02430 book_info.header.frame_id="odom_combined";
02431 book_info.centroid.x=all_objects_x_wrt_odom_combined[object_index];
02432 book_info.centroid.y=all_objects_y_wrt_odom_combined[object_index];
02433 book_info.centroid.z=all_objects_z_wrt_odom_combined[object_index];
02434
02435 if(enable_nav)
02436 {
02437 NavigateToObject(book_info);
02438 }
02439
02440
02441
02442 DragActionTemplate output_grasp_act={};
02443 bool explore_grasp_success=false;
02444 geometry_msgs::PoseStamped queried_pose_wrt_torso_lift_link;
02445
02446 explore_grasp_success=ExplorePlaceAction(book_info, output_dist, output_grasp_act, queried_pose_wrt_torso_lift_link);
02447
02448 if(explore_grasp_success)
02449 {
02450 ROS_INFO("place Action Feasible");
02451 if(ExecutePlaceAction(output_grasp_act))
02452 {
02453 ROS_INFO("place Action Executed Successfully");
02454 return true;
02455 }
02456 else
02457 {
02458 ROS_INFO("place Action Execution wasn't successful");
02459 return false;
02460 }
02461 }
02462 else
02463 {
02464 ROS_INFO("place Action Not Feasible. Move the base to optimal location");
02465 OmnixAdjustBaseForOptimalWorkspace(queried_pose_wrt_torso_lift_link);
02466 explore_grasp_success=ExplorePlaceAction(book_info, output_dist, output_grasp_act, queried_pose_wrt_torso_lift_link);
02467 if(explore_grasp_success)
02468 {
02469 ROS_INFO("2.place Action Feasible");
02470
02471 if(ExecutePlaceAction(output_grasp_act))
02472 {
02473 ROS_INFO("2. place Action Executed Successfully");
02474 return true;
02475 }
02476 else
02477 {
02478 ROS_INFO("2. place Action Execution wasn't successful");
02479 return false;
02480 }
02481 }
02482 else
02483 {
02484 ROS_INFO("2. place Action Not Feasible.");
02485 return false;
02486 }
02487 }
02488
02489 return true;
02490 }
02491
02492 bool GraspObject(int object_index, book_stacking_msgs::ObjectInfos allObjectInfos, double &output_dist)
02493 {
02494
02495
02496
02497 book_stacking_msgs::ObjectInfo book_info;
02498 book_info.header.frame_id="odom_combined";
02499 book_info.centroid.x=all_objects_x_wrt_odom_combined[object_index];
02500 book_info.centroid.y=all_objects_y_wrt_odom_combined[object_index];
02501 book_info.centroid.z=all_objects_z_wrt_odom_combined[object_index];
02502
02503
02504 DragActionTemplate output_grasp_act={};
02505 bool explore_grasp_success=false;
02506 geometry_msgs::PoseStamped queried_pose_wrt_torso_lift_link;
02507
02508
02509 explore_grasp_success= ExploreGraspAction(book_info, output_dist, output_grasp_act, queried_pose_wrt_torso_lift_link);
02510
02511 if(explore_grasp_success)
02512 {
02513 ROS_INFO("Grasp Action Feasible");
02514 if(ExecuteGraspAction(output_grasp_act))
02515 {
02516 ROS_INFO("Grasp Action Executed.");
02517 return true;
02518 }
02519 else
02520 {
02521 ROS_INFO("Grasp Action Execution wasn't successful");
02522 return false;
02523 }
02524 }
02525 else
02526 {
02527 ROS_INFO("Grasp Action Not Feasible. Move the base to optimal location");
02528 OmnixAdjustBaseForOptimalWorkspaceForGrasping(queried_pose_wrt_torso_lift_link);
02529 explore_grasp_success=ExploreGraspAction(book_info, output_dist, output_grasp_act, queried_pose_wrt_torso_lift_link);
02530 if(explore_grasp_success)
02531 {
02532 ROS_INFO("2.Grasp Action Feasible");
02533
02534 if(ExecuteGraspAction(output_grasp_act))
02535 {
02536 ROS_INFO("2. Grasp Action Executed Successfully");
02537 return true;
02538 }
02539 else
02540 {
02541 ROS_INFO("2. Grasp Action Execution wasn't successful");
02542 return false;
02543 }
02544 }
02545 else
02546 {
02547 ROS_INFO("2. Drag Action Not Feasible.");
02548 return false;
02549 }
02550
02551 }
02552
02553
02554
02555
02556 }
02557
02558 bool DragObject(book_stacking_msgs::DragRequest drag_req, double &output_dist)
02559 {
02560 DragActionTemplate output_drag_act={};
02561 bool explore_drag_success=false;
02562 geometry_msgs::PoseStamped queried_pose_wrt_torso_lift_link;
02563
02564 explore_drag_success=ExploreDragAction(drag_req.object, drag_req.dir, drag_req.dist, output_dist, output_drag_act,queried_pose_wrt_torso_lift_link);
02565
02566
02567
02568
02569
02570
02571
02572
02573
02574
02575
02576
02577
02578
02579
02580
02581
02582
02583 OmnixAdjustBaseForOptimalWorkspace(queried_pose_wrt_torso_lift_link);
02584
02585 explore_drag_success=ExploreDragAction(drag_req.object, drag_req.dir, drag_req.dist, output_dist,output_drag_act,queried_pose_wrt_torso_lift_link);
02586 if(explore_drag_success)
02587 {
02588 ROS_INFO("2.Drag Action Feasible");
02589
02590 if(ExecuteDragAction(drag_req.dir,drag_req.dist,output_drag_act))
02591 {
02592 ROS_INFO("2. Drag Action Executed Successfully");
02593 return true;
02594 }
02595 else
02596 {
02597 ROS_INFO("2. Drag Action Execution wasn't successful");
02598 return false;
02599 }
02600 }
02601 else
02602 {
02603 ROS_INFO("2. Drag Action Not Feasible.");
02604 return false;
02605 }
02606
02607
02608
02609 return false;
02610 }
02611
02612 bool PlaceEndEffector(bool use_right_arm, ArmActionClient *arm_ac_client_, arm_navigation_msgs::SimplePoseConstraint &desired_pose,bool disable_gripper)
02613 {
02614
02615
02616 std::cout<<"Req IK position(PlaceEndEffector): (" <<desired_pose.pose.position.x<<" "<<
02617 desired_pose.pose.position.y<<" "<<
02618 desired_pose.pose.position.z<<")"<<
02619 ". Orient: ("<<
02620 desired_pose.pose.orientation.x<<" "<<
02621 desired_pose.pose.orientation.y<<" "<<
02622 desired_pose.pose.orientation.z<<" "<<
02623 desired_pose.pose.orientation.w<<"). frame_id: "<<
02624 desired_pose.header.frame_id<<" "<<
02625 " link_name:"<< desired_pose.link_name<<std::endl;
02626
02627
02628
02629
02630
02631
02632
02633 desired_pose.absolute_position_tolerance.x = 0.02;
02634 desired_pose.absolute_position_tolerance.y = 0.02;
02635 desired_pose.absolute_position_tolerance.z = 0.02;
02636 desired_pose.absolute_roll_tolerance = 0.04;
02637 desired_pose.absolute_pitch_tolerance = 0.04;
02638 desired_pose.absolute_yaw_tolerance = 0.04;
02639
02640 arm_navigation_msgs::MoveArmGoal goalA;
02641 if(use_right_arm)
02642 {
02643 goalA.motion_plan_request.group_name = "right_arm";
02644 }
02645 else
02646 {
02647 goalA.motion_plan_request.group_name = "left_arm";
02648 }
02649 goalA.motion_plan_request.num_planning_attempts = 1;
02650 goalA.motion_plan_request.planner_id = std::string("");
02651 goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
02652 goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
02653 goalA.motion_plan_request.goal_constraints.set_position_constraints_size(1);
02654 goalA.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now();
02655
02656 goalA.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = desired_pose.header.frame_id;
02657 goalA.motion_plan_request.goal_constraints.position_constraints[0].link_name = desired_pose.link_name;
02658
02659
02660
02661 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.x = desired_pose.pose.position.x;
02662 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = desired_pose.pose.position.y;
02663 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.z = desired_pose.pose.position.z;
02664
02665
02666
02667
02668
02669
02670
02671 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = arm_navigation_msgs::Shape::BOX;
02672 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.04);
02673 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.04);
02674 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.04);
02675 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0;
02676 goalA.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;
02677
02678 goalA.motion_plan_request.goal_constraints.set_orientation_constraints_size(1);
02679 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
02680
02681 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = desired_pose.header.frame_id;
02682
02683 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = desired_pose.link_name;
02684
02685
02686
02687 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation=tf::createQuaternionMsgFromRollPitchYaw(M_PI/2,M_PI/2,0.0);
02688
02689
02690
02691
02692 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04;
02693 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04;
02694 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04;
02695 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;
02696
02697
02698
02699
02700
02701
02702
02703
02704
02705
02706
02707
02708
02709
02710
02711
02712
02713
02714
02715 bool finished_within_time = false;
02716 if(disable_gripper)
02717 {
02718 arm_navigation_msgs::CollisionOperation coll_disable_msg;
02719 if(use_right_arm)
02720 {
02721 coll_disable_msg.object1 = "r_end_effector";
02722 }
02723 else
02724 {
02725 coll_disable_msg.object1 = "l_end_effector";
02726 }
02727
02728 coll_disable_msg.object2 = arm_navigation_msgs::CollisionOperation::COLLISION_SET_ALL;
02729 coll_disable_msg.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
02730 goalA.operations.collision_operations[0] = coll_disable_msg;
02731 }
02732
02733
02734 arm_ac_client_->sendGoal(goalA);
02735 finished_within_time = arm_ac_client_->waitForResult(ros::Duration(20.0));
02736 if (!finished_within_time)
02737 {
02738 arm_ac_client_->cancelGoal();
02739 ROS_INFO("Timed out achieving goal A");
02740 }
02741 else
02742 {
02743 actionlib::SimpleClientGoalState state = arm_ac_client_->getState();
02744 bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
02745 if(success)
02746 ROS_INFO("Action finished: %s",state.toString().c_str());
02747 else
02748 ROS_INFO("Action failed: %s",state.toString().c_str());
02749 return success;
02750 }
02751 return false;
02752
02753 }
02754
02755 bool SetMoveArmGoal(double x,double y,double z)
02756 {
02757 arm_navigation_msgs::MoveArmGoal goalA;
02758 goalA.motion_plan_request.group_name = "right_arm";
02759 goalA.motion_plan_request.num_planning_attempts = 3;
02760 goalA.motion_plan_request.planner_id = std::string("");
02761 goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
02762 goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
02763 arm_navigation_msgs::SimplePoseConstraint desired_pose;
02764 desired_pose.header.frame_id = "torso_lift_link";
02765 desired_pose.link_name = "r_wrist_roll_link";
02766 desired_pose.pose.position.x = x;
02767 desired_pose.pose.position.y = y;
02768 desired_pose.pose.position.z = z;
02769 desired_pose.pose.orientation=tf::createQuaternionMsgFromRollPitchYaw(M_PI/2,M_PI/2,0.0);
02770 desired_pose.absolute_position_tolerance.x = 0.02;
02771 desired_pose.absolute_position_tolerance.y = 0.02;
02772 desired_pose.absolute_position_tolerance.z = 0.04;
02773 desired_pose.absolute_roll_tolerance = 0.04;
02774 desired_pose.absolute_pitch_tolerance = 0.04;
02775 desired_pose.absolute_yaw_tolerance = 0.04;
02776 arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);
02777 if (n_.ok())
02778 {
02779 bool finished_within_time = false;
02780 move_right_arm_client_->sendGoal(goalA);
02781 finished_within_time = move_right_arm_client_->waitForResult(ros::Duration(10.0));
02782 if (!finished_within_time)
02783 {
02784 move_right_arm_client_->cancelGoal();
02785 ROS_INFO("Timed out achieving goal A");
02786 }
02787 else
02788 {
02789 actionlib::SimpleClientGoalState state = move_right_arm_client_->getState();
02790 bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
02791 if(success)
02792 ROS_INFO("Action finished: %s",state.toString().c_str());
02793 else
02794 ROS_INFO("Action failed: %s",state.toString().c_str());
02795 return success;
02796 }
02797
02798 }
02799 return false;
02800 }
02801
02802 void TestArm()
02803 {
02804 ROS_INFO("In TestArm()");
02805
02806 arm_navigation_msgs::MoveArmGoal goalA;
02807 goalA.motion_plan_request.group_name = "right_arm";
02808 goalA.motion_plan_request.num_planning_attempts = 1;
02809 goalA.motion_plan_request.planner_id = std::string("");
02810 goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
02811 goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
02812
02813 arm_navigation_msgs::SimplePoseConstraint desired_pose;
02814 desired_pose.header.frame_id = "torso_lift_link";
02815 desired_pose.link_name = "r_wrist_roll_link";
02816
02817
02818 desired_pose.pose.position.x = 0.75;
02819 desired_pose.pose.position.y = -0.188;
02820 desired_pose.pose.position.z = 0;
02821
02822
02823
02824
02825
02826
02827
02828 desired_pose.pose.orientation=tf::createQuaternionMsgFromRollPitchYaw(0.0,0.0,M_PI/2);
02829
02830
02831 desired_pose.absolute_position_tolerance.x = 0.02;
02832 desired_pose.absolute_position_tolerance.y = 0.02;
02833 desired_pose.absolute_position_tolerance.z = 0.02;
02834
02835 desired_pose.absolute_roll_tolerance = 0.04;
02836 desired_pose.absolute_pitch_tolerance = 0.04;
02837 desired_pose.absolute_yaw_tolerance = 0.04;
02838
02839 arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);
02840
02841 if (n_.ok())
02842 {
02843 bool finished_within_time = false;
02844
02845
02846
02847 move_right_arm_client_->sendGoal(goalA);
02848 finished_within_time = move_right_arm_client_->waitForResult(ros::Duration(200.0));
02849 if (!finished_within_time)
02850 {
02851 move_right_arm_client_->cancelGoal();
02852 ROS_INFO("Timed out achieving goal A");
02853 }
02854 else
02855 {
02856 actionlib::SimpleClientGoalState state = move_right_arm_client_->getState();
02857 bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
02858 if(success)
02859 ROS_INFO("Action finished: %s",state.toString().c_str());
02860 else
02861 ROS_INFO("Action failed: %s",state.toString().c_str());
02862 }
02863 }
02864
02865 }
02866 void TestArm2()
02867 {
02868 ROS_INFO("In TestArm2()");
02869
02870 arm_navigation_msgs::MoveArmGoal goalA;
02871 goalA.motion_plan_request.group_name = "right_arm";
02872 goalA.motion_plan_request.num_planning_attempts = 1;
02873 goalA.motion_plan_request.planner_id = std::string("");
02874 goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
02875 goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
02876
02877 arm_navigation_msgs::SimplePoseConstraint desired_pose;
02878 desired_pose.header.frame_id = "torso_lift_link";
02879 desired_pose.link_name = "r_wrist_roll_link";
02880
02881
02882 desired_pose.pose.position.x = 0.75;
02883 desired_pose.pose.position.y = -0.188;
02884 desired_pose.pose.position.z = 0.0;
02885 desired_pose.pose.orientation=tf::createQuaternionMsgFromRollPitchYaw(0.0,-M_PI/2,0.0);
02886
02887 desired_pose.absolute_position_tolerance.x = 0.02;
02888 desired_pose.absolute_position_tolerance.y = 0.02;
02889 desired_pose.absolute_position_tolerance.z = 0.02;
02890
02891 desired_pose.absolute_roll_tolerance = 0.04;
02892 desired_pose.absolute_pitch_tolerance = 0.04;
02893 desired_pose.absolute_yaw_tolerance = 0.04;
02894
02895 arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);
02896
02897 if (n_.ok())
02898 {
02899 bool finished_within_time = false;
02900
02901
02902
02903 move_right_arm_client_->sendGoal(goalA);
02904 finished_within_time = move_right_arm_client_->waitForResult(ros::Duration(200.0));
02905 if (!finished_within_time)
02906 {
02907 move_right_arm_client_->cancelGoal();
02908 ROS_INFO("Timed out achieving goal A");
02909 }
02910 else
02911 {
02912 actionlib::SimpleClientGoalState state = move_right_arm_client_->getState();
02913 bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
02914 if(success)
02915 ROS_INFO("Action finished: %s",state.toString().c_str());
02916 else
02917 ROS_INFO("Action failed: %s",state.toString().c_str());
02918 }
02919 }
02920
02921 }
02922
02923 void TestArm3()
02924 {
02925 ROS_INFO("In TestArm3()");
02926
02927 arm_navigation_msgs::MoveArmGoal goalA;
02928 goalA.motion_plan_request.group_name = "right_arm";
02929 goalA.motion_plan_request.num_planning_attempts = 1;
02930 goalA.motion_plan_request.planner_id = std::string("");
02931 goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
02932 goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
02933
02934 arm_navigation_msgs::SimplePoseConstraint desired_pose;
02935 desired_pose.header.frame_id = "torso_lift_link";
02936 desired_pose.link_name = "r_wrist_roll_link";
02937
02938
02939 desired_pose.pose.position.x = 0.75;
02940 desired_pose.pose.position.y = -0.188;
02941 desired_pose.pose.position.z = 0.0;
02942 desired_pose.pose.orientation=tf::createQuaternionMsgFromRollPitchYaw(M_PI/2,M_PI/2,0.0);
02943
02944 desired_pose.absolute_position_tolerance.x = 0.02;
02945 desired_pose.absolute_position_tolerance.y = 0.02;
02946 desired_pose.absolute_position_tolerance.z = 0.02;
02947
02948 desired_pose.absolute_roll_tolerance = 0.04;
02949 desired_pose.absolute_pitch_tolerance = 0.04;
02950 desired_pose.absolute_yaw_tolerance = 0.04;
02951
02952 arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);
02953
02954 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].type = 1;
02955 if (n_.ok())
02956 {
02957 bool finished_within_time = false;
02958
02959
02960
02961 move_right_arm_client_->sendGoal(goalA);
02962 finished_within_time = move_right_arm_client_->waitForResult(ros::Duration(200.0));
02963 if (!finished_within_time)
02964 {
02965 move_right_arm_client_->cancelGoal();
02966 ROS_INFO("Timed out achieving goal A");
02967 }
02968 else
02969 {
02970 actionlib::SimpleClientGoalState state = move_right_arm_client_->getState();
02971 bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
02972 if(success)
02973 ROS_INFO("Action finished: %s",state.toString().c_str());
02974 else
02975 ROS_INFO("Action failed: %s",state.toString().c_str());
02976 }
02977 }
02978
02979 }
02980
02981 void KinectCallback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
02982 {
02983
02984
02985
02986
02987 ROS_INFO("PT CLOUD");
02988 XYZPointCloud raw_cloud;
02989 XYZPointCloud cloud;
02990 pcl::fromROSMsg(*cloud_msg,raw_cloud);
02991
02992
02993
02994
02995
02996 tf::StampedTransform transf;
02997 try{
02998 tf_listener.waitForTransform(base_frame_tf, raw_cloud.header.frame_id,
02999 cloud_msg->header.stamp, ros::Duration(2.0));
03000 tf_listener.lookupTransform(base_frame_tf, raw_cloud.header.frame_id,
03001 cloud_msg->header.stamp, transf);
03002 }
03003 catch(tf::TransformException ex)
03004 {
03005 ROS_ERROR("Scene segmentation unable to put kinect data in ptu reference frame due to TF error:%s", ex.what());
03006 return;
03007 }
03008 tf::Vector3 v3 = transf.getOrigin();
03009 tf::Quaternion quat = transf.getRotation();
03010 Eigen::Quaternionf rot(quat.w(), quat.x(), quat.y(), quat.z());
03011 Eigen::Vector3f offset(v3.x(), v3.y(), v3.z());
03012 pcl::transformPointCloud(raw_cloud,cloud,offset,rot);
03013 cloud.header = raw_cloud.header;
03014 cloud.header.frame_id = base_frame_tf;
03015
03016 latest_cloud=cloud;
03017
03018 latest_pt_cloud_ready=true;
03019 return;
03020 }
03021
03022 bool getTablePlane(XYZPointCloud& cloud, book_stacking_msgs::PlaneInfo &pl_info, bool draw_marker)
03023 {
03024 if(filter_spatial)
03025 {
03026 pcl::PassThrough<Point> pass;
03027 pass.setInputCloud(boost::make_shared<pcl::PointCloud<Point> >(cloud));
03028 pass.setFilterFieldName("z");
03029 pass.setFilterLimits(filter_spatial_zmin,filter_spatial_zmax);
03030 pass.filter(cloud);
03031 pass.setInputCloud(boost::make_shared<pcl::PointCloud<Point> >(cloud));
03032 pass.setFilterFieldName("x");
03033 pass.setFilterLimits(filter_spatial_xmin,filter_spatial_xmax);
03034 pass.filter(cloud);
03035 pass.setInputCloud(boost::make_shared<pcl::PointCloud<Point> >(cloud));
03036 pass.setFilterFieldName("y");
03037 pass.setFilterLimits(filter_spatial_ymin,filter_spatial_ymax);
03038 pass.filter(cloud);
03039 }
03040
03041
03042 if(filter_outliers){
03043 pcl::StatisticalOutlierRemoval<Point> sor_filter;
03044 sor_filter.setInputCloud(boost::make_shared<pcl::PointCloud<Point> >(cloud));
03045 sor_filter.setMeanK(filter_outliers_meank);
03046 sor_filter.setStddevMulThresh(filter_outliers_stddev_thresh);
03047 sor_filter.filter(cloud);
03048 }
03049
03050
03051 if(downsample_cloud)
03052 {
03053
03054 pcl::VoxelGrid<Point> grid;
03055 grid.setInputCloud(boost::make_shared<pcl::PointCloud<Point> >(cloud));
03056 grid.setLeafSize(downsample_grid_size_,downsample_grid_size_,downsample_grid_size_);
03057 grid.filter(cloud);
03058 }
03059
03060
03061 sensor_msgs::PointCloud2 filtered_msg;
03062 pcl::toROSMsg(cloud,filtered_msg);
03063
03064 filtered_cloud_pub_.publish(filtered_msg);
03065
03066 book_stacking_msgs::PlaneInfos plane_infos= getPlanesByNormals(cloud,4,true,concave_hull_mode_,use_omp_,plane_distance_thresh_,max_sac_iterations_,sac_probability_,min_plane_inliers_,normal_search_radius_,0.1);
03067 plane_infos.header.stamp=cloud.header.stamp;
03068
03069 if(draw_marker && plane_infos.planes.size()>0)
03070 {
03071 drawPlaneMarkers(plane_infos,plane_marker_pub_,1.0,0.0,0.0);
03072 }
03073
03074 if(plane_infos.planes.size()>0)
03075 {
03076 pl_info=plane_infos.planes[0];
03077 return true;
03078 }
03079 else
03080 {
03081 return false;
03082 }
03083
03084 }
03085
03086
03087 };
03088
03089
03090
03091
03092
03093 int main(int argc, char** argv)
03094 {
03095 ros::init(argc, argv, "BookStackingNode");
03096 book_stacking bs;
03097 ros::spin();
03098 }