ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
quaternion.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 <tf2/LinearMath/Quaternion.h>
17 #include <tf2/LinearMath/Matrix3x3.h>
18 #include <geometry_msgs/Pose.h>
19 
20 namespace adore
21 {
22  namespace if_ROS
23  {
28  {
29  public:
31  void setHeading(double heading, geometry_msgs::Pose& msg)const
32  {
33  tf2::Quaternion q;
34  q.setRPY(0.0,0.0,heading);
35 
36  msg.orientation.x = q.getX();
37  msg.orientation.y = q.getY();
38  msg.orientation.z = q.getZ();
39  msg.orientation.w = q.getW();
40  }
42  tf2::Quaternion headingToQuaternion(double heading)const
43  {
44  tf2::Quaternion q;
45  q.setRPY(0.0,0.0,heading);
46  return q;
47  }
49  double quaternionToHeading(const geometry_msgs::Pose& p)const
50  {
51  tf2::Quaternion q;
52  q.setValue(p.orientation.x,p.orientation.y,p.orientation.z,p.orientation.w);
53  tf2::Matrix3x3 m(q);
54  double roll,pitch,yaw;
55  m.getRPY(roll,pitch,yaw);
56  return yaw;
57  }
58  };
59  }
60 }
ALFunction< T, T > * heading(AScalarToN< T, 2 > *df)
Definition: heading.h:114
yaw
Definition: adore_set_pose.py:36
Definition: areaofeffectconverter.h:20
Definition: quaternion.h:28
tf2::Quaternion headingToQuaternion(double heading) const
convert heading to quaternion
Definition: quaternion.h:42
double quaternionToHeading(const geometry_msgs::Pose &p) const
convert quaternion to heading
Definition: quaternion.h:49
void setHeading(double heading, geometry_msgs::Pose &msg) const
set heading of a Pose message
Definition: quaternion.h:31