Skip to content

Commit d97dca6

Browse files
Levi-ArmstrongLevi-Armstrong
committed
Add raster only process managers
1 parent e26d475 commit d97dca6

File tree

11 files changed

+1451
-169
lines changed

11 files changed

+1451
-169
lines changed

‎tesseract/tesseract_planning/tesseract_process_managers/CMakeLists.txt

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,9 @@ add_library(${PROJECT_NAME}
4848
src/process_generators/motion_planner_process_generator.cpp
4949
src/process_generators/profile_switch_process_generator.cpp
5050
src/process_managers/raster_process_manager.cpp
51+
src/process_managers/raster_global_process_manager.cpp
52+
src/process_managers/raster_only_process_manager.cpp
53+
src/process_managers/raster_only_global_process_manager.cpp
5154
src/process_managers/raster_dt_process_manager.cpp
5255
src/process_managers/raster_waad_process_manager.cpp
5356
src/process_managers/raster_waad_dt_process_manager.cpp

‎tesseract/tesseract_planning/tesseract_process_managers/examples/raster_example_program.h

Lines changed: 86 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -136,6 +136,92 @@ inline CompositeInstruction rasterExampleProgram()
136136
return program;
137137
}
138138

139+
inline CompositeInstruction rasterOnlyExampleProgram()
140+
{
141+
CompositeInstruction program("raster_program", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator"));
142+
143+
Waypoint wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.8) *
144+
Eigen::Quaterniond(0, 0, -1.0, 0));
145+
146+
// Define start instruction
147+
PlanInstruction start_instruction(wp1, PlanInstructionType::START);
148+
start_instruction.setDescription("Start Instruction");
149+
program.setStartInstruction(start_instruction);
150+
151+
for (int i = 0; i < 4; ++i)
152+
{
153+
double x = 0.8 + (i * 0.1);
154+
Waypoint wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(x, -0.3, 0.8) *
155+
Eigen::Quaterniond(0, 0, -1.0, 0));
156+
Waypoint wp2 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(x, -0.2, 0.8) *
157+
Eigen::Quaterniond(0, 0, -1.0, 0));
158+
Waypoint wp3 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(x, -0.1, 0.8) *
159+
Eigen::Quaterniond(0, 0, -1.0, 0));
160+
Waypoint wp4 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(x, 0.0, 0.8) *
161+
Eigen::Quaterniond(0, 0, -1.0, 0));
162+
Waypoint wp5 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(x, 0.1, 0.8) *
163+
Eigen::Quaterniond(0, 0, -1.0, 0));
164+
Waypoint wp6 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(x, 0.2, 0.8) *
165+
Eigen::Quaterniond(0, 0, -1.0, 0));
166+
Waypoint wp7 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(x, 0.3, 0.8) *
167+
Eigen::Quaterniond(0, 0, -1.0, 0));
168+
169+
CompositeInstruction raster_segment;
170+
raster_segment.setDescription("Raster #" + std::to_string(i + 1));
171+
if (i == 0 || i == 2)
172+
{
173+
raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, "RASTER"));
174+
raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, "RASTER"));
175+
raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, "RASTER"));
176+
raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, "RASTER"));
177+
raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, "RASTER"));
178+
raster_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, "RASTER"));
179+
}
180+
else
181+
{
182+
raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, "RASTER"));
183+
raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, "RASTER"));
184+
raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, "RASTER"));
185+
raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, "RASTER"));
186+
raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, "RASTER"));
187+
raster_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, "RASTER"));
188+
}
189+
program.push_back(raster_segment);
190+
191+
// Add transition
192+
if (i == 0 || i == 2)
193+
{
194+
Waypoint wp7 =
195+
CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + ((i + 1) * 0.1), 0.3, 0.8) *
196+
Eigen::Quaterniond(0, 0, -1.0, 0));
197+
198+
PlanInstruction plan_f1(wp7, PlanInstructionType::FREESPACE, "freespace_profile");
199+
plan_f1.setDescription("transition_from_end_plan");
200+
201+
CompositeInstruction transition;
202+
transition.setDescription("transition_from_end");
203+
transition.push_back(plan_f1);
204+
program.push_back(transition);
205+
}
206+
else if (i == 1)
207+
{
208+
Waypoint wp1 =
209+
CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + ((i + 1) * 0.1), -0.3, 0.8) *
210+
Eigen::Quaterniond(0, 0, -1.0, 0));
211+
212+
PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, "freespace_profile");
213+
plan_f1.setDescription("transition_from_end_plan");
214+
215+
CompositeInstruction transition;
216+
transition.setDescription("transition_from_end");
217+
transition.push_back(plan_f1);
218+
program.push_back(transition);
219+
}
220+
}
221+
222+
return program;
223+
}
224+
139225
} // namespace tesseract_planning
140226

141227
#endif

‎tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_managers/raster_global_process_manager.h

Lines changed: 124 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,124 @@
1+
/**
2+
* @file raster_global_process_manager.h
3+
* @brief Plans raster paths
4+
*
5+
* @author Matthew Powelson
6+
* @date July 15, 2020
7+
* @version TODO
8+
* @bug No known bugs
9+
*
10+
* @copyright Copyright (c) 2020, Southwest Research Institute
11+
*
12+
* @par License
13+
* Software License Agreement (Apache License)
14+
* @par
15+
* Licensed under the Apache License, Version 2.0 (the "License");
16+
* you may not use this file except in compliance with the License.
17+
* You may obtain a copy of the License at
18+
* http://www.apache.org/licenses/LICENSE-2.0
19+
* @par
20+
* Unless required by applicable law or agreed to in writing, software
21+
* distributed under the License is distributed on an "AS IS" BASIS,
22+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
23+
* See the License for the specific language governing permissions and
24+
* limitations under the License.
25+
*/
26+
#ifndef TESSERACT_PROCESS_MANAGERS_RASTER_GLOBAL_PROCESS_MANAGER_H
27+
#define TESSERACT_PROCESS_MANAGERS_RASTER_GLOBAL_PROCESS_MANAGER_H
28+
29+
#include <tesseract_common/macros.h>
30+
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
31+
#include <functional>
32+
#include <vector>
33+
#include <thread>
34+
#include <taskflow/taskflow.hpp>
35+
TESSERACT_COMMON_IGNORE_WARNINGS_POP
36+
37+
#include <tesseract_process_managers/process_manager.h>
38+
#include <tesseract_process_managers/taskflow_generator.h>
39+
40+
namespace tesseract_planning
41+
{
42+
/**
43+
* @brief This class provides a process manager for a raster process.
44+
*
45+
* Given a ProcessInput in the correct format, it handles the creation of the process dependencies and uses Taskflow to
46+
* execute them efficiently in a parallel based on those dependencies.
47+
*
48+
* The required format is below.
49+
* for skipping of rasters without replanning. This logic must be handled in the execute process.
50+
*
51+
* Composite
52+
* {
53+
* Composite - from start
54+
* Composite - Raster segment
55+
* Composite - Transitions
56+
* Composite - Raster segment
57+
* Composite - Transitions
58+
* Composite - Raster segment
59+
* Composite - to end
60+
* }
61+
*/
62+
class RasterGlobalProcessManager : public ProcessManager
63+
{
64+
public:
65+
using Ptr = std::shared_ptr<RasterGlobalProcessManager>;
66+
using ConstPtr = std::shared_ptr<const RasterGlobalProcessManager>;
67+
68+
RasterGlobalProcessManager(TaskflowGenerator::UPtr global_taskflow_generator,
69+
TaskflowGenerator::UPtr freespace_taskflow_generator,
70+
TaskflowGenerator::UPtr transition_taskflow_generator,
71+
TaskflowGenerator::UPtr raster_taskflow_generator,
72+
std::size_t n = std::thread::hardware_concurrency());
73+
74+
~RasterGlobalProcessManager() override = default;
75+
RasterGlobalProcessManager(const RasterGlobalProcessManager&) = delete;
76+
RasterGlobalProcessManager& operator=(const RasterGlobalProcessManager&) = delete;
77+
RasterGlobalProcessManager(RasterGlobalProcessManager&&) = delete;
78+
RasterGlobalProcessManager& operator=(RasterGlobalProcessManager&&) = delete;
79+
80+
bool init(ProcessInput input) override;
81+
82+
bool execute() override;
83+
84+
bool terminate() override;
85+
86+
bool clear() override;
87+
88+
void enableDebug(bool enabled) override;
89+
90+
void enableProfile(bool enabled) override;
91+
92+
private:
93+
void successCallback(std::string message);
94+
void failureCallback(std::string message);
95+
bool success_{ false };
96+
bool debug_{ false };
97+
bool profile_{ false };
98+
99+
TaskflowGenerator::UPtr global_taskflow_generator_;
100+
TaskflowGenerator::UPtr freespace_taskflow_generator_;
101+
TaskflowGenerator::UPtr transition_taskflow_generator_;
102+
TaskflowGenerator::UPtr raster_taskflow_generator_;
103+
tf::Executor executor_;
104+
tf::Taskflow taskflow_;
105+
106+
tf::Task global_task_;
107+
tf::Task global_post_task_;
108+
std::vector<tf::Task> freespace_tasks_;
109+
std::vector<tf::Task> transition_tasks_;
110+
std::vector<tf::Task> raster_tasks_;
111+
112+
static void globalPostProcess(ProcessInput input);
113+
114+
/**
115+
* @brief Checks that the ProcessInput is in the correct format.
116+
* @param input ProcessInput to be checked
117+
* @return True if in the correct format
118+
*/
119+
bool checkProcessInput(const ProcessInput& input) const;
120+
};
121+
122+
} // namespace tesseract_planning
123+
124+
#endif

‎tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_managers/raster_only_global_process_manager.h

Lines changed: 119 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,119 @@
1+
/**
2+
* @file raster_only_global_process_manager.h
3+
* @brief Plans raster paths
4+
*
5+
* @author Matthew Powelson
6+
* @date July 15, 2020
7+
* @version TODO
8+
* @bug No known bugs
9+
*
10+
* @copyright Copyright (c) 2020, Southwest Research Institute
11+
*
12+
* @par License
13+
* Software License Agreement (Apache License)
14+
* @par
15+
* Licensed under the Apache License, Version 2.0 (the "License");
16+
* you may not use this file except in compliance with the License.
17+
* You may obtain a copy of the License at
18+
* http://www.apache.org/licenses/LICENSE-2.0
19+
* @par
20+
* Unless required by applicable law or agreed to in writing, software
21+
* distributed under the License is distributed on an "AS IS" BASIS,
22+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
23+
* See the License for the specific language governing permissions and
24+
* limitations under the License.
25+
*/
26+
#ifndef TESSERACT_PROCESS_MANAGERS_RASTER_ONLY_GLOBAL_PROCESS_MANAGER_H
27+
#define TESSERACT_PROCESS_MANAGERS_RASTER_ONLY_GLOBAL_PROCESS_MANAGER_H
28+
29+
#include <tesseract_common/macros.h>
30+
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
31+
#include <functional>
32+
#include <vector>
33+
#include <thread>
34+
#include <taskflow/taskflow.hpp>
35+
TESSERACT_COMMON_IGNORE_WARNINGS_POP
36+
37+
#include <tesseract_process_managers/process_manager.h>
38+
#include <tesseract_process_managers/taskflow_generator.h>
39+
40+
namespace tesseract_planning
41+
{
42+
/**
43+
* @brief This class provides a process manager for a raster process.
44+
*
45+
* Given a ProcessInput in the correct format, it handles the creation of the process dependencies and uses Taskflow to
46+
* execute them efficiently in a parallel based on those dependencies.
47+
*
48+
* The required format is below.
49+
* for skipping of rasters without replanning. This logic must be handled in the execute process.
50+
*
51+
* Composite
52+
* {
53+
* Composite - Raster segment
54+
* Composite - Transitions
55+
* Composite - Raster segment
56+
* Composite - Transitions
57+
* Composite - Raster segment
58+
* }
59+
*/
60+
class RasterOnlyGlobalProcessManager : public ProcessManager
61+
{
62+
public:
63+
using Ptr = std::shared_ptr<RasterOnlyGlobalProcessManager>;
64+
using ConstPtr = std::shared_ptr<const RasterOnlyGlobalProcessManager>;
65+
66+
RasterOnlyGlobalProcessManager(TaskflowGenerator::UPtr global_taskflow_generator,
67+
TaskflowGenerator::UPtr transition_taskflow_generator,
68+
TaskflowGenerator::UPtr raster_taskflow_generator,
69+
std::size_t n = std::thread::hardware_concurrency());
70+
71+
~RasterOnlyGlobalProcessManager() override = default;
72+
RasterOnlyGlobalProcessManager(const RasterOnlyGlobalProcessManager&) = delete;
73+
RasterOnlyGlobalProcessManager& operator=(const RasterOnlyGlobalProcessManager&) = delete;
74+
RasterOnlyGlobalProcessManager(RasterOnlyGlobalProcessManager&&) = delete;
75+
RasterOnlyGlobalProcessManager& operator=(RasterOnlyGlobalProcessManager&&) = delete;
76+
77+
bool init(ProcessInput input) override;
78+
79+
bool execute() override;
80+
81+
bool terminate() override;
82+
83+
bool clear() override;
84+
85+
void enableDebug(bool enabled) override;
86+
87+
void enableProfile(bool enabled) override;
88+
89+
private:
90+
void successCallback(std::string message);
91+
void failureCallback(std::string message);
92+
bool success_{ false };
93+
bool debug_{ false };
94+
bool profile_{ false };
95+
96+
TaskflowGenerator::UPtr global_taskflow_generator_;
97+
TaskflowGenerator::UPtr transition_taskflow_generator_;
98+
TaskflowGenerator::UPtr raster_taskflow_generator_;
99+
tf::Executor executor_;
100+
tf::Taskflow taskflow_;
101+
102+
tf::Task global_task_;
103+
tf::Task global_post_task_;
104+
std::vector<tf::Task> transition_tasks_;
105+
std::vector<tf::Task> raster_tasks_;
106+
107+
static void globalPostProcess(ProcessInput input);
108+
109+
/**
110+
* @brief Checks that the ProcessInput is in the correct format.
111+
* @param input ProcessInput to be checked
112+
* @return True if in the correct format
113+
*/
114+
bool checkProcessInput(const ProcessInput& input) const;
115+
};
116+
117+
} // namespace tesseract_planning
118+
119+
#endif

0 commit comments

Comments
 (0)