00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include <ros/ros.h>
00035 #include <math.h>
00036
00037 #include <interactive_markers/interactive_marker_server.h>
00038 #include <interactive_markers/menu_handler.h>
00039
00040 #include <object_manipulator/tools/mechanism_interface.h>
00041 #include <object_manipulator/tools/msg_helpers.h>
00042
00043 #include <boost/thread.hpp>
00044 #include <tf/transform_listener.h>
00045 #include <boost/thread/recursive_mutex.hpp>
00046
00047 #include <actionlib/client/simple_action_client.h>
00048 #include <actionlib/server/simple_action_server.h>
00049 #include <point_cloud_server/StoreCloudAction.h>
00050 #include <manipulation_msgs/GraspPlanningAction.h>
00051
00052 #include <pr2_object_manipulation_msgs/TestGripperPoseAction.h>
00053 #include <pr2_object_manipulation_msgs/GetGripperPoseAction.h>
00054 #include <pr2_object_manipulation_msgs/CameraFocus.h>
00055
00056 #include <sensor_msgs/point_cloud_conversion.h>
00057
00058 #include "eigen_conversions/eigen_msg.h"
00059
00060 #include "eigen3/Eigen/Geometry"
00061 #include "pcl/io/pcd_io.h"
00062 #include "pcl/point_types.h"
00063 #include "pcl_ros/transforms.h"
00064
00065 #include <household_objects_database_msgs/GetModelDescription.h>
00066 #include <household_objects_database_msgs/GetModelMesh.h>
00067
00068 #include <interactive_marker_helpers/interactive_marker_helpers.h>
00069
00070 using namespace object_manipulator;
00071 using namespace visualization_msgs;
00072 using namespace interactive_markers;
00073 using namespace pr2_object_manipulation_msgs;
00074 using namespace im_helpers;
00075
00076 typedef pcl::PointXYZ PointT;
00077
00079
00080
00083 class GripperPoseAction
00084 {
00085 protected:
00086
00087
00088
00089 std::vector<geometry_msgs::PoseStamped> planner_poses_;
00090 std::vector<PoseState> planner_states_;
00091 std::vector<int> planner_grasp_types_;
00092 int planner_index_;
00093 geometry_msgs::PoseStamped button_marker_pose_;
00094
00095 geometry_msgs::PoseStamped gripper_pose_;
00096 geometry_msgs::PoseStamped control_offset_;
00097 float gripper_opening_;
00098 float gripper_angle_;
00099 pcl::PointCloud<PointT>::Ptr object_cloud_;
00100 bool active_;
00101 bool object_model_;
00102 bool always_call_planner_;
00103 bool show_invalid_grasps_;
00104 bool always_find_alternatives_;
00105 bool gripper_opening_cycling_;
00106
00107 int interface_number_;
00108 int task_number_;
00109 bool testing_planned_grasp_;
00110 int tested_grasp_index_;
00111 bool testing_current_grasp_;
00112 bool testing_alternatives_;
00113
00114 double alternatives_search_dist_;
00115 double alternatives_search_dist_resolution_;
00116 double alternatives_search_angle_;
00117 double alternatives_search_angle_resolution_;
00118 double grasp_plan_region_len_x_;
00119 double grasp_plan_region_len_y_;
00120 double grasp_plan_region_len_z_;
00121
00122 std::string point_cloud_topic_;
00123
00124 PoseState pose_state_;
00125 ros::NodeHandle nh_;
00126 ros::NodeHandle pnh_;
00127 ros::Subscriber sub_seed_;
00128 ros::Subscriber sub_point_seed_;
00129 ros::ServiceClient get_model_mesh_client_;
00130 ros::Timer spin_timer_;
00131 ros::Timer slow_sync_timer_;
00132 InteractiveMarkerServer server_;
00133
00134 ros::Publisher pub_cloud_;
00135 ros::Publisher pub_focus_;
00136
00137 MenuHandler menu_gripper_;
00138 MenuHandler::EntryHandle accept_handle_;
00139 MenuHandler::EntryHandle cancel_handle_;
00140 MenuHandler::EntryHandle focus_handle_;
00141 MenuHandler::EntryHandle alternatives_handle_;
00142
00143 tf::TransformListener tfl_;
00144
00145 object_manipulator::MechanismInterface mechanism_;
00146
00147 object_manipulator::ActionWrapper<pr2_object_manipulation_msgs::TestGripperPoseAction> test_pose_client_;
00148 object_manipulator::ActionWrapper<manipulation_msgs::GraspPlanningAction> grasp_plan_client_;
00149 object_manipulator::ActionWrapper<point_cloud_server::StoreCloudAction> cloud_server_client_;
00150
00151 std::string get_pose_name_;
00152 actionlib::SimpleActionServer<pr2_object_manipulation_msgs::GetGripperPoseAction> get_pose_server_;
00153
00154 public:
00155
00156 GripperPoseAction() :
00157 planner_index_(0),
00158 gripper_angle_(0.541),
00159 object_cloud_(new pcl::PointCloud<PointT>()),
00160 active_(false),
00161 object_model_(false),
00162 pose_state_(UNTESTED),
00163 nh_("/"),
00164 pnh_("~"),
00165 server_(ros::names::resolve("interactive_gripper"), "server 2", false),
00166 tfl_(nh_),
00167 test_pose_client_("test_gripper_pose", true),
00168 grasp_plan_client_("grasp_plan_action", true),
00169 cloud_server_client_("point_cloud_server_action", true),
00170 get_pose_name_(ros::this_node::getName()),
00171 get_pose_server_(nh_, get_pose_name_, false)
00172 {
00173 ROS_INFO( "pr2_interactive_gripper_pose_action is starting up." );
00174
00175 ros::Duration(1.0).sleep();
00176
00177 pnh_.param<bool>("always_call_planner", always_call_planner_, false);
00178 pnh_.param<bool>("always_find_alternatives", always_find_alternatives_, true);
00179 pnh_.param<bool>("show_invalid_grasps", show_invalid_grasps_, false);
00180 pnh_.param<bool>("gripper_opening_cycling", gripper_opening_cycling_, false);
00181
00182 nh_.param<int>("interactive_grasping/interface_number", interface_number_, 0);
00183 if (!interface_number_) ROS_WARN("No interface number specified for grasping study; using default configuration");
00184 else ROS_INFO("Using interface number %d for grasping study", interface_number_);
00185 nh_.param<int>("interactive_grasping/task_number", task_number_, 0);
00186 if (!task_number_) ROS_WARN("No task number specified for grasping study; using default configuration");
00187 else ROS_INFO("Using task number %d for grasping study", task_number_);
00188 pnh_.param<double>("alternatives_search_dist", alternatives_search_dist_, .15);
00189 pnh_.param<double>("alternatives_search_dist_resolution", alternatives_search_dist_resolution_, .005);
00190 pnh_.param<double>("alternatives_search_angle", alternatives_search_angle_, M_PI/2);
00191 pnh_.param<double>("alternatives_search_angle_resolution", alternatives_search_angle_resolution_, M_PI/10);
00192 pnh_.param<double>("grasp_plan_region_len_x", grasp_plan_region_len_x_, 0.3);
00193 pnh_.param<double>("grasp_plan_region_len_y", grasp_plan_region_len_y_, 0.3);
00194 pnh_.param<double>("grasp_plan_region_len_z", grasp_plan_region_len_z_, 0.3);
00195 pnh_.param<std::string>("point_cloud_topic", point_cloud_topic_, "/head_mount_kinect/depth_registered/points");
00196
00197 if(interface_number_ == 4) always_call_planner_ = true;
00198 else if(interface_number_ >= 1) always_call_planner_ = false;
00199
00200
00201 geometry_msgs::PoseStamped ps;
00202 ps.header.frame_id = "/base_link";
00203 server_.insert(makeButtonBox( "test_box", ps, 0.01, false, false));
00204
00205 pub_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>("cloud_debug", 1);
00206 pub_focus_ = nh_.advertise<pr2_object_manipulation_msgs::CameraFocus>("camera_focus", 1);
00207
00208 get_model_mesh_client_ = nh_.serviceClient<household_objects_database_msgs::GetModelMesh>("objects_database_node/get_model_mesh", false);
00209
00210 spin_timer_ = nh_.createTimer(ros::Duration(0.05), boost::bind( &GripperPoseAction::spinOnce, this ) );
00211
00212 sub_seed_ = nh_.subscribe<geometry_msgs::PoseStamped>("/cloud_click_point", 1, boost::bind(&GripperPoseAction::setSeed, this, _1));
00213 sub_point_seed_ = nh_.subscribe<geometry_msgs::PointStamped>("/rviz/set_gripper", 1, boost::bind(&GripperPoseAction::setSeedPoint, this, _1));
00214
00215
00216
00217 initMenus();
00218
00219
00220
00221 get_pose_server_.registerGoalCallback( boost::bind(&GripperPoseAction::goalCB, this));
00222 get_pose_server_.registerPreemptCallback( boost::bind(&GripperPoseAction::preemptCB, this));
00223
00224 get_pose_server_.start();
00225 }
00226
00227 ~GripperPoseAction()
00228 {
00229 }
00230
00231 bool transformGripperPose(const std::string frame_id = "/base_link")
00232 {
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243 return true;
00244 }
00245
00246 void updateGripperOpening()
00247 {
00248 gripper_opening_ = gripper_angle_ * 0.1714;
00249 }
00250
00251 void updateGripperAngle()
00252 {
00253 gripper_angle_ = gripper_opening_ * 5.834;
00254 }
00255
00256 void setSeedPoint(const geometry_msgs::PointStampedConstPtr &seed_point)
00257 {
00258 geometry_msgs::PoseStampedPtr seed(new geometry_msgs::PoseStamped());
00259 seed->header = seed_point->header;
00260 seed->pose.orientation = geometry_msgs::Quaternion();
00261 seed->pose.orientation.w = 1.0;
00262 seed->pose.position = seed_point->point;
00263 setSeed(seed);
00264
00265 }
00266
00267 void setSeed(const geometry_msgs::PoseStampedConstPtr &seed)
00268 {
00269 if(!active_)
00270 {
00271 ROS_DEBUG("No active gripper goal pose.");
00272 return;
00273 }
00274 ROS_DEBUG("Setting seed.");
00275 geometry_msgs::PoseStamped ps = *seed;
00276 ROS_DEBUG_STREAM("Input seed was \n" << ps);
00277 tf::Pose pose;
00278 tf::poseMsgToTF(ps.pose, pose);
00279 tf::Quaternion q = pose.getRotation();
00280 tf::Matrix3x3 rot(q);
00281 tf::Matrix3x3 perm( 0, 0, 1,
00282 0, 1, 0,
00283 -1, 0, 0);
00284 (rot*perm).getRotation(q);
00285
00286 pose.setRotation(q);
00287
00288 if(object_model_) pose = pose*tf::Transform(tf::Quaternion(tf::Vector3(0,1,0), M_PI/2.0), tf::Vector3(0,0,0)).inverse();
00289
00290 tf::poseTFToMsg(pose, ps.pose);
00291
00292
00293
00294 gripper_pose_ = toWrist(ps);
00295
00296
00297
00298
00299
00300
00301 pose_state_ = UNTESTED;
00302 eraseAllGraspMarkers();
00303
00304 testing_planned_grasp_ = false;
00305 testing_alternatives_ = false;
00306 testing_current_grasp_ = true;
00307 if(always_call_planner_)
00308 {
00309 ROS_INFO("Always call planner is on");
00310 graspPlanCB();
00311 }
00312 else
00313 {
00314 initMarkers();
00315 testPose(gripper_pose_, gripper_opening_);
00316 }
00317
00318 focusCB();
00319 }
00320
00322 void setIdle(){
00323 active_ = false;
00324 server_.erase("ghosted_gripper");
00325 server_.erase("gripper_controls");
00326 server_.erase("object_cloud");
00327 eraseAllGraspMarkers();
00328 planner_states_.clear();
00329 planner_poses_.clear();
00330 }
00331
00332
00334 void goalCB()
00335 {
00336 active_ = true;
00337 object_model_ = false;
00338 planner_index_ = 0;
00339 ROS_INFO("Ghosted gripper called");
00340 pr2_object_manipulation_msgs::GetGripperPoseGoal goal = *get_pose_server_.acceptNewGoal();
00341 object_cloud_.reset( new pcl::PointCloud<PointT>());
00342 pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
00343 control_offset_.pose = object_manipulator::msg::createPoseMsg(tf::Pose(tf::Quaternion::getIdentity(), tf::Vector3(0.15,0,0)));
00344
00345
00346 if(interface_number_ == 0)
00347 {
00348 pnh_.param<bool>("always_call_planner", always_call_planner_, false);
00349 pnh_.param<bool>("always_find_alternatives", always_find_alternatives_, true);
00350 pnh_.param<bool>("show_invalid_grasps", show_invalid_grasps_, true);
00351 pnh_.param<bool>("gripper_opening_cycling", gripper_opening_cycling_, false);
00352 ROS_INFO("always_call_planner: %d, always_find_alt: %d, show_invalid: %d, gripper_opening_cycle: %d",
00353 (int)always_call_planner_, (int)always_find_alternatives_, (int)show_invalid_grasps_, (int)gripper_opening_cycling_);
00354 }
00355
00356 if( !goal.object.cluster.points.empty() ||
00357 !goal.object.potential_models.empty() )
00358 {
00359 ROS_INFO("Goal object contains %d cluster and %d models.",
00360 !goal.object.cluster.points.empty(), (int)goal.object.potential_models.size() );
00361
00362 geometry_msgs::PoseStamped grasp_pose = goal.grasp.grasp_pose;
00363 grasp_pose.header.stamp = ros::Time(0);
00364 if (grasp_pose.header.frame_id != goal.object.reference_frame_id)
00365 {
00366 try
00367 {
00368 tfl_.transformPose(goal.object.reference_frame_id, grasp_pose, grasp_pose);
00369 }
00370 catch(...)
00371 {
00372 ROS_ERROR("Interactive pose action: can not transfom grasp pose from frame %s into frame %s",
00373 grasp_pose.header.frame_id.c_str(), goal.object.reference_frame_id.c_str());
00374 }
00375 }
00376 try
00377 {
00378 ROS_INFO("Converting to reference_frame...");
00379 mechanism_.convertGraspableObjectComponentsToFrame(goal.object, goal.object.reference_frame_id);
00380
00381 bool use_cloud = goal.object.potential_models.empty();
00382
00383
00384 if(!use_cloud)
00385 {
00386 ROS_INFO("Goal contains object model; looking for model mesh...");
00387
00388 shape_msgs::Mesh mesh;
00389 if(!getModelMesh(goal.object.potential_models[0].model_id, mesh))
00390 {
00391 ROS_INFO("Unable to get database model, continuing with cluster.");
00392 use_cloud = true;
00393 }
00394
00395 if(!use_cloud)
00396 {
00397 object_model_ = true;
00398 for(unsigned int i = 0; i < mesh.vertices.size(); i++)
00399 {
00400 PointT pt;
00401 pt.x = mesh.vertices[i].x;
00402 pt.y = mesh.vertices[i].y;
00403 pt.z = mesh.vertices[i].z;
00404 cloud->points.push_back(pt);
00405 }
00406 cloud->width = cloud->points.size();
00407 cloud->height = 1;
00408 cloud->is_dense = false;
00409 cloud->header.frame_id = goal.object.reference_frame_id;
00410
00411 geometry_msgs::Pose &m = goal.object.potential_models[0].pose.pose;
00412 Eigen::Affine3f affine = Eigen::Translation3f(m.position.x,
00413 m.position.y,
00414 m.position.z) *
00415 Eigen::Quaternionf(m.orientation.w,
00416 m.orientation.x,
00417 m.orientation.y,
00418 m.orientation.z);
00419 pcl::transformPointCloud(*cloud, *cloud, affine);
00420
00421 tf::Transform T_o, T_g;
00422 tf::poseMsgToTF(goal.object.potential_models[0].pose.pose, T_o);
00423 tf::poseMsgToTF(grasp_pose.pose, T_g);
00424 tf::Transform T = T_g.inverse()*T_o;
00425 tf::poseTFToMsg(T, control_offset_.pose);
00426
00427 }
00428 }
00429
00430 if(use_cloud)
00431 {
00432
00433
00434 sensor_msgs::PointCloud2 converted_cloud;
00435 sensor_msgs::convertPointCloudToPointCloud2 (goal.object.cluster, converted_cloud);
00436
00437 pcl::fromROSMsg(converted_cloud, *cloud);
00438 }
00439
00440 geometry_msgs::Pose &m = grasp_pose.pose;
00441 Eigen::Affine3f affine = Eigen::Translation3f(m.position.x,
00442 m.position.y,
00443 m.position.z) *
00444 Eigen::Quaternionf(m.orientation.w,
00445 m.orientation.x,
00446 m.orientation.y,
00447 m.orientation.z);
00448 affine = affine.inverse();
00449 pcl::transformPointCloud(*cloud, *object_cloud_, affine);
00450 }
00451 catch(...){
00452 ROS_ERROR("%s: Error converting graspable object to reference frame id [%s]!",
00453 get_pose_name_.c_str(), goal.object.reference_frame_id.c_str());
00454 }
00455 }
00456 if (goal.gripper_pose.pose.orientation.x == 0 &&
00457 goal.gripper_pose.pose.orientation.y == 0 &&
00458 goal.gripper_pose.pose.orientation.z == 0 &&
00459 goal.gripper_pose.pose.orientation.w == 0 )
00460 {
00461 ROS_INFO("Empty pose passed in; using default");
00462 gripper_pose_ = getDefaultPose(goal.arm_name);
00463
00464
00465
00466 gripper_opening_ = 0.086;
00467 updateGripperAngle();
00468 }
00469 else
00470 {
00471 gripper_pose_ = goal.gripper_pose;
00472
00473 gripper_opening_ = goal.gripper_opening;
00474 updateGripperAngle();
00475 }
00476 if (get_pose_server_.isPreemptRequested())
00477 {
00478 if(test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::ACTIVE ||
00479 test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::PENDING )
00480 {
00481 test_pose_client_.client().cancelAllGoals();
00482 }
00483 get_pose_server_.setPreempted();
00484 setIdle();
00485 return;
00486 }
00487
00488 pose_state_ = UNTESTED;
00489 initMarkers();
00490 pr2_object_manipulation_msgs::TestGripperPoseGoal test_goal;
00491 test_goal.gripper_poses.push_back(gripper_pose_);
00492 test_goal.gripper_openings.push_back(gripper_opening_);
00493 test_pose_client_.client().sendGoal( test_goal, boost::bind(&GripperPoseAction::testGripperResultCallback, this, _1, _2));
00494 }
00495
00496
00498 void preemptCB()
00499 {
00500 ROS_INFO("%s: Preempted", get_pose_name_.c_str());
00501 test_pose_client_.client().cancelAllGoals();
00502 grasp_plan_client_.client().cancelGoal();
00503 get_pose_server_.setPreempted();
00504 setIdle();
00505 }
00506
00508 geometry_msgs::PoseStamped toWrist(const geometry_msgs::PoseStamped &ps)
00509 {
00510 geometry_msgs::PoseStamped out;
00511 out.header = ps.header;
00512 tf::Transform T, P;
00513 tf::poseMsgToTF(ps.pose, P);
00514 tf::poseMsgToTF(control_offset_.pose, T);
00515 tf::poseTFToMsg( P*T.inverse(), out.pose);
00516
00517 return out;
00518 }
00519
00521 geometry_msgs::PoseStamped fromWrist(const geometry_msgs::PoseStamped &ps)
00522 {
00523 geometry_msgs::PoseStamped out;
00524 out.header = ps.header;
00525 tf::Transform T, P;
00526 tf::poseMsgToTF(ps.pose, P);
00527 tf::poseMsgToTF(control_offset_.pose, T);
00528 tf::poseTFToMsg( P*T, out.pose);
00529
00530 return out;
00531 }
00532
00533
00536 void updatePoses()
00537 {
00538 server_.setPose("ghosted_gripper", gripper_pose_.pose, gripper_pose_.header);
00539 server_.setPose("object_cloud", gripper_pose_.pose, gripper_pose_.header);
00540 }
00541
00542 geometry_msgs::PoseStamped getDefaultPose(std::string arm_name)
00543 {
00544 ros::Time now = ros::Time(0);
00545 tfl_.waitForTransform("r_gripper_tool_frame","/base_link", now, ros::Duration(2.0));
00546 tf::StampedTransform stamped;
00547 geometry_msgs::TransformStamped ts;
00548 geometry_msgs::PoseStamped ps;
00549 std::string arm_frame;
00550 if (arm_name == "right_arm") arm_frame="r_wrist_roll_link";
00551 else if (arm_name == "left_arm") arm_frame="l_wrist_roll_link";
00552 else
00553 {
00554 arm_frame="r_wrist_roll_link";
00555 ROS_ERROR("Unknown arm name passed to ghosted gripper");
00556 }
00557 tfl_.lookupTransform("/base_link", arm_frame, now, stamped);
00558 tf::transformStampedTFToMsg(stamped, ts);
00559 ps = msg::createPoseStampedMsg(ts);
00560 return ps;
00561 }
00562
00563
00564
00565
00568 void initMarkers()
00569 {
00570 initGripperMarker();
00571 initObjectMarker();
00572 initGripperControl();
00573 }
00574
00575 void initGraspMarkers()
00576 {
00577 for(unsigned int i = 0; i < planner_poses_.size(); i++)
00578 {
00579
00580 std::stringstream ss;
00581 ss << "grasp_" << i;
00582 server_.insert(makeGraspMarker(ss.str().c_str(), planner_poses_[i], 1.5, planner_states_[i]));
00583 server_.setCallback(ss.str(), boost::bind( &GripperPoseAction::selectGrasp, this, _1),
00584 visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN );
00585
00586
00587 }
00588 }
00589
00590 void eraseAllGraspMarkers()
00591 {
00592 for(unsigned int i = 0; i < planner_poses_.size(); i++)
00593 {
00594
00595 std::stringstream ss;
00596 ss << "grasp_" << i;
00597 server_.erase(ss.str().c_str());
00598 }
00599 server_.erase("grasp_toggle");
00600 planner_index_ = 0;
00601 planner_poses_.resize(0);
00602 planner_states_.resize(0);
00603 planner_grasp_types_.resize(0);
00604
00605 }
00606
00607 void selectNextGrasp()
00608 {
00609 selectGraspIndex( planner_index_+1 );
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621 }
00622
00623 void selectGraspIndex(int index)
00624 {
00625 planner_index_ = index;
00626 if( planner_index_ >= (int)planner_poses_.size() ) planner_index_ = 0;
00627 gripper_pose_ = planner_poses_[ planner_index_ ];
00628 pose_state_ = planner_states_[ planner_index_ ];
00629 ROS_INFO("Selecting grasp %d", planner_index_);
00630 initMarkers();
00631
00632 if(pose_state_ == UNTESTED)
00633 {
00634 testing_planned_grasp_ = false;
00635 testing_alternatives_ = false;
00636 tested_grasp_index_ = planner_index_;
00637 testing_current_grasp_ = true;
00638 testPose(gripper_pose_, gripper_opening_);
00639 }
00640 }
00641
00642 void selectGrasp( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00643 {
00644 int index = atoi(feedback->marker_name.substr(6).c_str());
00645 if( index >= (int)planner_poses_.size() ) return;
00646 selectGraspIndex(index);
00647 }
00648
00649 void initButtonMarker()
00650 {
00651 button_marker_pose_.header.stamp = ros::Time(0);
00652 int num = 0;
00653 if(planner_poses_.size()) num = planner_index_ + 1;
00654
00655
00656
00657
00658
00659
00660
00661
00662 server_.insert(makeListControl("grasp_toggle", button_marker_pose_, num, planner_poses_.size(), 0.3));
00663 server_.setCallback("grasp_toggle", boost::bind( &GripperPoseAction::cycleGrasps, this, _1),
00664 visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN );
00665 }
00666
00667 void initObjectMarker()
00668 {
00669 if(object_cloud_->points.size())
00670 {
00671 server_.insert(makeCloudMarker( "object_cloud", gripper_pose_, 0.004,
00672 object_manipulator::msg::createColorMsg(0.2, 0.8, 0.2,1.0) ));
00673 }
00674 }
00675
00676 void initGripperMarker()
00677 {
00678 float r,g,b;
00679 switch(pose_state_)
00680 {
00681 case INVALID:
00682 r = 1.0; g = 0.2; b = 0.2;
00683 break;
00684 case VALID:
00685 r = 0.2; g = 1.0; b = 0.2;
00686 break;
00687 default:
00688 r = 0.5; g = 0.5; b = 0.5;
00689 }
00690 std_msgs::ColorRGBA color = object_manipulator::msg::createColorMsg(r,g,b,1.0);
00691
00692 server_.insert(makeGripperMarker( "ghosted_gripper", gripper_pose_, 1.01, gripper_angle_, false, color),
00693 boost::bind( &GripperPoseAction::gripperClickCB, this, _1));
00694 menu_gripper_.apply(server_, "ghosted_gripper");
00695 }
00696
00697 void initGripperControl()
00698 {
00699 geometry_msgs::PoseStamped ps = fromWrist(gripper_pose_);
00700 ps.header.stamp = ros::Time(0);
00701 server_.insert(make6DofMarker( "gripper_controls", ps, 0.2, false, false),
00702 boost::bind( &GripperPoseAction::updateGripper, this, _1));
00703 menu_gripper_.apply(server_, "gripper_controls");
00704 }
00705
00706
00707 protected:
00708
00709
00711 void spinOnce()
00712 {
00713
00714 server_.applyChanges();
00715 }
00716
00717 void slowSync()
00718 {
00719 }
00720
00722 void testGripperResultCallback(const actionlib::SimpleClientGoalState& state,
00723 const pr2_object_manipulation_msgs::TestGripperPoseResultConstPtr &result)
00724 {
00725 if (result->valid.empty())
00726 {
00727 ROS_ERROR("Test gripper pose returned with empty result list");
00728 return;
00729 }
00730 if(state.state_ == state.SUCCEEDED)
00731 {
00732 ROS_INFO("Test pose action returned with result %d", (int)result->valid[0]);
00733
00734 if (testing_planned_grasp_)
00735 {
00736 if (!planner_states_.empty())
00737 {
00738 if(planner_states_.size() == result->valid.size())
00739 {
00740 int dim_index = 0;
00741 bool dim_grasp_found = false;
00742 if(testing_alternatives_) dim_index = planner_grasp_types_[0];
00743 std::vector<geometry_msgs::PoseStamped> planner_poses_temp;
00744 std::vector<PoseState> planner_states_temp;
00745
00746
00747 for (size_t i=0; i<result->valid.size(); i++)
00748 {
00749 PoseState pose_state = INVALID;
00750
00751
00752 if (result->valid[i])
00753 {
00754 pose_state = VALID;
00755
00756
00757 if (testing_alternatives_){
00758 if(planner_grasp_types_[i] == dim_index)
00759 {
00760
00761 if(!dim_grasp_found)
00762 {
00763 dim_grasp_found = true;
00764 }
00765
00766
00767 else pose_state = UNTESTED;
00768 }
00769
00770
00771 else
00772 {
00773 dim_index = planner_grasp_types_[i];
00774 dim_grasp_found = true;
00775 }
00776 }
00777 }
00778
00779
00780 else
00781 {
00782 if(testing_alternatives_)
00783 {
00784
00785 pose_state = UNTESTED;
00786
00787
00788 if(planner_grasp_types_[i] != dim_index){
00789 dim_index = planner_grasp_types_[i];
00790 dim_grasp_found = false;
00791 }
00792 }
00793 }
00794
00795
00796 if(pose_state == UNTESTED || (pose_state == INVALID && !show_invalid_grasps_)) continue;
00797 planner_states_temp.push_back(pose_state);
00798 planner_poses_temp.push_back(planner_poses_[i]);
00799 }
00800
00801
00802 eraseAllGraspMarkers();
00803
00804
00805 planner_states_ = planner_states_temp;
00806 planner_poses_ = planner_poses_temp;
00807 ROS_INFO("planner_states has %zd entries", planner_states_.size());
00808
00809
00810 initGraspMarkers();
00811 }
00812 else
00813 {
00814 ROS_ERROR("planner_states_ size did not match result->valid size!");
00815 }
00816 }
00817 else
00818 {
00819 ROS_ERROR("planner_states_ was empty!");
00820 }
00821
00822
00823 if(planner_poses_.size() > 1 || testing_alternatives_) initButtonMarker();
00824 }
00825
00826
00827 if (testing_current_grasp_)
00828 {
00829 PoseState pose_state = INVALID;
00830 if (result->valid[0]) pose_state = VALID;
00831 pose_state_ = pose_state;
00832 initGripperMarker();
00833
00834
00835 if(always_find_alternatives_ && pose_state == INVALID)
00836 {
00837 alternativesCB();
00838 }
00839 }
00840 }
00841 else
00842 {
00843 ROS_WARN("Test pose action did not succeed; state = %d", (int)state.state_);
00844 }
00845 }
00846
00848 void graspPlanCB()
00849 {
00850 eraseAllGraspMarkers();
00851
00852 point_cloud_server::StoreCloudGoal cloud_goal;
00853 cloud_goal.action = cloud_goal.GET;
00854 cloud_goal.name = "interactive_manipulation_snapshot";
00855 cloud_goal.topic = point_cloud_topic_;
00856 cloud_server_client_.client().sendGoal(cloud_goal);
00857 if(!cloud_server_client_.client().waitForResult(ros::Duration(3.0)))
00858 {
00859 ROS_WARN("Interactive gripper pose action: timed out while waiting for cloud from server!");
00860 return;
00861 }
00862 if(cloud_server_client_.client().getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00863 {
00864 ROS_WARN("Interactive gripper pose action: getting cloud did not succeed!");
00865 return;
00866 }
00867 manipulation_msgs::GraspPlanningGoal plan_goal;
00868 plan_goal.target.region.cloud = cloud_server_client_.client().getResult()->cloud;
00869 plan_goal.target.region.roi_box_pose = fromWrist(gripper_pose_);
00870 plan_goal.target.region.roi_box_dims = object_manipulator::msg::createVector3Msg(grasp_plan_region_len_x_,
00871 grasp_plan_region_len_y_,
00872 grasp_plan_region_len_z_);
00873 int cloud_size = plan_goal.target.region.cloud.width * plan_goal.target.region.cloud.height;
00874 plan_goal.target.reference_frame_id = gripper_pose_.header.frame_id;
00875 manipulation_msgs::Grasp seed;
00876 seed.grasp_pose = gripper_pose_;
00877 plan_goal.grasps_to_evaluate.push_back(seed);
00878
00879 ROS_DEBUG_STREAM("Requesting adjustment on cloud with " << cloud_size << " points, pose \n" << seed);
00880
00881 grasp_plan_client_.client().sendGoal( plan_goal, boost::bind(&GripperPoseAction::graspPlanResultCB, this, _1, _2));
00882
00883 button_marker_pose_ = gripper_pose_;
00884 button_marker_pose_.header.stamp = ros::Time(0);
00885 button_marker_pose_.pose.orientation = geometry_msgs::Quaternion();
00886 button_marker_pose_.pose.orientation.w = 1;
00887 button_marker_pose_.pose.position.z -= 0.2;
00888
00889 server_.erase("gripper_controls");
00890 server_.erase("ghosted_gripper");
00891 }
00892
00894 void graspPlanResultCB(const actionlib::SimpleClientGoalState& state,
00895 const manipulation_msgs::GraspPlanningResultConstPtr &result)
00896 {
00897 planner_index_ = 0;
00898
00899
00900 if(state.state_ == state.SUCCEEDED)
00901 {
00902 ROS_INFO("Grasp plan action succeeded.");
00903
00904 int num = result->grasps.size();
00905
00906 planner_poses_.resize(num + 1);
00907 planner_states_.resize(num + 1);
00908
00909 for(int i = 0; i < num; i++)
00910 {
00911 planner_poses_[i].pose = result->grasps[i].grasp_pose.pose;
00912 planner_poses_[i].header = gripper_pose_.header;
00913 planner_poses_[i].header.stamp = ros::Time(0);
00914 planner_states_[i] = UNTESTED;
00915 }
00916 planner_poses_[num] = gripper_pose_;
00917 planner_poses_[num].header.stamp = ros::Time(0);
00918 planner_states_[num] = UNTESTED;
00919 initGraspMarkers();
00920 testing_planned_grasp_ = true;
00921 testing_alternatives_ = false;
00922 tested_grasp_index_ = 0;
00923 testing_current_grasp_ = false;
00924 testPoses(planner_poses_, gripper_opening_);
00925 }
00926
00927
00928 else
00929 {
00930 ROS_WARN("Grasp plan action did not succeed; state = %d", (int)state.state_);
00931 planner_poses_.resize(1);
00932 planner_states_.resize(1);
00933 planner_poses_[0] = gripper_pose_;
00934 planner_poses_[0].header.stamp = ros::Time(0);
00935 planner_states_[0] = UNTESTED;
00936 gripper_pose_ = planner_poses_[0];
00937 initMarkers();
00938
00939 pose_state_ = UNTESTED;
00940 testing_current_grasp_ = true;
00941 testing_planned_grasp_ = false;
00942 testing_alternatives_ = false;
00943 tested_grasp_index_ = 0;
00944 testPose(gripper_pose_, gripper_opening_);
00945 }
00946 }
00947
00949 void cycleGrasps(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
00950 {
00951 selectNextGrasp();
00952 initButtonMarker();
00953 }
00954
00956 void acceptCB()
00957 {
00958 if( pose_state_ == VALID )
00959 {
00960 if(test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::ACTIVE ||
00961 test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::PENDING )
00962 {
00963 test_pose_client_.client().cancelGoal();
00964 }
00965 setIdle();
00966 pr2_object_manipulation_msgs::GetGripperPoseResult result;
00967 result.gripper_pose = gripper_pose_;
00968 result.gripper_opening = gripper_opening_;
00969 get_pose_server_.setSucceeded(result);
00970 }
00971 }
00972
00974 void cancelCB()
00975 {
00976 if(test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::ACTIVE ||
00977 test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::PENDING )
00978 {
00979 test_pose_client_.client().cancelGoal();
00980 }
00981 get_pose_server_.setAborted();
00982 setIdle();
00983 }
00984
00986 void focusCB()
00987 {
00988 pr2_object_manipulation_msgs::CameraFocus msg;
00989 msg.focal_point.point = gripper_pose_.pose.position;
00990 msg.focal_point.header = gripper_pose_.header;
00991 pub_focus_.publish(msg);
00992 }
00993
00995 std::vector<geometry_msgs::PoseStamped> generatePosesAlongDir(tf::Pose pose, tf::Vector3 axis,
00996 double min_shift, double max_shift, double resolution, std::string frame_id)
00997 {
00998 std::vector<geometry_msgs::PoseStamped> shifted_poses;
00999
01000 for(double dist=min_shift; dist<=max_shift; dist += resolution)
01001 {
01002 if(dist < 1e-6 && dist > -1e-6) continue;
01003 tf::Pose shifted_pose = pose*tf::Transform(tf::createIdentityQuaternion(), axis*dist);
01004 geometry_msgs::PoseStamped shifted_pose_stamped;
01005 tf::poseTFToMsg(shifted_pose, shifted_pose_stamped.pose);
01006 shifted_pose_stamped.header.frame_id = frame_id;
01007 if(fabs(min_shift) > fabs(max_shift)){
01008 shifted_poses.insert(shifted_poses.begin(), shifted_pose_stamped);
01009 }
01010 else shifted_poses.push_back(shifted_pose_stamped);
01011 }
01012 return shifted_poses;
01013 }
01014
01016 std::vector<geometry_msgs::PoseStamped> generateRotatedPoses(tf::Pose pose, tf::Vector3 axis,
01017 double min_rot, double max_rot, double resolution, std::string frame_id)
01018 {
01019 std::vector<geometry_msgs::PoseStamped> rotated_poses;
01020
01021 for(double angle=min_rot; angle<=max_rot; angle += resolution)
01022 {
01023 if(angle < 1e-6 && angle > -1e-6) continue;
01024 tf::Quaternion rot = tf::Quaternion(axis, angle);
01025 tf::Pose rotated_pose = pose*tf::Transform(rot, tf::Vector3(0,0,0));
01026 geometry_msgs::PoseStamped rotated_pose_stamped;
01027 tf::poseTFToMsg(rotated_pose, rotated_pose_stamped.pose);
01028 rotated_pose_stamped.header.frame_id = frame_id;
01029 if(fabs(min_rot) > fabs(max_rot)){
01030 rotated_poses.insert(rotated_poses.begin(), rotated_pose_stamped);
01031 }
01032 else rotated_poses.push_back(rotated_pose_stamped);
01033 }
01034 return rotated_poses;
01035 }
01036
01038 void alternativesCB()
01039 {
01040 if(test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::ACTIVE ||
01041 test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::PENDING )
01042 {
01043 test_pose_client_.client().cancelGoal();
01044 }
01045 eraseAllGraspMarkers();
01046
01047
01048 geometry_msgs::PoseStamped fingertips_pose = fromWrist(gripper_pose_);
01049 tf::Pose pose;
01050 tf::poseMsgToTF(fingertips_pose.pose, pose);
01051 std::vector<geometry_msgs::PoseStamped> poses;
01052 std::string frame_id = gripper_pose_.header.frame_id;
01053 tf::Vector3 axes[3] = {tf::Vector3(1,0,0), tf::Vector3(0,1,0), tf::Vector3(0,0,1)};
01054 std::vector<geometry_msgs::PoseStamped> some_poses;
01055 std::vector<int> some_types;
01056
01057
01058 for(int ind=0; ind<3; ind++)
01059 {
01060 some_poses = generatePosesAlongDir(pose, axes[ind], 0, alternatives_search_dist_, alternatives_search_dist_resolution_, frame_id);
01061 poses.insert(poses.end(), some_poses.begin(), some_poses.end());
01062 planner_grasp_types_.insert(planner_grasp_types_.end(), some_poses.size(), ind*2);
01063 some_poses = generatePosesAlongDir(pose, axes[ind], -alternatives_search_dist_, 0, alternatives_search_dist_resolution_, frame_id);
01064 poses.insert(poses.end(), some_poses.begin(), some_poses.end());
01065 planner_grasp_types_.insert(planner_grasp_types_.end(), some_poses.size(), ind*2+1);
01066 }
01067
01068
01069 for(int ind=0; ind<3; ind++)
01070 {
01071 some_poses = generateRotatedPoses(pose, axes[ind], 0, alternatives_search_angle_, alternatives_search_angle_resolution_, frame_id);
01072 poses.insert(poses.end(), some_poses.begin(), some_poses.end());
01073 planner_grasp_types_.insert(planner_grasp_types_.end(), some_poses.size(), ind*2+6);
01074 some_poses = generateRotatedPoses(pose, axes[ind], -alternatives_search_angle_, 0, alternatives_search_angle_resolution_, frame_id);
01075 poses.insert(poses.end(), some_poses.begin(), some_poses.end());
01076 planner_grasp_types_.insert(planner_grasp_types_.end(), some_poses.size(), ind*2+7);
01077 }
01078
01079
01080 for(size_t i=0; i<poses.size(); i++)
01081 {
01082 planner_poses_.push_back(toWrist(poses[i]));
01083 planner_states_.push_back(UNTESTED);
01084 }
01085
01086
01087 planner_poses_.push_back(gripper_pose_);
01088 planner_states_.push_back(UNTESTED);
01089 planner_grasp_types_.push_back(-1);
01090
01091 button_marker_pose_ = gripper_pose_;
01092 button_marker_pose_.header.stamp = ros::Time(0);
01093 button_marker_pose_.pose.orientation = geometry_msgs::Quaternion();
01094 button_marker_pose_.pose.orientation.w = 1;
01095 button_marker_pose_.pose.position.z -= 0.2;
01096
01097
01098 testing_planned_grasp_ = true;
01099 testing_alternatives_ = true;
01100 tested_grasp_index_ = 0;
01101 testing_current_grasp_ = false;
01102
01103 testPoses(planner_poses_, gripper_opening_);
01104 }
01105
01107 void gripperClickCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
01108 {
01109
01110 if(interface_number_ != 0 || !gripper_opening_cycling_) return;
01111
01112 ros::Time now = ros::Time(0);
01113
01114 switch ( feedback->event_type )
01115 {
01116 case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
01117
01118 float max_gripper_angle = 0.541;
01119 gripper_angle_ -= 0.12;
01120 if(gripper_angle_ < 0.04)
01121 gripper_angle_ = max_gripper_angle;
01122 updateGripperOpening();
01123 initGripperMarker();
01124 ROS_DEBUG( "Gripper opening = %.2f, angle = %.2f", gripper_opening_, gripper_angle_);
01125 break;
01126 }
01127 }
01128
01130 void testPose(geometry_msgs::PoseStamped pose, float opening)
01131 {
01132 eraseAllGraspMarkers();
01133 pr2_object_manipulation_msgs::TestGripperPoseGoal goal;
01134 goal.gripper_poses.push_back(pose);
01135 goal.gripper_openings.push_back(opening);
01136 test_pose_client_.client().sendGoal( goal, boost::bind(&GripperPoseAction::testGripperResultCallback, this, _1, _2));
01137 }
01138
01140 void testPoses(std::vector<geometry_msgs::PoseStamped> poses, float opening)
01141 {
01142
01143 pr2_object_manipulation_msgs::TestGripperPoseGoal goal;
01144 for(size_t i=0; i<poses.size(); i++){
01145 goal.gripper_poses.push_back(poses[i]);
01146 goal.gripper_openings.push_back(opening);
01147 }
01148 test_pose_client_.client().sendGoal( goal, boost::bind(&GripperPoseAction::testGripperResultCallback, this, _1, _2));
01149 }
01150
01152 void updateGripper( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
01153 {
01154 ros::Time now = ros::Time(0);
01155
01156 switch ( feedback->event_type )
01157 {
01158 case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
01159 ROS_DEBUG_STREAM( "Marker is being moved, stored pose is invalidated." );
01160 test_pose_client_.client().cancelAllGoals();
01161 pose_state_ = UNTESTED;
01162 eraseAllGraspMarkers();
01163 break;
01164 case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
01165 ROS_DEBUG_STREAM( "Marker was released, storing pose and checking." );
01166 gripper_pose_.pose = feedback->pose;
01167 gripper_pose_.header = feedback->header;
01168 gripper_pose_ = toWrist(gripper_pose_);
01169 initMarkers();
01170 testing_planned_grasp_ = false;
01171 testing_alternatives_ = false;
01172 testing_current_grasp_ = true;
01173 testPose(gripper_pose_, gripper_opening_);
01174 break;
01175 case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
01176 ROS_DEBUG_STREAM("POSE_UPDATE in frame " << feedback->header.frame_id << std::endl << feedback->pose);
01177 gripper_pose_.pose = feedback->pose;
01178 gripper_pose_.header = feedback->header;
01179 gripper_pose_ = toWrist(gripper_pose_);
01180 updatePoses();
01181 break;
01182 }
01183 }
01184
01186 void initMenus()
01187 {
01188 accept_handle_ = menu_gripper_.insert("Accept", boost::bind( &GripperPoseAction::acceptCB, this ) );
01189 cancel_handle_ = menu_gripper_.insert("Cancel", boost::bind( &GripperPoseAction::cancelCB, this ) );
01190 focus_handle_ = menu_gripper_.insert("Focus camera", boost::bind( &GripperPoseAction::focusCB, this ) );
01191 alternatives_handle_ = menu_gripper_.insert("Find alternatives", boost::bind( &GripperPoseAction::alternativesCB, this ) );
01192 if(interface_number_ == 0 || interface_number_ == 4)
01193 {
01194 menu_gripper_.insert("Run Grasp Planner", boost::bind( &GripperPoseAction::graspPlanCB, this ) );
01195 }
01196 }
01197
01199 bool getModelMesh( int model_id, shape_msgs::Mesh& mesh )
01200 {
01201 household_objects_database_msgs::GetModelMesh mesh_srv;
01202
01203 mesh_srv.request.model_id = model_id;
01204 if ( !get_model_mesh_client_.call(mesh_srv) )
01205 {
01206 ROS_ERROR("Failed to call get model mesh service");
01207 return false;
01208 }
01209
01210 if (mesh_srv.response.return_code.code != household_objects_database_msgs::DatabaseReturnCode::SUCCESS)
01211 {
01212 ROS_ERROR("Model mesh service reports an error (code %d)", mesh_srv.response.return_code.code);
01213 return false;
01214 }
01215
01216 mesh = mesh_srv.response.mesh;
01217 return true;
01218 }
01219
01220
01221
01223 visualization_msgs::InteractiveMarker makeCloudMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
01224 float point_size, std_msgs::ColorRGBA color)
01225 {
01226 InteractiveMarker int_marker;
01227 int_marker.name = name;
01228 int_marker.pose = stamped.pose;
01229 int_marker.header = stamped.header;
01230
01231 Marker marker;
01232 marker.color = color;
01233 marker.frame_locked = false;
01234
01235 if(object_cloud_->points.size())
01236 {
01237 marker.scale.x = point_size;
01238 marker.scale.y = point_size;
01239 marker.scale.z = point_size;
01240 marker.type = visualization_msgs::Marker::SPHERE_LIST;
01241
01242 int num_points = object_cloud_->points.size();
01243 marker.points.resize( num_points );
01244
01245
01246 ROS_DEBUG_STREAM( "Adding point cluster. #points=" << object_cloud_->points.size() );
01247 for ( int i=0; i<num_points; i++)
01248 {
01249 marker.points[i].x = object_cloud_->points[i].x;
01250 marker.points[i].y = object_cloud_->points[i].y;
01251 marker.points[i].z = object_cloud_->points[i].z;
01252 }
01253 }
01254 else
01255 {
01256 ROS_WARN("Make cloud marker requested with empty point cloud");
01257 }
01258
01259 InteractiveMarkerControl control;
01260 control.always_visible = true;
01261 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
01262 control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;
01263
01264 control.markers.push_back( marker );
01265
01266 int_marker.controls.push_back( control );
01267
01268 return int_marker;
01269 }
01270
01271 };
01272
01273
01274 int main(int argc, char** argv)
01275 {
01276 ros::init(argc, argv, "pr2_interactive_pose_select_action");
01277 GripperPoseAction gripper_pose_action;
01278
01279 ros::Duration(1.0).sleep();
01280
01281 ros::spin();
01282 }