pick_place.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
39 #include <moveit_msgs/DisplayTrajectory.h>
40 #include <visualization_msgs/MarkerArray.h>
42 #include <ros/console.h>
43 
44 namespace pick_place
45 {
47 const std::string PickPlace::DISPLAY_GRASP_TOPIC = "display_grasp_markers";
48 const double PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION = 7.0; // seconds
49 
50 // functionality specific to pick-only is in pick.cpp;
51 // functionality specific to place-only is in place.cpp;
52 
53 PickPlacePlanBase::PickPlacePlanBase(const PickPlaceConstPtr& pick_place, const std::string& name)
54  : pick_place_(pick_place), pipeline_(name, 4), last_plan_time_(0.0), done_(false)
55 {
58 }
59 
61 {
62 }
63 
65 {
66  boost::mutex::scoped_lock slock(done_mutex_);
67  done_ = true;
68  done_condition_.notify_all();
69 }
70 
72 {
73  boost::mutex::scoped_lock slock(done_mutex_);
75  {
76  done_ = true;
77  done_condition_.notify_all();
78  }
79 }
80 
82 {
83  done_ = false;
84  pushed_all_poses_ = false;
85 }
86 
88 {
89  // wait till we're done
90  boost::unique_lock<boost::mutex> lock(done_mutex_);
91  pushed_all_poses_ = true;
92  while (!done_ && endtime > ros::WallTime::now())
93  done_condition_.timed_wait(lock, (endtime - ros::WallTime::now()).toBoost());
94 }
95 
96 PickPlace::PickPlace(const planning_pipeline::PlanningPipelinePtr& planning_pipeline)
97  : nh_("~"), planning_pipeline_(planning_pipeline), display_computed_motion_plans_(false), display_grasps_(false)
98 {
100 }
101 
103 {
104  if (display_grasps_ && !flag)
106  else if (!display_grasps_ && flag)
107  grasps_publisher_ = nh_.advertise<visualization_msgs::MarkerArray>(DISPLAY_GRASP_TOPIC, 10, true);
108  display_grasps_ = flag;
109 }
110 
112 {
113  if (display_computed_motion_plans_ && !flag)
115  else if (!display_computed_motion_plans_ && flag)
116  display_path_publisher_ = nh_.advertise<moveit_msgs::DisplayTrajectory>(DISPLAY_PATH_TOPIC, 10, true);
118 }
119 
120 void PickPlace::visualizePlan(const ManipulationPlanPtr& plan) const
121 {
122  moveit_msgs::DisplayTrajectory dtraj;
123  dtraj.model_id = getRobotModel()->getName();
124  bool first = true;
125  for (std::size_t i = 0; i < plan->trajectories_.size(); ++i)
126  {
127  if (!plan->trajectories_[i].trajectory_ || plan->trajectories_[i].trajectory_->empty())
128  continue;
129  if (first)
130  {
131  robot_state::robotStateToRobotStateMsg(plan->trajectories_[i].trajectory_->getFirstWayPoint(),
132  dtraj.trajectory_start);
133  first = false;
134  }
135  dtraj.trajectory.resize(dtraj.trajectory.size() + 1);
136  plan->trajectories_[i].trajectory_->getRobotTrajectoryMsg(dtraj.trajectory.back());
137  }
139 }
140 
141 void PickPlace::visualizeGrasp(const ManipulationPlanPtr& plan) const
142 {
143  visualizeGrasps(std::vector<ManipulationPlanPtr>(1, plan));
144 }
145 
146 namespace
147 {
148 std::vector<std_msgs::ColorRGBA> setupDefaultGraspColors()
149 {
150  std::vector<std_msgs::ColorRGBA> result;
151  result.resize(6);
152  result[0].r = 0.5f;
153  result[0].g = 0.5f;
154  result[0].b = 0.5f;
155  result[0].a = 1.0f;
156  result[1].r = 1.0f;
157  result[1].g = 0.0f;
158  result[1].b = 0.0f;
159  result[1].a = 1.0f;
160  result[2].r = 1.0f;
161  result[2].g = 0.5f;
162  result[2].b = 0.0f;
163  result[2].a = 1.0f;
164  result[3].r = 0.0f;
165  result[3].g = 1.0f;
166  result[3].b = 1.0f;
167  result[3].a = 1.0f;
168  result[4].r = 0.0f;
169  result[4].g = 1.0f;
170  result[4].b = 0.0f;
171  result[4].a = 1.0f;
172  result[5].r = 1.0f;
173  result[5].g = 0.0f;
174  result[5].b = 1.0f;
175  result[5].a = 0.75f;
176  return result;
177 }
178 }
179 
180 void PickPlace::visualizeGrasps(const std::vector<ManipulationPlanPtr>& plans) const
181 {
182  if (plans.empty())
183  return;
184 
185  robot_state::RobotState state(getRobotModel());
186  state.setToDefaultValues();
187 
188  static std::vector<std_msgs::ColorRGBA> colors(setupDefaultGraspColors());
189  visualization_msgs::MarkerArray ma;
190  for (std::size_t i = 0; i < plans.size(); ++i)
191  {
192  const robot_model::JointModelGroup* jmg = plans[i]->shared_data_->end_effector_group_;
193  if (jmg)
194  {
195  unsigned int type = std::min(plans[i]->processing_stage_, colors.size() - 1);
196  state.updateStateWithLinkAt(plans[i]->shared_data_->ik_link_, plans[i]->transformed_goal_pose_);
197  state.getRobotMarkers(ma, jmg->getLinkModelNames(), colors[type],
198  "moveit_grasps:stage_" + boost::lexical_cast<std::string>(plans[i]->processing_stage_),
199  ros::Duration(60));
200  }
201  }
202 
204 }
205 }
ros::Publisher display_path_publisher_
Definition: pick_place.h:157
constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr constraint_sampler_manager_loader_
Definition: pick_place.h:160
static const std::string DISPLAY_GRASP_TOPIC
Definition: pick_place.h:113
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle nh_
Definition: pick_place.h:153
boost::condition_variable done_condition_
Definition: pick_place.h:86
void displayComputedMotionPlans(bool flag)
Definition: pick_place.cpp:111
ros::Publisher grasps_publisher_
Definition: pick_place.h:158
static const std::string DISPLAY_PATH_TOPIC
Definition: pick_place.h:112
void displayProcessedGrasps(bool flag)
Definition: pick_place.cpp:102
void visualizeGrasp(const ManipulationPlanPtr &plan) const
Definition: pick_place.cpp:141
void setEmptyQueueCallback(const boost::function< void()> &callback)
void visualizeGrasps(const std::vector< ManipulationPlanPtr > &plans) const
Definition: pick_place.cpp:180
ManipulationPipeline pipeline_
Definition: pick_place.h:81
void waitForPipeline(const ros::WallTime &endtime)
Definition: pick_place.cpp:87
const robot_model::RobotModelConstPtr & getRobotModel() const
Definition: pick_place.h:130
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool display_computed_motion_plans_
Definition: pick_place.h:155
static const std::string DISPLAY_PATH_TOPIC
static WallTime now()
void visualizePlan(const ManipulationPlanPtr &plan) const
Definition: pick_place.cpp:120
PickPlace(const planning_pipeline::PlanningPipelinePtr &planning_pipeline)
Definition: pick_place.cpp:96
PickPlacePlanBase(const PickPlaceConstPtr &pick_place, const std::string &name)
Definition: pick_place.cpp:53
void setSolutionCallback(const boost::function< void()> &callback)
static const double DEFAULT_GRASP_POSTURE_COMPLETION_DURATION
Definition: pick_place.h:116


manipulation
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:18:24