ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
trafficsimulationfeed.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
18 #include <adore_if_ros_msg/TrafficParticipantSetSimulation.h>
19 
20 namespace adore
21 {
22  namespace if_ROS
23  {
24  class TrafficSimulationFeed:public adore::mad::AFeed<adore::env::traffic::Participant>
25  {
26  private:
27  std::list<adore::env::traffic::Participant> data_;
29  ros::Subscriber single_subscriber_;
30  ros::Subscriber multi_subscriber_;
32  void receive_single(adore_if_ros_msg::TrafficParticipantSimulationConstPtr msg)
33  {
34  data_.emplace_back();
35  converter_(msg,data_.back());
36  }
38  void receive_multi(adore_if_ros_msg::TrafficParticipantSetSimulationConstPtr msg)
39  {
40  for(auto& item:msg->data)
41  {
42  data_.emplace_back();
43  converter_(item,data_.back());
44  }
45  }
46  public:
47  TrafficSimulationFeed(ros::NodeHandle* n,const std::string& single_topic,int single_qsize,
48  const std::string& multi_topic,int multi_qsize)
49  {
50  bool no_delay;
51  n->param("/tcp_no_delay", no_delay, false);
52  single_subscriber_ = n->subscribe(single_topic,single_qsize,&TrafficSimulationFeed::receive_single,this,ros::TransportHints().tcpNoDelay(no_delay));
53  multi_subscriber_ = n->subscribe(multi_topic,multi_qsize,&TrafficSimulationFeed::receive_multi,this,ros::TransportHints().tcpNoDelay(no_delay));
54  }
55 
56  public:
57 
58 
62  virtual bool hasNext() const override
63  {
64  return data_.size()>0;
65  }
69  virtual void getNext(adore::env::traffic::Participant& value) override
70  {
71  value = data_.front();
72  data_.pop_front();
73  }
77  virtual void getLatest(adore::env::traffic::Participant& value) override
78  {
79  value = data_.back();
80  data_.clear();
81  }
82  };
83  }
84 }
Definition: trafficparticipantconverter.h:200
Definition: trafficsimulationfeed.h:25
void receive_single(adore_if_ros_msg::TrafficParticipantSimulationConstPtr msg)
receives single message and puts data element in data_queue
Definition: trafficsimulationfeed.h:32
virtual bool hasNext() const override
Definition: trafficsimulationfeed.h:62
TPSimulationConverter converter_
Definition: trafficsimulationfeed.h:28
void receive_multi(adore_if_ros_msg::TrafficParticipantSetSimulationConstPtr msg)
receives aggregates and unpacks into data_ queue
Definition: trafficsimulationfeed.h:38
virtual void getNext(adore::env::traffic::Participant &value) override
Definition: trafficsimulationfeed.h:69
std::list< adore::env::traffic::Participant > data_
Definition: trafficsimulationfeed.h:27
ros::Subscriber multi_subscriber_
Definition: trafficsimulationfeed.h:30
ros::Subscriber single_subscriber_
Definition: trafficsimulationfeed.h:29
TrafficSimulationFeed(ros::NodeHandle *n, const std::string &single_topic, int single_qsize, const std::string &multi_topic, int multi_qsize)
Definition: trafficsimulationfeed.h:47
virtual void getLatest(adore::env::traffic::Participant &value) override
Definition: trafficsimulationfeed.h:77
Definition: com_patterns.h:29
Definition: areaofeffectconverter.h:20
Struct for representing a participant in traffic.
Definition: participant.h:30