ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
borderconverter.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  * Daniel Heß - initial API and implementation
13  ********************************************************************************/
14 
15 #pragma once
16 #include <tf/tf.h>
17 #include <tf/LinearMath/Quaternion.h>
18 #include <math.h>
19 # include <adore/env/afactory.h>
20 #include <tf2/LinearMath/Quaternion.h>
21 #include <tf2/LinearMath/Matrix3x3.h>
22 #include <adore_if_ros_msg/Border.h>
23 #include <nav_msgs/Odometry.h>
24 
25 namespace adore
26 {
27  namespace if_ROS
28  {
33  {
34  public:
38  void operator()(adore_if_ros_msg::BorderConstPtr msg,adore::env::BorderBased::Border& border)
39  {
40  //border id: first node
41  border.m_id.m_first.m_X = msg->borderId.first.x;
42  border.m_id.m_first.m_Y = msg->borderId.first.y;
43  border.m_id.m_first.m_Z = msg->borderId.first.z;
44  //border id: last node
45  border.m_id.m_last.m_X = msg->borderId.last.x;
46  border.m_id.m_last.m_Y = msg->borderId.last.y;
47  border.m_id.m_last.m_Z = msg->borderId.last.z;
48  //path
49  border.m_path = new adore::mad::LLinearPiecewiseFunctionM<double,3>(msg->points.size()+2,0.0);
50  border.m_path->getData()(0,0) = 0;
51  border.m_path->getData()(1,0) = border.m_id.m_first.m_X;
52  border.m_path->getData()(2,0) = border.m_id.m_first.m_Y;
53  border.m_path->getData()(3,0) = border.m_id.m_first.m_Z;
54  double dx,dy,dz;
55  int counter = 1;
56  for(const auto& p:msg->points)
57  {
58  border.m_path->getData()(1,counter) = p.x;
59  border.m_path->getData()(2,counter) = p.y;
60  border.m_path->getData()(3,counter) = p.z;
61  dx = border.m_path->getData()(1,counter) - border.m_path->getData()(1,counter-1);
62  dy = border.m_path->getData()(2,counter) - border.m_path->getData()(2,counter-1);
63  dz = border.m_path->getData()(3,counter) - border.m_path->getData()(3,counter-1);
64  border.m_path->getData()(0,counter) = border.m_path->getData()(0,counter-1) + std::sqrt(dx*dx+dy*dy+dz*dz);
65  counter ++;
66  }
67  border.m_path->getData()(1,counter) = border.m_id.m_last.m_X;
68  border.m_path->getData()(2,counter) = border.m_id.m_last.m_Y;
69  border.m_path->getData()(3,counter) = border.m_id.m_last.m_Z;
70  dx = border.m_path->getData()(1,counter) - border.m_path->getData()(1,counter-1);
71  dy = border.m_path->getData()(2,counter) - border.m_path->getData()(2,counter-1);
72  dz = border.m_path->getData()(3,counter) - border.m_path->getData()(3,counter-1);
73  border.m_path->getData()(0,counter) = border.m_path->getData()(0,counter-1) + std::sqrt(dx*dx+dy*dy+dz*dz);
74 
75  border.m_left = 0;
76  if(msg->has_left_id)
77  {
79  //border id: first node
80  border.m_left->m_first.m_X = msg->leftId.first.x;
81  border.m_left->m_first.m_Y = msg->leftId.first.y;
82  border.m_left->m_first.m_Z = msg->leftId.first.z;
83  //border id: last node
84  border.m_left->m_last.m_X = msg->leftId.last.x;
85  border.m_left->m_last.m_Y = msg->leftId.last.y;
86  border.m_left->m_last.m_Z = msg->leftId.last.z;
87  }
88  border.m_type = static_cast<adore::env::BorderBased::BorderType::TYPE>(msg->borderType);
89  }
90 
94  adore_if_ros_msg::Border operator()(const adore::env::BorderBased::Border & border)
95  {
96  adore_if_ros_msg::Border msg;
97  // border id
98  geometry_msgs::Point first;
99  first.x = border.m_id.m_first.m_X;
100  first.y = border.m_id.m_first.m_Y;
101  first.z = border.m_id.m_first.m_Z;
102  msg.borderId.first = first;
103 
104  geometry_msgs::Point last;
105  last.x = border.m_id.m_last.m_X;
106  last.y = border.m_id.m_last.m_Y;
107  last.z = border.m_id.m_last.m_Z;
108  msg.borderId.last = last;
109 
110  // path - write all elements except first and last
111  if( border.m_path != nullptr)
112  {
113  for(int i = 1; i < border.m_path->getData().nc() -1; i++)
114  {
115  geometry_msgs::Point p;
116  p.x = border.m_path->getData()(1,i);
117  p.y = border.m_path->getData()(2,i);
118  p.z = border.m_path->getData()(3,i);
119  msg.points.push_back(p);
120  }
121  }
122 
123  // left
124  if(border.m_left != nullptr)
125  {
126  msg.has_left_id = true;
127  geometry_msgs::Point left_first;
128  left_first.x = border.m_left->m_first.m_X;
129  left_first.y = border.m_left->m_first.m_Y;
130  left_first.z = border.m_left->m_first.m_Z;
131  msg.leftId.first = left_first;
132 
133  geometry_msgs::Point left_last;
134  left_last.x = border.m_left->m_last.m_X;
135  left_last.y = border.m_left->m_last.m_Y;
136  left_last.z = border.m_left->m_last.m_Z;
137  msg.leftId.last = left_last;
138  }
139  else
140  {
141  msg.has_left_id = false;
142  }
143  msg.borderType = border.m_type;
144  return msg;
145  }
146  };
147  }
148 }
adoreMatrix< T, n+1, 0 > & getData()
Definition: llinearpiecewisefunction.h:147
TYPE
This enum holds the different types of borders.
Definition: border.h:37
Definition: areaofeffectconverter.h:20
This struct identifies a Border by the coordinates of the starting and the end point.
Definition: borderid.h:31
Coordinate m_last
Definition: borderid.h:32
Coordinate m_first
Definition: borderid.h:32
The border struct contains data of the smallest.
Definition: border.h:62
Tborderpath * m_path
Definition: border.h:70
BorderType::TYPE m_type
Definition: border.h:71
BorderID m_id
Definition: border.h:68
BorderID * m_left
Definition: border.h:69
double m_Y
Definition: coordinate.h:35
double m_Z
Definition: coordinate.h:35
double m_X
Definition: coordinate.h:35
Definition: borderconverter.h:33
void operator()(adore_if_ros_msg::BorderConstPtr msg, adore::env::BorderBased::Border &border)
Definition: borderconverter.h:38
adore_if_ros_msg::Border operator()(const adore::env::BorderBased::Border &border)
Definition: borderconverter.h:94