$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 00050 //#include <pr2_object_manipulation_msgs/TestBasePoseAction.h> 00051 #include <pr2_object_manipulation_msgs/GetNavPoseAction.h> 00052 #include <pr2_object_manipulation_msgs/CameraFocus.h> 00053 00054 #include <sensor_msgs/point_cloud_conversion.h> 00055 00056 #include "eigen_conversions/eigen_msg.h" 00057 00058 #include "eigen3/Eigen/Geometry" 00059 #include "pcl/io/pcd_io.h" 00060 #include "pcl/point_types.h" 00061 #include "pcl_ros/transforms.h" 00062 00063 #include <household_objects_database_msgs/GetModelDescription.h> 00064 #include <household_objects_database_msgs/GetModelMesh.h> 00065 00066 #include <interactive_marker_helpers/interactive_marker_helpers.h> 00067 00068 using namespace object_manipulator; 00069 using namespace visualization_msgs; 00070 using namespace interactive_markers; 00071 using namespace pr2_object_manipulation_msgs; 00072 using namespace im_helpers; 00073 00075 00076 00079 class BasePoseAction 00080 { 00081 protected: 00082 00083 // ****************** class members ********************* 00084 00085 geometry_msgs::PoseStamped base_pose_; 00086 geometry_msgs::PoseStamped control_offset_; 00087 00088 bool active_; 00089 00090 int interface_number_; 00091 int task_number_; 00092 00093 bool double_menu_; 00094 bool testing_planned_grasp_; 00095 int tested_grasp_index_; 00096 bool testing_current_grasp_; 00097 double max_direct_move_radius_; 00098 00099 ros::NodeHandle nh_; 00100 ros::NodeHandle pnh_; 00101 ros::Subscriber sub_seed_; 00102 00103 ros::Timer fast_update_timer_; 00104 ros::Timer slow_update_timer_; 00105 InteractiveMarkerServer server_; 00106 00107 ros::Publisher pub_focus_; 00108 00109 MenuHandler menu_controls_; 00110 MenuHandler::EntryHandle accept_handle_; 00111 MenuHandler::EntryHandle cancel_handle_; 00112 MenuHandler::EntryHandle focus_handle_; 00113 00114 tf::TransformListener tfl_; 00115 00116 object_manipulator::MechanismInterface mechanism_; 00117 00118 //actionlib::SimpleActionClient<pr2_object_manipulation_msgs::TestBasePoseAction> test_pose_client_; 00119 //actionlib::SimpleActionClient<object_manipulation_msgs::GraspPlanningAction> grasp_plan_client_; 00120 00121 // object_manipulator::ActionWrapper<pr2_object_manipulation_msgs::TestBasePoseAction> test_pose_client_; 00122 // object_manipulator::ActionWrapper<object_manipulation_msgs::GraspPlanningAction> grasp_plan_client_; 00123 00124 std::string get_pose_name_; 00125 actionlib::SimpleActionServer<pr2_object_manipulation_msgs::GetNavPoseAction> get_pose_server_; 00126 00127 public: 00128 00129 BasePoseAction() : 00130 active_(false), 00131 max_direct_move_radius_(10.0), 00132 nh_("/"), 00133 pnh_("~"), 00134 server_("pr2_marker_control", "nav_action", false), 00135 tfl_(nh_), 00136 // test_pose_client_("test_gripper_pose", true), 00137 // grasp_plan_client_("grasp_plan_action", true), 00138 get_pose_name_(ros::this_node::getName()), 00139 get_pose_server_(nh_, get_pose_name_, false) 00140 { 00141 ROS_INFO( "pr2_interactive_base_pose_action is starting up." ); 00142 00143 ros::Duration(1.0).sleep(); 00144 00145 nh_.param<int>("interactive_grasping/interface_number", interface_number_, 0); 00146 if (!interface_number_) ROS_WARN("No interface number specified for grasping study; using default configuration"); 00147 else ROS_INFO("Using interface number %d for grasping study", interface_number_); 00148 nh_.param<int>("interactive_grasping/task_number", task_number_, 0); 00149 if (!task_number_) ROS_WARN("No task number specified for grasping study; using default configuration"); 00150 else ROS_INFO("Using task number %d for grasping study", task_number_); 00151 pnh_.param<bool>("double_menu", double_menu_, false); 00152 00153 //nh_.param<double>("max_direct_move_radius", max_direct_move_radius_, 0.4); 00154 00155 // Dummy box to avoid empty_server bug... 00156 geometry_msgs::PoseStamped ps; 00157 ps.header.frame_id = "base_link"; 00158 server_.insert(makeButtonBox( "test_box", ps, 0.01, false, false)); 00159 00160 pub_focus_ = nh_.advertise<pr2_object_manipulation_msgs::CameraFocus>("camera_focus", 1); 00161 00162 fast_update_timer_ = nh_.createTimer(ros::Duration(0.05), boost::bind( &BasePoseAction::fast_update, this ) ); 00163 00164 sub_seed_ = nh_.subscribe<geometry_msgs::PoseStamped>("cloud_click_point", 1, boost::bind(&BasePoseAction::setSeed, this, _1)); 00165 00166 // Initialization must happen at the end! 00167 initMenus(); 00168 00169 //register the goal and feeback callbacks 00170 get_pose_server_.registerGoalCallback( boost::bind(&BasePoseAction::goalCB, this)); 00171 get_pose_server_.registerPreemptCallback( boost::bind(&BasePoseAction::preemptCB, this)); 00172 00173 get_pose_server_.start(); 00174 } 00175 00176 ~BasePoseAction() 00177 { 00178 } 00179 00180 void setSeed(const geometry_msgs::PoseStampedConstPtr &seed) 00181 { 00182 if(!active_) return; 00183 ROS_DEBUG("Setting seed."); 00184 // geometry_msgs::PoseStamped ps = *seed; 00185 // ROS_DEBUG_STREAM("Input seed was \n" << ps); 00186 // tf::Pose pose; 00187 // tf::poseMsgToTF(ps.pose, pose); 00188 // tf::Quaternion q = pose.getRotation(); 00189 // btMatrix3x3 rot(q); 00190 // btMatrix3x3 perm( 0, 0, 1, 00191 // 0, 1, 0, 00192 // -1, 0, 0); 00193 // (rot*perm).getRotation(q); 00194 // //tf::quaternionTFToMsg(q, ps.pose.orientation); 00195 // pose.setRotation(q); 00196 // 00197 // tf::poseTFToMsg(pose, ps.pose); 00198 00199 base_pose_ = *seed; 00200 base_pose_.pose.orientation = geometry_msgs::Quaternion(); 00201 00202 initMarkers(); 00203 //testPose(base_pose_, gripper_opening_); 00204 00205 //focus the camera on the new pose 00206 //focusCB(); 00207 } 00208 00210 void setIdle(){ 00211 active_ = false; 00212 // server_.erase("ghosted_gripper"); 00213 // server_.erase("gripper_controls"); 00214 // server_.erase("object_cloud"); 00215 // //server_.erase("grasp_toggle"); 00216 server_.clear(); 00217 } 00218 00219 00221 void goalCB() 00222 { 00223 active_ = true; 00224 ROS_INFO("pr2_interactive_nav_action_called!"); 00225 pr2_object_manipulation_msgs::GetNavPoseGoal goal = *get_pose_server_.acceptNewGoal(); 00226 //control_offset_.pose = object_manipulator::msg::createPoseMsg(tf::Pose(tf::Quaternion::getIdentity(), tf::Vector3(0.15,0,0))); 00227 00228 if (goal.starting_pose.pose.orientation.x == 0 && 00229 goal.starting_pose.pose.orientation.y == 0 && 00230 goal.starting_pose.pose.orientation.z == 0 && 00231 goal.starting_pose.pose.orientation.w == 0 ) 00232 { 00233 ROS_INFO("Empty pose passed in; using default"); 00234 base_pose_ = getDefaultPose(); 00235 } 00236 else 00237 { 00238 base_pose_ = goal.starting_pose; 00239 } 00240 00241 if (get_pose_server_.isPreemptRequested()) 00242 { 00243 preemptCB(); 00244 return; 00245 } 00246 if(goal.max_distance > 0) 00247 max_direct_move_radius_ = goal.max_distance; 00248 else 00249 max_direct_move_radius_ = 1E6; 00250 00251 initMarkers(); 00252 // pr2_object_manipulation_msgs::TestGripperPoseGoal test_goal; 00253 // test_goal.gripper_poses.push_back(base_pose_); 00254 // test_goal.gripper_openings.push_back(gripper_opening_); 00255 // test_pose_client_.client().sendGoal( test_goal, boost::bind(&BasePoseAction::testGripperResultCallback, this, _1, _2)); 00256 } 00257 00258 00260 void preemptCB() 00261 { 00262 ROS_INFO("%s: Preempted", get_pose_name_.c_str()); 00263 //test_pose_client_.client().cancelAllGoals(); 00264 get_pose_server_.setPreempted(); 00265 setIdle(); 00266 } 00267 00270 void updatePoses() 00271 { 00272 server_.setPose("meshes", base_pose_.pose, base_pose_.header); 00273 } 00274 00275 geometry_msgs::PoseStamped getDefaultPose() 00276 { 00277 geometry_msgs::PoseStamped ps; 00278 ps.header.stamp = ros::Time(0); 00279 ps.header.frame_id = "base_link"; 00280 ps.pose.orientation.w = 1; 00281 return ps; 00282 } 00283 00284 00285 // **** 10 ***** 20 ****** 30 ****** 40 ****** 50 ****** 60 ****** 70 ****** 80 ****** 90 ***** 100 ***** 110 ***** 120 00286 00289 void initMarkers() 00290 { 00291 initMeshMarkers(); 00292 initControlMarkers(); 00293 } 00294 00295 void initMeshMarkers() 00296 { 00297 // float r,g,b; 00298 // switch(pose_state_) 00299 // { 00300 // case INVALID: 00301 // r = 1.0; g = 0.2; b = 0.2; 00302 // break; 00303 // case VALID: 00304 // r = 0.2; g = 1.0; b = 0.2; 00305 // break; 00306 // default: 00307 // r = 0.5; g = 0.5; b = 0.5; 00308 // } 00309 // std_msgs::ColorRGBA color = object_manipulator::msg::createColorMsg(r,g,b,1.0); 00310 // 00311 // server_.insert(makeGripperMarker( "ghosted_gripper", base_pose_, 1.01, gripper_angle_, false, color), 00312 // boost::bind( &BasePoseAction::gripperClickCB, this, _1)); 00313 // menu_controls_.apply(server_, "ghosted_gripper"); 00314 } 00315 00316 void initControlMarkers() 00317 { 00318 geometry_msgs::PoseStamped ps = base_pose_; 00319 ps.header.stamp = ros::Time(0); 00320 00321 //ROS_ERROR("Populating meshes..."); 00322 00323 // TODO this should absolutely be read from the robot_description parameter 00324 std::vector< std::string > mesh_paths, mesh_frames; 00325 mesh_paths.push_back("package://pr2_description/meshes/base_v0/base.dae"); 00326 mesh_frames.push_back("base_link"); 00327 mesh_paths.push_back("package://pr2_description/meshes/torso_v0/torso_lift.dae"); 00328 mesh_frames.push_back("torso_lift_link"); 00329 mesh_paths.push_back("package://pr2_description/meshes/shoulder_v0/shoulder_pan.dae"); 00330 mesh_frames.push_back("r_shoulder_pan_link"); 00331 mesh_paths.push_back("package://pr2_description/meshes/shoulder_v0/shoulder_pan.dae"); 00332 mesh_frames.push_back("l_shoulder_pan_link"); 00333 mesh_paths.push_back("package://pr2_description/meshes/shoulder_v0/shoulder_lift.dae"); 00334 mesh_frames.push_back("r_shoulder_lift_link"); 00335 mesh_paths.push_back("package://pr2_description/meshes/shoulder_v0/shoulder_lift.dae"); 00336 mesh_frames.push_back("l_shoulder_lift_link"); 00337 mesh_paths.push_back("package://pr2_description/meshes/upper_arm_v0/upper_arm.dae"); 00338 mesh_frames.push_back("r_upper_arm_link"); 00339 mesh_paths.push_back("package://pr2_description/meshes/upper_arm_v0/upper_arm.dae"); 00340 mesh_frames.push_back("l_upper_arm_link"); 00341 mesh_paths.push_back("package://pr2_description/meshes/forearm_v0/forearm.dae"); 00342 mesh_frames.push_back("r_forearm_link"); 00343 mesh_paths.push_back("package://pr2_description/meshes/forearm_v0/forearm.dae"); 00344 mesh_frames.push_back("l_forearm_link"); 00345 00346 // Right Gripper 00347 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/gripper_palm.dae"); 00348 mesh_frames.push_back("r_gripper_palm_link"); 00349 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/upper_finger_r.stl"); 00350 mesh_frames.push_back("r_gripper_r_finger_link"); 00351 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/upper_finger_l.stl"); 00352 mesh_frames.push_back("r_gripper_l_finger_link"); 00353 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/finger_tip_r.stl"); 00354 mesh_frames.push_back("r_gripper_r_finger_tip_link"); 00355 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/finger_tip_l.stl"); 00356 mesh_frames.push_back("r_gripper_l_finger_tip_link"); 00357 00358 00359 // Left Gripper 00360 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/gripper_palm.dae"); 00361 mesh_frames.push_back("l_gripper_palm_link"); 00362 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/upper_finger_r.stl"); 00363 mesh_frames.push_back("l_gripper_r_finger_link"); 00364 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/upper_finger_l.stl"); 00365 mesh_frames.push_back("l_gripper_l_finger_link"); 00366 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/finger_tip_r.stl"); 00367 mesh_frames.push_back("l_gripper_r_finger_tip_link"); 00368 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/finger_tip_l.stl"); 00369 mesh_frames.push_back("l_gripper_l_finger_tip_link"); 00370 00371 // Head 00372 mesh_paths.push_back("package://pr2_description/meshes/head_v0/head_pan.dae"); 00373 mesh_frames.push_back("head_pan_link"); 00374 mesh_paths.push_back("package://pr2_description/meshes/head_v0/head_tilt.dae"); 00375 mesh_frames.push_back("head_tilt_link"); 00376 00377 00378 if(mesh_paths.size() != mesh_frames.size()) ROS_ERROR("The number of mesh paths and mash frame_ids is not equal!"); 00379 00380 std::vector< geometry_msgs::PoseStamped > mesh_poses; 00381 for(size_t i = 0; i < mesh_paths.size(); i++) 00382 { 00383 geometry_msgs::PoseStamped ps; 00384 ps.header.stamp = ros::Time(0); 00385 ps.header.frame_id = mesh_frames[i]; 00386 ps.pose.orientation.w = 1; 00387 tfl_.waitForTransform("base_link", ps.header.frame_id, ps.header.stamp,ros::Duration(3.0)); 00388 tfl_.transformPose("base_link", ps, ps); 00389 mesh_poses.push_back(ps); 00390 } 00391 00392 //ROS_ERROR("Creating marker..."); 00393 server_.insert(makeRobotModelMarker( "pose_controls", ps, mesh_poses, mesh_paths, 1.03), 00394 boost::bind( &BasePoseAction::poseControlCB, this, _1)); 00395 // server_.insert(makePlanarMarker( "pose_controls", ps, 1.0, false), 00396 // boost::bind( &BasePoseAction::poseControlCB, this, _1)); 00397 menu_controls_.apply(server_, "pose_controls"); 00398 //ROS_ERROR("Done creating markers."); 00399 } 00400 00401 00402 protected: 00403 00404 00406 void fast_update() 00407 { 00408 //updatePoses(); 00409 server_.applyChanges(); 00410 } 00411 00412 void slow_update() 00413 { 00414 } 00415 00416 00417 // void testPose(geometry_msgs::PoseStamped pose, float opening) 00418 // { 00419 // pr2_object_manipulation_msgs::TestGripperPoseGoal goal; 00420 // goal.gripper_poses.push_back(pose); 00421 // goal.gripper_openings.push_back(opening); 00422 // test_pose_client_.client().sendGoal( goal, boost::bind(&BasePoseAction::testGripperResultCallback, this, _1, _2)); 00423 // } 00424 00425 // //! Callback that receives the result of a TestGripperPose action. 00426 // void testPoseResultCallback(const actionlib::SimpleClientGoalState& state, 00427 // const pr2_object_manipulation_msgs::TestGripperPoseResultConstPtr &result) 00428 // { 00429 // if (result->valid.empty()) 00430 // { 00431 // ROS_ERROR("Test gripper pose returned with empty result list"); 00432 // return; 00433 // } 00434 // if(state.state_ == state.SUCCEEDED) 00435 // { 00436 // ROS_DEBUG("Test pose action returned with result %d", (int)result->valid[0]); 00437 // PoseState state = INVALID; 00438 // if (result->valid[0]) state = VALID; 00439 // if (testing_planned_grasp_) 00440 // { 00441 // if (planner_states_.empty() || tested_grasp_index_ < 0 || tested_grasp_index_ >= (int)planner_states_.size() ) 00442 // { 00443 // ROS_ERROR("Current grasp tested, planner states has %zd entries and tested_grasp_index is %d", 00444 // planner_states_.size(), tested_grasp_index_); 00445 // } 00446 // else 00447 // { 00448 // planner_states_[tested_grasp_index_] = state; 00449 // initGraspMarkers(); 00450 // } 00451 // } 00452 // if (testing_current_grasp_) 00453 // { 00454 // pose_state_ = state; 00455 // initGripperMarker(); 00456 // } 00457 // //fire up a new test if we have one to do 00458 // if( active_ ) 00459 // { 00460 // for (size_t i=0; i<planner_states_.size(); i++) 00461 // { 00462 // if (planner_states_[i] == UNTESTED) 00463 // { 00464 // ROS_ERROR("Here 6"); 00465 // testing_planned_grasp_ = true; 00466 // tested_grasp_index_ = i; 00467 // testing_current_grasp_ = false; 00468 // testPose(planner_poses_[ i ], gripper_opening_); 00469 // } 00470 // } 00471 // } 00472 // } 00473 // else 00474 // { 00475 // ROS_WARN("Test pose action did not succeed; state = %d", (int)state.state_); 00476 // } 00477 // } 00478 00480 void acceptCB() 00481 { 00482 setIdle(); 00483 pr2_object_manipulation_msgs::GetNavPoseResult result; 00484 base_pose_.header.stamp = ros::Time::now(); 00485 result.pose = base_pose_; 00486 get_pose_server_.setSucceeded(result); 00487 } 00488 00490 void cancelCB() 00491 { 00492 // if(test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::ACTIVE || 00493 // test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::PENDING ) 00494 // { 00495 // test_pose_client_.client().cancelGoal(); 00496 // } 00497 get_pose_server_.setAborted(); 00498 setIdle(); 00499 } 00500 00502 void focusCB() 00503 { 00504 pr2_object_manipulation_msgs::CameraFocus msg; 00505 msg.focal_point.point = base_pose_.pose.position; 00506 msg.focal_point.header = base_pose_.header; 00507 pub_focus_.publish(msg); 00508 } 00509 00511 void poseMeshCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) 00512 { 00513 } 00514 00515 geometry_msgs::PoseStamped clipToRadius(const geometry_msgs::PoseStamped &input, const float &radius) 00516 { 00517 geometry_msgs::PoseStamped ps; 00518 tfl_.waitForTransform(input.header.frame_id, "base_link", input.header.stamp, ros::Duration(5.0)); 00519 tfl_.transformPose("base_link", input, ps); 00520 tf::Vector3 pose_center; 00521 tf::pointMsgToTF(ps.pose.position, pose_center); 00522 float stored_z = pose_center.z(); 00523 pose_center.setZ(0); 00524 float length = pose_center.length(); 00525 if(length > radius) 00526 { 00527 pose_center = radius / length * pose_center; 00528 pose_center.setZ(stored_z); 00529 tf::pointTFToMsg(pose_center, ps.pose.position); 00530 } 00531 ps.header.stamp = ros::Time(0); 00532 if( tfl_.canTransform("map", ps.header.frame_id, ros::Time(0))) 00533 { 00534 tfl_.transformPose("map", ps, ps); 00535 } 00536 else if( tfl_.canTransform("odom_combined", ps.header.frame_id, ros::Time(0))) 00537 { 00538 tfl_.transformPose("odom_combined", ps, ps); 00539 } 00540 else 00541 { 00542 tfl_.transformPose(input.header.frame_id, ps, ps); 00543 } 00544 return ps; 00545 } 00546 00548 void poseControlCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) 00549 { 00550 ros::Time now = ros::Time(0); 00551 00552 switch ( feedback->event_type ) 00553 { 00554 case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN: 00555 break; 00556 case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP: 00557 base_pose_.pose = feedback->pose; 00558 base_pose_.header = feedback->header; 00559 ROS_DEBUG_STREAM("MOUSE_UP base_pose before clipping is\n" << base_pose_); 00560 base_pose_ = clipToRadius(base_pose_, max_direct_move_radius_); 00561 ROS_DEBUG_STREAM("MOUSE_UP base_pose after clipping is\n" << base_pose_); 00562 initMarkers(); 00563 break; 00564 case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE: 00565 ROS_DEBUG_STREAM("POSE_UPDATE in frame " << feedback->header.frame_id << std::endl << feedback->pose); 00566 //if(base_pose_.header.frame_id.compare(feedback->header.frame_id)) fix = true; 00567 base_pose_.pose = feedback->pose; 00568 base_pose_.header = feedback->header; 00569 //base_pose_ = toWrist(base_pose_); 00570 updatePoses(); 00571 break; 00572 } 00573 } 00574 00576 void initMenus() 00577 { 00578 accept_handle_ = menu_controls_.insert("Accept", boost::bind( &BasePoseAction::acceptCB, this ) ); 00579 if(double_menu_) menu_controls_.insert("------", boost::bind( &BasePoseAction::acceptCB, this ) ); 00580 cancel_handle_ = menu_controls_.insert("Cancel", boost::bind( &BasePoseAction::cancelCB, this ) ); 00581 if(double_menu_) menu_controls_.insert("------", boost::bind( &BasePoseAction::cancelCB, this ) ); 00582 focus_handle_ = menu_controls_.insert("Focus camera", boost::bind( &BasePoseAction::focusCB, this ) ); 00583 if(double_menu_) menu_controls_.insert("------", boost::bind( &BasePoseAction::focusCB, this ) ); 00584 } 00585 }; 00586 00587 int main(int argc, char** argv) 00588 { 00589 ros::init(argc, argv, "pr2_interactive_pose_select_action"); 00590 BasePoseAction base_pose_action; 00591 00592 ros::Duration(1.0).sleep(); 00593 00594 ros::spin(); 00595 }