ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
trafficlightsimconverter.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  * Stephan Lapoehn - initial API and implementation
13  ********************************************************************************/
14 
15 #pragma once
16 
17 #include <adore/fun/afactory.h>
20 #include <adore_if_ros_msg/TrafficLightSimulation.h>
21 
22 namespace adore
23 {
24  namespace if_ROS
25  {
26 
31  class TrafficLightSimWriter:public adore::mad::AWriter<adore::env::SimTrafficLight>
32  {
33  private:
34  ros::Publisher simTLPublisher_;
35 
36  public:
37  TrafficLightSimWriter(ros::NodeHandle* n,const std::string tlSimTopic, int qsize)
38  {
39  simTLPublisher_ = n->advertise<adore_if_ros_msg::TrafficLightSimulation>(tlSimTopic,qsize);
40 
41  }
43  virtual bool canWriteMore() const override
44  {
45  return true;
46  }
48  virtual void write(const adore::env::SimTrafficLight & value) override
49  {
50  adore_if_ros_msg::TrafficLightSimulation msg_simtl;
51 
52  //sim data
53  msg_simtl.greenDuration = value.green_duration_;
54  msg_simtl.redDuration = value.red_duration_;
55  msg_simtl.startState = (int) value.start_state_;
56  msg_simtl.yellowDuration = value.yellow_duration_;
57  msg_simtl.yellowRedDuration = value.yellow_red_duration_;
58  msg_simtl.timeToGreen = value.time_to_green_;
59  msg_simtl.timeToRed = value.time_to_red_;
60  msg_simtl.timeToRedYellow = value.time_to_red_yellow_;
61  msg_simtl.timeToYellow = value.time_to_yellow_;
62  msg_simtl.tl.tcd.id = value.getID();
63 
64  //tl data
65  msg_simtl.tl.junctionId = value.intersection_id_;
66  msg_simtl.tl.movementId = value.movement_id_;
67 
68  // auto
69  const auto status = value.getStatus();
70 
71  msg_simtl.tl.tlstate = (int) (status->getCurrentColor());
72 
73  msg_simtl.tl.tcd.pose.position.x = value.getCoordinate().m_X;
74  msg_simtl.tl.tcd.pose.position.y = value.getCoordinate().m_Y;
75  msg_simtl.tl.tcd.pose.position.z = value.getCoordinate().m_Z;
76 
77  simTLPublisher_.publish(msg_simtl);
78  }
79  virtual std::string getDesc() override
80  {
81  return std::string("Node will write to ROS-topic: ").append(simTLPublisher_.getTopic());
82  }
83  };
84 
89  class TrafficLightSimReader:public adore::mad::AReader<adore::env::SimTrafficLightMap>
90  {
91  private:
93 
94  ros::Subscriber simTLSubscriber_;
95 
96  bool changed_;
97 
98  void receive_simTL(adore_if_ros_msg::TrafficLightSimulation msg)
99  {
101 
102  stl.intersection_id_ = msg.tl.junctionId;
103  stl.movement_id_ = msg.tl.movementId;
104  stl.red_duration_ = msg.redDuration;
105  stl.green_duration_ = msg.greenDuration;
106  stl.yellow_red_duration_ = msg.yellowRedDuration;
107  stl.yellow_duration_ = msg.yellowDuration;
108  stl.time_to_green_ = msg.timeToGreen;
109  stl.time_to_red_ = msg.timeToRed;
110  stl.time_to_red_yellow_ = msg.timeToRedYellow;
111  stl.time_to_yellow_ = msg.timeToYellow;
112  stl.start_state_ = adore::env::TrafficLightColor(msg.startState);
113 
114 
116  coord.m_X = msg.tl.tcd.pose.position.x;
117  coord.m_Y = msg.tl.tcd.pose.position.y;
118  coord.m_Z = msg.tl.tcd.pose.position.z;
119 
120  stl.setCoordinate(coord);
123 
124  data_[stl.getCoordinate()] = stl;
125  }
126 
127  public:
128  TrafficLightSimReader(ros::NodeHandle* n,const std::string& topic_tlsim,int qsize)
129  :changed_(false)
130  {
131  bool no_delay;
132  n->param("/tcp_no_delay", no_delay, false);
133  simTLSubscriber_ = n->subscribe(topic_tlsim,qsize,&TrafficLightSimReader::receive_simTL,this,ros::TransportHints().tcpNoDelay(no_delay));
134  }
135  virtual bool hasData() const override
136  {
137  return true;
138  }
139  bool hasUpdate() const override
140  {
141  return hasData() && changed_;
142  }
143  virtual void getData(adore::env::SimTrafficLightMap& value) override
144  {
145  value = data_;
146  }
147  };
148  }
149 }
Definition: trafficlight.h:219
int time_to_yellow_
Definition: trafficlight.h:238
int yellow_red_duration_
Definition: trafficlight.h:230
TrafficLightColor start_state_
Definition: trafficlight.h:232
int time_to_red_
Definition: trafficlight.h:234
int green_duration_
Definition: trafficlight.h:228
int time_to_red_yellow_
Definition: trafficlight.h:236
int time_to_green_
Definition: trafficlight.h:240
int red_duration_
Definition: trafficlight.h:221
int yellow_duration_
Definition: trafficlight.h:226
void setType(TCDType type)
Definition: trafficcontroldevice.h:150
BorderBased::Coordinate getCoordinate() const
Definition: trafficcontroldevice.h:175
void setCoordinate(BorderBased::Coordinate coordinate)
Definition: trafficcontroldevice.h:170
int getID() const
Definition: trafficcontroldevice.h:146
@ TRAFFIC_LIGHT
Definition: trafficcontroldevice.h:29
void setCurrentColor(TrafficLightColor color)
Definition: trafficlight.h:89
int intersection_id_
Definition: trafficlight.h:183
virtual TrafficLightStatus * getStatus()
Definition: trafficlight.h:190
int movement_id_
Definition: trafficlight.h:178
Definition: trafficlightsimconverter.h:90
void receive_simTL(adore_if_ros_msg::TrafficLightSimulation msg)
Definition: trafficlightsimconverter.h:98
TrafficLightSimReader(ros::NodeHandle *n, const std::string &topic_tlsim, int qsize)
Definition: trafficlightsimconverter.h:128
bool hasUpdate() const override
Definition: trafficlightsimconverter.h:139
virtual bool hasData() const override
Definition: trafficlightsimconverter.h:135
adore::env::SimTrafficLightMap data_
Definition: trafficlightsimconverter.h:92
virtual void getData(adore::env::SimTrafficLightMap &value) override
Definition: trafficlightsimconverter.h:143
ros::Subscriber simTLSubscriber_
Definition: trafficlightsimconverter.h:94
bool changed_
Definition: trafficlightsimconverter.h:96
Definition: trafficlightsimconverter.h:32
ros::Publisher simTLPublisher_
Definition: trafficlightsimconverter.h:34
virtual void write(const adore::env::SimTrafficLight &value) override
write sends out data value
Definition: trafficlightsimconverter.h:48
TrafficLightSimWriter(ros::NodeHandle *n, const std::string tlSimTopic, int qsize)
Definition: trafficlightsimconverter.h:37
virtual bool canWriteMore() const override
canWriteMore indicates whether more data can be written
Definition: trafficlightsimconverter.h:43
virtual std::string getDesc() override
Definition: trafficlightsimconverter.h:79
Definition: com_patterns.h:68
Definition: com_patterns.h:97
TrafficLightColor
Definition: trafficlight.h:50
std::unordered_map< adore::env::BorderBased::Coordinate, adore::env::SimTrafficLight, adore::env::BorderBased::CoordinateHasher > SimTrafficLightMap
Definition: trafficlight.h:244
Definition: areaofeffectconverter.h:20
This struct represents 3-dimensional coordines.
Definition: coordinate.h:34
double m_Y
Definition: coordinate.h:35
double m_Z
Definition: coordinate.h:35
double m_X
Definition: coordinate.h:35