71 for(
auto it = itpair.first; it!= itpair.second; it++)
76 if(b->m_left !=
nullptr)
78 l = global_map->getBorder(*(b->m_left));
86 std::string trackConfig =
"";
87 std::stringstream trackstream(trackConfigs);
88 while(std::getline(trackstream,trackConfig,
';'))
91 std::stringstream trackConfigStream(trackConfig);
92 bool transform =
false;
93 std::string xodrFilename =
"";
94 std::string r2sReflineFilename =
"";
95 std::string r2sLaneFilename =
"";
96 std::string token =
"";
97 while(std::getline(trackConfigStream,token,
','))
101 LOG_W(
"Unrecognizable token: %s", token.c_str());
104 if(token.compare(
"transform")==0)
108 else if(token.substr(token.size()-5,5).compare(
".xodr")==0)
110 xodrFilename = token;
114 if(token.substr(token.size()-5,5).compare(
".r2sr")==0)
116 r2sReflineFilename = token;
118 else if(token.substr(token.size()-5,5).compare(
".r2sl")==0)
120 r2sLaneFilename = token;
124 LOG_W(
"Unrecognizable token: %s", token.c_str());
132 if(!xodrFilename.empty())
138 LOG_I(
"Processing file %s ...", xodrFilename.c_str());
139 converter.
convert(xodrFilename.c_str(),&partialSet,transform);
144 LOG_E(
"Could not parse file %s", xodrFilename.c_str());
148 for(;its.first!=its.second;its.first++)
155 else if(!(r2sReflineFilename.empty() || r2sLaneFilename.empty()))
159 LOG_I(
"Processing files %s and %s ...", r2sReflineFilename.c_str(), r2sLaneFilename.c_str());
161 converter.
convert(r2sReflineFilename,r2sLaneFilename, partialSet);
166 LOG_E(
"Could not parse R2S files %s and %s", r2sReflineFilename.c_str(), r2sLaneFilename.c_str());
171 for(;its.first!=its.second;its.first++)
179 LOG_E(
"Could not parse configuration: %s", trackConfig.c_str());
207 std::vector<env::BorderBased::BorderID> outdated_data;
227 for(
auto it = data.begin(); it!=data.end(); it++)
229 auto bId = (*it)->m_id;
Definition: navigation.h:32
Navigation(env::AFactory *env_factory, adore::params::AFactory *params_factory, std::string trackConfigs, Config config)
Definition: navigation.h:185
DLR_TS::PlotLab::FigureStubZMQ * figure_
Definition: navigation.h:63
Config config_
Definition: navigation.h:47
adore::mad::AReader< adore::env::VehicleMotionState9d > * motion_state_reader_
Definition: navigation.h:54
void plotGlobalNavigation()
Definition: navigation.h:65
adore::env::NavigationManagement nav_management_
Definition: navigation.h:51
params::APMapProvider * map_params_
Definition: navigation.h:49
virtual void run()
Definition: navigation.h:203
adore::mad::AReader< adore::fun::NavigationGoal > * nav_goal_reader_
Definition: navigation.h:57
void parseTrackConfigs(std::string trackConfigs, env::BorderBased::BorderSet &targetSet)
Definition: navigation.h:84
params::APNavigation * nav_params_
Definition: navigation.h:50
DLR_TS::PlotLab::FigureStubFactory * figure_factory_
Definition: navigation.h:62
adore::mad::AWriter< std::pair< adore::env::BorderBased::BorderID, double > > * nav_writer_
Definition: navigation.h:60
abstract factory for adore::env communication
Definition: afactory.h:41
virtual TNavigationGoalReader * getNavigationGoalReader()=0
virtual TVehicleMotionStateReader * getVehicleMotionStateReader()=0
virtual TNavgationDataWriter * getNavigationDataWriter()=0
virtual TBorderFeed * getBorderFeed()=0
efficiently store borders in boost R-tree
Definition: borderset.h:99
void insert_border(Border *b, bool force_insert=false)
insert new border into this
Definition: borderset.h:225
BorderIteratorPair2 getAllBorders()
get all borders in this
Definition: borderset.h:356
void setIsOwner(bool isOwner)
set whether this owns objects in pointers
Definition: borderset.h:214
Definition: navigation_management.h:30
void addFeed(adore::mad::AFeed< adore::env::BorderBased::Border > *feed)
Definition: navigation_management.h:137
void update(BorderBased::Coordinate newDestination, bool destinationValid)
Definition: navigation_management.h:163
void init(BorderBased::BorderSet *set)
Definition: navigation_management.h:152
bool goalChanged()
Definition: navigation_management.h:186
adore::env::BorderBased::BorderSet * getGlobalMap()
Definition: navigation_management.h:261
void run(double x, double y, double r, std::vector< adore::env::BorderBased::Border * > &newBorders, std::vector< adore::env::BorderBased::BorderID > &outdatedBorders, int MAX_SEND_NUMBER=40)
Definition: navigation_management.h:239
void setLaneChangePenalty(double value)
Definition: navigation_management.h:132
double getMaxCost()
Definition: navigation_management.h:142
double getBorderCost(BorderBased::BorderID id)
Definition: navigation_management.h:244
input: 2 files, reference lines and borders output: border file
Definition: r2s2borderbased.h:31
void convert(std::string referenceLineFile, std::string laneBorderFile, env::BorderBased::BorderSet &targetset)
convert to borders
Definition: r2s2borderbased.cpp:182
OpenDRIVE converter from file to object sets.
Definition: xodr2borderbased.h:43
struct adore::if_xodr::XODR2BorderBasedConverter::@1 sampling
sampling configuration object
double numberOfPointsPerBorder
Definition: xodr2borderbased.h:75
void convert(const char *filename, adore::env::BorderBased::BorderSet *target_set, adore::env::TCDSet *tcdSet, adore::env::BorderBased::LanePositionedObjectSet *stoplineSet, adore::env::BorderBased::ParkingSpotSet *parkingSpotSet, BorderIDTranslation *idTranslation, double *x0, double *y0, bool transform=false)
full conversion of OpenDRIVE map to object representations
Definition: xodr2borderbased.cpp:26
virtual void getData(T &value)=0
Definition: com_patterns.h:97
virtual void write(const T &value)=0
abstract factory for adore::params classes
Definition: afactory.h:54
virtual APNavigation * getNavigation() const =0
virtual APMapProvider * getMapProvider() const =0
abstract class containing parameters to configure aspects of the map provider
Definition: ap_map_provider.h:25
virtual int getXODRLoaderPointsPerBorder() const =0
virtual double getVisibiltyRadius() const =0
abstract class containing parameters which configure navigation behaviour
Definition: ap_navigation.h:25
virtual double getLaneChangePenalty() const =0
virtual bool getActivePlottingGlobal()=0
#define LOG_W(...)
log on warning level
Definition: csvlog.h:46
#define LOG_I(...)
log on info level
Definition: csvlog.h:37
#define LOG_E(...)
log on error level
Definition: csvlog.h:55
void plotBorderNavigation(adore::env::BorderBased::Border *right, adore::env::BorderBased::Border *left, double normedCost, DLR_TS::PlotLab::AFigureStub *figure)
Definition: plot_border.h:157
std::vector< Border * > BorderSubSet
Definition: borderset.h:92
void set(T *data, T value, int size)
Definition: adoremath.h:39
Definition: areaofeffectconverter.h:20
Definition: navigation.h:35
double trans_y_
Definition: navigation.h:36
double rot_y_
Definition: navigation.h:36
double rot_psi_
Definition: navigation.h:36
double trans_z_
Definition: navigation.h:36
double rot_z_
Definition: navigation.h:36
double rot_x_
Definition: navigation.h:36
Config()
Definition: navigation.h:37
double trans_x_
Definition: navigation.h:36
The border struct contains data of the smallest.
Definition: border.h:62
This struct represents 3-dimensional coordines.
Definition: coordinate.h:34
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39
double getX() const
Get the x-coordinate.
Definition: vehiclemotionstate9d.h:54
double getY() const
Get the y-coordinate.
Definition: vehiclemotionstate9d.h:60
Definition: navigationgoal.h:26
adore::mad::GlobalPosition target_
Definition: navigationgoal.h:28
double z_
Definition: globalposition.h:23
double x_
Definition: globalposition.h:23
double y_
Definition: globalposition.h:23