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
00050 #include <pr2_object_manipulation_msgs/GetNavPoseAction.h>
00051 #include <pr2_object_manipulation_msgs/CameraFocus.h>
00052
00053 #include <sensor_msgs/point_cloud_conversion.h>
00054
00055 #include "eigen_conversions/eigen_msg.h"
00056
00057 #include "eigen3/Eigen/Geometry"
00058 #include "pcl/io/pcd_io.h"
00059 #include "pcl/point_types.h"
00060 #include "pcl_ros/transforms.h"
00061
00062 #include <household_objects_database_msgs/GetModelDescription.h>
00063 #include <household_objects_database_msgs/GetModelMesh.h>
00064
00065 #include <interactive_marker_helpers/interactive_marker_helpers.h>
00066
00067 using namespace object_manipulator;
00068 using namespace visualization_msgs;
00069 using namespace interactive_markers;
00070 using namespace pr2_object_manipulation_msgs;
00071 using namespace im_helpers;
00072
00074
00075
00078 class BasePoseAction
00079 {
00080 protected:
00081
00082
00083
00084 geometry_msgs::PoseStamped base_pose_;
00085 geometry_msgs::PoseStamped control_offset_;
00086
00087 bool active_;
00088
00089 int interface_number_;
00090 int task_number_;
00091
00092 bool testing_planned_grasp_;
00093 int tested_grasp_index_;
00094 bool testing_current_grasp_;
00095 double max_direct_move_radius_;
00096
00097 ros::NodeHandle nh_;
00098 ros::NodeHandle pnh_;
00099 ros::Subscriber sub_seed_;
00100
00101 ros::Timer fast_update_timer_;
00102 ros::Timer slow_update_timer_;
00103 InteractiveMarkerServer server_;
00104
00105 ros::Publisher pub_focus_;
00106
00107 MenuHandler menu_controls_;
00108 MenuHandler::EntryHandle accept_handle_;
00109 MenuHandler::EntryHandle cancel_handle_;
00110 MenuHandler::EntryHandle focus_handle_;
00111
00112 tf::TransformListener tfl_;
00113
00114 object_manipulator::MechanismInterface mechanism_;
00115
00116 std::string get_pose_name_;
00117 actionlib::SimpleActionServer<pr2_object_manipulation_msgs::GetNavPoseAction> get_pose_server_;
00118
00119 public:
00120
00121 BasePoseAction() :
00122 active_(false),
00123 max_direct_move_radius_(10.0),
00124 nh_("/"),
00125 pnh_("~"),
00126 server_("pr2_marker_control", "nav_action", false),
00127 tfl_(nh_),
00128 get_pose_name_(ros::this_node::getName()),
00129 get_pose_server_(nh_, get_pose_name_, false)
00130 {
00131 ROS_INFO( "pr2_interactive_base_pose_action is starting up." );
00132
00133 ros::Duration(1.0).sleep();
00134
00135 nh_.param<int>("interactive_grasping/interface_number", interface_number_, 0);
00136 if (!interface_number_) ROS_WARN("No interface number specified for grasping study; using default configuration");
00137 else ROS_INFO("Using interface number %d for grasping study", interface_number_);
00138 nh_.param<int>("interactive_grasping/task_number", task_number_, 0);
00139 if (!task_number_) ROS_WARN("No task number specified for grasping study; using default configuration");
00140 else ROS_INFO("Using task number %d for grasping study", task_number_);
00141
00142
00143
00144
00145 geometry_msgs::PoseStamped ps;
00146 ps.header.frame_id = "base_link";
00147 server_.insert(makeButtonBox( "test_box", ps, 0.01, false, false));
00148
00149 pub_focus_ = nh_.advertise<pr2_object_manipulation_msgs::CameraFocus>("camera_focus", 1);
00150
00151 fast_update_timer_ = nh_.createTimer(ros::Duration(0.05), boost::bind( &BasePoseAction::fast_update, this ) );
00152
00153 sub_seed_ = nh_.subscribe<geometry_msgs::PoseStamped>("cloud_click_point", 1, boost::bind(&BasePoseAction::setSeed, this, _1));
00154
00155
00156 initMenus();
00157
00158
00159 get_pose_server_.registerGoalCallback( boost::bind(&BasePoseAction::goalCB, this));
00160 get_pose_server_.registerPreemptCallback( boost::bind(&BasePoseAction::preemptCB, this));
00161
00162 get_pose_server_.start();
00163 }
00164
00165 ~BasePoseAction()
00166 {
00167 }
00168
00169 void setSeed(const geometry_msgs::PoseStampedConstPtr &seed)
00170 {
00171 if(!active_) return;
00172 ROS_DEBUG("Setting seed.");
00173 base_pose_ = *seed;
00174 base_pose_.pose.orientation = geometry_msgs::Quaternion();
00175
00176 initMarkers();
00177 }
00178
00180 void setIdle(){
00181 active_ = false;
00182 server_.clear();
00183 }
00184
00185
00187 void goalCB()
00188 {
00189 active_ = true;
00190 ROS_INFO("pr2_interactive_nav_action_called!");
00191 pr2_object_manipulation_msgs::GetNavPoseGoal goal = *get_pose_server_.acceptNewGoal();
00192
00193 if (goal.starting_pose.pose.orientation.x == 0 &&
00194 goal.starting_pose.pose.orientation.y == 0 &&
00195 goal.starting_pose.pose.orientation.z == 0 &&
00196 goal.starting_pose.pose.orientation.w == 0 )
00197 {
00198 ROS_DEBUG("Empty pose passed in; using default");
00199 base_pose_ = getDefaultPose();
00200 }
00201 else
00202 {
00203 base_pose_ = goal.starting_pose;
00204 }
00205
00206 if (get_pose_server_.isPreemptRequested())
00207 {
00208 preemptCB();
00209 return;
00210 }
00211 if(goal.max_distance > 0)
00212 max_direct_move_radius_ = goal.max_distance;
00213 else
00214 max_direct_move_radius_ = 1E6;
00215
00216 initMarkers();
00217 }
00218
00219
00221 void preemptCB()
00222 {
00223 ROS_INFO("%s: Preempted", get_pose_name_.c_str());
00224
00225 get_pose_server_.setPreempted();
00226 setIdle();
00227 }
00228
00231 void updatePoses()
00232 {
00233 server_.setPose("meshes", base_pose_.pose, base_pose_.header);
00234 }
00235
00236 geometry_msgs::PoseStamped getDefaultPose()
00237 {
00238 geometry_msgs::PoseStamped ps;
00239 ps.header.stamp = ros::Time(0);
00240 ps.header.frame_id = "base_link";
00241 ps.pose.orientation.w = 1;
00242 return ps;
00243 }
00244
00245
00246
00247
00250 void initMarkers()
00251 {
00252 initMeshMarkers();
00253 initControlMarkers();
00254 }
00255
00256 void initMeshMarkers()
00257 {
00258 }
00259
00260 void initControlMarkers()
00261 {
00262 geometry_msgs::PoseStamped ps = base_pose_;
00263 ps.header.stamp = ros::Time(0);
00264
00265
00266
00267
00268 std::vector< std::string > mesh_paths, mesh_frames;
00269 mesh_paths.push_back("package://pr2_description/meshes/base_v0/base.dae");
00270 mesh_frames.push_back("base_link");
00271 mesh_paths.push_back("package://pr2_description/meshes/torso_v0/torso_lift.dae");
00272 mesh_frames.push_back("torso_lift_link");
00273 mesh_paths.push_back("package://pr2_description/meshes/shoulder_v0/shoulder_pan.dae");
00274 mesh_frames.push_back("r_shoulder_pan_link");
00275 mesh_paths.push_back("package://pr2_description/meshes/shoulder_v0/shoulder_pan.dae");
00276 mesh_frames.push_back("l_shoulder_pan_link");
00277 mesh_paths.push_back("package://pr2_description/meshes/shoulder_v0/shoulder_lift.dae");
00278 mesh_frames.push_back("r_shoulder_lift_link");
00279 mesh_paths.push_back("package://pr2_description/meshes/shoulder_v0/shoulder_lift.dae");
00280 mesh_frames.push_back("l_shoulder_lift_link");
00281 mesh_paths.push_back("package://pr2_description/meshes/upper_arm_v0/upper_arm.dae");
00282 mesh_frames.push_back("r_upper_arm_link");
00283 mesh_paths.push_back("package://pr2_description/meshes/upper_arm_v0/upper_arm.dae");
00284 mesh_frames.push_back("l_upper_arm_link");
00285 mesh_paths.push_back("package://pr2_description/meshes/forearm_v0/forearm.dae");
00286 mesh_frames.push_back("r_forearm_link");
00287 mesh_paths.push_back("package://pr2_description/meshes/forearm_v0/forearm.dae");
00288 mesh_frames.push_back("l_forearm_link");
00289
00290
00291 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/gripper_palm.dae");
00292 mesh_frames.push_back("r_gripper_palm_link");
00293 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/upper_finger_r.stl");
00294 mesh_frames.push_back("r_gripper_r_finger_link");
00295 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/upper_finger_l.stl");
00296 mesh_frames.push_back("r_gripper_l_finger_link");
00297 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/finger_tip_r.stl");
00298 mesh_frames.push_back("r_gripper_r_finger_tip_link");
00299 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/finger_tip_l.stl");
00300 mesh_frames.push_back("r_gripper_l_finger_tip_link");
00301
00302
00303
00304 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/gripper_palm.dae");
00305 mesh_frames.push_back("l_gripper_palm_link");
00306 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/upper_finger_r.stl");
00307 mesh_frames.push_back("l_gripper_r_finger_link");
00308 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/upper_finger_l.stl");
00309 mesh_frames.push_back("l_gripper_l_finger_link");
00310 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/finger_tip_r.stl");
00311 mesh_frames.push_back("l_gripper_r_finger_tip_link");
00312 mesh_paths.push_back("package://pr2_description/meshes/gripper_v0/finger_tip_l.stl");
00313 mesh_frames.push_back("l_gripper_l_finger_tip_link");
00314
00315
00316 mesh_paths.push_back("package://pr2_description/meshes/head_v0/head_pan.dae");
00317 mesh_frames.push_back("head_pan_link");
00318 mesh_paths.push_back("package://pr2_description/meshes/head_v0/head_tilt.dae");
00319 mesh_frames.push_back("head_tilt_link");
00320
00321
00322 if(mesh_paths.size() != mesh_frames.size()) ROS_ERROR("The number of mesh paths and mesh frame_ids is not equal!");
00323
00324 std::vector< geometry_msgs::PoseStamped > mesh_poses;
00325 for(size_t i = 0; i < mesh_paths.size(); i++)
00326 {
00327 geometry_msgs::PoseStamped ps;
00328 ps.header.stamp = ros::Time(0);
00329 ps.header.frame_id = mesh_frames[i];
00330 ps.pose.orientation.w = 1;
00331 tfl_.waitForTransform("base_link", ps.header.frame_id, ps.header.stamp,ros::Duration(3.0));
00332 try
00333 {
00334 tfl_.transformPose("base_link", ps, ps);
00335 }
00336 catch (tf::TransformException ex)
00337 {
00338 ROS_ERROR("pr2_interactive_nav_action: failed to transform from %s to base_link! Skipping mesh part", ps.header.frame_id.c_str());
00339 continue;
00340 }
00341 mesh_poses.push_back(ps);
00342 }
00343
00344 server_.insert(makePosedMultiMeshMarker( "pose_controls", ps, mesh_poses, mesh_paths, 1.03, false),
00345 boost::bind( &BasePoseAction::poseControlCB, this, _1));
00346 menu_controls_.apply(server_, "pose_controls");
00347 }
00348
00349
00350 protected:
00351
00352
00354 void fast_update()
00355 {
00356
00357 server_.applyChanges();
00358 }
00359
00360 void slow_update()
00361 {
00362 }
00363
00364
00366 void acceptCB()
00367 {
00368 setIdle();
00369 pr2_object_manipulation_msgs::GetNavPoseResult result;
00370 base_pose_.header.stamp = ros::Time::now();
00371 result.pose = base_pose_;
00372 get_pose_server_.setSucceeded(result);
00373 }
00374
00376 void cancelCB()
00377 {
00378 get_pose_server_.setAborted();
00379 setIdle();
00380 }
00381
00383 void focusCB()
00384 {
00385 pr2_object_manipulation_msgs::CameraFocus msg;
00386 msg.focal_point.point = base_pose_.pose.position;
00387 msg.focal_point.header = base_pose_.header;
00388 pub_focus_.publish(msg);
00389 }
00390
00392 void poseMeshCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00393 {
00394 }
00395
00396 geometry_msgs::PoseStamped clipToRadius(const geometry_msgs::PoseStamped &input, const float &radius)
00397 {
00398 geometry_msgs::PoseStamped ps;
00399 try
00400 {
00401 tfl_.waitForTransform(input.header.frame_id, "base_link", input.header.stamp, ros::Duration(5.0));
00402 tfl_.transformPose("base_link", input, ps);
00403 tf::Vector3 pose_center;
00404 tf::pointMsgToTF(ps.pose.position, pose_center);
00405 float stored_z = pose_center.z();
00406 pose_center.setZ(0);
00407 float length = pose_center.length();
00408 if(length > radius)
00409 {
00410 pose_center = radius / length * pose_center;
00411 pose_center.setZ(stored_z);
00412 tf::pointTFToMsg(pose_center, ps.pose.position);
00413 }
00414 ps.header.stamp = ros::Time(0);
00415 if( tfl_.canTransform("map", ps.header.frame_id, ros::Time(0)))
00416 {
00417 tfl_.transformPose("map", ps, ps);
00418 }
00419 else if( tfl_.canTransform("odom_combined", ps.header.frame_id, ros::Time(0)))
00420 {
00421 tfl_.transformPose("odom_combined", ps, ps);
00422 }
00423 else if( tfl_.canTransform(input.header.frame_id, ps.header.frame_id, ros::Time(0)))
00424 {
00425 tfl_.transformPose(input.header.frame_id, ps, ps);
00426 }
00427 else
00428 {
00429 ROS_ERROR("TF could not transform from map, odom_combined, or %s to %s!! clipToRadius failed\n", input.header.frame_id.c_str(), ps.header.frame_id.c_str());
00430 }
00431 }
00432 catch (tf::TransformException ex)
00433 {
00434 ROS_ERROR("pr2_interactive_nav_action: failed to transform from %s to base_link!", input.header.frame_id.c_str());
00435 }
00436 return ps;
00437 }
00438
00440 void poseControlCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00441 {
00442 ros::Time now = ros::Time(0);
00443
00444 switch ( feedback->event_type )
00445 {
00446 case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
00447 break;
00448 case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
00449 base_pose_.pose = feedback->pose;
00450 base_pose_.header = feedback->header;
00451 ROS_DEBUG_STREAM("MOUSE_UP base_pose before clipping is\n" << base_pose_);
00452 base_pose_ = clipToRadius(base_pose_, max_direct_move_radius_);
00453 ROS_DEBUG_STREAM("MOUSE_UP base_pose after clipping is\n" << base_pose_);
00454 initMarkers();
00455 break;
00456 case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
00457 ROS_DEBUG_STREAM("POSE_UPDATE in frame " << feedback->header.frame_id << std::endl << feedback->pose);
00458 base_pose_.pose = feedback->pose;
00459 base_pose_.header = feedback->header;
00460 updatePoses();
00461 break;
00462 }
00463 }
00464
00466 void initMenus()
00467 {
00468 accept_handle_ = menu_controls_.insert("Accept", boost::bind( &BasePoseAction::acceptCB, this ) );
00469 cancel_handle_ = menu_controls_.insert("Cancel", boost::bind( &BasePoseAction::cancelCB, this ) );
00470 focus_handle_ = menu_controls_.insert("Focus camera", boost::bind( &BasePoseAction::focusCB, this ) );
00471 }
00472 };
00473
00474 int main(int argc, char** argv)
00475 {
00476 ros::init(argc, argv, "pr2_interactive_pose_select_action");
00477 BasePoseAction base_pose_action;
00478
00479 ros::Duration(1.0).sleep();
00480
00481 ros::spin();
00482 }