Go to the documentation of this file.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
00035
00036
00037 #include <moveit/pick_place/pick_place.h>
00038 #include <moveit/robot_state/conversions.h>
00039 #include <moveit_msgs/DisplayTrajectory.h>
00040 #include <visualization_msgs/MarkerArray.h>
00041 #include <eigen_conversions/eigen_msg.h>
00042 #include <ros/console.h>
00043
00044 namespace pick_place
00045 {
00046
00047 const std::string PickPlace::DISPLAY_PATH_TOPIC = planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC;
00048 const std::string PickPlace::DISPLAY_GRASP_TOPIC = "display_grasp_markers";
00049 const double PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION = 7.0;
00050
00051
00052
00053
00054 PickPlacePlanBase::PickPlacePlanBase(const PickPlaceConstPtr &pick_place, const std::string &name) :
00055 pick_place_(pick_place),
00056 pipeline_(name, 4),
00057 last_plan_time_(0.0),
00058 done_(false)
00059 {
00060 pipeline_.setSolutionCallback(boost::bind(&PickPlacePlanBase::foundSolution, this));
00061 pipeline_.setEmptyQueueCallback(boost::bind(&PickPlacePlanBase::emptyQueue, this));
00062 }
00063
00064 PickPlacePlanBase::~PickPlacePlanBase()
00065 {
00066 }
00067
00068 void PickPlacePlanBase::foundSolution()
00069 {
00070 boost::mutex::scoped_lock slock(done_mutex_);
00071 done_ = true;
00072 done_condition_.notify_all();
00073 }
00074
00075 void PickPlacePlanBase::emptyQueue()
00076 {
00077 boost::mutex::scoped_lock slock(done_mutex_);
00078 if (pushed_all_poses_)
00079 {
00080 done_ = true;
00081 done_condition_.notify_all();
00082 }
00083 }
00084
00085 void PickPlacePlanBase::initialize()
00086 {
00087 done_ = false;
00088 pushed_all_poses_ = false;
00089 }
00090
00091 void PickPlacePlanBase::waitForPipeline(const ros::WallTime &endtime)
00092 {
00093
00094 boost::unique_lock<boost::mutex> lock(done_mutex_);
00095 pushed_all_poses_ = true;
00096 while (!done_ && endtime > ros::WallTime::now())
00097 done_condition_.timed_wait(lock, (endtime - ros::WallTime::now()).toBoost());
00098 }
00099
00100 PickPlace::PickPlace(const planning_pipeline::PlanningPipelinePtr &planning_pipeline) :
00101 nh_("~"),
00102 planning_pipeline_(planning_pipeline),
00103 display_computed_motion_plans_(false),
00104 display_grasps_(false)
00105 {
00106 constraint_sampler_manager_loader_.reset(new constraint_sampler_manager_loader::ConstraintSamplerManagerLoader());
00107 }
00108
00109 void PickPlace::displayProcessedGrasps(bool flag)
00110 {
00111 if (display_grasps_ && !flag)
00112 grasps_publisher_.shutdown();
00113 else
00114 if (!display_grasps_ && flag)
00115 grasps_publisher_ = nh_.advertise<visualization_msgs::MarkerArray>(DISPLAY_GRASP_TOPIC, 10, true);
00116 display_grasps_ = flag;
00117 }
00118
00119 void PickPlace::displayComputedMotionPlans(bool flag)
00120 {
00121 if (display_computed_motion_plans_ && !flag)
00122 display_path_publisher_.shutdown();
00123 else
00124 if (!display_computed_motion_plans_ && flag)
00125 display_path_publisher_ = nh_.advertise<moveit_msgs::DisplayTrajectory>(DISPLAY_PATH_TOPIC, 10, true);
00126 display_computed_motion_plans_ = flag;
00127 }
00128
00129 void PickPlace::visualizePlan(const ManipulationPlanPtr &plan) const
00130 {
00131 moveit_msgs::DisplayTrajectory dtraj;
00132 dtraj.model_id = getRobotModel()->getName();
00133 bool first = true;
00134 for (std::size_t i = 0 ; i < plan->trajectories_.size() ; ++i)
00135 {
00136 if (!plan->trajectories_[i].trajectory_ || plan->trajectories_[i].trajectory_->empty())
00137 continue;
00138 if (first)
00139 {
00140 robot_state::robotStateToRobotStateMsg(plan->trajectories_[i].trajectory_->getFirstWayPoint(), dtraj.trajectory_start);
00141 first = false;
00142 }
00143 dtraj.trajectory.resize(dtraj.trajectory.size() + 1);
00144 plan->trajectories_[i].trajectory_->getRobotTrajectoryMsg(dtraj.trajectory.back());
00145 }
00146 display_path_publisher_.publish(dtraj);
00147 }
00148
00149 void PickPlace::visualizeGrasp(const ManipulationPlanPtr &plan) const
00150 {
00151 visualizeGrasps(std::vector<ManipulationPlanPtr>(1, plan));
00152 }
00153
00154 namespace
00155 {
00156
00157 std::vector<std_msgs::ColorRGBA> setupDefaultGraspColors()
00158 {
00159 std::vector<std_msgs::ColorRGBA> result;
00160 result.resize(6);
00161 result[0].r = 0.5f; result[0].g = 0.5f; result[0].b = 0.5f; result[0].a = 1.0f;
00162 result[1].r = 1.0f; result[1].g = 0.0f; result[1].b = 0.0f; result[1].a = 1.0f;
00163 result[2].r = 1.0f; result[2].g = 0.5f; result[2].b = 0.0f; result[2].a = 1.0f;
00164 result[3].r = 0.0f; result[3].g = 1.0f; result[3].b = 1.0f; result[3].a = 1.0f;
00165 result[4].r = 0.0f; result[4].g = 1.0f; result[4].b = 0.0f; result[4].a = 1.0f;
00166 result[5].r = 1.0f; result[5].g = 0.0f; result[5].b = 1.0f; result[5].a = 0.75f;
00167 return result;
00168 }
00169
00170 }
00171
00172 void PickPlace::visualizeGrasps(const std::vector<ManipulationPlanPtr>& plans) const
00173 {
00174 if (plans.empty())
00175 return;
00176
00177 robot_state::RobotState state(getRobotModel());
00178 state.setToDefaultValues();
00179
00180 static std::vector<std_msgs::ColorRGBA> colors(setupDefaultGraspColors());
00181 visualization_msgs::MarkerArray ma;
00182 for (std::size_t i = 0 ; i < plans.size() ; ++i)
00183 {
00184 const robot_model::JointModelGroup *jmg = plans[i]->shared_data_->end_effector_group_;
00185 if (jmg)
00186 {
00187 unsigned int type = std::min(plans[i]->processing_stage_, colors.size() - 1);
00188 state.updateStateWithLinkAt(plans[i]->shared_data_->ik_link_, plans[i]->transformed_goal_pose_);
00189 state.getRobotMarkers(ma, jmg->getLinkModelNames(), colors[type], "moveit_grasps:stage_" + boost::lexical_cast<std::string>(plans[i]->processing_stage_), ros::Duration(60));
00190 }
00191 }
00192
00193 grasps_publisher_.publish(ma);
00194 }
00195
00196 }