moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
motion_planning_frame_manipulation.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: Sachin Chitta */
36
39
42#include <object_recognition_msgs/action/object_recognition.hpp>
43
44#include <tf2_eigen/tf2_eigen.hpp>
45
46#include "ui_motion_planning_rviz_plugin_frame.h"
47
48namespace moveit_rviz_plugin
49{
50
52void MotionPlanningFrame::detectObjectsButtonClicked()
53{
54 // TODO (ddengster): Enable when moveit_ros_perception is ported
55 // if (!semantic_world_)
56 // {
57 // const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
58 // if (ps)
59 // {
60 // semantic_world_ = std::make_shared<moveit::semantic_world::SemanticWorld>(ps);
61 // }
62 // if (semantic_world_)
63 // {
64 // semantic_world_->addTableCallback([this] { updateTables(); });
65 // }
66 // }
67 planning_display_->addBackgroundJob([this] { triggerObjectDetection(); }, "detect objects");
68}
69
70void MotionPlanningFrame::processDetectedObjects()
71{
72 pick_object_name_.clear();
73
74 std::vector<std::string> object_ids;
75 double min_x = ui_->roi_center_x->value() - ui_->roi_size_x->value() / 2.0;
76 double min_y = ui_->roi_center_y->value() - ui_->roi_size_y->value() / 2.0;
77 double min_z = ui_->roi_center_z->value() - ui_->roi_size_z->value() / 2.0;
78
79 double max_x = ui_->roi_center_x->value() + ui_->roi_size_x->value() / 2.0;
80 double max_y = ui_->roi_center_y->value() + ui_->roi_size_y->value() / 2.0;
81 double max_z = ui_->roi_center_z->value() + ui_->roi_size_z->value() / 2.0;
82
83 rclcpp::Time start_time = rclcpp::Clock().now();
84 while (object_ids.empty() && (rclcpp::Clock().now() - start_time) <= rclcpp::Duration::from_seconds(3))
85 {
86 // collect all objects in region of interest
87 {
89 const collision_detection::WorldConstPtr& world = ps->getWorld();
90 object_ids.clear();
91 for (const auto& object : *world)
92 {
93 const auto& position = object.second->pose_.translation();
94 if (position.x() >= min_x && position.x() <= max_x && position.y() >= min_y && position.y() <= max_y &&
95 position.z() >= min_z && position.z() <= max_z)
96 {
97 object_ids.push_back(object.first);
98 }
99 }
100 if (!object_ids.empty())
101 break;
102 }
103 rclcpp::sleep_for(std::chrono::milliseconds(500));
104 }
105
106 RCLCPP_DEBUG(logger_, "Found %d objects", static_cast<int>(object_ids.size()));
107 updateDetectedObjectsList(object_ids);
108}
109
110void MotionPlanningFrame::selectedDetectedObjectChanged()
111{
112 QList<QListWidgetItem*> sel = ui_->detected_objects_list->selectedItems();
113 if (sel.empty())
114 {
115 RCLCPP_INFO(logger_, "No objects to select");
116 return;
117 }
119 std_msgs::msg::ColorRGBA pick_object_color;
120 pick_object_color.r = 1.0;
121 pick_object_color.g = 0.0;
122 pick_object_color.b = 0.0;
123 pick_object_color.a = 1.0;
124
125 if (ps)
126 {
127 if (!selected_object_name_.empty())
128 ps->removeObjectColor(selected_object_name_);
129 selected_object_name_ = sel[0]->text().toStdString();
130 ps->setObjectColor(selected_object_name_, pick_object_color);
131 }
132}
133
134void MotionPlanningFrame::detectedObjectChanged(QListWidgetItem* /*item*/)
135{
136}
137
138void MotionPlanningFrame::triggerObjectDetection()
139{
140 if (!object_recognition_client_)
141 {
142 object_recognition_client_ = rclcpp_action::create_client<object_recognition_msgs::action::ObjectRecognition>(
144 if (!object_recognition_client_->wait_for_action_server(std::chrono::seconds(3)))
145 {
146 RCLCPP_ERROR(logger_, "Object recognition action server not responsive");
147 return;
148 }
149 }
150
151 object_recognition_msgs::action::ObjectRecognition::Goal goal;
152 auto goal_handle_future = object_recognition_client_->async_send_goal(goal);
153 goal_handle_future.wait();
154 if (goal_handle_future.get()->get_status() != rclcpp_action::GoalStatus::STATUS_SUCCEEDED)
155 {
156 RCLCPP_ERROR(logger_, "ObjectRecognition client: send goal call failed");
157 return;
158 }
159}
160
161void MotionPlanningFrame::listenDetectedObjects(
162 const object_recognition_msgs::msg::RecognizedObjectArray::ConstSharedPtr& /*msg*/)
163{
164 rclcpp::sleep_for(std::chrono::seconds(1));
165 planning_display_->addMainLoopJob([this] { processDetectedObjects(); });
166}
167
168void MotionPlanningFrame::updateDetectedObjectsList(const std::vector<std::string>& object_ids)
169{
170 ui_->detected_objects_list->setUpdatesEnabled(false);
171 bool old_state = ui_->detected_objects_list->blockSignals(true);
172 ui_->detected_objects_list->clear();
173 {
174 for (std::size_t i = 0; i < object_ids.size(); ++i)
175 {
176 QListWidgetItem* item =
177 new QListWidgetItem(QString::fromStdString(object_ids[i]), ui_->detected_objects_list, static_cast<int>(i));
178 item->setToolTip(item->text());
179 Qt::ItemFlags flags = item->flags();
180 flags &= ~(Qt::ItemIsUserCheckable);
181 item->setFlags(flags);
182 ui_->detected_objects_list->addItem(item);
183 }
184 }
185 ui_->detected_objects_list->blockSignals(old_state);
186 ui_->detected_objects_list->setUpdatesEnabled(true);
187 if (!object_ids.empty())
188 ui_->pick_button->setEnabled(true);
189}
190
192void MotionPlanningFrame::updateTables()
193{
194 RCLCPP_DEBUG(logger_, "Update table callback");
195 planning_display_->addBackgroundJob([this] { publishTables(); }, "publish tables");
196}
197
198void MotionPlanningFrame::publishTables()
199{
200 // TODO (ddengster): Enable when moveit_ros_perception is ported
201 // semantic_world_->addTablesToCollisionWorld();
202
203 planning_display_->addMainLoopJob([this] { updateSupportSurfacesList(); });
204}
205
206void MotionPlanningFrame::selectedSupportSurfaceChanged()
207{
208 QList<QListWidgetItem*> sel = ui_->support_surfaces_list->selectedItems();
209 if (sel.empty())
210 {
211 RCLCPP_INFO(logger_, "No tables to select");
212 return;
213 }
215 std_msgs::msg::ColorRGBA selected_support_surface_color;
216 selected_support_surface_color.r = 0.0;
217 selected_support_surface_color.g = 0.0;
218 selected_support_surface_color.b = 1.0;
219 selected_support_surface_color.a = 1.0;
220
221 if (ps)
222 {
223 if (!selected_support_surface_name_.empty())
224 ps->removeObjectColor(selected_support_surface_name_);
225 selected_support_surface_name_ = sel[0]->text().toStdString();
226 ps->setObjectColor(selected_support_surface_name_, selected_support_surface_color);
227 }
228}
229
230void MotionPlanningFrame::updateSupportSurfacesList()
231{
232 // TODO (ddengster): Enable when moveit_ros_perception is ported
233 // double min_x = ui_->roi_center_x->value() - ui_->roi_size_x->value() / 2.0;
234 // double min_y = ui_->roi_center_y->value() - ui_->roi_size_y->value() / 2.0;
235 // double min_z = ui_->roi_center_z->value() - ui_->roi_size_z->value() / 2.0;
236
237 // double max_x = ui_->roi_center_x->value() + ui_->roi_size_x->value() / 2.0;
238 // double max_y = ui_->roi_center_y->value() + ui_->roi_size_y->value() / 2.0;
239 // double max_z = ui_->roi_center_z->value() + ui_->roi_size_z->value() / 2.0;
240 // std::vector<std::string> support_ids = semantic_world_->getTableNamesInROI(min_x, min_y, min_z, max_x, max_y,
241 // max_z);
242 std::vector<std::string> support_ids;
243 RCLCPP_INFO(logger_, "%d Tables in collision world", static_cast<int>(support_ids.size()));
244
245 ui_->support_surfaces_list->setUpdatesEnabled(false);
246 bool old_state = ui_->support_surfaces_list->blockSignals(true);
247 ui_->support_surfaces_list->clear();
248 {
249 for (std::size_t i = 0; i < support_ids.size(); ++i)
250 {
251 QListWidgetItem* item =
252 new QListWidgetItem(QString::fromStdString(support_ids[i]), ui_->support_surfaces_list, static_cast<int>(i));
253 item->setToolTip(item->text());
254 Qt::ItemFlags flags = item->flags();
255 flags &= ~(Qt::ItemIsUserCheckable);
256 item->setFlags(flags);
257 ui_->support_surfaces_list->addItem(item);
258 }
259 }
260 ui_->support_surfaces_list->blockSignals(old_state);
261 ui_->support_surfaces_list->setUpdatesEnabled(true);
262}
263
265void MotionPlanningFrame::pickObjectButtonClicked()
266{
267 RCLCPP_WARN_STREAM(logger_, "Pick & Place capability isn't supported yet");
268 // QList<QListWidgetItem*> sel = ui_->detected_objects_list->selectedItems();
269 // QList<QListWidgetItem*> sel_table = ui_->support_surfaces_list->selectedItems();
270 //
271 // std::string group_name = planning_display_->getCurrentPlanningGroup();
272 // if (sel.empty())
273 // {
274 // RCLCPP_INFO(logger_, "No objects to pick");
275 // return;
276 // }
277 // pick_object_name_[group_name] = sel[0]->text().toStdString();
278 //
279 // if (!sel_table.empty())
280 // support_surface_name_ = sel_table[0]->text().toStdString();
281 // else
282 // {
283 // if (semantic_world_)
284 // {
285 // const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
286 // if (ps->getWorld()->hasObject(pick_object_name_[group_name]))
287 // {
288 // geometry_msgs::msg::Pose
289 // object_pose(tf2::toMsg(ps->getWorld()->getTransform(pick_object_name_[group_name])));
290 // RCLCPP_DEBUG(logger_, "Finding current table for object: " << pick_object_name_[group_name]);
291 // support_surface_name_ = semantic_world_->findObjectTable(object_pose);
292 // }
293 // else
294 // support_surface_name_.clear();
295 // }
296 // else
297 // support_surface_name_.clear();
298 // }
299 // RCLCPP_INFO(logger_, "Trying to pick up object %s from support surface %s", pick_object_name_[group_name].c_str(),
300 // support_surface_name_.c_str());
301 // planning_display_->addBackgroundJob([this] { pickObject(); }, "pick");
302}
303
304void MotionPlanningFrame::placeObjectButtonClicked()
305{
306 QList<QListWidgetItem*> sel_table = ui_->support_surfaces_list->selectedItems();
307 std::string group_name = planning_display_->getCurrentPlanningGroup();
308
309 if (!sel_table.empty())
310 {
311 support_surface_name_ = sel_table[0]->text().toStdString();
312 }
313 else
314 {
315 support_surface_name_.clear();
316 RCLCPP_ERROR(logger_, "Need to specify table to place object on");
317 return;
318 }
319
320 ui_->pick_button->setEnabled(false);
321 ui_->place_button->setEnabled(false);
322
323 std::vector<const moveit::core::AttachedBody*> attached_bodies;
325 if (!ps)
326 {
327 RCLCPP_ERROR(logger_, "No planning scene");
328 return;
329 }
330 const moveit::core::JointModelGroup* jmg = ps->getCurrentState().getJointModelGroup(group_name);
331 if (jmg)
332 ps->getCurrentState().getAttachedBodies(attached_bodies, jmg);
333
334 if (attached_bodies.empty())
335 {
336 RCLCPP_ERROR(logger_, "No bodies to place");
337 return;
338 }
339
340 geometry_msgs::msg::Quaternion upright_orientation;
341 upright_orientation.w = 1.0;
342
343 // Else place the first one
344 place_poses_.clear();
345 // TODO (ddengster): Enable when moveit_ros_perception is ported
346 // place_poses_ = semantic_world_->generatePlacePoses(support_surface_name_, attached_bodies[0]->getShapes()[0],
347 // upright_orientation, 0.1);
348 // planning_display_->visualizePlaceLocations(place_poses_);
349 // place_object_name_ = attached_bodies[0]->getName();
350 // planning_display_->addBackgroundJob([this] { placeObject(); }, "place");
351}
352
353// void MotionPlanningFrame::pickObject()
354//{
355// std::string group_name = planning_display_->getCurrentPlanningGroup();
356// ui_->pick_button->setEnabled(false);
357// if (pick_object_name_.find(group_name) == pick_object_name_.end())
358// {
359// RCLCPP_ERROR(logger_, "No pick object set for this group");
360// return;
361// }
362// if (!support_surface_name_.empty())
363// {
364// move_group_->setSupportSurfaceName(support_surface_name_);
365// }
366// if (move_group_->pick(pick_object_name_[group_name]))
367// {
368// ui_->place_button->setEnabled(true);
369// }
370//}
371//
372// void MotionPlanningFrame::placeObject()
373// move_group_->place(place_object_name_, place_poses_);
374// return;
375//}
376} // namespace moveit_rviz_plugin
planning_scene_monitor::LockedPlanningSceneRO getPlanningSceneRO() const
get read-only access to planning scene
planning_scene_monitor::LockedPlanningSceneRW getPlanningSceneRW()
get write access to planning scene
void addMainLoopJob(const std::function< void()> &job)
queue the execution of this function for the next time the main update() loop gets called
void addBackgroundJob(const std::function< void()> &job, const std::string &name)
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
const std::string OBJECT_RECOGNITION_ACTION