move_base_to_manip.cpp
Go to the documentation of this file.
00001 
00002 #include "move_base_to_manip.h"
00003 
00005 // The algorithm:
00007 
00008 // Put the end-effector at the height & orientation of the desired pose.
00009 
00010 // Calculate the X,Y vector from the ee's current pose to the desired pose.
00011 
00012 // Plan a Cartesian move to the grasp pose. What % completes? If 100%, we're done!
00013 
00014 // Based on the completed Cartesian %, how far along the (X,Y) vector must the base move?
00015 // Move the base to that position.
00016   
00017 // Should be able to reach it now. Run the program again to complete the motion or make minor
00018 // corrections as needed.
00019 
00021 
00022 
00023 int main(int argc, char **argv)
00024 {
00025   ros::init(argc, argv, "move_base_to_manip");
00026   ros::AsyncSpinner spinner(2);
00027   spinner.start();
00028   ros::NodeHandle nh;
00029   move_base_to_manip::set_node_params(nh);
00030 
00031   std::string move_group_name;
00032   nh.getParam("/move_base_to_manip/move_group_name", move_group_name);
00033   moveit::planning_interface::MoveGroup moveGroup( move_group_name );
00034   
00035   move_base_to_manip::setup_move_group(nh, moveGroup);
00036   
00037   moveit::planning_interface::MoveGroup::Plan move_plan;
00038   
00039   geometry_msgs::PoseStamped start_pose = moveGroup.getCurrentPose();
00040 
00041   // Set up services
00042   move_base_to_manip::clear_octomap_client = nh.serviceClient<std_srvs::Empty>("clear_octomap");
00043   move_base_to_manip::clear_costmaps_client = nh.serviceClient<std_srvs::Empty>("/move_base/clear_costmaps");
00044 
00046   // Get the desired EE pose from the "desired_robot_pose" service.
00048   ros::ServiceClient client = nh.serviceClient<move_base_to_manip::desired_robot_pose>("desired_robot_pose");
00049   move_base_to_manip::desired_robot_pose srv;
00050   srv.request.shutdown_service = true; // Shut down the service after it sends the pose.
00051   geometry_msgs::PoseStamped desired_pose;
00052 
00053   while ( !client.call(srv) ) // If we couldn't read the desired pose. The service prob isn't up yet
00054   {
00055     ROS_INFO_STREAM("Waiting for the 'desired_robot_pose' service.");
00056     ros::Duration(2).sleep();
00057   }
00058   
00059   desired_pose = srv.response.desired_robot_pose;
00060   
00061   // We don't want to move in (X,Y), initially
00062   geometry_msgs::PoseStamped desired_height_orient = desired_pose;
00063   desired_height_orient.pose.position.x = start_pose.pose.position.x;
00064   desired_height_orient.pose.position.y = start_pose.pose.position.y;
00065 
00067   // Put the EE at the height & orientation we desire
00069 
00070   ROS_INFO_STREAM("Moving to the desired height and orientation.");
00071   moveGroup.setPoseTarget( desired_height_orient );
00072 
00073   // Get the orientation as RPY so we can manipulate it
00074   tf::Quaternion gripper_quat;
00075   tf::quaternionMsgToTF( desired_height_orient.pose.orientation, gripper_quat );
00076   double object_roll, object_pitch, object_yaw;
00077   tf::Matrix3x3(gripper_quat).getRPY(object_roll, object_pitch, object_yaw);
00078 
00079 PLAN_AGAIN:
00080   bool ok_to_flip;
00081   nh.getParam("/move_base_to_manip/ok_to_flip", ok_to_flip);
00082   if ( !moveGroup.plan(move_plan) && ok_to_flip )  // If it fails, try spinning the gripper 180deg
00083   {
00084     geometry_msgs::Quaternion gripper_quat_msg = tf::createQuaternionMsgFromRollPitchYaw( 0., 0., object_yaw +3.14159);
00085     desired_height_orient.pose.orientation = gripper_quat_msg;
00086     moveGroup.setPoseTarget( desired_height_orient );
00087     if ( !moveGroup.plan(move_plan) ) // If it fails again, try spinning the gripper -180deg from the original attempt
00088     {  
00089       gripper_quat_msg = tf::createQuaternionMsgFromRollPitchYaw( 0., 0., object_yaw -3.14159);
00090       desired_height_orient.pose.orientation = gripper_quat_msg;
00091       moveGroup.setPoseTarget( desired_height_orient );
00092       if( !moveGroup.plan(move_plan) ) // One last attempt
00093       {
00094         ROS_ERROR("Failed to reach the desired height and orientation.");
00095         ROS_ERROR("Try starting from an arm position with better manipulability.");
00096         ros::shutdown();
00097         return FAILURE;
00098       }
00099     }
00100   }
00101 
00102   bool prompt_before_motion;
00103   nh.getParam("/move_base_to_manip/prompt_before_motion", prompt_before_motion);
00104   if ( prompt_before_motion )
00105   {
00106     char character;
00107     ROS_INFO_STREAM("Enter 'c' to continue, otherwise re-plan.");
00108     std::cin.clear();
00109     std::cin.get(character);
00110     if ( character != 'c' )
00111       goto PLAN_AGAIN;
00112   }
00113 
00114   moveGroup.execute(move_plan);
00115   
00117   // Calculate the X,Y vector from the EE's current pose to the desired pose
00119   geometry_msgs::Vector3 vec_from_cur_pose_to_goal;
00120   vec_from_cur_pose_to_goal.x = desired_pose.pose.position.x - start_pose.pose.position.x;
00121   vec_from_cur_pose_to_goal.y = desired_pose.pose.position.y - start_pose.pose.position.y;
00122   vec_from_cur_pose_to_goal.z = 0.;
00123   
00125   // Plan a Cartesian move to the grasp pose. What % completes?
00126   // If 100% complete, we're done!
00128   ROS_INFO_STREAM("Planning a Cartesian motion to the desired pose.");
00129   
00130   std::vector<geometry_msgs::Pose> waypoints;
00131   geometry_msgs::Pose cartesian_target_pose; // Cartesian motion requires a Pose (not PoseStamped)
00132   cartesian_target_pose.position = desired_pose.pose.position;
00133   cartesian_target_pose.orientation = desired_pose.pose.orientation;
00134   waypoints.push_back(cartesian_target_pose);
00135   
00136   moveit_msgs::RobotTrajectory trajectory;
00137 
00138 PLAN_CARTESIAN_AGAIN:
00139   double fraction = move_base_to_manip::cartesian_motion(waypoints, trajectory, moveGroup, nh);
00140   ROS_INFO("Cartesian path: %.2f%% achieved", fraction * 100.);
00141  
00142   if ( prompt_before_motion )
00143   { 
00144     char character;
00145     ROS_INFO_STREAM("Enter 'c' to continue, otherwise re-plan.");
00146     std::cin.ignore (INT_MAX, '\n'); // Make sure the buffer is empty. 
00147     std::cin.get(character);
00148     if ( character != 'c' )
00149       goto PLAN_CARTESIAN_AGAIN;
00150   }
00151  
00152   if ( ( 0.999 < fraction) && (fraction < 1.001) ) // We're there! Move then quit.
00153   {
00154     ROS_INFO_STREAM("Making the final move.");
00155     
00156     bool move_cartesian;
00157     nh.getParam("/move_base_to_manip/move_cartesian", move_cartesian);
00158     if ( move_cartesian ) // Use a Cartesian motion, i.e. keep the end-effector orientation constant as it moves
00159       moveGroup.move();
00160     else // Execute a regular motion
00161     {
00162       moveGroup.setPoseTarget( desired_pose );
00163       moveGroup.plan(move_plan);
00164       moveGroup.execute(move_plan);
00165     }
00166     
00167     ros::shutdown();
00168     return SUCCESS;
00169   }
00170   
00172   // Based on the completed Cartesian %, how far along the (X,Y) vector must the base move?
00173   // Move the base to that position
00175 
00176   // Tell the action client that we want to spin a thread by default
00177   MoveBaseClient ac("move_base", true);
00178 
00179   while(!ac.waitForServer(ros::Duration(5.0))){
00180     ROS_INFO("Waiting for the move_base action server to come up");
00181   }
00182   
00183   move_base_msgs::MoveBaseGoal goal;
00184   std::string base_frame_name;
00185   nh.getParam("/move_base_to_manip/base_frame_name", base_frame_name);
00186   goal.target_pose.header.frame_id = base_frame_name;
00187   goal.target_pose.header.stamp = ros::Time::now();
00188 
00189   // Goal position = current base position + calculated change
00190   // The current base position is (0,0) in /base_link by default, so we don't need to add anything
00191   geometry_msgs::PoseStamped currentPose = moveGroup.getCurrentPose();
00192   // motion_buffer: make the base move just a bit farther than the minimum req'd distance
00193   // fraction: fraction of the motion that the arm alone could complete
00194   double motion_buffer;
00195   nh.getParam("/move_base_to_manip/motion_buffer", motion_buffer);
00196   goal.target_pose.pose.position.x = (1-motion_buffer*fraction)*vec_from_cur_pose_to_goal.x;
00197   goal.target_pose.pose.position.y = (1-motion_buffer*fraction)*vec_from_cur_pose_to_goal.y;
00198   goal.target_pose.pose.position.z = 0.; // Stay in the plane
00199   // Maintain the base's current orientation
00200   goal.target_pose.pose.orientation.x = 0.;
00201   goal.target_pose.pose.orientation.y = 0.;
00202   goal.target_pose.pose.orientation.z = 0.;
00203   goal.target_pose.pose.orientation.w = 1.;
00204 
00205   // Mark the calculated base location in Rviz
00206   ros::Publisher baseVisualizationPublisher = nh.advertise<visualization_msgs::Marker>("base_pose_marker", 1);
00207   ros::Duration(1).sleep();
00208   visualization_msgs::Marker baseMarker;
00209   move_base_to_manip::setup_base_marker(baseMarker, goal);
00210   baseVisualizationPublisher.publish(baseMarker);
00211   ros::Duration(1).sleep();
00212   
00213   // May want to disable collision checking or the manipulator will not approach an object.
00214   bool clear_costmaps;
00215   if ( nh.getParam("/move_base_to_manip/clear_costmaps", clear_costmaps) )
00216     move_base_to_manip::clear_costmaps_client.call( move_base_to_manip::empty_srv );
00217 
00218   // Send the goal to move_base
00219   ac.sendGoal(goal);
00220 
00221   // If the robot still can't reach the goal (it should be very close), run this program again.
00222 
00223   ros::shutdown();
00224   return SUCCESS;
00225 }
00226 
00227 // Helper function to set node parameters, if they aren't defined in a launch file
00228 void move_base_to_manip::set_node_params(ros::NodeHandle &nh)
00229 {
00230   // Make the base move just a bit farther than the minimum req'd distance.
00231   // This should be a fraction between 0-1
00232   // Smaller ==> Will move closer to the goal pose 
00233   if (!nh.hasParam("/move_base_to_manip/motion_buffer"))
00234     nh.setParam("/move_base_to_manip/motion_buffer", 0.15);
00235 
00236   // Use a Cartesian motion plan or a regular motion plan?
00237   if (!nh.hasParam("/move_base_to_manip/move_cartesian"))
00238   {
00239     bool temp = false;
00240     nh.setParam("/move_base_to_manip/move_cartesian", temp);
00241   }
00242 
00243   // Clear the Octomap collision scene before planning the final arm motion?
00244   if (!nh.hasParam("/move_base_to_manip/clear_octomap"))
00245   {
00246     bool temp = true;  
00247     nh.setParam("/move_base_to_manip/clear_octomap", temp);
00248   }
00249 
00250   // Clear the move_base costmaps before moving the base?
00251   if (!nh.hasParam("/move_base_to_manip/clear_costmaps"))
00252   {
00253     bool temp = true;  
00254     nh.setParam("/move_base_to_manip/clear_costmaps", temp);
00255   }
00256 
00257   // Prompt the user to approve each arm motion before it executes?
00258   if (!nh.hasParam("/move_base_to_manip/prompt_before_motion"))
00259   {
00260     bool temp = true;
00261     nh.setParam("/move_base_to_manip/prompt_before_motion", temp);
00262   }
00263 
00264   // Cartesian planning resolution, in meters
00265   if (!nh.hasParam("/move_base_to_manip/cartesian_plan_res"))
00266     nh.setParam("/move_base_to_manip/cartesian_plan_res", 0.005);
00267 
00268   if (!nh.hasParam("/move_base_to_manip/move_group_name"))
00269     nh.setParam("/move_base_to_manip/move_group_name", "right_ur5");
00270 
00271   if (!nh.hasParam("/move_base_to_manip/move_group_planner"))
00272     nh.setParam("/move_base_to_manip/move_group_planner", "RRTConnectkConfigDefault");
00273 
00274   if (!nh.hasParam("/move_base_to_manip/velocity_scale"))
00275     nh.setParam("/move_base_to_manip/velocity_scale", 0.1);
00276 
00277   if (!nh.hasParam("/move_base_to_manip/base_frame_name"))
00278     nh.setParam("/move_base_to_manip/base_frame_name", "base_link");
00279     
00280  if (!nh.hasParam("/move_base_to_manip/position_tolerance"))
00281     nh.setParam("/move_base_to_manip/position_tolerance", 0.01);
00282     
00283  if (!nh.hasParam("/move_base_to_manip/orientation_tolerance"))
00284     nh.setParam("/move_base_to_manip/orientation_tolerance", 0.0001);
00285 
00286  // If true, the planner will try to flip the gripper +/-180 deg about Z when it cannot reach a pose
00287  if (!nh.hasParam("/move_base_to_manip/ok_to_flip"))
00288  {
00289    bool temp = true;
00290    nh.setParam("/move_base_to_manip/ok_to_flip", temp);
00291   }
00292 }
00293 
00294 // Helper function to plan a Cartesian motion
00295 const double move_base_to_manip::cartesian_motion(const std::vector<geometry_msgs::Pose>& waypoints, moveit_msgs::RobotTrajectory& trajectory, moveit::planning_interface::MoveGroup& moveGroup, ros::NodeHandle &nh)
00296 {
00297   // May want to disable collision checking or the manipulator will not approach an object.
00298   bool clear_octomap;
00299   if ( nh.getParam("/move_base_to_manip/clear_octomap", clear_octomap) )
00300     move_base_to_manip::clear_octomap_client.call(empty_srv);
00301   double cartesian_path_resolution;
00302   nh.getParam("/move_base_to_manip/cartesian_plan_res", cartesian_path_resolution);
00303   double fraction = moveGroup.computeCartesianPath( waypoints, cartesian_path_resolution, 0.0, trajectory);
00304 
00305   return fraction;
00306 }
00307 
00308 // Helper function to initialize move_group
00309 void move_base_to_manip::setup_move_group(ros::NodeHandle& nh, moveit::planning_interface::MoveGroup& moveGroup)
00310 {
00311   std::string move_group_planner;
00312   nh.getParam("/move_base_to_manip/move_group_planner", move_group_planner);
00313   moveGroup.setPlannerId( move_group_planner );
00314   double velocity_scale;
00315   nh.getParam("/move_base_to_manip/velocity_scale", velocity_scale);
00316   moveGroup.setMaxVelocityScalingFactor( velocity_scale );
00317   
00318   double pos_tol;
00319   nh.getParam("/move_base_to_manip/position_tolerance", pos_tol);
00320   moveGroup.setGoalPositionTolerance(pos_tol);
00321   
00322   double orient_tol;
00323   nh.getParam("/move_base_to_manip/orientation_tolerance", orient_tol);
00324   moveGroup.setGoalOrientationTolerance(orient_tol);
00325 }
00326 
00327 // Helper function to set the RViz marker
00328 void move_base_to_manip::setup_base_marker(visualization_msgs::Marker& baseMarker, move_base_msgs::MoveBaseGoal& goal)
00329 {
00330   baseMarker.header = goal.target_pose.header;
00331   baseMarker.id = 927;
00332   baseMarker.ns = "basic_shapes";
00333   baseMarker.type = visualization_msgs::Marker::CUBE;
00334   baseMarker.action = visualization_msgs::Marker::ADD;
00335   baseMarker.pose = goal.target_pose.pose;
00336 
00337   baseMarker.scale.x = 0.22;
00338   baseMarker.scale.y = 0.08;
00339   baseMarker.scale.z = 0.08;
00340   baseMarker.color.a = 1.0;
00341   baseMarker.color.r = 1.0f;
00342   baseMarker.color.g = 0.0f;
00343   baseMarker.color.b = 0.0f;
00344   baseMarker.lifetime = ros::Duration();
00345 }


move_base_to_manip
Author(s): Andy Zelenak, Andy Zelenak
autogenerated on Fri May 5 2017 02:30:02