moveit2
The MoveIt Motion Planning Framework for ROS 2.
planning_pipeline_test_plugins.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2023, PickNik 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 PickNik Inc. 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: Sebastian Jahr */
36 
37 #include <chrono>
41 #include <class_loader/class_loader.hpp>
42 
44 {
47 {
48 public:
49  std::string getDescription() const override
50  {
51  return "AlwaysSuccessRequestAdapter";
52  }
53  moveit::core::MoveItErrorCode adapt(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/,
54  planning_interface::MotionPlanRequest& /*req*/) const override
55  {
56  // Mock light computations
57  std::this_thread::sleep_for(std::chrono::milliseconds(100));
58  return moveit::core::MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, std::string(""), getDescription());
59  }
60 };
61 
64 {
65 public:
66  std::string getDescription() const override
67  {
68  return "AlwaysSuccessResponseAdapter";
69  }
70  void adapt(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/,
72  planning_interface::MotionPlanResponse& res) const override
73  {
74  // Mock light computations
75  std::this_thread::sleep_for(std::chrono::milliseconds(100));
76  res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
77  }
78 };
79 
82 {
83 public:
84  DummyPlanningContext() : planning_interface::PlanningContext("DummyPlanningContext", "empty_group")
85  {
86  }
88  {
89  // Mock heavy computations
90  std::this_thread::sleep_for(std::chrono::seconds(1));
91  // Planning succeeded
92  res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
93  }
95  {
96  // Mock heavy computations
97  std::this_thread::sleep_for(std::chrono::seconds(1));
98  res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
99  }
100  bool terminate() override
101  {
102  return true;
103  }
104  void clear() override{};
105 };
106 
109 {
110 public:
112  {
113  }
114  planning_interface::PlanningContextPtr
115  getPlanningContext(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/,
117  moveit_msgs::msg::MoveItErrorCodes& /*error_code*/) const override
118  {
119  return std::make_shared<DummyPlanningContext>();
120  }
121  bool canServiceRequest(const planning_interface::MotionPlanRequest& /*req*/) const override
122  {
123  return true;
124  }
125  std::string getDescription() const override
126  {
127  return std::string("DummyPlannerManager");
128  }
129 };
130 } // namespace planning_pipeline_test
131 
a wrapper around moveit_msgs::MoveItErrorCodes to make it easier to return an error code message from...
Base class for a MoveIt planner.
Representation of a particular planning context – the planning scene and the request are known,...
PlanningContext(const std::string &name, const std::string &group)
Construct a planning context named name for the group group.
Concept in MoveIt which can be used to modify the planning problem(pre-processing) in a planning pipe...
Concept in MoveIt which can be used to modify the planning solution (post-processing) in a planning p...
A dummy request adapter that does nothing and is always successful.
std::string getDescription() const override
Get a description of this adapter.
moveit::core::MoveItErrorCode adapt(const planning_scene::PlanningSceneConstPtr &, planning_interface::MotionPlanRequest &) const override
Adapt the planning request.
A dummy response adapter that does nothing and is always successful.
std::string getDescription() const override
Get a description of this adapter.
void adapt(const planning_scene::PlanningSceneConstPtr &, const planning_interface::MotionPlanRequest &, planning_interface::MotionPlanResponse &res) const override
Adapt the planning response.
A dummy planning manager that does nothing.
bool canServiceRequest(const planning_interface::MotionPlanRequest &) const override
Determine whether this plugin instance is able to represent this planning request.
planning_interface::PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr &, const planning_interface::MotionPlanRequest &, moveit_msgs::msg::MoveItErrorCodes &) const override
Construct a planning context given the current scene and a planning request. If a problem is encounte...
std::string getDescription() const override
Get a short string that identifies the planning interface.
A dummy planning context that does nothing and is always successful.
bool terminate() override
If solve() is running, terminate the computation. Return false if termination not possible....
void clear() override
Clear the data structures used by the planner.
void solve(planning_interface::MotionPlanResponse &res) override
Solve the motion planning problem and store the result in res. This function should not clear data st...
void solve(planning_interface::MotionPlanDetailedResponse &res) override
Solve the motion planning problem and store the detailed result in res. This function should not clea...
This namespace includes the base class for MoveIt planners.
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
CLASS_LOADER_REGISTER_CLASS(default_planning_request_adapters::ResolveConstraintFrames, planning_interface::PlanningRequestAdapter)
moveit_msgs::msg::MoveItErrorCodes error_code
Response to a planning query.
moveit::core::MoveItErrorCode error_code