CostmapBuilder.h
Go to the documentation of this file.
1 /**
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @author Fabian Reister ( fabian dot reister at kit dot edu )
17  * @date 2022
18  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
19  * GNU General Public License
20  */
21 
22 #pragma once
23 
24 #include <functional>
25 #include <optional>
26 #include <string>
27 #include <vector>
28 
29 #include <Eigen/Core>
30 
31 #include <VirtualRobot/VirtualRobot.h>
32 
36 
38 {
39 
41  {
42  public:
44  {
45  bool restrictToRooms = false;
46  Eigen::Vector3f collisionModelScaleFactor = Eigen::Vector3f::Ones();
47  };
48 
50  const VirtualRobot::SceneObjectSetPtr& obstacles,
51  const std::vector<VirtualRobot::RobotPtr>& articulatedObjects,
52  const std::vector<Room>& rooms,
53  const Costmap::Parameters& parameters,
54  const std::string& robotCollisonModelName,
55  const CostmapBuilderParams& builderParameters);
56 
57  Costmap create(const SceneBounds& init = SceneBounds());
58  Costmap extend(Costmap costmap);
59 
60 
61  static Eigen::MatrixXf createUniformGrid(const SceneBounds& sceneBounds,
62  const Costmap::Parameters& parameters);
63 
64  static void initializeMask(Costmap& costmap);
65 
66  private:
67  float computeCost(const Costmap::Position& position,
68  VirtualRobot::RobotPtr& collisionRobot,
69  const VirtualRobot::CollisionModelPtr& robotCollisionModel);
70 
71  void fillGridCosts(Costmap& costmap);
72  void extendGridCosts(Costmap& costmap, const std::vector<Room>& rooms);
73 
74  // fn: collisionRobot, robotCollisionModel, mask, costs, index -> costs
75  // expects mask calculation based on costs after this function
76  void applyFnToCostmap(Costmap& costmap,
77  const std::function<float(VirtualRobot::RobotPtr&,
78  VirtualRobot::CollisionModelPtr&,
79  const std::optional<float>,
80  const float,
81  const Costmap::Index&)>& fn);
82 
83  void initializeEmptyMask(Costmap& costmap);
84 
85  const VirtualRobot::RobotPtr robot;
86  const VirtualRobot::SceneObjectSetPtr obstacles;
87  const std::vector<VirtualRobot::RobotPtr> articulatedObjects;
88  const std::vector<Room> rooms;
89  const Costmap::Parameters parameters;
90  const std::string robotCollisionModelName;
91 
92  const CostmapBuilderParams builderParameters;
93  };
94 
95 } // namespace armarx::navigation::algorithms
armarx::navigation::algorithms::Costmap::Index
Eigen::Array2i Index
Definition: Costmap.h:54
armarx::navigation::algorithms::CostmapBuilder::CostmapBuilder
CostmapBuilder(const VirtualRobot::RobotPtr &robot, const VirtualRobot::SceneObjectSetPtr &obstacles, const std::vector< VirtualRobot::RobotPtr > &articulatedObjects, const std::vector< Room > &rooms, const Costmap::Parameters &parameters, const std::string &robotCollisonModelName, const CostmapBuilderParams &builderParameters)
Definition: CostmapBuilder.cpp:56
armarx::navigation::algorithms
This file is part of ArmarX.
Definition: aron_conversions.cpp:25
armarx::rooms
Brief description of class rooms.
Definition: rooms.h:38
armarx::navigation::algorithms::CostmapBuilder::createUniformGrid
static Eigen::MatrixXf createUniformGrid(const SceneBounds &sceneBounds, const Costmap::Parameters &parameters)
Definition: CostmapBuilder.cpp:190
armarx::navigation::algorithms::CostmapBuilder::CostmapBuilderParams::restrictToRooms
bool restrictToRooms
Definition: CostmapBuilder.h:45
Costmap.h
armarx::navigation::algorithms::CostmapBuilder::extend
Costmap extend(Costmap costmap)
Definition: CostmapBuilder.cpp:77
armarx::navigation::algorithms::Costmap::Position
Eigen::Vector2f Position
Definition: Costmap.h:55
armarx::navigation::algorithms::CostmapBuilder::CostmapBuilderParams::collisionModelScaleFactor
Eigen::Vector3f collisionModelScaleFactor
Definition: CostmapBuilder.h:46
armarx::navigation::algorithms::SceneBounds
Definition: types.h:29
armarx::navigation::algorithms::CostmapBuilder
Definition: CostmapBuilder.h:40
armarx::navigation::algorithms::CostmapBuilder::initializeMask
static void initializeMask(Costmap &costmap)
Definition: CostmapBuilder.cpp:171
armarx::navigation::algorithms::CostmapBuilder::CostmapBuilderParams
Definition: CostmapBuilder.h:43
armarx::navigation::algorithms::CostmapBuilder::create
Costmap create(const SceneBounds &init=SceneBounds())
Definition: CostmapBuilder.cpp:115
armarx::navigation::algorithms::Costmap::Parameters
Definition: Costmap.h:24
Room.h
types.h
armarx::navigation::algorithms::Costmap
Definition: Costmap.h:16
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19