$search
00001 /* 00002 * Copyright (c) 2011, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 // author: Adam Leeper 00031 00032 // **** 10 ***** 20 ****** 30 ****** 40 ****** 50 ****** 60 ****** 70 ****** 80 ****** 90 ***** 100 ***** 110 ***** 120 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 <object_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 // ****************** class members ********************* 00088 00089 std::vector<geometry_msgs::PoseStamped> planner_poses_; 00090 std::vector<PoseState> planner_states_; 00091 int planner_index_; 00092 geometry_msgs::PoseStamped button_marker_pose_; 00093 00094 geometry_msgs::PoseStamped gripper_pose_; 00095 geometry_msgs::PoseStamped control_offset_; 00096 float gripper_opening_; 00097 float gripper_angle_; 00098 pcl::PointCloud<PointT>::Ptr object_cloud_; 00099 bool active_; 00100 bool object_model_; 00101 bool always_call_planner_; 00102 00103 int interface_number_; 00104 int task_number_; 00105 bool double_menu_; 00106 bool testing_planned_grasp_; 00107 int tested_grasp_index_; 00108 bool testing_current_grasp_; 00109 00110 PoseState pose_state_; 00111 ros::NodeHandle nh_; 00112 ros::NodeHandle pnh_; 00113 ros::Subscriber sub_seed_; 00114 ros::ServiceClient get_model_mesh_client_; 00115 ros::Timer spin_timer_; 00116 ros::Timer slow_sync_timer_; 00117 InteractiveMarkerServer server_; 00118 00119 ros::Publisher pub_cloud_; 00120 ros::Publisher pub_focus_; 00121 00122 MenuHandler menu_gripper_; 00123 MenuHandler::EntryHandle accept_handle_; 00124 MenuHandler::EntryHandle cancel_handle_; 00125 MenuHandler::EntryHandle focus_handle_; 00126 00127 tf::TransformListener tfl_; 00128 00129 object_manipulator::MechanismInterface mechanism_; 00130 00131 //actionlib::SimpleActionClient<pr2_object_manipulation_msgs::TestGripperPoseAction> test_pose_client_; 00132 //actionlib::SimpleActionClient<object_manipulation_msgs::GraspPlanningAction> grasp_plan_client_; 00133 //actionlib::SimpleActionClient<point_cloud_server::StoreCloudAction> cloud_server_client_; 00134 00135 object_manipulator::ActionWrapper<pr2_object_manipulation_msgs::TestGripperPoseAction> test_pose_client_; 00136 object_manipulator::ActionWrapper<object_manipulation_msgs::GraspPlanningAction> grasp_plan_client_; 00137 object_manipulator::ActionWrapper<point_cloud_server::StoreCloudAction> cloud_server_client_; 00138 00139 std::string get_pose_name_; 00140 actionlib::SimpleActionServer<pr2_object_manipulation_msgs::GetGripperPoseAction> get_pose_server_; 00141 00142 public: 00143 00144 GripperPoseAction() : 00145 planner_index_(0), 00146 gripper_angle_(0.541), 00147 object_cloud_(new pcl::PointCloud<PointT>()), 00148 active_(false), 00149 object_model_(false), 00150 pose_state_(UNTESTED), 00151 nh_("/"), 00152 pnh_("~"), 00153 server_("pr2_marker_control", "server 2", false), 00154 tfl_(nh_), 00155 test_pose_client_("test_gripper_pose", true), 00156 grasp_plan_client_("grasp_plan_action", true), 00157 cloud_server_client_("point_cloud_server_action", true), 00158 get_pose_name_(ros::this_node::getName()), 00159 get_pose_server_(nh_, get_pose_name_, false) 00160 { 00161 ROS_INFO( "pr2_interactive_gripper_pose_action is starting up." ); 00162 00163 ros::Duration(1.0).sleep(); 00164 00165 pnh_.param<bool>("always_call_planner", always_call_planner_, false); 00166 00167 nh_.param<int>("interactive_grasping/interface_number", interface_number_, 0); 00168 if (!interface_number_) ROS_WARN("No interface number specified for grasping study; using default configuration"); 00169 else ROS_INFO("Using interface number %d for grasping study", interface_number_); 00170 nh_.param<int>("interactive_grasping/task_number", task_number_, 0); 00171 if (!task_number_) ROS_WARN("No task number specified for grasping study; using default configuration"); 00172 else ROS_INFO("Using task number %d for grasping study", task_number_); 00173 pnh_.param<bool>("double_menu", double_menu_, false); 00174 00175 if(interface_number_ == 4) always_call_planner_ = true; 00176 else if(interface_number_ >= 1) always_call_planner_ = false; 00177 00178 // Dummy box to avoid empty_server bug... 00179 geometry_msgs::PoseStamped ps; 00180 ps.header.frame_id = "/base_link"; 00181 server_.insert(makeButtonBox( "test_box", ps, 0.01, false, false)); 00182 00183 pub_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>("cloud_debug", 1); 00184 pub_focus_ = nh_.advertise<pr2_object_manipulation_msgs::CameraFocus>("camera_focus", 1); 00185 00186 get_model_mesh_client_ = nh_.serviceClient<household_objects_database_msgs::GetModelMesh>("objects_database_node/get_model_mesh", false); 00187 00188 spin_timer_ = nh_.createTimer(ros::Duration(0.05), boost::bind( &GripperPoseAction::spinOnce, this ) ); 00189 00190 sub_seed_ = nh_.subscribe<geometry_msgs::PoseStamped>("cloud_click_point", 1, boost::bind(&GripperPoseAction::setSeed, this, _1)); 00191 00192 // Initialization must happen at the end! 00193 initMenus(); 00194 //initMarkers(); 00195 00196 //register the goal and feeback callbacks 00197 get_pose_server_.registerGoalCallback( boost::bind(&GripperPoseAction::goalCB, this)); 00198 get_pose_server_.registerPreemptCallback( boost::bind(&GripperPoseAction::preemptCB, this)); 00199 00200 get_pose_server_.start(); 00201 } 00202 00203 ~GripperPoseAction() 00204 { 00205 } 00206 00207 bool transformGripperPose(const std::string frame_id = "/base_link") 00208 { 00209 /* try{ 00210 tfl_.waitForTransform(frame_id, gripper_pose_.header.frame_id, gripper_pose_.header.stamp, ros::Duration(3.0)); 00211 tfl_.transformPose(frame_id, gripper_pose_, gripper_pose_); 00212 return true; 00213 } 00214 catch(...){ 00215 ROS_ERROR("TF can't transform!"); 00216 return false; 00217 } 00218 */ 00219 return true; 00220 } 00221 00222 void updateGripperOpening() 00223 { 00224 gripper_opening_ = gripper_angle_ * 0.1714; 00225 } 00226 00227 void updateGripperAngle() 00228 { 00229 gripper_angle_ = gripper_opening_ * 5.834; 00230 } 00231 00232 void setSeed(const geometry_msgs::PoseStampedConstPtr &seed) 00233 { 00234 if(!active_) return; 00235 ROS_DEBUG("Setting seed."); 00236 geometry_msgs::PoseStamped ps = *seed; 00237 ROS_DEBUG_STREAM("Input seed was \n" << ps); 00238 tf::Pose pose; 00239 tf::poseMsgToTF(ps.pose, pose); 00240 tf::Quaternion q = pose.getRotation(); 00241 btMatrix3x3 rot(q); 00242 btMatrix3x3 perm( 0, 0, 1, 00243 0, 1, 0, 00244 -1, 0, 0); 00245 (rot*perm).getRotation(q); 00246 //tf::quaternionTFToMsg(q, ps.pose.orientation); 00247 pose.setRotation(q); 00248 00249 if(object_model_) pose = pose*tf::Transform(tf::Quaternion(tf::Vector3(0,1,0), M_PI/2.0), tf::Vector3(0,0,0)).inverse(); 00250 00251 tf::poseTFToMsg(pose, ps.pose); 00252 //ps.header = seed->header; 00253 //ROS_INFO_STREAM("Processed seed before wrist offset was \n" << ps); 00254 00255 gripper_pose_ = toWrist(ps); 00256 //ROS_INFO_STREAM("Processed seed was \n" << gripper_pose_); 00257 //transformGripperPose(); 00258 //ROS_DEBUG_STREAM("But after transforming it is \n" << gripper_pose_); 00259 //ROS_INFO_STREAM("ansforming it is \n" << gripper_pose_); 00260 //transformGripperPose(); 00261 00262 pose_state_ = UNTESTED; 00263 00264 testing_planned_grasp_ = false; 00265 testing_current_grasp_ = true; 00266 if(always_call_planner_) 00267 { 00268 ROS_ERROR("Always call planner is on"); 00269 graspPlanCB(); 00270 } 00271 else 00272 { 00273 initMarkers(); 00274 testPose(gripper_pose_, gripper_opening_); 00275 } 00276 //focus the camera on the new pose 00277 focusCB(); 00278 } 00279 00281 void setIdle(){ 00282 active_ = false; 00283 server_.erase("ghosted_gripper"); 00284 server_.erase("gripper_controls"); 00285 server_.erase("object_cloud"); 00286 //server_.erase("grasp_toggle"); 00287 eraseAllGraspMarkers(); 00288 planner_states_.clear(); 00289 planner_poses_.clear(); 00290 } 00291 00292 00294 void goalCB() 00295 { 00296 active_ = true; 00297 object_model_ = false; 00298 planner_index_ = 0; 00299 ROS_INFO("Ghosted gripper called"); 00300 pr2_object_manipulation_msgs::GetGripperPoseGoal goal = *get_pose_server_.acceptNewGoal(); 00301 object_cloud_.reset( new pcl::PointCloud<PointT>()); 00302 pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>()); 00303 control_offset_.pose = object_manipulator::msg::createPoseMsg(tf::Pose(tf::Quaternion::getIdentity(), tf::Vector3(0.15,0,0))); 00304 00305 if( !goal.object.cluster.points.empty() || 00306 !goal.object.potential_models.empty() ) 00307 { 00308 ROS_INFO("Goal object contains %d cluster and %d models.", 00309 !goal.object.cluster.points.empty(), (int)goal.object.potential_models.size() ); 00310 // Something to draw... 00311 try 00312 { 00313 ROS_INFO("Converting to reference_frame..."); 00314 mechanism_.convertGraspableObjectComponentsToFrame(goal.object, goal.object.reference_frame_id); 00315 00316 bool use_cloud = goal.object.potential_models.empty(); 00317 00318 // Try to use object model, if there is one 00319 if(!use_cloud) 00320 { 00321 ROS_INFO("Goal contains object model; looking for model mesh..."); 00322 // Connect to databse, get object mesh. 00323 arm_navigation_msgs::Shape mesh; 00324 if(!getModelMesh(goal.object.potential_models[0].model_id, mesh)) 00325 { 00326 ROS_INFO("Unable to get database model, continuing with cluster."); 00327 use_cloud = true; 00328 } 00329 00330 if(!use_cloud) 00331 { 00332 object_model_ = true; 00333 for(unsigned int i = 0; i < mesh.vertices.size(); i++) 00334 { 00335 PointT pt; 00336 pt.x = mesh.vertices[i].x; 00337 pt.y = mesh.vertices[i].y; 00338 pt.z = mesh.vertices[i].z; 00339 cloud->points.push_back(pt); 00340 } 00341 cloud->width = cloud->points.size(); 00342 cloud->height = 1; 00343 cloud->is_dense = false; 00344 cloud->header.frame_id = goal.object.reference_frame_id; 00345 00346 geometry_msgs::Pose &m = goal.object.potential_models[0].pose.pose; 00347 Eigen::Affine3f affine = Eigen::Translation3f(m.position.x, 00348 m.position.y, 00349 m.position.z) * 00350 Eigen::Quaternionf(m.orientation.w, 00351 m.orientation.x, 00352 m.orientation.y, 00353 m.orientation.z); 00354 pcl::transformPointCloud(*cloud, *cloud, affine); 00355 00356 tf::Transform T_o, T_g; 00357 tf::poseMsgToTF(goal.object.potential_models[0].pose.pose, T_o); 00358 tf::poseMsgToTF(goal.grasp.grasp_pose, T_g); 00359 tf::Transform T = T_g.inverse()*T_o; 00360 tf::poseTFToMsg(T, control_offset_.pose); 00361 00362 } 00363 } 00364 00365 if(use_cloud) 00366 { 00367 // Store point cloud 00368 00369 sensor_msgs::PointCloud2 converted_cloud; 00370 sensor_msgs::convertPointCloudToPointCloud2 (goal.object.cluster, converted_cloud); 00371 00372 pcl::fromROSMsg(converted_cloud, *cloud); 00373 } 00374 00375 geometry_msgs::Pose &m = goal.grasp.grasp_pose; 00376 Eigen::Affine3f affine = Eigen::Translation3f(m.position.x, 00377 m.position.y, 00378 m.position.z) * 00379 Eigen::Quaternionf(m.orientation.w, 00380 m.orientation.x, 00381 m.orientation.y, 00382 m.orientation.z); 00383 affine = affine.inverse(); 00384 pcl::transformPointCloud(*cloud, *object_cloud_, affine); 00385 } 00386 catch(...){ 00387 ROS_ERROR("%s: Error converting graspable object to reference frame id [%s]!", 00388 get_pose_name_.c_str(), goal.object.reference_frame_id.c_str()); 00389 } 00390 } 00391 if (goal.gripper_pose.pose.orientation.x == 0 && 00392 goal.gripper_pose.pose.orientation.y == 0 && 00393 goal.gripper_pose.pose.orientation.z == 0 && 00394 goal.gripper_pose.pose.orientation.w == 0 ) 00395 { 00396 ROS_INFO("Empty pose passed in; using default"); 00397 gripper_pose_ = getDefaultPose(goal.arm_name); 00398 //ROS_DEBUG_STREAM("Default was \n" << gripper_pose_); 00399 //transformGripperPose(); 00400 //ROS_DEBUG_STREAM("Default after transform is\n" << gripper_pose_); 00401 gripper_opening_ = 0.086; 00402 updateGripperAngle(); 00403 } 00404 else 00405 { 00406 gripper_pose_ = goal.gripper_pose; 00407 //transformGripperPose(); 00408 gripper_opening_ = goal.gripper_opening; 00409 updateGripperAngle(); 00410 } 00411 if (get_pose_server_.isPreemptRequested()) 00412 { 00413 if(test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::ACTIVE || 00414 test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::PENDING ) 00415 { 00416 test_pose_client_.client().cancelAllGoals(); 00417 } 00418 get_pose_server_.setPreempted(); 00419 setIdle(); 00420 return; 00421 } 00422 00423 pose_state_ = UNTESTED; 00424 initMarkers(); 00425 pr2_object_manipulation_msgs::TestGripperPoseGoal test_goal; 00426 test_goal.gripper_poses.push_back(gripper_pose_); 00427 test_goal.gripper_openings.push_back(gripper_opening_); 00428 test_pose_client_.client().sendGoal( test_goal, boost::bind(&GripperPoseAction::testGripperResultCallback, this, _1, _2)); 00429 } 00430 00431 00433 void preemptCB() 00434 { 00435 ROS_INFO("%s: Preempted", get_pose_name_.c_str()); 00436 test_pose_client_.client().cancelAllGoals(); 00437 grasp_plan_client_.client().cancelGoal(); 00438 get_pose_server_.setPreempted(); 00439 setIdle(); 00440 } 00441 00443 geometry_msgs::PoseStamped toWrist(const geometry_msgs::PoseStamped &ps) 00444 { 00445 geometry_msgs::PoseStamped out; 00446 out.header = ps.header; 00447 tf::Transform T, P; 00448 tf::poseMsgToTF(ps.pose, P); 00449 tf::poseMsgToTF(control_offset_.pose, T); 00450 tf::poseTFToMsg( P*T.inverse(), out.pose); 00451 //out.pose = object_manipulator::msg::applyShift(ps.pose, tf::Vector3(-0.15,0,0)); 00452 return out; 00453 } 00454 00456 geometry_msgs::PoseStamped fromWrist(const geometry_msgs::PoseStamped &ps) 00457 { 00458 geometry_msgs::PoseStamped out; 00459 out.header = ps.header; 00460 tf::Transform T, P; 00461 tf::poseMsgToTF(ps.pose, P); 00462 tf::poseMsgToTF(control_offset_.pose, T); 00463 tf::poseTFToMsg( P*T, out.pose); 00464 //out.pose = object_manipulator::msg::applyShift(ps.pose, tf::Vector3(-0.15,0,0)); 00465 return out; 00466 } 00467 00468 00471 void updatePoses() 00472 { 00473 server_.setPose("ghosted_gripper", gripper_pose_.pose, gripper_pose_.header); 00474 server_.setPose("object_cloud", gripper_pose_.pose, gripper_pose_.header); 00475 } 00476 00477 geometry_msgs::PoseStamped getDefaultPose(std::string arm_name) 00478 { 00479 ros::Time now = ros::Time(0); 00480 tfl_.waitForTransform("r_gripper_tool_frame","/base_link", now, ros::Duration(2.0)); 00481 tf::StampedTransform stamped; 00482 geometry_msgs::TransformStamped ts; 00483 geometry_msgs::PoseStamped ps; 00484 std::string arm_frame; 00485 if (arm_name == "right_arm") arm_frame="r_wrist_roll_link"; 00486 else if (arm_name == "left_arm") arm_frame="l_wrist_roll_link"; 00487 else 00488 { 00489 arm_frame="r_wrist_roll_link"; 00490 ROS_ERROR("Unknown arm name passed to ghosted gripper"); 00491 } 00492 tfl_.lookupTransform("/base_link", arm_frame, now, stamped); 00493 tf::transformStampedTFToMsg(stamped, ts); 00494 ps = msg::createPoseStampedMsg(ts); 00495 return ps; 00496 } 00497 00498 00499 // **** 10 ***** 20 ****** 30 ****** 40 ****** 50 ****** 60 ****** 70 ****** 80 ****** 90 ***** 100 ***** 110 ***** 120 00500 00503 void initMarkers() 00504 { 00505 initGripperMarker(); 00506 initObjectMarker(); 00507 initGripperControl(); 00508 } 00509 00510 void initGraspMarkers() 00511 { 00512 for(unsigned int i = 0; i < planner_poses_.size(); i++) 00513 { 00514 // create arrow marker 00515 std::stringstream ss; 00516 ss << "grasp_" << i; 00517 server_.insert(makeGraspMarker(ss.str().c_str(), planner_poses_[i], 1.0, planner_states_[i])); 00518 server_.setCallback(ss.str(), boost::bind( &GripperPoseAction::selectGrasp, this, _1), 00519 visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN ); 00520 00521 // TODO figure out how to delete stuff gracefully 00522 } 00523 } 00524 00525 void eraseAllGraspMarkers() 00526 { 00527 for(unsigned int i = 0; i < planner_poses_.size(); i++) 00528 { 00529 // delete 00530 std::stringstream ss; 00531 ss << "grasp_" << i; 00532 server_.erase(ss.str().c_str()); 00533 } 00534 } 00535 00536 void selectGrasp( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) 00537 { 00538 int index = atoi(feedback->marker_name.substr(6).c_str()); 00539 if( index >= (int)planner_poses_.size() ) return; 00540 ROS_ERROR("Selecting grasp %d", index); 00541 00542 planner_index_ = index; 00543 00544 gripper_pose_ = planner_poses_[ planner_index_ ]; 00545 pose_state_ = planner_states_[ planner_index_ ]; 00546 initMarkers(); 00547 //initButtonMarker(); 00548 { 00549 if(pose_state_ == UNTESTED) 00550 { 00551 ROS_ERROR("Here 5"); 00552 testing_planned_grasp_ = true; 00553 tested_grasp_index_ = planner_index_; 00554 testing_current_grasp_ = true; 00555 testPose(gripper_pose_, gripper_opening_); 00556 } 00557 } 00558 00559 } 00560 00561 void initButtonMarker() 00562 { 00563 button_marker_pose_.header.stamp = ros::Time(0); 00564 int num = 0; 00565 if(planner_poses_.size()) num = planner_index_ + 1; 00566 server_.insert(makeListControl("grasp_toggle", button_marker_pose_, num, planner_poses_.size(), 0.3)); 00567 // , boost::bind( &GripperPoseAction::cycleGrasps, this)); 00568 server_.setCallback("grasp_toggle", boost::bind( &GripperPoseAction::cycleGrasps, this, _1), 00569 visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN ); 00570 } 00571 00572 void initObjectMarker() 00573 { 00574 if(object_cloud_->points.size()) 00575 { 00576 server_.insert(makeCloudMarker( "object_cloud", gripper_pose_, 0.004, 00577 object_manipulator::msg::createColorMsg(0.2, 0.8, 0.2,1.0) )); 00578 } 00579 } 00580 00581 void initGripperMarker() 00582 { 00583 float r,g,b; 00584 switch(pose_state_) 00585 { 00586 case INVALID: 00587 r = 1.0; g = 0.2; b = 0.2; 00588 break; 00589 case VALID: 00590 r = 0.2; g = 1.0; b = 0.2; 00591 break; 00592 default: 00593 r = 0.5; g = 0.5; b = 0.5; 00594 } 00595 std_msgs::ColorRGBA color = object_manipulator::msg::createColorMsg(r,g,b,1.0); 00596 00597 server_.insert(makeGripperMarker( "ghosted_gripper", gripper_pose_, 1.01, gripper_angle_, false, color), 00598 boost::bind( &GripperPoseAction::gripperClickCB, this, _1)); 00599 menu_gripper_.apply(server_, "ghosted_gripper"); 00600 } 00601 00602 void initGripperControl() 00603 { 00604 geometry_msgs::PoseStamped ps = fromWrist(gripper_pose_); 00605 ps.header.stamp = ros::Time(0); 00606 server_.insert(make6DofMarker( "gripper_controls", ps, 0.2, false, false), 00607 boost::bind( &GripperPoseAction::updateGripper, this, _1)); 00608 menu_gripper_.apply(server_, "gripper_controls"); 00609 } 00610 00611 00612 protected: 00613 00614 00616 void spinOnce() 00617 { 00618 //updatePoses(); 00619 server_.applyChanges(); 00620 } 00621 00622 void slowSync() 00623 { 00624 } 00625 00627 void testGripperResultCallback(const actionlib::SimpleClientGoalState& state, 00628 const pr2_object_manipulation_msgs::TestGripperPoseResultConstPtr &result) 00629 { 00630 if (result->valid.empty()) 00631 { 00632 ROS_ERROR("Test gripper pose returned with empty result list"); 00633 return; 00634 } 00635 if(state.state_ == state.SUCCEEDED) 00636 { 00637 ROS_DEBUG("Test pose action returned with result %d", (int)result->valid[0]); 00638 PoseState state = INVALID; 00639 if (result->valid[0]) state = VALID; 00640 if (testing_planned_grasp_) 00641 { 00642 if (planner_states_.empty() || tested_grasp_index_ < 0 || tested_grasp_index_ >= (int)planner_states_.size() ) 00643 { 00644 ROS_ERROR("Current grasp tested, planner states has %zd entries and tested_grasp_index is %d", 00645 planner_states_.size(), tested_grasp_index_); 00646 } 00647 else 00648 { 00649 planner_states_[tested_grasp_index_] = state; 00650 initGraspMarkers(); 00651 } 00652 } 00653 if (testing_current_grasp_) 00654 { 00655 pose_state_ = state; 00656 initGripperMarker(); 00657 } 00658 //fire up a new test if we have one to do 00659 if( active_ ) 00660 { 00661 for (size_t i=0; i<planner_states_.size(); i++) 00662 { 00663 if (planner_states_[i] == UNTESTED) 00664 { 00665 ROS_ERROR("Here 6"); 00666 testing_planned_grasp_ = true; 00667 tested_grasp_index_ = i; 00668 testing_current_grasp_ = false; 00669 testPose(planner_poses_[ i ], gripper_opening_); 00670 } 00671 } 00672 } 00673 } 00674 else 00675 { 00676 ROS_WARN("Test pose action did not succeed; state = %d", (int)state.state_); 00677 } 00678 } 00679 00681 void graspPlanCB() 00682 { 00683 eraseAllGraspMarkers(); 00684 planner_index_ = 0; 00685 planner_poses_.resize(0); 00686 planner_states_.resize(0); 00687 00688 point_cloud_server::StoreCloudGoal cloud_goal; 00689 cloud_goal.action = cloud_goal.GET; 00690 cloud_goal.name = "interactive_manipulation_snapshot"; 00691 cloud_server_client_.client().sendGoal(cloud_goal); 00692 if(!cloud_server_client_.client().waitForResult(ros::Duration(3.0))) 00693 { 00694 ROS_WARN("Timed-out while waiting for cloud from server!"); 00695 return; 00696 } 00697 00698 object_manipulation_msgs::GraspPlanningGoal plan_goal; 00699 plan_goal.target.region.cloud = cloud_server_client_.client().getResult()->cloud; 00700 plan_goal.target.region.roi_box_pose = fromWrist(gripper_pose_); 00701 //plan_goal.target.region.roi_box_pose.header.frame_id = "/base_link"; 00702 plan_goal.target.region.roi_box_dims = object_manipulator::msg::createVector3Msg(0.3, 0.3, 0.3); 00703 int cloud_size = plan_goal.target.region.cloud.width * plan_goal.target.region.cloud.height; 00704 plan_goal.target.reference_frame_id = gripper_pose_.header.frame_id; 00705 object_manipulation_msgs::Grasp seed; 00706 seed.grasp_pose = gripper_pose_.pose; 00707 plan_goal.grasps_to_evaluate.push_back(seed); 00708 ROS_DEBUG_STREAM("Requesting adjustment on cloud with " << cloud_size << " points, pose \n" << seed); 00709 //pub_cloud_.publish(plan_goal.target.region.cloud); 00710 grasp_plan_client_.client().sendGoal( plan_goal, boost::bind(&GripperPoseAction::graspPlanResultCB, this, _1, _2)); 00711 00712 button_marker_pose_ = gripper_pose_; 00713 button_marker_pose_.header.stamp = ros::Time(0); 00714 button_marker_pose_.pose.orientation = geometry_msgs::Quaternion(); 00715 button_marker_pose_.pose.orientation.w = 1; 00716 button_marker_pose_.pose.position.z -= 0.2; 00717 //initButtonMarker(); 00718 //if(always_call_planner) 00719 //{ 00720 server_.erase("gripper_controls"); 00721 server_.erase("ghosted_gripper"); 00722 //} 00723 } 00724 00726 void graspPlanResultCB(const actionlib::SimpleClientGoalState& state, 00727 const object_manipulation_msgs::GraspPlanningResultConstPtr &result) 00728 { 00729 planner_index_ = 0; 00730 if(state.state_ == state.SUCCEEDED) 00731 { 00732 ROS_INFO("Grasp plan action succeeded."); 00733 00734 int num = result->grasps.size(); 00735 00736 planner_poses_.resize(num + 1); 00737 planner_states_.resize(num + 1); 00738 //gripper_score_.resize(num); 00739 //gripper_angle_.resize(num); 00740 00741 for(int i = 0; i < num; i++) 00742 { 00743 planner_poses_[i].pose = result->grasps[i].grasp_pose; 00744 planner_poses_[i].header = gripper_pose_.header; 00745 planner_poses_[i].header.stamp = ros::Time(0); 00746 planner_states_[i] = UNTESTED; 00747 //object_manipulation_msgs::Grasp grasp = result->grasps[i]; 00748 //gripper_angle_[i] = std::max( std::min(1.f , (float)0.541) , (float)0.0); 00749 //gripper_score_[i] = grasp.success_probability; 00750 } 00751 planner_poses_[num] = gripper_pose_; 00752 planner_poses_[num].header.stamp = ros::Time(0); 00753 planner_states_[num] = UNTESTED; 00754 initGraspMarkers(); 00755 ROS_ERROR("Here 1"); 00756 testing_planned_grasp_ = true; 00757 tested_grasp_index_ = 0; 00758 testing_current_grasp_ = false; 00759 testPose(planner_poses_[ 0 ], gripper_opening_); 00760 } 00761 else 00762 { 00763 ROS_WARN("Grasp plan action did not succeed; state = %d", (int)state.state_); 00764 planner_poses_.resize(1); 00765 planner_states_.resize(1); 00766 planner_poses_[0] = gripper_pose_; 00767 planner_poses_[0].header.stamp = ros::Time(0); 00768 planner_states_[0] = UNTESTED; 00769 gripper_pose_ = planner_poses_[0]; 00770 initMarkers(); 00771 //initButtonMarker(); 00772 pose_state_ = UNTESTED; 00773 testing_current_grasp_ = true; 00774 ROS_ERROR("Here 2"); 00775 testing_planned_grasp_ = true; 00776 tested_grasp_index_ = 0; 00777 testPose(gripper_pose_, gripper_opening_); 00778 } 00779 } 00780 00781 void cycleGrasps(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) 00782 { 00783 ROS_ERROR("Cycle grasps"); 00784 //ROS_WARN("Grasp cycling not yet implemented!"); 00785 // Need to: 00786 // Store the returned grasp list 00787 // Store a bool list to remeber if a grasp pose is valid 00788 // Set the gripper opening to max (?) 00789 // Cycle through the grasps, calling the test_pose action if needed 00790 //ROS_INFO_STREAM("Got feedback! \n" << *feedback); 00791 00792 //if(!feedback->control_name.compare("left")) planner_index_ --; 00793 //if(!feedback->control_name.compare("right")) planner_index_++; 00794 00795 planner_index_++; 00796 if( planner_index_ == (int)planner_poses_.size() ) planner_index_ = 0; 00797 //if( planner_index_ < 0 ) planner_index_ = 0; 00798 00799 gripper_pose_ = planner_poses_[ planner_index_ ]; 00800 pose_state_ = planner_states_[ planner_index_ ]; 00801 initMarkers(); 00802 initButtonMarker(); 00803 if(pose_state_ == UNTESTED) 00804 { 00805 testing_current_grasp_ = true; 00806 ROS_ERROR("Here 3"); 00807 testing_planned_grasp_ = true; 00808 tested_grasp_index_ = planner_index_; 00809 testPose(gripper_pose_, gripper_opening_); 00810 } 00811 } 00812 00814 void acceptCB() 00815 { 00816 if( pose_state_ == VALID ) 00817 { 00818 if(test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::ACTIVE || 00819 test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::PENDING ) 00820 { 00821 test_pose_client_.client().cancelGoal(); 00822 } 00823 setIdle(); 00824 pr2_object_manipulation_msgs::GetGripperPoseResult result; 00825 result.gripper_pose = gripper_pose_; 00826 result.gripper_opening = gripper_opening_; 00827 get_pose_server_.setSucceeded(result); 00828 } 00829 } 00830 00832 void cancelCB() 00833 { 00834 if(test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::ACTIVE || 00835 test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::PENDING ) 00836 { 00837 test_pose_client_.client().cancelGoal(); 00838 } 00839 get_pose_server_.setAborted(); 00840 setIdle(); 00841 } 00842 00844 void focusCB() 00845 { 00846 pr2_object_manipulation_msgs::CameraFocus msg; 00847 msg.focal_point.point = gripper_pose_.pose.position; 00848 msg.focal_point.header = gripper_pose_.header; 00849 pub_focus_.publish(msg); 00850 } 00851 00853 void gripperClickCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) 00854 { 00855 if(interface_number_ == 2) return; 00856 00857 ros::Time now = ros::Time(0); 00858 00859 switch ( feedback->event_type ) 00860 { 00861 case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK: 00862 //ROS_DEBUG_STREAM( "Marker is being moved, stored pose is invalidated." ); 00863 float max_gripper_angle = 0.541; 00864 gripper_angle_ -= 0.12; 00865 if(gripper_angle_ < 0.04) 00866 gripper_angle_ = max_gripper_angle; 00867 updateGripperOpening(); 00868 initGripperMarker(); 00869 ROS_DEBUG( "Gripper opening = %.2f, angle = %.2f", gripper_opening_, gripper_angle_); 00870 break; 00871 } 00872 } 00873 00874 void testPose(geometry_msgs::PoseStamped pose, float opening) 00875 { 00876 pr2_object_manipulation_msgs::TestGripperPoseGoal goal; 00877 goal.gripper_poses.push_back(pose); 00878 goal.gripper_openings.push_back(opening); 00879 test_pose_client_.client().sendGoal( goal, boost::bind(&GripperPoseAction::testGripperResultCallback, this, _1, _2)); 00880 } 00881 00883 void updateGripper( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) 00884 { 00885 ros::Time now = ros::Time(0); 00886 00887 switch ( feedback->event_type ) 00888 { 00889 case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN: 00890 ROS_DEBUG_STREAM( "Marker is being moved, stored pose is invalidated." ); 00891 test_pose_client_.client().cancelAllGoals(); 00892 pose_state_ = UNTESTED; 00893 break; 00894 case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP: 00895 //ROS_INFO_STREAM("MOUSE_UP \n" << feedback->pose); 00896 ROS_DEBUG_STREAM( "Marker was released, storing pose and checking." ); 00897 // if(gripper_pose_.header.frame_id.compare(feedback->header.frame_id)) fix = true; 00898 gripper_pose_.pose = feedback->pose; 00899 gripper_pose_.header = feedback->header; 00900 gripper_pose_ = toWrist(gripper_pose_); 00901 //transformGripperPose(); 00902 //ROS_INFO_STREAM("MOUSE_UP \n" << gripper_pose_); 00903 initMarkers(); 00904 testing_planned_grasp_ = false; 00905 testing_current_grasp_ = true; 00906 testPose(gripper_pose_, gripper_opening_); 00907 break; 00908 case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE: 00909 ROS_DEBUG_STREAM("POSE_UPDATE in frame " << feedback->header.frame_id << std::endl << feedback->pose); 00910 //if(gripper_pose_.header.frame_id.compare(feedback->header.frame_id)) fix = true; 00911 gripper_pose_.pose = feedback->pose; 00912 gripper_pose_.header = feedback->header; 00913 gripper_pose_ = toWrist(gripper_pose_); 00914 updatePoses(); 00915 break; 00916 } 00917 } 00918 00920 void initMenus() 00921 { 00922 accept_handle_ = menu_gripper_.insert("Accept", boost::bind( &GripperPoseAction::acceptCB, this ) ); 00923 if(double_menu_) menu_gripper_.insert("------", boost::bind( &GripperPoseAction::acceptCB, this ) ); 00924 cancel_handle_ = menu_gripper_.insert("Cancel", boost::bind( &GripperPoseAction::cancelCB, this ) ); 00925 if(double_menu_) menu_gripper_.insert("------", boost::bind( &GripperPoseAction::cancelCB, this ) ); 00926 focus_handle_ = menu_gripper_.insert("Focus camera", boost::bind( &GripperPoseAction::focusCB, this ) ); 00927 if(double_menu_) menu_gripper_.insert("------", boost::bind( &GripperPoseAction::focusCB, this ) ); 00928 if(interface_number_ == 0 || interface_number_ == 4) 00929 { 00930 menu_gripper_.insert("Auto-Adjust", boost::bind( &GripperPoseAction::graspPlanCB, this ) ); 00931 if(double_menu_) menu_gripper_.insert("-----------", boost::bind( &GripperPoseAction::graspPlanCB, this ) ); 00932 } 00933 } 00934 00935 bool getModelMesh( int model_id, arm_navigation_msgs::Shape& mesh ) 00936 { 00937 household_objects_database_msgs::GetModelMesh mesh_srv; 00938 00939 mesh_srv.request.model_id = model_id; 00940 if ( !get_model_mesh_client_.call(mesh_srv) ) 00941 { 00942 ROS_ERROR("Failed to call get model mesh service"); 00943 return false; 00944 } 00945 00946 if (mesh_srv.response.return_code.code != household_objects_database_msgs::DatabaseReturnCode::SUCCESS) 00947 { 00948 ROS_ERROR("Model mesh service reports an error (code %d)", mesh_srv.response.return_code.code); 00949 return false; 00950 } 00951 00952 mesh = mesh_srv.response.mesh; 00953 return true; 00954 } 00955 00956 00957 00959 visualization_msgs::InteractiveMarker makeCloudMarker( const char *name, const geometry_msgs::PoseStamped &stamped, 00960 float point_size, std_msgs::ColorRGBA color) 00961 { 00962 InteractiveMarker int_marker; 00963 int_marker.name = name; 00964 int_marker.pose = stamped.pose; 00965 int_marker.header = stamped.header; 00966 00967 Marker marker; 00968 marker.color = color; 00969 marker.frame_locked = false; 00970 00971 if(object_cloud_->points.size()) 00972 { 00973 //int_marker.header = object_cloud_->header; 00974 marker.scale.x = point_size; 00975 marker.scale.y = point_size; 00976 marker.scale.z = point_size; 00977 marker.type = visualization_msgs::Marker::SPHERE_LIST; 00978 00979 int num_points = object_cloud_->points.size(); 00980 marker.points.resize( num_points ); 00981 // marker.colors.resize( num_points ); 00982 00983 //ROS_INFO_STREAM( "Adding point cluster. #points=" << object_.cluster.points.size() ); 00984 00985 for ( int i=0; i<num_points; i++) 00986 { 00987 marker.points[i].x = object_cloud_->points[i].x; 00988 marker.points[i].y = object_cloud_->points[i].y; 00989 marker.points[i].z = object_cloud_->points[i].z; 00990 // marker.colors[i].r = object_cloud_->points[i].r/255.; 00991 // marker.colors[i].g = object_cloud_->points[i].g/255.; 00992 // marker.colors[i].b = object_cloud_->points[i].b/255.; 00993 // marker.colors[i].a = 1.0; 00994 } 00995 } 00996 else 00997 { 00998 00999 } 01000 01001 InteractiveMarkerControl control; 01002 control.always_visible = true; 01003 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON; 01004 control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT; 01005 01006 control.markers.push_back( marker ); 01007 01008 int_marker.controls.push_back( control ); 01009 01010 return int_marker; 01011 } 01012 01013 }; 01014 01015 01016 int main(int argc, char** argv) 01017 { 01018 ros::init(argc, argv, "pr2_interactive_pose_select_action"); 01019 GripperPoseAction gripper_pose_action; 01020 01021 ros::Duration(1.0).sleep(); 01022 01023 ros::spin(); 01024 }