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