ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
navigationdataconverter.h
Go to the documentation of this file.
1 /********************************************************************************
2  * Copyright (C) 2017-2020 German Aerospace Center (DLR).
3  * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
4  *
5  * This program and the accompanying materials are made available under the
6  * terms of the Eclipse Public License 2.0 which is available at
7  * http://www.eclipse.org/legal/epl-2.0.
8  *
9  * SPDX-License-Identifier: EPL-2.0
10  *
11  * Contributors:
12  * Robert Markowski - initial API and implementation
13  ********************************************************************************/
14 
15 #pragma once
16 
17 #include <adore_if_ros_msg/NavigationData.h>
19 
20 namespace adore
21 {
22  namespace if_ROS
23  {
25  {
29  adore_if_ros_msg::NavigationData operator()(const std::pair<adore::env::BorderBased::BorderID,double> & id2cost)
30  {
31  adore_if_ros_msg::NavigationData msg;
32 
33  msg.borderId.first.x = id2cost.first.m_first.m_X;
34  msg.borderId.first.y = id2cost.first.m_first.m_Y;
35  msg.borderId.first.z = id2cost.first.m_first.m_Z;
36  msg.borderId.last.x = id2cost.first.m_last.m_X;
37  msg.borderId.last.y = id2cost.first.m_last.m_Y;
38  msg.borderId.last.z = id2cost.first.m_last.m_Z;
39 
40  msg.cost = id2cost.second;
41 
42  return msg;
43  }
47  void operator()(adore_if_ros_msg::NavigationDataConstPtr msg,std::pair<adore::env::BorderBased::BorderID,double>& id2cost)
48  {
49  id2cost.first.m_first.m_X = msg->borderId.first.x;
50  id2cost.first.m_first.m_Y = msg->borderId.first.y;
51  id2cost.first.m_first.m_Z = msg->borderId.first.z;
52  id2cost.first.m_last.m_X = msg->borderId.last.x;
53  id2cost.first.m_last.m_Y = msg->borderId.last.y;
54  id2cost.first.m_last.m_Z = msg->borderId.last.z;
55 
56  id2cost.second = msg->cost;
57  }
58  };
59  }
60 }
Definition: areaofeffectconverter.h:20
Definition: navigationdataconverter.h:25
adore_if_ros_msg::NavigationData operator()(const std::pair< adore::env::BorderBased::BorderID, double > &id2cost)
Definition: navigationdataconverter.h:29
void operator()(adore_if_ros_msg::NavigationDataConstPtr msg, std::pair< adore::env::BorderBased::BorderID, double > &id2cost)
Definition: navigationdataconverter.h:47