util.h
Go to the documentation of this file.
1 
2 /**
3  * This file is part of ArmarX.
4  *
5  * ArmarX is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License version 2 as
7  * published by the Free Software Foundation.
8  *
9  * ArmarX is distributed in the hope that it will be useful, but
10  * WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  *
17  * @author Fabian Reister ( fabian dot reister at kit dot edu )
18  * @date 2022
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #pragma once
24 
25 #include <optional>
26 #include <vector>
27 
28 #include <VirtualRobot/VirtualRobot.h>
29 #include <VirtualRobot/Workspace/WorkspaceGrid.h>
30 
35 
37 {
38 
39  SceneBounds computeSceneBounds(const VirtualRobot::SceneObjectSetPtr& obstacles,
40  const std::vector<VirtualRobot::RobotPtr>& articulatedObjects,
41  const SceneBounds& init = SceneBounds(),
42  const std::vector<Room>& rooms = {},
43  float margin = 0,
44  bool restrictToRooms = false);
45 
46  SceneBounds toSceneBounds(const VirtualRobot::WorkspaceGrid::Extends& extends);
47 
48  SceneBounds merge(const std::vector<SceneBounds>& sceneBounds);
49 
50 
51  Costmap toCostmap(const VirtualRobot::WorkspaceGrid& workspaceGrid);
52 
53 
54  // Costmap mergeUnaligned(const std::vector<Costmap>& costmaps,
55  // const std::vector<float>& weights,
56  // float cellSize = -1,
57  // const Eigen::Array2f& offset = Eigen::Array2f::Zero());
58 
59  void checkSameSize(const std::vector<Costmap>& costmaps);
60 
61  /**
62  * @brief Defines how the
63  *
64  */
65  enum class CostmapMergeMode
66  {
67  MAX,
68  MIN,
69  AVERAGE
70  };
71 
72  Costmap mergeAligned(const std::vector<Costmap>& costmaps, CostmapMergeMode mode);
73  Costmap mergeAligned(const std::vector<Costmap>& costmaps, const std::vector<float>& weights);
74 
75  Costmap scaleCostmap(const Costmap& costmap, float cellSize);
76 
77  std::optional<core::Pose2D> sampleValidPositionInMap(const algorithms::Costmap& costmap);
78 
79 
80  void invalidateOutsideRooms(const std::vector<Room>& rooms, Costmap& costmap);
81 
82 } // namespace armarx::navigation::algorithms
armarx::navigation::algorithms::CostmapMergeMode
CostmapMergeMode
Defines how the.
Definition: util.h:65
basic_types.h
armarx::navigation::algorithms::merge
SceneBounds merge(const std::vector< SceneBounds > &sceneBounds)
Definition: util.cpp:159
armarx::objpose::util::articulatedObjects
objpose::ObjectPoseSeq articulatedObjects(objpose::ObjectPoseSeq objects)
Definition: util.cpp:82
armarx::navigation::algorithms::CostmapMergeMode::AVERAGE
@ AVERAGE
armarx::navigation::algorithms::computeSceneBounds
SceneBounds computeSceneBounds(const VirtualRobot::SceneObjectSetPtr &obstacles, const std::vector< VirtualRobot::RobotPtr > &articulatedObjects, const SceneBounds &init, const std::vector< Room > &rooms, const float margin, const bool restrictToRooms)
Definition: util.cpp:62
armarx::navigation::algorithms
This file is part of ArmarX.
Definition: aron_conversions.cpp:25
armarx::navigation::algorithms::checkSameSize
void checkSameSize(const std::vector< Costmap > &costmaps)
Definition: util.cpp:265
armarx::rooms
Brief description of class rooms.
Definition: rooms.h:38
armarx::navigation::algorithms::mergeAligned
Costmap mergeAligned(const std::vector< Costmap > &costmaps, CostmapMergeMode mode)
Definition: util.cpp:279
Costmap.h
armarx::navigation::algorithms::scaleCostmap
Costmap scaleCostmap(const Costmap &costmap, float cellSize)
Definition: util.cpp:351
armarx::navigation::algorithms::invalidateOutsideRooms
void invalidateOutsideRooms(const std::vector< Room > &rooms, Costmap &costmap)
Definition: util.cpp:439
armarx::navigation::algorithms::CostmapMergeMode::MAX
@ MAX
armarx::navigation::algorithms::CostmapMergeMode::MIN
@ MIN
armarx::navigation::algorithms::sampleValidPositionInMap
std::optional< core::Pose2D > sampleValidPositionInMap(const algorithms::Costmap &costmap)
Definition: util.cpp:408
armarx::navigation::algorithms::toSceneBounds
SceneBounds toSceneBounds(const VirtualRobot::WorkspaceGrid::Extends &extends)
Definition: util.cpp:152
armarx::navigation::algorithms::toCostmap
Costmap toCostmap(const VirtualRobot::WorkspaceGrid &workspaceGrid)
Definition: util.cpp:180
Room.h
types.h
armarx::navigation::algorithms::Costmap
Definition: Costmap.h:16