ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
station.h
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse ADORe, Automated Driving Open Research; see https://eclipse.org/adore
3 // Copyright (C) 2017-2020 German Aerospace Center (DLR).
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
16 // Station class tracks station position and time.
17 #pragma once
18 
19 #include <ros/ros.h>
20 #include <nav_msgs/Odometry.h>
21 #include <string>
22 
23 namespace adore_v2x_sim
24 {
28  class Station
29  {
30  private:
31  double X_;
32  double Y_;
33  double Z_;
34  double t_;
35  ros::Subscriber odom_subscriber_;
36  void odom_receive(nav_msgs::OdometryConstPtr msg)
37  {
38  X_ = msg->pose.pose.position.x;
39  Y_ = msg->pose.pose.position.y;
40  Z_ = msg->pose.pose.position.z;
41  t_ = msg->header.stamp.toSec();
42  }
43  public:
44  Station(ros::NodeHandle n,std::string odom_topic)
45  {
46  if(odom_topic!="")odom_subscriber_ = n.subscribe(odom_topic,10,&Station::odom_receive,this);
47  }
48  void setPosition(double X,double Y,double Z)
49  {
50  X_=X;Y_=Y;Z_=Z;
51  }
52  void setTime(double t)
53  {
54  t_=t;
55  }
56  double getX() const{return X_;}
57  double getY() const{return Y_;}
58  double getZ() const{return Z_;}
59  double getT() const{return t_;}
60  };
61 }
Definition: station.h:29
double X_
Definition: station.h:31
Station(ros::NodeHandle n, std::string odom_topic)
Definition: station.h:44
double getY() const
Definition: station.h:57
double Y_
Definition: station.h:32
void setPosition(double X, double Y, double Z)
Definition: station.h:48
double getZ() const
Definition: station.h:58
double t_
Definition: station.h:34
ros::Subscriber odom_subscriber_
Definition: station.h:35
void odom_receive(nav_msgs::OdometryConstPtr msg)
Definition: station.h:36
double Z_
Definition: station.h:33
void setTime(double t)
Definition: station.h:52
double getT() const
Definition: station.h:59
double getX() const
Definition: station.h:56
Definition: channel.h:29