ice_conversions.cpp
Go to the documentation of this file.
1 #include "ice_conversions.h"
2 
3 #include <algorithm>
4 #include <iterator>
5 
10 
12 
13 #include <armarx/navigation/client/ice/NavigatorInterface.h>
16 
18 {
19 
22  {
24 
25  switch (strategy)
26  {
28  ARMARX_IMPORTANT << "Free navigation";
30  break;
32  ARMARX_IMPORTANT << "Point2Point navigation";
34  break;
35  default:
37  }
38 
39  return strgy;
40  }
41 
42  WaypointTarget
44  {
45  ARMARX_CHECK((not waypoint.locationId.empty()) or waypoint.pose)
46  << "Either `locationId` or `pose` has to be set.";
47 
48  if (not waypoint.locationId.empty())
49  {
50  return WaypointTarget{
51  .locationId = waypoint.locationId, .strategy = defrost(waypoint.strategy)
52  // .config = defrost(waypoint.config)
53  };
54  }
55 
56  if (waypoint.pose)
57  {
58  return WaypointTarget{
59  .pose = core::Pose(armarx::fromIce(waypoint.pose)),
60  .strategy = defrost(waypoint.strategy)
61  // .config = defrost(waypoint.config)
62  };
63  }
64 
65  throw armarx::LocalException("Invalid use of client::detail::Waypoint: either `locationId` "
66  "or `pose` has to be set!");
67  }
68 
71  {
72  WaypointTargets targets;
73  targets.reserve(waypoints.size());
74 
75  std::transform(waypoints.begin(),
76  waypoints.end(),
77  std::back_inserter(targets),
78  [](const auto& wp) { return defrost(wp); });
79 
80  return targets;
81  }
82 
85  {
88 
89  switch (strategy)
90  {
93  break;
96  break;
97  default:
99  }
100 
101  return strgy;
102  }
103 
106  {
107  return client::detail::Waypoint{.locationId = target.locationId.value_or(""),
108  .pose =
109  target.pose ? toIce(target.pose->matrix()) : nullptr,
110  .strategy = freeze(target.strategy)};
111  }
112 
115  {
116  client::detail::Waypoints waypoints;
117  waypoints.reserve(targets.size());
118 
119  std::transform(targets.begin(),
120  targets.end(),
121  std::back_inserter(waypoints),
122  [](const auto& wp) { return freeze(wp); });
123 
124  return waypoints;
125  }
126 } // namespace armarx::navigation::client
ice_conversions.h
armarx::navigation::client::detail::Free
@ Free
Definition: NavigatorInterface.ice:44
armarx::navigation::client::detail::GlobalPlanningStrategy
GlobalPlanningStrategy
Definition: NavigatorInterface.ice:42
ARMARX_UNEXPECTED_ENUM_VALUE
#define ARMARX_UNEXPECTED_ENUM_VALUE(EnumType, value)
Throw an UnexpectedEnumValueException.
Definition: UnexpectedEnumValueException.h:58
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
LocalException.h
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
types.h
basic_types.h
armarx::navigation::client::GlobalPlanningStrategy::Free
@ Free
armarx::navigation::client::freeze
client::detail::GlobalPlanningStrategy freeze(const client::GlobalPlanningStrategy &strategy)
Definition: ice_conversions.cpp:84
Pose.h
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:668
UnexpectedEnumValueException.h
armarx::navigation::client
This file is part of ArmarX.
Definition: ComponentPlugin.cpp:21
armarx::navigation::client::WaypointTarget::pose
std::optional< core::Pose > pose
Definition: ice_conversions.h:42
armarx::navigation::client::detail::Waypoint::locationId
string locationId
Definition: NavigatorInterface.ice:51
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
armarx::toIce
void toIce(std::map< IceKeyT, IceValueT > &iceMap, const boost::container::flat_map< CppKeyT, CppValueT > &cppMap)
Definition: ice_conversions_boost_templates.h:15
armarx::navigation::client::WaypointTarget::locationId
std::optional< std::string > locationId
Definition: ice_conversions.h:41
armarx::navigation::client::defrost
client::GlobalPlanningStrategy defrost(const client::detail::GlobalPlanningStrategy &strategy)
Definition: ice_conversions.cpp:21
armarx::navigation::client::detail::Waypoints
sequence< Waypoint > Waypoints
Definition: NavigatorInterface.ice:57
armarx::navigation::client::detail::Point2Point
@ Point2Point
Definition: NavigatorInterface.ice:45
armarx::navigation::client::WaypointTarget
Definition: ice_conversions.h:39
armarx::fromIce
void fromIce(const std::map< IceKeyT, IceValueT > &iceMap, boost::container::flat_map< CppKeyT, CppValueT > &cppMap)
Definition: ice_conversions_boost_templates.h:27
armarx::transform
auto transform(const Container< InputT, Alloc > &in, OutputT(*func)(InputT const &)) -> Container< OutputT, typename std::allocator_traits< Alloc >::template rebind_alloc< OutputT >>
Convenience function (with less typing) to transform a container of type InputT into the same contain...
Definition: algorithm.h:351
ExpressionException.h
armarx::navigation::client::GlobalPlanningStrategy
GlobalPlanningStrategy
Definition: types.h:27
Logging.h
armarx::navigation::client::detail::Waypoint::strategy
GlobalPlanningStrategy strategy
Definition: NavigatorInterface.ice:54
armarx::navigation::client::detail::Waypoint::pose
armarx::PoseBase pose
Definition: NavigatorInterface.ice:52
armarx::navigation::client::GlobalPlanningStrategy::Point2Point
@ Point2Point
armarx::navigation::client::WaypointTargets
std::vector< WaypointTarget > WaypointTargets
Definition: ice_conversions.h:81
armarx::navigation::client::detail::Waypoint
Definition: NavigatorInterface.ice:49