ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
simvehicleresetconverter.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 
17 #include <nav_msgs/Odometry.h>
18 #include <tf2/LinearMath/Quaternion.h>
19 #include <tf/LinearMath/Matrix3x3.h>
20 #include <geometry_msgs/Pose.h>
21 #include <geometry_msgs/Twist.h>
24 
25 namespace adore
26 {
27  namespace if_ROS
28  {
33  {
34  public:
38  void operator()(geometry_msgs::PoseConstPtr msg,adore::sim::ResetVehiclePose& pose)
39  {
40  double r,p,y;
41  tf::Matrix3x3(tf::Quaternion(msg->orientation.x,msg->orientation.y,msg->orientation.z,msg->orientation.w)).getRPY(r,p,y);
42  pose.setX(msg->position.x);
43  pose.setY(msg->position.y);
44  pose.setZ(msg->position.z);
45  pose.setPSI(y);
46  }
50  geometry_msgs::Pose operator()(const adore::sim::ResetVehiclePose& pose)
51  {
52  geometry_msgs::Pose msg;
53  msg.position.x = pose.getX();
54  msg.position.y = pose.getY();
55  msg.position.z = pose.getZ();
56  tf2::Quaternion q;
57  q.setRPY(0.0, 0.0, pose.getPSI());
58  msg.orientation.w = q.getW();
59  msg.orientation.x = q.getX();
60  msg.orientation.y = q.getY();
61  msg.orientation.z = q.getZ();
62  return msg;
63  }
67  void operator()(geometry_msgs::TwistConstPtr msg,adore::sim::ResetVehicleTwist& twist)
68  {
69  twist.setVx(msg->linear.x);
70  twist.setVy(msg->linear.y);
71  twist.setOmega(msg->angular.z);
72  }
73 
77  geometry_msgs::Twist operator()(const adore::sim::ResetVehicleTwist& twist)
78  {
79  geometry_msgs::Twist msg;
80  msg.linear.x = twist.getVx();
81  msg.linear.y = twist.getVy();
82  msg.angular.z = twist.getOmega();
83  return msg;
84  }
85  };
86  } // namespace if_ROS
87 } // namespace adore
y
Definition: adore_set_goal.py:31
r
Definition: adore_suppress_lanechanges.py:209
Definition: areaofeffectconverter.h:20
Definition: simvehicleresetconverter.h:33
geometry_msgs::Twist operator()(const adore::sim::ResetVehicleTwist &twist)
Definition: simvehicleresetconverter.h:77
void operator()(geometry_msgs::PoseConstPtr msg, adore::sim::ResetVehiclePose &pose)
Definition: simvehicleresetconverter.h:38
void operator()(geometry_msgs::TwistConstPtr msg, adore::sim::ResetVehicleTwist &twist)
Definition: simvehicleresetconverter.h:67
geometry_msgs::Pose operator()(const adore::sim::ResetVehiclePose &pose)
Definition: simvehicleresetconverter.h:50
provides encapsulation of values needed to reset the vehicle pose in a simulation
Definition: resetvehiclepose.h:26
void setZ(double Z)
Definition: resetvehiclepose.h:61
void setY(double Y)
Definition: resetvehiclepose.h:46
double getZ() const
Definition: resetvehiclepose.h:58
double getX() const
Definition: resetvehiclepose.h:35
void setPSI(double PSI)
Definition: resetvehiclepose.h:54
double getY() const
Definition: resetvehiclepose.h:43
void setX(double X)
Definition: resetvehiclepose.h:38
double getPSI() const
Definition: resetvehiclepose.h:51
provides encapsulation of values needed to reset the vehicle twist (vx,vy and omega) in a simulation
Definition: resetvehicletwist.h:26
double getVy() const
Definition: resetvehicletwist.h:41
void setOmega(double omega)
Definition: resetvehicletwist.h:53
void setVx(double vx)
Definition: resetvehicletwist.h:35
void setVy(double vy)
Definition: resetvehicletwist.h:44
double getVx() const
Definition: resetvehicletwist.h:32
double getOmega() const
Definition: resetvehicletwist.h:50