RobotUnitModuleRobotData.cpp
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  * @package RobotAPI::ArmarXObjects::RobotUnit
17  * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
18  * @date 2018
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
24 
25 #include <SimoxUtility/algorithm/string/string_tools.h>
26 #include <VirtualRobot/RobotNodeSet.h>
27 #include <VirtualRobot/XML/RobotIO.h>
28 
31 
33 
35 {
36 
39  {
40  defineRequiredProperty<std::string>("RobotFileName",
41  "Robot file name, e.g. robot_model.xml");
42  defineOptionalProperty<std::string>("RobotFileNameProject",
43  "",
44  "Project in which the robot filename is located "
45  "(if robot is loaded from an external project)");
46 
47  defineOptionalProperty<std::string>(
48  "RobotName",
49  "",
50  "Override robot name if you want to load multiple robots of the same type");
51  defineOptionalProperty<std::string>(
52  "RobotNodeSetName",
53  "Robot",
54  "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
55  defineOptionalProperty<std::string>(
56  "PlatformName",
57  "Platform",
58  "Name of the platform needs to correspond to a node in the virtual robot.");
59  defineOptionalProperty<bool>("PlatformAndLocalizationUnitsEnabled",
60  true,
61  "Enable or disable the platform and localization units.");
62  defineOptionalProperty<std::string>("PlatformInstanceName",
63  "Platform",
64  "Name of the platform instance (will publish "
65  "values on PlatformInstanceName + 'State')");
66  }
67 
68  bool
70  {
71  return _arePlatformAndLocalizationUnitsEnabled;
72  }
73 
74  const std::string&
76  {
77  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
79  << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
80  return robotPlatformName;
81  }
82 
83  std::string
85  {
86  return robotPlatformInstanceName;
87  }
88 
89  const std::string&
91  {
92  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
94  << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
95  return robotNodeSetName;
96  }
97 
98  const std::string&
100  {
101  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
103  << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
104  return robotProjectName;
105  }
106 
107  const std::string&
109  {
110  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
112  << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
113  return robotFileName;
114  }
115 
116  std::string
118  {
119  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
120  std::lock_guard<std::mutex> guard{robotMutex};
122  << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
123  return robot->getName();
124  }
125 
127  RobotData::cloneRobot(bool updateCollisionModel) const
128  {
129  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
130  std::lock_guard<std::mutex> guard{robotMutex};
132  << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
133  ARMARX_CHECK_EXPRESSION(robotPool);
134  const VirtualRobot::RobotPtr clone = robotPool->getRobot();
135  clone->setUpdateVisualization(false);
136  clone->setUpdateCollisionModel(updateCollisionModel);
137  return clone;
138  }
139 
140  void
141  RobotData::_initVirtualRobot()
142  {
143  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
144  std::lock_guard<std::mutex> guard{robotMutex};
145  ARMARX_CHECK_IS_NULL(robot);
146 
147  std::string robotName = getProperty<std::string>("RobotName").getValue();
148 
149  robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
150  robotProjectName = getProperty<std::string>("RobotFileNameProject").getValue();
151  robotFileName = getProperty<std::string>("RobotFileName").getValue();
152  robotPlatformName = getProperty<std::string>("PlatformName").getValue();
153  _arePlatformAndLocalizationUnitsEnabled =
154  getProperty<bool>("PlatformAndLocalizationUnitsEnabled").getValue();
155  robotPlatformInstanceName = getProperty<std::string>("PlatformInstanceName").getValue();
156 
157  //load robot
158  {
159  Ice::StringSeq includePaths;
160  if (!robotProjectName.empty())
161  {
162  CMakePackageFinder finder(robotProjectName);
163  auto pathsString = finder.getDataDir();
164  includePaths = simox::alg::split(pathsString, ";,");
165  ArmarXDataPath::addDataPaths(includePaths);
166  }
167  if (!ArmarXDataPath::getAbsolutePath(robotFileName, robotFileName, includePaths))
168  {
169  std::stringstream str;
170  str << "Could not find robot file " + robotFileName
171  << "\nCollected paths from RobotFileNameProject '" << robotProjectName
172  << "':" << includePaths;
173  throw UserException(str.str());
174  }
175  // read the robot
176  try
177  {
178  // FIXME change the following to VirtualRobot::RobotIO::eStructure and see whether it breaks something
179  robot =
180  VirtualRobot::RobotIO::loadRobot(robotFileName, VirtualRobot::RobotIO::eFull);
181  if (robotName.empty())
182  {
183  robotName = robot->getName();
184  }
185  else
186  {
187  robot->setName(robotName);
188  }
189  }
190  catch (VirtualRobot::VirtualRobotException& e)
191  {
192  throw UserException(e.what());
193  }
194  ARMARX_INFO << "Loaded robot:"
195  << "\n\tProject : " << robotProjectName
196  << "\n\tName : " << robotName
197  << "\n\tRobot file : " << robotFileName
198  << "\n\tRobotNodeSet : " << robotNodeSetName << "\n\tNodeNames : "
199  << robot->getRobotNodeSet(robotNodeSetName)->getNodeNames();
200  ARMARX_CHECK_NOT_NULL(robot);
201  robotPool.reset(new RobotPool(robot, 20));
202  }
203  }
204 
205 } // namespace armarx::RobotUnitModule
armarx::RobotUnitModule::RobotData::getRobotNodetSeName
const std::string & getRobotNodetSeName() const
Returns the name of the robot's RobotNodeSet.
Definition: RobotUnitModuleRobotData.cpp:90
armarx::eFull
@ eFull
Definition: MorphingItem.h:35
str
std::string str(const T &t)
Definition: UserAssistedSegmenterGuiWidgetController.cpp:42
armarx::RobotUnitModule::RobotDataPropertyDefinitions::RobotDataPropertyDefinitions
RobotDataPropertyDefinitions(std::string prefix)
Definition: RobotUnitModuleRobotData.cpp:37
ARMARX_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition: ExpressionException.h:206
armarx::CMakePackageFinder
The CMakePackageFinder class provides an interface to the CMake Package finder capabilities.
Definition: CMakePackageFinder.h:53
armarx::RobotUnitModule::RobotData::arePlatformAndLocalizationUnitsEnabled
bool arePlatformAndLocalizationUnitsEnabled() const
Definition: RobotUnitModuleRobotData.cpp:69
RobotUnitModuleRobotData.h
ARMARX_CHECK_IS_NULL
#define ARMARX_CHECK_IS_NULL(ptr)
This macro evaluates whether ptr is null and if it turns out to be false it will throw an ExpressionE...
Definition: ExpressionException.h:194
armarx::RobotUnitModule::RobotData::getRobotPlatformName
const std::string & getRobotPlatformName() const
Returns the name of the robot's platform.
Definition: RobotUnitModuleRobotData.cpp:75
armarx::RobotUnitModule::RobotData::getRobotFileName
const std::string & getRobotFileName() const
Returns the file name of the robot's model.
Definition: RobotUnitModuleRobotData.cpp:108
CMakePackageFinder.h
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::ArmarXDataPath::addDataPaths
static void addDataPaths(const std::string &dataPathList)
Definition: ArmarXDataPath.cpp:559
armarx::RobotUnitModule::ModuleBase::throwIfInControlThread
void throwIfInControlThread(const std::string &fnc) const
Throws if the current thread is the ControlThread.
Definition: RobotUnitModuleBase.cpp:416
armarx::RobotUnitModule::RobotData::getRobotProjectName
const std::string & getRobotProjectName() const
Returns the name of the project containing the robot's model.
Definition: RobotUnitModuleRobotData.cpp:99
armarx::ArmarXDataPath::getAbsolutePath
static bool getAbsolutePath(const std::string &relativeFilename, std::string &storeAbsoluteFilename, const std::vector< std::string > &additionalSearchPaths={}, bool verbose=true)
Definition: ArmarXDataPath.cpp:111
armarx::RobotUnitModule
Definition: ControlDevice.h:34
ArmarXDataPath.h
armarx::RobotUnitModule::RobotData::cloneRobot
VirtualRobot::RobotPtr cloneRobot(bool updateCollisionModel=false) const
Returns a clone of the robot's model.
Definition: RobotUnitModuleRobotData.cpp:127
armarx::RobotUnitModule::ModuleBasePropertyDefinitions
Definition: RobotUnitModuleBase.h:107
armarx::RobotUnitModule::RobotData::getRobotName
std::string getRobotName() const
Returns the robot's name.
Definition: RobotUnitModuleRobotData.cpp:117
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18
RobotPool.h
armarx::split
std::vector< std::string > split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelpers.cpp:36
armarx::RobotUnitModule::RobotData::getRobotPlatformInstanceName
std::string getRobotPlatformInstanceName() const
Returns the name of the robot platform instance.
Definition: RobotUnitModuleRobotData.cpp:84