add project vtd_ros for vtd to ros, not complete.

src/ros/catkin/src/autoware_map/CMakeLists.txt

@@ -1,40 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(autoware_map)
-
-find_package(autoware_build_flags REQUIRED)
-find_package(autoware_map_msgs REQUIRED)
-
-find_package(catkin REQUIRED COMPONENTS
-  roscpp
-  tf
-  geometry_msgs
-  visualization_msgs
-)
-
-set(CMAKE_CXX_FLAGS "-O2 -Wall ${CMAKE_CXX_FLAGS}")
-
-catkin_package(
- INCLUDE_DIRS include
- LIBRARIES autoware_map
- CATKIN_DEPENDS roscpp tf visualization_msgs geometry_msgs
-)
-
-include_directories(
-	include
-  ${catkin_INCLUDE_DIRS}
-  ${autoware_map_msgs_INCLUDE_DIRS}
-)
-
-add_library( autoware_map lib/autoware_map/map_handler.cpp lib/autoware_map/util.cpp lib/autoware_map/visualization.cpp )
-add_dependencies( autoware_map ${catkin_EXPORTED_TARGETS} )
-target_link_libraries( autoware_map ${catkin_LIBRARIES} )
-
-## Install executables and/or libraries
-install(TARGETS autoware_map
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
-
-## Install project namespaced headers
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})

src/ros/catkin/src/autoware_map/README.md

@@ -1,86 +0,0 @@
-# autoware_map package
-
-## Related Packages
-+ *autoware_map_msgs package*: Defines the message types for Autoware Map Format
-+ *map_file package*: Loads semantic map written as Autoware Map Format.
-
-## Code API
-This package provides library to access to autoware_map messages, similar to library provided by vector_map package.
-
-
-### Sample Code
-```
-#include <autoware_map/autoware_map.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "autwoare_map_subscriber");
-    ros::NodeHandle nh;
-
-    autoware_map::AutowareMap awm;
-
-    autoware_map::category_t awm_required_category =  autoware_map::Category::POINT |
-                                                      autoware_map::Category::AREA;
-
-    //awm.subscribe(nh, awm_required_category);
-    awm.subscribe(nh, awm_required_category, ros::Duration(5));
-
-    //find object from id
-    int point_id = 1;
-    autoware_map_msgs::Point point = awm.findByKey(autoware_map::Key<autoware_map_msgs::Point>(point_id));
-    if(point.point_id == point_id)
-    {
-        std::cout << point << std::endl;
-    }
-    else
-    {
-        std::cerr << "failed to find a point with id: " << point_id << std::endl;
-    }
-
-    //find multiple object that satisfies desired conditions
-    std::vector<autoware_map_msgs::Area> area_arrays;
-    area_arrays = awm.findByFilter( [](const autoware_map_msgs::Area){return true;} );
-    for(auto area: area_arrays)
-    {
-        std::cout << area << std::endl;
-    }
-}
-```
-### Code Explanation
-```
-    autoware_map::category_t awm_required_category =  autoware_map::Category::POINT |
-                                                      autoware_map::Category::AREA;
-```
-Above code enables to specify topics that users would like to subscribe.   
-Category is set for each autoware_map_msgs type as follwing:
-+ autoware_map::Category::NONE
-+ autoware_map::Category::LANE
-+ autoware_map::Category::LANE_ATTR_RELATION
-+ autoware_map::Category::LANE_RELATION
-+ autoware_map::Category::LANE_SIGNAL_LIGHT_RELATION
-+ autoware_map::Category::LANE_CHANGE_RELATION
-+ autoware_map::Category::OPPOSITE_LANE_RELATION
-+ autoware_map::Category::POINT
-+ autoware_map::Category::AREA
-+ autoware_map::Category::SIGNAL
-+ autoware_map::Category::SIGNAL_LIGHT
-+ autoware_map::Category::WAYAREA
-+ autoware_map::Category::WAYPOINT
-+ autoware_map::Category::WAYPOINT_LANE_RELATION
-+ autoware_map::Category::WAYPOINT_RELATION
-+ autoware_map::Category::WAYPOINT_SIGNAL_RELATION
-+ autoware_map::Category::ALL
-
-```
-//awm.subscribe(nh, awm_required_category);
-awm.subscribe(nh, awm_required_category, ros::Duration(5));
-```
-Above code actually subscribes to specified topics.
-The function in the comment blocks the process until messages are recieved from all specified categories,  
-whereas the second function blocks for user specified duration.
-
-```
-awm.findByKey(autoware_map::Key<autoware_map_msgs::Point>(point_id));
-awm.findByFilter( [](const autoware_map_msgs::Area){return true;});
-```
-The above code allows to retrieve user specified object.

src/ros/catkin/src/autoware_map/include/autoware_map/map_elements.hpp

@@ -1,512 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-/**
- * @file map_element.hpp
- * @brief Contains wrappers for autoware_map_msgs
- * @author Ryohsuke Mitsudome
- * @date 2019/03/28
- */
-
-#ifndef __AUTOWARE_MAP_ELEMENTS_HPP__
-#define __AUTOWARE_MAP_ELEMENTS_HPP__
-#include <memory>
-#include <ros/ros.h>
-#include <autoware_map_msgs/LaneArray.h>
-#include <autoware_map_msgs/LaneAttributeRelationArray.h>
-#include <autoware_map_msgs/LaneRelationArray.h>
-#include <autoware_map_msgs/LaneSignalLightRelationArray.h>
-#include <autoware_map_msgs/LaneChangeRelationArray.h>
-#include <autoware_map_msgs/OppositeLaneRelationArray.h>
-#include <autoware_map_msgs/PointArray.h>
-#include <autoware_map_msgs/AreaArray.h>
-#include <autoware_map_msgs/SignalArray.h>
-#include <autoware_map_msgs/SignalLightArray.h>
-#include <autoware_map_msgs/WayareaArray.h>
-#include <autoware_map_msgs/WaypointArray.h>
-#include <autoware_map_msgs/WaypointLaneRelationArray.h>
-#include <autoware_map_msgs/WaypointRelationArray.h>
-#include <autoware_map_msgs/WaypointSignalRelationArray.h>
-#include <unordered_map>
-
-template<typename T>
-using shared_vector = std::vector<std::shared_ptr<T> >;
-template<typename T>
-using weak_vector = std::vector<std::weak_ptr<T> >;
-template<typename U,typename T>
-using shared_unordered_map = std::unordered_map<U,std::shared_ptr<T> >;
-
-template<typename T>
-std::vector<T> weakToStdVector(weak_vector<T> weak_vec)
-{
-  std::vector<T> vec;
-  for (auto weak : weak_vec)
-  {
-    vec.push_back(*(weak.lock()));
-  }
-  return vec;
-}
-
-template<typename T>
-shared_vector<T> weakToSharedVector(weak_vector<T> weak_vec)
-{
-  shared_vector<T> shared_vec;
-  for (auto weak : weak_vec)
-  {
-    shared_vec.push_back(weak.lock());
-  }
-  return shared_vec;
-}
-
-namespace autoware_map {
-using AreaMsg = autoware_map_msgs::Area;
-using LaneMsg = autoware_map_msgs::Lane;
-using LaneAttributeRelationMsg = autoware_map_msgs::LaneAttributeRelation;
-using LaneChangeRelationMsg = autoware_map_msgs::LaneChangeRelation;
-using LaneRelationMsg = autoware_map_msgs::LaneRelation;
-using LaneSignalLightRelationMsg = autoware_map_msgs::LaneSignalLightRelation;
-using OppositeLaneRelationMsg = autoware_map_msgs::OppositeLaneRelation;
-using PointMsg = autoware_map_msgs::Point;
-using SignalLightMsg = autoware_map_msgs::SignalLight;
-using SignalMsg = autoware_map_msgs::Signal;
-using WayareaMsg = autoware_map_msgs::Wayarea;
-using WaypointMsg = autoware_map_msgs::Waypoint;
-using WaypointLaneRelationMsg = autoware_map_msgs::WaypointLaneRelation;
-using WaypointRelationMsg = autoware_map_msgs::WaypointRelation;
-using WaypointSignalRelationMsg = autoware_map_msgs::WaypointSignalRelation;
-
-class Area;
-class Lane;
-class LaneAttributeRelation;
-class LaneChangeRelation;
-class LaneRelation;
-class LaneSignalLightRelation;
-class OppositeLaneRelation;
-class Point;
-class SignalLight;
-class Signal;
-class Wayarea;
-class Waypoint;
-class WaypointLaneRelation;
-class WaypointRelation;
-class WaypointSignalRelation;
-
-
-class WaypointLaneRelation : public WaypointLaneRelationMsg {
-public:
-  std::weak_ptr<Waypoint> waypoint;
-  std::weak_ptr<Lane> lane;
-  WaypointLaneRelation()
-  {
-  }
-  WaypointLaneRelation(autoware_map_msgs::WaypointLaneRelation relation)
-  {
-    waypoint_id = relation.waypoint_id;
-    lane_id = relation.lane_id;
-  }
-  std::shared_ptr<Waypoint> getWaypointPtr()
-  {
-    return waypoint.lock();
-  }
-  std::shared_ptr<Lane> getLane()
-  {
-    return lane.lock();
-  }
-};
-
-class WaypointRelation : public WaypointRelationMsg {
-public:
-  std::weak_ptr<Waypoint> waypoint;
-  std::weak_ptr<Waypoint> next_waypoint;
-  WaypointRelation()
-  {
-  }
-  WaypointRelation(autoware_map_msgs::WaypointRelation relation)
-  {
-    waypoint_id = relation.waypoint_id;
-    next_waypoint_id = relation.next_waypoint_id;
-    yaw = relation.yaw;
-    blinker = relation.blinker;
-    distance =relation.distance;
-  }
-  std::shared_ptr<Waypoint> getWaypointPtr()
-  {
-    return waypoint.lock();
-  }
-  std::shared_ptr<Waypoint> getNextWaypointPtr()
-  {
-    return next_waypoint.lock();
-  }
-};
-
-class WaypointSignalRelation : public WaypointSignalRelationMsg {
-public:
-  std::weak_ptr<Waypoint> waypoint;
-  std::weak_ptr<Signal> signal;
-  WaypointSignalRelation(WaypointSignalRelationMsg relation)
-  {
-    waypoint_id = relation.waypoint_id;
-    signal_id = relation.signal_id;
-  }
-  std::shared_ptr<Waypoint> getWaypoint()
-  {
-    return waypoint.lock();
-  }
-  std::shared_ptr<Signal> getSignal()
-  {
-    return signal.lock();
-  }
-};
-
-class LaneAttributeRelation : public LaneAttributeRelationMsg {
-public:
-  std::weak_ptr<Lane> lane;
-  std::weak_ptr<Area> area;
-
-  LaneAttributeRelation(LaneAttributeRelationMsg relation)
-  {
-    this->lane_id = relation.lane_id;
-    this->attribute_type = relation.attribute_type;
-    this->area_id = relation.area_id;
-  }
-  std::shared_ptr<Lane> getLane()
-  {
-    return lane.lock();
-  }
-  std::shared_ptr<Area> getArea()
-  {
-    if(area_id == 0) {
-      ROS_ERROR_STREAM( "no area available!!" << std::endl );
-      exit(1);
-    }
-    return area.lock();
-  }
-};
-
-class LaneChangeRelation : public LaneChangeRelationMsg {
-public:
-  std::weak_ptr<Lane> lane;
-  std::weak_ptr<Lane> next_lane;
-
-  LaneChangeRelation(LaneChangeRelationMsg relation)
-  {
-    lane_id = relation.lane_id;
-    next_lane_id = relation.next_lane_id;
-    blinker = blinker;
-  }
-  std::shared_ptr<Lane> getLane()
-  {
-    return lane.lock();
-  }
-  std::shared_ptr<Lane> getNextLane()
-  {
-    return next_lane.lock();
-  }
-
-};
-
-class LaneRelation : public LaneRelationMsg {
-public:
-  std::weak_ptr<Lane> lane;
-  std::weak_ptr<Lane> next_lane;
-
-  LaneRelation(LaneRelationMsg relation)
-  {
-    lane_id = relation.lane_id;
-    next_lane_id = relation.next_lane_id;
-    blinker = relation.blinker;
-  }
-  std::shared_ptr<Lane> getLane()
-  {
-    return lane.lock();
-  }
-  std::shared_ptr<Lane> getNextLane()
-  {
-    return next_lane.lock();
-  }
-
-};
-
-class LaneSignalLightRelation : public LaneSignalLightRelationMsg {
-public:
-  std::weak_ptr<Lane> lane;
-  std::weak_ptr<SignalLight> signal_light;
-
-  LaneSignalLightRelation(LaneSignalLightRelationMsg relation)
-  {
-    lane_id = relation.lane_id;
-    signal_light_id = relation.signal_light_id;
-  }
-  std::shared_ptr<Lane> getLane(){ return lane.lock(); }
-  std::shared_ptr<SignalLight> getSignalLight(){ return signal_light.lock(); }
-
-};
-
-class OppositeLaneRelation : public OppositeLaneRelationMsg {
-public:
-  std::weak_ptr<Lane> lane;
-  std::weak_ptr<Lane> opposite_lane;
-
-  OppositeLaneRelation(OppositeLaneRelationMsg relation)
-  {
-    lane_id = relation.lane_id;
-    opposite_lane_id = relation.opposite_lane_id;
-  }
-  std::shared_ptr<Lane> getLane(){ return lane.lock(); }
-  std::shared_ptr<Lane> getOppositeLane(){ return opposite_lane.lock(); }
-
-};
-
-
-class Area : public AreaMsg {
-public:
-  int id;
-  weak_vector<Point> points;
-  Area(AreaMsg area)
-  {
-    id = area.area_id;
-    area_id = area.area_id;
-    point_ids = area.point_ids;
-  }
-  shared_vector<Point> getPoints()
-  {
-    return weakToSharedVector(points);
-  }
-};
-
-class Lane : public LaneMsg
-{
-public:
-  int id;
-  weak_vector<Waypoint> waypoints;
-
-  weak_vector<LaneAttributeRelation> lane_attribute_relations;
-  weak_vector<LaneChangeRelation> lane_change_relations;
-
-  weak_vector<LaneRelation> lane_relations;
-  weak_vector<LaneSignalLightRelation> signal_light_relations;
-  weak_vector<OppositeLaneRelation> opposite_lane_relations;
-  weak_vector<WaypointLaneRelation> waypoint_lane_relations;
-  std::weak_ptr<Waypoint> start_waypoint;
-  std::weak_ptr<Waypoint> end_waypoint;
-
-  shared_vector<Waypoint> getWaypoints()
-  {
-    return weakToSharedVector(waypoints);
-  }
-
-  std::shared_ptr<Waypoint> getStartWaypoint()
-  {
-    return start_waypoint.lock();
-  }
-  std::shared_ptr<Waypoint> getEndWaypoint()
-  {
-    return end_waypoint.lock();
-  }
-
-  Lane(LaneMsg lane)
-  {
-    id = lane.lane_id;
-    lane_id = lane.lane_id;
-    start_waypoint_id = lane.start_waypoint_id;
-    end_waypoint_id = lane.end_waypoint_id;
-    lane_number = lane.lane_number;
-    num_of_lanes = lane.num_of_lanes;
-    speed_limit = lane.speed_limit;
-    length = lane.length;
-    width_limit = lane.width_limit;
-    height_limit = lane.height_limit;
-    weight_limit = lane.weight_limit;
-  }
-  Lane& operator=(LaneMsg lane)
-  {
-    this->id = lane.lane_id;
-    this->lane_id = lane.lane_id;
-    this->start_waypoint_id = lane.start_waypoint_id;
-    this->end_waypoint_id = lane.end_waypoint_id;
-    this->lane_number = lane.lane_number;
-    this->num_of_lanes = lane.num_of_lanes;
-    this->speed_limit = lane.speed_limit;
-    this->length = lane.length;
-    this->width_limit = lane.width_limit;
-    this->height_limit = lane.height_limit;
-    this->weight_limit = lane.weight_limit;
-    return *this;
-  }
-};
-
-
-class Point : public autoware_map_msgs::Point {
-public:
-  int id;
-  Point(PointMsg point)
-  {
-    id = point.point_id;
-    point_id = point.point_id;
-    x = point.x;
-    y = point.y;
-    z = point.z;
-    mgrs = point.mgrs;
-    epsg = point.epsg;
-    pcd = point.pcd;
-    lat = point.lat;
-    lng = point.lng;
-  }
-};
-
-class Signal : public SignalMsg {
-public:
-  int id;
-  weak_vector<SignalLight> signal_lights;
-  weak_vector<WaypointSignalRelation> waypoint_signal_relations;
-
-  Signal(autoware_map_msgs::Signal signal)
-  {
-    signal_id = signal.signal_id;
-    id = signal.signal_id;
-  }
-
-};
-
-class SignalLight : public SignalLightMsg {
-public:
-  int id;
-  std::weak_ptr<Signal> signal;
-  weak_vector<LaneSignalLightRelation> signal_light_relations;
-
-  SignalLight(autoware_map_msgs::SignalLight light)
-  {
-    id = light.signal_light_id;
-    signal_light_id = light.signal_light_id;
-    signal_id = light.signal_id;
-    point_id = light.point_id;
-    horizontal_angle = light.horizontal_angle;
-    vertical_angle = light.vertical_angle;
-    color_type = light.color_type;
-    arrow_type = light.arrow_type;
-  }
-};
-
-class Wayarea : public WayareaMsg {
-public:
-  int id;
-  std::weak_ptr<Area> area;
-
-  Wayarea(autoware_map_msgs::Wayarea wayarea)
-  {
-    id = wayarea.wayarea_id;
-    wayarea_id = wayarea.wayarea_id;
-    area_id = wayarea.area_id;
-  }
-};
-
-class Waypoint : public WaypointMsg {
-public:
-  int id;
-  std::weak_ptr<Point> point;
-
-  weak_vector<WaypointLaneRelation> waypoint_lane_relations;
-  weak_vector<WaypointRelation> waypoint_relations;
-  weak_vector<WaypointSignalRelation> waypoint_signal_relations;
-
-  Waypoint( autoware_map_msgs::Waypoint waypoint)
-  {
-    id = waypoint.waypoint_id;
-    waypoint_id = waypoint.waypoint_id;
-    point_id = waypoint.point_id;
-    velocity= waypoint.velocity;
-    stop_line= waypoint.stop_line;
-    left_width= waypoint.left_width;
-    right_width= waypoint.right_width;
-    height= waypoint.height;
-  }
-
-  std::shared_ptr<Point> getPointPtr()
-  {
-    return point.lock();
-  }
-
-  shared_vector<WaypointRelation> getWaypointRelations()
-  {
-    return weakToSharedVector( waypoint_relations);
-  }
-  std::vector<WaypointRelation> getStdWaypointRelations()
-  {
-    return weakToStdVector( waypoint_relations);
-  }
-
-
-  shared_vector<Waypoint> getNextWaypoints()
-  {
-    shared_vector<Waypoint> waypoints;
-    for(auto relation : waypoint_relations)
-    {
-      if(relation.lock()->waypoint_id == id) {
-        waypoints.push_back(relation.lock()->next_waypoint.lock());
-      }
-    }
-    return waypoints;
-  }
-
-  bool getWaypointRelation(int lane_id, WaypointRelationMsg &relation_msg)
-  {
-    for (auto relation : getWaypointRelations())
-    {
-      if(relation->getNextWaypointPtr()->belongLane(lane_id)) {
-        relation_msg = *relation;
-        return true;
-      }
-    }
-    return false;
-  }
-
-  bool belongLane(int lane_id)
-  {
-    for(auto relation : waypoint_lane_relations)
-    {
-      if(relation.lock()->lane_id == lane_id) {
-        return true;
-      }
-    }
-    return false;
-  }
-  shared_vector<Lane> getBelongingLanes()
-  {
-    shared_vector<Lane> belonging_lanes;
-    for(const auto &relation : waypoint_lane_relations)
-    {
-      belonging_lanes.push_back(relation.lock()->lane.lock());
-    }
-    return belonging_lanes;
-  }
-  std::shared_ptr<Waypoint> getNextWaypoint(int lane_id)
-  {
-    shared_vector<Waypoint> waypoints = getNextWaypoints();
-    for(auto waypoint : waypoints)
-    {
-      if(waypoint->belongLane(lane_id)) {
-        return waypoint;
-      }
-    }
-    //return itself if there is not next waypoint
-    ROS_WARN_STREAM("failed to find next waypoint from waypoint " << id << " in lane " << lane_id);
-    return std::make_shared<Waypoint>(*this);
-  }
-};
-
-}
-
-#endif

src/ros/catkin/src/autoware_map/include/autoware_map/map_handler.hpp

@@ -1,120 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef __AUTOWARE_MAP_HANDLER_HPP__
-#define __AUTOWARE_MAP_HANDLER_HPP__
-
-#include <autoware_map/map_elements.hpp>
-namespace autoware_map {
-
-using category_t = uint64_t;
-
-enum Category : category_t
-{
-  NONE = 0LLU,
-
-  AREA = 1LLU << 0,
-  LANE = 1LLU << 1,
-  LANE_ATTRIBUTE_RELATION = 1LLU << 2,
-  LANE_RELATION = 1LLU << 3,
-  LANE_SIGNAL_LIGHT_RELATION = 1LLU << 4,
-  LANE_CHANGE_RELATION = 1LLU << 5,
-  OPPOSITE_LANE_RELATION = 1LLU << 6,
-  POINT = 1LLU << 7,
-  SIGNAL = 1LLU << 8,
-  SIGNAL_LIGHT = 1LLU << 9,
-  WAYAREA = 1LLU << 10,
-  WAYPOINT = 1LLU << 11,
-  WAYPOINT_LANE_RELATION = 1LLU << 12,
-  WAYPOINT_RELATION = 1LLU << 13,
-  WAYPOINT_SIGNAL_RELATION = 1LLU << 14,
-
-  ALL = (1LLU << 32) - 1
-};
-
-template <class T>
-using Filter = std::function<bool(const T&)>;
-
-class AutowareMapHandler {
-  //objects
-  shared_unordered_map<int, Area> areas_;
-  shared_unordered_map<int, Lane> lanes_;
-  shared_unordered_map<int, Point> points_;
-  shared_unordered_map<int, Signal> signals_;
-  shared_unordered_map<int, SignalLight> signal_lights_;
-  shared_unordered_map<int, Wayarea> wayareas_;
-  shared_unordered_map<int, Waypoint> waypoints_;
-
-  //relations
-  shared_vector<LaneAttributeRelation> lane_attribute_relations_;
-  shared_vector<LaneChangeRelation> lane_change_relations_;
-  shared_vector<LaneRelation> lane_relations_;
-  shared_vector<LaneSignalLightRelation> lane_signal_light_relations_;
-  shared_vector<OppositeLaneRelation> opposite_lane_relations_;
-  shared_vector<WaypointLaneRelation> waypoint_lane_relations_;
-  shared_vector<WaypointRelation> waypoint_relations_;
-  shared_vector<WaypointSignalRelation> waypoint_signal_relations_;
-
-public:
-  AutowareMapHandler(){};
-  void registerSubscriber(ros::NodeHandle& nh, category_t category);
-  void subscribe(ros::NodeHandle& nh, category_t category);
-  void subscribe(ros::NodeHandle& nh, category_t category, const ros::Duration& timeout);
-  void subscribe(ros::NodeHandle& nh, category_t category, const size_t max_retries);
-
-
-  void setFromAutowareMapMsgs(const std::vector<autoware_map_msgs::Area> areas,
-                              const std::vector<autoware_map_msgs::Lane> lanes,
-                              const std::vector<autoware_map_msgs::LaneAttributeRelation> lane_attribute_relations,
-                              const std::vector<autoware_map_msgs::LaneChangeRelation> lane_change_relations,
-                              const std::vector<autoware_map_msgs::LaneRelation> lane_relations,
-                              const std::vector<autoware_map_msgs::LaneSignalLightRelation> lane_signal_light_relations,
-                              const std::vector<autoware_map_msgs::OppositeLaneRelation> opposite_lane_relations,
-                              const std::vector<autoware_map_msgs::Point> points,
-                              const std::vector<autoware_map_msgs::Signal> signals,
-                              const std::vector<autoware_map_msgs::SignalLight> signal_lights,
-                              const std::vector<autoware_map_msgs::Wayarea> wayareas,
-                              const std::vector<autoware_map_msgs::Waypoint> waypoints,
-                              const std::vector<autoware_map_msgs::WaypointLaneRelation> waypoint_lane_relations,
-                              const std::vector<autoware_map_msgs::WaypointRelation> waypoint_relations,
-                              const std::vector<autoware_map_msgs::WaypointSignalRelation> waypoint_signal_relations
-                              );
-  void resolveRelations();
-
-  template<typename T>
-  T findById(const int id) const;
-
-  std::vector<Area> findByFilter(const Filter<Area>& filter) const;
-  std::vector<Lane> findByFilter(const Filter<Lane>& filter) const;
-  std::vector<Point> findByFilter(const Filter<Point>& filter) const;
-  std::vector<Signal> findByFilter(const Filter<Signal>& filter) const;
-  std::vector<SignalLight> findByFilter(const Filter<SignalLight>& filter) const;
-  std::vector<Wayarea> findByFilter(const Filter<Wayarea>& filter) const;
-  std::vector<Waypoint> findByFilter(const Filter<Waypoint>& filter) const;
-  std::vector<LaneAttributeRelation> findByFilter(const Filter<LaneAttributeRelation>& filter) const;
-  std::vector<LaneChangeRelation> findByFilter(const Filter<LaneChangeRelation>& filter) const;
-  std::vector<LaneRelation> findByFilter(const Filter<LaneRelation>& filter) const;
-  std::vector<LaneSignalLightRelation> findByFilter(const Filter<LaneSignalLightRelation>& filter) const;
-  std::vector<OppositeLaneRelation> findByFilter(const Filter<OppositeLaneRelation>& filter) const;
-  std::vector<WaypointLaneRelation> findByFilter(const Filter<WaypointLaneRelation>& filter) const;
-  std::vector<WaypointRelation> findByFilter(const Filter<WaypointRelation>& filter) const;
-  std::vector<WaypointSignalRelation> findByFilter(const Filter<WaypointSignalRelation>& filter) const;
-
-};
-}
-
-
-#endif

src/ros/catkin/src/autoware_map/include/autoware_map/util.h

@@ -1,69 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-#ifndef __AUTOWARE_MAP_UTIL_H__
-#define __AUTOWARE_MAP_UTIL_H__
-
-#include <autoware_map/map_handler.hpp>
-#include <geometry_msgs/Point.h>
-#include <geometry_msgs/Quaternion.h>
-
-#ifndef M_PI
-#define M_PI 3.14159265358979
-#endif
-
-inline double degreeToRadian(double x) {return x * M_PI / 180.0;}
-
-autoware_map_msgs::Point getPointFromWaypointId(int waypoint_id, autoware_map::AutowareMapHandler autoware_map);
-geometry_msgs::Point convertPointToGeomPoint(const autoware_map_msgs::Point& autoware_point);
-geometry_msgs::Quaternion convertAngleToGeomQuaternion(const double horizontal_angle, const double vertical_angle);
-bool isJapaneseCoordinate(int epsg);
-
-//
-std::ostream& operator<<(std::ostream& os, const autoware_map_msgs::Lane& obj);
-std::ostream& operator<<(std::ostream& os, const autoware_map_msgs::LaneAttributeRelation& obj);
-std::ostream& operator<<(std::ostream& os, const autoware_map_msgs::LaneRelation& obj);
-std::ostream& operator<<(std::ostream& os, const autoware_map_msgs::LaneSignalLightRelation& obj);
-std::ostream& operator<<(std::ostream& os, const autoware_map_msgs::LaneChangeRelation& obj);
-std::ostream& operator<<(std::ostream& os, const autoware_map_msgs::OppositeLaneRelation& obj);
-std::ostream& operator<<(std::ostream& os, const autoware_map_msgs::Point& obj);
-std::ostream& operator<<(std::ostream& os, const autoware_map_msgs::Area& obj);
-std::ostream& operator<<(std::ostream& os, const autoware_map_msgs::Signal& obj);
-std::ostream& operator<<(std::ostream& os, const autoware_map_msgs::SignalLight& obj);
-std::ostream& operator<<(std::ostream& os, const autoware_map_msgs::Wayarea& obj);
-std::ostream& operator<<(std::ostream& os, const autoware_map_msgs::Waypoint& obj);
-std::ostream& operator<<(std::ostream& os, const autoware_map_msgs::WaypointLaneRelation& obj);
-std::ostream& operator<<(std::ostream& os, const autoware_map_msgs::WaypointRelation& obj);
-std::ostream& operator<<(std::ostream& os, const autoware_map_msgs::WaypointSignalRelation& obj);
-std::ostream& operator<<(std::ostream& os, const autoware_map::Category& cat);
-
-//
-std::istream& operator>>(std::istream& os, autoware_map_msgs::Lane& obj);
-std::istream& operator>>(std::istream& os, autoware_map_msgs::LaneAttributeRelation& obj);
-std::istream& operator>>(std::istream& os, autoware_map_msgs::LaneRelation& obj);
-std::istream& operator>>(std::istream& os, autoware_map_msgs::LaneSignalLightRelation& obj);
-std::istream& operator>>(std::istream& os, autoware_map_msgs::LaneChangeRelation& obj);
-std::istream& operator>>(std::istream& os, autoware_map_msgs::OppositeLaneRelation& obj);
-std::istream& operator>>(std::istream& os, autoware_map_msgs::Point& obj);
-std::istream& operator>>(std::istream& os, autoware_map_msgs::Area& obj);
-std::istream& operator>>(std::istream& os, autoware_map_msgs::Signal& obj);
-std::istream& operator>>(std::istream& os, autoware_map_msgs::SignalLight& obj);
-std::istream& operator>>(std::istream& os, autoware_map_msgs::Wayarea& obj);
-std::istream& operator>>(std::istream& os, autoware_map_msgs::Waypoint& obj);
-std::istream& operator>>(std::istream& os, autoware_map_msgs::WaypointLaneRelation& obj);
-std::istream& operator>>(std::istream& os, autoware_map_msgs::WaypointSignalRelation& obj);
-std::istream& operator>>(std::istream& os, autoware_map_msgs::WaypointRelation& obj);
-
-#endif

src/ros/catkin/src/autoware_map/include/autoware_map/visualization.h

@@ -1,60 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-#ifndef __AUTOWARE_MAP_VISUALIZATION_H__
-#define __AUTOWARE_MAP_VISUALIZATION_H__
-
-
-#include <ros/console.h>
-#include <std_msgs/Bool.h>
-#include <visualization_msgs/MarkerArray.h>
-#include <autoware_map/map_handler.hpp>
-#include <autoware_map/visualization.h>
-
-enum Color : int
-{
-    BLACK,
-    GRAY,
-    LIGHT_RED,
-    LIGHT_GREEN,
-    LIGHT_BLUE,
-    LIGHT_YELLOW,
-    LIGHT_CYAN,
-    LIGHT_MAGENTA,
-    RED,
-    GREEN,
-    BLUE,
-    YELLOW,
-    CYAN,
-    MAGENTA,
-    WHITE
-};
-
-std_msgs::ColorRGBA createColorRGBA(Color color);
-void insertMarkerArray(visualization_msgs::MarkerArray& a1, const visualization_msgs::MarkerArray& a2);
-void enableMarker(visualization_msgs::Marker& marker);
-void disableMarker(visualization_msgs::Marker& marker);
-bool isValidMarker(const visualization_msgs::Marker& marker);
-visualization_msgs::Marker createMarker(const std::string& ns, int id, int type);
-visualization_msgs::Marker createVectorMarkerFromSignal(const std::string& ns, int id, Color color, const autoware_map::AutowareMapHandler& autoware_map,
-                                              const autoware_map_msgs::SignalLight signal_light);
-visualization_msgs::MarkerArray createSignalMarkerArray(const autoware_map::AutowareMapHandler& autoware_map, Color red_color = Color::RED, Color blue_color = Color::GREEN,
-                                                        Color yellow_color = Color::YELLOW, Color other_color = Color::GRAY);
-visualization_msgs::MarkerArray createAreaMarkerArray(const autoware_map::AutowareMapHandler& autoware_map, Color color);
-visualization_msgs::MarkerArray createWaypointSignalRelationMarkerArray(const autoware_map::AutowareMapHandler& amap, Color color);
-visualization_msgs::MarkerArray createPointMarkerArray(const autoware_map::AutowareMapHandler& amap, Color color);
-visualization_msgs::MarkerArray createWaypointMarkerArray(const autoware_map::AutowareMapHandler& autoware_map, Color color, Color stop_color = Color::RED);
-visualization_msgs::MarkerArray createWaypointRelationMarkerArray(const autoware_map::AutowareMapHandler& autoware_map, Color color);
-#endif

src/ros/catkin/src/autoware_map/package.xml

@@ -1,29 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <name>autoware_map</name>
-  <version>0.1.0</version>
-  <description>This package contains library to handle Autoware Map Format</description>
-  <maintainer email="ryohsuke.mitsudome@tier4.jp">mitsudome-r</maintainer>
-  <license>Apache 2</license>
-  <buildtool_depend>catkin</buildtool_depend>
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>tf</build_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>visualization_msgs</build_depend>
-  <build_depend>autoware_map_msgs</build_depend>
-  <build_export_depend>roscpp</build_export_depend>
-  <build_export_depend>geometry_msgs</build_export_depend>
-  <build_export_depend>tf</build_export_depend>
-  <build_export_depend>visualization_msgs</build_export_depend>
-  <build_export_depend>autoware_map_msgs</build_export_depend>
-  <exec_depend>roscpp</exec_depend>
-  <exec_depend>tf</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>visualization_msgs</exec_depend>
-  <exec_depend>autoware_map_msgs</exec_depend>
-  <exec_depend>visualization_msgs</exec_depend>
-  <exec_depend>autoware_map_msgs</exec_depend>
-  <export>
-  </export>
-</package>

src/ros/catkin/src/controllertocan/CMakeLists.txt

@@ -5,10 +5,11 @@ project(controllertocan)
 message(STATUS  "Enter controllertocan")
 
 
-SET(QT_PATH  /opt/qt/5.13.2/gcc_64)
+SET(QT_PATH  /usr/include/x86_64-linux-gnu/qt5)
+SET(QT2_PATH  /usr/include/x86_64-linux-gnu/qt5)
 SET(AGX_QT_PATH  /usr/include/aarch64-linux-gnu/qt5)
 
-find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore)
+find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore  ${QT2_PATH}/QtCore)
 
 if(QT_EXIST)
 include_directories(
@@ -16,6 +17,8 @@ include_directories(
   ${QT_PATH}/include/QtCore
   ${AGX_QT_PATH}
   ${AGX_QT_PATH}/QtCore
+    ${QT2_PATH}
+    ${QT2_PATH}/QtCore
 )
 link_directories(
   ${QT_PATH}/lib

src/ros/catkin/src/controllertocan_haomo/CMakeLists.txt

@@ -5,10 +5,11 @@ project(controllertocan_haomo)
 message(STATUS  "Enter controllertocan_haomo")
 
 
-SET(QT_PATH  /opt/qt/5.13.2/gcc_64)
+SET(QT_PATH  /usr/include/x86_64-linux-gnu/qt5)
+SET(QT2_PATH  /usr/include/x86_64-linux-gnu/qt5)
 SET(AGX_QT_PATH  /usr/include/aarch64-linux-gnu/qt5)
 
-find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore)
+find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore  ${QT2_PATH}/QtCore)
 
 if(QT_EXIST)
 include_directories(
@@ -16,6 +17,8 @@ include_directories(
   ${QT_PATH}/include/QtCore
   ${AGX_QT_PATH}
   ${AGX_QT_PATH}/QtCore
+    ${QT2_PATH}
+    ${QT2_PATH}/QtCore
 )
 link_directories(
   ${QT_PATH}/lib

src/ros/catkin/src/driver_can_kvaser/CMakeLists.txt

@@ -5,10 +5,11 @@ project(driver_can_kvaser)
 message(STATUS  "Enter driver_can_kvaser")
 
 
-SET(QT_PATH  /opt/qt/5.13.2/gcc_64)
+SET(QT_PATH  /usr/include/x86_64-linux-gnu/qt5)
+SET(QT2_PATH  /usr/include/x86_64-linux-gnu/qt5)
 SET(AGX_QT_PATH  /usr/include/aarch64-linux-gnu/qt5)
 
-find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore)
+find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore  ${QT2_PATH}/QtCore)
 
 if(QT_EXIST)
 include_directories(
@@ -16,6 +17,8 @@ include_directories(
   ${QT_PATH}/include/QtCore
   ${AGX_QT_PATH}
   ${AGX_QT_PATH}/QtCore
+    ${QT2_PATH}
+    ${QT2_PATH}/QtCore
 )
 link_directories(
   ${QT_PATH}/lib

src/ros/catkin/src/driver_can_nvidia/CMakeLists.txt

@@ -5,10 +5,11 @@ project(driver_can_nvidia)
 message(STATUS  "Enter driver_can_nvidia")
 
 
-SET(QT_PATH  /opt/qt/5.13.2/gcc_64)
+SET(QT_PATH  /usr/include/x86_64-linux-gnu/qt5)
+SET(QT2_PATH  /usr/include/x86_64-linux-gnu/qt5)
 SET(AGX_QT_PATH  /usr/include/aarch64-linux-gnu/qt5)
 
-find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore)
+find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore  ${QT2_PATH}/QtCore)
 
 if(QT_EXIST)
 include_directories(
@@ -16,6 +17,8 @@ include_directories(
   ${QT_PATH}/include/QtCore
   ${AGX_QT_PATH}
   ${AGX_QT_PATH}/QtCore
+    ${QT2_PATH}
+    ${QT2_PATH}/QtCore
 )
 link_directories(
   ${QT_PATH}/lib

src/ros/catkin/src/lane_planner/lib/lane_planner/lane_planner_vmap.cpp

@@ -0,0 +1,778 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <fstream>
+#include <tuple>
+
+#include <ros/console.h>
+
+#include <tablet_socket_msgs/Waypoint.h>
+
+#include <gnss/geo_pos_conv.hpp>
+#include <lane_planner/lane_planner_vmap.hpp>
+
+namespace lane_planner {
+
+namespace vmap {
+
+namespace {
+
+void write_waypoint(const vector_map::Point& point, double yaw, double velocity, const std::string& path,
+        bool first);
+
+double compute_direction_angle(const vector_map::Point& p1, const vector_map::Point& p2);
+
+bool is_branching_point(const VectorMap& vmap, const vector_map::Point& point);
+bool is_merging_point(const VectorMap& vmap, const vector_map::Point& point);
+bool is_branching_lane(const vector_map::Lane& lane);
+bool is_merging_lane(const vector_map::Lane& lane);
+
+vector_map::Point find_start_point(const VectorMap& vmap, const vector_map::Lane& lane);
+vector_map::Point find_end_point(const VectorMap& vmap, const vector_map::Lane& lane);
+vector_map::Point find_departure_point(const VectorMap& lane_vmap, int lno,
+               const std::vector<vector_map::Point>& coarse_points,
+               double search_radius);
+vector_map::Point find_arrival_point(const VectorMap& lane_vmap, int lno,
+             const std::vector<vector_map::Point>& coarse_points,
+             double search_radius);
+vector_map::Point find_nearest_point(const VectorMap& vmap, const vector_map::Point& point);
+std::vector<vector_map::Point> find_near_points(const VectorMap& vmap, const vector_map::Point& point,
+            double search_radius);
+
+vector_map::Lane find_lane(const VectorMap& vmap, int lno, const vector_map::Point& point);
+vector_map::Lane find_prev_lane(const VectorMap& vmap, int lno, const vector_map::Lane& lane);
+vector_map::Lane find_next_lane(const VectorMap& vmap, int lno, const vector_map::Lane& lane);
+vector_map::Lane find_next_branching_lane(const VectorMap& vmap, int lno, const vector_map::Lane& lane,
+            double coarse_angle, double search_radius);
+
+void write_waypoint(const vector_map::Point& point, double yaw, double velocity, const std::string& path,
+        bool first)
+{
+  // reverse X-Y axis
+  if (first) {
+    std::ofstream ofs(path.c_str());
+    ofs << std::fixed << point.ly << ","
+        << std::fixed << point.bx << ","
+        << std::fixed << point.h << ","
+        << std::fixed << yaw << std::endl;
+  } else {
+    std::ofstream ofs(path.c_str(), std::ios_base::app);
+    ofs << std::fixed << point.ly << ","
+        << std::fixed << point.bx << ","
+        << std::fixed << point.h << ","
+        << std::fixed << yaw << ","
+        << std::fixed << velocity << std::endl;
+  }
+}
+
+double compute_direction_angle(const vector_map::Point& p1, const vector_map::Point& p2)
+{
+  return (atan2(p2.ly - p1.ly, p2.bx - p1.bx) * (180 / M_PI)); // -180 to 180 degrees
+}
+
+bool is_branching_point(const VectorMap& vmap, const vector_map::Point& point)
+{
+  vector_map::Lane lane = find_lane(vmap, LNO_ALL, point);
+  if (lane.lnid < 0)
+    return false;
+
+  lane = find_prev_lane(vmap, LNO_ALL, lane);
+  if (lane.lnid < 0)
+    return false;
+
+  return is_branching_lane(lane);
+}
+
+bool is_merging_point(const VectorMap& vmap, const vector_map::Point& point)
+{
+  vector_map::Lane lane = find_lane(vmap, LNO_ALL, point);
+  if (lane.lnid < 0)
+    return false;
+
+  return is_merging_lane(lane);
+}
+
+bool is_branching_lane(const vector_map::Lane& lane)
+{
+  return (lane.jct == 1 || lane.jct == 2 || lane.jct == 5);
+}
+
+bool is_merging_lane(const vector_map::Lane& lane)
+{
+  return (lane.jct == 3 || lane.jct == 4 || lane.jct == 5);
+}
+
+vector_map::Point find_start_point(const VectorMap& vmap, const vector_map::Lane& lane)
+{
+  vector_map::Point error;
+  error.pid = -1;
+
+  for (const vector_map::Node& n : vmap.nodes) {
+    if (n.nid != lane.bnid)
+      continue;
+    for (const vector_map::Point& p : vmap.points) {
+      if (p.pid != n.pid)
+        continue;
+      return p;
+    }
+  }
+
+  return error;
+}
+
+vector_map::Point find_end_point(const VectorMap& vmap, const vector_map::Lane& lane)
+{
+  vector_map::Point error;
+  error.pid = -1;
+
+  for (const vector_map::Node& n : vmap.nodes) {
+    if (n.nid != lane.fnid)
+      continue;
+    for (const vector_map::Point& p : vmap.points) {
+      if (p.pid != n.pid)
+        continue;
+      return p;
+    }
+  }
+
+  return error;
+}
+
+vector_map::Point find_departure_point(const VectorMap& lane_vmap, int lno,
+               const std::vector<vector_map::Point>& coarse_points,
+               double search_radius)
+{
+  vector_map::Point coarse_p1 = coarse_points[0];
+  vector_map::Point coarse_p2 = coarse_points[1];
+
+  vector_map::Point nearest_point = find_nearest_point(lane_vmap, coarse_p1);
+  if (nearest_point.pid < 0)
+    return nearest_point;
+
+  std::vector<vector_map::Point> near_points = find_near_points(lane_vmap, coarse_p1, search_radius);
+  double coarse_angle = compute_direction_angle(coarse_p1, coarse_p2);
+  double score = 180 + search_radius; // XXX better way?
+  for (const vector_map::Point& p1 : near_points) {
+    vector_map::Lane l = find_lane(lane_vmap, lno, p1);
+    if (l.lnid < 0)
+      continue;
+
+    vector_map::Point p2 = find_end_point(lane_vmap, l);
+    if (p2.pid < 0)
+      continue;
+
+    double a = compute_direction_angle(p1, p2);
+    a = fabs(a - coarse_angle);
+    if (a > 180)
+      a = fabs(a - 360);
+    double d = hypot(p1.bx - coarse_p1.bx, p1.ly - coarse_p1.ly);
+    double s = a + d;
+    if (s <= score) {
+      nearest_point = p1;
+      score = s;
+    }
+  }
+
+  return nearest_point;
+}
+
+vector_map::Point find_arrival_point(const VectorMap& lane_vmap, int lno,
+             const std::vector<vector_map::Point>& coarse_points,
+             double search_radius)
+{
+  vector_map::Point coarse_p1 = coarse_points[coarse_points.size() - 1];
+  vector_map::Point coarse_p2 = coarse_points[coarse_points.size() - 2];
+
+  vector_map::Point nearest_point = find_nearest_point(lane_vmap, coarse_p1);
+  if (nearest_point.pid < 0)
+    return nearest_point;
+
+  std::vector<vector_map::Point> near_points = find_near_points(lane_vmap, coarse_p1, search_radius);
+  double coarse_angle = compute_direction_angle(coarse_p1, coarse_p2);
+  double score = 180 + search_radius; // XXX better way?
+  for (const vector_map::Point& p1 : near_points) {
+    vector_map::Lane l = find_lane(lane_vmap, lno, p1);
+    if (l.lnid < 0)
+      continue;
+
+    l = find_prev_lane(lane_vmap, lno, l);
+    if (l.lnid < 0)
+      continue;
+
+    vector_map::Point p2 = find_start_point(lane_vmap, l);
+    if (p2.pid < 0)
+      continue;
+
+    double a = compute_direction_angle(p1, p2);
+    a = fabs(a - coarse_angle);
+    if (a > 180)
+      a = fabs(a - 360);
+    double d = hypot(p1.bx - coarse_p1.bx, p1.ly - coarse_p1.ly);
+    double s = a + d;
+    if (s <= score) {
+      nearest_point = p1;
+      score = s;
+    }
+  }
+
+  return nearest_point;
+}
+
+vector_map::Point find_nearest_point(const VectorMap& vmap, const vector_map::Point& point)
+{
+  vector_map::Point nearest_point;
+  nearest_point.pid = -1;
+
+  double distance = DBL_MAX;
+  for (const vector_map::Point& p : vmap.points) {
+    double d = hypot(p.bx - point.bx, p.ly - point.ly);
+    if (d <= distance) {
+      nearest_point = p;
+      distance = d;
+    }
+  }
+
+  return nearest_point;
+}
+
+std::vector<vector_map::Point> find_near_points(const VectorMap& vmap, const vector_map::Point& point,
+            double search_radius)
+{
+  std::vector<vector_map::Point> near_points;
+  for (const vector_map::Point& p : vmap.points) {
+    double d = hypot(p.bx - point.bx, p.ly - point.ly);
+    if (d <= search_radius)
+      near_points.push_back(p);
+  }
+
+  return near_points;
+}
+
+vector_map::Lane find_lane(const VectorMap& vmap, int lno, const vector_map::Point& point)
+{
+  vector_map::Lane error;
+  error.lnid = -1;
+
+  for (const vector_map::Node& n : vmap.nodes) {
+    if (n.pid != point.pid)
+      continue;
+    for (const vector_map::Lane& l : vmap.lanes) {
+      if (lno != LNO_ALL && l.lno != lno)
+        continue;
+      if (l.bnid != n.nid)
+        continue;
+      return l;
+    }
+  }
+
+  return error;
+}
+
+vector_map::Lane find_prev_lane(const VectorMap& vmap, int lno, const vector_map::Lane& lane)
+{
+  vector_map::Lane error;
+  error.lnid = -1;
+
+  if (is_merging_lane(lane)) {
+    for (const vector_map::Lane& l : vmap.lanes) {
+      if (lno != LNO_ALL && l.lno != lno)
+        continue;
+      if (l.lnid != lane.blid && l.lnid != lane.blid2 && l.lnid != lane.blid3 &&
+          l.lnid != lane.blid4)
+        continue;
+      return l;
+    }
+  } else {
+    for (const vector_map::Lane& l : vmap.lanes) {
+      if (l.lnid != lane.blid)
+        continue;
+      return l;
+    }
+  }
+
+  return error;
+}
+
+vector_map::Lane find_next_lane(const VectorMap& vmap, int lno, const vector_map::Lane& lane)
+{
+  vector_map::Lane error;
+  error.lnid = -1;
+
+  if (is_branching_lane(lane)) {
+    for (const vector_map::Lane& l : vmap.lanes) {
+      if (lno != LNO_ALL && l.lno != lno)
+        continue;
+      if (l.lnid != lane.flid && l.lnid != lane.flid2 && l.lnid != lane.flid3 &&
+          l.lnid != lane.flid4)
+        continue;
+      return l;
+    }
+  } else {
+    for (const vector_map::Lane& l : vmap.lanes) {
+      if (l.lnid != lane.flid)
+        continue;
+      return l;
+    }
+  }
+
+  return error;
+}
+
+vector_map::Lane find_next_branching_lane(const VectorMap& vmap, int lno, const vector_map::Lane& lane,
+            double coarse_angle, double search_radius)
+{
+  vector_map::Lane error;
+  error.lnid = -1;
+
+  vector_map::Point p1 = find_end_point(vmap, lane);
+  if (p1.pid < 0)
+    return error;
+
+  std::vector<std::tuple<vector_map::Point, vector_map::Lane>> candidates;
+  for (const vector_map::Lane& l1 : vmap.lanes) {
+    if (lno != LNO_ALL && l1.lno != lno)
+      continue;
+    if (l1.lnid == lane.flid || l1.lnid == lane.flid2 || l1.lnid == lane.flid3 || l1.lnid == lane.flid4) {
+      vector_map::Lane l2 = l1;
+      vector_map::Point p = find_end_point(vmap, l2);
+      if (p.pid < 0)
+        continue;
+      vector_map::Point p2 = p;
+      double d = hypot(p2.bx - p1.bx, p2.ly - p1.ly);
+      while (d <= search_radius && l2.flid != 0 && !is_branching_lane(l2)) {
+        l2 = find_next_lane(vmap, LNO_ALL, l2);
+        if (l2.lnid < 0)
+          break;
+        p = find_end_point(vmap, l2);
+        if (p.pid < 0)
+          break;
+        p2 = p;
+        d = hypot(p2.bx - p1.bx, p2.ly - p1.ly);
+      }
+      candidates.push_back(std::make_tuple(p2, l1));
+    }
+  }
+
+  if (candidates.empty())
+    return error;
+
+  vector_map::Lane branching_lane;
+  double angle = 180;
+  for (const std::tuple<vector_map::Point, vector_map::Lane>& c : candidates) {
+    vector_map::Point p2 = std::get<0>(c);
+    double a = compute_direction_angle(p1, p2);
+    a = fabs(a - coarse_angle);
+    if (a > 180)
+      a = fabs(a - 360);
+    if (a <= angle) {
+      branching_lane = std::get<1>(c);
+      angle = a;
+    }
+  }
+
+  return branching_lane;
+}
+
+} // namespace
+
+void write_waypoints(const std::vector<vector_map::Point>& points, double velocity, const std::string& path)
+{
+  if (points.size() < 2)
+    return;
+
+  size_t last_index = points.size() - 1;
+  for (size_t i = 0; i < points.size(); ++i) {
+    double yaw;
+    if (i == last_index) {
+      geometry_msgs::Point p1 = create_geometry_msgs_point(points[i]);
+      geometry_msgs::Point p2 = create_geometry_msgs_point(points[i - 1]);
+      yaw = atan2(p2.y - p1.y, p2.x - p1.x);
+      yaw -= M_PI;
+    } else {
+      geometry_msgs::Point p1 = create_geometry_msgs_point(points[i]);
+      geometry_msgs::Point p2 = create_geometry_msgs_point(points[i + 1]);
+      yaw = atan2(p2.y - p1.y, p2.x - p1.x);
+    }
+
+    write_waypoint(points[i], yaw, velocity, path, (i == 0));
+  }
+}
+
+double compute_reduction(const vector_map::DTLane& d, double w)
+{
+  return (1 - fabs(1 / d.r) * w); // 0 to 1 rates
+}
+
+bool is_straight_dtlane(const vector_map::DTLane& dtlane)
+{
+  return (dtlane.apara == 0 && dtlane.r == RADIUS_MAX);
+}
+
+bool is_curve_dtlane(const vector_map::DTLane& dtlane)
+{
+  return (dtlane.apara == 0 && dtlane.r != RADIUS_MAX);
+}
+
+// XXX better way?
+bool is_crossroad_dtlane(const vector_map::DTLane& dtlane)
+{
+  // take crossroad for 10 radius or less
+  return (fabs(dtlane.r) <= 10);
+}
+
+bool is_clothoid_dtlane(const vector_map::DTLane& dtlane)
+{
+  return (dtlane.apara != 0);
+}
+
+// XXX better way?
+bool is_connection_dtlane(const VectorMap& fine_vmap, int index)
+{
+  const vector_map::DTLane& dtlane = fine_vmap.dtlanes[index];
+  int size = fine_vmap.dtlanes.size();
+
+  int change = 0;
+  int straight = 0;
+  for (int i = index - 1; i >= 0; --i) {
+    if (dtlane.r != fine_vmap.dtlanes[i].r) {
+      ++change;
+      if (is_straight_dtlane(fine_vmap.dtlanes[i]))
+        ++straight;
+      break;
+    }
+  }
+  for (int i = index + 1; i < size; ++i) {
+    if (dtlane.r != fine_vmap.dtlanes[i].r) {
+      ++change;
+      if (is_straight_dtlane(fine_vmap.dtlanes[i]))
+        ++straight;
+      break;
+    }
+  }
+  if (change == 1 && straight == 1)
+    return true;
+  if (straight == 2)
+    return true;
+
+  return false;
+}
+
+geometry_msgs::Point create_geometry_msgs_point(const vector_map::Point& vp)
+{
+  // reverse X-Y axis
+  geometry_msgs::Point gp;
+  gp.x = vp.ly;
+  gp.y = vp.bx;
+  gp.z = vp.h;
+
+  return gp;
+}
+
+vector_map::Point create_vector_map_point(const geometry_msgs::Point& gp)
+{
+  // reverse X-Y axis
+  vector_map::Point vp;
+  vp.bx = gp.y;
+  vp.ly = gp.x;
+  vp.h = gp.z;
+
+  return vp;
+}
+
+autoware_msgs::DTLane create_waypoint_follower_dtlane(const vector_map::DTLane& vd)
+{
+  autoware_msgs::DTLane wd;
+  wd.dist = vd.dist;
+  wd.dir = vd.dir;
+  wd.apara = vd.apara;
+  wd.r = vd.r;
+  wd.slope = vd.slope;
+  wd.cant = vd.cant;
+  wd.lw = vd.lw;
+  wd.rw = vd.rw;
+
+  return wd;
+}
+
+vector_map::DTLane create_vector_map_dtlane(const autoware_msgs::DTLane& wd)
+{
+  vector_map::DTLane vd;
+  vd.dist = wd.dist;
+  vd.dir = wd.dir;
+  vd.apara = wd.apara;
+  vd.r = wd.r;
+  vd.slope = wd.slope;
+  vd.cant = wd.cant;
+  vd.lw = wd.lw;
+  vd.rw = wd.rw;
+
+  return vd;
+}
+
+VectorMap create_lane_vmap(const VectorMap& vmap, int lno)
+{
+  VectorMap lane_vmap;
+  for (const vector_map::Lane& l : vmap.lanes) {
+    if (lno != LNO_ALL && l.lno != lno)
+      continue;
+    lane_vmap.lanes.push_back(l);
+
+    for (const vector_map::Node& n : vmap.nodes) {
+      if (n.nid != l.bnid && n.nid != l.fnid)
+        continue;
+      lane_vmap.nodes.push_back(n);
+
+      for (const vector_map::Point& p : vmap.points) {
+        if (p.pid != n.pid)
+          continue;
+        lane_vmap.points.push_back(p);
+      }
+    }
+
+    for (const vector_map::StopLine& s : vmap.stoplines) {
+      if (s.linkid != l.lnid)
+        continue;
+      lane_vmap.stoplines.push_back(s);
+    }
+
+    for (const vector_map::DTLane& d : vmap.dtlanes) {
+      if (d.did != l.did)
+        continue;
+      lane_vmap.dtlanes.push_back(d);
+    }
+  }
+
+  return lane_vmap;
+}
+
+VectorMap create_coarse_vmap_from_lane(const autoware_msgs::Lane& lane)
+{
+  VectorMap coarse_vmap;
+  for (const autoware_msgs::Waypoint& w : lane.waypoints)
+    coarse_vmap.points.push_back(create_vector_map_point(w.pose.pose.position));
+
+  return coarse_vmap;
+}
+
+VectorMap create_coarse_vmap_from_route(const tablet_socket_msgs::route_cmd& route)
+{
+  geo_pos_conv geo;
+  geo.set_plane(7);
+
+  VectorMap coarse_vmap;
+  for (const tablet_socket_msgs::Waypoint& w : route.point) {
+    geo.llh_to_xyz(w.lat, w.lon, 0);
+
+    vector_map::Point p;
+    p.bx = geo.x();
+    p.ly = geo.y();
+    coarse_vmap.points.push_back(p);
+  }
+
+  return coarse_vmap;
+}
+
+VectorMap create_fine_vmap(const VectorMap& lane_vmap, int lno, const VectorMap& coarse_vmap, double search_radius,
+         int waypoint_max)
+{
+  VectorMap fine_vmap;
+  VectorMap null_vmap;
+
+  vector_map::Point departure_point;
+  departure_point.pid = -1;
+  if (lno == LNO_ALL)
+    departure_point = find_nearest_point(lane_vmap, coarse_vmap.points.front());
+  else {
+    for (int i = lno; i >= LNO_CROSSING; --i) {
+      departure_point = find_departure_point(lane_vmap, i, coarse_vmap.points, search_radius);
+      if (departure_point.pid >= 0)
+        break;
+    }
+  }
+  if (departure_point.pid < 0)
+    return null_vmap;
+
+  vector_map::Point arrival_point;
+  arrival_point.pid = -1;
+  if (lno == LNO_ALL)
+    arrival_point = find_nearest_point(lane_vmap, coarse_vmap.points.back());
+  else {
+    for (int i = lno; i >= LNO_CROSSING; --i) {
+      arrival_point = find_arrival_point(lane_vmap, i, coarse_vmap.points, search_radius);
+      if (arrival_point.pid >= 0)
+        break;
+    }
+  }
+  if (arrival_point.pid < 0)
+    return null_vmap;
+
+  vector_map::Point point = departure_point;
+  vector_map::Lane lane = find_lane(lane_vmap, LNO_ALL, point);
+  if (lane.lnid < 0)
+    return null_vmap;
+
+  bool finish = false;
+  for (int i = 0; i < waypoint_max; ++i) {
+    fine_vmap.points.push_back(point);
+
+    // last is equal to previous dtlane
+    vector_map::DTLane dtlane;
+    dtlane.did = -1;
+    for (const vector_map::DTLane& d : lane_vmap.dtlanes) {
+      if (d.did == lane.did) {
+        dtlane = d;
+        break;
+      }
+    }
+    fine_vmap.dtlanes.push_back(dtlane);
+
+    // last is equal to previous stopline
+    vector_map::StopLine stopline;
+    stopline.id = -1;
+    for (const vector_map::StopLine& s : lane_vmap.stoplines) {
+      if (s.linkid == lane.lnid) {
+        stopline = s;
+        break;
+      }
+    }
+    fine_vmap.stoplines.push_back(stopline);
+
+    if (finish)
+      break;
+
+    fine_vmap.lanes.push_back(lane);
+
+    point = find_end_point(lane_vmap, lane);
+    if (point.pid < 0)
+      return null_vmap;
+    if (point.bx == arrival_point.bx && point.ly == arrival_point.ly) {
+      finish = true;
+      continue;
+    }
+
+    if (is_branching_lane(lane)) {
+      vector_map::Point coarse_p1 = find_end_point(lane_vmap, lane);
+      if (coarse_p1.pid < 0)
+        return null_vmap;
+
+      coarse_p1 = find_nearest_point(coarse_vmap, coarse_p1);
+      if (coarse_p1.pid < 0)
+        return null_vmap;
+
+      vector_map::Point coarse_p2;
+      double distance = -1;
+      for (const vector_map::Point& p : coarse_vmap.points) {
+        if (distance == -1) {
+          if (p.bx == coarse_p1.bx && p.ly == coarse_p1.ly)
+            distance = 0;
+          continue;
+        }
+        coarse_p2 = p;
+        distance = hypot(coarse_p2.bx - coarse_p1.bx, coarse_p2.ly - coarse_p1.ly);
+        if (distance > search_radius)
+          break;
+      }
+      if (distance <= 0)
+        return null_vmap;
+
+      double coarse_angle = compute_direction_angle(coarse_p1, coarse_p2);
+      if (lno == LNO_ALL) {
+        lane = find_next_branching_lane(lane_vmap, LNO_ALL, lane, coarse_angle, search_radius);
+      } else {
+        vector_map::Lane l;
+        l.lnid = -1;
+        for (int j = lno; j >= LNO_CROSSING; --j) {
+          l = find_next_branching_lane(lane_vmap, j, lane, coarse_angle, search_radius);
+          if (l.lnid >= 0)
+            break;
+        }
+        lane = l;
+      }
+    } else {
+      lane = find_next_lane(lane_vmap, LNO_ALL, lane);
+    }
+    if (lane.lnid < 0)
+      return null_vmap;
+  }
+
+  if (!finish) {
+    ROS_ERROR_STREAM("lane is too long");
+    return null_vmap;
+  }
+
+  return fine_vmap;
+}
+
+std::vector<vector_map::Point> create_branching_points(const VectorMap& vmap)
+{
+  std::vector<vector_map::Point> branching_points;
+  for (const vector_map::Point& p : vmap.points) {
+    if (!is_branching_point(vmap, p))
+      continue;
+    branching_points.push_back(p);
+  }
+
+  return branching_points;
+}
+
+std::vector<vector_map::Point> create_merging_points(const VectorMap& vmap)
+{
+  std::vector<vector_map::Point> merging_points;
+  for (const vector_map::Point& p : vmap.points) {
+    if (!is_merging_point(vmap, p))
+      continue;
+    merging_points.push_back(p);
+  }
+
+  return merging_points;
+}
+
+void publish_add_marker(const ros::Publisher& pub, const visualization_msgs::Marker& marker,
+      const std::vector<vector_map::Point>& points)
+{
+  visualization_msgs::Marker m;
+  m.header.frame_id = marker.header.frame_id;
+  m.ns = marker.ns;
+  m.id = marker.id;
+  m.type = marker.type;
+  m.scale = marker.scale;
+  m.color = marker.color;
+  m.frame_locked = marker.frame_locked;
+  for (const vector_map::Point& p : points)
+    m.points.push_back(create_geometry_msgs_point(p));
+
+  m.header.stamp = ros::Time::now();
+  m.action = visualization_msgs::Marker::ADD;
+
+  pub.publish(m);
+}
+
+void publish_delete_marker(const ros::Publisher& pub, const visualization_msgs::Marker& marker)
+{
+  visualization_msgs::Marker m;
+  m.header.frame_id = marker.header.frame_id;
+  m.ns = marker.ns;
+  m.id = marker.id;
+
+  m.header.stamp = ros::Time::now();
+  m.action = visualization_msgs::Marker::DELETE;
+
+  pub.publish(m);
+}
+
+} // namespace vmap
+
+} // namespace lane_planner

src/ros/catkin/src/lanelet2_extension/lib/autoware_osm_parser.cpp

@@ -0,0 +1,91 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Authors: Ryohsuke Mitsudome
+ */
+
+#include <lanelet2_extension/io/autoware_osm_parser.h>
+
+#include <lanelet2_core/geometry/LineString.h>
+#include <lanelet2_io/io_handlers/Factory.h>
+#include <lanelet2_io/io_handlers/OsmFile.h>
+#include <lanelet2_io/io_handlers/OsmHandler.h>
+
+#include <string>
+
+namespace lanelet
+{
+namespace io_handlers
+{
+std::unique_ptr<LaneletMap> AutowareOsmParser::parse(const std::string& filename, ErrorMessages& errors) const
+{
+  auto map = OsmParser::parse(filename, errors);
+
+  // overwrite x and y values if there are local_x, local_y tags
+  for (Point3d point : map->pointLayer)
+  {
+    if (point.hasAttribute("local_x"))
+    {
+      point.x() = point.attribute("local_x").asDouble().value();
+    }
+    if (point.hasAttribute("local_y"))
+    {
+      point.y() = point.attribute("local_y").asDouble().value();
+    }
+  }
+
+  // rerun align function in just in case
+  for (Lanelet& lanelet : map->laneletLayer)
+  {
+    LineString3d new_left, new_right;
+    std::tie(new_left, new_right) = geometry::align(lanelet.leftBound(), lanelet.rightBound());
+    lanelet.setLeftBound(new_left);
+    lanelet.setRightBound(new_right);
+  }
+
+  return map;
+}
+
+namespace
+{
+RegisterParser<AutowareOsmParser> regParser;
+}
+
+void AutowareOsmParser::parseVersions(const std::string& filename, std::string* format_version,
+                                      std::string* map_version)
+{
+  if (format_version == nullptr || map_version == nullptr)
+  {
+    std::cerr << __FUNCTION__ << ": either format_version or map_version is null pointer!";
+    return;
+  }
+
+  pugi::xml_document doc;
+  auto result = doc.load_file(filename.c_str());
+  if (!result)
+  {
+    throw lanelet::ParseError(std::string("Errors occured while parsing osm file: ") + result.description());
+  }
+
+  auto osmNode = doc.child("osm");
+  auto metainfo = osmNode.child("MetaInfo");
+  if (metainfo.attribute("format_version"))
+    *format_version = metainfo.attribute("format_version").value();
+  if (metainfo.attribute("map_version"))
+    *map_version = metainfo.attribute("map_version").value();
+}
+
+}  // namespace io_handlers
+}  // namespace lanelet

src/ros/catkin/src/lanelet2_extension/lib/autoware_traffic_light.cpp

@@ -0,0 +1,155 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Authors: Ryohsuke Mitsudome
+ */
+
+#include <boost/variant.hpp>
+
+#include <lanelet2_core/primitives/RegulatoryElement.h>
+#include <lanelet2_extension/regulatory_elements/autoware_traffic_light.h>
+
+#include <algorithm>
+#include <vector>
+
+namespace lanelet
+{
+namespace autoware
+{
+namespace
+{
+template <typename T>
+bool findAndErase(const T& primitive, RuleParameters* member)
+{
+  if (member == nullptr)
+  {
+    std::cerr << __FUNCTION__ << ": member is null pointer";
+    return false;
+  }
+  auto it = std::find(member->begin(), member->end(), RuleParameter(primitive));
+  if (it == member->end())
+  {
+    return false;
+  }
+  member->erase(it);
+  return true;
+}
+
+template <typename T>
+RuleParameters toRuleParameters(const std::vector<T>& primitives)
+{
+  auto cast_func = [](const auto& elem) { return static_cast<RuleParameter>(elem); };
+  return utils::transform(primitives, cast_func);
+}
+
+template <>
+RuleParameters toRuleParameters(const std::vector<LineStringOrPolygon3d>& primitives)
+{
+  auto cast_func = [](const auto& elem) { return elem.asRuleParameter(); };
+  return utils::transform(primitives, cast_func);
+}
+
+LineStringsOrPolygons3d getLsOrPoly(const RuleParameterMap& paramsMap, RoleName role)
+{
+  auto params = paramsMap.find(role);
+  if (params == paramsMap.end())
+  {
+    return {};
+  }
+  LineStringsOrPolygons3d result;
+  for (auto& param : params->second)
+  {
+    auto l = boost::get<LineString3d>(&param);
+    if (l != nullptr)
+    {
+      result.push_back(*l);
+    }
+    auto p = boost::get<Polygon3d>(&param);
+    if (p != nullptr)
+    {
+      result.push_back(*p);
+    }
+  }
+  return result;
+}
+
+ConstLineStringsOrPolygons3d getConstLsOrPoly(const RuleParameterMap& params, RoleName role)
+{
+  auto cast_func = [](auto& lsOrPoly) { return static_cast<ConstLineStringOrPolygon3d>(lsOrPoly); };
+  return utils::transform(getLsOrPoly(params, role), cast_func);
+}
+
+RegulatoryElementDataPtr constructAutowareTrafficLightData(Id id, const AttributeMap& attributes,
+                                                           const LineStringsOrPolygons3d& trafficLights,
+                                                           const Optional<LineString3d>& stopLine,
+                                                           const LineStrings3d& lightBulbs)
+{
+  RuleParameterMap rpm = { { RoleNameString::Refers, toRuleParameters(trafficLights) } };
+
+  if (!!stopLine)
+  {
+    RuleParameters rule_parameters = { *stopLine };
+    rpm.insert(std::make_pair(RoleNameString::RefLine, rule_parameters));
+  }
+  if (!lightBulbs.empty())
+  {
+    rpm.insert(std::make_pair(AutowareRoleNameString::LightBulbs, toRuleParameters(lightBulbs)));
+  }
+
+  auto data = std::make_shared<RegulatoryElementData>(id, rpm, attributes);
+  data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement;
+  data->attributes[AttributeName::Subtype] = AttributeValueString::TrafficLight;
+  return data;
+}
+}  // namespace
+
+constexpr const char AutowareRoleNameString::LightBulbs[];
+
+AutowareTrafficLight::AutowareTrafficLight(const RegulatoryElementDataPtr& data) : TrafficLight(data)
+{
+}
+
+AutowareTrafficLight::AutowareTrafficLight(Id id, const AttributeMap& attributes,
+                                           const LineStringsOrPolygons3d& trafficLights,
+                                           const Optional<LineString3d>& stopLine, const LineStrings3d& lightBulbs)
+  : TrafficLight(id, attributes, trafficLights, stopLine)
+{
+  for (const auto& lightBulb : lightBulbs)
+  {
+    addLightBulbs(lightBulb);
+  }
+}
+
+ConstLineStrings3d AutowareTrafficLight::lightBulbs() const
+{
+  return getParameters<ConstLineString3d>(AutowareRoleNameString::LightBulbs);
+}
+
+void AutowareTrafficLight::addLightBulbs(const LineStringOrPolygon3d& primitive)
+{
+  parameters()[AutowareRoleNameString::LightBulbs].emplace_back(primitive.asRuleParameter());
+}
+
+bool AutowareTrafficLight::removeLightBulbs(const LineStringOrPolygon3d& primitive)
+{
+  return findAndErase(primitive.asRuleParameter(), &parameters().find(AutowareRoleNameString::LightBulbs)->second);
+}
+
+#if __cplusplus < 201703L
+constexpr char AutowareTrafficLight::RuleName[];  // instanciate string in cpp file
+#endif
+
+}  // namespace autoware
+}  // namespace lanelet

src/ros/catkin/src/lanelet2_extension/lib/message_conversion.cpp

@@ -0,0 +1,168 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Authors: Simon Thompson, Ryohsuke Mitsudome
+ */
+
+#include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_io/Exceptions.h>
+#include <lanelet2_io/Projection.h>
+#include <lanelet2_io/io_handlers/OsmFile.h>
+#include <lanelet2_io/io_handlers/OsmHandler.h>
+#include <lanelet2_io/io_handlers/Serialize.h>
+#include <lanelet2_projection/UTM.h>
+#include <ros/ros.h>
+
+#include <lanelet2_extension/projection/mgrs_projector.h>
+#include <lanelet2_extension/utility/message_conversion.h>
+
+#include <boost/archive/binary_iarchive.hpp>
+#include <boost/archive/binary_oarchive.hpp>
+
+#include <sstream>
+#include <string>
+
+namespace lanelet
+{
+namespace utils
+{
+namespace conversion
+{
+void toBinMsg(const lanelet::LaneletMapPtr& map, autoware_lanelet2_msgs::MapBin* msg)
+{
+  if (msg == nullptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << "msg is null pointer!");
+    return;
+  }
+
+  std::stringstream ss;
+  boost::archive::binary_oarchive oa(ss);
+  oa << *map;
+  auto id_counter = lanelet::utils::getId();
+  oa << id_counter;
+
+  std::string data_str(ss.str());
+
+  msg->data.clear();
+  msg->data.assign(data_str.begin(), data_str.end());
+}
+
+void fromBinMsg(const autoware_lanelet2_msgs::MapBin& msg, lanelet::LaneletMapPtr map)
+{
+  if (!map)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << ": map is null pointer!");
+    return;
+  }
+
+  std::string data_str;
+  data_str.assign(msg.data.begin(), msg.data.end());
+
+  std::stringstream ss;
+  ss << data_str;
+  boost::archive::binary_iarchive oa(ss);
+  oa >> *map;
+  lanelet::Id id_counter;
+  oa >> id_counter;
+  lanelet::utils::registerId(id_counter);
+  // *map = std::move(laneletMap);
+}
+
+void toGeomMsgPt(const geometry_msgs::Point32& src, geometry_msgs::Point* dst)
+{
+  if (dst == nullptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << "pointer is null!");
+    return;
+  }
+  dst->x = src.x;
+  dst->y = src.y;
+  dst->z = src.z;
+}
+void toGeomMsgPt(const Eigen::Vector3d& src, geometry_msgs::Point* dst)
+{
+  if (dst == nullptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << "pointer is null!");
+    return;
+  }
+  dst->x = src.x();
+  dst->y = src.y();
+  dst->z = src.z();
+}
+void toGeomMsgPt(const lanelet::ConstPoint3d& src, geometry_msgs::Point* dst)
+{
+  if (dst == nullptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << "pointer is null!");
+    return;
+  }
+  dst->x = src.x();
+  dst->y = src.y();
+  dst->z = src.z();
+}
+void toGeomMsgPt(const lanelet::ConstPoint2d& src, geometry_msgs::Point* dst)
+{
+  if (dst == nullptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << "pointer is null!");
+    return;
+  }
+  dst->x = src.x();
+  dst->y = src.y();
+  dst->z = 0;
+}
+
+void toGeomMsgPt32(const Eigen::Vector3d& src, geometry_msgs::Point32* dst)
+{
+  if (dst == nullptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << "pointer is null!");
+    return;
+  }
+  dst->x = src.x();
+  dst->y = src.y();
+  dst->z = src.z();
+}
+
+geometry_msgs::Point toGeomMsgPt(const geometry_msgs::Point32& src)
+{
+  geometry_msgs::Point dst;
+  toGeomMsgPt(src, &dst);
+  return dst;
+}
+geometry_msgs::Point toGeomMsgPt(const Eigen::Vector3d& src)
+{
+  geometry_msgs::Point dst;
+  toGeomMsgPt(src, &dst);
+  return dst;
+}
+geometry_msgs::Point toGeomMsgPt(const lanelet::ConstPoint3d& src)
+{
+  geometry_msgs::Point dst;
+  toGeomMsgPt(src, &dst);
+  return dst;
+}
+geometry_msgs::Point toGeomMsgPt(const lanelet::ConstPoint2d& src)
+{
+  geometry_msgs::Point dst;
+  toGeomMsgPt(src, &dst);
+  return dst;
+}
+
+}  // namespace conversion
+}  // namespace utils
+}  // namespace lanelet

src/ros/catkin/src/lanelet2_extension/lib/mgrs_projector.cpp

@@ -0,0 +1,149 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Authors: Simon Thompson, Ryohsuke Mitsudome
+ */
+
+#include <lanelet2_extension/projection/mgrs_projector.h>
+#include <ros/ros.h>
+
+#include <vector>
+#include <set>
+#include <string>
+
+namespace lanelet
+{
+namespace projection
+{
+MGRSProjector::MGRSProjector(Origin origin) : Projector(origin)
+{
+}
+
+BasicPoint3d MGRSProjector::forward(const GPSPoint& gps) const
+{
+  BasicPoint3d mgrs_point(forward(gps, 0));
+  return mgrs_point;
+}
+
+BasicPoint3d MGRSProjector::forward(const GPSPoint& gps, const int precision) const
+{
+  std::string prev_projected_grid = projected_grid_;
+
+  BasicPoint3d mgrs_point{ 0., 0., gps.ele };
+  BasicPoint3d utm_point{ 0., 0., gps.ele };
+  int zone;
+  bool northp;
+  std::string mgrs_code;
+
+  try
+  {
+    GeographicLib::UTMUPS::Forward(gps.lat, gps.lon, zone, northp, utm_point.x(), utm_point.y());
+    GeographicLib::MGRS::Forward(zone, northp, utm_point.x(), utm_point.y(), gps.lat, precision, mgrs_code);
+  }
+  catch (GeographicLib::GeographicErr err)
+  {
+    ROS_ERROR_STREAM(err.what());
+    return mgrs_point;
+  }
+
+  // get mgrs values from utm values
+  mgrs_point.x() = fmod(utm_point.x(), 1e5);
+  mgrs_point.y() = fmod(utm_point.y(), 1e5);
+  projected_grid_ = mgrs_code;
+
+  if (!prev_projected_grid.empty() && prev_projected_grid != projected_grid_)
+  {
+    ROS_ERROR_STREAM("Projected MGRS Grid changed from last projection. Projected point "
+                     "might be far away from previously projected point."
+                     << std::endl
+                     << "You may want to use different projector.");
+  }
+
+  return mgrs_point;
+}
+
+GPSPoint MGRSProjector::reverse(const BasicPoint3d& mgrs_point) const
+{
+  GPSPoint gps{ 0., 0., 0. };
+  // reverse function cannot be used if mgrs_code_ is not set
+  if (isMGRSCodeSet())
+  {
+    gps = reverse(mgrs_point, mgrs_code_);
+  }
+  else if (!projected_grid_.empty())
+  {
+    gps = reverse(mgrs_point, projected_grid_);
+  }
+  else
+  {
+    ROS_ERROR_STREAM("cannot run reverse operation if mgrs code is not set in projector." << std::endl
+                                                                                          << "use setMGRSCode function "
+                                                                                             "or explicitly give mgrs "
+                                                                                             "code as an "
+                                                                                             "argument.");
+  }
+  return gps;
+}
+
+GPSPoint MGRSProjector::reverse(const BasicPoint3d& mgrs_point, const std::string& mgrs_code) const
+{
+  GPSPoint gps{ 0., 0., mgrs_point.z() };
+  BasicPoint3d utm_point{ 0., 0., gps.ele };
+
+  int zone, prec;
+  bool northp;
+  try
+  {
+    GeographicLib::MGRS::Reverse(mgrs_code, zone, northp, utm_point.x(), utm_point.y(), prec, false);
+    utm_point.x() += fmod(mgrs_point.x(), pow(10, 5 - prec));
+    utm_point.y() += fmod(mgrs_point.y(), pow(10, 5 - prec));
+    GeographicLib::UTMUPS::Reverse(zone, northp, utm_point.x(), utm_point.y(), gps.lat, gps.lon);
+  }
+  catch (GeographicLib::GeographicErr err)
+  {
+    ROS_ERROR_STREAM("Failed to convert from MGRS to WGS" << err.what());
+    return gps;
+  }
+
+  return gps;
+}
+
+void MGRSProjector::setMGRSCode(const std::string& mgrs_code)
+{
+  mgrs_code_ = mgrs_code;
+}
+
+void MGRSProjector::setMGRSCode(const GPSPoint& gps, const int precision)
+{
+  BasicPoint3d utm_point{ 0., 0., gps.ele };
+  int zone;
+  bool northp;
+  std::string mgrs_code;
+
+  try
+  {
+    GeographicLib::UTMUPS::Forward(gps.lat, gps.lon, zone, northp, utm_point.x(), utm_point.y());
+    GeographicLib::MGRS::Forward(zone, northp, utm_point.x(), utm_point.y(), gps.lat, precision, mgrs_code);
+  }
+  catch (GeographicLib::GeographicErr err)
+  {
+    ROS_ERROR_STREAM(err.what());
+  }
+
+  setMGRSCode(mgrs_code);
+}
+
+}  // namespace projection
+}  // namespace lanelet

src/ros/catkin/src/lanelet2_extension/lib/query.cpp

@@ -0,0 +1,298 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Authors: Simon Thompson, Ryohsuke Mitsudome
+ */
+
+#include <ros/ros.h>
+
+#include <Eigen/Eigen>
+
+#include <lanelet2_extension/utility/message_conversion.h>
+#include <lanelet2_extension/utility/query.h>
+
+#include <set>
+#include <string>
+#include <vector>
+
+namespace lanelet
+{
+namespace utils
+{
+// returns all lanelets in laneletLayer - don't know how to convert
+// PrimitveLayer<Lanelets> -> std::vector<Lanelets>
+lanelet::ConstLanelets query::laneletLayer(const lanelet::LaneletMapPtr ll_map)
+{
+  lanelet::ConstLanelets lanelets;
+  if (!ll_map)
+  {
+    ROS_WARN("No map received!");
+    return lanelets;
+  }
+
+  for (auto li = ll_map->laneletLayer.begin(); li != ll_map->laneletLayer.end(); li++)
+  {
+    lanelets.push_back(*li);
+  }
+
+  return lanelets;
+}
+
+lanelet::ConstLanelets query::subtypeLanelets(const lanelet::ConstLanelets lls, const char subtype[])
+{
+  lanelet::ConstLanelets subtype_lanelets;
+
+  for (auto li = lls.begin(); li != lls.end(); li++)
+  {
+    lanelet::ConstLanelet ll = *li;
+
+    if (ll.hasAttribute(lanelet::AttributeName::Subtype))
+    {
+      lanelet::Attribute attr = ll.attribute(lanelet::AttributeName::Subtype);
+      if (attr.value() == subtype)
+      {
+        subtype_lanelets.push_back(ll);
+      }
+    }
+  }
+
+  return subtype_lanelets;
+}
+
+lanelet::ConstLanelets query::crosswalkLanelets(const lanelet::ConstLanelets lls)
+{
+  return (query::subtypeLanelets(lls, lanelet::AttributeValueString::Crosswalk));
+}
+
+lanelet::ConstLanelets query::roadLanelets(const lanelet::ConstLanelets lls)
+{
+  return (query::subtypeLanelets(lls, lanelet::AttributeValueString::Road));
+}
+
+std::vector<lanelet::TrafficLightConstPtr> query::trafficLights(const lanelet::ConstLanelets lanelets)
+{
+  std::vector<lanelet::TrafficLightConstPtr> tl_reg_elems;
+
+  for (auto i = lanelets.begin(); i != lanelets.end(); i++)
+  {
+    lanelet::ConstLanelet ll = *i;
+    std::vector<lanelet::TrafficLightConstPtr> ll_tl_re = ll.regulatoryElementsAs<lanelet::TrafficLight>();
+
+    // insert unique tl into array
+    for (auto tli = ll_tl_re.begin(); tli != ll_tl_re.end(); tli++)
+    {
+      lanelet::TrafficLightConstPtr tl_ptr = *tli;
+      lanelet::Id id = tl_ptr->id();
+      bool unique_id = true;
+      for (auto ii = tl_reg_elems.begin(); ii != tl_reg_elems.end(); ii++)
+      {
+        if (id == (*ii)->id())
+        {
+          unique_id = false;
+          break;
+        }
+      }
+      if (unique_id)
+      {
+        tl_reg_elems.push_back(tl_ptr);
+      }
+    }
+  }
+  return tl_reg_elems;
+}
+
+std::vector<lanelet::AutowareTrafficLightConstPtr> query::autowareTrafficLights(const lanelet::ConstLanelets lanelets)
+{
+  std::vector<lanelet::AutowareTrafficLightConstPtr> tl_reg_elems;
+
+  for (auto i = lanelets.begin(); i != lanelets.end(); i++)
+  {
+    lanelet::ConstLanelet ll = *i;
+    std::vector<lanelet::AutowareTrafficLightConstPtr> ll_tl_re =
+        ll.regulatoryElementsAs<lanelet::autoware::AutowareTrafficLight>();
+
+    // insert unique tl into array
+    for (auto tli = ll_tl_re.begin(); tli != ll_tl_re.end(); tli++)
+    {
+      lanelet::AutowareTrafficLightConstPtr tl_ptr = *tli;
+      lanelet::Id id = tl_ptr->id();
+      bool unique_id = true;
+
+      for (auto ii = tl_reg_elems.begin(); ii != tl_reg_elems.end(); ii++)
+      {
+        if (id == (*ii)->id())
+        {
+          unique_id = false;
+          break;
+        }
+      }
+
+      if (unique_id)
+        tl_reg_elems.push_back(tl_ptr);
+    }
+  }
+  return tl_reg_elems;
+}
+
+// return all stop lines and ref lines from a given set of lanelets
+std::vector<lanelet::ConstLineString3d> query::getTrafficLightStopLines(const lanelet::ConstLanelets lanelets)
+{
+  std::vector<lanelet::ConstLineString3d> stoplines;
+
+  for (auto lli = lanelets.begin(); lli != lanelets.end(); lli++)
+  {
+    std::vector<lanelet::ConstLineString3d> ll_stoplines;
+    ll_stoplines = query::getTrafficLightStopLines(*lli);
+    stoplines.insert(stoplines.end(), ll_stoplines.begin(), ll_stoplines.end());
+  }
+
+  return stoplines;
+}
+
+// return all stop and ref lines from a given lanelet
+std::vector<lanelet::ConstLineString3d> query::getTrafficLightStopLines(const lanelet::ConstLanelet ll)
+{
+  std::vector<lanelet::ConstLineString3d> stoplines;
+
+  // find stop lines referenced by traffic lights
+  std::vector<std::shared_ptr<const lanelet::TrafficLight> > traffic_light_reg_elems =
+      ll.regulatoryElementsAs<const lanelet::TrafficLight>();
+
+  // lanelet has a traffic light elem element
+  for (const auto reg_elem : traffic_light_reg_elems)
+  {
+    lanelet::Optional<lanelet::ConstLineString3d> traffic_light_stopline_opt = reg_elem->stopLine();
+    if (!!traffic_light_stopline_opt)
+    {
+      stoplines.push_back(traffic_light_stopline_opt.get());
+    }
+  }
+
+  return stoplines;
+}
+
+std::vector<lanelet::ConstLineString3d> query::getStopSignStopLines(const lanelet::ConstLanelets lanelets,
+                                                                    const std::string& stop_sign_id)
+{
+  std::vector<lanelet::ConstLineString3d> all_stoplines;
+  std::vector<lanelet::ConstLineString3d> traffic_sign_stoplines;
+  std::vector<lanelet::ConstLineString3d> right_of_way_stoplines;
+  std::vector<lanelet::ConstLineString3d> all_way_stop_stoplines;
+
+  traffic_sign_stoplines = getTrafficSignStopLines(lanelets, stop_sign_id);
+  right_of_way_stoplines = getRightOfWayStopLines(lanelets);
+  all_way_stop_stoplines = getAllWayStopStopLines(lanelets);
+
+  all_stoplines.reserve(traffic_sign_stoplines.size() + right_of_way_stoplines.size() + all_way_stop_stoplines.size());
+  all_stoplines.insert(all_stoplines.end(), traffic_sign_stoplines.begin(), traffic_sign_stoplines.end());
+  all_stoplines.insert(all_stoplines.end(), right_of_way_stoplines.begin(), right_of_way_stoplines.end());
+  all_stoplines.insert(all_stoplines.end(), all_way_stop_stoplines.begin(), all_way_stop_stoplines.end());
+
+  return all_stoplines;
+}
+
+std::vector<lanelet::ConstLineString3d> query::getTrafficSignStopLines(const lanelet::ConstLanelets lanelets,
+                                                                       const std::string& stop_sign_id)
+{
+  std::vector<lanelet::ConstLineString3d> stoplines;
+  std::set<lanelet::Id> checklist;
+
+  for (const auto& ll : lanelets)
+  {
+    // find stop lines referenced by traffic signs
+    std::vector<std::shared_ptr<const lanelet::TrafficSign> > traffic_sign_reg_elems =
+        ll.regulatoryElementsAs<const lanelet::TrafficSign>();
+
+    if (traffic_sign_reg_elems.size() > 0)
+    {
+      // lanelet has a traffic sign reg elem - can have multiple ref lines (but
+      // stop sign should have 1
+      for (const auto& ts : traffic_sign_reg_elems)
+      {
+        // skip if traffic sign is not stop sign
+        if (ts->type() != stop_sign_id)
+        {
+          continue;
+        }
+
+        lanelet::ConstLineStrings3d traffic_sign_stoplines = ts->refLines();
+
+        // only add new items
+        if (traffic_sign_stoplines.size() > 0)
+        {
+          auto id = traffic_sign_stoplines.front().id();
+          if (checklist.find(id) == checklist.end())
+          {
+            checklist.insert(id);
+            stoplines.push_back(traffic_sign_stoplines.front());
+          }
+        }
+      }
+    }
+  }
+  return stoplines;
+}
+
+std::vector<lanelet::ConstLineString3d> query::getRightOfWayStopLines(const lanelet::ConstLanelets lanelets)
+{
+  std::vector<lanelet::ConstLineString3d> stoplines;
+
+  for (const auto& ll : lanelets)
+  {
+    // find stop lines referenced by RightOfWay reg. elems.
+    std::vector<std::shared_ptr<const lanelet::RightOfWay> > right_of_way_reg_elems =
+        ll.regulatoryElementsAs<const lanelet::RightOfWay>();
+
+    for (const auto reg_elem : right_of_way_reg_elems)
+    {
+      if (reg_elem->getManeuver(ll) == lanelet::ManeuverType::Yield)
+      {
+        // lanelet has a yield reg. elem.
+        lanelet::Optional<lanelet::ConstLineString3d> row_stopline_opt = reg_elem->stopLine();
+        if (!!row_stopline_opt)
+        {
+          stoplines.push_back(row_stopline_opt.get());
+        }
+      }
+    }
+  }
+  return stoplines;
+}
+
+std::vector<lanelet::ConstLineString3d> query::getAllWayStopStopLines(const lanelet::ConstLanelets lanelets)
+{
+  std::vector<lanelet::ConstLineString3d> stoplines;
+
+  for (const auto& ll : lanelets)
+  {
+    // Get every AllWayStop reg. elem. that this lanelet references.
+    std::vector<std::shared_ptr<const lanelet::AllWayStop>> all_way_stop_reg_elems =
+        ll.regulatoryElementsAs<const lanelet::AllWayStop>();
+
+    for (const auto reg_elem : all_way_stop_reg_elems)
+    {
+      // Only get the stopline for this lanelet
+      lanelet::Optional<lanelet::ConstLineString3d> stopline = reg_elem->getStopLine(ll);
+      if (!!stopline)
+      {
+        stoplines.push_back(stopline.get());
+      }
+    }
+  }
+  return stoplines;
+}
+
+}  // namespace utils
+}  // namespace lanelet

src/ros/catkin/src/lanelet2_extension/lib/utilities.cpp

@@ -0,0 +1,397 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Authors: Kenji Miyake, Ryohsuke Mitsudome
+ *
+ */
+
+#include <lanelet2_core/geometry/LineString.h>
+#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
+#include <lanelet2_traffic_rules/TrafficRules.h>
+#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
+
+#include <lanelet2_extension/utility/utilities.h>
+#include <ros/ros.h>
+
+#include <algorithm>
+#include <map>
+#include <utility>
+#include <vector>
+
+namespace lanelet
+{
+namespace utils
+{
+namespace
+{
+bool exists(const std::vector<int>& array, const int element)
+{
+  return std::find(array.begin(), array.end(), element) != array.end();
+}
+
+/**
+ * [removeImpossibleCandidates eliminates the impossible lanelet id candidates
+ * according to lanelet routing graph information ]
+ * @method removeImpossibleCandidates
+ * @param  waypoints [list of waypoints]
+ * @param  wp_candidate_lanelets  list of lanelet id candidates for each
+ * waypoint
+ */
+void removeImpossibleCandidates(const lanelet::LaneletMapPtr lanelet_map,
+                                const lanelet::routing::RoutingGraphPtr routing_graph,
+                                const std::vector<autoware_msgs::Waypoint>& waypoints,
+                                std::map<int, std::vector<int> >* wp_candidate_lanelets, const bool reverse)
+{
+  if (!lanelet_map)
+  {
+    ROS_ERROR_STREAM("No lanelet map is set!");
+    return;
+  }
+
+  int prev_wp_gid;
+  bool first_wp = true;
+  for (const auto& wp : waypoints)
+  {
+    if (first_wp)
+    {
+      prev_wp_gid = wp.gid;
+      first_wp = false;
+      continue;
+    }
+
+    auto candidate_ids_ptr = &wp_candidate_lanelets->at(wp.gid);
+    const auto prev_wp_candidate_ids_ptr = &wp_candidate_lanelets->at(prev_wp_gid);
+
+    // do not eliminate if previous waypoint do not have any candidate
+    if (prev_wp_candidate_ids_ptr->empty())
+      continue;
+
+    std::vector<int> removing_ids;
+    for (const auto candidate_id : *candidate_ids_ptr)
+    {
+      // do not eliminate if the belonging lane exists in candidates of previous
+      // waypoint
+      if (exists(*prev_wp_candidate_ids_ptr, candidate_id))
+        continue;
+
+      auto lanelet = lanelet_map->laneletLayer.get(candidate_id);
+      // get available previous lanelet from routing graph
+      lanelet::ConstLanelets previous_lanelets;
+      if (reverse)
+      {
+        previous_lanelets = routing_graph->following(lanelet);
+      }
+      else
+      {
+        previous_lanelets = routing_graph->previous(lanelet);
+      }
+
+      // connection is impossible if none of predecessor lanelets match with
+      // lanelet candidates from previous waypoint
+      bool connection_possible = false;
+      for (const auto& previous_lanelet : previous_lanelets)
+      {
+        if (exists(*prev_wp_candidate_ids_ptr, previous_lanelet.id()))
+        {
+          connection_possible = true;
+          break;
+        }
+      }
+      if (!connection_possible)
+      {
+        removing_ids.push_back(candidate_id);
+      }
+    }
+
+    // declare function for remove_if separately, because roslint is not supporting lambda functions very well.
+    auto remove_existing_func = [removing_ids](int id) { return exists(removing_ids, id); };
+
+    auto result = std::remove_if(candidate_ids_ptr->begin(), candidate_ids_ptr->end(), remove_existing_func);
+
+    candidate_ids_ptr->erase(result, candidate_ids_ptr->end());
+    prev_wp_gid = wp.gid;
+  }
+}
+
+/**
+ * [getContactingLanelets retrieves id of lanelets which has distance 0m to
+ * search_point]
+ * @param  lanelet_map   [pointer to lanelet]
+ * @param  trafficRules  [traffic rules to ignore lanelets that are not
+ * traversible]
+ * @param  search_point  [2D point used for searching]
+ * @param  search_n      [initial guess used for findNearest function (default
+ * 5)]
+ * @param  contacting_lanelet_ids [array of lanelet ids that is contacting with
+ * search_point]
+ */
+void getContactingLanelets(const lanelet::LaneletMapPtr lanelet_map,
+                           const lanelet::traffic_rules::TrafficRulesPtr traffic_rules,
+                           const lanelet::BasicPoint2d search_point, const int search_n,
+                           std::vector<int>* contacting_lanelet_ids)
+{
+  if (!lanelet_map)
+  {
+    ROS_ERROR_STREAM("No lanelet map is set!");
+    return;
+  }
+
+  if (contacting_lanelet_ids == nullptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << " contacting_lanelet_ids is null pointer!");
+    return;
+  }
+
+  int n = search_n;
+  double max_distance = 0.0;
+  const int increment = 3;
+  const double epsilon = 1e-6;
+  std::vector<std::pair<double, lanelet::Lanelet> > actuallyNearestLanelets;
+
+  // keep searching nearest lanelet as long as all retrieved lanelet has
+  // distance == 0m.
+  while (max_distance < epsilon)
+  {
+    actuallyNearestLanelets = lanelet::geometry::findNearest(lanelet_map->laneletLayer, search_point, n);
+    max_distance = 0.0;
+    for (auto const& item : actuallyNearestLanelets)
+    {
+      if (item.first > max_distance)
+      {
+        max_distance = item.first;
+      }
+    }
+
+    // exit loop if all lanelets in the map intersects with the waypoint,
+    // e.g. when there is only one lanelet in the map.
+    if (actuallyNearestLanelets.size() < n)
+    {
+      break;
+    }
+
+    n += increment;
+  }
+
+  contacting_lanelet_ids->reserve(n - increment);
+  for (auto const& item : actuallyNearestLanelets)
+  {
+    if (item.first < epsilon && traffic_rules->canPass(item.second))
+    {
+      contacting_lanelet_ids->push_back(item.second.id());
+    }
+  }
+}
+
+std::vector<double> calculateSegmentDistances(const lanelet::ConstLineString3d& line_string)
+{
+  std::vector<double> segment_distances;
+  segment_distances.reserve(line_string.size() - 1);
+
+  for (size_t i = 1; i < line_string.size(); ++i)
+  {
+    const auto distance = lanelet::geometry::distance2d(line_string[i], line_string[i - 1]);
+    segment_distances.push_back(distance);
+  }
+
+  return segment_distances;
+}
+
+std::vector<double> calculateAccumulatedLengths(const lanelet::ConstLineString3d& line_string)
+{
+  const auto segment_distances = calculateSegmentDistances(line_string);
+
+  std::vector<double> accumulated_lengths{ 0 };
+  accumulated_lengths.reserve(segment_distances.size() + 1);
+  std::partial_sum(std::begin(segment_distances), std::end(segment_distances), std::back_inserter(accumulated_lengths));
+
+  return accumulated_lengths;
+}
+
+std::pair<size_t, size_t> findNearestIndexPair(const std::vector<double>& accumulated_lengths,
+                                               const double target_length)
+{
+  // List size
+  const auto N = accumulated_lengths.size();
+
+  // Front
+  if (target_length < accumulated_lengths.at(1))
+  {
+    return std::make_pair(0, 1);
+  }
+
+  // Back
+  if (target_length > accumulated_lengths.at(N - 2))
+  {
+    return std::make_pair(N - 2, N - 1);
+  }
+
+  // Middle
+  for (auto i = 1; i < N; ++i)
+  {
+    if (accumulated_lengths.at(i - 1) <= target_length && target_length <= accumulated_lengths.at(i))
+    {
+      return std::make_pair(i - 1, i);
+    }
+  }
+
+  // Throw an exception because this never happens
+  throw std::runtime_error("No nearest point found.");
+}
+
+std::vector<lanelet::BasicPoint3d> resamplePoints(const lanelet::ConstLineString3d& line_string, const int num_segments)
+{
+  // Calculate length
+  const auto line_length = lanelet::geometry::length(line_string);
+
+  // Calculate accumulated lengths
+  const auto accumulated_lengths = calculateAccumulatedLengths(line_string);
+
+  // Create each segment
+  std::vector<lanelet::BasicPoint3d> resampled_points;
+  for (auto i = 0; i <= num_segments; ++i)
+  {
+    // Find two nearest points
+    const auto target_length = (static_cast<double>(i) / num_segments) * line_length;
+    const auto index_pair = findNearestIndexPair(accumulated_lengths, target_length);
+
+    // Apply linear interpolation
+    const lanelet::BasicPoint3d back_point = line_string[index_pair.first];
+    const lanelet::BasicPoint3d front_point = line_string[index_pair.second];
+    const auto direction_vector = (front_point - back_point);
+
+    const auto back_length = accumulated_lengths.at(index_pair.first);
+    const auto front_length = accumulated_lengths.at(index_pair.second);
+    const auto segment_length = front_length - back_length;
+    const auto target_point = back_point + (direction_vector * (target_length - back_length) / segment_length);
+
+    // Add to list
+    resampled_points.push_back(target_point);
+  }
+
+  return resampled_points;
+}
+
+lanelet::LineString3d generateFineCenterline(const lanelet::ConstLanelet& lanelet_obj)
+{
+  // Parameter
+  constexpr double point_interval = 1.0;  // [m]
+
+  // Get length of longer border
+  const double left_length = lanelet::geometry::length(lanelet_obj.leftBound());
+  const double right_length = lanelet::geometry::length(lanelet_obj.rightBound());
+  const double longer_distance = (left_length > right_length) ? left_length : right_length;
+  const int num_segments = std::max(static_cast<int>(ceil(longer_distance / point_interval)), 1);
+
+  // Resample points
+  const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments);
+  const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments);
+
+  // Create centerline
+  lanelet::LineString3d centerline(lanelet::utils::getId());
+  for (int i = 0; i < num_segments + 1; i++)
+  {
+    // Add ID for the average point of left and right
+    const auto center_basic_point = (right_points.at(i) + left_points.at(i)) / 2;
+    const lanelet::Point3d center_point(lanelet::utils::getId(), center_basic_point.x(), center_basic_point.y(),
+                                        center_basic_point.z());
+    centerline.push_back(center_point);
+  }
+  return centerline;
+}
+
+}  // namespace
+
+void matchWaypointAndLanelet(const lanelet::LaneletMapPtr lanelet_map,
+                             const lanelet::routing::RoutingGraphPtr routing_graph,
+                             const autoware_msgs::LaneArray& lane_array,
+                             std::map<int, lanelet::Id>* waypointid2laneletid)
+{
+  if (!lanelet_map)
+  {
+    ROS_ERROR_STREAM("No lanelet map is set!");
+    return;
+  }
+
+  if (waypointid2laneletid == nullptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << ": waypointid2laneletid null pointer!");
+    return;
+  }
+
+  // map of waypoint_id to lanelet_id
+  // first item = gid of waypoint
+  // second item = lanelet_ids
+  std::map<int, std::vector<int> > wp_candidate_lanelet_ids;
+
+  lanelet::traffic_rules::TrafficRulesPtr traffic_rules =
+      lanelet::traffic_rules::TrafficRulesFactory::create(lanelet::Locations::Germany, lanelet::Participants::Vehicle);
+
+  // get possible candidates of lanelets for each waypoint
+  // "candidate lanelets" means lanelets that have 0 distance with waypoint.
+  // multiple candidates could appear at intersections.
+  for (auto& lane : lane_array.lanes)
+  {
+    for (auto& wp : lane.waypoints)
+    {
+      std::vector<int> contacting_lanelet_ids;
+      lanelet::BasicPoint2d search_point(wp.pose.pose.position.x, wp.pose.pose.position.y);
+      getContactingLanelets(lanelet_map, traffic_rules, search_point, 5, &contacting_lanelet_ids);
+      wp_candidate_lanelet_ids[wp.gid] = contacting_lanelet_ids;
+    }
+  }
+
+  // eliminate impossible candidates using routing graph. (forward direction)
+  for (const auto& lane : lane_array.lanes)
+  {
+    removeImpossibleCandidates(lanelet_map, routing_graph, lane.waypoints, &wp_candidate_lanelet_ids, false);
+  }
+
+  // eliminate impossible candidates using routing graph. (reverse direction)
+  for (const auto& lane : lane_array.lanes)
+  {
+    auto reverse_waypoints = lane.waypoints;
+    std::reverse(reverse_waypoints.begin(), reverse_waypoints.end());
+    removeImpossibleCandidates(lanelet_map, routing_graph, reverse_waypoints, &wp_candidate_lanelet_ids, true);
+  }
+
+  for (auto candidate : wp_candidate_lanelet_ids)
+  {
+    if (candidate.second.empty())
+    {
+      ROS_WARN_STREAM("No lanelet was matched for waypoint with gid: " << candidate.first);
+      continue;
+    }
+    if (candidate.second.size() >= 2)
+    {
+      ROS_WARN("ambiguous waypoint. Randomly choosing from candidates");
+    }
+    (*waypointid2laneletid)[candidate.first] = candidate.second.front();
+  }
+}
+
+void overwriteLaneletsCenterline(lanelet::LaneletMapPtr lanelet_map, const bool force_overwrite)
+{
+  for (auto& lanelet_obj : lanelet_map->laneletLayer)
+  {
+    if (force_overwrite || !lanelet_obj.hasCustomCenterline())
+    {
+      const auto fine_center_line = generateFineCenterline(lanelet_obj);
+      lanelet_obj.setCenterline(fine_center_line);
+    }
+  }
+}
+
+}  // namespace utils
+}  // namespace lanelet

src/ros/catkin/src/lanelet2_extension/lib/visualization.cpp

@@ -0,0 +1,683 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Authors: Simon Thompson, Ryohsuke Mitsudome
+ */
+
+#include <ros/ros.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+#include <Eigen/Eigen>
+
+#include <string>
+#include <vector>
+
+#include <lanelet2_extension/utility/message_conversion.h>
+#include <lanelet2_extension/utility/query.h>
+#include <lanelet2_extension/visualization/visualization.h>
+
+#include <amathutils_lib/amathutils.hpp>
+
+namespace
+{
+void adjacentPoints(const int i, const int N, const geometry_msgs::Polygon poly, geometry_msgs::Point32* p0,
+                    geometry_msgs::Point32* p1, geometry_msgs::Point32* p2)
+{
+  if (p0 == nullptr || p1 == nullptr || p2 == nullptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << ": either p0, p1, or p2 is null pointer!");
+    return;
+  }
+
+  *p1 = poly.points[i];
+  if (i == 0)
+    *p0 = poly.points[N - 1];
+  else
+    *p0 = poly.points[i - 1];
+
+  if (i < N - 1)
+    *p2 = poly.points[i + 1];
+  else
+    *p2 = poly.points[0];
+}
+
+double hypot(const geometry_msgs::Point32& p0, const geometry_msgs::Point32& p1)
+{
+  return (sqrt(pow((p1.x - p0.x), 2.0) + pow((p1.y - p0.y), 2.0)));
+}
+
+bool isAttributeValue(const lanelet::ConstPoint3d p, const std::string attr_str, const std::string value_str)
+{
+  lanelet::Attribute attr = p.attribute(attr_str);
+  if (attr.value().compare(value_str) == 0)
+    return true;
+  return false;
+}
+
+bool isLaneletAttributeValue(const lanelet::ConstLanelet ll, const std::string attr_str, const std::string value_str)
+{
+  lanelet::Attribute attr = ll.attribute(attr_str);
+  if (attr.value().compare(value_str) == 0)
+    return true;
+  return false;
+}
+
+void lightAsMarker(lanelet::ConstPoint3d p, visualization_msgs::Marker* marker, const std::string ns)
+{
+  if (marker == nullptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << ": marker is null pointer!");
+    return;
+  }
+
+  marker->header.frame_id = "map";
+  marker->header.stamp = ros::Time();
+  marker->ns = ns;
+  marker->id = p.id();
+  marker->type = visualization_msgs::Marker::SPHERE;
+  marker->pose.position.x = p.x();
+  marker->pose.position.y = p.y();
+  marker->pose.position.z = p.z();
+
+  float s = 0.3;
+
+  marker->scale.x = s;
+  marker->scale.y = s;
+  marker->scale.z = s;
+
+  marker->color.r = 0.0f;
+  marker->color.g = 0.0f;
+  marker->color.b = 0.0f;
+  marker->color.a = 1.0f;
+
+  if (isAttributeValue(p, "color", "red"))
+    marker->color.r = 1.0f;
+  else if (isAttributeValue(p, "color", "green"))
+    marker->color.g = 1.0f;
+  else if (isAttributeValue(p, "color", "yellow"))
+  {
+    marker->color.r = 1.0f;
+    marker->color.g = 1.0f;
+  }
+  else
+  {
+    marker->color.r = 1.0f;
+    marker->color.g = 1.0f;
+    marker->color.b = 1.0f;
+  }
+  marker->lifetime = ros::Duration();
+}
+
+void laneletDirectionAsMarker(const lanelet::ConstLanelet ll, visualization_msgs::Marker* marker, const int id,
+                              const std::string ns)
+{
+  if (marker == nullptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << ": marker is null pointer!");
+    return;
+  }
+
+  marker->header.frame_id = "map";
+  marker->header.stamp = ros::Time();
+  marker->ns = ns;
+  marker->id = id;
+  marker->type = visualization_msgs::Marker::TRIANGLE_LIST;
+
+  lanelet::BasicPoint3d pt[3];
+  pt[0].x() = 0.0;
+  pt[0].y() = -0.3;
+  pt[0].z() = 0.0;
+  pt[1].x() = 0.0;
+  pt[1].y() = 0.3;
+  pt[1].z() = 0;
+  pt[2].x() = 1.0;
+  pt[2].y() = 0.0;
+  pt[2].z() = 0;
+
+  lanelet::BasicPoint3d pc, pc2;
+
+  lanelet::ConstLineString3d center_ls = ll.centerline();
+  float s = 1.0;
+
+  marker->lifetime = ros::Duration();
+
+  marker->pose.position.x = 0.0;  // p.x();
+  marker->pose.position.y = 0.0;  // p.y();
+  marker->pose.position.z = 0.0;  // p.z();
+  marker->scale.x = s;
+  marker->scale.y = s;
+  marker->scale.z = s;
+  marker->color.r = 1.0f;
+  marker->color.g = 1.0f;
+  marker->color.b = 1.0f;
+  marker->color.a = 1.0f;
+
+  lanelet::Attribute attr = ll.attribute("turn_direction");
+  double turn_dir = 0;
+
+  std_msgs::ColorRGBA c;
+  c.r = 0.0;
+  c.g = 0.0;
+  c.b = 1.0;
+  c.a = 0.6;
+
+  if (isLaneletAttributeValue(ll, "turn_direction", "right"))
+  {
+    turn_dir = -M_PI / 2.0;
+    c.r = 1.0;
+    c.g = 0.0;
+    c.b = 1.0;
+  }
+  else if (isLaneletAttributeValue(ll, "turn_direction", "left"))
+  {
+    turn_dir = M_PI / 2.0;
+    c.r = 0.0;
+    c.g = 1.0;
+    c.b = 1.0;
+  }
+
+  for (int ci = 0; ci < center_ls.size() - 1;)
+  {
+    pc = center_ls[ci];
+    if (center_ls.size() > 1)
+      pc2 = center_ls[ci + 1];
+    else
+      return;
+
+    double heading = atan2(pc2.y() - pc.y(), pc2.x() - pc.x());
+
+    lanelet::BasicPoint3d pt_tf[3];
+
+    Eigen::Vector3d axis(0, 0, 1);
+    Eigen::Transform<double, 3, Eigen::Affine> t(Eigen::AngleAxis<double>(heading, axis));
+
+    for (int i = 0; i < 3; i++)
+      pt_tf[i] = t * pt[i] + pc;
+
+    geometry_msgs::Point gp[3];
+
+    for (int i = 0; i < 3; i++)
+    {
+      std_msgs::ColorRGBA cn = c;
+
+      gp[i].x = pt_tf[i].x();
+      gp[i].y = pt_tf[i].y();
+      gp[i].z = pt_tf[i].z();
+      marker->points.push_back(gp[i]);
+      marker->colors.push_back(cn);
+    }
+    ci = ci + 1;
+  }
+}
+
+bool isReflexAngle(const geometry_msgs::Point32& a, const geometry_msgs::Point32& o, const geometry_msgs::Point32& b)
+{
+  // angle aob is reflex in counterclock wise direction if point b is on the right side of vector oa
+  // which can be found out by calculating cross product of oa and ob
+  return (a.x - o.x) * (b.y - o.y) - (a.y - o.y) * (b.x - o.x) < 0;
+}
+
+bool isWithinTriangle(const geometry_msgs::Point32& a, const geometry_msgs::Point32& b, const geometry_msgs::Point32& c,
+                      const geometry_msgs::Point32& p)
+{
+  // point p in within triangle if p is on the same side
+  const double side1 = (b.x - a.x) * (p.y - a.y) - (b.y - a.y) * (p.x - a.x);  // a to b
+  const double side2 = (c.x - b.x) * (p.y - b.y) - (c.y - b.y) * (p.x - b.x);  // b to c
+  const double side3 = (a.x - c.x) * (p.y - c.y) - (a.y - c.y) * (p.x - c.x);  // c to a
+  return (side1 > 0.0 && side2 > 0.0 && side3 > 0.0) || (side1 < 0.0 && side2 < 0.0 && side3 < 0.0);
+}
+
+}  // anonymous namespace
+
+namespace lanelet
+{
+void visualization::lanelet2Triangle(const lanelet::ConstLanelet& ll, std::vector<geometry_msgs::Polygon>* triangles)
+{
+  if (triangles == nullptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << ": triangles is null pointer!");
+    return;
+  }
+
+  triangles->clear();
+  geometry_msgs::Polygon ll_poly;
+  lanelet2Polygon(ll, &ll_poly);
+  polygon2Triangle(ll_poly, triangles);
+}
+
+void visualization::polygon2Triangle(const geometry_msgs::Polygon& polygon,
+                                     std::vector<geometry_msgs::Polygon>* triangles)
+{
+  geometry_msgs::Polygon poly = polygon;
+  // ear clipping: find smallest internal angle in polygon
+  int N = poly.points.size();
+
+  // array of angles for each vertex
+  std::vector<bool> is_reflex_angle;
+  is_reflex_angle.assign(N, false);
+  for (int i = 0; i < N; i++)
+  {
+    geometry_msgs::Point32 p0, p1, p2;
+
+    adjacentPoints(i, N, poly, &p0, &p1, &p2);
+    is_reflex_angle.at(i) = isReflexAngle(p0, p1, p2);
+  }
+
+  // start ear clipping
+  while (N >= 3)
+  {
+    int clipped_vertex = -1;
+
+    for (int i = 0; i < N; i++)
+    {
+      bool is_reflex = is_reflex_angle.at(i);
+      if (!is_reflex)
+      {
+        geometry_msgs::Point32 p0, p1, p2;
+        adjacentPoints(i, N, poly, &p0, &p1, &p2);
+
+        int j_begin = (i + 2) % N;
+        int j_end = (i - 1 + N) % N;
+        bool is_ear = true;
+        for (int j = j_begin; j != j_end; j = (j + 1) % N)
+        {
+          if (isWithinTriangle(p0, p1, p2, poly.points.at(j)))
+          {
+            is_ear = false;
+            break;
+          }
+        }
+
+        if (is_ear)
+        {
+          clipped_vertex = i;
+          break;
+        }
+      }
+    }
+    if (clipped_vertex < 0 || clipped_vertex >= N)
+    {
+      ROS_ERROR("Could not find valid vertex for ear clipping triangulation. Triangulation result might be invalid");
+      clipped_vertex = 0;
+    }
+
+    // create triangle
+    geometry_msgs::Point32 p0, p1, p2;
+    adjacentPoints(clipped_vertex, N, poly, &p0, &p1, &p2);
+    geometry_msgs::Polygon triangle;
+    triangle.points.push_back(p0);
+    triangle.points.push_back(p1);
+    triangle.points.push_back(p2);
+    triangles->push_back(triangle);
+
+    // remove vertex of center of angle
+    auto it = poly.points.begin();
+    std::advance(it, clipped_vertex);
+    poly.points.erase(it);
+
+    // remove from angle list
+    auto it_angle = is_reflex_angle.begin();
+    std::advance(it_angle, clipped_vertex);
+    is_reflex_angle.erase(it_angle);
+
+    // update angle list
+    N = poly.points.size();
+    if (clipped_vertex == N)
+    {
+      clipped_vertex = 0;
+    }
+    adjacentPoints(clipped_vertex, N, poly, &p0, &p1, &p2);
+    is_reflex_angle.at(clipped_vertex) = isReflexAngle(p0, p1, p2);
+
+    int i_prev = (clipped_vertex == 0) ? N - 1 : clipped_vertex - 1;
+    adjacentPoints(i_prev, N, poly, &p0, &p1, &p2);
+    is_reflex_angle.at(i_prev) = isReflexAngle(p0, p1, p2);
+  }
+}
+
+void visualization::lanelet2Polygon(const lanelet::ConstLanelet& ll, geometry_msgs::Polygon* polygon)
+{
+  if (polygon == nullptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << ": polygon is null pointer!");
+    return;
+  }
+
+  lanelet::CompoundPolygon3d ll_poly = ll.polygon3d();
+
+  polygon->points.clear();
+  polygon->points.reserve(ll_poly.size());
+
+  for (const auto& pt : ll_poly)
+  {
+    geometry_msgs::Point32 pt32;
+    utils::conversion::toGeomMsgPt32(pt.basicPoint(), &pt32);
+    polygon->points.push_back(pt32);
+  }
+}
+
+visualization_msgs::MarkerArray visualization::laneletDirectionAsMarkerArray(const lanelet::ConstLanelets lanelets)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int ll_dir_count = 0;
+  for (auto lli = lanelets.begin(); lli != lanelets.end(); lli++)
+  {
+    lanelet::ConstLanelet ll = *lli;
+
+    //      bool road_found = false;
+    if (ll.hasAttribute(std::string("turn_direction")))
+    {
+      lanelet::Attribute attr = ll.attribute("turn_direction");
+      visualization_msgs::Marker marker;
+
+      laneletDirectionAsMarker(ll, &marker, ll_dir_count, "lanelet direction");
+
+      marker_array.markers.push_back(marker);
+      ll_dir_count++;
+    }
+  }
+
+  return (marker_array);
+}
+
+visualization_msgs::MarkerArray visualization::autowareTrafficLightsAsMarkerArray(
+    const std::vector<lanelet::AutowareTrafficLightConstPtr> tl_reg_elems, const std_msgs::ColorRGBA c,
+    const ros::Duration duration, const double scale)
+{
+  visualization_msgs::MarkerArray tl_marker_array;
+
+  int tl_count = 0;
+  for (auto tli = tl_reg_elems.begin(); tli != tl_reg_elems.end(); tli++)
+  {
+    lanelet::ConstLineStrings3d light_bulbs;
+    lanelet::AutowareTrafficLightConstPtr tl = *tli;
+
+    const auto lights = tl->trafficLights();
+    for (const auto& lsp : lights)
+    {
+      if (lsp.isLineString())  // traffic ligths can either polygons or
+      {                        // linestrings
+        lanelet::ConstLineString3d ls = static_cast<lanelet::ConstLineString3d>(lsp);
+
+        visualization_msgs::Marker marker;
+        visualization::trafficLight2TriangleMarker(ls, &marker, "traffic_light_triangle", c, duration, scale);
+        tl_marker_array.markers.push_back(marker);
+      }
+    }
+
+    light_bulbs = tl->lightBulbs();
+    for (auto ls : light_bulbs)
+    {
+      lanelet::ConstLineString3d l = static_cast<lanelet::ConstLineString3d>(ls);
+
+      for (auto pt : l)
+      {
+        if (pt.hasAttribute("color"))
+        {
+          visualization_msgs::Marker marker;
+          lightAsMarker(pt, &marker, "traffic_light");
+          tl_marker_array.markers.push_back(marker);
+
+          tl_count++;
+        }
+      }
+    }
+  }
+
+  return (tl_marker_array);
+}
+
+visualization_msgs::MarkerArray
+visualization::lineStringsAsMarkerArray(const std::vector<lanelet::ConstLineString3d> line_strings,
+                                        const std::string name_space, const std_msgs::ColorRGBA c, const double lss)
+{
+  visualization_msgs::MarkerArray ls_marker_array;
+  for (auto i = line_strings.begin(); i != line_strings.end(); i++)
+  {
+    lanelet::ConstLineString3d ls = *i;
+    visualization_msgs::Marker ls_marker;
+
+    visualization::lineString2Marker(ls, &ls_marker, "map", name_space, c, 0.2);
+
+    ls_marker_array.markers.push_back(ls_marker);
+  }
+
+  return (ls_marker_array);
+}
+
+visualization_msgs::MarkerArray visualization::laneletsBoundaryAsMarkerArray(const lanelet::ConstLanelets& lanelets,
+                                                                             const std_msgs::ColorRGBA c,
+                                                                             const bool viz_centerline)
+{
+  double lss = 0.2;  // line string size
+  visualization_msgs::MarkerArray marker_array;
+  for (auto li = lanelets.begin(); li != lanelets.end(); li++)
+  {
+    lanelet::ConstLanelet lll = *li;
+
+    lanelet::ConstLineString3d left_ls = lll.leftBound();
+    lanelet::ConstLineString3d right_ls = lll.rightBound();
+    lanelet::ConstLineString3d center_ls = lll.centerline();
+
+    visualization_msgs::Marker left_line_strip, right_line_strip, center_line_strip;
+
+    visualization::lineString2Marker(left_ls, &left_line_strip, "map", "left_lane_bound", c, lss);
+    visualization::lineString2Marker(right_ls, &right_line_strip, "map", "right_lane_bound", c, lss);
+    marker_array.markers.push_back(left_line_strip);
+    marker_array.markers.push_back(right_line_strip);
+    if (viz_centerline)
+    {
+      visualization::lineString2Marker(center_ls, &center_line_strip, "map", "center_lane_line", c, lss * 0.5);
+      marker_array.markers.push_back(center_line_strip);
+    }
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray visualization::trafficLightsAsTriangleMarkerArray(
+    const std::vector<lanelet::TrafficLightConstPtr> tl_reg_elems, const std_msgs::ColorRGBA c,
+    const ros::Duration duration, const double scale)
+{
+  // convert to to an array of linestrings and publish as marker array using
+  // exisitng function
+
+  int tl_count = 0;
+  std::vector<lanelet::ConstLineString3d> line_strings;
+  visualization_msgs::MarkerArray marker_array;
+
+  for (auto tli = tl_reg_elems.begin(); tli != tl_reg_elems.end(); tli++)
+  {
+    lanelet::TrafficLightConstPtr tl = *tli;
+    lanelet::LineString3d ls;
+
+    auto lights = tl->trafficLights();
+    for (auto lsp : lights)
+    {
+      if (lsp.isLineString())  // traffic ligths can either polygons or
+      {                        // linestrings
+        lanelet::ConstLineString3d ls = static_cast<lanelet::ConstLineString3d>(lsp);
+
+        visualization_msgs::Marker marker;
+        visualization::trafficLight2TriangleMarker(ls, &marker, "traffic_light_triangle", c, duration, scale);
+        marker_array.markers.push_back(marker);
+        tl_count++;
+      }
+    }
+  }
+
+  return (marker_array);
+}
+
+visualization_msgs::MarkerArray visualization::laneletsAsTriangleMarkerArray(const std::string ns,
+                                                                             const lanelet::ConstLanelets& lanelets,
+                                                                             const std_msgs::ColorRGBA c)
+{
+  visualization_msgs::MarkerArray marker_array;
+  visualization_msgs::Marker marker;
+
+  marker.header.frame_id = "map";
+  marker.header.stamp = ros::Time();
+  marker.ns = ns;
+  marker.id = 0;
+  marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
+  marker.lifetime = ros::Duration();
+  marker.pose.position.x = 0.0;  // p.x();
+  marker.pose.position.y = 0.0;  // p.y();
+  marker.pose.position.z = 0.0;  // p.z();
+  marker.scale.x = 1.0;
+  marker.scale.y = 1.0;
+  marker.scale.z = 1.0;
+  marker.color.r = 1.0f;
+  marker.color.g = 1.0f;
+  marker.color.b = 1.0f;
+  marker.color.a = 1.0f;
+
+  for (auto ll : lanelets)
+  {
+    std::vector<geometry_msgs::Polygon> triangles;
+    lanelet2Triangle(ll, &triangles);
+
+    for (auto tri : triangles)
+    {
+      geometry_msgs::Point tri0[3];
+
+      for (int i = 0; i < 3; i++)
+      {
+        utils::conversion::toGeomMsgPt(tri.points[i], &tri0[i]);
+
+        marker.points.push_back(tri0[i]);
+        marker.colors.push_back(c);
+      }
+    }
+  }
+  if (!marker.points.empty())
+  {
+    marker_array.markers.push_back(marker);
+  }
+
+  return (marker_array);
+}
+
+void visualization::trafficLight2TriangleMarker(const lanelet::ConstLineString3d ls, visualization_msgs::Marker* marker,
+                                                const std::string ns, const std_msgs::ColorRGBA cl,
+                                                const ros::Duration duration, const double scale)
+{
+  if (marker == nullptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << ": marker is null pointer!");
+    return;
+  }
+  marker->header.frame_id = "map";
+  marker->header.stamp = ros::Time();
+  marker->ns = ns;
+  marker->id = ls.id();
+  marker->type = visualization_msgs::Marker::TRIANGLE_LIST;
+
+  marker->lifetime = duration;
+
+  marker->pose.position.x = 0.0;  // p.x();
+  marker->pose.position.y = 0.0;  // p.y();
+  marker->pose.position.z = 0.0;  // p.z();
+  marker->scale.x = 1.0;
+  marker->scale.y = 1.0;
+  marker->scale.z = 1.0;
+  marker->color.r = 1.0f;
+  marker->color.g = 1.0f;
+  marker->color.b = 1.0f;
+  marker->color.a = 1.0f;
+
+  double h = 0.7;
+  if (ls.hasAttribute("height"))
+  {
+    lanelet::Attribute attr = ls.attribute("height");
+    h = std::stod(attr.value());
+  }
+
+  // construct triangles and add to marker
+
+  // define polygon of traffic light border
+  Eigen::Vector3d v[4];
+  v[0] << ls.front().x(), ls.front().y(), ls.front().z();
+  v[1] << ls.back().x(), ls.back().y(), ls.back().z();
+  v[2] << ls.back().x(), ls.back().y(), ls.back().z() + h;
+  v[3] << ls.front().x(), ls.front().y(), ls.front().z() + h;
+
+  Eigen::Vector3d c = (v[0] + v[1] + v[2] + v[3]) / 4;
+
+  if (scale > 0.0 && scale != 1.0)
+  {
+    for (int i = 0; i < 4; i++)
+    {
+      v[i] = (v[i] - c) * scale + c;
+    }
+  }
+  geometry_msgs::Point tri0[3];
+  utils::conversion::toGeomMsgPt(v[0], &tri0[0]);
+  utils::conversion::toGeomMsgPt(v[1], &tri0[1]);
+  utils::conversion::toGeomMsgPt(v[2], &tri0[2]);
+  geometry_msgs::Point tri1[3];
+  utils::conversion::toGeomMsgPt(v[0], &tri1[0]);
+  utils::conversion::toGeomMsgPt(v[2], &tri1[1]);
+  utils::conversion::toGeomMsgPt(v[3], &tri1[2]);
+
+  for (int i = 0; i < 3; i++)
+  {
+    marker->points.push_back(tri0[i]);
+    marker->colors.push_back(cl);
+  }
+  for (int i = 0; i < 3; i++)
+  {
+    marker->points.push_back(tri1[i]);
+    marker->colors.push_back(cl);
+  }
+}
+
+void visualization::lineString2Marker(const lanelet::ConstLineString3d ls, visualization_msgs::Marker* line_strip,
+                                      const std::string frame_id, const std::string ns, const std_msgs::ColorRGBA c,
+                                      const float lss)
+{
+  if (line_strip == nullptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << ": line_strip is null pointer!");
+    return;
+  }
+
+  line_strip->header.frame_id = frame_id;
+  line_strip->header.stamp = ros::Time();
+  line_strip->ns = ns;
+  line_strip->action = visualization_msgs::Marker::ADD;
+
+  line_strip->pose.orientation.w = 1.0;
+  line_strip->id = ls.id();
+
+  line_strip->type = visualization_msgs::Marker::LINE_STRIP;
+
+  line_strip->scale.x = lss;
+
+  line_strip->color = c;
+
+  // fill out lane line
+  for (auto i = ls.begin(); i != ls.end(); i++)
+  {
+    geometry_msgs::Point p;
+    p.x = (*i).x();
+    p.y = (*i).y();
+    p.z = (*i).z();
+    line_strip->points.push_back(p);
+  }
+}
+
+}  // namespace lanelet

src/ros/catkin/src/map_file/lib/map_file/get_file.cpp

@@ -0,0 +1,105 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <string>
+#include <fstream>
+#include <sstream>
+#include <ext/stdio_filebuf.h>
+#include <curl/curl.h>
+
+#include <map_file/get_file.h>
+
+
+GetFile::GetFile()
+  : GetFile(HTTP_HOSTNAME, HTTP_PORT, HTTP_USER, HTTP_PASSWORD)
+{
+}
+
+GetFile::GetFile(const std::string& host_name, int port, const std::string& user, const std::string& password)
+  : host_name_(host_name), port_(port), user_(user), password_(password)
+{
+}
+
+typedef std::basic_ofstream<char>::__filebuf_type buffer_t;
+typedef __gnu_cxx::stdio_filebuf<char> io_buffer_t; 
+static FILE* cfile(buffer_t* const fb) {
+  return (static_cast<io_buffer_t* const>(fb))->file();
+}
+
+int GetFile::GetHTTPFile(const std::string& value) 
+{
+  CURL *curl;
+
+  curl = curl_easy_init();
+  if (! curl) {
+    std::cerr << "curl_easy_init failed" << std::endl;
+    return -1;
+  }
+
+  std::string filepath("/tmp/" + value);
+  std::ofstream ofs;
+  ofs.open(filepath);
+  if (! ofs.is_open()) {
+    std::cerr << "cannot open /tmp/" << value << std::endl;
+    curl_easy_cleanup(curl);
+    return -2;
+  }
+
+  std::ostringstream urlss;
+  urlss << "http://" << host_name_ << ":" << port_ << "/" << value;
+  curl_easy_setopt(curl, CURLOPT_URL, urlss.str().c_str());
+  curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1);
+  curl_easy_setopt(curl, CURLOPT_MAXREDIRS, 5);
+  curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, fwrite);
+  curl_easy_setopt(curl, CURLOPT_WRITEDATA, cfile(ofs.rdbuf()));
+  if (user_ != "" && password_ != "") {
+    std::string userpwd = user_ + ":" + password_;
+    curl_easy_setopt(curl, CURLOPT_HTTPAUTH, CURLAUTH_DIGEST);
+    curl_easy_setopt(curl, CURLOPT_USERPWD, userpwd.c_str());
+  }
+  CURLcode res = curl_easy_perform(curl);
+  if (res != CURLE_OK) {
+    std::cerr << "curl_easy_perform failed: " <<
+      curl_easy_strerror(res) << std::endl;
+    curl_easy_cleanup(curl);
+    ofs.close();
+    unlink(filepath.c_str());
+    return -3;
+  }
+  long response_code;
+  res = curl_easy_getinfo(curl, CURLINFO_RESPONSE_CODE, &response_code);
+  if (res != CURLE_OK) {
+    std::cerr << "curl_easy_getinfo failed: " <<
+      curl_easy_strerror(res) << std::endl;
+    curl_easy_cleanup(curl);
+    ofs.close();
+    unlink(filepath.c_str());
+    return -4;
+  }
+  curl_easy_cleanup(curl);
+  ofs.close();
+  if (response_code != 200) {
+    unlink(filepath.c_str());
+    return -5;
+  }
+
+  return 0;
+}

src/ros/catkin/src/opendrive_if/CMakeLists.txt

@@ -5,10 +5,11 @@ project(opendrive_if)
 message(STATUS  "Enter opendrive_if")
 
 
-SET(QT_PATH  /opt/qt/5.13.2/gcc_64)
+SET(QT_PATH  /usr/include/x86_64-linux-gnu/qt5)
+SET(QT2_PATH  /usr/include/x86_64-linux-gnu/qt5)
 SET(AGX_QT_PATH  /usr/include/aarch64-linux-gnu/qt5)
 
-find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore)
+find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore  ${QT2_PATH}/QtCore)
 
 if(QT_EXIST)
 include_directories(
@@ -16,6 +17,8 @@ include_directories(
   ${QT_PATH}/include/QtCore
   ${AGX_QT_PATH}
   ${AGX_QT_PATH}/QtCore
+    ${QT2_PATH}
+    ${QT2_PATH}/QtCore
 )
 link_directories(
   ${QT_PATH}/lib

src/ros/catkin/src/pilottoros/CMakeLists.txt

@@ -4,11 +4,11 @@ project(pilottoros)
 
 message(STATUS  "Enter pilottoros")
 
-
-SET(QT_PATH  /opt/qt/5.13.2/gcc_64)
+SET(QT_PATH  /usr/include/x86_64-linux-gnu/qt5)
+SET(QT2_PATH  /usr/include/x86_64-linux-gnu/qt5)
 SET(AGX_QT_PATH  /usr/include/aarch64-linux-gnu/qt5)
 
-find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore)
+find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore  ${QT2_PATH}/QtCore)
 
 if(QT_EXIST)
 include_directories(
@@ -16,6 +16,8 @@ include_directories(
   ${QT_PATH}/include/QtCore
   ${AGX_QT_PATH}
   ${AGX_QT_PATH}/QtCore
+    ${QT2_PATH}
+    ${QT2_PATH}/QtCore
 )
 link_directories(
   ${QT_PATH}/lib

src/ros/catkin/src/rostopilot/CMakeLists.txt

@@ -4,11 +4,11 @@ project(rostopilot)
 
 message(STATUS  "Enter rostopilot")
 
-
-SET(QT_PATH  /opt/qt/5.13.2/gcc_64)
+SET(QT_PATH  /usr/include/x86_64-linux-gnu/qt5)
+SET(QT2_PATH  /usr/include/x86_64-linux-gnu/qt5)
 SET(AGX_QT_PATH  /usr/include/aarch64-linux-gnu/qt5)
 
-find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore)
+find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore  ${QT2_PATH}/QtCore)
 
 if(QT_EXIST)
 include_directories(
@@ -16,6 +16,8 @@ include_directories(
   ${QT_PATH}/include/QtCore
   ${AGX_QT_PATH}
   ${AGX_QT_PATH}/QtCore
+    ${QT2_PATH}
+    ${QT2_PATH}/QtCore
 )
 link_directories(
   ${QT_PATH}/lib

src/ros/catkin/src/rtk_hcp2/CMakeLists.txt

@@ -6,18 +6,25 @@ message(STATUS  "Enter rtk_hcp2")
 
 
 SET(QT_PATH  /opt/qt/5.13.2/gcc_64)
+SET(QT2_PATH  /usr/include/x86_64-linux-gnu/qt5)
 SET(AGX_QT_PATH  /usr/include/aarch64-linux-gnu/qt5)
 
-find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore)
+find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore  ${QT2_PATH}/QtCore)
 
 if(QT_EXIST)
 include_directories(
   ${QT_PATH}/include
   ${QT_PATH}/include/QtCore
   ${QT_PATH}/include/QtSerialPort
+  ${QT_PATH}/include/QtNetwork
   ${AGX_QT_PATH}
   ${AGX_QT_PATH}/QtCore
   ${AGX_QT_PATH}/QtSerialPort
+  ${AGX_QT_PATH}/QtNetwork
+  ${QT2_PATH}
+  ${QT2_PATH}/QtCore
+  ${QT2_PATH}/QtSerialPort
+  ${QT2_PATH}/QtNetwork
 )
 link_directories(
   ${QT_PATH}/lib

src/ros/catkin/src/rtk_hcp2_baselink/CMakeLists.txt

@@ -6,18 +6,25 @@ message(STATUS  "Enter rtk_hcp2_baselink")
 
 
 SET(QT_PATH  /opt/qt/5.13.2/gcc_64)
+SET(QT2_PATH  /usr/include/x86_64-linux-gnu/qt5)
 SET(AGX_QT_PATH  /usr/include/aarch64-linux-gnu/qt5)
 
-find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore)
+find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore  ${QT2_PATH}/QtCore)
 
 if(QT_EXIST)
 include_directories(
   ${QT_PATH}/include
   ${QT_PATH}/include/QtCore
   ${QT_PATH}/include/QtSerialPort
+  ${QT_PATH}/include/QtNetwork
   ${AGX_QT_PATH}
   ${AGX_QT_PATH}/QtCore
   ${AGX_QT_PATH}/QtSerialPort
+  ${AGX_QT_PATH}/QtNetwork
+  ${QT2_PATH}
+  ${QT2_PATH}/QtCore
+  ${QT2_PATH}/QtSerialPort
+  ${QT2_PATH}/QtNetwork
 )
 link_directories(
   ${QT_PATH}/lib

src/ros/catkin/src/rtk_nav992/CMakeLists.txt

@@ -6,9 +6,10 @@ message(STATUS  "Enter rtk_nav992")
 add_compile_options(-std=c++14)
 
 SET(QT_PATH  /opt/qt/5.13.2/gcc_64)
+SET(QT2_PATH  /usr/include/x86_64-linux-gnu/qt5)
 SET(AGX_QT_PATH  /usr/include/aarch64-linux-gnu/qt5)
 
-find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore)
+find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore  ${QT2_PATH}/QtCore)
 
 if(QT_EXIST)
 include_directories(
@@ -20,6 +21,10 @@ include_directories(
   ${AGX_QT_PATH}/QtCore
   ${AGX_QT_PATH}/QtSerialPort
   ${AGX_QT_PATH}/QtNetwork
+  ${QT2_PATH}
+  ${QT2_PATH}/QtCore
+  ${QT2_PATH}/QtSerialPort
+  ${QT2_PATH}/QtNetwork
 )
 link_directories(
   ${QT_PATH}/lib

src/ros/catkin/src/testrosmodulecomm/CMakeLists.txt

@@ -5,10 +5,11 @@ project(testrosmodulecomm)
 message(STATUS  "Enter testrosmodulecomm")
 
 
-SET(QT_PATH  /opt/qt/5.13.2/gcc_64)
+SET(QT_PATH  /usr/include/x86_64-linux-gnu/qt5)
+SET(QT2_PATH  /usr/include/x86_64-linux-gnu/qt5)
 SET(AGX_QT_PATH  /usr/include/aarch64-linux-gnu/qt5)
 
-find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore)
+find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore  ${QT2_PATH}/QtCore)
 
 if(QT_EXIST)
 include_directories(
@@ -16,6 +17,8 @@ include_directories(
   ${QT_PATH}/include/QtCore
   ${AGX_QT_PATH}
   ${AGX_QT_PATH}/QtCore
+    ${QT2_PATH}
+    ${QT2_PATH}/QtCore
 )
 link_directories(
   ${QT_PATH}/lib

src/ros/catkin/src/vector_map/lib/vector_map/vector_map.cpp

@@ -0,0 +1,2472 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <tf/transform_datatypes.h>
+#include <vector_map/vector_map.h>
+
+#include <map>
+#include <string>
+#include <vector>
+
+namespace vector_map
+{
+namespace
+{
+void updatePoint(std::map<Key<Point>, Point>& map, const PointArray& msg)
+{
+  map = std::map<Key<Point>, Point>();
+  for (const auto& item : msg.data)
+  {
+    if (item.pid == 0)
+      continue;
+    map.insert(std::make_pair(Key<Point>(item.pid), item));
+  }
+}
+
+void updateVector(std::map<Key<Vector>, Vector>& map, const VectorArray& msg)
+{
+  map = std::map<Key<Vector>, Vector>();
+  for (const auto& item : msg.data)
+  {
+    if (item.vid == 0)
+      continue;
+    map.insert(std::make_pair(Key<Vector>(item.vid), item));
+  }
+}
+
+void updateLine(std::map<Key<Line>, Line>& map, const LineArray& msg)
+{
+  map = std::map<Key<Line>, Line>();
+  for (const auto& item : msg.data)
+  {
+    if (item.lid == 0)
+      continue;
+    map.insert(std::make_pair(Key<Line>(item.lid), item));
+  }
+}
+
+void updateArea(std::map<Key<Area>, Area>& map, const AreaArray& msg)
+{
+  map = std::map<Key<Area>, Area>();
+  for (const auto& item : msg.data)
+  {
+    if (item.aid == 0)
+      continue;
+    map.insert(std::make_pair(Key<Area>(item.aid), item));
+  }
+}
+
+void updatePole(std::map<Key<Pole>, Pole>& map, const PoleArray& msg)
+{
+  map = std::map<Key<Pole>, Pole>();
+  for (const auto& item : msg.data)
+  {
+    if (item.plid == 0)
+      continue;
+    map.insert(std::make_pair(Key<Pole>(item.plid), item));
+  }
+}
+
+void updateBox(std::map<Key<Box>, Box>& map, const BoxArray& msg)
+{
+  map = std::map<Key<Box>, Box>();
+  for (const auto& item : msg.data)
+  {
+    if (item.bid == 0)
+      continue;
+    map.insert(std::make_pair(Key<Box>(item.bid), item));
+  }
+}
+
+void updateDTLane(std::map<Key<DTLane>, DTLane>& map, const DTLaneArray& msg)
+{
+  map = std::map<Key<DTLane>, DTLane>();
+  for (const auto& item : msg.data)
+  {
+    if (item.did == 0)
+      continue;
+    map.insert(std::make_pair(Key<DTLane>(item.did), item));
+  }
+}
+
+void updateNode(std::map<Key<Node>, Node>& map, const NodeArray& msg)
+{
+  map = std::map<Key<Node>, Node>();
+  for (const auto& item : msg.data)
+  {
+    if (item.nid == 0)
+      continue;
+    map.insert(std::make_pair(Key<Node>(item.nid), item));
+  }
+}
+
+void updateLane(std::map<Key<Lane>, Lane>& map, const LaneArray& msg)
+{
+  map = std::map<Key<Lane>, Lane>();
+  for (const auto& item : msg.data)
+  {
+    if (item.lnid == 0)
+      continue;
+    map.insert(std::make_pair(Key<Lane>(item.lnid), item));
+  }
+}
+
+void updateWayArea(std::map<Key<WayArea>, WayArea>& map, const WayAreaArray& msg)
+{
+  map = std::map<Key<WayArea>, WayArea>();
+  for (const auto& item : msg.data)
+  {
+    if (item.waid == 0)
+      continue;
+    map.insert(std::make_pair(Key<WayArea>(item.waid), item));
+  }
+}
+
+void updateRoadEdge(std::map<Key<RoadEdge>, RoadEdge>& map, const RoadEdgeArray& msg)
+{
+  map = std::map<Key<RoadEdge>, RoadEdge>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<RoadEdge>(item.id), item));
+  }
+}
+
+void updateGutter(std::map<Key<Gutter>, Gutter>& map, const GutterArray& msg)
+{
+  map = std::map<Key<Gutter>, Gutter>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<Gutter>(item.id), item));
+  }
+}
+
+void updateCurb(std::map<Key<Curb>, Curb>& map, const CurbArray& msg)
+{
+  map = std::map<Key<Curb>, Curb>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<Curb>(item.id), item));
+  }
+}
+
+void updateWhiteLine(std::map<Key<WhiteLine>, WhiteLine>& map, const WhiteLineArray& msg)
+{
+  map = std::map<Key<WhiteLine>, WhiteLine>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<WhiteLine>(item.id), item));
+  }
+}
+
+void updateStopLine(std::map<Key<StopLine>, StopLine>& map, const StopLineArray& msg)
+{
+  map = std::map<Key<StopLine>, StopLine>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<StopLine>(item.id), item));
+  }
+}
+
+void updateZebraZone(std::map<Key<ZebraZone>, ZebraZone>& map, const ZebraZoneArray& msg)
+{
+  map = std::map<Key<ZebraZone>, ZebraZone>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<ZebraZone>(item.id), item));
+  }
+}
+
+void updateCrossWalk(std::map<Key<CrossWalk>, CrossWalk>& map, const CrossWalkArray& msg)
+{
+  map = std::map<Key<CrossWalk>, CrossWalk>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<CrossWalk>(item.id), item));
+  }
+}
+
+void updateRoadMark(std::map<Key<RoadMark>, RoadMark>& map, const RoadMarkArray& msg)
+{
+  map = std::map<Key<RoadMark>, RoadMark>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<RoadMark>(item.id), item));
+  }
+}
+
+void updateRoadPole(std::map<Key<RoadPole>, RoadPole>& map, const RoadPoleArray& msg)
+{
+  map = std::map<Key<RoadPole>, RoadPole>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<RoadPole>(item.id), item));
+  }
+}
+
+void updateRoadSign(std::map<Key<RoadSign>, RoadSign>& map, const RoadSignArray& msg)
+{
+  map = std::map<Key<RoadSign>, RoadSign>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<RoadSign>(item.id), item));
+  }
+}
+
+void updateSignal(std::map<Key<Signal>, Signal>& map, const SignalArray& msg)
+{
+  map = std::map<Key<Signal>, Signal>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<Signal>(item.id), item));
+  }
+}
+
+void updateStreetLight(std::map<Key<StreetLight>, StreetLight>& map, const StreetLightArray& msg)
+{
+  map = std::map<Key<StreetLight>, StreetLight>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<StreetLight>(item.id), item));
+  }
+}
+
+void updateUtilityPole(std::map<Key<UtilityPole>, UtilityPole>& map, const UtilityPoleArray& msg)
+{
+  map = std::map<Key<UtilityPole>, UtilityPole>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<UtilityPole>(item.id), item));
+  }
+}
+
+void updateGuardRail(std::map<Key<GuardRail>, GuardRail>& map, const GuardRailArray& msg)
+{
+  map = std::map<Key<GuardRail>, GuardRail>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<GuardRail>(item.id), item));
+  }
+}
+
+void updateSideWalk(std::map<Key<SideWalk>, SideWalk>& map, const SideWalkArray& msg)
+{
+  map = std::map<Key<SideWalk>, SideWalk>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<SideWalk>(item.id), item));
+  }
+}
+
+void updateDriveOnPortion(std::map<Key<DriveOnPortion>, DriveOnPortion>& map, const DriveOnPortionArray& msg)
+{
+  map = std::map<Key<DriveOnPortion>, DriveOnPortion>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<DriveOnPortion>(item.id), item));
+  }
+}
+
+void updateCrossRoad(std::map<Key<CrossRoad>, CrossRoad>& map, const CrossRoadArray& msg)
+{
+  map = std::map<Key<CrossRoad>, CrossRoad>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<CrossRoad>(item.id), item));
+  }
+}
+
+void updateSideStrip(std::map<Key<SideStrip>, SideStrip>& map, const SideStripArray& msg)
+{
+  map = std::map<Key<SideStrip>, SideStrip>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<SideStrip>(item.id), item));
+  }
+}
+
+void updateCurveMirror(std::map<Key<CurveMirror>, CurveMirror>& map, const CurveMirrorArray& msg)
+{
+  map = std::map<Key<CurveMirror>, CurveMirror>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<CurveMirror>(item.id), item));
+  }
+}
+
+void updateWall(std::map<Key<Wall>, Wall>& map, const WallArray& msg)
+{
+  map = std::map<Key<Wall>, Wall>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<Wall>(item.id), item));
+  }
+}
+
+void updateFence(std::map<Key<Fence>, Fence>& map, const FenceArray& msg)
+{
+  map = std::map<Key<Fence>, Fence>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<Fence>(item.id), item));
+  }
+}
+
+void updateRailCrossing(std::map<Key<RailCrossing>, RailCrossing>& map, const RailCrossingArray& msg)
+{
+  map = std::map<Key<RailCrossing>, RailCrossing>();
+  for (const auto& item : msg.data)
+  {
+    if (item.id == 0)
+      continue;
+    map.insert(std::make_pair(Key<RailCrossing>(item.id), item));
+  }
+}
+}  // namespace
+
+bool VectorMap::hasSubscribed(category_t category) const
+{
+  if (category & POINT)
+  {
+    if (point_.empty())
+      return false;
+  }
+  if (category & VECTOR)
+  {
+    if (vector_.empty())
+      return false;
+  }
+  if (category & LINE)
+  {
+    if (line_.empty())
+      return false;
+  }
+  if (category & AREA)
+  {
+    if (area_.empty())
+      return false;
+  }
+  if (category & POLE)
+  {
+    if (pole_.empty())
+      return false;
+  }
+  if (category & BOX)
+  {
+    if (box_.empty())
+      return false;
+  }
+  if (category & DTLANE)
+  {
+    if (dtlane_.empty())
+      return false;
+  }
+  if (category & NODE)
+  {
+    if (node_.empty())
+      return false;
+  }
+  if (category & LANE)
+  {
+    if (lane_.empty())
+      return false;
+  }
+  if (category & WAY_AREA)
+  {
+    if (way_area_.empty())
+      return false;
+  }
+  if (category & ROAD_EDGE)
+  {
+    if (road_edge_.empty())
+      return false;
+  }
+  if (category & GUTTER)
+  {
+    if (gutter_.empty())
+      return false;
+  }
+  if (category & CURB)
+  {
+    if (curb_.empty())
+      return false;
+  }
+  if (category & WHITE_LINE)
+  {
+    if (white_line_.empty())
+      return false;
+  }
+  if (category & STOP_LINE)
+  {
+    if (stop_line_.empty())
+      return false;
+  }
+  if (category & ZEBRA_ZONE)
+  {
+    if (zebra_zone_.empty())
+      return false;
+  }
+  if (category & CROSS_WALK)
+  {
+    if (cross_walk_.empty())
+      return false;
+  }
+  if (category & ROAD_MARK)
+  {
+    if (road_mark_.empty())
+      return false;
+  }
+  if (category & ROAD_POLE)
+  {
+    if (road_pole_.empty())
+      return false;
+  }
+  if (category & ROAD_SIGN)
+  {
+    if (road_sign_.empty())
+      return false;
+  }
+  if (category & SIGNAL)
+  {
+    if (signal_.empty())
+      return false;
+  }
+  if (category & STREET_LIGHT)
+  {
+    if (street_light_.empty())
+      return false;
+  }
+  if (category & UTILITY_POLE)
+  {
+    if (utility_pole_.empty())
+      return false;
+  }
+  if (category & GUARD_RAIL)
+  {
+    if (guard_rail_.empty())
+      return false;
+  }
+  if (category & SIDE_WALK)
+  {
+    if (side_walk_.empty())
+      return false;
+  }
+  if (category & DRIVE_ON_PORTION)
+  {
+    if (drive_on_portion_.empty())
+      return false;
+  }
+  if (category & CROSS_ROAD)
+  {
+    if (cross_road_.empty())
+      return false;
+  }
+  if (category & SIDE_STRIP)
+  {
+    if (side_strip_.empty())
+      return false;
+  }
+  if (category & CURVE_MIRROR)
+  {
+    if (curve_mirror_.empty())
+      return false;
+  }
+  if (category & WALL)
+  {
+    if (wall_.empty())
+      return false;
+  }
+  if (category & FENCE)
+  {
+    if (fence_.empty())
+      return false;
+  }
+  if (category & RAIL_CROSSING)
+  {
+    if (rail_crossing_.empty())
+      return false;
+  }
+  return true;
+}
+
+void VectorMap::registerSubscriber(ros::NodeHandle& nh, category_t category)
+{
+  if (category & POINT)
+  {
+    point_.registerSubscriber(nh, "/vector_map_info/point");
+    point_.registerUpdater(updatePoint);
+  }
+  if (category & VECTOR)
+  {
+    vector_.registerSubscriber(nh, "/vector_map_info/vector");
+    vector_.registerUpdater(updateVector);
+  }
+  if (category & LINE)
+  {
+    line_.registerSubscriber(nh, "/vector_map_info/line");
+    line_.registerUpdater(updateLine);
+  }
+  if (category & AREA)
+  {
+    area_.registerSubscriber(nh, "/vector_map_info/area");
+    area_.registerUpdater(updateArea);
+  }
+  if (category & POLE)
+  {
+    pole_.registerSubscriber(nh, "/vector_map_info/pole");
+    pole_.registerUpdater(updatePole);
+  }
+  if (category & BOX)
+  {
+    box_.registerSubscriber(nh, "/vector_map_info/box");
+    box_.registerUpdater(updateBox);
+  }
+  if (category & DTLANE)
+  {
+    dtlane_.registerSubscriber(nh, "/vector_map_info/dtlane");
+    dtlane_.registerUpdater(updateDTLane);
+  }
+  if (category & NODE)
+  {
+    node_.registerSubscriber(nh, "/vector_map_info/node");
+    node_.registerUpdater(updateNode);
+  }
+  if (category & LANE)
+  {
+    lane_.registerSubscriber(nh, "/vector_map_info/lane");
+    lane_.registerUpdater(updateLane);
+  }
+  if (category & WAY_AREA)
+  {
+    way_area_.registerSubscriber(nh, "/vector_map_info/way_area");
+    way_area_.registerUpdater(updateWayArea);
+  }
+  if (category & ROAD_EDGE)
+  {
+    road_edge_.registerSubscriber(nh, "/vector_map_info/road_edge");
+    road_edge_.registerUpdater(updateRoadEdge);
+  }
+  if (category & GUTTER)
+  {
+    gutter_.registerSubscriber(nh, "/vector_map_info/gutter");
+    gutter_.registerUpdater(updateGutter);
+  }
+  if (category & CURB)
+  {
+    curb_.registerSubscriber(nh, "/vector_map_info/curb");
+    curb_.registerUpdater(updateCurb);
+  }
+  if (category & WHITE_LINE)
+  {
+    white_line_.registerSubscriber(nh, "/vector_map_info/white_line");
+    white_line_.registerUpdater(updateWhiteLine);
+  }
+  if (category & STOP_LINE)
+  {
+    stop_line_.registerSubscriber(nh, "/vector_map_info/stop_line");
+    stop_line_.registerUpdater(updateStopLine);
+  }
+  if (category & ZEBRA_ZONE)
+  {
+    zebra_zone_.registerSubscriber(nh, "/vector_map_info/zebra_zone");
+    zebra_zone_.registerUpdater(updateZebraZone);
+  }
+  if (category & CROSS_WALK)
+  {
+    cross_walk_.registerSubscriber(nh, "/vector_map_info/cross_walk");
+    cross_walk_.registerUpdater(updateCrossWalk);
+  }
+  if (category & ROAD_MARK)
+  {
+    road_mark_.registerSubscriber(nh, "/vector_map_info/road_mark");
+    road_mark_.registerUpdater(updateRoadMark);
+  }
+  if (category & ROAD_POLE)
+  {
+    road_pole_.registerSubscriber(nh, "/vector_map_info/road_pole");
+    road_pole_.registerUpdater(updateRoadPole);
+  }
+  if (category & ROAD_SIGN)
+  {
+    road_sign_.registerSubscriber(nh, "/vector_map_info/road_sign");
+    road_sign_.registerUpdater(updateRoadSign);
+  }
+  if (category & SIGNAL)
+  {
+    signal_.registerSubscriber(nh, "/vector_map_info/signal");
+    signal_.registerUpdater(updateSignal);
+  }
+  if (category & STREET_LIGHT)
+  {
+    street_light_.registerSubscriber(nh, "/vector_map_info/street_light");
+    street_light_.registerUpdater(updateStreetLight);
+  }
+  if (category & UTILITY_POLE)
+  {
+    utility_pole_.registerSubscriber(nh, "/vector_map_info/utility_pole");
+    utility_pole_.registerUpdater(updateUtilityPole);
+  }
+  if (category & GUARD_RAIL)
+  {
+    guard_rail_.registerSubscriber(nh, "/vector_map_info/guard_rail");
+    guard_rail_.registerUpdater(updateGuardRail);
+  }
+  if (category & SIDE_WALK)
+  {
+    side_walk_.registerSubscriber(nh, "/vector_map_info/side_walk");
+    side_walk_.registerUpdater(updateSideWalk);
+  }
+  if (category & DRIVE_ON_PORTION)
+  {
+    drive_on_portion_.registerSubscriber(nh, "/vector_map_info/drive_on_portion");
+    drive_on_portion_.registerUpdater(updateDriveOnPortion);
+  }
+  if (category & CROSS_ROAD)
+  {
+    cross_road_.registerSubscriber(nh, "/vector_map_info/cross_road");
+    cross_road_.registerUpdater(updateCrossRoad);
+  }
+  if (category & SIDE_STRIP)
+  {
+    side_strip_.registerSubscriber(nh, "/vector_map_info/side_strip");
+    side_strip_.registerUpdater(updateSideStrip);
+  }
+  if (category & CURVE_MIRROR)
+  {
+    curve_mirror_.registerSubscriber(nh, "/vector_map_info/curve_mirror");
+    curve_mirror_.registerUpdater(updateCurveMirror);
+  }
+  if (category & WALL)
+  {
+    wall_.registerSubscriber(nh, "/vector_map_info/wall");
+    wall_.registerUpdater(updateWall);
+  }
+  if (category & FENCE)
+  {
+    fence_.registerSubscriber(nh, "/vector_map_info/fence");
+    fence_.registerUpdater(updateFence);
+  }
+  if (category & RAIL_CROSSING)
+  {
+    rail_crossing_.registerSubscriber(nh, "/vector_map_info/rail_crossing");
+    rail_crossing_.registerUpdater(updateRailCrossing);
+  }
+}
+
+VectorMap::VectorMap()
+{
+}
+
+void VectorMap::subscribe(ros::NodeHandle& nh, category_t category)
+{
+  registerSubscriber(nh, category);
+  ros::Rate rate(1);
+  while (ros::ok() && !hasSubscribed(category))
+  {
+    ros::spinOnce();
+    rate.sleep();
+  }
+}
+
+void VectorMap::subscribe(ros::NodeHandle& nh, category_t category, const ros::Duration& timeout)
+{
+  registerSubscriber(nh, category);
+  ros::Rate rate(1);
+  ros::Time end = ros::Time::now() + timeout;
+  while (ros::ok() && !hasSubscribed(category) && ros::Time::now() < end)
+  {
+    ros::spinOnce();
+    rate.sleep();
+  }
+}
+
+void VectorMap::subscribe(ros::NodeHandle& nh, category_t category, const size_t max_retries)
+{
+  size_t tries = 0;
+  registerSubscriber(nh, category);
+  ros::Rate rate(1);
+  while (ros::ok() && !hasSubscribed(category) && tries++ < max_retries)
+  {
+    ros::spinOnce();
+    rate.sleep();
+  }
+}
+
+Point VectorMap::findByKey(const Key<Point>& key) const
+{
+  return point_.findByKey(key);
+}
+
+Vector VectorMap::findByKey(const Key<Vector>& key) const
+{
+  return vector_.findByKey(key);
+}
+
+Line VectorMap::findByKey(const Key<Line>& key) const
+{
+  return line_.findByKey(key);
+}
+
+Area VectorMap::findByKey(const Key<Area>& key) const
+{
+  return area_.findByKey(key);
+}
+
+Pole VectorMap::findByKey(const Key<Pole>& key) const
+{
+  return pole_.findByKey(key);
+}
+
+Box VectorMap::findByKey(const Key<Box>& key) const
+{
+  return box_.findByKey(key);
+}
+
+DTLane VectorMap::findByKey(const Key<DTLane>& key) const
+{
+  return dtlane_.findByKey(key);
+}
+
+Node VectorMap::findByKey(const Key<Node>& key) const
+{
+  return node_.findByKey(key);
+}
+
+Lane VectorMap::findByKey(const Key<Lane>& key) const
+{
+  return lane_.findByKey(key);
+}
+
+WayArea VectorMap::findByKey(const Key<WayArea>& key) const
+{
+  return way_area_.findByKey(key);
+}
+
+RoadEdge VectorMap::findByKey(const Key<RoadEdge>& key) const
+{
+  return road_edge_.findByKey(key);
+}
+
+Gutter VectorMap::findByKey(const Key<Gutter>& key) const
+{
+  return gutter_.findByKey(key);
+}
+
+Curb VectorMap::findByKey(const Key<Curb>& key) const
+{
+  return curb_.findByKey(key);
+}
+
+WhiteLine VectorMap::findByKey(const Key<WhiteLine>& key) const
+{
+  return white_line_.findByKey(key);
+}
+
+StopLine VectorMap::findByKey(const Key<StopLine>& key) const
+{
+  return stop_line_.findByKey(key);
+}
+
+ZebraZone VectorMap::findByKey(const Key<ZebraZone>& key) const
+{
+  return zebra_zone_.findByKey(key);
+}
+
+CrossWalk VectorMap::findByKey(const Key<CrossWalk>& key) const
+{
+  return cross_walk_.findByKey(key);
+}
+
+RoadMark VectorMap::findByKey(const Key<RoadMark>& key) const
+{
+  return road_mark_.findByKey(key);
+}
+
+RoadPole VectorMap::findByKey(const Key<RoadPole>& key) const
+{
+  return road_pole_.findByKey(key);
+}
+
+RoadSign VectorMap::findByKey(const Key<RoadSign>& key) const
+{
+  return road_sign_.findByKey(key);
+}
+
+Signal VectorMap::findByKey(const Key<Signal>& key) const
+{
+  return signal_.findByKey(key);
+}
+
+StreetLight VectorMap::findByKey(const Key<StreetLight>& key) const
+{
+  return street_light_.findByKey(key);
+}
+
+UtilityPole VectorMap::findByKey(const Key<UtilityPole>& key) const
+{
+  return utility_pole_.findByKey(key);
+}
+
+GuardRail VectorMap::findByKey(const Key<GuardRail>& key) const
+{
+  return guard_rail_.findByKey(key);
+}
+
+SideWalk VectorMap::findByKey(const Key<SideWalk>& id) const
+{
+  return side_walk_.findByKey(id);
+}
+
+DriveOnPortion VectorMap::findByKey(const Key<DriveOnPortion>& key) const
+{
+  return drive_on_portion_.findByKey(key);
+}
+
+CrossRoad VectorMap::findByKey(const Key<CrossRoad>& key) const
+{
+  return cross_road_.findByKey(key);
+}
+
+SideStrip VectorMap::findByKey(const Key<SideStrip>& id) const
+{
+  return side_strip_.findByKey(id);
+}
+
+CurveMirror VectorMap::findByKey(const Key<CurveMirror>& key) const
+{
+  return curve_mirror_.findByKey(key);
+}
+
+Wall VectorMap::findByKey(const Key<Wall>& key) const
+{
+  return wall_.findByKey(key);
+}
+
+Fence VectorMap::findByKey(const Key<Fence>& key) const
+{
+  return fence_.findByKey(key);
+}
+
+RailCrossing VectorMap::findByKey(const Key<RailCrossing>& key) const
+{
+  return rail_crossing_.findByKey(key);
+}
+
+std::vector<Point> VectorMap::findByFilter(const Filter<Point>& filter) const
+{
+  return point_.findByFilter(filter);
+}
+
+std::vector<Vector> VectorMap::findByFilter(const Filter<Vector>& filter) const
+{
+  return vector_.findByFilter(filter);
+}
+
+std::vector<Line> VectorMap::findByFilter(const Filter<Line>& filter) const
+{
+  return line_.findByFilter(filter);
+}
+
+std::vector<Area> VectorMap::findByFilter(const Filter<Area>& filter) const
+{
+  return area_.findByFilter(filter);
+}
+
+std::vector<Pole> VectorMap::findByFilter(const Filter<Pole>& filter) const
+{
+  return pole_.findByFilter(filter);
+}
+
+std::vector<Box> VectorMap::findByFilter(const Filter<Box>& filter) const
+{
+  return box_.findByFilter(filter);
+}
+
+std::vector<DTLane> VectorMap::findByFilter(const Filter<DTLane>& filter) const
+{
+  return dtlane_.findByFilter(filter);
+}
+
+std::vector<Node> VectorMap::findByFilter(const Filter<Node>& filter) const
+{
+  return node_.findByFilter(filter);
+}
+
+std::vector<Lane> VectorMap::findByFilter(const Filter<Lane>& filter) const
+{
+  return lane_.findByFilter(filter);
+}
+
+std::vector<WayArea> VectorMap::findByFilter(const Filter<WayArea>& filter) const
+{
+  return way_area_.findByFilter(filter);
+}
+
+std::vector<RoadEdge> VectorMap::findByFilter(const Filter<RoadEdge>& filter) const
+{
+  return road_edge_.findByFilter(filter);
+}
+
+std::vector<Gutter> VectorMap::findByFilter(const Filter<Gutter>& filter) const
+{
+  return gutter_.findByFilter(filter);
+}
+
+std::vector<Curb> VectorMap::findByFilter(const Filter<Curb>& filter) const
+{
+  return curb_.findByFilter(filter);
+}
+
+std::vector<WhiteLine> VectorMap::findByFilter(const Filter<WhiteLine>& filter) const
+{
+  return white_line_.findByFilter(filter);
+}
+
+std::vector<StopLine> VectorMap::findByFilter(const Filter<StopLine>& filter) const
+{
+  return stop_line_.findByFilter(filter);
+}
+
+std::vector<ZebraZone> VectorMap::findByFilter(const Filter<ZebraZone>& filter) const
+{
+  return zebra_zone_.findByFilter(filter);
+}
+
+std::vector<CrossWalk> VectorMap::findByFilter(const Filter<CrossWalk>& filter) const
+{
+  return cross_walk_.findByFilter(filter);
+}
+
+std::vector<RoadMark> VectorMap::findByFilter(const Filter<RoadMark>& filter) const
+{
+  return road_mark_.findByFilter(filter);
+}
+
+std::vector<RoadPole> VectorMap::findByFilter(const Filter<RoadPole>& filter) const
+{
+  return road_pole_.findByFilter(filter);
+}
+
+std::vector<RoadSign> VectorMap::findByFilter(const Filter<RoadSign>& filter) const
+{
+  return road_sign_.findByFilter(filter);
+}
+
+std::vector<Signal> VectorMap::findByFilter(const Filter<Signal>& filter) const
+{
+  return signal_.findByFilter(filter);
+}
+
+std::vector<StreetLight> VectorMap::findByFilter(const Filter<StreetLight>& filter) const
+{
+  return street_light_.findByFilter(filter);
+}
+
+std::vector<UtilityPole> VectorMap::findByFilter(const Filter<UtilityPole>& filter) const
+{
+  return utility_pole_.findByFilter(filter);
+}
+
+std::vector<GuardRail> VectorMap::findByFilter(const Filter<GuardRail>& filter) const
+{
+  return guard_rail_.findByFilter(filter);
+}
+
+std::vector<SideWalk> VectorMap::findByFilter(const Filter<SideWalk>& filter) const
+{
+  return side_walk_.findByFilter(filter);
+}
+
+std::vector<DriveOnPortion> VectorMap::findByFilter(const Filter<DriveOnPortion>& filter) const
+{
+  return drive_on_portion_.findByFilter(filter);
+}
+
+std::vector<CrossRoad> VectorMap::findByFilter(const Filter<CrossRoad>& filter) const
+{
+  return cross_road_.findByFilter(filter);
+}
+
+std::vector<SideStrip> VectorMap::findByFilter(const Filter<SideStrip>& filter) const
+{
+  return side_strip_.findByFilter(filter);
+}
+
+std::vector<CurveMirror> VectorMap::findByFilter(const Filter<CurveMirror>& filter) const
+{
+  return curve_mirror_.findByFilter(filter);
+}
+
+std::vector<Wall> VectorMap::findByFilter(const Filter<Wall>& filter) const
+{
+  return wall_.findByFilter(filter);
+}
+
+std::vector<Fence> VectorMap::findByFilter(const Filter<Fence>& filter) const
+{
+  return fence_.findByFilter(filter);
+}
+
+std::vector<RailCrossing> VectorMap::findByFilter(const Filter<RailCrossing>& filter) const
+{
+  return rail_crossing_.findByFilter(filter);
+}
+
+void VectorMap::registerCallback(const Callback<PointArray>& cb)
+{
+  point_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<VectorArray>& cb)
+{
+  vector_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<LineArray>& cb)
+{
+  line_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<AreaArray>& cb)
+{
+  area_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<PoleArray>& cb)
+{
+  pole_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<BoxArray>& cb)
+{
+  box_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<DTLaneArray>& cb)
+{
+  dtlane_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<NodeArray>& cb)
+{
+  node_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<LaneArray>& cb)
+{
+  lane_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<WayAreaArray>& cb)
+{
+  way_area_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<RoadEdgeArray>& cb)
+{
+  road_edge_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<GutterArray>& cb)
+{
+  gutter_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<CurbArray>& cb)
+{
+  curb_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<WhiteLineArray>& cb)
+{
+  white_line_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<StopLineArray>& cb)
+{
+  stop_line_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<ZebraZoneArray>& cb)
+{
+  zebra_zone_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<CrossWalkArray>& cb)
+{
+  cross_walk_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<RoadMarkArray>& cb)
+{
+  road_mark_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<RoadPoleArray>& cb)
+{
+  road_pole_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<RoadSignArray>& cb)
+{
+  road_sign_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<SignalArray>& cb)
+{
+  signal_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<StreetLightArray>& cb)
+{
+  street_light_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<UtilityPoleArray>& cb)
+{
+  utility_pole_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<GuardRailArray>& cb)
+{
+  guard_rail_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<SideWalkArray>& cb)
+{
+  side_walk_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<DriveOnPortionArray>& cb)
+{
+  drive_on_portion_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<CrossRoadArray>& cb)
+{
+  cross_road_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<SideStripArray>& cb)
+{
+  side_strip_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<CurveMirrorArray>& cb)
+{
+  curve_mirror_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<WallArray>& cb)
+{
+  wall_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<FenceArray>& cb)
+{
+  fence_.registerCallback(cb);
+}
+
+void VectorMap::registerCallback(const Callback<RailCrossingArray>& cb)
+{
+  rail_crossing_.registerCallback(cb);
+}
+
+const double COLOR_VALUE_MIN = 0.0;
+const double COLOR_VALUE_MAX = 1.0;
+const double COLOR_VALUE_MEDIAN = 0.5;
+const double COLOR_VALUE_LIGHT_LOW = 0.56;
+const double COLOR_VALUE_LIGHT_HIGH = 0.93;
+
+std_msgs::ColorRGBA createColorRGBA(Color color)
+{
+  std_msgs::ColorRGBA color_rgba;
+  color_rgba.r = COLOR_VALUE_MIN;
+  color_rgba.g = COLOR_VALUE_MIN;
+  color_rgba.b = COLOR_VALUE_MIN;
+  color_rgba.a = COLOR_VALUE_MAX;
+
+  switch (color)
+  {
+  case BLACK:
+    break;
+  case GRAY:
+    color_rgba.r = COLOR_VALUE_MEDIAN;
+    color_rgba.g = COLOR_VALUE_MEDIAN;
+    color_rgba.b = COLOR_VALUE_MEDIAN;
+    break;
+  case LIGHT_RED:
+    color_rgba.r = COLOR_VALUE_LIGHT_HIGH;
+    color_rgba.g = COLOR_VALUE_LIGHT_LOW;
+    color_rgba.b = COLOR_VALUE_LIGHT_LOW;
+    break;
+  case LIGHT_GREEN:
+    color_rgba.r = COLOR_VALUE_LIGHT_LOW;
+    color_rgba.g = COLOR_VALUE_LIGHT_HIGH;
+    color_rgba.b = COLOR_VALUE_LIGHT_LOW;
+    break;
+  case LIGHT_BLUE:
+    color_rgba.r = COLOR_VALUE_LIGHT_LOW;
+    color_rgba.g = COLOR_VALUE_LIGHT_LOW;
+    color_rgba.b = COLOR_VALUE_LIGHT_HIGH;
+    break;
+  case LIGHT_YELLOW:
+    color_rgba.r = COLOR_VALUE_LIGHT_HIGH;
+    color_rgba.g = COLOR_VALUE_LIGHT_HIGH;
+    color_rgba.b = COLOR_VALUE_LIGHT_LOW;
+    break;
+  case LIGHT_CYAN:
+    color_rgba.r = COLOR_VALUE_LIGHT_LOW;
+    color_rgba.g = COLOR_VALUE_LIGHT_HIGH;
+    color_rgba.b = COLOR_VALUE_LIGHT_HIGH;
+    break;
+  case LIGHT_MAGENTA:
+    color_rgba.r = COLOR_VALUE_LIGHT_HIGH;
+    color_rgba.g = COLOR_VALUE_LIGHT_LOW;
+    color_rgba.b = COLOR_VALUE_LIGHT_HIGH;
+    break;
+  case RED:
+    color_rgba.r = COLOR_VALUE_MAX;
+    break;
+  case GREEN:
+    color_rgba.g = COLOR_VALUE_MAX;
+    break;
+  case BLUE:
+    color_rgba.b = COLOR_VALUE_MAX;
+    break;
+  case YELLOW:
+    color_rgba.r = COLOR_VALUE_MAX;
+    color_rgba.g = COLOR_VALUE_MAX;
+    break;
+  case CYAN:
+    color_rgba.g = COLOR_VALUE_MAX;
+    color_rgba.b = COLOR_VALUE_MAX;
+    break;
+  case MAGENTA:
+    color_rgba.r = COLOR_VALUE_MAX;
+    color_rgba.b = COLOR_VALUE_MAX;
+    break;
+  case WHITE:
+    color_rgba.r = COLOR_VALUE_MAX;
+    color_rgba.g = COLOR_VALUE_MAX;
+    color_rgba.b = COLOR_VALUE_MAX;
+    break;
+  default:
+    color_rgba.a = COLOR_VALUE_MIN;  // hide color from view
+    break;
+  }
+
+  return color_rgba;
+}
+
+void enableMarker(visualization_msgs::Marker& marker)
+{
+  marker.action = visualization_msgs::Marker::ADD;
+}
+
+void disableMarker(visualization_msgs::Marker& marker)
+{
+  marker.action = visualization_msgs::Marker::DELETE;
+}
+
+bool isValidMarker(const visualization_msgs::Marker& marker)
+{
+  return marker.action == visualization_msgs::Marker::ADD;
+}
+
+extern const double MAKER_SCALE_POINT = 0.08;
+extern const double MAKER_SCALE_VECTOR = 0.08;
+extern const double MAKER_SCALE_VECTOR_LENGTH = 0.64;
+extern const double MAKER_SCALE_LINE = 0.08;
+extern const double MAKER_SCALE_AREA = 0.08;
+extern const double MAKER_SCALE_BOX = 0.08;
+
+visualization_msgs::Marker createMarker(const std::string& ns, int id, int type)
+{
+  visualization_msgs::Marker marker;
+  // NOTE: Autoware want to use map messages with or without /use_sim_time.
+  // Therefore we don't set marker.header.stamp.
+  // marker.header.stamp = ros::Time::now();
+  marker.header.frame_id = "map";
+  marker.ns = ns;
+  marker.id = id;
+  marker.type = type;
+  marker.lifetime = ros::Duration();
+  marker.frame_locked = true;
+  disableMarker(marker);
+  return marker;
+}
+
+visualization_msgs::Marker createPointMarker(const std::string& ns, int id, Color color, const Point& point)
+{
+  visualization_msgs::Marker marker = createMarker(ns, id, visualization_msgs::Marker::SPHERE);
+  if (point.pid == 0)
+    return marker;
+
+  marker.pose.position = convertPointToGeomPoint(point);
+  marker.pose.orientation = convertVectorToGeomQuaternion(Vector());
+  marker.scale.x = MAKER_SCALE_POINT;
+  marker.scale.y = MAKER_SCALE_POINT;
+  marker.scale.z = MAKER_SCALE_POINT;
+  marker.color = createColorRGBA(color);
+  if (marker.color.a == COLOR_VALUE_MIN)
+    return marker;
+
+  enableMarker(marker);
+  return marker;
+}
+
+visualization_msgs::Marker createVectorMarker(const std::string& ns, int id, Color color, const VectorMap& vmap,
+                                              const Vector& vector)
+{
+  visualization_msgs::Marker marker = createMarker(ns, id, visualization_msgs::Marker::ARROW);
+  if (vector.vid == 0)
+    return marker;
+
+  Point point = vmap.findByKey(Key<Point>(vector.pid));
+  if (point.pid == 0)
+    return marker;
+
+  marker.pose.position = convertPointToGeomPoint(point);
+  marker.pose.orientation = convertVectorToGeomQuaternion(vector);
+  marker.scale.x = MAKER_SCALE_VECTOR_LENGTH;
+  marker.scale.y = MAKER_SCALE_VECTOR;
+  marker.scale.z = MAKER_SCALE_VECTOR;
+  marker.color = createColorRGBA(color);
+  if (marker.color.a == COLOR_VALUE_MIN)
+    return marker;
+
+  enableMarker(marker);
+  return marker;
+}
+
+visualization_msgs::Marker createLineMarker(const std::string& ns, int id, Color color, const VectorMap& vmap,
+                                            const Line& line)
+{
+  visualization_msgs::Marker marker = createMarker(ns, id, visualization_msgs::Marker::LINE_STRIP);
+  if (line.lid == 0)
+    return marker;
+
+  Point bp = vmap.findByKey(Key<Point>(line.bpid));
+  if (bp.pid == 0)
+    return marker;
+
+  Point fp = vmap.findByKey(Key<Point>(line.fpid));
+  if (fp.pid == 0)
+    return marker;
+
+  marker.points.push_back(convertPointToGeomPoint(bp));
+  marker.points.push_back(convertPointToGeomPoint(fp));
+
+  marker.scale.x = MAKER_SCALE_LINE;
+  marker.color = createColorRGBA(color);
+  if (marker.color.a == COLOR_VALUE_MIN)
+    return marker;
+
+  enableMarker(marker);
+  return marker;
+}
+
+visualization_msgs::Marker createAreaMarker(const std::string& ns, int id, Color color, const VectorMap& vmap,
+                                            const Area& area)
+{
+  visualization_msgs::Marker marker = createMarker(ns, id, visualization_msgs::Marker::LINE_STRIP);
+  if (area.aid == 0)
+    return marker;
+
+  Line line = vmap.findByKey(Key<Line>(area.slid));
+  if (line.lid == 0)
+    return marker;
+  if (line.blid != 0)  // must set beginning line
+    return marker;
+
+  while (line.flid != 0)
+  {
+    Point bp = vmap.findByKey(Key<Point>(line.bpid));
+    if (bp.pid == 0)
+      return marker;
+
+    Point fp = vmap.findByKey(Key<Point>(line.fpid));
+    if (fp.pid == 0)
+      return marker;
+
+    marker.points.push_back(convertPointToGeomPoint(bp));
+    marker.points.push_back(convertPointToGeomPoint(fp));
+
+    line = vmap.findByKey(Key<Line>(line.flid));
+    if (line.lid == 0)
+      return marker;
+  }
+
+  Point bp = vmap.findByKey(Key<Point>(line.bpid));
+  if (bp.pid == 0)
+    return marker;
+
+  Point fp = vmap.findByKey(Key<Point>(line.fpid));
+  if (fp.pid == 0)
+    return marker;
+
+  marker.points.push_back(convertPointToGeomPoint(bp));
+  marker.points.push_back(convertPointToGeomPoint(fp));
+
+  marker.scale.x = MAKER_SCALE_AREA;
+  marker.color = createColorRGBA(color);
+  if (marker.color.a == COLOR_VALUE_MIN)
+    return marker;
+
+  enableMarker(marker);
+  return marker;
+}
+
+visualization_msgs::Marker createPoleMarker(const std::string& ns, int id, Color color, const VectorMap& vmap,
+                                            const Pole& pole)
+{
+  visualization_msgs::Marker marker = createMarker(ns, id, visualization_msgs::Marker::CYLINDER);
+  if (pole.plid == 0)
+    return marker;
+  // XXX: The following conditions are workaround for pole.csv of Nagoya University's campus.
+  if (pole.length == 0 || pole.dim == 0)
+    return marker;
+
+  Vector vector = vmap.findByKey(Key<Vector>(pole.vid));
+  if (vector.vid == 0)
+    return marker;
+  // XXX: The visualization_msgs::Marker::CYLINDER is difficult to display other than vertical pole.
+  if (vector.vang != 0)
+    return marker;
+
+  Point point = vmap.findByKey(Key<Point>(vector.pid));
+  if (point.pid == 0)
+    return marker;
+
+  geometry_msgs::Point geom_point = convertPointToGeomPoint(point);
+  geom_point.z += pole.length / 2;
+  marker.pose.position = geom_point;
+  vector.vang -= 90;
+  marker.pose.orientation = convertVectorToGeomQuaternion(vector);
+  marker.scale.x = pole.dim;
+  marker.scale.y = pole.dim;
+  marker.scale.z = pole.length;
+  marker.color = createColorRGBA(color);
+  if (marker.color.a == COLOR_VALUE_MIN)
+    return marker;
+
+  enableMarker(marker);
+  return marker;
+}
+
+visualization_msgs::Marker createBoxMarker(const std::string& ns, int id, Color color, const VectorMap& vmap,
+                                           const Box& box)
+{
+  visualization_msgs::Marker marker = createMarker(ns, id, visualization_msgs::Marker::LINE_STRIP);
+  if (box.bid == 0)
+    return marker;
+
+  Point p1 = vmap.findByKey(Key<Point>(box.pid1));
+  if (p1.pid == 0)
+    return marker;
+
+  Point p2 = vmap.findByKey(Key<Point>(box.pid2));
+  if (p2.pid == 0)
+    return marker;
+
+  Point p3 = vmap.findByKey(Key<Point>(box.pid3));
+  if (p3.pid == 0)
+    return marker;
+
+  Point p4 = vmap.findByKey(Key<Point>(box.pid4));
+  if (p4.pid == 0)
+    return marker;
+
+  std::vector<geometry_msgs::Point> bottom_points(4);
+  bottom_points[0] = convertPointToGeomPoint(p1);
+  bottom_points[1] = convertPointToGeomPoint(p2);
+  bottom_points[2] = convertPointToGeomPoint(p3);
+  bottom_points[3] = convertPointToGeomPoint(p4);
+
+  std::vector<geometry_msgs::Point> top_points(4);
+  for (size_t i = 0; i < 4; ++i)
+  {
+    top_points[i] = bottom_points[i];
+    top_points[i].z += box.height;
+  }
+
+  for (size_t i = 0; i < 4; ++i)
+  {
+    marker.points.push_back(bottom_points[i]);
+    marker.points.push_back(top_points[i]);
+    marker.points.push_back(top_points[i]);
+    if (i != 3)
+    {
+      marker.points.push_back(top_points[i + 1]);
+      marker.points.push_back(top_points[i + 1]);
+      marker.points.push_back(bottom_points[i + 1]);
+    }
+    else
+    {
+      marker.points.push_back(top_points[0]);
+      marker.points.push_back(top_points[0]);
+      marker.points.push_back(bottom_points[0]);
+    }
+  }
+  for (size_t i = 0; i < 4; ++i)
+  {
+    marker.points.push_back(bottom_points[i]);
+    if (i != 3)
+      marker.points.push_back(bottom_points[i + 1]);
+    else
+      marker.points.push_back(bottom_points[0]);
+  }
+
+  marker.scale.x = MAKER_SCALE_BOX;
+  marker.color = createColorRGBA(color);
+  if (marker.color.a == COLOR_VALUE_MIN)
+    return marker;
+
+  enableMarker(marker);
+  return marker;
+}
+
+double convertDegreeToRadian(double degree)
+{
+  return degree * M_PI / 180;
+}
+
+double convertRadianToDegree(double radian)
+{
+  return radian * 180 / M_PI;
+}
+
+geometry_msgs::Point convertPointToGeomPoint(const Point& point)
+{
+  geometry_msgs::Point geom_point;
+  // NOTE: Autwoare use Japan Plane Rectangular Coordinate System.
+  // Therefore we swap x and y axis.
+  geom_point.x = point.ly;
+  geom_point.y = point.bx;
+  geom_point.z = point.h;
+  return geom_point;
+}
+
+Point convertGeomPointToPoint(const geometry_msgs::Point& geom_point)
+{
+  Point point;
+  // NOTE: Autwoare use Japan Plane Rectangular Coordinate System.
+  // Therefore we swap x and y axis.
+  point.bx = geom_point.y;
+  point.ly = geom_point.x;
+  point.h = geom_point.z;
+  return point;
+}
+
+geometry_msgs::Quaternion convertVectorToGeomQuaternion(const Vector& vector)
+{
+  double pitch = convertDegreeToRadian(vector.vang - 90);  // convert vertical angle to pitch
+  double yaw = convertDegreeToRadian(-vector.hang + 90);   // convert horizontal angle to yaw
+  return tf::createQuaternionMsgFromRollPitchYaw(0, pitch, yaw);
+}
+
+Vector convertGeomQuaternionToVector(const geometry_msgs::Quaternion& geom_quaternion)
+{
+  tf::Quaternion quaternion(geom_quaternion.x, geom_quaternion.y, geom_quaternion.z, geom_quaternion.w);
+  double roll, pitch, yaw;
+  tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
+  Vector vector;
+  vector.vang = convertRadianToDegree(pitch) + 90;
+  vector.hang = -convertRadianToDegree(yaw) + 90;
+  return vector;
+}
+}  // namespace vector_map
+
+std::ostream& operator<<(std::ostream& os, const vector_map::Point& obj)
+{
+  os << obj.pid << ","
+     << obj.b << ","
+     << obj.l << ","
+     << obj.h << ","
+     << obj.bx << ","
+     << obj.ly << ","
+     << obj.ref << ","
+     << obj.mcode1 << ","
+     << obj.mcode2 << ","
+     << obj.mcode3;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::Vector& obj)
+{
+  os << obj.vid << ","
+     << obj.pid << ","
+     << obj.hang << ","
+     << obj.vang;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::Line& obj)
+{
+  os << obj.lid << ","
+     << obj.bpid << ","
+     << obj.fpid << ","
+     << obj.blid << ","
+     << obj.flid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::Area& obj)
+{
+  os << obj.aid << ","
+     << obj.slid << ","
+     << obj.elid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::Pole& obj)
+{
+  os << obj.plid << ","
+     << obj.vid << ","
+     << obj.length << ","
+     << obj.dim;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::Box& obj)
+{
+  os << obj.bid << ","
+     << obj.pid1 << ","
+     << obj.pid2 << ","
+     << obj.pid3 << ","
+     << obj.pid4 << ","
+     << obj.height;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::DTLane& obj)
+{
+  os << obj.did << ","
+     << obj.dist << ","
+     << obj.pid << ","
+     << obj.dir << ","
+     << obj.apara << ","
+     << obj.r << ","
+     << obj.slope << ","
+     << obj.cant << ","
+     << obj.lw << ","
+     << obj.rw;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::Node& obj)
+{
+  os << obj.nid << ","
+     << obj.pid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::Lane& obj)
+{
+  os << obj.lnid << ","
+     << obj.did << ","
+     << obj.blid << ","
+     << obj.flid << ","
+     << obj.bnid << ","
+     << obj.fnid << ","
+     << obj.jct << ","
+     << obj.blid2 << ","
+     << obj.blid3 << ","
+     << obj.blid4 << ","
+     << obj.flid2 << ","
+     << obj.flid3 << ","
+     << obj.flid4 << ","
+     << obj.clossid << ","
+     << obj.span << ","
+     << obj.lcnt << ","
+     << obj.lno << ","
+     << obj.lanetype << ","
+     << obj.limitvel << ","
+     << obj.refvel << ","
+     << obj.roadsecid << ","
+     << obj.lanecfgfg << ","
+     << obj.linkwaid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::WayArea& obj)
+{
+  os << obj.waid << ","
+     << obj.aid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::RoadEdge& obj)
+{
+  os << obj.id << ","
+     << obj.lid << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::Gutter& obj)
+{
+  os << obj.id << ","
+     << obj.aid << ","
+     << obj.type << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::Curb& obj)
+{
+  os << obj.id << ","
+     << obj.lid << ","
+     << obj.height << ","
+     << obj.width << ","
+     << obj.dir << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::WhiteLine& obj)
+{
+  os << obj.id << ","
+     << obj.lid << ","
+     << obj.width << ","
+     << obj.color << ","
+     << obj.type << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::StopLine& obj)
+{
+  os << obj.id << ","
+     << obj.lid << ","
+     << obj.tlid << ","
+     << obj.signid << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::ZebraZone& obj)
+{
+  os << obj.id << ","
+     << obj.aid << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::CrossWalk& obj)
+{
+  os << obj.id << ","
+     << obj.aid << ","
+     << obj.type << ","
+     << obj.bdid << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::RoadMark& obj)
+{
+  os << obj.id << ","
+     << obj.aid << ","
+     << obj.type << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::RoadPole& obj)
+{
+  os << obj.id << ","
+     << obj.plid << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::RoadSign& obj)
+{
+  os << obj.id << ","
+     << obj.vid << ","
+     << obj.plid << ","
+     << obj.type << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::Signal& obj)
+{
+  os << obj.id << ","
+     << obj.vid << ","
+     << obj.plid << ","
+     << obj.type << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::StreetLight& obj)
+{
+  os << obj.id << ","
+     << obj.lid << ","
+     << obj.plid << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::UtilityPole& obj)
+{
+  os << obj.id << ","
+     << obj.plid << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::GuardRail& obj)
+{
+  os << obj.id << ","
+     << obj.aid << ","
+     << obj.type << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::SideWalk& obj)
+{
+  os << obj.id << ","
+     << obj.aid << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::DriveOnPortion& obj)
+{
+  os << obj.id << ","
+     << obj.aid << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::CrossRoad& obj)
+{
+  os << obj.id << ","
+     << obj.aid << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::SideStrip& obj)
+{
+  os << obj.id << ","
+     << obj.lid << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::CurveMirror& obj)
+{
+  os << obj.id << ","
+     << obj.vid << ","
+     << obj.plid << ","
+     << obj.type << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::Wall& obj)
+{
+  os << obj.id << ","
+     << obj.aid << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::Fence& obj)
+{
+  os << obj.id << ","
+     << obj.aid << ","
+     << obj.linkid;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const vector_map::RailCrossing& obj)
+{
+  os << obj.id << ","
+     << obj.aid << ","
+     << obj.linkid;
+  return os;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::Point& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.pid = std::stoi(columns[0]);
+  obj.b = std::stod(columns[1]);
+  obj.l = std::stod(columns[2]);
+  obj.h = std::stod(columns[3]);
+  obj.bx = std::stod(columns[4]);
+  obj.ly = std::stod(columns[5]);
+  obj.ref = std::stoi(columns[6]);
+  obj.mcode1 = std::stoi(columns[7]);
+  obj.mcode2 = std::stoi(columns[8]);
+  obj.mcode3 = std::stoi(columns[9]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::Vector& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.vid = std::stoi(columns[0]);
+  obj.pid = std::stoi(columns[1]);
+  obj.hang = std::stod(columns[2]);
+  obj.vang = std::stod(columns[3]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::Line& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.lid = std::stoi(columns[0]);
+  obj.bpid = std::stoi(columns[1]);
+  obj.fpid = std::stoi(columns[2]);
+  obj.blid = std::stoi(columns[3]);
+  obj.flid = std::stoi(columns[4]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::Area& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.aid = std::stoi(columns[0]);
+  obj.slid = std::stoi(columns[1]);
+  obj.elid = std::stoi(columns[2]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::Pole& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.plid = std::stoi(columns[0]);
+  obj.vid = std::stoi(columns[1]);
+  obj.length = std::stod(columns[2]);
+  obj.dim = std::stod(columns[3]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::Box& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.bid = std::stoi(columns[0]);
+  obj.pid1 = std::stoi(columns[1]);
+  obj.pid2 = std::stoi(columns[2]);
+  obj.pid3 = std::stoi(columns[3]);
+  obj.pid4 = std::stoi(columns[4]);
+  obj.height = std::stod(columns[5]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::DTLane& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.did = std::stoi(columns[0]);
+  obj.dist = std::stod(columns[1]);
+  obj.pid = std::stoi(columns[2]);
+  obj.dir = std::stod(columns[3]);
+  obj.apara = std::stod(columns[4]);
+  obj.r = std::stod(columns[5]);
+  obj.slope = std::stod(columns[6]);
+  obj.cant = std::stod(columns[7]);
+  obj.lw = std::stod(columns[8]);
+  obj.rw = std::stod(columns[9]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::Node& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.nid = std::stoi(columns[0]);
+  obj.pid = std::stoi(columns[1]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::Lane& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  int n = 0;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+    ++n;
+  }
+  obj.lnid = std::stoi(columns[0]);
+  obj.did = std::stoi(columns[1]);
+  obj.blid = std::stoi(columns[2]);
+  obj.flid = std::stoi(columns[3]);
+  obj.bnid = std::stoi(columns[4]);
+  obj.fnid = std::stoi(columns[5]);
+  obj.jct = std::stoi(columns[6]);
+  obj.blid2 = std::stoi(columns[7]);
+  obj.blid3 = std::stoi(columns[8]);
+  obj.blid4 = std::stoi(columns[9]);
+  obj.flid2 = std::stoi(columns[10]);
+  obj.flid3 = std::stoi(columns[11]);
+  obj.flid4 = std::stoi(columns[12]);
+  obj.clossid = std::stoi(columns[13]);
+  obj.span = std::stod(columns[14]);
+  obj.lcnt = std::stoi(columns[15]);
+  obj.lno = std::stoi(columns[16]);
+  if (n == 17)
+  {
+    obj.lanetype = 0;
+    obj.limitvel = 0;
+    obj.refvel = 0;
+    obj.roadsecid = 0;
+    obj.lanecfgfg = 0;
+    obj.linkwaid = 0;
+    return is;
+  }
+  obj.lanetype = std::stoi(columns[17]);
+  obj.limitvel = std::stoi(columns[18]);
+  obj.refvel = std::stoi(columns[19]);
+  obj.roadsecid = std::stoi(columns[20]);
+  obj.lanecfgfg = std::stoi(columns[21]);
+  if (n == 22)
+  {
+    obj.linkwaid = 0;
+    return is;
+  }
+  obj.linkwaid = std::stoi(columns[22]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::WayArea& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.waid = std::stoi(columns[0]);
+  obj.aid = std::stoi(columns[1]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::RoadEdge& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.lid = std::stoi(columns[1]);
+  obj.linkid = std::stoi(columns[2]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::Gutter& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.aid = std::stoi(columns[1]);
+  obj.type = std::stoi(columns[2]);
+  obj.linkid = std::stoi(columns[3]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::Curb& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.lid = std::stoi(columns[1]);
+  obj.height = std::stod(columns[2]);
+  obj.width = std::stod(columns[3]);
+  obj.dir = std::stoi(columns[4]);
+  obj.linkid = std::stoi(columns[5]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::WhiteLine& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.lid = std::stoi(columns[1]);
+  obj.width = std::stod(columns[2]);
+  obj.color = columns[3].c_str()[0];
+  obj.type = std::stoi(columns[4]);
+  obj.linkid = std::stoi(columns[5]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::StopLine& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.lid = std::stoi(columns[1]);
+  obj.tlid = std::stoi(columns[2]);
+  obj.signid = std::stoi(columns[3]);
+  obj.linkid = std::stoi(columns[4]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::ZebraZone& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.aid = std::stoi(columns[1]);
+  obj.linkid = std::stoi(columns[2]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::CrossWalk& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.aid = std::stoi(columns[1]);
+  obj.type = std::stoi(columns[2]);
+  obj.bdid = std::stoi(columns[3]);
+  obj.linkid = std::stoi(columns[4]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::RoadMark& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.aid = std::stoi(columns[1]);
+  obj.type = std::stoi(columns[2]);
+  obj.linkid = std::stoi(columns[3]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::RoadPole& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.plid = std::stoi(columns[1]);
+  obj.linkid = std::stoi(columns[2]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::RoadSign& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.vid = std::stoi(columns[1]);
+  obj.plid = std::stoi(columns[2]);
+  obj.type = std::stoi(columns[3]);
+  obj.linkid = std::stoi(columns[4]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::Signal& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.vid = std::stoi(columns[1]);
+  obj.plid = std::stoi(columns[2]);
+  obj.type = std::stoi(columns[3]);
+  obj.linkid = std::stoi(columns[4]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::StreetLight& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.lid = std::stoi(columns[1]);
+  obj.plid = std::stoi(columns[2]);
+  obj.linkid = std::stoi(columns[3]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::UtilityPole& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.plid = std::stoi(columns[1]);
+  obj.linkid = std::stoi(columns[2]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::GuardRail& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.aid = std::stoi(columns[1]);
+  obj.type = std::stoi(columns[2]);
+  obj.linkid = std::stoi(columns[3]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::SideWalk& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.aid = std::stoi(columns[1]);
+  obj.linkid = std::stoi(columns[2]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::DriveOnPortion& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.aid = std::stoi(columns[1]);
+  obj.linkid = std::stoi(columns[2]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::CrossRoad& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.aid = std::stoi(columns[1]);
+  obj.linkid = std::stoi(columns[2]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::SideStrip& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.lid = std::stoi(columns[1]);
+  obj.linkid = std::stoi(columns[2]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::CurveMirror& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.vid = std::stoi(columns[1]);
+  obj.plid = std::stoi(columns[2]);
+  obj.type = std::stoi(columns[3]);
+  obj.linkid = std::stoi(columns[4]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::Wall& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.aid = std::stoi(columns[1]);
+  obj.linkid = std::stoi(columns[2]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::Fence& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.aid = std::stoi(columns[1]);
+  obj.linkid = std::stoi(columns[2]);
+  return is;
+}
+
+std::istream& operator>>(std::istream& is, vector_map::RailCrossing& obj)
+{
+  std::vector<std::string> columns;
+  std::string column;
+  while (std::getline(is, column, ','))
+  {
+    columns.push_back(column);
+  }
+  obj.id = std::stoi(columns[0]);
+  obj.aid = std::stoi(columns[1]);
+  obj.linkid = std::stoi(columns[2]);
+  return is;
+}

src/ros/catkin/src/vector_map_converter/CMakeLists.txt

@@ -1,85 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(vector_map_converter)
-
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
-
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
-  amathutils_lib
-  autoware_map
-  autoware_map_msgs
-  geodesy
-  geometry_msgs
-  lanelet2_core
-  lanelet2_io
-  lanelet2_projection
-  lanelet2_routing
-  lanelet2_traffic_rules
-  roscpp
-  vector_map
-  op_planner
-)
-
-catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES vector_map_converter
-#  CATKIN_DEPENDS autoware_map geodesy geometry_msgs lanelet2_core lanelet2_io lanelet2_projection lanelet2_routing lanelet2_traffic_rules roscpp vector_map
-#  DEPENDS system_lib
-)
-
-if(${CMAKE_VERSION} VERSION_LESS "3.8.0")
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17")
-else()
-  set(CMAKE_CXX_STANDARD 17)
-endif()
-
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
-)
-
-add_executable(autowaremap2vectormap src/autoware2vectormap.cpp src/autoware2vectormap_core.cpp)
-add_executable(lanelet2autowaremap src/lanelet2autowaremap.cpp src/lanelet2autowaremap_core.cpp src/autoware2vectormap_core.cpp)
-add_executable(lanelet2vectormap src/lanelet2vectormap.cpp src/lanelet2vectormap_core.cpp src/lanelet2autowaremap_core.cpp src/autoware2vectormap_core.cpp)
-
-add_executable(opendrive2autowaremap 
-  src/opendrive2autowaremap_core/opendrive_loader.cpp
-  src/opendrive2autowaremap_core/map_writer.cpp
-  src/opendrive2autowaremap_core/xml_helpers.cpp
-  src/opendrive2autowaremap_core/odr_spiral.cpp
-  src/opendrive2autowaremap_core/opendrive_road.cpp
-  src/opendrive2autowaremap_core/opendrive_objects.cpp
-  src/opendrive2autowaremap.cpp 
-)
-
-add_dependencies(autowaremap2vectormap ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-add_dependencies(lanelet2autowaremap ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-add_dependencies(lanelet2vectormap ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-add_dependencies(opendrive2autowaremap ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-target_link_libraries(autowaremap2vectormap ${catkin_LIBRARIES})
-target_link_libraries(lanelet2autowaremap ${catkin_LIBRARIES})
-target_link_libraries(lanelet2vectormap ${catkin_LIBRARIES})
-target_link_libraries(opendrive2autowaremap ${catkin_LIBRARIES})
-
-## Install executables and/or libraries
-install(TARGETS
-        autowaremap2vectormap
-        lanelet2autowaremap
-        lanelet2vectormap
-        opendrive2autowaremap
-    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-        )
-
-## Install project namespaced headers
-install(DIRECTORY include/${PROJECT_NAME}/
-        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
-
-#install(DIRECTORY launch/
-#        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
-#        PATTERN ".svn" EXCLUDE)

src/ros/catkin/src/vector_map_converter/README.md

@@ -1,169 +0,0 @@
-# Vector Map Converter
-This package contains converters to convert different map formats into Vector Map information used by Autoware.
-
-## opendrive2autoware_converter
-
-opendrive2autoware_converter is a rosnode that convert OpenDRIVE map format (.xodr) to autoware map format (list of .csv files).
-
-### How to use: 
-example:
-```
-rosrun vector_map_converter opendrive2autowaremap _map_file:=sample1.1.xodr _country_codes_dir:=~/Autoware/src/autoware/utilities/vector_map_converter/countries/ _save_dir:=autoware_map/ _resolution:=0.5 _keep_right:=True
-```
-Mandatory ROS Parameters:
-+ ~map\_file: file path, source OpenDRIVE map (.xodr) file. 
-+ ~country\_codes\_dir: folder path, country code definition files, for traffic signs and road marks.
-
-Optional ROS parameter
-+ ~save\_dir: folder path, destination folder to write the autoware map .csv files. (default is current working directory)
-+ ~resolution: float, optional, waypoints resolution, default is 0.5
-+ ~keep\_right: bool, optional(default: True), specify whether car should drive in right lane or left lane. 
-
-## autowaremap2vectormap node
-This node converts Autoware Map Format into vector map csv files:
-
-### Usage
-`rosrun vector_map_converter autowaremap2vectormap \_map\_dir:=[autowaremap directory]`
-
-#### Parameters:
-- ~map_dir (string, required): directory where autoware map csv files are saved
-- ~save_dir (string, default: "./"): directory to save converted map
-- ~create_whitelines (bool, default: "false"): create whiteline.csv from lane width
-- ~wayareas_from_lanes (bool, default: "false"): create wayareas from lane shapes
-
-#### Output Files
-- area.csv
-- intersection.csv
-- crosswalk.csv
-- dtlane.csv
-- lane.csv
-- line.csv
-- node.csv
-- point.csv
-- pole.csv
-- roadsign.csv
-- signaldata.csv
-- stopline.csv
-- vector.csv
-- wayarea.csv
-
-#### Brief explanation about conversion
-+ area.csv <br>
-Converted from `autoware_map/area.csv`.
-+ cross_walk.csv <br>
-Converted from `autoware_map/lane_attr_relation.csv` files.
-+ dtlane.csv <br>
-Converted from `autoware_map/waypoint_relations.csv` and other related files.   
-+ intersection.csv <br>
-Converted using `autoware_map/lane_attr_relation.csv`
-+ lane.csv <br>
-Converted from `autoware_map/waypoint_relations.csv` and other related files.   
-+ line.csv <br>
-converted from `autoware_map/area`, `autoware_map_info/waypoint.csv`, and other related topics. Contains line information for areas and stop lines
-+ node.csv<br>
-Converted from `autoware_map/waypoints.csv`
-+ pole.csv <br>
-Dummy poles created for signals
-+ signal.csv <br>
-Converted from `autoware_map/signal.csv` files
-+ stop_line.csv <br>
-Converted from `autoware_map/waypoint.csv`. Creates virtual line from yaw in `waypoint_relations.csv` and also creates dummy road sign to be linked.
-+ road_sign.csv <br>
-Dummy road signs are created for stop line
-+ vector.csv  <br>
-converted from `autoware_map/signal_lights.csv` and other related files.
-+ way_area.csv <br>
-Converted from `autoware_map/wayarea.csv`. Not tested yet.
-
-## lanelet2vectormap node
-This node converts lanelet2 map into vector map csv files used in Autoware.
-### Usage
-`rosrun vector_map_converter lanelet2vectormap _map_file:=<path to lanelet map> _origin_lat:=<origin for projection>  _origin_lon:=<origin for projection>`<br>
-For sample lanelet map:<br>
-`rosrun vector_map_converter lanelet2vectormap _map_file:=~/catkin_ws/src/vector_map_converter/lanelet2/lanelet2_maps/res/mapping_example.osm _origin_lat:=49.00331750371  _origin_lon:=8.42403027399`
-
-#### Parameters
-- ~map_file (string, required): lanelet OSM file
-- ~save_dir (string, default: "./"): directory to save converted map
-- ~origin_lat (double, required): latitude origin of the map.
-- ~origin_lon (double, required): longitude origin of the map.
-- ~wayareas_from_lanes (bool, default: "false"): create wayareas from lane shapes
-
-#### Selecting Origin
-Origin can be anywhere near your map location.
-For example, you can use lat/lon value of the first point that appears in your osm file.
-Lanelet2 default loader needs an origin to load a map and use the origin to convert it to lat/lon to xyz coordinates at loading time.
-However, the xyz coordinate will be then converted into MGRS coordinate. (x=northing and y=easting within 100km grid of MGRS)
-Therefore, the origin does not affect the final output of the converter, but user needs to specify it to satisfy the needs of lanelet2 loader.
-(TODO: implement custom projector so that user won't need to set origin in the future.)
-
-### Output Files
-- area.csv
-- intersection.csv
-- crosswalk.csv
-- dtlane.csv
-- lane.csv
-- line.csv
-- node.csv
-- point.csv
-- pole.csv
-- roadsign.csv
-- signaldata.csv
-- stopline.csv
-- vector.csv
-- wayarea.csv
-
-### Notes about conversion from lanelet2.
-- Any centerline information used during conversion is calculated using original function since the default centerline information in Lanelet2 did not have sufficient quality.
-- coordinates in point.csv is in MGRS. (bx = northing, ly = easting)
-- all dtlanes assume straight line.
-- position and shape of pole.csv does not match with real environment because lanelet do not have pole information linked to traffic light. (They are placed near the related objects though)
-- all white lines are converted as solid white lines. (no yellow lines, dashed lines, etc.)
-- wayareas are converted from lane information since lanelet do not have such class.
-- all light bulbs in traffic lights are represented as unknown color since lanelets do not contain such data(no red, green, yellow information)
-- Any intersecting lanelets are interpreted as intersections (convex hull of lanelets would be intersection area)
-
-### Limitations
-Following fields in each csv files are missing(all values are 0). Most of them should be irrelevant in current Autoware (v1.11) and shouldn't have critical issue.
-- crosswalk.csv: linkid
-- intersection.csv: linkid
-- lane.csv: roadsecid, linkwaid
-- point.csv: ref
-- stopline.csv: tlid
-
-### Brief Explanation about conversion
-+ area.csv<br>
-Created for vectormap/crosswalk.csv, vectormap/intersection.csv, vectormap/wayarea.csv
-+ crosswalk.csv<br>
-Created from lanelets with type "crosswalk".
-Since there are no area inforamtion for each stripes in Lanelet, stripes are created as grid within outer bound of crosswalk.
-+ dtlane.csv<br>
-Calculated from two consecutive points in Lanelet/Centerline.
-All dtlanes are described as straight line.
-Therefore, following fields are constant:
-+ intersection.csv<br>
-Any intersecting lanelets are interpreted as intersections.
-Convex hull of intersecting lanelets would be intersection area.
-+ lane.csv<br>
-Calculated from two consective waypoints.
-+ line.csv<br>
-Created for Aisan/stoplines, Aisan/whitelines.
-+ node.csv<br>
-Calculated from Lanelet/Centerline.
-+ point.csv<br>
-bx and ly are calculated as MGRS coordinate from WGS84 coordinate in Lanelet.
-+ pole.csv<br>
-Virtual poles are created for all road signs and signaldata.
-+ roadsign.csv <br>
-Virtual Roadsigns are created for stop lines
-+ signaldata.csv<br>
-Calculated from lanelet's traffic light regulatory elements.
-Since there are no signal orientaiton information in lanelet,  direction of related lanelet is used as direction of traffic light.
-+ stopline.csv<br>
-Calculated from lanelet's linestring with type "stopline".
-+ vector.csv<br>
-Calculated for signal lights and road signs.
-+ wayarea.csv<br>
-Calculated from lane and lane width.
-+ whiteline.csv<br>
-Calculated from rightbound and leftbound of lanelets.

src/ros/catkin/src/vector_map_converter/countries/OpenDRIVE.csv

@@ -1,4 +0,0 @@
-Defined_autoware_type,Define_autoware_sub_type,Standard_id,Standard_sub_id,comments
-TRAFFIC_LIGHT,VERTICAL_DEFAULT_LIGHT,1000001,-1,Default vertical traffic light 
-ROAD_MARK,STOP_LINE_MARK,294,-1,Stop line
-ROAD_MARK,WAITING_LINE_MARK,341,-1,waiting line 

src/ros/catkin/src/vector_map_converter/include/opendrive2autoware_converter/map_writer.h

@@ -1,56 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef OPENDRIVE2OP_CONVERTER
-#define OPENDRIVE2OP_CONVERTER
-
-#include "opendrive2autoware_converter/opendrive_road.h"
-
-namespace opendrive_converter
-{
-class MapWriter
-{
-
-public:
-	MapWriter();
-    ~MapWriter();
-    void writeAutowareMap(std::string folder_name, PlannerHNS::RoadNetwork& map);
-
-
-private:
-
-	template <class T>
-	void writeCSVFile(const std::string& folder, const std::string& title, const std::string& header, const std::vector<T>& data_list);
-	PlannerHNS::Lane* getLaneFromID(PlannerHNS::RoadNetwork& map, int _l_id)
-	{
-		for(unsigned int i=0; i<map.roadSegments.size(); i++)
-		{
-			for(unsigned int j=0; j<map.roadSegments.at(i).Lanes.size(); j++)
-			{
-				if(map.roadSegments.at(i).Lanes.at(j).points.size() > 0 &&  map.roadSegments.at(i).Lanes.at(j).id == _l_id)
-				{
-					return &map.roadSegments.at(i).Lanes.at(j);
-				}
-			}
-		}
-
-		return nullptr;
-	}
-};
-
-}
-
-#endif // OPENDRIVE2AUTOWARE_CONVERTER

src/ros/catkin/src/vector_map_converter/include/opendrive2autoware_converter/odr_spiral.h

@@ -1,35 +0,0 @@
-/* ===================================================
- *  file:       odrSpiral.c
- * ---------------------------------------------------
- *  purpose:	free sample for computing spirals
- *              in OpenDRIVE applications 
- * ---------------------------------------------------
- *  first edit:	09.03.2010 by M. Dupuis @ VIRES GmbH
- *  last mod.:  09.03.2010 by M. Dupuis @ VIRES GmbH
- * ===================================================
- 
-    Copyright 2010 VIRES Simulationstechnologie GmbH
-
-    Licensed under the Apache License, Version 2.0 (the "License");
-    you may not use this file except in compliance with the License.
-    You may obtain a copy of the License at
-
-        http://www.apache.org/licenses/LICENSE-2.0
-
-    Unless required by applicable law or agreed to in writing, software
-    distributed under the License is distributed on an "AS IS" BASIS,
-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-    See the License for the specific language governing permissions and
-    limitations under the License.
- */
- 
-/**
-* compute the actual "standard" spiral, starting with curvature 0
-* @param s      run-length along spiral
-* @param cDot   first derivative of curvature [1/m2]
-* @param x      resulting x-coordinate in spirals local co-ordinate system [m]
-* @param y      resulting y-coordinate in spirals local co-ordinate system [m]
-* @param t      tangent direction at s [rad]
-*/
-
-void odrSpiral( double s, double cDot, double *x, double *y, double *t );

src/ros/catkin/src/vector_map_converter/include/opendrive2autoware_converter/opendrive_loader.h

@@ -1,58 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef OPENDRIVE2AUTOWARE_CONVERTER
-#define OPENDRIVE2AUTOWARE_CONVERTER
-
-#include "opendrive_road.h"
-#include "dirent.h"
-
-namespace opendrive_converter
-{
-class OpenDriveLoader
-{
-
-public:
-	OpenDriveLoader();
-    ~OpenDriveLoader();
-    void getFileNameInFolder(const std::string& path, std::vector<std::string>& out_list);
-    void loadCountryCods(const std::string& codes_csv_folder);
-    void loadOpenDRIVE(const std::string& xodr_file, const std::string& codes_folder, PlannerHNS::RoadNetwork& map,double resolution = 0.5, bool keep_right = true);
-    void getMapLanes(std::vector<PlannerHNS::Lane>& all_lanes, double resolution = 0.5);
-    void getTrafficLights(std::vector<PlannerHNS::TrafficLight>& all_lights);
-    void getTrafficSigns(std::vector<PlannerHNS::TrafficSign>& all_signs);
-    void getStopLines(std::vector<PlannerHNS::StopLine>& all_stop_lines);
-    void connectRoads();
-
-private:
-		bool keep_right_;
-    std::vector<OpenDriveRoad> roads_list_;
-    std::vector<Junction> junctions_list_;
-    std::vector<std::pair<std::string, std::vector<CSV_Reader::LINE_DATA> > > country_signal_codes_;
-
-
-    std::vector<OpenDriveRoad*> getRoadsBySuccId(int _id);
-    std::vector<OpenDriveRoad*> getRoadsByPredId(int _id);
-
-    OpenDriveRoad* getRoadById(int _id);
-    Junction* getJunctionById(int _id);
-    void linkWayPoints(PlannerHNS::RoadNetwork& map);
-    void linkLanesPointers(PlannerHNS::RoadNetwork& map);
-};
-
-}
-
-#endif // OPENDRIVE2AUTOWARE_CONVERTER

src/ros/catkin/src/vector_map_converter/include/opendrive2autoware_converter/opendrive_objects.h

@@ -1,980 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef OPENDRIVE_OBJECTS
-#define OPENDRIVE_OBJECTS
-
-#include "op_planner/RoadNetwork.h"
-#include "time.h"
-#include "opendrive2autoware_converter/odr_spiral.h"
-#include "opendrive2autoware_converter/xml_helpers.h"
-
-namespace opendrive_converter
-{
-
-//enum LINK_TYPE {PREDECESSOR_LINK, SUCCESSOR_LINK, EMPTY_LINK };
-enum LINK_TYPE{ROAD_LINK, JUNCTION_LINK};
-enum CONTACT_POINT{START_POINT, END_POINT, EMPTY_POINT };
-enum GEOMETRY_TYPE {LINE_GEOMETRY, SPIRAL_GEOMETRY, ARC_GEOMETRY, POLY3_GEOMETRY,
-	PARAM_POLY3_GEOMETRY, UNKNOWN_GEOMETRY };
-enum ELEVATION_TYPE {ELEVATION_PROFILE, LATERAL_PROFILE};
-enum LANE_DIRECTION {LEFT_LANE, RIGHT_LANE, CENTER_LANE };
-enum LINE_MARKING_TYPE {BROKEN_LINE_MARK, SOLID_LINE_MARK, NONE_LINE_MARK, UNKNOWN_LINE_MARK};
-//enum OBJECT_TYPE{TRAFFIC_LIGHT_1 = 1000001, TRAFFIC_LIGHT_2 = 1000002, TRAFFIC_LIGHT_3 = 1000003, STOP_LINE_SIGNAL = 294, WAIT_LINE_SIGNAL = 341, UNKNOWN_SIGNAL = 0};
-enum OBJECT_TYPE{
-	TRAFFIC_LIGHT,
-	ROAD_SIGN,
-	ROAD_MARK,
-	UNKNOWN_OBJECT};
-
-enum TRAFFIC_LIGHT_TYPE{
-	VERTICAL_DEFAULT_LIGHT,
-	HORIZONTAL_DEFAULTLIGHT,
-	PEDESTRIAN_DEFAULT_LIGHT,
-	UNKNOWN_LIGHT
-};
-
-enum ROAD_SIGN_TYPE{
-	SPEED_LIMIT_SIGN,
-	STOP_SIGN,
-	NO_PARKING_SIGN,
-	UNKNOWN_SIGN
-};
-
-enum ROAD_MARK_TYPE{
-	STOP_LINE_MARK,
-	WAITING_LINE_MARK,
-	FORWARD_DIRECTION_MARK,
-	LEFT_DIRECTION_MARK,
-	RIGHT_DIRECTION_MARK,
-	FORWARD_LEFT_DIRECTION_MARK,
-	FORWARD_RIGHT_DIRECTION_MARK,
-	ALL_DIRECTION_MARK,
-	U_TURN_DIRECTION_MARK,
-	NO_U_TURN_DIRECTION_MARK,
-	UNKNOWN_ROAD_MARK
-};
-
-enum ORIENTATION_TYPE{SAME_DIRECTION, OPPOSIT_DIRECTION, BOTH_DIRECTIONS};
-enum ROAD_OBJECT_TYPE{PARKING_SPACE_OBJ, UNKNOWN_ROAD_OBJ};
-enum LANE_TYPE { NONE_LANE, DRIVING_LANE, STOP_LANE, SHOULDER_LANE, BIKING_LANE,
-	SIDEWALK_LANE, BORDER_LANE, RESTRICTED_LANE, PARKING_LANE, BIDIRECTIONAL_LANE,
-    MEDIAN_LANE, SPECIAL1_LANE, SPECIAL2_LANE, SPECIAL3_LANE, ROADWORKS_LANE,
-    TRAM_LANE, RAIL_LANE, ENTRY_LANE, EXIT_LANE, OFFRAMP_LANE, ONRAMP_LANE, PLANE_LANE};
-
-
-class OpenDriveHeader
-{
-
-public:
-	OpenDriveHeader()
-	{
-		rev_major_ = 0;
-		rev_minor_ = 0;
-		north_ = 0;
-		south_ = 0;
-		east_ = 0;
-		west_ = 0;
-		max_road_ = 0;
-		max_junc_ = 0;
-		max_prg_ = 0;
-		date_ = time(0);
-	}
-
-	OpenDriveHeader(TiXmlElement* main_element)
-	{
-		if(main_element != nullptr)
-		{
-			if(main_element->Attribute("revMajor") != nullptr)
-				rev_major_ =  strtol(main_element->Attribute("revMajor"), NULL, 10);
-			else
-				rev_major_ = 0;
-
-			if(main_element->Attribute("revMinor") != nullptr)
-				rev_minor_ =  strtol(main_element->Attribute("revMinor"), NULL, 10);
-			else
-				rev_minor_ = 0;
-			if(main_element->Attribute("name") != nullptr)
-				name_ = std::string(main_element->Attribute("name"));
-
-			if(main_element->Attribute("version") != nullptr)
-				version_ =  std::string(main_element->Attribute("version"));
-
-			if(main_element->Attribute("date") != nullptr)
-			{
-				struct tm _tm;
-				strptime(main_element->Attribute("date"), "%Day %Mon %dd %hh:%mm:%ss %yyyy", &_tm);
-				date_ = mktime(&_tm);
-			}
-
-			if(main_element->Attribute("north") != nullptr)
-				north_ =  strtod(main_element->Attribute("north"), NULL);
-			else
-				north_ =  0;
-
-			if(main_element->Attribute("south") != nullptr)
-				south_ =  strtod(main_element->Attribute("south"), NULL);
-			else
-				south_ =  0;
-
-			if(main_element->Attribute("east") != nullptr)
-				east_ =  strtod(main_element->Attribute("east"), NULL);
-			else
-				east_ =  0;
-
-			if(main_element->Attribute("west") != nullptr)
-				west_ =  strtod(main_element->Attribute("west"), NULL);
-			else
-				west_ =  0;
-
-			if(main_element->Attribute("maxRoad") != nullptr)
-				max_road_ =  strtol(main_element->Attribute("maxRoad"), NULL, 10);
-			else
-				max_road_ =  0;
-
-			if(main_element->Attribute("maxJunc") != nullptr)
-				max_junc_ =  strtol(main_element->Attribute("maxJunc"), NULL, 10);
-			else
-				max_junc_ =  0;
-
-			if(main_element->Attribute("maxPrg") != nullptr)
-				max_prg_ =  strtol(main_element->Attribute("maxPrg"), NULL, 10);
-			else
-				max_prg_ =  0;
-
-			if(main_element->Attribute("vendor") != nullptr)
-				vendor_ = std::string(main_element->Attribute("vendor"));
-
-		}
-	}
-
-	int rev_major_;
-    int rev_minor_;
-    std::string name_ ;
-    std::string version_;
-    time_t date_ ;
-    double north_;
-    double south_;
-    double east_;
-    double west_;
-    int max_road_;
-    int max_junc_;
-    int max_prg_;
-    std::string vendor_;
-};
-
-//from <width> from <lane> from <left,right,center> from <laneSection> from <road>
-class LaneWidth
-{
-public:
-	double sOffset, a, b, c, d;
-	LaneWidth(TiXmlElement* main_element)
-	{
-		sOffset = XmlHelpers::getDoubleAttribute(main_element, "sOffset", 0);
-		a = XmlHelpers::getDoubleAttribute(main_element, "a", 0);
-		b = XmlHelpers::getDoubleAttribute(main_element, "b", 0);
-		c = XmlHelpers::getDoubleAttribute(main_element, "c", 0);
-		d = XmlHelpers::getDoubleAttribute(main_element, "d", 0);
-	}
-};
-
-//from <width> from <lane> from <left,right,center> from <laneSection> from <road>
-class RoadMark
-{
-public:
-	double sOffset, width;
-	LINE_MARKING_TYPE type;
-	std::string weight;
-	std::string color;
-
-	RoadMark(TiXmlElement* main_element)
-	{
-		sOffset = XmlHelpers::getDoubleAttribute(main_element, "sOffset", 0);
-		width = XmlHelpers::getDoubleAttribute(main_element, "width", 0);
-		weight = XmlHelpers::getStringAttribute(main_element, "weight", "");
-		color = XmlHelpers::getStringAttribute(main_element, "color", "");
-		std::string str_type = XmlHelpers::getStringAttribute(main_element, "type", "");
-
-		if(str_type.compare("broken")==0)
-		{
-			type = BROKEN_LINE_MARK;
-		}
-		else if(str_type.compare("solid")==0)
-		{
-			type = SOLID_LINE_MARK;
-		}
-		else if(str_type.compare("none")==0)
-		{
-			type = NONE_LINE_MARK;
-		}
-		else
-		{
-			type = UNKNOWN_LINE_MARK;
-		}
-	}
-};
-
-//from <laneOffset> from <lanes> from <road>
-class LaneOffset
-{
-public:
-	double s, a, b, c, d;
-	LaneOffset(TiXmlElement* main_element)
-	{
-		s = XmlHelpers::getDoubleAttribute(main_element, "s", 0);
-		a = XmlHelpers::getDoubleAttribute(main_element, "a", 0);
-		b = XmlHelpers::getDoubleAttribute(main_element, "b", 0);
-		c = XmlHelpers::getDoubleAttribute(main_element, "c", 0);
-		d = XmlHelpers::getDoubleAttribute(main_element, "d", 0);
-	}
-
-	double getOffset(const double _s)
-	{
-		double internal_s = _s - s;
-		double h = a + (b * internal_s) + (c * pow(internal_s,2)) + (d * pow(internal_s,3));
-		return h;
-	}
-};
-
-//from <elevation> from <elevationProfile> from <road>
-//from <superelevation> from <lateralProfile> from <road>
-class Elevation
-{
-public:
-	double s, a, b, c, d;
-	ELEVATION_TYPE type;
-	Elevation(TiXmlElement* main_element)
-	{
-		std::string val = XmlHelpers::getStringValue(main_element, "");
-		if(val.compare("elevation") == 0)
-		{
-			type = ELEVATION_PROFILE;
-		}
-		else if(val.compare("superelevation") == 0)
-		{
-			type = LATERAL_PROFILE;
-		}
-
-		s = XmlHelpers::getDoubleAttribute(main_element, "s", 0);
-		a = XmlHelpers::getDoubleAttribute(main_element, "a", 0);
-		b = XmlHelpers::getDoubleAttribute(main_element, "b", 0);
-		c = XmlHelpers::getDoubleAttribute(main_element, "c", 0);
-		d = XmlHelpers::getDoubleAttribute(main_element, "d", 0);
-	}
-
-	double getHeigh(const double _s)
-	{
-		double internal_s = _s - s;
-		double h = a + (b * internal_s) + (c * pow(internal_s,2)) + (d * pow(internal_s,3));
-		return h;
-	}
-
-};
-
-//from <Junction> from <OpenDRIVE>
-class Connection
-{
-public:
-	int id_;
-	int incoming_section_;
-	int outgoing_section_;
-	int incoming_road_;
-	int outgoing_road_;
-	std::string contact_point_;
-	std::vector<std::pair<int, int> > lane_links;
-
-	Connection()
-	{
-		id_ = -1;
-		incoming_section_ = -1;
-		outgoing_section_ = -1; 
-		incoming_road_ = -1;
-		outgoing_road_ = -1;
-	}
-
-	Connection(TiXmlElement* main_element)
-	{
-		incoming_section_ = -1;
-		id_ = XmlHelpers::getIntAttribute(main_element, "id", 0);
-		incoming_road_ = XmlHelpers::getIntAttribute(main_element, "incomingRoad", 0);
-		outgoing_road_ = XmlHelpers::getIntAttribute(main_element, "connectingRoad", 0);
-		contact_point_ = XmlHelpers::getStringAttribute(main_element, "contactPoint", "");
-
-		std::vector<TiXmlElement*> elements;
-		if(main_element != nullptr)
-		{
-			XmlHelpers::findElements("laneLink", main_element->FirstChildElement(), elements);
-			for(unsigned int i=0; i < elements.size(); i++)
-			{
-				int from_id = XmlHelpers::getIntAttribute(elements.at(i), "from", 0);
-				int to_id =  XmlHelpers::getIntAttribute(elements.at(i), "to", 0);
-				lane_links.push_back(std::make_pair(from_id, to_id));
-			}
-		}
-	}
-
-	int getToLane(const int& _from)
-	{
-		for(unsigned int i=0; i < lane_links.size(); i++)
-		{
-			if(lane_links.at(i).first == _from)
-				return lane_links.at(i).second;
-		}
-
-		return 0;
-	}
-
-	int getFromLane(const int& _to)
-	{
-		for(unsigned int i=0; i < lane_links.size(); i++)
-		{
-			if(lane_links.at(i).second == _to)
-				return lane_links.at(i).first;
-		}
-
-		return 0;
-	}
-
-	void flip()
-	{
-		int tmp = incoming_road_;
-		incoming_road_ = outgoing_road_;
-		outgoing_road_ = tmp;
-
-		tmp = incoming_section_;
-		incoming_section_ = outgoing_section_;
-		outgoing_section_ = tmp; 
-		
-		for (auto &lane_link : lane_links)
-		{
-			tmp = lane_link.first;
-			lane_link.first = lane_link.second;
-			lane_link.second = tmp;
-		}
-	}
-
-	void flipRoad()
-	{
-		int tmp = incoming_road_;
-		incoming_road_ = outgoing_road_;
-		outgoing_road_ = tmp;
-		tmp = incoming_section_;
-		incoming_section_ = outgoing_section_;
-		outgoing_section_ = tmp;
-		
-	}
-
-};
-
-//from <OpenDRIVE>
-class Junction
-{
-public:
-	std::string name_;
-	int id_;
-	std::vector<Connection> connections_;
-
-	Junction(TiXmlElement* main_element)
-	{
-		name_ = XmlHelpers::getStringAttribute(main_element, "name", "");
-		id_ = XmlHelpers::getIntAttribute(main_element, "id", 0);
-		std::vector<TiXmlElement*> elements;
-		if(main_element != nullptr)
-		{
-			XmlHelpers::findElements("connection", main_element->FirstChildElement(), elements);
-			for(unsigned int i=0; i < elements.size(); i++)
-			{
-				connections_.push_back(Connection(elements.at(i)));
-			}
-		}
-	}
-	std::vector<Connection> getConnectionsByRoadId(int road_id)
-	{
-		std::vector<Connection> ret_connections;
-		for( const auto connection: connections_)
-		{
-			if( connection.incoming_road_ == road_id)
-			{
-				ret_connections.push_back(connection);
-			}
-		}
-		return ret_connections;
-	}
-};
-
-//from  <planView> from <road>
-class Geometry
-{
-public:
-	double s,x, y, heading, length; //genral use 'line'
-	double curveStart, curveEnd; // for spiral
-	double curvature; //for arc
-
-
-	GEOMETRY_TYPE type;
-
-	Geometry(TiXmlElement* main_element)
-	{
-		type = UNKNOWN_GEOMETRY;
-		curvature = 0;
-		curveStart = 0;
-		curveEnd = 0;
-		s = XmlHelpers::getDoubleAttribute(main_element, "s", 0);
-		x = XmlHelpers::getDoubleAttribute(main_element, "x", 0);
-		y = XmlHelpers::getDoubleAttribute(main_element, "y", 0);
-		heading = XmlHelpers::getDoubleAttribute(main_element, "hdg", 0);
-		length = XmlHelpers::getDoubleAttribute(main_element, "length", 0);
-
-		if(main_element != nullptr)
-		{
-			TiXmlElement* type_element = main_element->FirstChildElement();
-			std::string val = XmlHelpers::getStringValue(type_element, "");
-			if(val.compare("line") == 0)
-			{
-				type = LINE_GEOMETRY;
-			}
-			else if(val.compare("arc") == 0)
-			{
-				type = ARC_GEOMETRY;
-				curvature = XmlHelpers::getDoubleAttribute(type_element, "curvature", 0);
-			}
-			else if(val.compare("spiral") == 0)
-			{
-				type = SPIRAL_GEOMETRY;
-				curveStart = XmlHelpers::getDoubleAttribute(type_element, "curvStart", 0);
-				curveEnd = XmlHelpers::getDoubleAttribute(type_element, "curvEnd", 0);
-			}
-			else if(val.compare("poly3") == 0)
-			{
-			}
-		}
-	}
-
-	bool getPoint(const double _s, PlannerHNS::WayPoint& wp)
-	{
-		if(_s < s || _s > (s+length)) //only calculate points inside the geometry
-			return false;
-
-		double internal_s = _s - s;
-
-		if(type == LINE_GEOMETRY)
-		{
-			wp.pos.x = x + internal_s*cos(heading);
-			wp.pos.y = y + internal_s*sin(heading);
-			wp.pos.a = heading;
-		//	std::cout << " Line Calc, " << _s <<", " << s << ", " << internal_s <<  std::endl;
-		}
-		else if(type == ARC_GEOMETRY)
-		{
-
-			double c = curvature;
-			double hdg = heading - M_PI_2;
-
-			double a = 2.0 / c * sin(internal_s * c / 2.0);
-			double alpha = ((M_PI - (internal_s * c)) / 2.0) - hdg;
-
-			wp.pos.x = x + (-1.0 * a * cos(alpha));
-			wp.pos.y = y + (a * sin(alpha));
-			wp.pos.a = heading + internal_s * c;
-
-		//	std::cout << " ARC Calc" << std::endl;
-		}
-		else if(type == SPIRAL_GEOMETRY)
-		{
-
-			double curvDot = (curveEnd - curveStart)/length;
-
-			double _x = 0;
-			double _y = 0;
-			double _a = 0;
-
-			double rotation_angle = heading;
-			if(fabs(curveStart) > 0.00001)
-			{
-				curvDot = 2.0*(curveStart - curveEnd)/length;
-				odrSpiral(internal_s, curvDot, &_x, &_y, &_a);
-			}
-			else
-				odrSpiral(internal_s, curvDot, &_x, &_y, &_a);
-
-
-			Mat3 rotationMat(rotation_angle);
-			Mat3 translationMat(x, y);
-
-			PlannerHNS::GPSPoint p(_x, _y, 0, _a);
-
-			PlannerHNS::GPSPoint t_p = rotationMat*p;
-			t_p = translationMat*t_p;
-
-			wp.pos.x = t_p.x;
-			wp.pos.y = t_p.y;
-			wp.pos.z = t_p.z;
-			wp.pos.a = heading + t_p.a;
-
-			//std::cout << " Spiral Calc, " << _s <<", " << s << ", " << internal_s << ", " << curvDot << ", "<<curveStart <<  std::endl;
-		}
-
-		return true;
-	}
-};
-
-class FromLaneLink
-{
-public:
-	int from_lane_id;
-	FromLaneLink(TiXmlElement* main_element)
-	{
-		from_lane_id = XmlHelpers::getIntAttribute(main_element, "id", 0);
-	}
-};
-
-class ToLaneLink
-{
-public:
-	int to_lane_id;
-	ToLaneLink(TiXmlElement* main_element)
-	{
-		to_lane_id = XmlHelpers::getIntAttribute(main_element, "id", 0);
-	}
-};
-
-//from <left/center/right> from <laneSection> <lanes> from <road>
-class OpenDriveLane
-{
-public:
-	int id_;
-	int level_;
-
-	LANE_DIRECTION dir_;
-	LANE_TYPE type_;
-	std::vector<int> fromIds_;
-	std::vector<int> toIds_;
-	std::vector<LaneWidth> width_list_;
-	std::vector<RoadMark> mark_list_;
-	std::vector<FromLaneLink> from_lane_;
-	std::vector<ToLaneLink> to_lane_;
-
-	OpenDriveLane(TiXmlElement* main_element, const LANE_DIRECTION& _dir, const double& s_offset, const double& lane_length, const int& section_index)
-	{
-		dir_ = _dir;
-		type_ = DRIVING_LANE;
-		id_ = XmlHelpers::getIntAttribute(main_element, "id", 0);
-		level_ = XmlHelpers::getIntAttribute(main_element, "level", 0);
-		std::string str_type = XmlHelpers::getStringAttribute(main_element, "type", "");
-
-		if(str_type.compare("driving")==0)
-			type_ = DRIVING_LANE;
-		else
-			type_ = NONE_LANE;
-
-		std::vector<TiXmlElement*> elements;
-		XmlHelpers::findFirstElement("link", main_element, elements);
-		if(elements.size() > 0)
-		{
-			std::vector<TiXmlElement*> pred_elements, succ_elements;
-
-			XmlHelpers::findElements("predecessor", elements.at(0)->FirstChildElement(), pred_elements);
-			for(unsigned int j=0; j < pred_elements.size(); j++)
-			{
-				if(id_ > 0 )//left lanes
-				{
-					to_lane_.push_back(ToLaneLink(pred_elements.at(j)));
-				}
-				else if ( id_ < 0 )//right lanes
-				{
-					from_lane_.push_back(FromLaneLink(pred_elements.at(j)));	
-				}
-			}
-
-			XmlHelpers::findElements("successor", elements.at(0)->FirstChildElement(), succ_elements);
-			for(unsigned int j=0; j < succ_elements.size(); j++)
-			{
-				if(id_ > 0 )//left lanes
-				{
-					from_lane_.push_back(FromLaneLink(succ_elements.at(j)));
-				}
-				else if (id_ < 0)
-				{
-					to_lane_.push_back(ToLaneLink(succ_elements.at(j)));	
-				}
-			}
-		}
-
-		std::vector<TiXmlElement*> width_elements;
-		if(main_element != nullptr)
-		{
-			XmlHelpers::findElements("width", main_element->FirstChildElement(), width_elements);
-			for(unsigned int i=0; i < width_elements.size(); i++)
-			{
-				width_list_.push_back(LaneWidth(width_elements.at(i)));
-			}
-		}
-	}
-
-	LaneWidth* 	getMatchingWidth(const double& sOffset)
-	{
-		if(width_list_.size() == 0)
-			return nullptr;
-
-		if(width_list_.size() == 1)
-			return &width_list_.at(0);
-
-		for(int i=1; i < width_list_.size(); i++)
-		{
-			if(sOffset >= width_list_.at(i-1).sOffset && sOffset < width_list_.at(i).sOffset)
-			{
-				return &width_list_.at(i-1);
-			}
-		}
-
-		return &width_list_.at(width_list_.size()-1);
-	}
-
-	double getLaneWidth(const double& sOffset)
-	{
-		LaneWidth* p_width = getMatchingWidth(sOffset);
-
-		if(p_width != nullptr)
-		{
-			double s_local = sOffset - p_width->sOffset;
-			double width = p_width->a + (p_width->b * s_local) + (p_width->c * pow(s_local,2)) + (p_width->d * pow(s_local,3));
-			return width;
-		}
-
-		return 0;
-	}
-};
-
-class RoadCenterInfo
-{
-public:
-	double ds_;
-	PlannerHNS::GPSPoint center_p_;
-	double offset_width_;
-
-	RoadCenterInfo()
-	{
-		ds_ = 0;
-		offset_width_ = 0;
-	}
-};
-
-//from <lanes> from <road>
-class RoadSection
-{
-public:
-	double s_;
-	int id_;
-	double length_;
-	std::vector<OpenDriveLane> left_lanes_;
-	std::vector<OpenDriveLane> right_lanes_;
-	std::vector<OpenDriveLane> center_lane_;
-	RoadSection* p_next_section_;
-	RoadSection* p_prev_section_;
-
-	RoadSection(TiXmlElement* main_element, const double& s_offset, const double& section_length, const int& section_index)
-	{
-		length_ = section_length;
-		id_ = section_index;
-		s_ = s_offset;
-		p_next_section_ = nullptr;
-		p_prev_section_ = nullptr;
-
-		std::vector<TiXmlElement*> sub_elements;
-		std::vector<TiXmlElement*> lane_elements;
-
-		sub_elements.clear();
-		XmlHelpers::findFirstElement("left", main_element, sub_elements);
-		if(sub_elements.size() > 0)
-		{
-			lane_elements.clear();
-			XmlHelpers::findElements("lane", sub_elements.at(0)->FirstChildElement(), lane_elements);
-			for(unsigned int k=0; k < lane_elements.size(); k++)
-			{
-				OpenDriveLane parsed_lane = OpenDriveLane(lane_elements.at(k), LEFT_LANE, s_offset, section_length, section_index);
-				if(parsed_lane.type_ == DRIVING_LANE)
-					left_lanes_.push_back(parsed_lane );
-			}
-		}
-
-		sub_elements.clear();
-		XmlHelpers::findFirstElement("center", main_element, sub_elements);
-		if(sub_elements.size() > 0)
-		{
-			lane_elements.clear();
-			XmlHelpers::findElements("lane", sub_elements.at(0)->FirstChildElement(), lane_elements);
-			for(unsigned int k=0; k < lane_elements.size(); k++)
-			{
-				center_lane_.push_back(OpenDriveLane(lane_elements.at(k), CENTER_LANE, s_offset, section_length, section_index));
-			}
-		}
-
-		sub_elements.clear();
-		XmlHelpers::findFirstElement("right", main_element, sub_elements);
-		if(sub_elements.size() > 0)
-		{
-			lane_elements.clear();
-			XmlHelpers::findElements("lane", sub_elements.at(0)->FirstChildElement(), lane_elements);
-			for(unsigned int k=0; k < lane_elements.size(); k++)
-			{
-				OpenDriveLane parsed_lane = OpenDriveLane(lane_elements.at(k), RIGHT_LANE, s_offset, section_length, section_index);
-				if(parsed_lane.type_ == DRIVING_LANE)
-					right_lanes_.push_back(parsed_lane);
-			}
-		}
-
-		std::sort(left_lanes_.begin(), left_lanes_.end(), sort_lanes_asc);
-		std::sort(right_lanes_.begin(), right_lanes_.end(), sort_lanes_desc);
-	}
-
-
-	static bool sort_lanes_asc(const OpenDriveLane& l1, const OpenDriveLane& l2)
-	{
-		if(l1.id_ < l2.id_)
-			return true;
-		else
-			return false;
-	}
-
-	static bool sort_lanes_desc(const OpenDriveLane& l1, const OpenDriveLane& l2)
-	{
-		if(l1.id_ > l2.id_)
-			return true;
-		else
-			return false;
-	}
-};
-
-//from <signals> from <road>
-class Signal
-{
-public:
-	int id_;
-	std::string name_;
-	std::string text_;
-	std::string unit_;
-	std::string country_code_;
-
-	double s_;
-	double t_;
-	double value_;
-	double h_;
-	double w_;
-	double yaw_; //hOffset
-	double pitch_;
-	double roll_;
-	double zOffset_;
-
-	bool dynamic_;
-	ORIENTATION_TYPE orientation_;
-	std::string type_;
-	std::string sub_type_;
-
-	std::vector<int> valid_lanes_ids_;
-
-
-	Signal(TiXmlElement* main_element)
-	{
-		orientation_ = BOTH_DIRECTIONS;
-
-		s_ = XmlHelpers::getDoubleAttribute(main_element, "s", 0);
-		t_ = XmlHelpers::getDoubleAttribute(main_element, "t", 0);
-		id_ = XmlHelpers::getIntAttribute(main_element, "id", 0);
-
-		name_ = XmlHelpers::getStringAttribute(main_element, "name", "");
-		text_ = XmlHelpers::getStringAttribute(main_element, "text", "");
-		unit_ = XmlHelpers::getStringAttribute(main_element, "unit", "");
-		country_code_ = XmlHelpers::getStringAttribute(main_element, "country", "");
-
-		zOffset_ = XmlHelpers::getDoubleAttribute(main_element, "zOffset", 0);
-		value_ = XmlHelpers::getDoubleAttribute(main_element, "value", 0);
-		h_ = XmlHelpers::getDoubleAttribute(main_element, "height", 0);
-		w_ = XmlHelpers::getDoubleAttribute(main_element, "width", 0);
-		yaw_ = XmlHelpers::getDoubleAttribute(main_element, "hOffset", 0);
-		roll_ = XmlHelpers::getDoubleAttribute(main_element, "roll", 0);
-		pitch_ = XmlHelpers::getDoubleAttribute(main_element, "pitch", 0);
-
-		std::string dynamic_str = XmlHelpers::getStringAttribute(main_element, "dynamic", "");
-		if(dynamic_str.compare("yes")==0)
-			dynamic_ = true;
-		else
-			dynamic_ = false;
-
-		std::string orientation_str = XmlHelpers::getStringAttribute(main_element, "orientation", "");
-		if(orientation_str.compare("+")==0)
-			orientation_ = SAME_DIRECTION;
-		else if(orientation_str.compare("-")==0)
-			orientation_ = OPPOSIT_DIRECTION;
-
-		type_ = XmlHelpers::getStringAttribute(main_element, "type", "");
-		sub_type_ = XmlHelpers::getStringAttribute(main_element, "subtype", "");
-
-//		if(type_str.compare("294")==0)
-//			type_ = STOP_LINE_SIGNAL;
-//		else if(type_str.compare("341")==0)
-//			type_ = WAIT_LINE_SIGNAL;
-//		else if(type_str.compare("1000001")==0)
-//			type_ = TRAFFIC_LIGHT_1;
-//		else if(type_str.compare("1000002")==0)
-//			type_ = TRAFFIC_LIGHT_2;
-//		else if(type_str.compare("1000003")==0)
-//			type_ = TRAFFIC_LIGHT_3;
-
-		if(main_element != nullptr)
-		{
-			std::vector<TiXmlElement*> elements;
-			elements.clear();
-			XmlHelpers::findElements("validity", main_element->FirstChildElement(), elements);
-			for(unsigned int i=0; i < elements.size(); i++)
-			{
-				int _from_lane = XmlHelpers::getIntAttribute(elements.at(i), "fromLane", 0);
-				int _to_lane = XmlHelpers::getIntAttribute(elements.at(i), "toLane", 0);
-
-				for(int k=_from_lane; k < _to_lane; k++)
-				{
-					valid_lanes_ids_.push_back(k);
-				}
-			}
-		}
-
-	}
-};
-
-//from <signals> from <road>
-class SignalRef
-{
-public:
-	double s_;
-	double t_;
-	int id_;
-	ORIENTATION_TYPE orientation_;
-	std::vector<int> valid_lanes_ids_;
-
-	SignalRef(TiXmlElement* main_element)
-	{
-		s_ = XmlHelpers::getDoubleAttribute(main_element, "s", 0);
-		t_ = XmlHelpers::getDoubleAttribute(main_element, "t", 0);
-		id_ = XmlHelpers::getIntAttribute(main_element, "id", 0);
-		std::string orientation_str = XmlHelpers::getStringAttribute(main_element, "orientation", "");
-		if(orientation_str.compare("+")==0)
-			orientation_ = SAME_DIRECTION;
-		else if(orientation_str.compare("-")==0)
-			orientation_ = OPPOSIT_DIRECTION;
-
-		if(main_element != nullptr)
-		{
-			std::vector<TiXmlElement*> elements;
-			elements.clear();
-			XmlHelpers::findElements("validity", main_element->FirstChildElement(), elements);
-			for(unsigned int i=0; i < elements.size(); i++)
-			{
-				int _from_lane = XmlHelpers::getIntAttribute(elements.at(i), "fromLane", 0);
-				int _to_lane = XmlHelpers::getIntAttribute(elements.at(i), "toLane", 0);
-
-				for(int k=_from_lane; k < _to_lane; k++)
-				{
-					valid_lanes_ids_.push_back(k);
-				}
-			}
-		}
-	}
-};
-
-//from <objects> from <road>
-class RoadObject
-{
-public:
-	std::string name_;
-	ROAD_OBJECT_TYPE type_;
-	int id_;
-	double s_;
-	double t_;
-	double zOffset_;
-	double validation_length_;
-	ORIENTATION_TYPE orientation_;
-	double l_;
-	double w_;
-	double h_;
-	double r_;
-	double yaw_; //hOffset, hdg
-	double pitch_;
-	double roll_;
-
-	RoadObject(TiXmlElement* main_element)
-	{
-		orientation_ = BOTH_DIRECTIONS;
-		type_ = UNKNOWN_ROAD_OBJ;
-
-		s_ = XmlHelpers::getDoubleAttribute(main_element, "s", 0);
-		t_ = XmlHelpers::getDoubleAttribute(main_element, "t", 0);
-		id_ = XmlHelpers::getIntAttribute(main_element, "id", 0);
-		name_ = XmlHelpers::getStringAttribute(main_element, "name", "");
-
-		validation_length_ = XmlHelpers::getDoubleAttribute(main_element, "validLength", 0);
-
-		zOffset_ = XmlHelpers::getDoubleAttribute(main_element, "zOffset", 0);
-		h_ = XmlHelpers::getDoubleAttribute(main_element, "height", 0);
-		w_ = XmlHelpers::getDoubleAttribute(main_element, "width", 0);
-		l_ = XmlHelpers::getDoubleAttribute(main_element, "length", 0);
-		r_ = XmlHelpers::getDoubleAttribute(main_element, "radius", 0);
-
-		yaw_ = XmlHelpers::getDoubleAttribute(main_element, "hdg", 0);
-		roll_ = XmlHelpers::getDoubleAttribute(main_element, "roll", 0);
-		pitch_ = XmlHelpers::getDoubleAttribute(main_element, "pitch", 0);
-
-		std::string orientation_str = XmlHelpers::getStringAttribute(main_element, "orientation", "");
-		if(orientation_str.compare("+")==0)
-			orientation_ = SAME_DIRECTION;
-		else if(orientation_str.compare("-")==0)
-			orientation_ = OPPOSIT_DIRECTION;
-
-		std::string type_str = XmlHelpers::getStringAttribute(main_element, "type", "");
-		if(type_str.compare("parkingSpace")==0)
-			type_ = PARKING_SPACE_OBJ;
-	}
-};
-
-class RoadObjectRef
-{
-public:
-	RoadObjectRef(TiXmlElement* main_element)
-	{
-
-	}
-};
-
-class RoadObjectTunnel
-{
-public:
-	RoadObjectTunnel(TiXmlElement* main_element)
-	{
-
-	}
-
-};
-
-class RoadObjectBridge
-{
-public:
-	RoadObjectBridge(TiXmlElement* main_element)
-	{
-
-	}
-
-};
-}
-
-#endif // OPENDRIVE_OBJECTS

src/ros/catkin/src/vector_map_converter/include/opendrive2autoware_converter/opendrive_road.h

@@ -1,285 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef OPENDRIVE_ROAD
-#define OPENDRIVE_ROAD
-
-#include "opendrive_objects.h"
-
-namespace opendrive_converter
-{
-
-class FromRoadLink
-{
-public:
-	LINK_TYPE link_type_;
-	int from_road_id_;
-	CONTACT_POINT contact_point_;
-
-	FromRoadLink(TiXmlElement* main_element)
-	{
-		if(XmlHelpers::getStringAttribute(main_element, "elementType", "").compare("road") == 0)
-			link_type_ = ROAD_LINK;
-		else
-			link_type_ = JUNCTION_LINK;
-
-		from_road_id_ = XmlHelpers::getIntAttribute(main_element, "elementId", 0);
-
-		if(XmlHelpers::getStringAttribute(main_element, "contactPoint", "").compare("start") == 0)
-			contact_point_ = START_POINT;
-		else if(XmlHelpers::getStringAttribute(main_element, "contactPoint", "").compare("end") == 0)
-			contact_point_ = END_POINT;
-		else
-			contact_point_ = EMPTY_POINT;
-	}
-};
-
-class ToRoadLink
-{
-public:
-	LINK_TYPE link_type_;
-	int to_road_id_;
-	CONTACT_POINT contact_point_;
-
-	ToRoadLink(TiXmlElement* main_element)
-	{
-		if(XmlHelpers::getStringAttribute(main_element, "elementType", "").compare("road") == 0)
-			link_type_ = ROAD_LINK;
-		else
-			link_type_ = JUNCTION_LINK;
-
-		to_road_id_ = XmlHelpers::getIntAttribute(main_element, "elementId", 0);
-
-		if(XmlHelpers::getStringAttribute(main_element, "contactPoint", "").compare("start") == 0)
-			contact_point_ = START_POINT;
-		else if(XmlHelpers::getStringAttribute(main_element, "contactPoint", "").compare("end") == 0)
-			contact_point_ = END_POINT;
-		else
-			contact_point_ = EMPTY_POINT;
-	}
-};
-
-class OpenDriveRoad
-{
-public:
-	std::string name_;
-	int id_;
-	int junction_id_;
-	double length_;
-	bool keep_right_;
-	std::vector<FromRoadLink> predecessor_road_;
-	std::vector<ToRoadLink> successor_road_;
-	std::vector<Geometry> geometries_;
-	std::vector<Elevation> elevations_;
-	std::vector<RoadSection> sections_;
-	std::vector<LaneOffset> laneOffsets_;
-	std::vector<Signal> road_signals_;
-	std::vector<SignalRef> road_signals_references_;
-	std::vector<RoadObject> road_objects_;
-	std::vector<RoadObjectRef> road_objects_references_;
-	std::vector<RoadObjectTunnel> road_objects_tunnels_;
-	std::vector<RoadObjectBridge> road_objects_bridges_;
-	std::vector<Connection> to_roads_;
-	std::vector<Connection> from_roads_;
-	std::vector<std::pair<std::string, std::vector<CSV_Reader::LINE_DATA> > >* p_country_signal_codes_;
-
-	std::vector<Connection> getFirstSectionConnections( OpenDriveRoad *_p_predecessor_road);
-	std::vector<Connection> getLastSectionConnections( OpenDriveRoad *_p_predecessor_road);
-	void getRoadLanes(std::vector<PlannerHNS::Lane>& lanes_list, double resolution = 0.5);
-	void getTrafficLights(std::vector<PlannerHNS::TrafficLight>& all_lights);
-	void getTrafficSigns(std::vector<PlannerHNS::TrafficSign>& all_signs);
-	void getStopLines(std::vector<PlannerHNS::StopLine>& all_stop_lines);
-	void insertUniqueToConnection(const Connection& _connection);
-	void insertUniqueFromConnection(const Connection& _connection);
-
-	OpenDriveRoad()
-	{
-		p_country_signal_codes_ = nullptr;
-		id_ = 0;
-		junction_id_ = 0;
-		length_ = 0;
-		keep_right_ = true;
-
-	}
-
-	OpenDriveRoad(TiXmlElement* main_element, std::vector<std::pair<std::string, std::vector<CSV_Reader::LINE_DATA> > >* country_signal_codes = nullptr, bool keep_right = true);
-
-	RoadSection* getFirstSection()
-	{
-		if(sections_.size() == 0)
-			return nullptr;
-
-		return &sections_.at(0);
-	}
-
-	RoadSection* getLastSection()
-	{
-		if(sections_.size() == 0)
-			return nullptr;
-
-		return &sections_.at(sections_.size()-1);
-	}
-
-private:
-	Geometry* getMatchingGeometry(const double& sOffset)
-	{
-		for(unsigned int i=0; i < geometries_.size(); i++)
-		{
-			if(sOffset >= geometries_.at(i).s && sOffset < (geometries_.at(i).s+geometries_.at(i).length))
-			{
-				return &geometries_.at(i);
-			}
-		}
-
-		return nullptr;
-	}
-
-	Elevation* getMatchingElevations(const double& sOffset)
-	{
-		if(elevations_.size() == 0)
-			return nullptr;
-
-		if(elevations_.size() == 1)
-			return &elevations_.at(0);
-
-		for(int i=1; i < elevations_.size(); i++)
-		{
-			if(sOffset >= elevations_.at(i-1).s && sOffset < elevations_.at(i).s)
-			{
-				return &elevations_.at(i-1);
-			}
-		}
-
-		return &elevations_.at(elevations_.size()-1);
-	}
-
-	LaneOffset* getMatchingLaneOffset(const double& sOffset)
-	{
-		if(laneOffsets_.size() == 0)
-			return nullptr;
-
-		if(laneOffsets_.size() == 1)
-			return &laneOffsets_.at(0);
-
-		for(int i=1; i < laneOffsets_.size(); i++)
-		{
-			if(sOffset >= laneOffsets_.at(i-1).s && sOffset < laneOffsets_.at(i).s)
-			{
-				return &laneOffsets_.at(i-1);
-			}
-		}
-
-		return &laneOffsets_.at(laneOffsets_.size()-1);
-	}
-
-	RoadSection* getMatchingSection(const double& sOffset)
-	{
-		if(sections_.size() == 0)
-			return nullptr;
-
-		if(sections_.size() == 1)
-			return &sections_.at(0);
-
-		for(unsigned int i=0; i < sections_.size(); i++)
-		{
-			double end_offset = sections_.at(i).s_ + sections_.at(i).length_;
-
-			if(sOffset <= end_offset)
-			{
-				return &sections_.at(i);
-			}
-		}
-
-		return nullptr;
-	}
-
-	RoadSection* getExactMatchingSection(const double& sOffset)
-	{
-		if(sections_.size() == 0)
-			return nullptr;
-
-		if(sections_.size() == 1)
-			return &sections_.at(0);
-
-		for(unsigned int i=0; i < sections_.size(); i++)
-		{
-			if(sOffset == sections_.at(i).s_)
-			{
-				return &sections_.at(i);
-			}
-		}
-
-		return nullptr;
-	}
-
-	bool createSingleCenterPoint(double _ds, PlannerHNS::WayPoint& _p);
-
-	void insertUniqueFromSectionIds(int from_section_id, const OpenDriveLane* curr_lane, PlannerHNS::Lane& _l);
-	void insertUniqueToSectionIds(int to_section_id, const OpenDriveLane* curr_lane, PlannerHNS::Lane& _l);
-
-	void insertUniqueFromRoadIds(int curr_section_id, int curr_lane_id, PlannerHNS::Lane& _l);
-	void insertUniqueToRoadIds(int curr_section_id, int curr_lane_id, PlannerHNS::Lane& _l);
-
-
-
-
-	void createRoadLanes(std::vector<PlannerHNS::Lane>& lanes_list);
-	void createRoadCenterInfo(std::vector<RoadCenterInfo>& points_list, double resolution = 0.5);
-	bool createRoadCenterPoint(RoadCenterInfo& inf_point, double _s);
-	void insertRoadCenterInfo(std::vector<RoadCenterInfo>& points_list, RoadCenterInfo& inf_point);
-	void fixRedundantPointsLanes(PlannerHNS::Lane& _lane);
-	void createSectionPoints(const RoadCenterInfo& ref_info, std::vector<PlannerHNS::Lane>& lanes_list, RoadSection* p_sec, int& wp_id_seq, std::vector<int> &left_lane_ids, std::vector<int> &right_lane_ids);
-
-
-	PlannerHNS::Lane* getLaneById(const int& _l_id, std::vector<PlannerHNS::Lane>& _lanes_list)
-	{
-		for(unsigned int i=0; i < _lanes_list.size(); i++)
-		{
-			if(_lanes_list.at(i).id == _l_id)
-				return &_lanes_list.at(i);
-		}
-
-		return nullptr;
-	}
-
-	bool exists(const std::vector<int>& _list, int _val)
-	{
-		for(unsigned int j=0; j< _list.size(); j++)
-		{
-			if(_list.at(j) == _val)
-			{
-				return true;
-			}
-		}
-
-		return false;
-	}
-
-	OBJECT_TYPE getAutowareMainTypeFromCode(const std::string& country_code, const std::string& type, const std::string& sub_type);
-	TRAFFIC_LIGHT_TYPE getAutowareLightTypeFromCode(const std::string& country_code, const std::string& type, const std::string& sub_type);
-	ROAD_SIGN_TYPE getAutowareRoadSignTypeFromCode(const std::string& country_code, const std::string& type, const std::string& sub_type);
-	ROAD_MARK_TYPE getAutowareRoadMarksTypeFromCode(const std::string& country_code, const std::string& type, const std::string& sub_type);
-
-	OBJECT_TYPE getObjTypeFromText(const std::string& autoware_type);
-	TRAFFIC_LIGHT_TYPE getLightTypeFromText(const std::string& autoware_type);
-	ROAD_SIGN_TYPE getSignTypeFromText(const std::string& autoware_type);
-	ROAD_MARK_TYPE getMarkTypeFromText(const std::string& autoware_type);
-
-};
-
-}
-
-#endif // OPENDRIVE2AUTOWARE_CONVERTER

src/ros/catkin/src/vector_map_converter/include/opendrive2autoware_converter/xml_helpers.h

@@ -1,223 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef XML_HELPERS_H
-#define XML_HELPERS_H
-
-#include <string>
-#include <fstream>
-#include <sstream>
-#include <vector>
-#include <iostream>
-#include <limits>
-#include <algorithm>
-
-#include "op_planner/RoadNetwork.h"
-#include "tinyxml.h"
-
-namespace opendrive_converter
-{
-class XmlHelpers
-{
-
-public:
-
-	static void findElements(std::string name, TiXmlElement* parent_element, std::vector<TiXmlElement*>& element_list);
-	static void findFirstElement(std::string name, TiXmlElement* parent_element, std::vector<TiXmlElement*>& element_list);
-	static int getIntAttribute(TiXmlElement* p_elem, std::string name, int def_val = 0);
-	static double getDoubleAttribute(TiXmlElement* p_elem, std::string name, double def_val = 0.0);
-	static std::string getStringAttribute(TiXmlElement* p_elem, std::string name, std::string def_val);
-	static std::string getStringValue(TiXmlElement* p_elem, std::string def_val);
-};
-
-class Mat3
-{
-	double m[3][3];
-
-public:
-	Mat3()
-	{
-		for(unsigned int i=0;i<3;i++)
-		{
-			for(unsigned int j=0;j<3;j++)
-			{
-				m[i][j] = 0;
-			}
-		}
-
-		m[0][0] = m[1][1] = m[2][2] = 1;
-	}
-
-	Mat3(double transX, double transY)
-	{
-		m[0][0] = 1; m[0][1] =  0; m[0][2] =  transX;
-		m[1][0] = 0; m[1][1] =  1; m[1][2] =  transY;
-		m[2][0] = 0; m[2][1] =  0; m[2][2] =  1;
-	}
-
-	Mat3(double rotation_angle)
-	{
-		double c = cos(rotation_angle);
-		double s = sin(rotation_angle);
-		m[0][0] = c; m[0][1] = -s; m[0][2] =  0;
-		m[1][0] = s; m[1][1] =  c; m[1][2] =  0;
-		m[2][0] = 0; m[2][1] =  0; m[2][2] =  1;
-	}
-
-	Mat3(PlannerHNS::GPSPoint rotationCenter)
-	{
-		double c = cos(rotationCenter.a);
-		double s = sin(rotationCenter.a);
-		double u = rotationCenter.x;
-		double v = rotationCenter.y;
-		m[0][0] = c; m[0][1] = -s; m[0][2] = -u*c + v*s + u;
-		m[1][0] = s; m[1][1] =  c; m[1][2] = -u*s - v*c + v;
-		m[2][0] = 0; m[2][1] =  0; m[2][2] =  1;
-	}
-
-
-	PlannerHNS::GPSPoint operator * (PlannerHNS::GPSPoint v)
-	{
-		PlannerHNS::GPSPoint _v = v;
-		v.x = m[0][0]*_v.x + m[0][1]*_v.y + m[0][2]*1;
-		v.y = m[1][0]*_v.x + m[1][1]*_v.y + m[1][2]*1;
-		return v;
-	}
-};
-
-class CSV_Reader
-{
-private:
-	std::ifstream* p_file_;
-	std::vector<std::string> headers_;
-	std::vector<std::string> data_titles_header_;
-	std::vector<std::vector<std::vector<std::string> > > all_data_;
-
-	void ReadHeaders()
-	{
-		if(!p_file_->is_open()) return;
-		std::string strLine;
-		headers_.clear();
-		if(!p_file_->eof())
-		{
-			getline(*p_file_, strLine);
-			headers_.push_back(strLine);
-			ParseDataTitles(strLine);
-		}
-	}
-	void ParseDataTitles(const std::string& header)
-	{
-		if(header.size()==0) return;
-
-		std::string innerToken;
-		std::istringstream str_stream(header);
-		data_titles_header_.clear();
-		while(getline(str_stream, innerToken, ','))
-		{
-			data_titles_header_.push_back(innerToken);
-		}
-	}
-
-public:
-	struct LINE_DATA
-	{
-		std::string gen_type_;
-		std::string gen_sub_type_;
-		std::string id_;
-		std::string sub_id_;
-		std::string desc_;
-	};
-
-	CSV_Reader(std::string file_name)
-	{
-		p_file_ = new std::ifstream(file_name.c_str(), std::ios::in);
-	  if(!p_file_->is_open())
-	  {
-		  printf("\n Can't Open CSV File !, %s", file_name.c_str());
-		  return;
-	  }
-
-	  p_file_->precision(16);
-
-		ReadHeaders();
-
-	}
-
-	~CSV_Reader()
-	{
-		if(p_file_->is_open())
-			p_file_->close();
-	}
-
-	int ReadAllData(std::vector<LINE_DATA>& data_list)
-	{
-		data_list.clear();
-		LINE_DATA data;
-		int count = 0;
-		while(ReadNextLine(data))
-		{
-			data_list.push_back(data);
-			count++;
-		}
-		return count;
-	}
-
-	bool ReadSingleLine(std::vector<std::vector<std::string> >& line)
-	{
-		if(!p_file_->is_open() || p_file_->eof()) return false;
-
-			std::string strLine, innerToken;
-			line.clear();
-			getline(*p_file_, strLine);
-			std::istringstream str_stream(strLine);
-
-			std::vector<std::string> obj_part;
-
-			while(getline(str_stream, innerToken, ','))
-			{
-				obj_part.push_back(innerToken);
-			}
-
-			line.push_back(obj_part);
-			return true;
-	}
-
-	bool ReadNextLine(LINE_DATA& data)
-	{
-		std::vector<std::vector<std::string> > lineData;
-		if(ReadSingleLine(lineData))
-		{
-			if(lineData.size()==0) return false;
-			if(lineData.at(0).size() < 5) return false;
-
-			data.gen_type_ = lineData.at(0).at(0);
-			data.gen_sub_type_ = lineData.at(0).at(1);
-			data.id_ =   lineData.at(0).at(2);
-			data.sub_id_ =   lineData.at(0).at(3);
-			data.desc_ = lineData.at(0).at(4);
-
-			return true;
-
-		}
-		else
-			return false;
-	}
-
-};
-
-}
-
-#endif

src/ros/catkin/src/vector_map_converter/include/vector_map_converter/autoware2vectormap.hpp

@@ -1,144 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef __AUTOWARE2VECTORMAP_HPP__
-#define __AUTOWARE2VECTORMAP_HPP__
-
-#include <autoware_map/map_handler.hpp>
-#include <string>
-#include <iostream>
-#include <ros/ros.h>
-#include <sstream>
-#include <fstream>
-#include <autoware_map/util.h>
-#include <vector_map_msgs/Area.h>
-#include <vector_map_msgs/CrossRoad.h>
-#include <vector_map_msgs/CrossWalk.h>
-#include <vector_map_msgs/DTLane.h>
-#include <vector_map_msgs/Lane.h>
-#include <vector_map_msgs/Line.h>
-#include <vector_map_msgs/Node.h>
-#include <vector_map_msgs/Point.h>
-#include <vector_map_msgs/Pole.h>
-#include <vector_map_msgs/RoadSign.h>
-#include <vector_map_msgs/Signal.h>
-#include <vector_map_msgs/StopLine.h>
-#include <vector_map_msgs/UtilityPole.h>
-#include <vector_map_msgs/Vector.h>
-#include <vector_map_msgs/WayArea.h>
-#include <vector_map_msgs/WhiteLine.h>
-
-struct WaypointWithYaw{
-    autoware_map_msgs::Waypoint waypoint;
-    autoware_map_msgs::Point point;
-    autoware_map_msgs::Point left_point;
-    autoware_map_msgs::Point right_point;
-    std::vector<double> yaws;
-    double yaw_avg;
-};
-
-void getVectorMapMsgs(  const autoware_map::AutowareMapHandler &map_handler,
-                        std::vector<vector_map_msgs::Area> &vmap_areas,
-                        std::vector<vector_map_msgs::CrossRoad> &vmap_cross_roads,
-                        std::vector<vector_map_msgs::CrossWalk> &vmap_cross_walks,
-                        std::vector<vector_map_msgs::DTLane> &vmap_dtlanes,
-                        std::vector<vector_map_msgs::Lane> &vmap_lanes,
-                        std::vector<vector_map_msgs::Line> &vmap_lines,
-                        std::vector<vector_map_msgs::Node> &vmap_nodes,
-                        std::vector<vector_map_msgs::Point> &vmap_points,
-                        std::vector<vector_map_msgs::Pole> &vmap_dummy_poles,
-                        std::vector<vector_map_msgs::RoadSign> &vmap_road_signs,
-                        std::vector<vector_map_msgs::Signal> &vmap_signals,
-                        std::vector<vector_map_msgs::StopLine> &vmap_stop_lines,
-                        std::vector<vector_map_msgs::UtilityPole> &vmap_dummy_utility_poles,
-                        std::vector<vector_map_msgs::Vector> &vmap_vectors,
-                        std::vector<vector_map_msgs::WayArea> &vmap_way_areas,
-                        std::vector<vector_map_msgs::WhiteLine> &vmap_white_lines);
-void writeVectorMapMsgs(const std::string output_dir,
-                        const std::vector<vector_map_msgs::Area> &vmap_areas,
-                        const std::vector<vector_map_msgs::CrossRoad> &vmap_cross_roads,
-                        const std::vector<vector_map_msgs::CrossWalk> &vmap_cross_walks,
-                        const std::vector<vector_map_msgs::DTLane> &vmap_dtlanes,
-                        const std::vector<vector_map_msgs::Lane> &vmap_lanes,
-                        const std::vector<vector_map_msgs::Line> &vmap_lines,
-                        const std::vector<vector_map_msgs::Node> &vmap_nodes,
-                        const std::vector<vector_map_msgs::Point> &vmap_points,
-                        const std::vector<vector_map_msgs::Pole> &vmap_poles,
-                        const std::vector<vector_map_msgs::RoadSign> &vmap_road_signs,
-                        const std::vector<vector_map_msgs::Signal> &vmap_signals,
-                        const std::vector<vector_map_msgs::StopLine> &vmap_stop_lines,
-                        const std::vector<vector_map_msgs::UtilityPole> &vmap_utility_poles,
-                        const std::vector<vector_map_msgs::Vector> &vmap_vectors,
-                        const std::vector<vector_map_msgs::WayArea> &vmap_way_areas,
-                        const std::vector<vector_map_msgs::WhiteLine> &vmap_white_lines
-                        );
-void createAreas(const autoware_map::AutowareMapHandler &map_handler, std::vector<vector_map_msgs::Area> &vmap_areas, std::vector<vector_map_msgs::Line> &vmap_lines);
-void createPoints(const autoware_map::AutowareMapHandler &awmap, std::vector<vector_map_msgs::Point> &vmap_points);
-void createNodes(const autoware_map::AutowareMapHandler &awmap, std::vector<vector_map_msgs::Node> &vmap_nodes);
-void createDTLanes(const autoware_map::AutowareMapHandler &awmap,
-                   std::vector<vector_map_msgs::DTLane> &vmap_dtlanes,
-                   std::vector<vector_map_msgs::Lane> &vmap_lanes);
-void createCrossRoads(const autoware_map::AutowareMapHandler &awmap, std::vector<vector_map_msgs::CrossRoad> &vmap_cross_roads);
-void createCrossWalks(const autoware_map::AutowareMapHandler &awmap, std::vector<vector_map_msgs::CrossWalk> &vmap_cross_walks,
-                      std::vector<vector_map_msgs::Area> &vmap_areas, std::vector<vector_map_msgs::Line> &vmap_lines,
-                      std::vector<vector_map_msgs::Point> &vmap_points);
-void createSignals( const autoware_map::AutowareMapHandler &awmap,
-                    std::vector<vector_map_msgs::Signal> &vmap_signals,
-                    std::vector<vector_map_msgs::Vector> &vmap_vectors,
-                    std::vector<vector_map_msgs::Pole> &vmap_dummy_poles);
-void createStopLines( const autoware_map::AutowareMapHandler &awmap,
-                      std::vector<vector_map_msgs::Line> &vmap_lines,
-                      std::vector<vector_map_msgs::Point> &vmap_points,
-                      std::vector<vector_map_msgs::StopLine> &vmap_stop_lines,
-                      std::vector<vector_map_msgs::RoadSign> &vmap_road_signs,
-                      std::vector<vector_map_msgs::Vector> &vmap_vectors,
-                      std::vector<vector_map_msgs::Pole> &vmap_poles);
-void createWayAreas(const autoware_map::AutowareMapHandler &awmap, std::vector<vector_map_msgs::WayArea> &vmap_way_areas);
-void createWayAreasFromLanes(const autoware_map::AutowareMapHandler &awmap,
-                             std::vector<vector_map_msgs::WayArea> &vmap_way_areas,
-                             std::vector<vector_map_msgs::Area> &vmap_areas,
-                             std::vector<vector_map_msgs::Line> &vmap_lines,
-                             std::vector<vector_map_msgs::Point> &vmap_points );
-void createWhitelines(const autoware_map::AutowareMapHandler &awmap,
-                      std::vector<vector_map_msgs::Point> &vmap_points,
-                      std::vector<vector_map_msgs::Line> &vmap_lines,
-                      std::vector<vector_map_msgs::WhiteLine> &vmap_white_lines);
-void createDummyUtilityPoles(const std::vector<vector_map_msgs::Pole> vmap_poles,
-                             std::vector<vector_map_msgs::UtilityPole> &vmap_utility_poles);
-void convertPoint(vector_map_msgs::Point &vmap_point,const autoware_map_msgs::Point awmap_point);
-
-double distance2d(const autoware_map_msgs::Point p1, const autoware_map_msgs::Point p2);
-//keep angles within (M_PI, -M_PI]
-double addAngles(double angle1, double angle2);
-double getAngleAverage(std::vector<double> angles);
-double angleAverage(double angle1, double angle2);
-double convertDecimalToDDMMSS(const double decimal);
-void getMinMax(autoware_map_msgs::Point &min, autoware_map_msgs::Point &max, const std::vector<autoware_map_msgs::Point>points);
-//determine whether point lies within an area by counting winding number
-bool isWithinArea(double x, double y, const std::vector<autoware_map_msgs::Point> vertices);
-bool getIntersect(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4, double &intersect_x, double &intersect_y );
-int convertESPGToRef(int epsg);
-int getMaxId(std::vector<vector_map_msgs::Point> points);
-int getMaxId(std::vector<vector_map_msgs::Line> lines);
-int getMaxId(std::vector<vector_map_msgs::StopLine> stop_lines);
-int getMaxId(std::vector<vector_map_msgs::RoadSign> signs);
-int getMaxId(std::vector<vector_map_msgs::Area> areas);
-int getMaxId(const std::vector<vector_map_msgs::Vector> vectors);
-int getMaxId(const std::vector<vector_map_msgs::Pole> poles);
-int getMaxId(const std::vector<vector_map_msgs::WhiteLine> white_lines);
-int getMaxId(const std::vector<vector_map_msgs::WayArea> wayareas);
-
-#endif

src/ros/catkin/src/vector_map_converter/include/vector_map_converter/lanelet2autowaremap.hpp

@@ -1,107 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-#ifndef __LANELET2AUTOWAREMAP_HPP__
-#define __LANELET2AUTOWAREMAP_HPP__
-
-#include <typeinfo>
-#include <lanelet2_core/primitives/Lanelet.h>
-
-#include <lanelet2_core/geometry/Area.h>
-#include <lanelet2_core/geometry/Lanelet.h>
-#include <lanelet2_core/geometry/LineString.h>
-
-#include <lanelet2_io/Io.h>
-#include <lanelet2_io/io_handlers/Factory.h>
-#include <lanelet2_io/io_handlers/Writer.h>
-#include <lanelet2_projection/UTM.h>
-#include <lanelet2_traffic_rules/TrafficRules.h>
-#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
-#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
-#include <lanelet2_routing/Route.h>
-#include <lanelet2_routing/RoutingCost.h>
-#include <lanelet2_routing/RoutingGraph.h>
-#include <lanelet2_routing/RoutingGraphContainer.h>
-
-#include <boost/geometry.hpp>
-#include <boost/geometry/geometries/register/point.hpp>
-#include <boost/geometry/multi/geometries/register/multi_point.hpp>
-#include <boost/geometry/geometries/polygon.hpp>
-
-#include <string>
-#include <iostream>
-#include <sstream>
-#include <fstream>
-
-#include <ros/ros.h>
-#include <autoware_map/util.h>
-#include <autoware_map/map_handler.hpp>
-#include <vector_map_msgs/Area.h>
-#include <vector_map_msgs/CrossRoad.h>
-#include <vector_map_msgs/CrossWalk.h>
-#include <vector_map_msgs/DTLane.h>
-#include <vector_map_msgs/Lane.h>
-#include <vector_map_msgs/Line.h>
-#include <vector_map_msgs/Node.h>
-#include <vector_map_msgs/Point.h>
-#include <vector_map_msgs/Pole.h>
-#include <vector_map_msgs/RoadSign.h>
-#include <vector_map_msgs/Signal.h>
-#include <vector_map_msgs/StopLine.h>
-#include <vector_map_msgs/UtilityPole.h>
-#include <vector_map_msgs/Vector.h>
-#include <vector_map_msgs/WayArea.h>
-#include <vector_map_msgs/WhiteLine.h>
-
-void convertLanelet2AutowareMap(lanelet::LaneletMapPtr map,
-                                lanelet::projection::UtmProjector projector,
-                                std::vector<autoware_map_msgs::Area> &areas,
-                                std::vector<autoware_map_msgs::Lane> &lanes,
-                                std::vector<autoware_map_msgs::LaneAttributeRelation> &lane_attribute_relations,
-                                std::vector<autoware_map_msgs::LaneChangeRelation> &lane_change_relations,
-                                std::vector<autoware_map_msgs::LaneRelation> &lane_relations,
-                                std::vector<autoware_map_msgs::LaneSignalLightRelation> &lane_signal_light_relations,
-                                std::vector<autoware_map_msgs::OppositeLaneRelation> &opposite_lane_relations,
-                                std::vector<autoware_map_msgs::Point> &points,
-                                std::vector<autoware_map_msgs::Signal> &signals,
-                                std::vector<autoware_map_msgs::SignalLight> &signal_lights,
-                                std::vector<autoware_map_msgs::Wayarea> &wayareas,
-                                std::vector<autoware_map_msgs::Waypoint> &waypoints,
-                                std::vector<autoware_map_msgs::WaypointLaneRelation> &waypoint_lane_relations,
-                                std::vector<autoware_map_msgs::WaypointRelation> &waypoint_relations,
-                                std::vector<autoware_map_msgs::WaypointSignalRelation> &waypoint_signal_relations);
-
-void writeAutowareMapMsgs(std::string output_dir,
-                          std::vector<autoware_map_msgs::Area> &areas,
-                          std::vector<autoware_map_msgs::Lane> &lanes,
-                          std::vector<autoware_map_msgs::LaneAttributeRelation> &lane_attribute_relations,
-                          std::vector<autoware_map_msgs::LaneChangeRelation> &lane_change_relations,
-                          std::vector<autoware_map_msgs::LaneRelation> &lane_relations,
-                          std::vector<autoware_map_msgs::LaneSignalLightRelation> &lane_signal_light_relations,
-                          std::vector<autoware_map_msgs::OppositeLaneRelation> &opposite_lane_relations,
-                          std::vector<autoware_map_msgs::Point> &points,
-                          std::vector<autoware_map_msgs::Signal> &signals,
-                          std::vector<autoware_map_msgs::SignalLight> &signal_lights,
-                          std::vector<autoware_map_msgs::Wayarea> &wayareas,
-                          std::vector<autoware_map_msgs::Waypoint> &waypoints,
-                          std::vector<autoware_map_msgs::WaypointLaneRelation> &waypoint_lane_relations,
-                          std::vector<autoware_map_msgs::WaypointRelation> &waypoint_relations,
-                          std::vector<autoware_map_msgs::WaypointSignalRelation> &waypoint_signal_relations);
-
-autoware_map_msgs::Point convertPoint(const lanelet::Point3d lanelet_point, const lanelet::projection::UtmProjector &projector);
-autoware_map_msgs::Point convertPoint(const lanelet::ConstPoint3d &lanelet_point, const lanelet::projection::UtmProjector &projector);
-void fixPointCoordinate(autoware_map_msgs::Point &point);
-
-#endif

src/ros/catkin/src/vector_map_converter/include/vector_map_converter/lanelet2vectormap.hpp

@@ -1,46 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-#ifndef __LANELET2__LANELET2VECTORMAP_HPP__
-#define __LANELET2__LANELET2VECTORMAP_HPP__
-#include <vector_map_converter/autoware2vectormap.hpp>
-#include <vector_map_converter/lanelet2autowaremap.hpp>
-
-void complementVectorMap( lanelet::LaneletMapPtr map,
-                          lanelet::projection::UtmProjector projector,
-                          std::vector<vector_map_msgs::Line> &vmap_lines,
-                          std::vector<vector_map_msgs::Point> &vmap_points,
-                          std::vector<vector_map_msgs::Pole> &vmap_dummy_poles,
-                          std::vector<vector_map_msgs::RoadSign> &vmap_road_signs,
-                          std::vector<vector_map_msgs::StopLine> &vmap_stop_lines,
-                          std::vector<vector_map_msgs::Vector> &vmap_vectors,
-                          std::vector<vector_map_msgs::WhiteLine> &vmap_white_lines
-                          );
-
-void convertLineString2WhiteLine(lanelet::ConstLineString3d line_string,
-                                 lanelet::projection::UtmProjector projector,
-                                 std::vector<vector_map_msgs::Point> &vmap_points,
-                                 std::vector<vector_map_msgs::Line> &vmap_lines,
-                                 std::vector<vector_map_msgs::WhiteLine> &vmap_white_lines,
-                                 int &point_id, int &line_id, int &white_line_id );
-
-void createWhitelines(const std::vector<lanelet::ConstLanelet> &lanelets,
-                      lanelet::projection::UtmProjector projector,
-                      std::vector<vector_map_msgs::Point> &vmap_points,
-                      std::vector<vector_map_msgs::Line> &vmap_lines,
-                      std::vector<vector_map_msgs::WhiteLine> &vmap_white_lines
-                      );
-
-#endif

src/ros/catkin/src/vector_map_converter/package.xml

@@ -1,54 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <name>vector_map_converter</name>
-  <version>0.1.0</version>
-  <description>This package contains tools to convert different map formats.</description>
-  
-  <maintainer email="mitsudome-r@todo.todo">mitsudome-r</maintainer>
-
-  <license>Apache 2</license>
-
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>autoware_map</build_depend>
-  <build_depend>autoware_map_msgs</build_depend>
-  <build_depend>amathutils_lib</build_depend>
-  <build_depend>geodesy</build_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>lanelet2_core</build_depend>
-  <build_depend>lanelet2_io</build_depend>
-  <build_depend>lanelet2_projection</build_depend>
-  <build_depend>lanelet2_routing</build_depend>
-  <build_depend>lanelet2_traffic_rules</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>vector_map</build_depend>
-  <build_depend>op_planner</build_depend>
-  <build_export_depend>autoware_map</build_export_depend>
-  <build_export_depend>autoware_map_msgs</build_export_depend>
-  <build_export_depend>amathutils_lib</build_export_depend>
-  <build_export_depend>geodesy</build_export_depend>
-  <build_export_depend>geometry_msgs</build_export_depend>
-  <build_export_depend>lanelet2_core</build_export_depend>
-  <build_export_depend>lanelet2_io</build_export_depend>
-  <build_export_depend>lanelet2_projection</build_export_depend>
-  <build_export_depend>lanelet2_routing</build_export_depend>
-  <build_export_depend>lanelet2_traffic_rules</build_export_depend>
-  <build_export_depend>roscpp</build_export_depend>
-  <build_export_depend>vector_map</build_export_depend>
-  <build_export_depend>op_planner</build_export_depend>
-  <exec_depend>autoware_map</exec_depend>
-  <exec_depend>autoware_map_msgs</exec_depend>
-  <exec_depend>amathutils_lib</exec_depend>
-  <exec_depend>geodesy</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>lanelet2_core</exec_depend>
-  <exec_depend>lanelet2_io</exec_depend>
-  <exec_depend>lanelet2_projection</exec_depend>
-  <exec_depend>lanelet2_routing</exec_depend>
-  <exec_depend>lanelet2_traffic_rules</exec_depend>
-  <exec_depend>roscpp</exec_depend>
-  <exec_depend>vector_map</exec_depend>
-  <exec_depend>op_planner</exec_depend>
-
-  <export>
-  </export>
-</package>

src/ros/catkin/src/vector_map_converter/src/autoware2vectormap.cpp

@@ -1,254 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-#include <vector_map_converter/autoware2vectormap.hpp>
-
-template <class T>
-std::vector<T> parse(const std::string& csv_file)
-{
-    std::vector<T> objs;    
-    std::ifstream ifs(csv_file.c_str());
-    if( !ifs.good() )
-    {
-        ROS_WARN_STREAM("could not find file: " << csv_file);
-        return objs;        
-    }
-
-    std::string line;
-    std::getline(ifs, line); // remove first line
-    while (std::getline(ifs, line))
-    {
-        T obj;
-        std::istringstream iss(line);
-        iss >> obj;
-        objs.push_back(obj);
-    }
-    return objs;
-}
-
-void readFiles( const std::string directory_path,
-                std::vector<autoware_map_msgs::Area> &areas,
-                std::vector<autoware_map_msgs::Lane> &lanes,
-                std::vector<autoware_map_msgs::LaneAttributeRelation> &lane_attribute_relations,
-                std::vector<autoware_map_msgs::LaneRelation> &lane_relations,
-                std::vector<autoware_map_msgs::LaneSignalLightRelation> &lane_signal_relations,
-                std::vector<autoware_map_msgs::LaneChangeRelation> &lane_change_relations,
-                std::vector<autoware_map_msgs::OppositeLaneRelation> &opposite_lane_relations,
-                std::vector<autoware_map_msgs::Point> &points,
-                std::vector<autoware_map_msgs::Signal> &signals,
-                std::vector<autoware_map_msgs::SignalLight> &signal_lights,
-                std::vector<autoware_map_msgs::Wayarea> &wayareas,
-                std::vector<autoware_map_msgs::Waypoint> &waypoints,
-                std::vector<autoware_map_msgs::WaypointLaneRelation> &waypoint_lane_relations,
-                std::vector<autoware_map_msgs::WaypointRelation> &waypoint_relations,
-                std::vector<autoware_map_msgs::WaypointSignalRelation> &waypoint_signal_relations
-                )
-{
-    std::string file_path;
-
-    file_path = directory_path + "/points.csv";
-    points = parse<autoware_map_msgs::Point>(file_path);
-    
-    file_path = directory_path + "/lanes.csv";
-    lanes = parse<autoware_map_msgs::Lane>(file_path);
-    
-    file_path = directory_path + "/lane_attribute_relations.csv";
-    lane_attribute_relations = parse<autoware_map_msgs::LaneAttributeRelation>(file_path);
-    
-    file_path = directory_path + "/lane_relations.csv";
-    lane_relations = parse<autoware_map_msgs::LaneRelation>(file_path);
-    
-    file_path = directory_path + "/lane_signal_light_relations.csv";
-    lane_signal_relations = parse<autoware_map_msgs::LaneSignalLightRelation>(file_path);
-    
-    file_path = directory_path + "/lane_change_relations.csv";
-    lane_change_relations = parse<autoware_map_msgs::LaneChangeRelation>(file_path);
-    
-    file_path = directory_path + "/opposite_lane_relations.csv";
-    opposite_lane_relations = parse<autoware_map_msgs::OppositeLaneRelation>(file_path);
-    
-    file_path = directory_path + "/areas.csv";
-    areas = parse<autoware_map_msgs::Area>(file_path);
-    
-    file_path = directory_path + "/signals.csv";
-    signals = parse<autoware_map_msgs::Signal>(file_path);
-    
-    file_path = directory_path + "/signal_lights.csv";
-    signal_lights = parse<autoware_map_msgs::SignalLight>(file_path);
-    
-    file_path = directory_path + "/wayareas.csv";
-    wayareas = parse<autoware_map_msgs::Wayarea>(file_path);
-    
-    file_path = directory_path + "/waypoints.csv";
-    waypoints = parse<autoware_map_msgs::Waypoint>(file_path);
-    
-    file_path = directory_path + "/waypoint_lane_relations.csv";
-    waypoint_lane_relations = parse<autoware_map_msgs::WaypointLaneRelation>(file_path);
-    
-    file_path = directory_path + "/waypoint_relations.csv";
-    waypoint_relations = parse<autoware_map_msgs::WaypointRelation>(file_path);
-    
-    file_path = directory_path + "/waypoint_signal_relations.csv";
-    waypoint_signal_relations = parse<autoware_map_msgs::WaypointSignalRelation>(file_path);
-}
-
-
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "awm_vmap_converter");
-    ros::NodeHandle nh;
-    ros::NodeHandle pnh("~");
-
-    autoware_map::category_t category = autoware_map::Category::NONE;
-
-    std::vector<std::string> file_paths;
-    for (int i = 1; i < argc; ++i)
-    {
-        std::string file_path(argv[i]);
-        file_paths.push_back(file_path);
-    }
-    
-    std::string map_dir;
-    pnh.param<std::string>("map_dir", map_dir, "./");
-    
-    std::string save_dir;
-    pnh.param<std::string>("save_dir", save_dir, "./");
-
-    bool create_whitelines;
-    bool wayareas_from_lanes;
-    pnh.param<bool>("create_whitelines", create_whitelines, false);
-    pnh.param<bool>("wayareas_from_lanes", wayareas_from_lanes, false);
-
-
-    std::vector<autoware_map_msgs::Point> points;
-    std::vector<autoware_map_msgs::Lane> lanes;
-    std::vector<autoware_map_msgs::LaneAttributeRelation> lane_attribute_relations;
-    std::vector<autoware_map_msgs::LaneRelation> lane_relations;
-    std::vector<autoware_map_msgs::LaneSignalLightRelation> lane_signal_light_relations;
-    std::vector<autoware_map_msgs::LaneChangeRelation> lane_change_relations;
-    std::vector<autoware_map_msgs::OppositeLaneRelation> opposite_lane_relations;
-    std::vector<autoware_map_msgs::Area> areas;
-    std::vector<autoware_map_msgs::Signal> signals;
-    std::vector<autoware_map_msgs::SignalLight> signal_lights;
-    std::vector<autoware_map_msgs::Wayarea> wayareas;
-    std::vector<autoware_map_msgs::Waypoint> waypoints;
-    std::vector<autoware_map_msgs::WaypointLaneRelation> waypoint_lane_relations;
-    std::vector<autoware_map_msgs::WaypointRelation> waypoint_relations;
-    std::vector<autoware_map_msgs::WaypointSignalRelation> waypoint_signal_relations;
-
-    std::vector<vector_map_msgs::Area> vmap_areas;
-    std::vector<vector_map_msgs::CrossRoad> vmap_cross_roads;
-    std::vector<vector_map_msgs::CrossWalk> vmap_cross_walks;
-    std::vector<vector_map_msgs::DTLane> vmap_dtlanes;
-    std::vector<vector_map_msgs::Lane> vmap_lanes;
-    std::vector<vector_map_msgs::Line> vmap_lines;
-    std::vector<vector_map_msgs::Node> vmap_nodes;
-    std::vector<vector_map_msgs::Point> vmap_points;
-    std::vector<vector_map_msgs::Pole> vmap_dummy_poles;
-    std::vector<vector_map_msgs::RoadSign> vmap_road_signs;
-    std::vector<vector_map_msgs::Signal> vmap_signals;
-    std::vector<vector_map_msgs::StopLine> vmap_stop_lines;
-    std::vector<vector_map_msgs::UtilityPole> vmap_dummy_utility_poles;
-    std::vector<vector_map_msgs::Vector> vmap_vectors;
-    std::vector<vector_map_msgs::WayArea> vmap_way_areas;
-    std::vector<vector_map_msgs::WhiteLine> vmap_white_lines;
-
-    readFiles(map_dir,
-              areas,
-              lanes,
-              lane_attribute_relations,
-              lane_relations,
-              lane_signal_light_relations,
-              lane_change_relations,
-              opposite_lane_relations,
-              points,
-              signals,
-              signal_lights,
-              wayareas,
-              waypoints,
-              waypoint_lane_relations,
-              waypoint_relations,
-              waypoint_signal_relations
-              );
-
-    autoware_map::AutowareMapHandler map_handler;
-
-    map_handler.setFromAutowareMapMsgs(areas,
-                                       lanes,
-                                       lane_attribute_relations,
-                                       lane_change_relations,
-                                       lane_relations,
-                                       lane_signal_light_relations,
-                                       opposite_lane_relations,
-                                       points,
-                                       signals,
-                                       signal_lights,
-                                       wayareas,
-                                       waypoints,
-                                       waypoint_lane_relations,
-                                       waypoint_relations,
-                                       waypoint_signal_relations
-                                       );
-    map_handler.resolveRelations();
-
-    getVectorMapMsgs( map_handler,
-                      vmap_areas,
-                      vmap_cross_roads,
-                      vmap_cross_walks,
-                      vmap_dtlanes,
-                      vmap_lanes,
-                      vmap_lines,
-                      vmap_nodes,
-                      vmap_points,
-                      vmap_dummy_poles,
-                      vmap_road_signs,
-                      vmap_signals,
-                      vmap_stop_lines,
-                      vmap_dummy_utility_poles,
-                      vmap_vectors,
-                      vmap_way_areas,
-                      vmap_white_lines
-                      );
-
-   if( create_whitelines )
-   {
-     createWhitelines(map_handler, vmap_points, vmap_lines, vmap_white_lines);
-   }
-
-   if( wayareas_from_lanes )
-   {
-     createWayAreasFromLanes(map_handler, vmap_way_areas, vmap_areas, vmap_lines, vmap_points);
-   }
-
-    writeVectorMapMsgs( save_dir,
-                        vmap_areas,
-                        vmap_cross_roads,
-                        vmap_cross_walks,
-                        vmap_dtlanes,
-                        vmap_lanes,
-                        vmap_lines,
-                        vmap_nodes,
-                        vmap_points,
-                        vmap_dummy_poles,
-                        vmap_road_signs,
-                        vmap_signals,
-                        vmap_stop_lines,
-                        vmap_dummy_utility_poles,
-                        vmap_vectors,
-                        vmap_way_areas,
-                        vmap_white_lines
-                        );
-}

src/ros/catkin/src/vector_map_converter/src/autoware2vectormap_core.cpp

@@ -1,1577 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-#include <vector_map_converter/autoware2vectormap.hpp>
-#include <unordered_map>
-#include <boost/functional/hash.hpp>
-#include <vector_map/vector_map.h>
-#include <amathutils_lib/amathutils.hpp>
-template <class T>
-void write(std::ofstream &ofs, std::vector<T> objs)
-{
-  for( auto obj : objs)
-  {
-    ofs << std::fixed << obj << std::endl;
-  }
-}
-
-bool open_file(const std::string file_path, std::ofstream &ofs)
-{
-  ofs.open(file_path);
-  if(!ofs.good())
-  {
-    ROS_ERROR_STREAM("Failed to open " << file_path);
-  }
-  return ofs.good();
-}
-
-using uint_pair = std::pair<unsigned int, unsigned int>;
-
-void writeVectorMapMsgs(const std::string output_dir,
-                        const std::vector<vector_map_msgs::Area> &vmap_areas,
-                        const std::vector<vector_map_msgs::CrossRoad> &vmap_cross_roads,
-                        const std::vector<vector_map_msgs::CrossWalk> &vmap_cross_walks,
-                        const std::vector<vector_map_msgs::DTLane> &vmap_dtlanes,
-                        const std::vector<vector_map_msgs::Lane> &vmap_lanes,
-                        const std::vector<vector_map_msgs::Line> &vmap_lines,
-                        const std::vector<vector_map_msgs::Node> &vmap_nodes,
-                        const std::vector<vector_map_msgs::Point> &vmap_points,
-                        const std::vector<vector_map_msgs::Pole> &vmap_poles,
-                        const std::vector<vector_map_msgs::RoadSign> &vmap_road_signs,
-                        const std::vector<vector_map_msgs::Signal> &vmap_signals,
-                        const std::vector<vector_map_msgs::StopLine> &vmap_stop_lines,
-                        const std::vector<vector_map_msgs::UtilityPole> &vmap_utility_poles,
-                        const std::vector<vector_map_msgs::Vector> &vmap_vectors,
-                        const std::vector<vector_map_msgs::WayArea> &vmap_way_areas,
-                        const std::vector<vector_map_msgs::WhiteLine> &vmap_white_lines
-                        )
-{
-  std::ofstream ofs;
-  std::string filename;
-
-  //areas
-  if(!vmap_areas.empty())
-  {
-    filename = output_dir + "/area.csv";
-    if( open_file(filename, ofs))
-    {
-      ofs << "AID,SLID,ELID" << std::endl;
-      write(ofs, vmap_areas);
-    }
-    ofs.close();
-  }
-
-  //cross_roads
-  if(!vmap_cross_roads.empty())
-  {
-    filename = output_dir + "/intersection.csv";
-    if( open_file(filename, ofs))
-    {
-      ofs << "ID,AID,LinkID" << std::endl;
-      write(ofs, vmap_cross_roads);
-    }
-    ofs.close();
-  }
-  //cross_walk
-  if(!vmap_cross_walks.empty())
-  {
-    filename = output_dir + "/crosswalk.csv";
-    if( open_file(filename, ofs))
-    {
-      ofs << "ID,AID,Type,BdID,LinkID" << std::endl;
-      write(ofs, vmap_cross_walks);
-    }
-    ofs.close();
-  }
-
-  //DTLane
-  if(!vmap_dtlanes.empty())
-  {
-    filename = output_dir + "/dtlane.csv";
-    if( open_file(filename, ofs))
-    {
-      ofs << "DID,Dist,PID,Dir,Apara,r,slope,cant,LW,RW" << std::endl;
-      write(ofs, vmap_dtlanes);
-    }
-    ofs.close();
-  }
-  //Lanes
-  if(!vmap_lanes.empty())
-  {
-    filename = output_dir + "/lane.csv";
-    if( open_file(filename, ofs))
-    {
-      ofs << "LnID,DID,BLID,FLID,BNID,FNID,JCT,BLID2,BLID3,BLID4,FLID2,FLID3,FLID4,ClossID,Span,LCnt,Lno,LaneType,LimitVel,RefVel,RoadSecID,LaneChgFG,LinkWAID" << std::endl;
-      write(ofs, vmap_lanes);
-    }
-    ofs.close();
-  }
-  //Lanes
-  if(!vmap_lines.empty())
-  {
-    filename = output_dir + "/line.csv";
-    if( open_file(filename, ofs))
-    {
-      ofs << "LID,BPID,FPID,BLID,FLID" << std::endl;
-      write(ofs, vmap_lines);
-    }
-    ofs.close();
-  }
-  //nodes
-  if(!vmap_nodes.empty())
-  {
-    filename = output_dir + "/node.csv";
-    if( open_file(filename, ofs))
-    {
-      ofs << "NID,PID" << std::endl;
-      write(ofs, vmap_nodes);
-    }
-    ofs.close();
-  }
-  //points
-  if(!vmap_points.empty())
-  {
-    filename = output_dir + "/point.csv";
-    if( open_file(filename, ofs))
-    {
-      ofs << "PID,B,L,H,Bx,Ly,ReF,MCODE1,MCODE2,MCODE3" << std::endl;
-      write(ofs, vmap_points);
-    }
-    ofs.close();
-  }
-  //poles
-  if(!vmap_poles.empty())
-  {
-    filename = output_dir + "/pole.csv";
-    if( open_file(filename, ofs))
-    {
-      ofs << "PLID,VID,Length,Dim" << std::endl;
-      write(ofs, vmap_poles);
-    }
-    ofs.close();
-  }
-  //utility poles
-  if(!vmap_utility_poles.empty())
-  {
-    filename = output_dir + "/poledata.csv";
-    if( open_file(filename, ofs))
-    {
-      ofs << "ID,PLID,LinkID" << std::endl;
-      write(ofs, vmap_utility_poles);
-    }
-    ofs.close();
-  }
-  //road_signs
-  if(!vmap_road_signs.empty())
-  {
-    filename = output_dir + "/roadsign.csv";
-    if( open_file(filename, ofs))
-    {
-      ofs << "ID,VID,PLID,Type,LinkID" << std::endl;
-      write(ofs, vmap_road_signs);
-    }
-    ofs.close();
-  }
-  //signals
-  if(!vmap_signals.empty())
-  {
-    filename = output_dir + "/signaldata.csv";
-    if( open_file(filename, ofs))
-    {
-      ofs << "ID,VID,PLID,Type,LinkID" << std::endl;
-      write(ofs, vmap_signals);
-    }
-    ofs.close();
-  }
-  //stop_line
-  if(!vmap_stop_lines.empty())
-  {
-    filename = output_dir + "/stopline.csv";
-    if( open_file(filename, ofs))
-    {
-      ofs << "ID,LID,TLID,SignID,LinkID" << std::endl;
-      write(ofs, vmap_stop_lines);
-    }
-    ofs.close();
-  }
-  //vectors
-  if(!vmap_vectors.empty())
-  {
-    filename = output_dir + "/vector.csv";
-    if( open_file(filename, ofs))
-    {
-      ofs << "VID,PID,Hang,Vang" << std::endl;
-      write(ofs, vmap_vectors);
-    }
-    ofs.close();
-  }
-  //wayareas
-  if(!vmap_way_areas.empty())
-  {
-    filename = output_dir + "/wayarea.csv";
-    if( open_file(filename, ofs))
-    {
-      ofs << "ID,AID" << std::endl;
-      write(ofs, vmap_way_areas);
-    }
-    ofs.close();
-  }
-
-  if(!vmap_white_lines.empty())
-  {
-    filename = output_dir + "/whiteline.csv";
-    if( open_file(filename, ofs))
-    {
-      ofs << "ID,LID,Width,Color,type,LinkID" << std::endl;
-      write(ofs, vmap_white_lines);
-    }
-    ofs.close();
-  }
-}
-void getVectorMapMsgs(
-  const autoware_map::AutowareMapHandler &map_handler,
-  std::vector<vector_map_msgs::Area> &vmap_areas,
-  std::vector<vector_map_msgs::CrossRoad> &vmap_cross_roads,
-  std::vector<vector_map_msgs::CrossWalk> &vmap_cross_walks,
-  std::vector<vector_map_msgs::DTLane> &vmap_dtlanes,
-  std::vector<vector_map_msgs::Lane> &vmap_lanes,
-  std::vector<vector_map_msgs::Line> &vmap_lines,
-  std::vector<vector_map_msgs::Node> &vmap_nodes,
-  std::vector<vector_map_msgs::Point> &vmap_points,
-  std::vector<vector_map_msgs::Pole> &vmap_dummy_poles,
-  std::vector<vector_map_msgs::RoadSign> &vmap_road_signs,
-  std::vector<vector_map_msgs::Signal> &vmap_signals,
-  std::vector<vector_map_msgs::StopLine> &vmap_stop_lines,
-  std::vector<vector_map_msgs::UtilityPole> &vmap_dummy_utility_poles,
-  std::vector<vector_map_msgs::Vector> &vmap_vectors,
-  std::vector<vector_map_msgs::WayArea> &vmap_way_areas,
-  std::vector<vector_map_msgs::WhiteLine> &vmap_white_lines
-  )
-{
-  createAreas(map_handler, vmap_areas, vmap_lines);
-  createNodes(map_handler, vmap_nodes);
-  createPoints(map_handler, vmap_points);
-  createDTLanes( map_handler, vmap_dtlanes,vmap_lanes);
-  createCrossWalks(map_handler, vmap_cross_walks, vmap_areas, vmap_lines, vmap_points);
-  createCrossRoads(map_handler, vmap_cross_roads);
-  createSignals(map_handler, vmap_signals, vmap_vectors,vmap_dummy_poles);
-  createStopLines(map_handler, vmap_lines, vmap_points, vmap_stop_lines, vmap_road_signs, vmap_vectors, vmap_dummy_poles);
-  createWayAreas(map_handler, vmap_way_areas);
-  // createWayAreasFromLanes(map_handler, vmap_way_areas, vmap_areas, vmap_lines, vmap_points);
-  createDummyUtilityPoles(vmap_dummy_poles, vmap_dummy_utility_poles);
-//  createWhitelines(map_handler, vmap_points, vmap_lines, vmap_white_lines);
-}
-
-void createAreas(const autoware_map::AutowareMapHandler &map_handler, std::vector<vector_map_msgs::Area> &vmap_areas, std::vector<vector_map_msgs::Line> &vmap_lines)
-{
-  int line_id = getMaxId(vmap_lines) + 1;
-  for ( auto awmap_area : map_handler.findByFilter( [&](autoware_map::Area a){return true; }))
-  {
-    vector_map_msgs::Area vmap_area;
-    vmap_area.aid = awmap_area.area_id;
-    vmap_area.slid = line_id;
-
-    //create lines that represent area
-    auto end_itr = awmap_area.point_ids.end();
-    auto begin_itr = awmap_area.point_ids.begin();
-    //assumes that point id of corresponding points in AutowareMap and VectorMap are the same. (please check createPoints function)
-    for (auto point_itr = begin_itr; point_itr != end_itr; point_itr++)
-    {
-      vector_map_msgs::Line line;
-      line.lid =line_id;
-      line.bpid = *point_itr;
-
-      if(point_itr == begin_itr)
-      {
-        line.blid = 0;
-      }
-      else{
-        line.blid = line_id - 1;
-      }
-
-      if(point_itr + 1 == end_itr)
-      {
-        line.flid = 0;
-        line.fpid = *begin_itr;           //close the loop
-      }
-      else{
-        line.flid = line_id + 1;
-        line.fpid = *(point_itr + 1);
-      }
-      vmap_lines.push_back(line);
-      vmap_area.elid = line_id;
-      line_id++;
-    }
-    vmap_areas.push_back(vmap_area);
-  }
-}
-
-
-void convertPoint(vector_map_msgs::Point &vmap_point,const autoware_map_msgs::Point awmap_point)
-{
-  if(isJapaneseCoordinate(awmap_point.epsg))
-  {
-    vmap_point.bx = awmap_point.x;
-    vmap_point.ly = awmap_point.y;
-  }
-  else{
-    vmap_point.bx = awmap_point.y;
-    vmap_point.ly = awmap_point.x;
-  }
-  vmap_point.ref = convertESPGToRef(awmap_point.epsg);
-  vmap_point.pid = awmap_point.point_id;
-  vmap_point.b = convertDecimalToDDMMSS( awmap_point.lat );
-  vmap_point.l = convertDecimalToDDMMSS( awmap_point.lng );
-  vmap_point.h = awmap_point.z;
-
-  //cannot convert mcodes from autoware_map_format
-  vmap_point.mcode1 = 0;
-  vmap_point.mcode2 = 0;
-  vmap_point.mcode3 = 0;
-}
-
-
-void createPoints(const autoware_map::AutowareMapHandler &awmap, std::vector<vector_map_msgs::Point> &vmap_points)
-{
-
-  bool epsg_fail_flag = false;
-  for ( auto awmap_pt : awmap.findByFilter( [&](autoware_map::Point pt){return true; }) )
-  {
-    if(isJapaneseCoordinate(awmap_pt.epsg) && !epsg_fail_flag) {
-      epsg_fail_flag =true;
-      ROS_WARN_STREAM("no corresponding Japanese Plane Rectangular CS Number for specified epsg value" );
-    }
-    vector_map_msgs::Point vmap_point;
-    convertPoint(vmap_point, awmap_pt);
-    vmap_points.push_back(vmap_point);
-  }
-}
-
-void createNodes(const autoware_map::AutowareMapHandler &awmap, std::vector<vector_map_msgs::Node> &vmap_nodes)
-{
-  for ( auto awmap_wp : awmap.findByFilter([&](autoware_map::Waypoint){return true; } ))
-  {
-    vector_map_msgs::Node vmap_node;
-    vmap_node.nid = awmap_wp.waypoint_id;
-    vmap_node.pid = awmap_wp.point_id;
-    vmap_nodes.push_back(vmap_node);
-  }
-}
-
-std::vector<int> findBranchingIdx(const std::vector<autoware_map::WaypointRelation> &relation, int root_index, const std::unordered_map<uint_pair, unsigned int, boost::hash<uint_pair> > &lookup_table)
-{
-  std::vector<int> branching_indices;
-  for(const auto &r : relation)
-  {
-    if(r.waypoint_id == root_index) {
-      auto key = std::make_pair(r.waypoint_id, r.next_waypoint_id);
-      branching_indices.push_back(lookup_table.at(key));
-    }
-  }
-  return branching_indices;
-}
-std::vector<int> findMergingIdx(const std::vector<autoware_map::WaypointRelation> &relation, int merged_index,const std::unordered_map<uint_pair, unsigned int, boost::hash<uint_pair> > &lookup_table)
-{
-  std::vector<int> merging_indices;
-
-  for(const auto &r : relation)
-  {
-    if(r.next_waypoint_id == merged_index)
-    {
-      auto key = std::make_pair(r.waypoint_id, r.next_waypoint_id);
-      merging_indices.push_back(lookup_table.at(key));
-    }
-  }
-  return merging_indices;
-}
-
-int getJunctionType(const std::vector<autoware_map::WaypointRelation> &awmap_waypoint_relations,
-                    std::vector<int> branching_indices,
-                    std::vector<int> merging_indices)
-{
-  if(branching_indices.size() <= 1 && merging_indices.size() <= 1 )
-  {
-    return vector_map_msgs::Lane::NORMAL;
-  }
-
-  // if(merging_indices.size() > 1) return vector_map_msgs::Lane::RIGHT_MERGING;
-  // else         return vector_map_msgs::Lane::NORMAL;
-  sort(merging_indices.begin(), merging_indices.end(), [&](const int x, const int y){ return awmap_waypoint_relations.at(x).blinker < awmap_waypoint_relations.at(y).blinker; });
-  sort(branching_indices.begin(), branching_indices.end(), [&](const int x, const int y){ return awmap_waypoint_relations.at(x).blinker < awmap_waypoint_relations.at(y).blinker; });
-
-  int left_branching_cnt = 0;
-  int right_branching_cnt = 0;
-  int straight_branching_cnt = 0;
-  int left_merging_cnt = 0;
-  int right_merging_cnt = 0;
-  int straight_merging_cnt = 0;
-
-  for(auto idx : branching_indices)
-  {
-    if( awmap_waypoint_relations.at(idx).blinker == 1 )
-    {
-      left_branching_cnt++;
-    }
-    if( awmap_waypoint_relations.at(idx).blinker == 2 )
-    {
-      right_branching_cnt++;
-    }else{
-      straight_branching_cnt++;
-    }
-  }
-  for(auto idx : merging_indices)
-  {
-    if( awmap_waypoint_relations.at(idx).blinker == 1 )
-    {
-      left_merging_cnt++;
-    }
-    if( awmap_waypoint_relations.at(idx).blinker == 2 )
-    {
-      right_merging_cnt++;
-    }else{
-      straight_merging_cnt++;
-    }
-  }
-
-  if(branching_indices.size() >= 2 && merging_indices.size() >= 2 )
-  {
-    return vector_map_msgs::Lane::COMPOSITION;
-  }
-  if ( right_branching_cnt >= 1 )
-  {
-    return vector_map_msgs::Lane::RIGHT_BRANCHING;
-  }
-  if ( left_branching_cnt >= 1 )
-  {
-    return vector_map_msgs::Lane::LEFT_BRANCHING;
-  }
-  if( straight_branching_cnt >= 2) {
-    return vector_map_msgs::Lane::LEFT_BRANCHING;
-  }
-  if ( right_merging_cnt >= 1 )
-  {
-    return vector_map_msgs::Lane::RIGHT_MERGING;
-  }
-  if ( left_merging_cnt >= 1 )
-  {
-    return vector_map_msgs::Lane::LEFT_MERGING;
-  }
-  if( straight_merging_cnt >= 2) {
-    return vector_map_msgs::Lane::LEFT_MERGING;
-  }
-
-  ROS_ERROR_STREAM("could not find appropriate junction type!!!!!!!");
-
-  return vector_map_msgs::Lane::NORMAL;
-}
-
-
-
-void createDTLanes(const autoware_map::AutowareMapHandler &awmap,
-                   std::vector<vector_map_msgs::DTLane> &vmap_dtlanes,
-                   std::vector<vector_map_msgs::Lane> &vmap_lanes)
-{
-  std::vector<autoware_map::WaypointRelation> awmap_waypoint_relations = awmap.findByFilter( [&](autoware_map::WaypointRelation wr){return true; });
-
-  //create lookup table:
-  // key = waypoint_relation
-  // value = index in waypoint_relation;
-  std::unordered_map<uint_pair, unsigned int, boost::hash<uint_pair> > wp_relation2index;
-  unsigned int idx = 0;
-  for ( auto r : awmap_waypoint_relations )
-  {
-    auto key = std::make_pair(r.waypoint_id,r.next_waypoint_id);
-    wp_relation2index[key] = idx++;
-  }
-
-  unsigned lnid = 1;
-  for ( auto awmap_waypoint_relation : awmap_waypoint_relations )
-  {
-    if(lnid % 1000 == 0)
-      std::cout << lnid << "/" << awmap_waypoint_relations.size() << std::endl;
-    autoware_map::Waypoint awmap_waypoint = *(awmap_waypoint_relation.getWaypointPtr());
-    autoware_map::Waypoint awmap_next_waypoint = *(awmap_waypoint_relation.getNextWaypointPtr());
-    //create dtlane
-    vector_map_msgs::DTLane vmap_dtlane;
-    vmap_dtlane.did = lnid;
-    vmap_dtlane.dist = awmap_waypoint_relation.distance;
-    vmap_dtlane.pid = awmap_waypoint_relation.getWaypointPtr()->point_id;
-    vmap_dtlane.dir = convertDecimalToDDMMSS(awmap_waypoint_relation.yaw);
-    vmap_dtlane.apara = 0;
-    vmap_dtlane.r = 90000000000;
-    autoware_map_msgs::Point pt1, pt2;
-    pt1 = *(awmap_waypoint.getPointPtr());
-    pt2 = *(awmap_next_waypoint.getPointPtr());
-    double horizontal_dist = hypot(pt2.x - pt1.x, pt2.y - pt1.y);
-    double vertical_dist = pt2.z - pt1.z;
-    vmap_dtlane.slope = vertical_dist / horizontal_dist * 100;     //decimal to percentage
-    vmap_dtlane.cant = 0;
-    vmap_dtlane.lw = awmap_waypoint.left_width;
-    vmap_dtlane.rw = awmap_waypoint.right_width;
-    vmap_dtlanes.push_back(vmap_dtlane);
-
-    //create lane
-    vector_map_msgs::Lane vmap_lane;
-    vmap_lane.lnid = lnid;
-    vmap_lane.did = lnid;
-
-    std::vector<int> merging_indices = findMergingIdx(awmap_waypoint.getStdWaypointRelations(), awmap_waypoint.waypoint_id, wp_relation2index);
-    std::vector<int> branching_indices = findBranchingIdx(awmap_next_waypoint.getStdWaypointRelations(), awmap_next_waypoint.waypoint_id, wp_relation2index);
-    // //change order of branch/merge lanes according to blinkers. (staright < left turn < right turn)
-    vmap_lane.jct = getJunctionType(awmap_waypoint_relations, branching_indices, merging_indices);
-    vmap_lane.blid = 0;
-    vmap_lane.flid = 0;
-    vmap_lane.blid2 = 0;
-    vmap_lane.blid3 = 0;
-    vmap_lane.blid4 = 0;
-    vmap_lane.flid2 = 0;
-    vmap_lane.flid3 = 0;
-    vmap_lane.flid4 = 0;
-    //lnid = [index of waypoint_relations] + 1
-    if(merging_indices.size() >= 1)
-      vmap_lane.blid = merging_indices.at(0) + 1;
-    if(merging_indices.size() >= 2)
-      vmap_lane.blid2 = merging_indices.at(1) + 1;
-    if(merging_indices.size() >= 3)
-      vmap_lane.blid3 = merging_indices.at(2) + 1;
-    if(merging_indices.size() >= 4)
-      vmap_lane.blid4 = merging_indices.at(3) + 1;
-    if(branching_indices.size() >= 1)
-      vmap_lane.flid = branching_indices.at(0) + 1;
-    if(branching_indices.size() >= 2)
-      vmap_lane.flid2 = branching_indices.at(1) + 1;
-    if(branching_indices.size() >= 3)
-      vmap_lane.flid3 = branching_indices.at(2) + 1;
-    if(branching_indices.size() >= 4)
-      vmap_lane.flid4 = branching_indices.at(3) + 1;
-    vmap_lane.bnid = awmap_waypoint.waypoint_id;
-    vmap_lane.fnid = awmap_next_waypoint.waypoint_id;
-    vmap_lane.span =  awmap_waypoint_relation.distance;
-    vmap_lane.lanetype = awmap_waypoint_relation.blinker;
-    vmap_lane.refvel = awmap_waypoint.velocity;
-
-    //find lane that both current and next waypoint belongs to
-    autoware_map_msgs::Lane awmap_lane;
-    for( auto candidate : awmap_waypoint.getBelongingLanes())
-    {
-      if( awmap_next_waypoint.belongLane(candidate->lane_id))
-      {
-        awmap_lane = *candidate;
-        break;
-      }
-    }
-
-    vmap_lane.lcnt = awmap_lane.num_of_lanes;
-    vmap_lane.limitvel = awmap_lane.speed_limit;
-    vmap_lane.lanecfgfg = (vmap_lane.lcnt > 1) ? 1 : 0;
-    vmap_lane.lno = awmap_lane.lane_number;
-
-    vmap_lane.roadsecid = 0;
-    vmap_lane.linkwaid = 0;
-    vmap_lanes.push_back(vmap_lane);
-    lnid++;
-  }
-}
-
-void createCrossRoads(const autoware_map::AutowareMapHandler &awmap, std::vector<vector_map_msgs::CrossRoad> &vmap_cross_roads)
-{
-  unsigned int id = 1;
-  for ( auto awmap_relation : awmap.findByFilter( [&](autoware_map::LaneAttributeRelation lar){return lar.attribute_type == autoware_map_msgs::LaneAttributeRelation::INTERSECTION; }) )
-  {
-    //check whether same cross_road is already created
-    if(std::find_if(vmap_cross_roads.begin(), vmap_cross_roads.end(), [&](vector_map_msgs::CrossRoad cr){return cr.aid == awmap_relation.area_id; }) != vmap_cross_roads.end())
-    {
-      continue;
-    }
-    vector_map_msgs::CrossRoad cross_road;
-    cross_road.id = id++;
-    cross_road.aid = awmap_relation.area_id;
-    cross_road.linkid = 0;
-    vmap_cross_roads.push_back(cross_road);
-  }
-}
-
-
-int createSquareArea(const double x, const double y, const double z, const double length,
-                     std::vector<vector_map_msgs::Area> &vmap_areas, std::vector<vector_map_msgs::Line> &vmap_lines,
-                     std::vector<vector_map_msgs::Point> &vmap_points)
-{
-  vector_map_msgs::Point v1, v2, v3, v4;
-  int point_id = getMaxId(vmap_points) + 1;
-  v1.pid = point_id++;
-  v1.bx = x - length / 2;
-  v1.ly = y - length / 2;
-  v1.h = z;
-
-  v2.pid = point_id++;
-  v2.bx = x - length / 2;
-  v2.ly = y + length / 2;
-  v2.h = z;
-
-  v3.pid = point_id++;
-  v3.bx = x + length / 2;
-  v3.ly = y + length / 2;
-  v3.h = z;
-
-  v4.pid = point_id++;
-  v4.bx = x + length / 2;
-  v4.ly = y - length / 2;
-  v4.h = z;
-
-  vmap_points.push_back(v1);
-  vmap_points.push_back(v2);
-  vmap_points.push_back(v3);
-  vmap_points.push_back(v4);
-
-  vector_map_msgs::Line line;
-  int lid, start_lid;
-  lid =start_lid= getMaxId(vmap_lines) + 1;
-  line.lid = lid;
-  line.bpid = v1.pid;
-  line.fpid = v2.pid;
-  line.blid = 0;
-  line.flid = lid + 1;
-  vmap_lines.push_back(line);
-  lid++;
-  line.lid = lid;
-  line.bpid = v2.pid;
-  line.fpid = v3.pid;
-  line.blid = lid - 1;
-  line.flid = lid + 1;
-  vmap_lines.push_back(line);
-  lid++;
-  line.lid = lid;
-  line.bpid = v3.pid;
-  line.fpid = v4.pid;
-  line.blid = lid - 1;
-  line.flid = lid + 1;
-  vmap_lines.push_back(line);
-  lid++;
-  line.lid = lid;
-  line.bpid = v4.pid;
-  line.fpid = v1.pid;
-  line.blid = lid - 1;
-  line.flid = 0;
-  vmap_lines.push_back(line);
-
-  vector_map_msgs::Area area;
-  area.aid = getMaxId(vmap_areas) + 1;
-  area.slid = start_lid;
-  area.elid = lid;
-  vmap_areas.push_back(area);
-
-  return area.aid;
-}
-
-void createCrossWalks(const autoware_map::AutowareMapHandler &awmap, std::vector<vector_map_msgs::CrossWalk> &vmap_cross_walks,
-                      std::vector<vector_map_msgs::Area> &vmap_areas, std::vector<vector_map_msgs::Line> &vmap_lines,
-                      std::vector<vector_map_msgs::Point> &vmap_points)
-{
-  unsigned int id = 1;
-  for( auto awmap_relation : awmap.findByFilter([&](const autoware_map::LaneAttributeRelation lar){ return lar.attribute_type == autoware_map_msgs::LaneAttributeRelation::CROSS_WALK; }) )
-  {
-    //skip if area is already added
-    if(std::find_if(vmap_cross_walks.begin(), vmap_cross_walks.end(), [&](vector_map_msgs::CrossWalk cw){return cw.aid == awmap_relation.area_id; }) != vmap_cross_walks.end())
-    {
-      continue;
-    }
-
-    int parent_id = id++;
-    vector_map_msgs::CrossWalk cross_walk;
-    cross_walk.id = parent_id;
-    cross_walk.aid = awmap_relation.area_id;
-    cross_walk.type = 0;
-    cross_walk.linkid = 0;
-    vmap_cross_walks.push_back(cross_walk);
-
-    cross_walk.id = id++;
-    cross_walk.aid = awmap_relation.area_id;
-    cross_walk.type = 1;
-    cross_walk.bdid = parent_id;
-    cross_walk.linkid = 0;
-    vmap_cross_walks.push_back(cross_walk);
-
-    //create stripes for detection area
-    autoware_map_msgs::Area awmap_area = awmap.findById<autoware_map::Area>(awmap_relation.area_id);
-    std::vector<autoware_map_msgs::Point> vertices;
-    for ( int vertex : awmap_area.point_ids)
-    {
-      vertices.push_back(awmap.findById<autoware_map::Point>(vertex));
-    }
-
-    autoware_map_msgs::Point min,max;
-    getMinMax(min,max,vertices);
-
-    double height = (min.z + max.z) / 2;
-
-    double resolution = 2;
-    for (double x = min.x - resolution; x < max.x + resolution; x += resolution / 2)
-    {
-      for (double y = min.y - resolution; y < max.y + resolution; y += resolution / 2)
-      {
-        if( vertices.empty() ) continue;
-        if( isWithinArea(x,y,vertices) )
-        {
-          int area_id;
-          if(isJapaneseCoordinate(vertices.front().epsg))
-          {
-            area_id = createSquareArea(x,y,height,resolution, vmap_areas, vmap_lines, vmap_points);
-          }
-          else
-          {
-            area_id = createSquareArea(y,x,height,resolution, vmap_areas, vmap_lines, vmap_points);
-          }
-
-          cross_walk.id = id++;
-          cross_walk.aid = area_id;
-          cross_walk.type = 1;
-          cross_walk.bdid = parent_id;
-          cross_walk.linkid = 0;
-          vmap_cross_walks.push_back(cross_walk);
-        }
-      }
-    }
-  }
-}
-
-vector_map_msgs::Pole createDummyPole(const int plid,const int vid)
-{
-  vector_map_msgs::Pole vmap_pole;
-  vmap_pole.plid = plid;
-  vmap_pole.vid = vid;
-  vmap_pole.length = 1;
-  vmap_pole.dim = 0.02;
-  return vmap_pole;
-}
-
-vector_map_msgs::RoadSign createDummyRoadSign(const int id, const int vid, const int plid)
-{
-  vector_map_msgs::RoadSign road_sign;
-  road_sign.id = id;
-  road_sign.vid = vid;
-  road_sign.plid = plid;
-  road_sign.type = 1;
-  road_sign.linkid = 0;
-  return road_sign;
-}
-
-vector_map_msgs::Vector createDummyVector(int vid, int pid)
-{
-  vector_map_msgs::Vector vmap_vector;
-  vmap_vector.vid = vid;
-  vmap_vector.pid = pid;
-  vmap_vector.hang = 0;
-  vmap_vector.vang = 0;
-  return vmap_vector;
-}
-
-void createSignals( const autoware_map::AutowareMapHandler &awmap,
-                    std::vector<vector_map_msgs::Signal> &vmap_signals,
-                    std::vector<vector_map_msgs::Vector> &vmap_vectors,
-                    std::vector<vector_map_msgs::Pole> &vmap_dummy_poles)
-{
-  std::vector<autoware_map::SignalLight> awmap_signal_lights = awmap.findByFilter( [&](autoware_map::SignalLight sl){return true; });
-  std::vector<autoware_map::WaypointRelation> awmap_waypoint_relations = awmap.findByFilter( [&](autoware_map::WaypointRelation wr){return true; });
-  std::vector<autoware_map::LaneSignalLightRelation> awmap_lane_signal_relations = awmap.findByFilter( [&](autoware_map::LaneSignalLightRelation lslr){return true; });
-
-  int vector_id = getMaxId(vmap_vectors) + 1;
-  int max_lnid = 1;
-  bool failed_to_find_lnid = false;
-  std::set<int> lnid_checklist;
-  
-  for ( auto awmap_signal_light : awmap_signal_lights)
-  {
-    //create dummy poles if not created yet
-    auto pole = std::find_if( vmap_dummy_poles.begin(),
-                              vmap_dummy_poles.end(),
-                              [&](vector_map_msgs::Pole pole){ return pole.plid == awmap_signal_light.signal_id; });
-    if ( pole == vmap_dummy_poles.end()) {
-      vector_map_msgs::Vector pole_vector = createDummyVector(vector_id, awmap_signal_light.point_id);
-      // pole_vector.vang = 90;
-      vmap_vectors.push_back(pole_vector);
-      vmap_dummy_poles.push_back(createDummyPole(awmap_signal_light.signal_id, vector_id));
-      vector_id++;
-    }
-
-    vector_map_msgs::Signal vmap_signal;
-    vmap_signal.id = awmap_signal_light.signal_light_id;
-    vmap_signal.plid = awmap_signal_light.signal_id;
-    //color:{1 = red, 2=green, 3=yellow}
-    if(awmap_signal_light.color_type <=3 && awmap_signal_light.arrow_type == 0)
-    {
-      vmap_signal.type = awmap_signal_light.color_type;
-    }
-    else{
-      vmap_signal.type = 9;       //other
-    }
-
-    //create Vector to describe signal direction
-    vector_map_msgs::Vector vmap_vector;
-    vmap_vector.vid = vector_id;
-    vmap_vector.pid = awmap_signal_light.point_id;
-    vmap_vector.hang =  convertDecimalToDDMMSS( awmap_signal_light.horizontal_angle );
-    vmap_vector.vang =  convertDecimalToDDMMSS( awmap_signal_light.vertical_angle );
-    vmap_vectors.push_back(vmap_vector);
-    vmap_signal.vid = vector_id;
-    vector_id += 1;
-
-    //find link to lane
-    auto lane_signal_itr = std::find_if(   awmap_lane_signal_relations.begin(),
-                                           awmap_lane_signal_relations.end(),
-                                           [awmap_signal_light](autoware_map_msgs::LaneSignalLightRelation lslr){ return lslr.signal_light_id == awmap_signal_light.signal_light_id; });
-    autoware_map_msgs::Lane awmap_lane;
-    if (lane_signal_itr != awmap_lane_signal_relations.end())
-    {
-      awmap_lane = awmap.findById<autoware_map::Lane>(lane_signal_itr->lane_id);
-    }
-
-    int linkid = 0;
-    for(auto itr = awmap_waypoint_relations.begin(); itr != awmap_waypoint_relations.end(); itr++)
-    {
-      if(itr->next_waypoint_id == awmap_lane.end_waypoint_id)
-      {
-        auto awmap_waypoint_lane_relations = awmap.findByFilter([&](autoware_map_msgs::WaypointLaneRelation wlr){return wlr.waypoint_id == itr->waypoint_id; });
-        for(auto awmap_waypoint_lane_relation : awmap_waypoint_lane_relations)
-        {
-          if( awmap_waypoint_lane_relation.lane_id == awmap_lane.lane_id)
-          {
-            //lnid = index of waypoint_relations + 1 
-            linkid = std::distance(awmap_waypoint_relations.begin(), itr) + 1;
-          }
-        }
-      }
-    }
-    
-    //check for validity 
-    if(linkid == 0)
-    {
-      ROS_ERROR_STREAM("failed to find valid linkid for signal");
-      failed_to_find_lnid = true;
-    }
-    if(lnid_checklist.find(linkid) != lnid_checklist.end())
-    {
-      ROS_WARN_STREAM("vector map format does not support lanes linked to multiple traffic lights." << std::endl
-                    << "Conversion will continue, but note that converted map might not be able to support all nodes in Autoware(e.g. vector_map_server)");
-      failed_to_find_lnid = true;
-    }
-    
-    if(failed_to_find_lnid)
-    {
-      while(lnid_checklist.find(linkid) != lnid_checklist.end())  linkid = max_lnid + 1; 
-    }
-    
-    vmap_signal.linkid = linkid;
-    vmap_signals.push_back(vmap_signal);
-    
-    lnid_checklist.insert(linkid);
-    max_lnid = (linkid > max_lnid)?linkid:max_lnid;
-  }
-}
-
-void createStopLines( const autoware_map::AutowareMapHandler &awmap,
-                      std::vector<vector_map_msgs::Line> &vmap_lines,
-                      std::vector<vector_map_msgs::Point> &vmap_points,
-                      std::vector<vector_map_msgs::StopLine> &vmap_stop_lines,
-                      std::vector<vector_map_msgs::RoadSign> &vmap_road_signs,
-                      std::vector<vector_map_msgs::Vector> &vmap_vectors,
-                      std::vector<vector_map_msgs::Pole> &vmap_poles)
-{
-  const std::vector<autoware_map::WaypointRelation> awmap_waypoint_relations = awmap.findByFilter([&](autoware_map::WaypointRelation ){return true; });
-  int line_id = getMaxId(vmap_lines) + 1;
-  int point_id = getMaxId(vmap_points) + 1;
-  int stop_line_id = getMaxId(vmap_stop_lines) + 1;
-
-  for(auto wp : awmap.findByFilter([] (const autoware_map::Waypoint wp){return wp.stop_line == 1; }))
-  {
-
-    autoware_map::Point awmap_pt = *(wp.getPointPtr());
-    if(wp.getNextWaypoints().empty()) continue;
-    autoware_map::Waypoint next_wp = *(wp.getNextWaypoints().front());
-    autoware_map::Point awmap_next_pt = *(next_wp.getPointPtr());
-    auto next_waypoint_relation = std::find_if(awmap_waypoint_relations.begin(),
-                                               awmap_waypoint_relations.end(),
-                                               [&](autoware_map_msgs::WaypointRelation wr){return wr.waypoint_id == wp.waypoint_id; });
-
-    double yaw = atan2(awmap_next_pt.y - awmap_pt.y, awmap_next_pt.x - awmap_pt.x);
-    double angle_left, angle_right;
-    if(isJapaneseCoordinate(awmap_pt.epsg)) {
-      angle_left = addAngles(yaw, -M_PI / 2);
-      angle_right = addAngles(yaw, M_PI / 2);
-    }else
-    {
-      angle_left = addAngles(yaw, M_PI / 2);
-      angle_right = addAngles(yaw, -M_PI / 2);
-    }
-
-    vector_map_msgs::Point start_point, end_point;
-    start_point.pid = point_id++;
-
-    double r = (wp.left_width > wp.right_width) ? wp.left_width : wp.right_width;
-    //stop line cannot be right on waypoint with current rebuild_decision_maker
-    double epsilon_x = cos(yaw) * 0.001;
-    double epsilon_y = sin(yaw) * 0.001;
-
-    //stop line must intersect with waypoints left side of the line, lengthen left side
-    start_point.bx = awmap_pt.x + r * cos(angle_left) + epsilon_x;
-    start_point.ly = awmap_pt.y + r * sin(angle_left) + epsilon_y;
-    start_point.h = awmap_pt.z;
-
-    end_point.pid = point_id++;
-    end_point.bx = awmap_pt.x + r * cos(angle_right) + epsilon_x;
-    end_point.ly = awmap_pt.y + r * sin(angle_right) + epsilon_y;
-    end_point.h = awmap_pt.z;
-
-    // make sure that stop line does not intersect with other lanes.
-    for(auto awmap_wp_relation : awmap_waypoint_relations )
-    {
-      if(awmap_wp_relation.waypoint_id == wp.waypoint_id || awmap_wp_relation.next_waypoint_id == wp.waypoint_id) {
-        continue;
-      }
-      autoware_map::Waypoint wp1 = *(awmap_wp_relation.getWaypointPtr());
-      autoware_map::Waypoint wp2 = *(awmap_wp_relation.getNextWaypointPtr());
-      autoware_map::Point p1 = *(wp1.getPointPtr());
-      autoware_map::Point p2 = *(wp2.getPointPtr());
-
-      double intersect_x, intersect_y;
-      if ( getIntersect(p1.x, p1.y, p2.x, p2.y,
-                        start_point.bx, start_point.ly, end_point.bx, end_point.ly,
-                        intersect_x, intersect_y))
-      {
-        double distance = std::hypot( awmap_pt.x - intersect_x, awmap_pt.y - intersect_y );
-        r = distance * 0.98;           //shorten length of stop line so that it does not cross any other lanes
-
-        start_point.bx = awmap_pt.x + r * cos(angle_left) + epsilon_x;
-        start_point.ly = awmap_pt.y + r * sin(angle_left) + epsilon_y;
-        end_point.bx = awmap_pt.x + r * cos(angle_right) + epsilon_x;
-        end_point.ly = awmap_pt.y + r * sin(angle_right) + epsilon_y;
-      }
-    }
-
-    //swap x and y if the coordinate is not in japanese rectangular coordinate system
-    if(!isJapaneseCoordinate(awmap_pt.epsg))
-    {
-      double tmp = start_point.bx;
-      start_point.bx = start_point.ly;
-      start_point.ly = tmp;
-      tmp = end_point.bx;
-      end_point.bx = end_point.ly;
-      end_point.ly = tmp;
-    }
-
-    vmap_points.push_back(start_point);
-    vmap_points.push_back(end_point);
-
-    vector_map_msgs::Line vmap_line;
-    vmap_line.lid = line_id++;
-    vmap_line.bpid = start_point.pid;
-    vmap_line.fpid = end_point.pid;
-    vmap_line.blid = vmap_line.flid = 0;
-    vmap_lines.push_back(vmap_line);
-
-    // only create road sign if stopline is not related to signal
-    std::vector<autoware_map::WaypointSignalRelation> wsr_vector = awmap.findByFilter([&](const autoware_map::WaypointSignalRelation wsr){ return wsr.waypoint_id == wp.waypoint_id; });
-    vector_map_msgs::StopLine vmap_stop_line;
-    vmap_stop_line.id = stop_line_id++;
-    vmap_stop_line.lid = vmap_line.lid;
-    vmap_stop_line.tlid = 0;
-    if( wsr_vector.empty()) {
-      int vector_id = getMaxId(vmap_vectors) + 1;
-      vmap_vectors.push_back(createDummyVector(vector_id, vmap_line.bpid));
-
-      int pole_id = getMaxId(vmap_poles)+1;
-      vmap_poles.push_back(createDummyPole(pole_id, vector_id));
-
-      int road_sign_id = getMaxId(vmap_road_signs) + 1;
-      vmap_road_signs.push_back(createDummyRoadSign(road_sign_id, vector_id, pole_id));
-      vmap_stop_line.signid = road_sign_id;
-    }
-
-    vmap_stop_line.linkid = std::distance(awmap_waypoint_relations.begin(), next_waypoint_relation) + 1;
-    vmap_stop_lines.push_back(vmap_stop_line);
-  }
-}
-
-
-void createWayAreas(const autoware_map::AutowareMapHandler &awmap, std::vector<vector_map_msgs::WayArea> &vmap_way_areas)
-{
-  for ( auto awmap_area : awmap.findByFilter( [&](autoware_map::Wayarea wa){return true; }))
-  {
-    vector_map_msgs::WayArea way_area;
-    way_area.waid = awmap_area.wayarea_id;
-    way_area.aid = awmap_area.area_id;
-    vmap_way_areas.push_back(way_area);
-  }
-}
-
-
-void createWayAreasFromLanes(const autoware_map::AutowareMapHandler &awmap,
-                             std::vector<vector_map_msgs::WayArea> &vmap_way_areas,
-                             std::vector<vector_map_msgs::Area> &vmap_areas,
-                             std::vector<vector_map_msgs::Line> &vmap_lines,
-                             std::vector<vector_map_msgs::Point> &vmap_points )
-{
-  const std::vector<autoware_map::WaypointRelation> awmap_waypoint_relations = awmap.findByFilter([&](autoware_map::WaypointRelation wr){return true; });
-  int line_id = getMaxId(vmap_lines) + 1;
-  int point_id = getMaxId(vmap_points) + 1;
-  int area_id = getMaxId(vmap_areas) + 1;
-  int wayarea_id = getMaxId(vmap_way_areas) + 1;
-
-  std::unordered_map<int, WaypointWithYaw> wp_yaw_map;
-  for(auto relation : awmap_waypoint_relations)
-  {
-    autoware_map::Waypoint awmap_wp = *(relation.getWaypointPtr());
-    autoware_map::Waypoint awmap_next_wp = *(relation.getNextWaypointPtr());
-    autoware_map::Point awmap_pt = *(awmap_wp.getPointPtr());
-    autoware_map::Point awmap_next_pt = *(awmap_next_wp.getPointPtr());
-
-    // autoware_map_msgs::Waypoint awmap_wp = awmap.findById<autoware_map::Waypoint>(relation.waypoint_id);
-    // autoware_map_msgs::Waypoint awmap_next_wp = awmap.findById<autoware_map::Waypoint>(relation.next_waypoint_id);
-    // autoware_map_msgs::Point awmap_pt = awmap.findById<autoware_map::Point>(awmap_wp.point_id);
-    // autoware_map_msgs::Point awmap_next_pt = awmap.findById<autoware_map::Point>(awmap_next_wp.point_id);
-
-    double yaw = atan2(awmap_next_pt.y - awmap_pt.y, awmap_next_pt.x - awmap_pt.x);
-
-    WaypointWithYaw wp_yaw;
-    if(wp_yaw_map.find(relation.waypoint_id) != wp_yaw_map.end()) {
-      wp_yaw = wp_yaw_map.at(relation.waypoint_id);
-    }
-    wp_yaw.waypoint = awmap_wp;
-    wp_yaw.point = awmap_pt;
-    wp_yaw.yaws.push_back(yaw);
-    wp_yaw_map[relation.waypoint_id] = wp_yaw;
-
-    WaypointWithYaw next_wp_yaw;
-    if(wp_yaw_map.find(relation.next_waypoint_id) != wp_yaw_map.end()) {
-      next_wp_yaw = wp_yaw_map.at(relation.next_waypoint_id);
-    }
-    next_wp_yaw.waypoint = awmap_next_wp;
-    next_wp_yaw.point = awmap_next_pt;
-    next_wp_yaw.yaws.push_back(yaw);
-    wp_yaw_map[relation.next_waypoint_id] = next_wp_yaw;
-  }
-
-  for( auto &item : wp_yaw_map)
-  {
-    WaypointWithYaw wp_yaw = item.second;
-    wp_yaw.yaw_avg = getAngleAverage(wp_yaw.yaws);
-    double yaw = wp_yaw.yaw_avg;
-    double left_width = wp_yaw.waypoint.left_width;
-    double right_width = wp_yaw.waypoint.right_width;
-
-    double angle_left, angle_right;
-    if(isJapaneseCoordinate(wp_yaw.point.epsg)) {
-      angle_left = addAngles(yaw, -M_PI / 2);
-      angle_right = addAngles(yaw, M_PI / 2);
-    }else
-    {
-      angle_left = addAngles(yaw, M_PI / 2);
-      angle_right = addAngles(yaw, -M_PI / 2);
-    }
-
-    autoware_map_msgs::Point left_point,right_point;
-    left_point.x = wp_yaw.point.x + left_width * cos(angle_left);
-    left_point.y = wp_yaw.point.y + left_width * sin(angle_left);
-    left_point.z = wp_yaw.point.z;
-    left_point.point_id = point_id++;
-    wp_yaw.left_point = left_point;
-
-    right_point.x = wp_yaw.point.x + right_width * cos(angle_right);
-    right_point.y = wp_yaw.point.y + right_width * sin(angle_right);
-    right_point.z = wp_yaw.point.z;
-    right_point.point_id = point_id++;
-    wp_yaw.right_point = right_point;
-    item.second = wp_yaw;
-  }
-
-  for(auto relation : awmap_waypoint_relations)
-  {
-    WaypointWithYaw wp_yaw = wp_yaw_map.at(relation.waypoint_id);
-    WaypointWithYaw next_wp_yaw = wp_yaw_map.at(relation.next_waypoint_id);
-
-    vector_map_msgs::Point pt1, pt2, pt3, pt4;
-
-    convertPoint(pt1, wp_yaw.left_point);
-    pt1.pid = wp_yaw.left_point.point_id;
-    convertPoint(pt2, next_wp_yaw.left_point);
-    pt2.pid = next_wp_yaw.left_point.point_id;
-    convertPoint(pt3, next_wp_yaw.right_point);
-    pt3.pid = next_wp_yaw.right_point.point_id;
-    convertPoint(pt4, wp_yaw.right_point);
-    pt4.pid = wp_yaw.right_point.point_id;
-
-    vmap_points.push_back(pt1);
-    vmap_points.push_back(pt2);
-    vmap_points.push_back(pt3);
-    vmap_points.push_back(pt4);
-
-    vector_map_msgs::Line vmap_line;
-    vmap_line.lid = line_id;
-    vmap_line.bpid = pt1.pid;
-    vmap_line.fpid = pt2.pid;
-    vmap_line.blid = 0;
-    vmap_line.flid = line_id + 1;
-    vmap_lines.push_back(vmap_line);
-    line_id++;
-
-    vmap_line.lid = line_id;
-    vmap_line.bpid = pt2.pid;
-    vmap_line.fpid = pt3.pid;
-    vmap_line.blid = line_id - 1;
-    vmap_line.flid = line_id + 1;
-    vmap_lines.push_back(vmap_line);
-    line_id++;
-
-    vmap_line.lid = line_id;
-    vmap_line.bpid = pt3.pid;
-    vmap_line.fpid = pt4.pid;
-    vmap_line.blid = line_id - 1;
-    vmap_line.flid = line_id + 1;
-    vmap_lines.push_back(vmap_line);
-    line_id++;
-
-    vmap_line.lid = line_id;
-    vmap_line.bpid = pt4.pid;
-    vmap_line.fpid = pt1.pid;
-    vmap_line.blid = line_id - 1;
-    vmap_line.flid = 0;
-    vmap_lines.push_back(vmap_line);
-    line_id++;
-
-    vector_map_msgs::Area vmap_area;
-    vmap_area.aid = area_id++;
-    vmap_area.slid = line_id - 4;
-    vmap_area.elid = line_id - 1;
-    vmap_areas.push_back(vmap_area);
-
-    vector_map_msgs::WayArea vmap_way_area;
-    vmap_way_area.aid = vmap_area.aid;
-    vmap_way_area.waid = wayarea_id++;
-    vmap_way_areas.push_back(vmap_way_area);
-  }
-}
-
-//move point to specified direction, where direction is angle from north in clockwise
-autoware_map_msgs::Point movePointByDirection(autoware_map::Point point,double direction_rad, double length)
-{
-  autoware_map_msgs::Point moved_point = point;
-  moved_point.x = point.x + length * cos(direction_rad);
-  moved_point.y = point.y + length * sin(direction_rad);
-  moved_point.z = point.z;
-  return moved_point;
-}
-
-double getAngleToNextWaypoint(std::shared_ptr<autoware_map::Waypoint> waypoint, int lane_id)
-{
-  double yaw = 0;
-  autoware_map::Waypoint next =*( waypoint->getNextWaypoint(lane_id) );
-  yaw = atan2(next.getPointPtr()->y - waypoint->getPointPtr()->y, next.getPointPtr()->x - waypoint->getPointPtr()->x);
-  return yaw;
-}
-
-void createWhitelines(const autoware_map::AutowareMapHandler &awmap,
-                      std::vector<vector_map_msgs::Point> &vmap_points,
-                      std::vector<vector_map_msgs::Line> &vmap_lines,
-                      std::vector<vector_map_msgs::WhiteLine> &vmap_white_lines)
-{
-  int white_line_id = getMaxId(vmap_white_lines)+ 1;
-  int line_id = getMaxId(vmap_lines)+ 1;
-  int point_id = getMaxId(vmap_points) + 1;
-  const double epsilon = 1e-6;
-  for ( auto awmap_lane : awmap.findByFilter([] (autoware_map::Lane l){return true; }))
-  {
-    vector_map_msgs::Point current_vmap_point_left,current_vmap_point_right;
-    vector_map_msgs::Point prev_vmap_point_left,prev_vmap_point_right;
-
-    auto waypoint = awmap_lane.getStartWaypoint();
-    double prev_yaw = getAngleToNextWaypoint(awmap_lane.getStartWaypoint(),awmap_lane.lane_id);
-    double next_yaw = getAngleToNextWaypoint(awmap_lane.getStartWaypoint(),awmap_lane.lane_id);
-
-    for(auto waypoint : awmap_lane.getWaypoints())
-    {
-      autoware_map_msgs::WaypointRelation tmp;
-      if(waypoint->waypoint_id == awmap_lane.end_waypoint_id ) {
-        next_yaw = prev_yaw;
-      }else{
-        autoware_map::Waypoint next =*( waypoint->getNextWaypoint(awmap_lane.lane_id) );
-        if ( distance2d(*waypoint->getPointPtr(), *next.getPointPtr()) < epsilon) continue;
-
-        next_yaw = getAngleToNextWaypoint(waypoint,awmap_lane.lane_id);
-      }
-
-      double yaw = prev_yaw;
-      double angle_left, angle_right;
-      if(isJapaneseCoordinate(waypoint->getPointPtr()->epsg)) {
-        angle_left = addAngles(yaw, -M_PI / 2);
-        angle_right = addAngles(yaw, M_PI / 2);
-      }else
-      {
-        angle_left = addAngles(yaw, M_PI / 2);
-        angle_right = addAngles(yaw, -M_PI / 2);
-      }
-
-      autoware_map_msgs::Point awmap_right_pt = movePointByDirection(*(waypoint->getPointPtr()), angle_right,waypoint->right_width);
-      autoware_map_msgs::Point awmap_left_pt = movePointByDirection(*(waypoint->getPointPtr()), angle_left,waypoint->left_width);
-      // autoware_map_msgs::Point awmap_right_pt = movePointByDirection(*(waypoint->getPointPtr()), angle_right,1);
-      // autoware_map_msgs::Point awmap_left_pt = movePointByDirection(*(waypoint->getPointPtr()), angle_left,1);
-      awmap_right_pt.point_id = point_id++;
-      awmap_left_pt.point_id = point_id++;
-      convertPoint(current_vmap_point_right,awmap_right_pt);
-      convertPoint(current_vmap_point_left,awmap_left_pt);
-      current_vmap_point_right.pid = awmap_right_pt.point_id;
-      current_vmap_point_left.pid = awmap_left_pt.point_id;
-      vmap_points.push_back(current_vmap_point_right);
-      vmap_points.push_back(current_vmap_point_left);
-
-      //continue if this is initial point of lane
-      if(prev_vmap_point_left.pid == 0 || prev_vmap_point_right.pid == 0) {
-        prev_vmap_point_left = current_vmap_point_left;
-        prev_vmap_point_right = current_vmap_point_right;
-        prev_yaw = next_yaw;
-        continue;
-      }
-
-      //right line
-      vector_map_msgs::Line vmap_line_right;
-      vmap_line_right.lid = line_id;
-      vmap_line_right.bpid = prev_vmap_point_right.pid;
-      vmap_line_right.fpid = current_vmap_point_right.pid;
-      vmap_line_right.blid = vmap_line_right.flid = 0;
-      vmap_lines.push_back(vmap_line_right);
-
-      vector_map_msgs::WhiteLine vmap_white_line_right;
-      vmap_white_line_right.id = white_line_id++;
-      vmap_white_line_right.lid = line_id++;
-      vmap_white_line_right.width = 0.1;
-      if(awmap_lane.lane_number == 0)       //intersection
-      {
-        vmap_white_line_right.color = 'W';
-        vmap_white_line_right.type =vector_map_msgs::WhiteLine::DASHED_LINE_BLANK;
-      }else if(awmap_lane.lane_number == awmap_lane.num_of_lanes)
-      {
-        vmap_white_line_right.color = 'W';
-        vmap_white_line_right.type = vector_map_msgs::WhiteLine::SOLID_LINE;
-      }
-      else{
-        vmap_white_line_right.color = 'W';
-        vmap_white_line_right.type =vector_map_msgs::WhiteLine::DASHED_LINE_SOLID;
-      }
-      vmap_white_line_right.linkid = 0;
-      vmap_white_lines.push_back(vmap_white_line_right);
-
-      //left line
-      vector_map_msgs::Line vmap_line_left;
-      vmap_line_left.lid = line_id;
-      vmap_line_left.bpid = prev_vmap_point_left.pid;
-      vmap_line_left.fpid = current_vmap_point_left.pid;
-      vmap_line_left.blid = vmap_line_left.flid = 0;
-      vmap_lines.push_back(vmap_line_left);
-
-      vector_map_msgs::WhiteLine vmap_white_line_left;
-      vmap_white_line_left.id = white_line_id++;
-      vmap_white_line_left.lid = line_id++;
-      vmap_white_line_left.width = 0.1;
-      if(awmap_lane.lane_number == 0)
-      {
-        vmap_white_line_left.color = 'W';
-        vmap_white_line_left.type =vector_map_msgs::WhiteLine::DASHED_LINE_BLANK;
-      }
-      else if(awmap_lane.lane_number == 1)
-      {
-        vmap_white_line_left.color = 'W';
-        vmap_white_line_left.type = vector_map_msgs::WhiteLine::SOLID_LINE;
-      }
-      else{
-        vmap_white_line_left.color = 'W';
-        vmap_white_line_left.type =vector_map_msgs::WhiteLine::DASHED_LINE_SOLID;
-      }
-      vmap_white_line_left.linkid = 0;
-      vmap_white_lines.push_back(vmap_white_line_left);
-
-      //update previous points
-      prev_vmap_point_left = current_vmap_point_left;
-      prev_vmap_point_right = current_vmap_point_right;
-      prev_yaw = next_yaw;
-
-    }
-  }
-}
-
-void createDummyUtilityPoles(const std::vector<vector_map_msgs::Pole> vmap_poles,
-                             std::vector<vector_map_msgs::UtilityPole> &vmap_utility_poles)
-{
-  int id = 1;
-  for(auto pole : vmap_poles)
-  {
-    vector_map_msgs::UtilityPole utility_pole;
-    utility_pole.id = id++;
-    utility_pole.plid = pole.plid;
-    utility_pole.linkid = 0;
-    vmap_utility_poles.push_back(utility_pole);
-  }
-}
-
-double distance2d(const autoware_map_msgs::Point p1, const autoware_map_msgs::Point p2)
-{
-  geometry_msgs::Point geo_p1, geo_p2;
-  
-  geo_p1.x = p1.x;
-  geo_p1.y = p1.y;
-  geo_p1.z = 0;
-  geo_p2.x = p2.x;
-  geo_p2.y = p2.y;
-  geo_p2.z = 0;
-  
-  return amathutils::find_distance(geo_p1, geo_p2);  
-}
-
-//keep angles within (M_PI, -M_PI]
-double addAngles(double angle1, double angle2)
-{
-  double sum = angle1 + angle2;
-  while( sum > M_PI ) sum -= 2 * M_PI;
-  while( sum <= -M_PI ) sum += 2 * M_PI;
-  return sum;
-}
-
-double angleAverage(double angle1, double angle2)
-{
-  double x1, x2, y1, y2;
-  x1 = cos(angle1);
-  y1 = sin(angle1);
-  x2 = cos(angle2);
-  y2 = sin(angle2);
-  return atan2((y2 + y1)/2, (x2 + x1)/2);
-}
-
-double getAngleAverage(const std::vector<double> angles)
-{
-  autoware_map_msgs::Point avg, sum;
-  for ( double angle : angles)
-  {
-    sum.x += cos(angle);
-    sum.y += sin(angle);
-  }
-  avg.x = sum.x / angles.size();
-  avg.y = sum.y / angles.size();
-  return atan2(avg.y, avg.x);
-}
-
-double convertDecimalToDDMMSS(const double decimal)
-{
-  int sign, degree, minutes, seconds;
-  if( decimal < 0 )
-    sign = -1;
-  else
-    sign = 1; 
-    
-  double unsigned_decimal = sign * decimal;
-  
-  degree = floor(unsigned_decimal);
-  minutes = floor( (unsigned_decimal - degree ) * 60);
-  seconds = floor( (unsigned_decimal - degree - minutes * 1.0 / 60) * 3600);
-  return sign * (degree + minutes * 0.01 + seconds * 0.0001);
-}
-
-
-void getMinMax(autoware_map_msgs::Point &min, autoware_map_msgs::Point &max, const std::vector<autoware_map_msgs::Point>points)
-{
-  if(points.empty()) {
-    ROS_ERROR_STREAM("No points to calculate min/max point!");
-  }
-  min = max = points.front();
-  for (auto pt : points)
-  {
-    min.x = min.x < pt.x ? min.x : pt.x;
-    min.y = min.y < pt.y ? min.y : pt.y;
-    min.z = min.z < pt.z ? min.z : pt.z;
-    max.x = max.x > pt.x ? max.x : pt.x;
-    max.y = max.y > pt.y ? max.y : pt.y;
-    max.z = max.z > pt.z ? max.z : pt.z;
-  }
-}
-
-//determine whether point lies within an area by counting winding number
-bool isWithinArea(double x, double y, const std::vector<autoware_map_msgs::Point> vertices)
-{
-  std::vector<double> angles;
-  for (auto pt : vertices)
-  {
-    if(pt.x == x && pt.y== y) return false;
-    angles.push_back( atan2(pt.y - y, pt.x - x));
-  }
-
-  double angle_sum = 0;
-  for (unsigned int idx = 0; idx < vertices.size(); idx++)
-  {
-    double angle1, angle2;
-    angle1 = angles.at(idx);
-    if(idx + 1 >= vertices.size())
-    {
-      angle2 = angles.front();
-    }else
-    {
-      angle2 = angles.at(idx + 1);
-    }
-    double angle_diff = addAngles(angle1, -angle2);
-    angle_sum += angle_diff;
-  }
-  //allow some precision error
-  if(fabs(angle_sum) < 1e-3) return false;
-  else return true;
-}
-
-bool getIntersect(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4, double &intersect_x, double &intersect_y )
-{
-  //let p1(x1, y1), p2(x2, y2), p3(x3, y3), p4(x4,y4)
-  //intersect of line segment p1 to p2 and p3 to p4 satisfies
-  // p1 + r(p2 - p1) = p3 + s(p4 - p3)
-  // 0 <= r <= 1
-  // 0 <= s <= 1
-  double denominator = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
-
-  if(denominator == 0) {
-    //line is parallel
-    return false;
-  }
-
-  double r = ( (y4 - y3) * (x3 - x1) - (x4 - x3) * (y3 - y1) ) / denominator;
-  double s = ( (y2 - y1) * (x3 - x1) - (x2 - x1) * (y3 - y1) ) / denominator;
-
-  if( r >= 0 && r <= 1 && s >= 0 && s <= 1) {
-    intersect_x = x1 + r * (x2 - x1);
-    intersect_y = y1 + r * (y2 - y1);
-    return true;
-  }else{
-    return false;
-  }
-}
-
-int convertESPGToRef(int epsg)
-{
-  if(epsg >= 2443 && epsg <= 2461)
-  {
-    return epsg - 2442;
-  }
-  if(epsg >= 6669 && epsg <= 6687)
-  {
-    return epsg - 6668;
-  }
-  return 0;
-}
-
-
-int getMaxId(std::vector<vector_map_msgs::Point> points)
-{
-  int max = 0;
-  for (auto p : points)
-  {
-    if (p.pid > max) {
-      max = p.pid;
-    }
-  }
-  return max;
-}
-
-int getMaxId(std::vector<vector_map_msgs::Line> lines)
-{
-  int max = 0;
-  for (auto l : lines)
-  {
-    if (l.lid > max) {
-      max = l.lid;
-    }
-  }
-  return max;
-}
-int getMaxId(std::vector<vector_map_msgs::StopLine> stop_lines)
-{
-  int max = 0;
-  for (auto l : stop_lines)
-  {
-    if (l.id > max) {
-      max = l.id;
-    }
-  }
-  return max;
-}
-int getMaxId(std::vector<vector_map_msgs::RoadSign> signs)
-{
-  int max = 0;
-  for (auto s : signs)
-  {
-    if (s.id > max) {
-      max = s.id;
-    }
-  }
-  return max;
-}
-
-int getMaxId(std::vector<vector_map_msgs::Area> areas)
-{
-  int max = 0;
-  for (auto a : areas)
-  {
-    if (a.aid > max) {
-      max = a.aid;
-    }
-  }
-  return max;
-}
-
-int getMaxId(const std::vector<vector_map_msgs::Vector> vectors)
-{
-  int max = 0;
-  for (auto v : vectors)
-  {
-    if (v.vid > max) {
-      max = v.vid;
-    }
-  }
-  return max;
-}
-
-int getMaxId(const std::vector<vector_map_msgs::Pole> poles)
-{
-  int max = 0;
-  for (auto p : poles)
-  {
-    if (p.plid > max) {
-      max = p.plid;
-    }
-  }
-  return max;
-}
-int getMaxId(const std::vector<vector_map_msgs::WhiteLine> white_lines)
-{
-  int max = 0;
-  for (auto w : white_lines)
-  {
-    if (w.id > max) {
-      max = w.id;
-    }
-  }
-  return max;
-}
-int getMaxId(const std::vector<vector_map_msgs::WayArea> wayareas)
-{
-  int max = 0;
-  for (auto w : wayareas)
-  {
-    if (w.waid > max) {
-      max = w.waid;
-    }
-  }
-  return max;
-}

src/ros/catkin/src/vector_map_converter/src/lanelet2autowaremap.cpp

@@ -1,109 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-#include <vector_map_converter/lanelet2autowaremap.hpp>
-#include <vector_map_converter/autoware2vectormap.hpp>
-
-void printUsage()
-{
-  std::cout << "Required parameters" << std::endl;
-  std::cout << "_map_file:=<path to laneletmap>" << std::endl;
-  std::cout << "_origin_lat:=latitude origin used for xyz projection>" << std::endl;
-  std::cout << "_origin_lon:=<longitude origin used for xyz projection>" << std::endl;
-}
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "lanelet2autowaremap_converter");
-    ros::NodeHandle nh;
-    ros::NodeHandle pnh("~");
-
-
-    if(!pnh.hasParam("map_file")){
-      ROS_ERROR_STREAM("You must specify file path!");
-      printUsage();
-      exit(1);
-    }
-    if(!pnh.hasParam("origin_lat") || !pnh.hasParam("origin_lon") ){
-      ROS_ERROR_STREAM("You must specify an origin!");
-      printUsage();
-      exit(1);
-    }
-
-    std::string map_file_path;
-    std::string save_dir;
-    double origin_lat;
-    double origin_lon;
-    pnh.param<std::string>("map_file", map_file_path, "");
-    pnh.param<std::string>("save_dir", save_dir, "./");
-    pnh.param<double>("origin_lat", origin_lat, 0.0);
-    pnh.param<double>("origin_lon", origin_lon,0.0);
-
-    std::vector<autoware_map_msgs::Point> points;
-    std::vector<autoware_map_msgs::Lane> lanes;
-    std::vector<autoware_map_msgs::LaneAttributeRelation> lane_attribute_relations;
-    std::vector<autoware_map_msgs::LaneRelation> lane_relations;
-    std::vector<autoware_map_msgs::LaneSignalLightRelation> lane_signal_light_relations;
-    std::vector<autoware_map_msgs::LaneChangeRelation> lane_change_relations;
-    std::vector<autoware_map_msgs::OppositeLaneRelation> opposite_lane_relations;
-    std::vector<autoware_map_msgs::Area> areas;
-    std::vector<autoware_map_msgs::Signal> signals;
-    std::vector<autoware_map_msgs::SignalLight> signal_lights;
-    std::vector<autoware_map_msgs::Wayarea> wayareas;
-    std::vector<autoware_map_msgs::Waypoint> waypoints;
-    std::vector<autoware_map_msgs::WaypointLaneRelation> waypoint_lane_relations;
-    std::vector<autoware_map_msgs::WaypointRelation> waypoint_relations;
-    std::vector<autoware_map_msgs::WaypointSignalRelation> waypoint_signal_relations;
-
-    using namespace lanelet;
-    ErrorMessages errors;
-    projection::UtmProjector projector(Origin({origin_lat,origin_lon}));
-    LaneletMapPtr map = load(map_file_path, projector, &errors);
-    autoware_map::AutowareMapHandler map_handler;
-
-    convertLanelet2AutowareMap(map,
-                               projector,
-                               areas,
-                               lanes,
-                               lane_attribute_relations,
-                               lane_change_relations,
-                               lane_relations,
-                               lane_signal_light_relations,
-                               opposite_lane_relations,
-                               points,
-                               signals,
-                               signal_lights,
-                               wayareas,
-                               waypoints,
-                               waypoint_lane_relations,
-                               waypoint_relations,
-                               waypoint_signal_relations);
-    writeAutowareMapMsgs(save_dir,
-                         areas,
-                         lanes,
-                         lane_attribute_relations,
-                         lane_change_relations,
-                         lane_relations,
-                         lane_signal_light_relations,
-                         opposite_lane_relations,
-                         points,
-                         signals,
-                         signal_lights,
-                         wayareas,
-                         waypoints,
-                         waypoint_lane_relations,
-                         waypoint_relations,
-                         waypoint_signal_relations);
-}

src/ros/catkin/src/vector_map_converter/src/lanelet2autowaremap_core.cpp

@@ -1,1073 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-#include <vector_map_converter/lanelet2autowaremap.hpp>
-#include <amathutils_lib/amathutils.hpp>
-#include <geodesy/utm.h>
-
-using namespace lanelet;
-
-#define BLINKER_RIGHT 2
-#define BLINKER_LEFT 1
-#define SIGNAL_UNKNOWN 0
-#define SIGNAL_RED 1
-#define SIGNAL_GREEN 2
-#define SIGNAL_YELLOW 3
-
-
-BOOST_GEOMETRY_REGISTER_MULTI_POINT(decltype(std::vector<BasicPoint2d>{} ))
-
-template <class T>
-void write(std::ofstream &ofs, std::vector<T> objs)
-{
-  for( auto obj : objs)
-  {
-    ofs << obj << std::endl;
-  }
-}
-
-/**
- * [calculates MGRS x,y from lat/lng information]
- * @method fixPointCoordinate
- * @param  point              [point with lat/lng and information]
- */
-
-void fixPointCoordinate(autoware_map_msgs::Point &point)
-{
-  geographic_msgs::GeoPoint wgs_point;
-  wgs_point.latitude = point.lat;
-  wgs_point.longitude = point.lng;
-  wgs_point.altitude = point.z;
-
-  geodesy::UTMPoint utm_point;
-  geodesy::fromMsg(wgs_point,utm_point);
-  point.x = fmod(utm_point.easting,1e5);
-  point.y = fmod(utm_point.northing,1e5);
-}
-
-void writeAutowareMapMsgs(std::string output_dir,
-                          std::vector<autoware_map_msgs::Area> &areas,
-                          std::vector<autoware_map_msgs::Lane> &lanes,
-                          std::vector<autoware_map_msgs::LaneAttributeRelation> &lane_attribute_relations,
-                          std::vector<autoware_map_msgs::LaneChangeRelation> &lane_change_relations,
-                          std::vector<autoware_map_msgs::LaneRelation> &lane_relations,
-                          std::vector<autoware_map_msgs::LaneSignalLightRelation> &lane_signal_light_relations,
-                          std::vector<autoware_map_msgs::OppositeLaneRelation> &opposite_lane_relations,
-                          std::vector<autoware_map_msgs::Point> &points,
-                          std::vector<autoware_map_msgs::Signal> &signals,
-                          std::vector<autoware_map_msgs::SignalLight> &signal_lights,
-                          std::vector<autoware_map_msgs::Wayarea> &wayareas,
-                          std::vector<autoware_map_msgs::Waypoint> &waypoints,
-                          std::vector<autoware_map_msgs::WaypointLaneRelation> &waypoint_lane_relations,
-                          std::vector<autoware_map_msgs::WaypointRelation> &waypoint_relations,
-                          std::vector<autoware_map_msgs::WaypointSignalRelation> &waypoint_signal_relations)
-{
-  std::ofstream ofs;
-  std::string filename;
-
-  //areas
-  if(!areas.empty())
-  {
-    filename = output_dir + "/areas.csv";
-    ofs.open(filename);
-    ofs << "area_id,point_ids" << std::endl;
-    write(ofs, areas);
-    ofs.close();
-  }
-  //lanes
-  if(!lanes.empty())
-  {
-    filename = output_dir + "/lanes.csv";
-    ofs.open(filename);
-    ofs << "lane_id,start_waypoint_id,end_waypoint_id,lane_number,num_of_lanes,speed_limit,length,width_limit,height_limit,weight_limit" << std::endl;
-    write(ofs, lanes);
-    ofs.close();
-  }
-  //lane_attribute_relations
-  if(!lane_attribute_relations.empty())
-  {
-    filename = output_dir + "/lane_attribute_relations.csv";
-    ofs.open(filename);
-    ofs << "lane_id,attribute_type,area_id" << std::endl;
-    write(ofs, lane_attribute_relations);
-    ofs.close();
-  }
-  //lane_change_relations
-  if(!lane_change_relations.empty())
-  {
-    filename = output_dir + "/lane_change_relations.csv";
-    ofs.open(filename);
-    ofs << "lane_id,next_lane_id,blinker" << std::endl;
-    write(ofs, lane_change_relations);
-    ofs.close();
-  }
-  //lane_relations
-  if(!lane_relations.empty())
-  {
-    filename = output_dir + "/lane_relations.csv";
-    ofs.open(filename);
-    ofs << "lane_id,next_lane_id,blinker" << std::endl;
-    write(ofs, lane_relations);
-    ofs.close();
-  }
-  //lane_signal_light_relations
-  if(!lane_signal_light_relations.empty())
-  {
-    filename = output_dir + "/lane_signal_light_relations.csv";
-    ofs.open(filename);
-    ofs << "lane_id,signal_light_id" << std::endl;
-    write(ofs, lane_signal_light_relations);
-    ofs.close();
-  }
-  //opposite_lane_relations
-  if(!opposite_lane_relations.empty())
-  {
-    filename = output_dir + "/opposite_lane_relations.csv";
-    ofs.open(filename);
-    ofs << "lane_id,opposite_lane_id" << std::endl;
-    write(ofs, opposite_lane_relations);
-    ofs.close();
-  }
-  //points
-  if(!points.empty())
-  {
-    filename = output_dir + "/points.csv";
-    ofs.open(filename);
-    ofs << "point_id,y,x,z,lat,lng,pcd,mgrs,epsg" << std::endl;
-    write(ofs, points);
-    ofs.close();
-  }
-  //signals
-  if(!signals.empty())
-  {
-    filename = output_dir + "/signals.csv";
-    ofs.open(filename);
-    ofs << "signal_id" << std::endl;
-    write(ofs, signals);
-    ofs.close();
-  }
-  //signal_lights
-  if(!signal_lights.empty())
-  {
-    filename = output_dir + "/signal_lights.csv";
-    ofs.open(filename);
-    ofs << "signal_light_id,signal_id,point_id,horizontal_angle,vertical_angle,color_type,arrow_type" << std::endl;
-    write(ofs, signal_lights);
-    ofs.close();
-  }
-  //wayareas
-  if(!wayareas.empty())
-  {
-    filename = output_dir + "/wayareas.csv";
-    ofs.open(filename);
-    ofs << "wayarea_id,area_id" << std::endl;
-    write(ofs, wayareas);
-    ofs.close();
-  }
-  //waypoints
-  if(!waypoints.empty())
-  {
-    filename = output_dir + "/waypoints.csv";
-    ofs.open(filename);
-    ofs << "waypoint_id,point_id,velocity,stop_line,left_width,right_width,height" << std::endl;
-    write(ofs, waypoints);
-    ofs.close();
-  }
-  //waypoint_lane_relations
-  if(!waypoint_lane_relations.empty())
-  {
-    filename = output_dir + "/waypoint_lane_relations.csv";
-    ofs.open(filename);
-    ofs << "waypoint_id,lane_id,order" << std::endl;
-    write(ofs, waypoint_lane_relations);
-    ofs.close();
-  }
-  //waypoint_relations
-  if(!waypoint_relations.empty())
-  {
-    filename = output_dir + "/waypoint_relations.csv";
-    ofs.open(filename);
-    ofs << "waypoint_id,next_waypoint_id,yaw,blinker,distance" << std::endl;
-    write(ofs, waypoint_relations);
-    ofs.close();
-  }
-  if(!waypoint_signal_relations.empty())
-  {
-    filename = output_dir + "/waypoint_signal_relations.csv";
-    ofs.open(filename);
-    ofs << "waypoint_id,signal_id" << std::endl;
-    write(ofs, waypoint_signal_relations);
-    ofs.close();
-  }
-}
-autoware_map_msgs::Point convertPoint(const lanelet::ConstPoint3d &lanelet_point, const projection::UtmProjector &projector)
-{
-  autoware_map_msgs::Point awmap_point;
-  GPSPoint utm_point = projector.reverse(lanelet_point.basicPoint());
-  awmap_point.point_id = lanelet_point.id();
-  awmap_point.x = lanelet_point.x();
-  awmap_point.y = lanelet_point.y();
-  awmap_point.z = utm_point.ele;
-  awmap_point.pcd="";
-  awmap_point.mgrs=0;
-  awmap_point.epsg = 0;
-  awmap_point.lat = utm_point.lat;
-  awmap_point.lng = utm_point.lon;
-  return awmap_point;
-}
-
-autoware_map_msgs::Point convertPoint(const lanelet::Point3d &lanelet_point, const projection::UtmProjector &projector)
-{
-  lanelet::ConstPoint3d const_point(lanelet_point);
-  return convertPoint(const_point, projector);
-}
-
-boost::optional<ConstLanelet> getClosestOppositeLane(const LaneletMapPtr map,const routing::RoutingGraphPtr graph, const ConstLanelet &lanelet,const traffic_rules::TrafficRulesPtr traffic_rules)
-{
-  double padding = 1;
-  BasicPoint2d min,max;
-  for (auto beside : graph->besides(lanelet))
-  {
-    for(auto pt : beside.rightBound())
-    {
-      min.x() = (pt.x() < min.x()) ? pt.x() : min.x();
-      min.y() = (pt.y() < min.y()) ? pt.y() : min.y();
-      max.x() = (pt.x() > max.x()) ? pt.x() : max.x();
-      max.y() = (pt.y() > max.y()) ? pt.y() : max.y();
-    }
-  }
-  min.x() -= padding;
-  min.y() -= padding;
-  max.x() += padding;
-  max.y() += padding;
-
-  auto local_lanelets = map->laneletLayer.search(BoundingBox2d(min,max));
-
-  for (auto beside : graph->besides(lanelet))
-  {
-    for( auto l : local_lanelets )
-    {
-      if(!traffic_rules->canPass(l)) continue;
-      if ( beside.rightBound() == l.rightBound().invert()) {
-        return l;
-      }
-      if ( beside.leftBound() == l.leftBound().invert()) {
-        return l;
-      }
-    }
-  }
-  return boost::none;
-}
-
-void splitLine(ConstLineString3d laneBound, std::vector<BasicPoint3d> &splitted_points, const double resolution, const int partitions)
-{
-  splitted_points.push_back(laneBound.front());
-  double residue = 0; //residue length from previous segment
-  //loop through each segments in bound
-  BasicPoint3d prev_pt = laneBound.front();
-  for(const BasicPoint3d pt : laneBound)
-  {
-    if(splitted_points.size() >= partitions) break;
-    //continue if no points are made in current segment
-    double segment_length = geometry::distance2d(pt, prev_pt);
-    if(segment_length + residue < resolution) {
-      residue += segment_length;
-      prev_pt = pt;
-      continue;
-    }
-
-    BasicPoint3d direction = ( pt - prev_pt ) / segment_length;
-    for(double length = resolution - residue; length < segment_length; length += resolution)
-    {
-      BasicPoint3d partition_point = prev_pt + direction * length;
-      splitted_points.push_back(partition_point);
-      if(splitted_points.size() >= partitions) break;
-      residue = segment_length - length;
-    }
-    if(splitted_points.size() >= partitions) break;
-    prev_pt = pt;
-  }
-  splitted_points.push_back(laneBound.back());
-}
-
-void set_fine_centerline(Lanelet &lanelet)
-{
-  double resolution = 1;
-  LineString3d centerline;
-  std::vector<BasicPoint3d> left_points;
-  std::vector<BasicPoint3d> right_points;
-
-  //get length of longer border
-  double left_length = geometry::rangedLength(lanelet.leftBound().begin(), lanelet.leftBound().end());
-  double right_length = geometry::rangedLength(lanelet.rightBound().begin(), lanelet.rightBound().end());
-  double longer_distance = (left_length > right_length) ? left_length : right_length;
-  unsigned int partitions = ceil(longer_distance/resolution);
-  if(partitions == 0)
-  {
-    partitions = 1;
-  }
-  double left_resolution = left_length / partitions;
-  double right_resolution = right_length / partitions;
-
-  splitLine(lanelet.leftBound(), left_points, left_resolution, partitions);
-  splitLine(lanelet.rightBound(), right_points, right_resolution, partitions);
-
-  if(left_points.size() != partitions + 1 || right_points.size() != partitions + 1) {
-    ROS_ERROR_STREAM("something wrong with number of points. (right,left, partitions) " << right_points.size() << ","<< left_points.size() << "," << partitions << "failed to calculate centerline!!!" << std::endl);
-    exit(1);
-  }
-    for(unsigned int i = 0; i < partitions+1; i++)
-  {
-    BasicPoint3d center_basic_point = (right_points.at(i) + left_points.at(i)) / 2;
-    Point3d center_point(utils::getId(), center_basic_point.x(), center_basic_point.y(), center_basic_point.z());
-    centerline.push_back(center_point);
-  }
-  lanelet.setCenterline(centerline);
-    return;
-}
-
-int getNewId(const int id, const std::unordered_map<int, int> &waypoint_id_correction )
-{
-  auto new_waypoint_id = waypoint_id_correction.find(id);
-  if(new_waypoint_id != waypoint_id_correction.end()) {
-    return new_waypoint_id->second;
-  }else{
-    return id;
-  }
-}
-
-void createPoints(const std::vector<Lanelet> &vehicle_lanelets, const projection::UtmProjector projector,
-                  std::unordered_map<int, autoware_map_msgs::Point> &points_map)
-{
-  for (const auto &lanelet : vehicle_lanelets)
-  {
-    for( const auto &center_point : lanelet.centerline())
-    {
-      autoware_map_msgs::Point awmap_point = convertPoint(center_point, projector);
-      points_map[awmap_point.point_id] = awmap_point;
-    }
-  }
-}
-
-void createWaypoints(const std::vector<Lanelet> &vehicle_lanelets, const traffic_rules::TrafficRulesPtr traffic_rules,
-                     std::unordered_map<int, autoware_map_msgs::Waypoint> &waypoints_map)
-{
-  for (const auto &lanelet : vehicle_lanelets)
-  {
-    for( const auto &center_point : lanelet.centerline())
-    {
-      //waypoint_id must be same as point_id 
-      autoware_map_msgs::Waypoint awmap_waypoint;
-      awmap_waypoint.waypoint_id = center_point.id(); 
-      awmap_waypoint.point_id = center_point.id();
-      awmap_waypoint.velocity = traffic_rules->speedLimit(lanelet).speedLimit.value();
-      awmap_waypoint.stop_line = 0;
-      double distance_left = geometry::distance2d(center_point, lanelet.leftBound());
-      double distance_right= geometry::distance2d(center_point, lanelet.rightBound());
-      awmap_waypoint.left_width = distance_left;
-      awmap_waypoint.right_width = distance_right;
-      awmap_waypoint.height = 0;
-      waypoints_map[awmap_waypoint.waypoint_id] = awmap_waypoint;
-    }
-  }
-}
-
-void createWaypointLaneRelations(const std::vector<Lanelet> &vehicle_lanelets,
-                                 std::vector<autoware_map_msgs::WaypointLaneRelation> &waypoint_lane_relations)
-{
-  for (const auto &lanelet : vehicle_lanelets)
-  {
-    int order = 1;
-    for( const auto &center_point : lanelet.centerline() )
-    {
-      //waypoint lane relation
-      autoware_map_msgs::WaypointLaneRelation waypoint_lane_relation;
-      waypoint_lane_relation.waypoint_id = center_point.id();
-      waypoint_lane_relation.lane_id = lanelet.id();
-      waypoint_lane_relation.order = order;
-      waypoint_lane_relations.push_back(waypoint_lane_relation);
-      order++;
-    }
-  }
-}
-
-void createWaypointRelations(const std::vector<Lanelet> &vehicle_lanelets,
-                             std::vector<autoware_map_msgs::WaypointRelation> &waypoint_relations)
-{
-  for (const auto &lanelet : vehicle_lanelets)
-  {
-    int order = 1;
-    ConstPoint3d prev_center_point;
-    for( const auto &center_point : lanelet.centerline() )
-    {
-      if (order > 1 ) {
-        autoware_map_msgs::WaypointRelation waypoint_relation;
-        waypoint_relation.waypoint_id = prev_center_point.id();
-        waypoint_relation.next_waypoint_id = center_point.id();
-        //angle from north(y) in CCW
-        waypoint_relation.yaw = M_PI/2 - atan2(center_point.y() - prev_center_point.y(), center_point.x() - prev_center_point.x());
-        waypoint_relation.blinker = 0;
-        waypoint_relation.distance = geometry::distance2d(center_point, prev_center_point);
-        waypoint_relations.push_back(waypoint_relation);
-      }
-      prev_center_point = center_point;
-      order++;
-    }
-  }
-}
-
-void createLanes(const std::vector<Lanelet> &vehicle_lanelets, const traffic_rules::TrafficRulesPtr traffic_rules, const routing::RoutingGraphPtr vehicle_graph,
-                 std::unordered_map<int, autoware_map_msgs::Lane> &lanes_map)
-{
-  for (const auto &lanelet : vehicle_lanelets)
-  {
-    int order = 1;
-    Point3d prev_center_point;
-    double width_limit;
-    for( const auto &center_point : lanelet.centerline() )
-    {
-      double distance_left = geometry::distance2d(center_point, lanelet.leftBound());
-      double distance_right= geometry::distance2d(center_point, lanelet.rightBound());
-      double width = distance_left + distance_right;
-      width_limit = (width_limit > width) ? width_limit : width;
-    }
-
-    autoware_map_msgs::Lane awmap_lane;
-    awmap_lane.lane_id = lanelet.id();
-    awmap_lane.start_waypoint_id = lanelet.centerline().front().id();
-    awmap_lane.end_waypoint_id = lanelet.centerline().back().id();
-    awmap_lane.speed_limit = traffic_rules->speedLimit(lanelet).speedLimit.value();
-    awmap_lane.lane_number =vehicle_graph->lefts(lanelet).size() + 1;
-    awmap_lane.num_of_lanes = vehicle_graph->besides(lanelet).size() + 1;
-    awmap_lane.length = geometry::rangedLength(lanelet.centerline().begin(), lanelet.centerline().end());
-    awmap_lane.width_limit = lanelet::geometry::distance(lanelet.rightBound().front(), lanelet.leftBound().front());
-    awmap_lane.height_limit = 0;
-    awmap_lane.width_limit = width_limit;
-    lanes_map[awmap_lane.lane_id] = awmap_lane;
-  }
-}
-
-void createStopLines(const LineStringLayer &linestring_layer,
-                     const std::vector<autoware_map_msgs::WaypointRelation> &waypoint_relations,
-                     const std::unordered_map<int, autoware_map_msgs::Point> &points_map,
-                     std::unordered_map<int, autoware_map_msgs::Waypoint> &waypoints_map)
-{
-  //stop_line
-  for (const auto &line : linestring_layer)
-  {
-    if(line.attributeOr(AttributeName::Type, "")==AttributeValueString::StopLine) {
-      for(const auto &relation : waypoint_relations)
-      {
-        
-        auto wp = waypoints_map.at(relation.waypoint_id);
-                auto next_wp = waypoints_map.at(relation.next_waypoint_id);
-                auto next_pt = points_map.at(next_wp.point_id);
-                auto pt = points_map.at(wp.point_id);
-                double epsilon = 0.1;
-
-        geometry_msgs::Point l1, l2, p1, p2;
-        l1.x = line.front().x();
-        l1.y = line.front().y();
-        l1.z = 0;
-        l2.x = line.back().x();
-        l2.y = line.back().y();
-        l2.z = 0;
-        p1.x = pt.x;
-        p1.y = pt.y;
-        p1.z = 0;
-        p2.x = next_pt.x;
-        p2.y = next_pt.y;
-        p2.z = 0;
-
-                //stopline intersect with current waypoint
-        if(amathutils::distanceFromSegment(l1, l2, p1) <= epsilon )
-        {
-                    waypoints_map.at(relation.waypoint_id).stop_line = 1;
-                    continue;
-        }
-        //stopline intersect with next waypoint
-        if(amathutils::distanceFromSegment(l1, l2, p2) <= epsilon) {
-                    waypoints_map.at(relation.next_waypoint_id).stop_line = 1;
-                    continue;
-        }
-        //stopline intersets between current and next waypoint
-        if (amathutils::isIntersectLine(l1, l2, p1, p2)) {
-                    waypoints_map.at(relation.waypoint_id).stop_line = 1;
-                  }
-      }
-    }
-  }
-}
-
-void createLaneRelations( const std::vector<Lanelet> &vehicle_lanelets,
-                          const routing::RoutingGraphPtr vehicle_graph,
-                          std::vector<autoware_map_msgs::LaneRelation> &lane_relations)
-{
-  for (const auto &lanelet : vehicle_lanelets)
-  {
-    for( const auto &following : vehicle_graph->following(lanelet))
-    {
-      autoware_map_msgs::LaneRelation lane_relation;
-      lane_relation.lane_id = lanelet.id();
-      lane_relation.next_lane_id = following.id();
-      lane_relation.blinker = 0;
-      lane_relations.push_back(lane_relation);
-    }
-  }
-}
-
-void createLaneChangeRelations(const std::vector<Lanelet> &vehicle_lanelets,
-                               const routing::RoutingGraphPtr vehicle_graph,
-                               std::vector<autoware_map_msgs::LaneChangeRelation> &lane_change_relations)
-{
-  for (const auto &lanelet : vehicle_lanelets)
-  {
-    if( vehicle_graph->right(lanelet) ) {
-      ConstLanelet right_lanelet = vehicle_graph->right(lanelet).value();
-      autoware_map_msgs::LaneChangeRelation lane_change_relation;
-      lane_change_relation.lane_id = lanelet.id();
-      lane_change_relation.next_lane_id = right_lanelet.id();
-      lane_change_relation.blinker = BLINKER_RIGHT;
-      lane_change_relations.push_back(lane_change_relation);
-    }
-    if( vehicle_graph->left(lanelet)) {
-      ConstLanelet left_lanelet = vehicle_graph->left(lanelet).value();
-      autoware_map_msgs::LaneChangeRelation lane_change_relation;
-      lane_change_relation.lane_id = lanelet.id();
-      lane_change_relation.next_lane_id = left_lanelet.id();
-      lane_change_relation.blinker = BLINKER_LEFT;
-      lane_change_relations.push_back(lane_change_relation);
-    }
-  }
-}
-
-void createOppositeLaneRelation(const std::vector<Lanelet> &vehicle_lanelets,
-                                const LaneletMapPtr map,
-                                const traffic_rules::TrafficRulesPtr traffic_rules,
-                                const routing::RoutingGraphPtr vehicle_graph,
-                                std::vector<autoware_map_msgs::OppositeLaneRelation> &opposite_lane_relations)
-{
-  for (const auto &lanelet : vehicle_lanelets)
-  {
-    auto closest_opposite_opt = getClosestOppositeLane(map,vehicle_graph,lanelet,traffic_rules);
-    if(closest_opposite_opt!=boost::none) {
-      ConstLanelet closest_opposite = closest_opposite_opt.value();
-      for(const auto &opposite_lane : vehicle_graph->besides(closest_opposite))
-      {
-        autoware_map_msgs::OppositeLaneRelation opposite_lane_relation;
-        opposite_lane_relation.lane_id = lanelet.id();
-        opposite_lane_relation.opposite_lane_id = opposite_lane.id();
-        bool already_added = false;
-        for(auto relation : opposite_lane_relations)
-        {
-          if(relation.lane_id == opposite_lane_relation.lane_id &&
-             relation.opposite_lane_id == opposite_lane_relation.opposite_lane_id) {
-            already_added =true;
-          }
-          if(relation.opposite_lane_id == opposite_lane_relation.lane_id &&
-             relation.lane_id == opposite_lane_relation.opposite_lane_id) {
-            already_added =true;
-          }
-        }
-        if(already_added ==false) {
-          opposite_lane_relations.push_back(opposite_lane_relation);
-        }
-      }
-    }
-  }
-}
-
-void createSignalLights(const std::vector<Lanelet> &vehicle_lanelets,
-                        const projection::UtmProjector &projector,
-                        std::unordered_map<int, autoware_map_msgs::Point> &points_map,
-                        std::vector<autoware_map_msgs::Signal> &signals,
-                        std::vector<autoware_map_msgs::SignalLight> &signal_lights)
-{
-  std::map<Id, bool> checklist;
-  for (const auto &lanelet : vehicle_lanelets)
-  {
-    auto traffic_light_reg_elems = lanelet.regulatoryElementsAs<TrafficLight>();
-    for (const auto traffic_light_reg_elem : traffic_light_reg_elems)
-    {
-      for( const auto traffic_light : traffic_light_reg_elem->trafficLights())
-      {
-
-        //skip if it is already added.
-        if(checklist.find(traffic_light.id()) != checklist.end() ) continue;
-        if(!traffic_light.isLineString())
-        {
-          ROS_ERROR_STREAM("current converter only supports traffic light expressed in linestrings. (no polygons)");
-          continue;
-        }
-
-        autoware_map_msgs::Signal awmap_signal;
-        awmap_signal.signal_id = traffic_light.id();
-        signals.push_back(awmap_signal);
-        checklist[traffic_light.id()] = true;
-
-        int color_count = 1;
-        ConstLineString3d light_bulbs =  traffic_light.lineString().value();
-        for(const auto &bulb : light_bulbs)
-        {
-          autoware_map_msgs::Point awmap_point;
-          awmap_point = convertPoint(ConstPoint3d(bulb), projector);
-          points_map[awmap_point.point_id] = awmap_point;
-
-          autoware_map_msgs::SignalLight awmap_signal_light;
-          awmap_signal_light.signal_light_id = bulb.id();
-          awmap_signal_light.point_id = awmap_point.point_id;
-          awmap_signal_light.signal_id = awmap_signal.signal_id;
-          //angle from north in CCW
-          double horizontal_angle = 0;
-          if(lanelet.centerline().size() >= 2)
-          {
-            int size = lanelet.centerline().size();
-            auto p1 = lanelet.centerline()[size - 1];
-            auto p2 = lanelet.centerline()[size - 2];
-            horizontal_angle = ( M_PI /2 - atan2(p2.y() - p1.y(), p2.x() - p1.x() ) )* 180.0/M_PI;
-          }
-          awmap_signal_light.horizontal_angle = horizontal_angle;
-          awmap_signal_light.vertical_angle = 90; //lamps are always facing parallel to ground
-          awmap_signal_light.arrow_type = 0;
-
-          //traffic lights are assumed to be in red, yellow, green order
-          if(color_count == 1) awmap_signal_light.color_type = SIGNAL_RED;
-          else if (color_count == 2)
-            awmap_signal_light.color_type = SIGNAL_YELLOW;
-          else if (color_count == 3)
-            awmap_signal_light.color_type = SIGNAL_GREEN;
-          else awmap_signal_light.color_type = SIGNAL_UNKNOWN;
-          color_count++;
-          signal_lights.push_back(awmap_signal_light);
-        }
-      }
-    }
-  }
-}
-
-void createLaneSignalLightRelations( const std::vector<Lanelet> &vehicle_lanelets,
-                                     std::vector<autoware_map_msgs::LaneSignalLightRelation> &lane_signal_light_relations)
-{
-  //signal_light
-  for (const auto &lanelet : vehicle_lanelets)
-  {
-    auto traffic_light_reg_elems = lanelet.regulatoryElementsAs<TrafficLight>();
-    for (const auto traffic_light_reg_elem : traffic_light_reg_elems)
-    {
-      for( const auto traffic_light : traffic_light_reg_elem->trafficLights())
-      {
-        if(!traffic_light.lineString()) continue;
-        ConstLineString3d light_bulbs =  traffic_light.lineString().value();
-        for(const auto &bulb : light_bulbs)
-        {
-          //lane signal relations
-          autoware_map_msgs::LaneSignalLightRelation lane_signal_light_relation;
-          lane_signal_light_relation.lane_id = lanelet.id();
-          lane_signal_light_relation.signal_light_id = bulb.id();
-          lane_signal_light_relations.push_back(lane_signal_light_relation);
-        }
-      }
-    }
-  }
-}
-
-void createWaypointSignalRelations(const std::vector<Lanelet> &vehicle_lanelets,
-                                   std::vector<autoware_map_msgs::WaypointSignalRelation> &waypoint_signal_relations)
-{
-  //signal_light
-  for (const auto &lanelet : vehicle_lanelets)
-  {
-    auto traffic_light_reg_elems = lanelet.regulatoryElementsAs<TrafficLight>();
-    for (const auto traffic_light_reg_elem : traffic_light_reg_elems)
-    {
-      ConstLineString3d stopline;
-      if(traffic_light_reg_elem->stopLine())
-      {
-        stopline = traffic_light_reg_elem->stopLine().value();
-      }
-      else
-      {
-        ConstPoint3d p1_const = lanelet.leftBound().back();
-        ConstPoint3d p2_const = lanelet.rightBound().back();
-        Point3d p1(p1_const.id(), p1_const.x(), p1_const.y(), p1_const.z());
-        Point3d p2(p2_const.id(), p2_const.x(), p2_const.y(), p2_const.z());
-        LineString3d ls(utils::getId(), {p1, p2});
-        stopline = ls;
-      }
-
-      for( const auto traffic_light : traffic_light_reg_elem->trafficLights())
-      {
-        if(!traffic_light.lineString()) continue;
-
-        //get closest waypoint from stopline
-        int stopping_point_id = 0;
-        double min_distance = std::numeric_limits<double>::max();
-        for(auto pt : lanelet.centerline())
-        {
-          double distance = geometry::distance2d(pt, stopline);
-          if(distance < min_distance)
-          {
-            min_distance = distance;
-            stopping_point_id = pt.id();
-          }
-        }
-        autoware_map_msgs::WaypointSignalRelation waypoint_signal_relation;
-        waypoint_signal_relation.signal_id = traffic_light.lineString()->id();
-        waypoint_signal_relation.waypoint_id = stopping_point_id;
-        waypoint_signal_relations.push_back(waypoint_signal_relation);
-      }
-    }
-  }
-}
-
-void getAllConflictingLanelets( const Lanelet &lanelet,
-                                const routing::RoutingGraphPtr vehicle_graph,
-                                std::vector<ConstLanelet> &conflicting_lanelets,
-                                std::set<Id> &already_calculated)
-{
-  const auto conflicting_lanelets_or_areas = vehicle_graph->conflicting(lanelet);
-  if(conflicting_lanelets_or_areas.size() == 0) return;
-  conflicting_lanelets.push_back(lanelet);
-  already_calculated.insert(lanelet.id());
-  for ( auto conflicting_lanelet_area : conflicting_lanelets_or_areas)
-  {
-    if(!conflicting_lanelet_area.lanelet()) continue;
-    ConstLanelet conflicting_lanelet = conflicting_lanelet_area.lanelet().value();
-    if(already_calculated.find(conflicting_lanelet.id()) != already_calculated.end()) continue;
-    conflicting_lanelets.push_back(conflicting_lanelet);
-    already_calculated.insert(conflicting_lanelet.id());
-  }
-
-  //continue if it is only conflicting with areas
-  if(conflicting_lanelets.size() == 1) {
-    return;
-  }
-  //get all linked intersecting lanes
-  std::stack<ConstLanelet> stack;
-  //initial setup for stack
-  for ( auto conflicting_lanelet : conflicting_lanelets)
-  {
-    stack.push(conflicting_lanelet);
-  }
-  while(!stack.empty()) {
-    const auto tmp_conflicting_lanelets_or_areas = vehicle_graph->conflicting(stack.top());
-    stack.pop();
-
-    for (const auto &la : tmp_conflicting_lanelets_or_areas)
-    {
-      if(!la.lanelet()) continue;
-      ConstLanelet l = la.lanelet().value();
-      if(already_calculated.find(l.id()) == already_calculated.end()) {
-        stack.push(l);
-        conflicting_lanelets.push_back(l);
-        already_calculated.insert(l.id());
-      }
-    }
-  }
-  return;
-}
-
-void createIntersections(const std::vector<Lanelet> &vehicle_lanelets,
-                         const routing::RoutingGraphPtr vehicle_graph,
-                         const projection::UtmProjector &projector,
-                         std::unordered_map<int, autoware_map_msgs::Point> &points_map,
-                         std::vector<autoware_map_msgs::Area> &areas,
-                         std::vector<autoware_map_msgs::LaneAttributeRelation> &lane_attribute_relations)
-{
-  std::set<Id> already_calculated;
-
-  for (const auto &lanelet : vehicle_lanelets)
-  {
-    //skip if lanelet is already used
-    if(already_calculated.find(lanelet.id()) != already_calculated.end()) {
-      continue;
-    }
-    std::vector<ConstLanelet> conflicting_lanelets;
-    getAllConflictingLanelets(lanelet, vehicle_graph, conflicting_lanelets, already_calculated);    
-    if(conflicting_lanelets.size() <= 1) continue;
-    
-    //calculate convex hull
-    std::vector<BasicPoint2d> vertices;
-    std::vector<BasicPoint2d> hull;
-    for ( const auto &conflicting_lanelet : conflicting_lanelets)
-    {
-      for( const auto &pt : conflicting_lanelet.polygon3d())
-      {
-        vertices.push_back(utils::to2D(pt.basicPoint()));
-      }
-    }
-    boost::geometry::convex_hull(vertices, hull);
-    std::vector<int> ids;
-    for( const auto &pt : hull)
-    {
-      autoware_map_msgs::Point awmap_point;
-      const auto basic_pt3d = utils::to3D(pt);
-      Point3d pt3d(utils::getId(), basic_pt3d.x(), basic_pt3d.y(), basic_pt3d.z());
-      awmap_point = convertPoint(ConstPoint3d(pt3d), projector);
-      points_map[awmap_point.point_id] = awmap_point;
-      ids.push_back(awmap_point.point_id);
-    }
-    //intersection area
-    autoware_map_msgs::Area awmap_area;
-    awmap_area.area_id = utils::getId();
-    awmap_area.point_ids = ids;
-    areas.push_back(awmap_area);
-
-    //lane attribute relations
-    autoware_map_msgs::LaneAttributeRelation lane_attribute_relation;
-    lane_attribute_relation.attribute_type = autoware_map_msgs::LaneAttributeRelation::INTERSECTION;
-    lane_attribute_relation.area_id = awmap_area.area_id;
-    for ( const auto &conflicting_lanelet : conflicting_lanelets)
-    {
-      lane_attribute_relation.lane_id = conflicting_lanelet.id();
-      lane_attribute_relations.push_back(lane_attribute_relation);
-    }
-  }
-}
-
-void createCrossWalks(const std::vector<Lanelet> &vehicle_lanelets,
-                      const routing::RoutingGraphContainer &overall_graphs,
-                      const projection::UtmProjector &projector,
-                      std::unordered_map<int, autoware_map_msgs::Point> &points_map,
-                      std::vector<autoware_map_msgs::Area> &areas,
-                      std::vector<autoware_map_msgs::LaneAttributeRelation> &lane_attribute_relations )
-{
-  std::set<Id> already_calculated;
-  for (const auto &lanelet : vehicle_lanelets)
-  {
-    const auto cross_walk_lanelets = overall_graphs.conflictingInGraph(lanelet,1);
-    for ( const auto &cross_walk_lanelet : cross_walk_lanelets)
-    {
-      int cross_walk_area_id;
-      if(already_calculated.find(cross_walk_lanelet.id()) == already_calculated.end() )
-      {
-        std::vector<ConstPoint3d> vertices;
-        for( const auto &pt : cross_walk_lanelet.polygon3d())
-        {
-          Point3d point3d(pt);
-          point3d.setId(utils::getId());
-          vertices.push_back(pt);
-        }
-
-        //add points
-        std::vector<int> ids;
-        for( const auto &pt : vertices)
-        {
-          autoware_map_msgs::Point awmap_point;
-          awmap_point = convertPoint(pt, projector);
-          points_map[awmap_point.point_id] = awmap_point;
-          ids.push_back(awmap_point.point_id);
-        }
-
-        //cross walk area
-        autoware_map_msgs::Area awmap_area;
-        awmap_area.area_id = cross_walk_lanelet.id();
-        awmap_area.point_ids = ids;
-        areas.push_back(awmap_area);
-
-        already_calculated.insert(cross_walk_lanelet.id());
-      }
-
-      autoware_map_msgs::LaneAttributeRelation lane_attribute_relation;
-      lane_attribute_relation.lane_id = lanelet.id();
-      lane_attribute_relation.attribute_type = autoware_map_msgs::LaneAttributeRelation::CROSS_WALK;
-      lane_attribute_relation.area_id = cross_walk_lanelet.id();
-      lane_attribute_relations.push_back(lane_attribute_relation);
-    }
-  }
-}
-
-void removeOverlappingWaypoints(  const std::unordered_map<int, autoware_map_msgs::Point> points_map,
-                                  std::unordered_map<int, autoware_map_msgs::Waypoint> &waypoints_map,
-                                  std::vector<autoware_map_msgs::Lane> &lanes,
-                                  std::vector<autoware_map_msgs::WaypointLaneRelation> &waypoint_lane_relations,
-                                  std::vector<autoware_map_msgs::WaypointRelation> &waypoint_relations,
-                                  std::vector<autoware_map_msgs::WaypointSignalRelation> &waypoint_signal_relations )
-{
-  //preparation to remove overlapping waypoint
-  std::set<Id> wp_checklist;
-  std::unordered_map<int, int> waypoint_id_correction;
-
-  std::vector<int> start_end_points;
-  for(const auto &lane : lanes)
-  {
-    start_end_points.push_back(lane.start_waypoint_id);
-    start_end_points.push_back(lane.end_waypoint_id);
-  }
-
-  const double epsilon = 1e-6;
-  for (int i = 0; i < start_end_points.size(); i++)
-  {
-    // for (auto i_id : start_end_points)
-    Id i_id = start_end_points.at(i);
-    if(wp_checklist.find(i_id) != wp_checklist.end()) continue;
-    std::vector<Id> same_points;
-    same_points.push_back(i_id);
-    for(int j = i+1; j < start_end_points.size(); j++)
-    {
-      // for(auto j_id : start_end_points)
-      Id j_id = start_end_points.at(j);
-      if (i_id == j_id || wp_checklist.find(i_id) != wp_checklist.end() ) continue;
-      //find position of waypoint using waypoint_id = point_id
-      auto i_wp = waypoints_map.at(i_id);
-      auto j_wp = waypoints_map.at(j_id);
-      auto i_point = points_map.at(i_wp.point_id);
-      auto j_point = points_map.at(j_wp.point_id);
-      auto i_point3d = Point3d(i_point.x, i_point.y, i_point.z);
-      auto j_point3d = Point3d(j_point.x, j_point.y, j_point.z);
-      if(geometry::distance2d(i_point3d, j_point3d) < epsilon) {
-        same_points.push_back(j_id);
-      }
-    }
-    Id smallest_id = i_id;
-    for(const auto id : same_points)
-    {
-      if(smallest_id > id) smallest_id = id;
-    }
-    for(const auto id : same_points)
-    {
-      wp_checklist.insert(id);
-      if(id == smallest_id) continue;
-      waypoint_id_correction[id] = smallest_id;
-    }
-  }
-
-  //rewrite waypoint_id in relations 
-  for(auto &lane : lanes)
-  {
-    lane.start_waypoint_id = getNewId(lane.start_waypoint_id, waypoint_id_correction);
-    lane.end_waypoint_id = getNewId(lane.end_waypoint_id, waypoint_id_correction);
-  }
-  for(auto &relation : waypoint_lane_relations)
-  {
-    relation.waypoint_id = getNewId(relation.waypoint_id, waypoint_id_correction);
-  }
-  for(auto &relation : waypoint_relations)
-  {
-    relation.waypoint_id = getNewId(relation.waypoint_id, waypoint_id_correction);
-    relation.next_waypoint_id = getNewId(relation.next_waypoint_id, waypoint_id_correction);
-  }
-  for(auto &relation : waypoint_signal_relations)
-  {
-    relation.waypoint_id = getNewId(relation.waypoint_id, waypoint_id_correction);
-  }
-  
-  //remove overlapping waypoints
-  for(auto itr =  waypoints_map.begin(); itr != waypoints_map.end();)
-  {
-    if (waypoint_id_correction.find(itr->first) != waypoint_id_correction.end())
-    {
-      itr = waypoints_map.erase(itr);
-    }
-    else
-    {
-      ++itr;
-    }
-  }
-}
-
-void convertLanelet2AutowareMap(LaneletMapPtr map,
-                                projection::UtmProjector projector,
-                                std::vector<autoware_map_msgs::Area> &areas,
-                                std::vector<autoware_map_msgs::Lane> &lanes,
-                                std::vector<autoware_map_msgs::LaneAttributeRelation> &lane_attribute_relations,
-                                std::vector<autoware_map_msgs::LaneChangeRelation> &lane_change_relations,
-                                std::vector<autoware_map_msgs::LaneRelation> &lane_relations,
-                                std::vector<autoware_map_msgs::LaneSignalLightRelation> &lane_signal_light_relations,
-                                std::vector<autoware_map_msgs::OppositeLaneRelation> &opposite_lane_relations,
-                                std::vector<autoware_map_msgs::Point> &points,
-                                std::vector<autoware_map_msgs::Signal> &signals,
-                                std::vector<autoware_map_msgs::SignalLight> &signal_lights,
-                                std::vector<autoware_map_msgs::Wayarea> &wayareas,
-                                std::vector<autoware_map_msgs::Waypoint> &waypoints,
-                                std::vector<autoware_map_msgs::WaypointLaneRelation> &waypoint_lane_relations,
-                                std::vector<autoware_map_msgs::WaypointRelation> &waypoint_relations,
-                                std::vector<autoware_map_msgs::WaypointSignalRelation> &waypoint_signal_relations)
-{
-
-
-
-  traffic_rules::TrafficRulesPtr traffic_rules =
-    traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle);
-  routing::RoutingGraphPtr vehicle_graph = routing::RoutingGraph::build(*map, *traffic_rules);
-  traffic_rules::TrafficRulesPtr pedestrian_rules =
-    traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Pedestrian);
-  routing::RoutingGraphConstPtr pedestrian_graph = routing::RoutingGraph::build(*map, *pedestrian_rules);
-  routing::RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph});
-
-  //get Lanes
-  int waypoint_id = 1;
-  int point_id = 1;
-  int lane_id = 1;
-  std::unordered_map<Id, int> lane_id_table;
-  std::unordered_map<int, autoware_map_msgs::Lane> lanes_map;
-  std::unordered_map<int, autoware_map_msgs::Point> points_map;
-  std::unordered_map<int, autoware_map_msgs::Waypoint> waypoints_map;
-
-
-
-  std::vector<Lanelet> vehicle_lanelets;
-
-  for (const auto &lanelet : map->laneletLayer)
-  {
-    if(traffic_rules->canPass(lanelet)) {
-      if(lanelet.leftBound().empty() || lanelet.rightBound().empty()) {
-        continue;
-      }
-      vehicle_lanelets.push_back(lanelet);
-      lane_id_table[lanelet.id()] = lane_id++;
-    }
-  }
-
-  for( auto & lanelet : vehicle_lanelets)
-  {
-    set_fine_centerline(lanelet);
-  }
-
-    createPoints(vehicle_lanelets, projector, points_map);
-    createWaypoints(vehicle_lanelets, traffic_rules, waypoints_map);
-    createLanes(vehicle_lanelets, traffic_rules, vehicle_graph, lanes_map);
-    createLaneRelations(vehicle_lanelets, vehicle_graph, lane_relations);
-    createLaneChangeRelations(vehicle_lanelets, vehicle_graph, lane_change_relations);
-    createOppositeLaneRelation(vehicle_lanelets, map, traffic_rules, vehicle_graph, opposite_lane_relations);
-    createWaypointLaneRelations(vehicle_lanelets, waypoint_lane_relations);
-    createSignalLights(vehicle_lanelets, projector, points_map,signals, signal_lights);
-    createWaypointRelations(vehicle_lanelets, waypoint_relations);
-    createLaneSignalLightRelations(vehicle_lanelets, lane_signal_light_relations);
-    createWaypointSignalRelations(vehicle_lanelets, waypoint_signal_relations);
-    createStopLines(map->lineStringLayer, waypoint_relations, points_map, waypoints_map);
-    createIntersections(vehicle_lanelets, vehicle_graph, projector, points_map, areas, lane_attribute_relations);
-    createCrossWalks(vehicle_lanelets, overall_graphs, projector, points_map, areas, lane_attribute_relations);
-  
-  removeOverlappingWaypoints(points_map, waypoints_map, lanes, waypoint_lane_relations, waypoint_relations, waypoint_signal_relations);
-
-  for(const auto &item : waypoints_map)
-  {
-    waypoints.push_back(item.second);
-  }
-  for(const auto &item : lanes_map)
-  {
-    lanes.push_back(item.second);
-  }
-  for(const auto &item : points_map)
-  {
-    points.push_back(item.second);
-  }
-  
-  for( auto &point : points)
-  {
-    fixPointCoordinate(point);
-  }  
-}

src/ros/catkin/src/vector_map_converter/src/lanelet2vectormap.cpp

@@ -1,193 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-#include <vector_map_converter/lanelet2vectormap.hpp>
-#include <vector_map_converter/autoware2vectormap.hpp>
-#include <vector_map_converter/lanelet2autowaremap.hpp>
-
-void printUsage()
-{
-  std::cout << "Required parameters" << std::endl;
-  std::cout << "_map_file:=<path to laneletmap>" << std::endl;
-  std::cout << "_origin_lat:=latitude origin used for xyz projection>" << std::endl;
-  std::cout << "_origin_lon:=<longitude origin used for xyz projection>" << std::endl;
-}
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "lanelet2vectormap_converter");
-    ros::NodeHandle nh;
-    ros::NodeHandle pnh("~");
-
-    if(!pnh.hasParam("map_file")){
-      ROS_ERROR_STREAM("You must specify file path!");
-      printUsage();
-      exit(1);
-    }
-    if(!pnh.hasParam("origin_lat") || !pnh.hasParam("origin_lon") ){
-      ROS_ERROR_STREAM("You must specify an origin!");
-      printUsage();
-      exit(1);
-    }
-
-    std::string map_file_path;
-    std::string save_dir;
-    double origin_lat;
-    double origin_lon;
-    bool wayareas_from_lanes;
-    pnh.param<std::string>("map_file", map_file_path, "");
-    pnh.param<std::string>("save_dir", save_dir, "./");
-    pnh.param<double>("origin_lat", origin_lat, 0.0);
-    pnh.param<double>("origin_lon", origin_lon,0.0);
-    pnh.param<bool>("wayareas_from_lanes", wayareas_from_lanes, false);
-
-
-    std::vector<autoware_map_msgs::Point> points;
-    std::vector<autoware_map_msgs::Lane> lanes;
-    std::vector<autoware_map_msgs::LaneAttributeRelation> lane_attribute_relations;
-    std::vector<autoware_map_msgs::LaneRelation> lane_relations;
-    std::vector<autoware_map_msgs::LaneSignalLightRelation> lane_signal_light_relations;
-    std::vector<autoware_map_msgs::LaneChangeRelation> lane_change_relations;
-    std::vector<autoware_map_msgs::OppositeLaneRelation> opposite_lane_relations;
-    std::vector<autoware_map_msgs::Area> areas;
-    std::vector<autoware_map_msgs::Signal> signals;
-    std::vector<autoware_map_msgs::SignalLight> signal_lights;
-    std::vector<autoware_map_msgs::Wayarea> wayareas;
-    std::vector<autoware_map_msgs::Waypoint> waypoints;
-    std::vector<autoware_map_msgs::WaypointLaneRelation> waypoint_lane_relations;
-    std::vector<autoware_map_msgs::WaypointRelation> waypoint_relations;
-    std::vector<autoware_map_msgs::WaypointSignalRelation> waypoint_signal_relations;
-
-    std::vector<vector_map_msgs::Area> vmap_areas;
-    std::vector<vector_map_msgs::CrossRoad> vmap_cross_roads;
-    std::vector<vector_map_msgs::CrossWalk> vmap_cross_walks;
-    std::vector<vector_map_msgs::DTLane> vmap_dtlanes;
-    std::vector<vector_map_msgs::Lane> vmap_lanes;
-    std::vector<vector_map_msgs::Line> vmap_lines;
-    std::vector<vector_map_msgs::Node> vmap_nodes;
-    std::vector<vector_map_msgs::Point> vmap_points;
-    std::vector<vector_map_msgs::Pole> vmap_dummy_poles;
-    std::vector<vector_map_msgs::RoadSign> vmap_road_signs;
-    std::vector<vector_map_msgs::Signal> vmap_signals;
-    std::vector<vector_map_msgs::StopLine> vmap_stop_lines;
-    std::vector<vector_map_msgs::UtilityPole> vmap_dummy_uility_poles;
-    std::vector<vector_map_msgs::Vector> vmap_vectors;
-    std::vector<vector_map_msgs::WayArea> vmap_way_areas;
-    std::vector<vector_map_msgs::WhiteLine> vmap_white_lines;
-
-    using namespace lanelet;
-    ErrorMessages errors;
-    projection::UtmProjector projector(Origin({origin_lat,origin_lon}));
-    std::cerr << map_file_path << std::endl;
-    LaneletMapPtr map = load(map_file_path, projector, &errors);
-    autoware_map::AutowareMapHandler map_handler;
-
-    // using namespace lanelet;
-    // ErrorMessages errors;
-    // projection::UtmProjector projector(Origin({40.68608001822,-89.59589859017}));
-    // LaneletMapPtr map = load("/home/mitsudome-r/data/peoria/lanelet_map/Peoria_2_24_2019_fixed.osm", projector,&errors);
-    //
-    // autoware_map::AutowareMapHandler map_handler;
-
-    convertLanelet2AutowareMap(map,
-                               projector,
-                               areas,
-                               lanes,
-                               lane_attribute_relations,
-                               lane_change_relations,
-                               lane_relations,
-                               lane_signal_light_relations,
-                               opposite_lane_relations,
-                               points,
-                               signals,
-                               signal_lights,
-                               wayareas,
-                               waypoints,
-                               waypoint_lane_relations,
-                               waypoint_relations,
-                               waypoint_signal_relations);
-
-    map_handler.setFromAutowareMapMsgs(areas,
-                                       lanes,
-                                       lane_attribute_relations,
-                                       lane_change_relations,
-                                       lane_relations,
-                                       lane_signal_light_relations,
-                                       opposite_lane_relations,
-                                       points,
-                                       signals,
-                                       signal_lights,
-                                       wayareas,
-                                       waypoints,
-                                       waypoint_lane_relations,
-                                       waypoint_relations,
-                                       waypoint_signal_relations
-                                       );
-    map_handler.resolveRelations();
-
-    getVectorMapMsgs( map_handler,
-                      vmap_areas,
-                      vmap_cross_roads,
-                      vmap_cross_walks,
-                      vmap_dtlanes,
-                      vmap_lanes,
-                      vmap_lines,
-                      vmap_nodes,
-                      vmap_points,
-                      vmap_dummy_poles,
-                      vmap_road_signs,
-                      vmap_signals,
-                      vmap_stop_lines,
-                      vmap_dummy_uility_poles,
-                      vmap_vectors,
-                      vmap_way_areas,
-                      vmap_white_lines
-                      );
-
-    complementVectorMap(map,
-                        projector,
-                        vmap_lines,
-                        vmap_points,
-                        vmap_dummy_poles,
-                        vmap_road_signs,
-                        vmap_stop_lines,
-                        vmap_vectors,
-                        vmap_white_lines
-                        );
-
-    if( wayareas_from_lanes )
-    {
-       createWayAreasFromLanes(map_handler, vmap_way_areas, vmap_areas, vmap_lines, vmap_points);
-    }
-
-    writeVectorMapMsgs( save_dir,
-                        vmap_areas,
-                        vmap_cross_roads,
-                        vmap_cross_walks,
-                        vmap_dtlanes,
-                        vmap_lanes,
-                        vmap_lines,
-                        vmap_nodes,
-                        vmap_points,
-                        vmap_dummy_poles,
-                        vmap_road_signs,
-                        vmap_signals,
-                        vmap_stop_lines,
-                        vmap_dummy_uility_poles,
-                        vmap_vectors,
-                        vmap_way_areas,
-                        vmap_white_lines
-                        );
-}

src/ros/catkin/src/vector_map_converter/src/lanelet2vectormap_core.cpp

@@ -1,121 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-#include <vector_map_converter/lanelet2vectormap.hpp>
-#include <vector_map_converter/autoware2vectormap.hpp>
-#include <vector_map_converter/lanelet2autowaremap.hpp>
-using namespace lanelet;
-void complementVectorMap(LaneletMapPtr map,
-                         projection::UtmProjector projector,
-                         std::vector<vector_map_msgs::Line> &vmap_lines,
-                         std::vector<vector_map_msgs::Point> &vmap_points,
-                         std::vector<vector_map_msgs::Pole> &vmap_dummy_poles,
-                         std::vector<vector_map_msgs::RoadSign> &vmap_road_signs,
-                         std::vector<vector_map_msgs::StopLine> &vmap_stop_lines,
-                         std::vector<vector_map_msgs::Vector> &vmap_vectors,
-                         std::vector<vector_map_msgs::WhiteLine> &vmap_white_lines
-                         )
-{
-    traffic_rules::TrafficRulesPtr traffic_rules =
-        traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle);
-    // routing::RoutingGraphPtr vehicle_graph = routing::RoutingGraph::build(*map, *traffic_rules);
-
-    std::vector<ConstLanelet> vehicle_lanelets;
-    for (const auto &lanelet : map->laneletLayer)
-    {
-        if(traffic_rules->canPass(lanelet)) {
-            if(lanelet.leftBound().empty() || lanelet.rightBound().empty()) {
-                continue;
-            }
-            vehicle_lanelets.push_back(lanelet);
-        }
-    }
-    // createStopLines(map, vmap_lines, vmap_points, vmap_stop_lines, vmap_road_signs, vmap_vectors, vmap_dummy_poles);
-    createWhitelines(vehicle_lanelets, projector, vmap_points, vmap_lines, vmap_white_lines);
-}
-
-void convertLineString2WhiteLine(ConstLineString3d line_string,
-                                 projection::UtmProjector projector,
-                                 std::vector<vector_map_msgs::Point> &vmap_points,
-                                 std::vector<vector_map_msgs::Line> &vmap_lines,
-                                 std::vector<vector_map_msgs::WhiteLine> &vmap_white_lines,
-                                 int &point_id, int &line_id, int &white_line_id )
-{
-
-    int cnt = 0;
-    for(auto lanelet_point : line_string)
-    {
-        autoware_map_msgs::Point amm_point;
-        vector_map_msgs::Point vmm_point;
-        amm_point = convertPoint(lanelet_point, projector);
-        fixPointCoordinate(amm_point);
-        convertPoint(vmm_point, amm_point);
-        vmm_point.pid = point_id;
-        vmap_points.push_back(vmm_point);
-        if(cnt == 0) {
-            cnt++;
-            point_id++;
-            continue;
-        }
-
-        vector_map_msgs::Line line;
-        line.lid = line_id;
-        line.bpid = point_id - 1;
-        line.fpid = point_id;
-        line.blid = 0;
-        line.flid = 0;
-        vmap_lines.push_back(line);
-
-        vector_map_msgs::WhiteLine white_line;
-        white_line.id = white_line_id;
-        white_line.lid = line_id;
-        white_line.width = 0.1;
-        white_line.color = 'W';
-        white_line.type = 0;
-        white_line.linkid = 0;
-        vmap_white_lines.push_back(white_line);
-
-        white_line_id++;
-        line_id++;
-        point_id++;
-        cnt++;
-    }
-
-}
-
-
-void createWhitelines(const std::vector<ConstLanelet> &lanelets,
-                      projection::UtmProjector projector,
-                      std::vector<vector_map_msgs::Point> &vmap_points,
-                      std::vector<vector_map_msgs::Line> &vmap_lines,
-                      std::vector<vector_map_msgs::WhiteLine> &vmap_white_lines
-                      )
-{
-    int line_id = getMaxId(vmap_lines) + 1;
-    int point_id = getMaxId(vmap_points) + 1;
-    int white_line_id = getMaxId(vmap_white_lines) + 1;
-    std::unordered_map<long long int, bool> checklist;
-    for (const auto &lanelet : lanelets)
-    {
-        if(checklist.find(lanelet.leftBound().id()) == checklist.end()) {
-            convertLineString2WhiteLine(lanelet.leftBound(), projector, vmap_points, vmap_lines, vmap_white_lines, point_id, line_id, white_line_id);
-            checklist[lanelet.leftBound().id()] = true;
-        }
-        if(checklist.find(lanelet.rightBound().id()) == checklist.end()) {
-            convertLineString2WhiteLine(lanelet.rightBound(), projector, vmap_points, vmap_lines, vmap_white_lines,point_id, line_id, white_line_id);
-            checklist[lanelet.rightBound().id()] = true;
-        }
-    }
-}

src/ros/catkin/src/vector_map_converter/src/opendrive2autowaremap.cpp

@@ -1,46 +0,0 @@
-
-#include <ros/ros.h>
-#include <opendrive2autoware_converter/opendrive_loader.h>
-#include <opendrive2autoware_converter/map_writer.h>
-
-void printUsage()
-{
-  std::cout << "opendrive2autoware_converter is a rosnode that convert OpenDRIVE map format (.xodr) to autoware map format (list of .csv files)." << std::endl << std::endl;
-  std::cout << "Commands: " << std::endl;
-  std::cout << "        opendrive2autoware_converter _map_file:=Source OpenDRIVE .xodr file name,  _country_codes_dir:=Countries' codes folder name, _save_dir:=Destination folder for Autoware map files, _resolution:=(optional)Waypoints resolution default is 0.5 meters, _keep_right:=(optional) True if car has to drive right lanes default is True" <<std::endl <<std::endl;  
-}
-
-int main(int argc, char **argv)
-{
-  ros::init(argc, argv, "opendrive2autoware_converter");
-
-  std::string src_path;
-  std::string dst_path;
-  std::string country_codes_path;
-  double wp_res = 0.5;
-  bool keep_right = true;
-  ros::NodeHandle pnh("~");
-
-  if(!pnh.hasParam("map_file")){
-    ROS_ERROR_STREAM("You must specify xodr file path!");
-    printUsage();
-    exit(1);
-  }
-  if(!pnh.hasParam("country_codes_dir")){
-    ROS_ERROR_STREAM("You must specify country code directory path!");
-    printUsage();
-    exit(1);
-  }
-
-  pnh.getParam("map_file", src_path);
-  pnh.getParam("country_codes_dir", country_codes_path);
-  pnh.param<std::string>("save_dir", dst_path, "./");
-  pnh.param<double>("resolution", wp_res, 0.5);
-  pnh.param<bool>("keep_right", keep_right, true);
-  
-  opendrive_converter::OpenDriveLoader map_loader;
-  PlannerHNS::RoadNetwork map;
-  map_loader.loadOpenDRIVE(src_path, country_codes_path, map, wp_res, keep_right);
-  opendrive_converter::MapWriter map_save;
-  map_save.writeAutowareMap(dst_path, map);
-}

src/ros/catkin/src/vector_map_converter/src/opendrive2autowaremap_core/map_writer.cpp

@@ -1,301 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-/*
- * opendrive2op_map_converter_core.cpp
- *
- *  Created on: Feb 11, 2019
- *      Author: hatem
- */
-
-#include "opendrive2autoware_converter/map_writer.h"
-#include "op_planner/RoadNetwork.h"
-#include <fstream>
-
-#include <autoware_map_msgs/LaneArray.h>
-#include <autoware_map_msgs/LaneAttributeRelationArray.h>
-#include <autoware_map_msgs/LaneRelationArray.h>
-#include <autoware_map_msgs/LaneSignalLightRelationArray.h>
-#include <autoware_map_msgs/LaneChangeRelationArray.h>
-#include <autoware_map_msgs/OppositeLaneRelationArray.h>
-#include <autoware_map_msgs/PointArray.h>
-#include <autoware_map_msgs/AreaArray.h>
-#include <autoware_map_msgs/SignalArray.h>
-#include <autoware_map_msgs/SignalLightArray.h>
-#include <autoware_map_msgs/WayareaArray.h>
-#include <autoware_map_msgs/WaypointArray.h>
-#include <autoware_map_msgs/WaypointLaneRelationArray.h>
-#include <autoware_map_msgs/WaypointRelationArray.h>
-#include <autoware_map_msgs/WaypointSignalRelationArray.h>
-
-#include "autoware_map/util.h"
-
-namespace opendrive_converter
-{
-	
-	bool exists(autoware_map_msgs::LaneArray &lanes, int lane_id){
-		for(const auto lane: lanes.data){
-			if(lane_id == lane.lane_id) return true;
-		}
-		return false;
-	}
-
-	bool exists(autoware_map_msgs::WaypointArray &waypoints, int waypoint_id){
-		for(const auto waypoint: waypoints.data){
-			if(waypoint_id == waypoint.waypoint_id) return true;
-		}
-		return false;
-	}
-
-MapWriter::MapWriter()
-{
-}
-
-MapWriter::~MapWriter()
-{
-}
-
-template <class T>
-void MapWriter::writeCSVFile(const std::string& folder, const std::string& title, const std::string& header, const std::vector<T>& data_list)
-{
-	if(data_list.size() < 2)
-			return;
-
-	std::ostringstream file_name;
-	file_name << folder;
-	file_name << title;
-	file_name << ".csv";
-
-	std::ofstream f(file_name.str().c_str());
-
-	if(f.is_open())
-	{
-		if(header.size() > 0)
-			f << header << "\r\n";
-		for(unsigned int i = 0 ; i < data_list.size(); i++)
-			f << data_list.at(i) << "\r\n";
-	}
-
-	f.close();
-}
-
-void MapWriter::writeAutowareMap(std::string folder_name, PlannerHNS::RoadNetwork& map)
-{
-	if(map.roadSegments.size() == 0)
-	{
-		std::cout << "Can't Write Empty Map (no lanes) into autoware format ! " << std::endl;
-		return;
-	}
-
-	autoware_map_msgs::PointArray points_list;
-	autoware_map_msgs::LaneArray lanes_list;
-	autoware_map_msgs::LaneRelationArray lane_relations_list;
-	autoware_map_msgs::SignalArray signals_list;
-	autoware_map_msgs::SignalLightArray lights_list;
-	autoware_map_msgs::WaypointSignalRelationArray wp_lights_relations_list;
-	autoware_map_msgs::WaypointArray waypoints_list;
-	autoware_map_msgs::WaypointRelationArray wp_relations_list;
-	autoware_map_msgs::WaypointLaneRelationArray wp_lanes_relations_list;
-
-	int points_id_seq = 1;
-	int signal_id_seq = 1;
-
-
-	for(unsigned int i=0; i<map.roadSegments.at(0).Lanes.size(); i++)
-	{
-		PlannerHNS::Lane* p_lane = &map.roadSegments.at(0).Lanes.at(i);
-
-		if(p_lane->points.size() < 2)
-		{
-			std::cout << "Writing Autoware format, Skip lane with less than 2 waypoints, with ID: " << p_lane->id << std::endl;
-			continue;
-		}
-
-		autoware_map_msgs::Lane l;
-		l.lane_id = p_lane->id;
-		l.lane_number = p_lane->num; //TODO check in document
-		l.length = p_lane->length;
-		l.num_of_lanes = p_lane->num; //TODO check in document
-		l.speed_limit = p_lane->speed;
-		l.start_waypoint_id = p_lane->points.at(0).id;
-		l.end_waypoint_id = p_lane->points.at(p_lane->points.size()-1).id;
-		l.height_limit = 0;
-		l.weight_limit = 0;
-		l.width_limit = p_lane->width;
-		lanes_list.data.push_back(l);
-
-		autoware_map_msgs::LaneRelation lr;
-		lr.lane_id = p_lane->id;
-		for(unsigned int k=0; k< p_lane->toIds.size(); k++)
-		{
-			lr.blinker = 0; //TODO check in document for later support
-			lr.next_lane_id = p_lane->toIds.at(k);
-			lane_relations_list.data.push_back(lr);
-		}
-
-		for(unsigned int j=0; j< p_lane->points.size(); j++)
-		{
-			autoware_map_msgs::Point p;
-			p.mgrs = 0; //TODO check in document
-			p.epsg = 0;  //TODO check in document
-			p.lat = 0;
-			p.lng = 0;
-			p.pcd = "";
-			p.point_id = points_id_seq++;
-			p.x = p_lane->points.at(j).pos.x;
-			p.y = p_lane->points.at(j).pos.y;
-			p.z = p_lane->points.at(j).pos.z;
-			points_list.data.push_back(p);
-
-			autoware_map_msgs::Waypoint wp;
-			wp.point_id = p.point_id;
-			wp.height = 0; //TODO check in document
-			wp.left_width = p_lane->points.at(j).collisionCost/2.0;
-			wp.right_width = p_lane->points.at(j).collisionCost/2.0;
-			wp.stop_line = p_lane->points.at(j).stopLineID;
-			wp.velocity = p_lane->points.at(j).v;
-			wp.waypoint_id = p_lane->points.at(j).id;
-			waypoints_list.data.push_back(wp);
-
-			autoware_map_msgs::WaypointRelation wpr;
-			wpr.waypoint_id = wp.waypoint_id;
-			wpr.yaw = p_lane->points.at(j).pos.a;
-			wpr.blinker = 0; //TODO check in document
-
-			if(j < p_lane->points.size()-1)
-			{
-				wpr.distance = hypot(p_lane->points.at(j+1).pos.y - p.y, p_lane->points.at(j+1).pos.x - p.x);
-			}
-			else
-			{
-				wpr.distance = 0;
-			}
-
-			for(unsigned int k=0; k < p_lane->points.at(j).toIds.size(); k++)
-			{
-				wpr.next_waypoint_id =  p_lane->points.at(j).toIds.at(k);
-				wp_relations_list.data.push_back(wpr);
-			}
-
-			autoware_map_msgs::WaypointLaneRelation wplr;
-			wplr.waypoint_id = wp.waypoint_id;
-			wplr.lane_id = l.lane_id;
-			wplr.order = j;
-			wp_lanes_relations_list.data.push_back(wplr);
-		}
-	}
-
-	//remove invalid relations
-	auto erase_itr = std::remove_if(lane_relations_list.data.begin(),lane_relations_list.data.end(),
- 								[&](autoware_map_msgs::LaneRelation lr){return (!exists(lanes_list, lr.next_lane_id)) ;});
-  for(auto itr = erase_itr; itr != lane_relations_list.data.end() ; itr++ ){
-		std::cout << "deleting: " << itr->lane_id << "->" << itr->next_lane_id << " " << (!exists(lanes_list, itr->lane_id)) << std::endl;
-	}
-	lane_relations_list.data.erase(erase_itr, lane_relations_list.data.end());
-
-	auto erase_itr2 = std::remove_if(wp_relations_list.data.begin(),wp_relations_list.data.end(),
- 								[&](autoware_map_msgs::WaypointRelation wr){return !exists(waypoints_list, wr.waypoint_id) || !exists(waypoints_list, wr.next_waypoint_id) ;});
-	wp_relations_list.data.erase(erase_itr2, wp_relations_list.data.end());
-
-
-	for(unsigned int i=0; i<map.trafficLights.size(); i++)
-	{
-
-		PlannerHNS::TrafficLight* p_light = &map.trafficLights.at(i);
-
-		autoware_map_msgs::Point p;
-		p.mgrs = 0; //TODO check in document
-		p.epsg = 0;  //TODO check in document
-		p.lat = 0;
-		p.lng = 0;
-		p.pcd = "";
-		p.point_id = points_id_seq++;
-		p.x = p_light->pos.x;
-		p.y = p_light->pos.y;
-		p.z = p_light->pos.z;
-		points_list.data.push_back(p);
-
-		autoware_map_msgs::Signal s;
-		s.signal_id = signal_id_seq++;
-		signals_list.data.push_back(s);
-
-		autoware_map_msgs::SignalLight sl;
-		sl.arrow_type = 0; //TODO support standard
-		sl.color_type = 0;
-		sl.point_id = p.point_id;
-		sl.signal_id = s.signal_id;
-		sl.signal_light_id = p_light->id;
-		sl.horizontal_angle = p_light->pos.a;
-
-		//sl.vertical_angle = p_light->rot.y; //TODO support standard and opendrive
-		lights_list.data.push_back(sl);
-
-		for(unsigned int j=0; j < p_light->laneIds.size(); j++)
-		{
-			autoware_map_msgs::WaypointSignalRelation wp_s_r;
-			PlannerHNS::Lane* p_op_lane = getLaneFromID(map, p_light->laneIds.at(j));
-			if(p_op_lane != nullptr)
-			{
-				wp_s_r.signal_id = sl.signal_id;
-				wp_s_r.waypoint_id = p_op_lane->points.at(p_op_lane->points.size()-1).id;
-				wp_lights_relations_list.data.push_back(wp_s_r);
-			}
-		}
-	}
-
-	for(unsigned int i=0; i<map.signs.size(); i++)
-	{
-		PlannerHNS::TrafficSign* p_sign = &map.signs.at(i);
-
-
-		autoware_map_msgs::Point p;
-		p.mgrs = 0; //TODO check in document
-		p.epsg = 0;  //TODO check in document
-		p.lat = 0;
-		p.lng = 0;
-		p.pcd = "";
-		p.point_id = points_id_seq++;
-		p.x = p_sign->pos.x;
-		p.y = p_sign->pos.y;
-		p.z = p_sign->pos.z;
-		points_list.data.push_back(p);
-
-
-	}
-
-
-	writeCSVFile(folder_name, "points", "point_id,x,y,z,lat,lng,pcd,mgrs,epsg", points_list.data);
-
-	writeCSVFile(folder_name, "lanes", "lane_id,start_waypoint_id,end_waypoint_id,"
-			"lane_number,num_of_lanes,speed_limit,length,width_limit,height_limit,weight_limit", lanes_list.data);
-
-	writeCSVFile(folder_name, "lane_relations", "lane_id,next_lane_id,blinker", lane_relations_list.data);
-
-	writeCSVFile(folder_name, "waypoints", "waypoint_id,point_id,velocity,stop_line,left_width,right_width,height", waypoints_list.data);
-
-	writeCSVFile(folder_name, "waypoint_relations", "waypoint_id,next_waypoint_id,yaw,blinker,distance", wp_relations_list.data);
-
-	writeCSVFile(folder_name, "waypoint_lane_relations", "waypoint_id,lane_id,order", wp_lanes_relations_list.data);
-
-	writeCSVFile(folder_name, "signals", "signal_id", signals_list.data);
-
-	writeCSVFile(folder_name, "signal_lights", "signal_light_id,signal_id,point_id,horizontal_angle,vertical_angle,color_type,arrow_type", lights_list.data);
-
-	writeCSVFile(folder_name, "waypoint_signal_relations", "waypoint_id,signal_id", wp_lights_relations_list.data);
-
-	std::cout << "Finish Writing map files .csv to folder: " << folder_name << std::endl;
-}
-
-}

src/ros/catkin/src/vector_map_converter/src/opendrive2autowaremap_core/odr_spiral.cpp

@@ -1,249 +0,0 @@
-/* ===================================================
- *  file:       odrSpiral.c
- * ---------------------------------------------------
- *  purpose:	free method for computing spirals
- *              in OpenDRIVE applications 
- * ---------------------------------------------------
- *  using methods of CEPHES library
- * ---------------------------------------------------
- *  first edit:	09.03.2010 by M. Dupuis @ VIRES GmbH
- *  last mod.:  09.03.2010 by M. Dupuis @ VIRES GmbH
- * ===================================================
-    Copyright 2010 VIRES Simulationstechnologie GmbH
-
-    Licensed under the Apache License, Version 2.0 (the "License");
-    you may not use this file except in compliance with the License.
-    You may obtain a copy of the License at
-
-        http://www.apache.org/licenses/LICENSE-2.0
-
-    Unless required by applicable law or agreed to in writing, software
-    distributed under the License is distributed on an "AS IS" BASIS,
-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-    See the License for the specific language governing permissions and
-    limitations under the License.
-    
-    
-    NOTE:
-    The methods have been realized using the CEPHES library 
-
-        http://www.netlib.org/cephes/
-
-    and do neither constitute the only nor the exclusive way of implementing 
-    spirals for OpenDRIVE applications. Their sole purpose is to facilitate 
-    the interpretation of OpenDRIVE spiral data.
- */
-
-/* ====== INCLUSIONS ====== */
-#include <stdio.h>
-#include <unistd.h>
-#include <math.h>
-#include "opendrive2autoware_converter/odr_spiral.h"
-
-/* ====== LOCAL VARIABLES ====== */
-
-/* S(x) for small x */
-static double sn[6] = {
--2.99181919401019853726E3,
- 7.08840045257738576863E5,
--6.29741486205862506537E7,
- 2.54890880573376359104E9,
--4.42979518059697779103E10,
- 3.18016297876567817986E11,
-};
-static double sd[6] = {
-/* 1.00000000000000000000E0,*/
- 2.81376268889994315696E2,
- 4.55847810806532581675E4,
- 5.17343888770096400730E6,
- 4.19320245898111231129E8,
- 2.24411795645340920940E10,
- 6.07366389490084639049E11,
-};
-
-/* C(x) for small x */
-static double cn[6] = {
--4.98843114573573548651E-8,
- 9.50428062829859605134E-6,
--6.45191435683965050962E-4,
- 1.88843319396703850064E-2,
--2.05525900955013891793E-1,
- 9.99999999999999998822E-1,
-};
-static double cd[7] = {
- 3.99982968972495980367E-12,
- 9.15439215774657478799E-10,
- 1.25001862479598821474E-7,
- 1.22262789024179030997E-5,
- 8.68029542941784300606E-4,
- 4.12142090722199792936E-2,
- 1.00000000000000000118E0,
-};
-
-/* Auxiliary function f(x) */
-static double fn[10] = {
-  4.21543555043677546506E-1,
-  1.43407919780758885261E-1,
-  1.15220955073585758835E-2,
-  3.45017939782574027900E-4,
-  4.63613749287867322088E-6,
-  3.05568983790257605827E-8,
-  1.02304514164907233465E-10,
-  1.72010743268161828879E-13,
-  1.34283276233062758925E-16,
-  3.76329711269987889006E-20,
-};
-static double fd[10] = {
-/*  1.00000000000000000000E0,*/
-  7.51586398353378947175E-1,
-  1.16888925859191382142E-1,
-  6.44051526508858611005E-3,
-  1.55934409164153020873E-4,
-  1.84627567348930545870E-6,
-  1.12699224763999035261E-8,
-  3.60140029589371370404E-11,
-  5.88754533621578410010E-14,
-  4.52001434074129701496E-17,
-  1.25443237090011264384E-20,
-};
-
-/* Auxiliary function g(x) */
-static double gn[11] = {
-  5.04442073643383265887E-1,
-  1.97102833525523411709E-1,
-  1.87648584092575249293E-2,
-  6.84079380915393090172E-4,
-  1.15138826111884280931E-5,
-  9.82852443688422223854E-8,
-  4.45344415861750144738E-10,
-  1.08268041139020870318E-12,
-  1.37555460633261799868E-15,
-  8.36354435630677421531E-19,
-  1.86958710162783235106E-22,
-};
-static double gd[11] = {
-/*  1.00000000000000000000E0,*/
-  1.47495759925128324529E0,
-  3.37748989120019970451E-1,
-  2.53603741420338795122E-2,
-  8.14679107184306179049E-4,
-  1.27545075667729118702E-5,
-  1.04314589657571990585E-7,
-  4.60680728146520428211E-10,
-  1.10273215066240270757E-12,
-  1.38796531259578871258E-15,
-  8.39158816283118707363E-19,
-  1.86958710162783236342E-22,
-};
-
-
-static double polevl( double x, double* coef, int n )
-{
-    double ans;
-    double *p = coef;
-    int i;
-
-    ans = *p++;
-    i   = n;
-
-    do
-    {
-        ans = ans * x + *p++;
-    }
-    while (--i);
-
-    return ans;
-}
-
-static double p1evl( double x, double* coef, int n )
-{
-    double ans;
-    double *p = coef;
-    int i;
-
-    ans = x + *p++;
-    i   = n - 1;
-
-    do
-    {
-        ans = ans * x + *p++;
-    }
-    while (--i);
-
-    return ans;
-}
-
-
-static void fresnel( double xxa, double *ssa, double *cca )
-{
-    double f, g, cc, ss, c, s, t, u;
-    double x, x2;
-
-    x  = fabs( xxa );
-    x2 = x * x;
-    
-    if ( x2 < 2.5625 )
-    {
-        t = x2 * x2;
-        ss = x * x2 * polevl (t, sn, 5) / p1evl (t, sd, 6);
-        cc = x * polevl (t, cn, 5) / polevl (t, cd, 6);
-    }
-    else if ( x > 36974.0 )
-    {
-        cc = 0.5;
-        ss = 0.5;
-    }
-    else
-    {
-        x2 = x * x;
-        t = M_PI * x2;
-        u = 1.0 / (t * t);
-        t = 1.0 / t;
-        f = 1.0 - u * polevl (u, fn, 9) / p1evl(u, fd, 10);
-        g = t * polevl (u, gn, 10) / p1evl (u, gd, 11);
-
-        t = M_PI * 0.5 * x2;
-        c = cos (t);
-        s = sin (t);
-        t = M_PI * x;
-        cc = 0.5 + (f * s - g * c) / t;
-        ss = 0.5 - (f * c + g * s) / t;
-    }
-
-    if ( xxa < 0.0 )
-    {
-        cc = -cc;
-        ss = -ss;
-    }
-
-    *cca = cc;
-    *ssa = ss;
-}
-
-
-/**
-* compute the actual "standard" spiral, starting with curvature 0
-* @param s      run-length along spiral
-* @param cDot   first derivative of curvature [1/m2]
-* @param x      resulting x-coordinate in spirals local co-ordinate system [m]
-* @param y      resulting y-coordinate in spirals local co-ordinate system [m]
-* @param t      tangent direction at s [rad]
-*/
-
-void odrSpiral( double s, double cDot, double *x, double *y, double *t )
-{
-    double a;
-
-    a = 1.0 / sqrt( fabs( cDot ) );
-    a *= sqrt( M_PI );
-    
-    fresnel( s / a, y, x );
-    
-    *x *= a;
-    *y *= a;
-    
-    if ( cDot < 0.0 )
-        *y *= -1.0;
-
-    *t = s * s * cDot * 0.5;
-}

src/ros/catkin/src/vector_map_converter/src/opendrive2autowaremap_core/opendrive_loader.cpp

@@ -1,560 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-/*
- * opendrive2autoware_converter_core.cpp
- *
- *  Created on: Feb 11, 2019
- *      Author: hatem
- */
-
-#include "opendrive2autoware_converter/opendrive_loader.h"
-#include <fstream>
-#include <ros/ros.h>
-
-namespace opendrive_converter
-{
-
-
-
-OpenDriveLoader::OpenDriveLoader(){
-}
-
-OpenDriveLoader::~OpenDriveLoader()
-{
-}
-
-void OpenDriveLoader::getFileNameInFolder(const std::string& path, std::vector<std::string>& out_list)
-{
-	out_list.clear();
-	DIR *dir;
-	struct dirent *ent;
-	if ((dir = opendir (path.c_str())) != NULL)
-	{
-	  while ((ent = readdir (dir)) != NULL)
-	  {
-		  std::string str(ent->d_name);
-		  if(str.compare(".") !=0 && str.compare("..") !=0)
-			  out_list.push_back(path+str);
-	  }
-	  closedir (dir);
-	}
-}
-
-void OpenDriveLoader::loadCountryCods(const std::string& codes_csv_folder)
-{
-	if(codes_csv_folder.size() > 0)
-	{
-		std::vector<std::string> files_names;
-		getFileNameInFolder(codes_csv_folder, files_names);
-		country_signal_codes_.clear();
-
-		for(unsigned int i=0; i < files_names.size();i++)
-		{
-			CSV_Reader reader(files_names.at(i));
-			int i_ext_dot = files_names.at(i).find('.');
-			int i_last_folder = files_names.at(i).find_last_of('/')+1;
-			std::string country_code = files_names.at(i).substr(i_last_folder, i_ext_dot - i_last_folder);
-
-			std::cout << "Reading Country Codes file: " << country_code << std::endl;
-
-			std::vector<CSV_Reader::LINE_DATA> country_data;
-			reader.ReadAllData(country_data);
-
-			country_signal_codes_.push_back(std::make_pair(country_code, country_data));
-		}
-	}
-	else
-	{
-		std::cout << "No Signs will be recognized, no Country Code files at: " << codes_csv_folder << std::endl;
-	}
-}
-
-void OpenDriveLoader::loadOpenDRIVE(const std::string& xodr_file, const std::string& codes_folder, PlannerHNS::RoadNetwork& map, double resolution, bool keep_right)
-{
-	keep_right_ = keep_right;
-	
-	std::ifstream f(xodr_file.c_str());
-	if(!f.good())
-	{
-		std::cout << "Can't Open OpenDRIVE Map File: (" << xodr_file << ")" << std::endl;
-		return;
-	}
-
-	std::cout << " >> Loading OpenDRIVE Map file ... " << std::endl;
-
-	TiXmlDocument doc(xodr_file);
-	try
-	{
-		doc.LoadFile();
-	}
-	catch(std::exception& e)
-	{
-		std::cout << "OpenDRIVE Custom Reader Error, Can't Load .xodr File, path is: "<<  xodr_file << std::endl;
-		std::cout << e.what() << std::endl;
-		return;
-	}
-
-	loadCountryCods(codes_folder);
-
-	std::cout << " >> Reading Header Data from OpenDRIVE map file ... " << std::endl;
-	std::vector<TiXmlElement*> elements;
-	XmlHelpers::findFirstElement("header", doc.FirstChildElement(), elements);
-
-	std::cout << "Final Results, Num:" << elements.size() <<std::endl;
-
-	if(elements.size() > 0)
-	{
-		OpenDriveHeader header(elements.at(0));
-		std::cout << "Final Results, Num:" << elements.size() << ", main element: " <<  elements.at(0)->Value() << std::endl;
-	}
-
-	std::cout << " >> Reading Data from OpenDRIVE map file ... " << std::endl;
-	elements.clear();
-	XmlHelpers::findElements("road", doc.FirstChildElement(), elements);
-	std::cout << "Final Results Roads, Num:" << elements.size() << std::endl;
-
-	roads_list_.clear();
-	for(unsigned int i=0; i < elements.size(); i++)
-	{
-		roads_list_.push_back(OpenDriveRoad(elements.at(i), &country_signal_codes_, keep_right_));
-	}
-
-
-	elements.clear();
-	XmlHelpers::findElements("junction", doc.FirstChildElement(), elements);
-
-	std::cout << "Final Results Junctions, Num:" << elements.size() << std::endl;
-
-	junctions_list_.clear();
-	for(unsigned int i=0; i < elements.size(); i++)
-	{
-		junctions_list_.push_back(Junction(elements.at(i)));
-	}
-
-	//Connect Roads
-	connectRoads();
-	
-	// for(unsigned int i=0; i < roads_list_.size(); i++)
-	// {
-	// 	std::cout << "Road ID: " << roads_list_.at(i).id_ << std::endl;
-	// 	std::cout << "From: ";
-	// 	for(unsigned int j=0; j < roads_list_.at(i).from_roads_.size(); j++)
-	// 	{
-	// 		std::cout << "("  << roads_list_.at(i).from_roads_.at(j).incoming_road_ << "|";
-	// 		for(unsigned int k=0; k < roads_list_.at(i).from_roads_.at(j).lane_links.size(); k++)
-	// 		{
-	// 			std::cout << roads_list_.at(i).from_roads_.at(j).lane_links.at(k).first << ", " << roads_list_.at(i).from_roads_.at(j).lane_links.at(k).second << " ; ";
-	// 		}
-	// 		std::cout << ")";
-	// 	}
-	// 
-	// 	std::cout << std::endl;
-	// 	std::cout << "To : " ;
-	// 
-	// 	for(unsigned int j=0; j < roads_list_.at(i).to_roads_.size(); j++)
-	// 	{
-	// 		std::cout << "("  << roads_list_.at(i).to_roads_.at(j).outgoing_road_ <<"|";
-	// 		for(unsigned int k=0; k < roads_list_.at(i).to_roads_.at(j).lane_links.size(); k++)
-	// 		{
-	// 			std::cout << roads_list_.at(i).to_roads_.at(j).lane_links.at(k).first << ", " << roads_list_.at(i).to_roads_.at(j).lane_links.at(k).second << " ; ";
-	// 		}
-	// 		std::cout << ")";
-	// 	}
-	// 
-	// 	std::cout << std::endl <<std::endl;
-	// }
-
-	std::cout << "Finish Linking Road Network .. " << std::endl;
-
-	PlannerHNS::RoadSegment segment;
-	segment.id = 1;
-	map.roadSegments.push_back(segment);
-	getMapLanes(map.roadSegments.at(0).Lanes, resolution);
-	std::cout << "Finish Extracting Lanes: " << map.roadSegments.at(0).Lanes.size() << std::endl;
-
-	getTrafficLights(map.trafficLights);
-	getTrafficSigns(map.signs);
-	getStopLines(map.stopLines);
-
-	std::cout << "Finish Extracting Traffic Objects ... " << std::endl;
-
-	linkWayPoints(map);
-
-	std::cout << "Finish Linking Way Points ... " << std::endl;
-
-}
-
-std::vector<OpenDriveRoad*> OpenDriveLoader::getRoadsBySuccId(int _id)
-{
-	std::vector<OpenDriveRoad*> _ret_list;
-	for(unsigned int i=0; i < roads_list_.size(); i++)
-	{
-		for(unsigned int j=0; j < roads_list_.at(i).successor_road_.size(); j++)
-		{
-			if(roads_list_.at(i).successor_road_.at(j).to_road_id_ == _id)
-			{
-				_ret_list.push_back(&roads_list_.at(i));
-			}
-		}
-	}
-
-	return _ret_list;
-}
-
-std::vector<OpenDriveRoad*> OpenDriveLoader::getRoadsByPredId(int _id)
-{
-	std::vector<OpenDriveRoad*> _ret_list;
-	for(unsigned int i=0; i < roads_list_.size(); i++)
-	{
-		for(unsigned int j=0; j < roads_list_.at(i).predecessor_road_.size(); j++)
-		{
-			if(roads_list_.at(i).predecessor_road_.at(j).from_road_id_ == _id)
-			{
-				_ret_list.push_back(&roads_list_.at(i));
-			}
-		}
-	}
-
-	return _ret_list;
-}
-
-OpenDriveRoad* OpenDriveLoader::getRoadById(int _id)
-{
-	for(unsigned int i=0; i < roads_list_.size(); i++)
-	{
-		if(roads_list_.at(i).id_ == _id)
-		{
-			return &roads_list_.at(i);
-		}
-	}
-
-	return nullptr;
-}
-
-Junction* OpenDriveLoader::getJunctionById(int _id)
-{
-	for(unsigned int i=0; i < junctions_list_.size(); i++)
-	{
-		if(junctions_list_.at(i).id_ == _id)
-		{
-			return &junctions_list_.at(i);
-		}
-	}
-
-	return nullptr;
-}
-
-void OpenDriveLoader::connectRoads()
-{
-	for(unsigned int i=0; i < roads_list_.size(); i++)
-	{
-		if(roads_list_.at(i).predecessor_road_.size() > 0)
-		{
-			//connect normal roads , junctions will be handeled alone
-			if(roads_list_.at(i).predecessor_road_.at(0).link_type_ == ROAD_LINK)
-			{
-				OpenDriveRoad* p_pre_road = getRoadById(roads_list_.at(i).predecessor_road_.at(0).from_road_id_);
-				if(p_pre_road != nullptr)
-				{
-					std::vector<Connection> pre_conn_list = roads_list_.at(i).getFirstSectionConnections(p_pre_road);
-					for(unsigned k=0; k < pre_conn_list.size(); k++)
-					{
-						if( !keep_right_)
-						{
-							pre_conn_list.at(k).flip();
-						}
-						if(pre_conn_list.at(k).outgoing_road_ == roads_list_.at(i).id_)
-						{
-							roads_list_.at(i).insertUniqueFromConnection(pre_conn_list.at(k));							
-						}
-						else if (pre_conn_list.at(k).incoming_road_ == roads_list_.at(i).id_)
-						{
-							roads_list_.at(i).insertUniqueToConnection(pre_conn_list.at(k));
-						}
-					}
-				}
-			}
-		}
-
-		if(roads_list_.at(i).successor_road_.size() > 0)
-		{
-			//connect normal roads , junctions will be handeled alone
-			if(roads_list_.at(i).successor_road_.at(0).link_type_ == ROAD_LINK)
-			{
-				OpenDriveRoad* p_suc_road = getRoadById(roads_list_.at(i).successor_road_.at(0).to_road_id_);
-				if(p_suc_road != nullptr)
-				{
-					std::vector<Connection> suc_conn_list = roads_list_.at(i).getLastSectionConnections(p_suc_road);
-					for(unsigned k=0; k < suc_conn_list.size(); k++)
-					{
-						if(!keep_right_)
-						{
-							suc_conn_list.at(k).flip();
-						}
-						if(suc_conn_list.at(k).outgoing_road_ == roads_list_.at(i).id_)
-						{
-							roads_list_.at(i).insertUniqueFromConnection(suc_conn_list.at(k));							
-						}
-						else if (suc_conn_list.at(k).incoming_road_ == roads_list_.at(i).id_)
-						{
-							roads_list_.at(i).insertUniqueToConnection(suc_conn_list.at(k));
-						}
-					}
-				}
-			}
-		}
-	}
-
-	//Link Junctions
-	for(unsigned int i=0; i < roads_list_.size(); i++)
-	{
-		if(roads_list_.at(i).predecessor_road_.size() > 0)
-		{
-			//connect normal roads , junctions will be handeled alone
-			if(roads_list_.at(i).predecessor_road_.at(0).link_type_ == JUNCTION_LINK)
-			{
-				Junction* p_junction = getJunctionById(roads_list_.at(i).predecessor_road_.at(0).from_road_id_);
-				if(p_junction != nullptr)
-				{
-					for( const auto junction_connection : p_junction->getConnectionsByRoadId(roads_list_.at(i).id_))
-					{
-						OpenDriveRoad* incoming_road = getRoadById(junction_connection.outgoing_road_);
-						if( incoming_road == nullptr) continue;
-	
-						RoadSection *outgoing_section = roads_list_.at(i).getLastSection();
-						RoadSection *incoming_section = nullptr;
-						if(junction_connection.contact_point_ == "end")
-							incoming_section = incoming_road->getLastSection();
-						else
-							incoming_section = incoming_road->getFirstSection();
-	
-						Connection connection;
-						connection.incoming_road_ = junction_connection.outgoing_road_;
-						connection.outgoing_road_ = roads_list_.at(i).id_;
-						if(incoming_section != nullptr)
-							connection.incoming_section_ = incoming_section->id_;
-						if(outgoing_section != nullptr)
-							connection.outgoing_section_ = roads_list_.at(i).getLastSection()->id_;
-						connection.lane_links = junction_connection.lane_links;
-	
-						if( !connection.lane_links.empty() )
-						{
-							//flip appropriate lane depending on keep_right flag
-							if( (keep_right_ && connection.lane_links.at(0).first > 0 ) || (!keep_right_ && connection.lane_links.at(0).first < 0))
-							{
-								connection.flipRoad();							
-								roads_list_.at(i).insertUniqueToConnection(connection);															
-							} 
-							else
-							{
-								roads_list_.at(i).insertUniqueFromConnection(connection);							
-							}
-						}
-					}
-				}
-			}
-		}
-	
-		if(roads_list_.at(i).successor_road_.size() > 0)
-		{
-			//connect normal roads , junctions will be handeled alone
-			if(roads_list_.at(i).successor_road_.at(0).link_type_ == JUNCTION_LINK)
-			{
-				Junction* p_junction = getJunctionById(roads_list_.at(i).successor_road_.at(0).to_road_id_);
-				if(p_junction != nullptr)
-				{
-					for( const auto junction_connection : p_junction->getConnectionsByRoadId(roads_list_.at(i).id_))
-					{
-						OpenDriveRoad* outgoing_road = getRoadById(junction_connection.outgoing_road_);
-						if( outgoing_road == nullptr) continue;
-	
-						RoadSection *incoming_section = roads_list_.at(i).getLastSection();
-						RoadSection *outgoing_section = nullptr;
-						if(junction_connection.contact_point_ == "end")
-							outgoing_section = outgoing_road->getLastSection();
-						else
-							outgoing_section = outgoing_road->getFirstSection();
-						if(incoming_section == nullptr || outgoing_section == nullptr) continue;
-	
-						Connection connection;
-						connection.incoming_road_ = roads_list_.at(i).id_;
-						connection.outgoing_road_ = junction_connection.outgoing_road_;
-						connection.incoming_section_ = incoming_section->id_;
-						connection.outgoing_section_ = outgoing_section->id_;
-						connection.lane_links = junction_connection.lane_links;
-	
-						//flip appropriate lane depending on keep_right flag
-						if( !connection.lane_links.empty() )
-						{
-							if( (keep_right_ && connection.lane_links.at(0).first > 0) || (!keep_right_ && connection.lane_links.at(0).first < 0) )
-							{
-								connection.flipRoad();
-								roads_list_.at(i).insertUniqueFromConnection(connection);															
-							}
-							else
-							{
-								roads_list_.at(i).insertUniqueToConnection(connection);							
-							}
-						}
-					}
-				}
-			}
-		}
-	}
-
-	// for(unsigned int i=0; i < junctions_list_.size(); i++)
-	// {
-	// 	for(unsigned int j=0; j < junctions_list_.at(i).connections_.size(); j++)
-	// 	{
-	// 		//std::cout << "J_ID: " << junctions_list_.at(i).id_ << ", (" << junctions_list_.at(i).connections_.at(j).incoming_road_ << ", " << junctions_list_.at(i).connections_.at(j).outgoing_road_ << " )" <<std::endl;
-	// 		OpenDriveRoad* p_from_road = getRoadById(junctions_list_.at(i).connections_.at(j).incoming_road_);
-	// 		if(p_from_road != nullptr)
-	// 		{
-	// 			p_from_road->insertUniqueToConnection(junctions_list_.at(i).connections_.at(j));
-	// 		}
-	// 
-	// 		OpenDriveRoad* p_to_road = getRoadById(junctions_list_.at(i).connections_.at(j).outgoing_road_);
-	// 		if(p_to_road != nullptr)
-	// 		{
-	// 			p_to_road->insertUniqueFromConnection(junctions_list_.at(i).connections_.at(j));
-	// 		}
-	// 	}
-	// }
-	// 
-	// //Link Missing successors that are linked to junctions
-	// for(unsigned int i=0; i < roads_list_.size(); i++)
-	// {
-	// 	if(roads_list_.at(i).predecessor_road_.size() > 0)
-	// 	{
-	// 		if(roads_list_.at(i).predecessor_road_.at(0).link_type_ != ROAD_LINK)
-	// 		{
-	// 			std::vector<OpenDriveRoad*> pred_list = getRoadsBySuccId(roads_list_.at(i).id_);
-	// 			for(unsigned int j=0; j < pred_list.size(); j++)
-	// 			{
-	// 				for(unsigned int k=0; k < pred_list.at(j)->to_roads_.size(); k++)
-	// 				{
-	// 					if(pred_list.at(j)->to_roads_.at(k).outgoing_road_ == roads_list_.at(i).id_)
-	// 					{
-	// 						roads_list_.at(i).insertUniqueFromConnection(pred_list.at(j)->to_roads_.at(k));
-	// 					}
-	// 				}
-	// 			}
-	// 		}
-	// 	}
-	// }
-}
-
-void OpenDriveLoader::getMapLanes(std::vector<PlannerHNS::Lane>& all_lanes, double resolution)
-{
-	for(unsigned int i=0; i < roads_list_.size(); i++)
-	{
-		roads_list_.at(i).getRoadLanes(all_lanes, resolution);
-	}
-}
-
-void OpenDriveLoader::getTrafficLights(std::vector<PlannerHNS::TrafficLight>& all_lights)
-{
-	for(unsigned int i=0; i < roads_list_.size(); i++)
-	{
-		roads_list_.at(i).getTrafficLights(all_lights);
-	}
-}
-
-void OpenDriveLoader::getTrafficSigns(std::vector<PlannerHNS::TrafficSign>& all_signs)
-{
-	for(unsigned int i=0; i < roads_list_.size(); i++)
-	{
-		roads_list_.at(i).getTrafficSigns(all_signs);
-	}
-}
-
-void OpenDriveLoader::getStopLines(std::vector<PlannerHNS::StopLine>& all_stop_lines)
-{
-	for(unsigned int i=0; i < roads_list_.size(); i++)
-	{
-		roads_list_.at(i).getStopLines(all_stop_lines);
-	}
-}
-
-void OpenDriveLoader::linkWayPoints(PlannerHNS::RoadNetwork& map)
-{
-	linkLanesPointers(map);
-
-	for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
-	{
-		for(unsigned int i = 0; i < map.roadSegments.at(rs).Lanes.size(); i++)
-		{
-			PlannerHNS::Lane* pL = &map.roadSegments.at(rs).Lanes.at(i);
-			for(unsigned int iwp= 0; iwp < pL->points.size(); iwp++)
-			{
-				if(iwp < pL->points.size()-1)
-				{
-					pL->points.at(iwp).toIds.push_back(pL->points.at(iwp+1).id);
-				}
-				else
-				{
-					for(unsigned int k=0; k< pL->toLanes.size(); k++)
-					{
-						if(pL->toLanes.at(k) != nullptr && pL->toLanes.at(k)->points.size()>0)
-						{
-							pL->points.at(iwp).toIds.push_back(pL->toLanes.at(k)->points.at(0).id);
-						}
-					}
-				}
-			}
-		}
-	}
-}
-
-void OpenDriveLoader::linkLanesPointers(PlannerHNS::RoadNetwork& map)
-{
-	for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
-	{
-		//Link Lanes
-		for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
-		{
-			PlannerHNS::Lane* pL = &map.roadSegments.at(rs).Lanes.at(i);
-			for(unsigned int j = 0 ; j < pL->fromIds.size(); j++)
-			{
-				for(unsigned int l= 0; l < map.roadSegments.at(rs).Lanes.size(); l++)
-				{
-					if(map.roadSegments.at(rs).Lanes.at(l).id == pL->fromIds.at(j))
-					{
-						pL->fromLanes.push_back(&map.roadSegments.at(rs).Lanes.at(l));
-					}
-				}
-			}
-
-			for(unsigned int j = 0 ; j < pL->toIds.size(); j++)
-			{
-				for(unsigned int l= 0; l < map.roadSegments.at(rs).Lanes.size(); l++)
-				{
-					if(map.roadSegments.at(rs).Lanes.at(l).id == pL->toIds.at(j))
-					{
-						pL->toLanes.push_back(&map.roadSegments.at(rs).Lanes.at(l));
-					}
-				}
-			}
-
-			for(unsigned int j = 0 ; j < pL->points.size(); j++)
-			{
-				pL->points.at(j).pLane  = pL;
-			}
-		}
-	}
-}
-
-}

src/ros/catkin/src/vector_map_converter/src/opendrive2autowaremap_core/opendrive_objects.cpp

@@ -1,28 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-/*
- * opendrive_objects.cpp
- *
- *  Created on: Feb 11, 2019
- *      Author: hatem
- */
-
-#include "opendrive2autoware_converter/opendrive_objects.h"
-#include <fstream>
-
-namespace opendrive_converter
-{
-}

src/ros/catkin/src/vector_map_converter/src/opendrive2autowaremap_core/opendrive_road.cpp

@@ -1,1186 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-/*
- * opendrive_road.cpp
- *
- *  Created on: Feb 11, 2019
- *      Author: hatem
- */
-
-#include "opendrive2autoware_converter/opendrive_road.h"
-#include <fstream>
-#include <functional>
-namespace opendrive_converter
-{
-
-double g_epsilon = 0.0000001;
-
-OpenDriveRoad::OpenDriveRoad(TiXmlElement* main_element, std::vector<std::pair<std::string, std::vector<CSV_Reader::LINE_DATA> > >* country_signal_codes, bool keep_right)
-{
-	name_ = XmlHelpers::getStringAttribute(main_element, "name", "");
-	id_ = XmlHelpers::getIntAttribute(main_element, "id", 0);
-	junction_id_ = XmlHelpers::getIntAttribute(main_element, "junction", -1);
-	length_ = XmlHelpers::getDoubleAttribute(main_element, "length", 0.0);
-	p_country_signal_codes_ = country_signal_codes;
-	keep_right_ = keep_right;
-
-	//Read Links
-	std::vector<TiXmlElement*> sub_elements;
-	std::vector<TiXmlElement*> lane_elements;
-	std::vector<TiXmlElement*> elements;
-
-	XmlHelpers::findFirstElement("link", main_element, elements);
-	if(elements.size() > 0)
-	{
-		std::vector<TiXmlElement*> pred_elements, succ_elements;
-
-		XmlHelpers::findElements("predecessor", elements.at(0)->FirstChildElement(), pred_elements);
-		for(unsigned int j=0; j < pred_elements.size(); j++)
-		{
-			predecessor_road_.push_back(FromRoadLink(pred_elements.at(j)));
-		}
-
-		XmlHelpers::findElements("successor", elements.at(0)->FirstChildElement(), succ_elements);
-		for(unsigned int j=0; j < succ_elements.size(); j++)
-		{
-			successor_road_.push_back(ToRoadLink(succ_elements.at(j)));
-		}
-	}
-
-	elements.clear();
-	XmlHelpers::findFirstElement("planView", main_element, elements);
-	if(elements.size() > 0)
-	{
-		//Get Geometries
-		std::vector<TiXmlElement*> geom_elements;
-		XmlHelpers::findElements("geometry", elements.at(0), geom_elements);
-		for(unsigned int j=0; j < geom_elements.size(); j++)
-		{
-			geometries_.push_back(Geometry(geom_elements.at(j)));
-		}
-	}
-
-	elements.clear();
-	XmlHelpers::findFirstElement("elevationProfile", main_element, elements);
-	if(elements.size() > 0)
-	{
-		//Get Geometries
-		std::vector<TiXmlElement*> elev_elements;
-		XmlHelpers::findElements("elevation", elements.at(0), elev_elements);
-		for(unsigned int j=0; j < elev_elements.size(); j++)
-		{
-			elevations_.push_back(Elevation(elev_elements.at(j)));
-		}
-	}
-
-	int lanes_index_count = 1;
-	elements.clear();
-	XmlHelpers::findFirstElement("lanes", main_element, elements);
-	if(elements.size()>0)
-	{
-		//laneOffsets
-		std::vector<TiXmlElement*> offsets;
-		XmlHelpers::findElements("laneOffsets", elements.at(0), offsets);
-		for(unsigned int j=0; j < offsets.size(); j++)
-		{
-			laneOffsets_.push_back(LaneOffset(offsets.at(j)));
-		}
-
-		//std::cout << "LaneOffsets Loaded with: " << laneOffsets_.size() <<" offsets."<<std::endl;
-
-		//laneSections and lanes
-		std::vector<TiXmlElement*> sections;
-		XmlHelpers::findElements("laneSection", elements.at(0), sections);
-
-		for(unsigned int j=0; j < sections.size(); j++)
-		{
-			double curr_section_s = XmlHelpers::getDoubleAttribute(sections.at(j), "s", 0.0);
-			double next_section_s = 0.0;
-			double section_length = 0.0;
-
-			if(j+1 < sections.size())
-			{
-				next_section_s = XmlHelpers::getDoubleAttribute(sections.at(j+1), "s", 0.0);
-				section_length = next_section_s - curr_section_s;
-			}
-			else
-			{
-				section_length = length_ - curr_section_s;
-			}
-
-			if(section_length <= 0)
-			{
-				std::cout << "Load From OpenDrive with Section length is 0.0 !!" << std::endl;
-				continue;
-			}
-
-			sections_.push_back(RoadSection(sections.at(j),curr_section_s, section_length, j));
-
-			if(j > 0)
-			{
-				sections_.at(sections_.size()-1).p_prev_section_ = &sections_.at(sections_.size()-2);
-				sections_.at(sections_.size()-2).p_next_section_ = &sections_.at(sections_.size()-1);
-			}
-		}
-
-		//std::cout << "LaneSections Loaded with: " << sections.size() <<" Sections , And: " << lanes_.size() << " Lanes" <<std::endl;
-	}
-
-	elements.clear();
-	XmlHelpers::findFirstElement("signals", main_element, elements);
-	if(elements.size()>0)
-	{
-		sub_elements.clear();
-		XmlHelpers::findElements("signal", main_element, sub_elements);
-		for(unsigned int j=0; j < sub_elements.size(); j++)
-		{
-			road_signals_.push_back(Signal(sub_elements.at(j)));
-		}
-
-		sub_elements.clear();
-		XmlHelpers::findElements("signalReference", main_element, sub_elements);
-		for(unsigned int j=0; j < sub_elements.size(); j++)
-		{
-			road_signals_references_.push_back(SignalRef(sub_elements.at(j)));
-		}
-	}
-
-	elements.clear();
-	XmlHelpers::findFirstElement("objects", main_element, elements);
-	if(elements.size()>0)
-	{
-		sub_elements.clear();
-		XmlHelpers::findElements("object", main_element, sub_elements);
-		for(unsigned int j=0; j < sub_elements.size(); j++)
-		{
-			road_objects_.push_back(RoadObject(sub_elements.at(j)));
-		}
-
-		sub_elements.clear();
-		XmlHelpers::findElements("objectReference", main_element, sub_elements);
-		for(unsigned int j=0; j < sub_elements.size(); j++)
-		{
-			road_objects_references_.push_back(RoadObjectRef(sub_elements.at(j)));
-		}
-
-		sub_elements.clear();
-		XmlHelpers::findElements("tunnel", main_element, sub_elements);
-		for(unsigned int j=0; j < sub_elements.size(); j++)
-		{
-			road_objects_tunnels_.push_back(RoadObjectTunnel(sub_elements.at(j)));
-		}
-
-		sub_elements.clear();
-		XmlHelpers::findElements("object", main_element, sub_elements);
-		for(unsigned int j=0; j < sub_elements.size(); j++)
-		{
-			road_objects_bridges_.push_back(RoadObjectBridge(sub_elements.at(j)));
-		}
-	}
-
-	//std::cout << "Road Loaded With ID: " << id_ << " , Length: " << length_  << std::endl;
-}
-
-bool OpenDriveRoad::createSingleCenterPoint(double _ds, PlannerHNS::WayPoint& _p)
-{
-	for(unsigned int i=0; i < geometries_.size(); i++)
-	{
-		if(_ds >= geometries_.at(i).s && _ds <= (geometries_.at(i).s+geometries_.at(i).length))
-		{
-			if(geometries_.at(i).getPoint(_ds, _p))
-			{
-				Elevation* p_elv = getMatchingElevations(_ds);
-				if(p_elv != nullptr)
-				{
-					_p.pos.z = p_elv->getHeigh(_ds);
-				}
-				return true;
-			}
-		}
-	}
-
-	return false;
-}
-
-bool OpenDriveRoad::createRoadCenterPoint(RoadCenterInfo& inf_point, double _s)
-{
-	for(unsigned int i=0; i < geometries_.size(); i++)
-	{
-		double end_distance = geometries_.at(i).s + geometries_.at(i).length;
-
-		if(_s >= geometries_.at(i).s && _s <= end_distance)
-		{
-			RoadCenterInfo inf;
-			PlannerHNS::WayPoint p;
-			if(geometries_.at(i).getPoint(_s, p))
-			{
-				Elevation* p_elv = getMatchingElevations(_s);
-				if(p_elv != nullptr)
-				{
-					p.pos.z = p_elv->getHeigh(_s);
-				}
-
-				LaneOffset* p_lane_off = getMatchingLaneOffset(_s);
-				if(p_lane_off != nullptr)
-				{
-					inf.offset_width_ = p_lane_off->getOffset(_s);
-				}
-
-				inf.ds_ = _s;
-				inf.center_p_ = p.pos;
-				inf_point = inf;
-				return true;
-			}
-		}
-	}
-
-	return false;
-}
-
-void OpenDriveRoad::insertRoadCenterInfo(std::vector<RoadCenterInfo>& points_list, RoadCenterInfo& inf_point)
-{
-	for(unsigned int i = 0; i < points_list.size(); i++)
-	{
-		if(inf_point.ds_ == points_list.at(i).ds_) // exist
-		{
-			return;
-		}
-		else if(inf_point.ds_  < points_list.at(i).ds_)
-		{
-			points_list.insert(points_list.begin()+i, inf_point);
-			return;
-		}
-	}
-
-	points_list.push_back(inf_point);
-}
-
-void OpenDriveRoad::createRoadCenterInfo(std::vector<RoadCenterInfo>& points_list, double resolution)
-{
-	PlannerHNS::WayPoint p;
-	RoadCenterInfo inf;
-	points_list.clear();
-	for(unsigned int i=0; i < geometries_.size(); i++)
-	{
-		int n_waypoints = floor(geometries_.at(i).length / resolution) + 1;
-
-//		std::cout << "### WayPoints are: " << n_waypoints << ", for Road Geometry : (" << id_ << "," << i << "), With Res: " << resolution << ", and Length: " << geometries_.at(i).length << std::endl;
-
-		double s_inc = geometries_.at(i).s;
-		double remaining_distance = 0.0;
-		double end_distance = geometries_.at(i).s + geometries_.at(i).length;
-		bool bOneMorePoint = false;
-
-		for(int j=0; j< n_waypoints; j++)
-		{
-			if(geometries_.at(i).getPoint(s_inc, p))
-			{
-				Elevation* p_elv = getMatchingElevations(s_inc);
-				if(p_elv != nullptr)
-				{
-					p.pos.z = p_elv->getHeigh(s_inc);
-				}
-
-				LaneOffset* p_lane_off = getMatchingLaneOffset(s_inc);
-				if(p_lane_off != nullptr)
-				{
-					inf.offset_width_ = p_lane_off->getOffset(s_inc);
-				}
-
-				inf.ds_ = s_inc;
-				inf.center_p_ = p.pos;
-				points_list.push_back(inf);
-			}
-
-			remaining_distance = end_distance - s_inc;
-
-			if(remaining_distance < resolution)
-			{
-				s_inc += remaining_distance;
-
-				if(remaining_distance > g_epsilon)
-				{
-					bOneMorePoint = true;
-				}
-				else if(remaining_distance != 0)
-				{
-					std::cout << "$$$ Too Small Geometry !: " << "(" << id_ << "," << i << ", " << j << "), With Res: " << resolution << ", and Length: " << geometries_.at(i).length << ", " << remaining_distance << std::endl;
-				}
-			}
-			else
-				s_inc += resolution;
-		}
-
-
-		if(bOneMorePoint == true)
-		{
-			if(geometries_.at(i).getPoint(s_inc, p))
-			{
-				Elevation* p_elv = getMatchingElevations(s_inc);
-				if(p_elv != nullptr)
-				{
-					p.pos.z = p_elv->getHeigh(s_inc);
-				}
-
-				LaneOffset* p_lane_off = getMatchingLaneOffset(s_inc);
-				if(p_lane_off != nullptr)
-				{
-					inf.offset_width_ = p_lane_off->getOffset(s_inc);
-				}
-
-				inf.ds_ = s_inc;
-				inf.center_p_ = p.pos;
-				points_list.push_back(inf);
-			}
-		}
-	}
-
-	for(unsigned int i=0; i< sections_.size(); i++)
-	{
-		double s_inc = sections_.at(i).s_;
-		if(createRoadCenterPoint(inf, s_inc))
-		{
-			insertRoadCenterInfo(points_list, inf);
-		}
-		else
-		{
-			std::cout << ">>>> Can't Find Geometry for Start of Section: " << i << ", Road: " << id_ << std::endl;
-		}
-
-		s_inc = sections_.at(i).s_ + sections_.at(i).length_;
-
-		if(createRoadCenterPoint(inf, s_inc))
-		{
-			insertRoadCenterInfo(points_list, inf);
-		}
-		else
-		{
-			std::cout << ">>>> Can't Find Geometry for Start of Section: " << i << ", Road: " << id_ << std::endl;
-		}
-	}
-}
-
-void OpenDriveRoad::insertUniqueFromSectionIds(int from_section_id, const OpenDriveLane* curr_lane, PlannerHNS::Lane& _l)
-{
-	for(unsigned int i=0; i < curr_lane->from_lane_.size(); i++)
-	{
-		int from_lane_id = curr_lane->from_lane_.at(i).from_lane_id;
-		int from_gen_id = (id_*100000 + 50000) + from_section_id * 1000 + from_lane_id * 100;
-
-		if(exists(_l.fromIds, from_gen_id))
-		{
-			std::cout << "Redundant Connection, from road: " << id_ <<", to road: " << id_
-						<< ", to section: " << from_section_id+1 << ", GenLaneID: " << from_gen_id << std::endl;
-		}
-		else
-		{
-			_l.fromIds.push_back(from_gen_id);
-		}
-	}
-}
-
-void OpenDriveRoad::insertUniqueToSectionIds(int to_section_id, const OpenDriveLane* curr_lane, PlannerHNS::Lane& _l)
-{
-	for(unsigned int i=0; i < curr_lane->to_lane_.size(); i++)
-	{
-		int to_lane_id = curr_lane->to_lane_.at(i).to_lane_id;
-		int to_gen_id = (id_*100000 + 50000) + to_section_id *1000 + to_lane_id * 100;
-
-		if(exists(_l.toIds, to_gen_id))
-		{
-			std::cout << "Redundant Connection, from road: " << id_ <<", to road: " << id_
-						<< ", from section: " << to_section_id - 1 << ", GenLaneID: " << to_gen_id << std::endl;
-		}
-		else
-		{
-			_l.toIds.push_back(to_gen_id);
-		}
-	}
-}
-
-void OpenDriveRoad::insertUniqueFromRoadIds(int curr_section_id, int curr_lane_id, PlannerHNS::Lane& _l)
-{
-	for(unsigned int i=0; i < from_roads_.size(); i++)
-	{
-		int from_lane_id = from_roads_.at(i).getFromLane(curr_lane_id);
-		if(from_lane_id != 0)
-		{
-			if(from_roads_.at(i).outgoing_road_ != id_)
-				std::cout << " >>>  Something Very Bad Happened in InsertUniqueFromRoadIds, outgoing_road doesn't match current_road, " << from_roads_.at(i).outgoing_road_ << ", " << id_ << std::endl;
-
-			int from_gen_id = (from_roads_.at(i).incoming_road_*100000 + 50000) + from_roads_.at(i).incoming_section_*1000 + from_lane_id * 100;
-			if(exists(_l.fromIds, from_gen_id))
-			{
-				std::cout << "Redundant Connection, from road: " << from_roads_.at(i).incoming_road_ <<", to road: " << id_
-							<< ", to section: " << curr_section_id << ", GenLaneID: " << from_gen_id << std::endl;
-			}
-			else
-			{
-				_l.fromIds.push_back(from_gen_id);
-			}
-		}
-	}
-}
-
-void OpenDriveRoad::insertUniqueToRoadIds(int curr_section_id, int curr_lane_id, PlannerHNS::Lane& _l)
-{
-	for(unsigned int i=0; i < to_roads_.size(); i++)
-	{
-		int to_lane_id = to_roads_.at(i).getToLane(curr_lane_id);
-		if(to_lane_id != 0)
-		{
-			if(to_roads_.at(i).incoming_road_ != id_)
-				std::cout << " >>>  Something Very Bad Happened in InsertUniqueToRoadIds, incoming_road doesn't match current_road, " << to_roads_.at(i).incoming_road_ << ", " << id_ << std::endl;
-
-			int to_gen_id = (to_roads_.at(i).outgoing_road_*100000 + 50000) + to_roads_.at(i).outgoing_section_*1000 + to_lane_id * 100;
-			if(exists(_l.toIds, to_gen_id))
-			{
-				std::cout << "Redundant Connection, to road: " << to_roads_.at(i).outgoing_road_ <<", from road: " << id_
-							<< ", from section: " << curr_section_id << ", GenLaneID: " << to_gen_id << std::endl;
-			}
-			else
-			{
-				_l.toIds.push_back(to_gen_id);
-			}
-		}
-	}
-}
-
-void OpenDriveRoad::insertUniqueToConnection(const Connection& _connection)
-{
-
-	bool bFound = false;
-	for(unsigned int j=0; j < to_roads_.size(); j++)
-	{
-		if(to_roads_.at(j).incoming_road_== _connection.incoming_road_ && to_roads_.at(j).outgoing_road_ == _connection.outgoing_road_)
-		{
-			for(unsigned int k=0; k < to_roads_.at(j).lane_links.size(); k++)
-			{
-				for(unsigned int lk=0; lk < _connection.lane_links.size(); lk++)
-				{
-					if(to_roads_.at(j).lane_links.at(k).first == _connection.lane_links.at(lk).first && to_roads_.at(j).lane_links.at(k).second == _connection.lane_links.at(lk).second)
-					{
-						bFound = true;
-						break;
-					}
-				}
-
-				if(bFound == true)
-					break;
-			}
-
-			if(bFound == true)
-				break;
-		}
-	}
-
-	if(!bFound)
-	{
-		to_roads_.push_back(_connection);
-	}
-	else
-	{
-		//std::cout << "To Connection already exists, From : " << _connection.incoming_road_ << ", To: " << _connection.outgoing_road_ << std::endl;
-	}
-}
-
-void OpenDriveRoad::insertUniqueFromConnection(const Connection& _connection)
-{
-	bool bFound = false;
-	for(unsigned int j=0; j < from_roads_.size(); j++)
-	{
-		if(from_roads_.at(j).incoming_road_== _connection.incoming_road_ && from_roads_.at(j).outgoing_road_ == _connection.outgoing_road_)
-		{
-			for(unsigned int k=0; k < from_roads_.at(j).lane_links.size(); k++)
-			{
-				for(unsigned int lk=0; lk < _connection.lane_links.size(); lk++)
-				{
-					if(from_roads_.at(j).lane_links.at(k).first == _connection.lane_links.at(lk).first && from_roads_.at(j).lane_links.at(k).second == _connection.lane_links.at(lk).second)
-					{
-						bFound = true;
-						break;
-					}
-				}
-
-				if(bFound == true)
-					break;
-			}
-
-			if(bFound == true)
-				break;
-		}
-	}
-
-	if(!bFound)
-	{
-		from_roads_.push_back(_connection);
-	}
-	else
-	{
-		//std::cout << "From Connection already exists, From : " << _connection.incoming_road_ << ", To: " << _connection.outgoing_road_ << std::endl;
-	}
-}
-
-void OpenDriveRoad::createRoadLanes(std::vector<PlannerHNS::Lane>& lanes_list)
-{
-	using namespace std::placeholders;
-	std::function<void(int, int, PlannerHNS::Lane&)> insertToRoad;
-	std::function<void(int, int, PlannerHNS::Lane&)> insertFromRoad;
-	std::function<void(int, const OpenDriveLane*, PlannerHNS::Lane&)> insertToSection;
-	std::function<void(int, const OpenDriveLane*, PlannerHNS::Lane&)> insertFromSection;
-
-	//flip to and from depending on keep right or keep left rule
-	if(keep_right_)
-	{
-		insertToRoad = std::bind(&OpenDriveRoad::insertUniqueToRoadIds, this, _1, _2, _3);
-		insertFromRoad = std::bind(&OpenDriveRoad::insertUniqueFromRoadIds, this, _1, _2, _3);
-		insertToSection = std::bind(&OpenDriveRoad::insertUniqueToSectionIds, this, _1, _2, _3);
-		insertFromSection = std::bind(&OpenDriveRoad::insertUniqueFromSectionIds, this, _1, _2, _3);
-	}
-	else
-	{
-		insertToRoad = std::bind(&OpenDriveRoad::insertUniqueFromRoadIds, this, _1, _2, _3);
-		insertFromRoad = std::bind(&OpenDriveRoad::insertUniqueToRoadIds, this, _1, _2, _3);
-		insertToSection = std::bind(&OpenDriveRoad::insertUniqueFromSectionIds, this, _1, _2, _3);
-		insertFromSection = std::bind(&OpenDriveRoad::insertUniqueToSectionIds, this, _1, _2, _3);		
-	}
-	
-	for(unsigned int i=0; i < sections_.size(); i++)
-	{
-		RoadSection* p_sec = & sections_.at(i);
-
-		for(unsigned int lj=0; lj < p_sec->left_lanes_.size(); lj++)
-		{
-			PlannerHNS::Lane op_lane;
-			OpenDriveLane* p_l_l = &p_sec->left_lanes_.at(lj);
-
-//			if(p_l_l->type_ != DRIVING_LANE)
-//				continue;
-
-			op_lane.id = (this->id_*100000 + 50000) + p_sec->id_*1000 + p_l_l->id_ * 100;
-			op_lane.num = p_l_l->id_;
-			op_lane.roadId = id_;
-
-
-			if(i == 0){
-				insertToRoad(p_sec->id_, p_l_l->id_, op_lane);
-			}else{
-				insertToSection(p_sec->id_-1, p_l_l, op_lane);
-			}
-			if( i == sections_.size()-1){
-				insertFromRoad(p_sec->id_, p_l_l->id_, op_lane);
-			}else{
-				insertFromSection(p_sec->id_+1, p_l_l, op_lane);
-			}
-			lanes_list.push_back(op_lane);
-		}
-
-		for(unsigned int rj=0; rj < p_sec->right_lanes_.size(); rj++)
-		{
-			PlannerHNS::Lane op_lane;
-			OpenDriveLane* p_r_l = &p_sec->right_lanes_.at(rj);
-
-			op_lane.id = (this->id_*100000 + 50000) + p_sec->id_*1000 + p_r_l->id_ * 100;
-			op_lane.num = p_r_l->id_;
-			op_lane.roadId = id_;
-
-			if(i == 0){
-				insertFromRoad(p_sec->id_, p_r_l->id_, op_lane);
-			}else{
-				insertFromSection(p_sec->id_-1, p_r_l, op_lane);
-			}
-			if( i == sections_.size()-1){					
-				insertToRoad(p_sec->id_, p_r_l->id_, op_lane);
-			}else{
-				insertToSection(p_sec->id_+1, p_r_l, op_lane);
-			}
-			
-			lanes_list.push_back(op_lane);
-		}
-	}
-}
-
-void OpenDriveRoad::fixRedundantPointsLanes(PlannerHNS::Lane& _lane)
-{
-	for(int ip = 1; ip < _lane.points.size(); ip++)
-	{
-		PlannerHNS::WayPoint* p1 = &_lane.points.at(ip-1);
-		PlannerHNS::WayPoint* p2 = &_lane.points.at(ip);
-		PlannerHNS::WayPoint* p3 = nullptr;
-		if(ip+1 < _lane.points.size())
-			p3 = &_lane.points.at(ip+1);
-
-		double d = hypot(p2->pos.y-p1->pos.y, p2->pos.x-p1->pos.x);
-		if(d < g_epsilon)
-		{
-			p1->toIds = p2->toIds;
-			p1->originalMapID = p2->originalMapID;
-			if(p3 != nullptr)
-				p3->fromIds = p2->fromIds;
-
-			_lane.points.erase(_lane.points.begin()+ip);
-			ip--;
-
-//			std::cout << "Fixed Redundant Points for Lane:" << _lane.id << ", Current: " << ip << ", Size: " << _lane.points.size() << std::endl;
-		}
-	}
-}
-
-void OpenDriveRoad::createSectionPoints(const RoadCenterInfo& ref_info, std::vector<PlannerHNS::Lane>& lanes_list, RoadSection* p_sec, int& wp_id_seq, std::vector<int> &left_lane_ids,std::vector<int> &right_lane_ids )
-{
-	if(p_sec == nullptr) return;
-
-	double accum_offset_width = ref_info.offset_width_;
-
-	for(unsigned int lj=0; lj < p_sec->left_lanes_.size(); lj++)
-	{
-		OpenDriveLane* p_l_l = &p_sec->left_lanes_.at(lj);
-
-		int combined_lane_id = (this->id_*100000 + 50000) + p_sec->id_*1000 + p_l_l->id_ * 100;
-		PlannerHNS::Lane* p_op_lane = getLaneById(combined_lane_id, lanes_list);
-
-
-		if(p_op_lane != nullptr)
-		{
-			double lane_width = p_l_l->getLaneWidth(ref_info.ds_ - p_sec->s_);
-			double center_point_margin = accum_offset_width + (lane_width / 2.0);
-
-			PlannerHNS::WayPoint p;
-			p.pos = ref_info.center_p_;
-			double a = p.pos.a+M_PI_2;
-			p.pos.x += center_point_margin * cos(a);
-			p.pos.y += center_point_margin * sin(a);
-			p.collisionCost = lane_width;
-			p.id = wp_id_seq++;
-			//TODO apply super elevation later
-
-			p_op_lane->width = lane_width;
-			p_op_lane->points.push_back(p);
-
-			accum_offset_width += lane_width;
-			
-			if(!exists(left_lane_ids, combined_lane_id))
-			{
-				left_lane_ids.push_back(combined_lane_id);
-			}
-		}
-		else
-		{
-			std::cout << " >>>>> Can't Find Left Lane:  " << combined_lane_id << std::endl;
-		}
-	}
-
-	accum_offset_width = ref_info.offset_width_;
-
-	for(unsigned int rj=0; rj < p_sec->right_lanes_.size(); rj++)
-	{
-		OpenDriveLane* p_r_l = &p_sec->right_lanes_.at(rj);
-		int combined_lane_id = (this->id_*100000 + 50000) + p_sec->id_*1000 + p_r_l->id_ * 100;
-		PlannerHNS::Lane* p_op_lane = getLaneById(combined_lane_id, lanes_list);
-
-		if(p_op_lane != nullptr)
-		{
-			double lane_width = p_r_l->getLaneWidth(ref_info.ds_ - p_sec->s_);
-			double center_point_margin = accum_offset_width + (lane_width / 2.0);
-
-			PlannerHNS::WayPoint p;
-			p.pos = ref_info.center_p_;
-			double a = p.pos.a-M_PI_2;
-			p.pos.x += center_point_margin * cos(a);
-			p.pos.y += center_point_margin * sin(a);
-			p.collisionCost = lane_width;
-			p.id = wp_id_seq++;
-			//TODO apply super elevation later
-
-			p_op_lane->width = lane_width;
-			p_op_lane->points.push_back(p);
-
-			accum_offset_width += lane_width;
-			
-			if(!exists(right_lane_ids, combined_lane_id))
-			{
-				right_lane_ids.push_back(combined_lane_id);
-			}
-		}
-		else
-		{
-			std::cout << " >>>>> Can't Find Right Lane:  " << combined_lane_id << std::endl;
-		}
-	}
-}
-
-void OpenDriveRoad::getRoadLanes(std::vector<PlannerHNS::Lane>& lanes_list, double resolution)
-{
-	std::vector<RoadCenterInfo> ref_info;
-	createRoadCenterInfo(ref_info, resolution);
-	createRoadLanes(lanes_list);
-	static int wp_id_seq = 1;
-	std::vector<int> left_lane_ids, right_lane_ids;
-
-	for(unsigned int i=0; i < ref_info.size(); i++)
-	{
-		RoadSection* p_sec = getExactMatchingSection(ref_info.at(i).ds_);
-		if(p_sec != nullptr)
-		{
-			createSectionPoints(ref_info.at(i), lanes_list, p_sec, wp_id_seq, left_lane_ids, right_lane_ids);
-//			std::cout << " >>>>> Found Exact Section:  " << ref_info.at(i).ds_ << std::endl;
-		}
-
-		p_sec = getMatchingSection(ref_info.at(i).ds_);
-		if(p_sec != nullptr)
-		{
-			createSectionPoints(ref_info.at(i), lanes_list, p_sec, wp_id_seq, left_lane_ids, right_lane_ids);
-		}
-		else
-		{
-			std::cout << " >>>>> Can't Find Section:  " << ref_info.at(i).ds_ << std::endl;
-		}
-	}
-	
-	if(keep_right_)
-	{
-			for( auto id: left_lane_ids )
-			{
-					PlannerHNS::Lane* p_op_lane = getLaneById(id, lanes_list);
-					if(p_op_lane != nullptr)
-					{
-						std::reverse(	p_op_lane->points.begin(), p_op_lane->points.end() );
-					}
-			}
-	}
-	else
-	{
-		for( auto id: right_lane_ids )
-		{
-				PlannerHNS::Lane* p_op_lane = getLaneById(id, lanes_list);
-				if(p_op_lane != nullptr)
-				{
-					std::reverse(	p_op_lane->points.begin(), p_op_lane->points.end() );
-				}
-			}
-	}
-
-	for(unsigned int i=0; i < lanes_list.size(); i++)
-	{
-		fixRedundantPointsLanes(lanes_list.at(i));
-	}
-}
-
-std::vector<Connection> OpenDriveRoad::getLastSectionConnections(OpenDriveRoad *_p_successor_road)
-{	
-		std::vector<Connection> connections_list;
-		Connection conn;
-
-		if( _p_successor_road == nullptr)
-		{
-			return connections_list;
-		}
-		
-		//for right lanes
-		RoadSection* p_l_sec = getLastSection();
-		if(p_l_sec != nullptr)
-		{
-			conn.incoming_road_ = id_;
-			conn.incoming_section_ = p_l_sec->id_;
-			conn.outgoing_road_ = _p_successor_road->id_; 
-			conn.outgoing_section_ = 0;
-
-			for(unsigned int i =0 ; i < p_l_sec->right_lanes_.size(); i++)
-			{
-				if(p_l_sec->right_lanes_.at(i).to_lane_.size() > 0)
-				{
-					int _to_id = p_l_sec->right_lanes_.at(i).to_lane_.at(0).to_lane_id;
-					conn.lane_links.push_back(std::make_pair(p_l_sec->right_lanes_.at(i).id_, _to_id));
-				}
-				
-				if(conn.lane_links.size() > 0)
-				{
-					connections_list.push_back(conn);
-					conn.lane_links.clear();
-				}
-			}
-		}
-		
-		//for left lanes
-		if(p_l_sec != nullptr )
-		{
-			conn.outgoing_road_ = id_;
-			conn.outgoing_section_ = p_l_sec->id_;
-			conn.incoming_road_ = _p_successor_road->id_; 
-			conn.incoming_section_ = 0;
-
-			for(unsigned int i =0 ; i < p_l_sec->left_lanes_.size(); i++)
-			{
-				if(p_l_sec->left_lanes_.at(i).from_lane_.size() > 0)
-				{
-					int _from_id = p_l_sec->left_lanes_.at(i).from_lane_.at(0).from_lane_id;
-					conn.lane_links.push_back(std::make_pair( _from_id, p_l_sec->left_lanes_.at(i).id_));
-				}
-				
-				if(conn.lane_links.size() > 0)
-				{
-					connections_list.push_back(conn);
-					conn.lane_links.clear();
-				}
-			}
-		}
-		
-		return connections_list;
-}
-
-
-std::vector<Connection> OpenDriveRoad::getFirstSectionConnections( OpenDriveRoad *_p_predecessor_road )
-{
-	std::vector<Connection> connections_list;
-	Connection conn;
-
-	if(_p_predecessor_road == nullptr)
-	{
-		return connections_list;
-	}
-	
-	//for right lanes
-	RoadSection* p_f_sec =  getFirstSection();
-	RoadSection* p_prev_sec = _p_predecessor_road->getLastSection(); 
-	if(p_prev_sec != nullptr &&  p_f_sec != nullptr)
-	{
-		conn.outgoing_road_ = id_;
-		conn.outgoing_section_ = p_f_sec->id_;
-		conn.incoming_road_ = _p_predecessor_road->id_; 
-		conn.incoming_section_ = p_prev_sec->id_;
-
-		for(unsigned int i =0 ; i < p_f_sec->right_lanes_.size(); i++)
-		{
-			if(p_f_sec->right_lanes_.at(i).from_lane_.size() > 0)
-			{
-				int _from_id = p_f_sec->right_lanes_.at(i).from_lane_.at(0).from_lane_id;
-				conn.lane_links.push_back(std::make_pair(_from_id, p_f_sec->right_lanes_.at(i).id_));
-			}
-			
-			if(conn.lane_links.size() > 0)
-			{
-				connections_list.push_back(conn);
-				conn.lane_links.clear();
-			}
-		}		
-	}
-
-	 
-	//left lanes 
-	if(p_prev_sec != nullptr && p_f_sec != nullptr)
-	{
-		conn.incoming_road_ = id_;
-		conn.incoming_section_ = p_f_sec->id_;
-		conn.outgoing_road_ = _p_predecessor_road->id_; 
-		conn.outgoing_section_ = p_prev_sec->id_;
-
-		for(unsigned int i =0 ; i < p_f_sec->left_lanes_.size(); i++)
-		{
-			if(p_f_sec->left_lanes_.at(i).to_lane_.size() > 0)
-			{
-				int _to_id = p_f_sec->left_lanes_.at(i).to_lane_.at(0).to_lane_id;
-				conn.lane_links.push_back(std::make_pair(p_f_sec->left_lanes_.at(i).id_, _to_id));
-			}
-
-			if(conn.lane_links.size() > 0)
-			{
-				connections_list.push_back(conn);
-				conn.lane_links.clear();
-			}
-		}
-	}
-	
-	return connections_list;
-}
-
-OBJECT_TYPE OpenDriveRoad::getObjTypeFromText(const std::string& autoware_type)
-{
-	if(autoware_type.compare("TRAFFIC_LIGHT"))
-	{
-		return TRAFFIC_LIGHT;
-	}
-	else if(autoware_type.compare("ROAD_SIGN"))
-	{
-		return ROAD_SIGN;
-	}
-	else if(autoware_type.compare("ROAD_MARK"))
-	{
-		return ROAD_MARK;
-	}
-	else
-	{
-		return UNKNOWN_OBJECT;
-	}
-}
-
-OBJECT_TYPE OpenDriveRoad::getAutowareMainTypeFromCode(const std::string& country_code, const std::string& type, const std::string& sub_type)
-{
-	if(p_country_signal_codes_ != nullptr)
-	{
-		for(unsigned int i=0; i < p_country_signal_codes_->size(); i++)
-		{
-			if(p_country_signal_codes_->at(i).first.compare(country_code) == 0)
-			{
-				for(unsigned int j=0; j < p_country_signal_codes_->at(i).second.size(); j++)
-				{
-					if(p_country_signal_codes_->at(i).second.at(j).id_.compare(type) == 0 &&
-							p_country_signal_codes_->at(i).second.at(j).sub_id_.compare(sub_type) == 0)
-					{
-						return getObjTypeFromText(p_country_signal_codes_->at(i).second.at(j).gen_type_);
-					}
-				}
-			}
-		}
-	}
-
-	return UNKNOWN_OBJECT;
-}
-
-TRAFFIC_LIGHT_TYPE OpenDriveRoad::getLightTypeFromText(const std::string& autoware_type)
-{
-	if(autoware_type.compare("VERTICAL_DEFAULT_LIGHT"))
-	{
-		return VERTICAL_DEFAULT_LIGHT;
-	}
-	else if(autoware_type.compare("HORIZONTAL_DEFAULTLIGHT"))
-	{
-		return HORIZONTAL_DEFAULTLIGHT;
-	}
-	else if(autoware_type.compare("PEDESTRIAN_DEFAULT_LIGHT"))
-	{
-		return PEDESTRIAN_DEFAULT_LIGHT;
-	}
-	else
-	{
-		return UNKNOWN_LIGHT;
-	}
-}
-
-TRAFFIC_LIGHT_TYPE OpenDriveRoad::getAutowareLightTypeFromCode(const std::string& country_code, const std::string& type, const std::string& sub_type)
-{
-	if(p_country_signal_codes_ != nullptr)
-	{
-		for(unsigned int i=0; i < p_country_signal_codes_->size(); i++)
-		{
-			if(p_country_signal_codes_->at(i).first.compare(country_code) == 0)
-			{
-				for(unsigned int j=0; j < p_country_signal_codes_->at(i).second.size(); j++)
-				{
-					if(p_country_signal_codes_->at(i).second.at(j).id_.compare(type) == 0 &&
-							p_country_signal_codes_->at(i).second.at(j).sub_id_.compare(sub_type) == 0)
-					{
-						return getLightTypeFromText(p_country_signal_codes_->at(i).second.at(j).gen_sub_type_);
-					}
-				}
-			}
-		}
-	}
-
-	return UNKNOWN_LIGHT;
-}
-
-ROAD_SIGN_TYPE OpenDriveRoad::getSignTypeFromText(const std::string& autoware_type)
-{
-	if(autoware_type.compare("SPEED_LIMIT_SIGN"))
-	{
-		return SPEED_LIMIT_SIGN;
-	}
-	else if(autoware_type.compare("STOP_SIGN"))
-	{
-		return STOP_SIGN;
-	}
-	else if(autoware_type.compare("NO_PARKING_SIGN"))
-	{
-		return NO_PARKING_SIGN;
-	}
-	else
-	{
-		return UNKNOWN_SIGN;
-	}
-}
-
-ROAD_SIGN_TYPE OpenDriveRoad::getAutowareRoadSignTypeFromCode(const std::string& country_code, const std::string& type, const std::string& sub_type)
-{
-	if(p_country_signal_codes_ != nullptr)
-	{
-		for(unsigned int i=0; i < p_country_signal_codes_->size(); i++)
-		{
-			if(p_country_signal_codes_->at(i).first.compare(country_code) == 0)
-			{
-				for(unsigned int j=0; j < p_country_signal_codes_->at(i).second.size(); j++)
-				{
-					if(p_country_signal_codes_->at(i).second.at(j).id_.compare(type) == 0 &&
-							p_country_signal_codes_->at(i).second.at(j).sub_id_.compare(sub_type) == 0)
-					{
-						return getSignTypeFromText(p_country_signal_codes_->at(i).second.at(j).gen_sub_type_);
-					}
-				}
-			}
-		}
-	}
-
-	return UNKNOWN_SIGN;
-
-}
-
-ROAD_MARK_TYPE OpenDriveRoad::getMarkTypeFromText(const std::string& autoware_type)
-{
-	if(autoware_type.compare("STOP_LINE_MARK"))
-	{
-		return STOP_LINE_MARK;
-	}
-	else if(autoware_type.compare("WAITING_LINE_MARK"))
-	{
-		return WAITING_LINE_MARK;
-	}
-	else if(autoware_type.compare("FORWARD_DIRECTION_MARK"))
-	{
-		return FORWARD_DIRECTION_MARK;
-	}
-	else if(autoware_type.compare("LEFT_DIRECTION_MARK"))
-	{
-		return LEFT_DIRECTION_MARK;
-	}
-	else if(autoware_type.compare("RIGHT_DIRECTION_MARK"))
-	{
-		return RIGHT_DIRECTION_MARK;
-	}
-	else if(autoware_type.compare("FORWARD_LEFT_DIRECTION_MARK"))
-	{
-		return FORWARD_LEFT_DIRECTION_MARK;
-	}
-	else if(autoware_type.compare("FORWARD_RIGHT_DIRECTION_MARK"))
-	{
-		return FORWARD_RIGHT_DIRECTION_MARK;
-	}
-	else if(autoware_type.compare("ALL_DIRECTION_MARK"))
-	{
-		return ALL_DIRECTION_MARK;
-	}
-	else if(autoware_type.compare("U_TURN_DIRECTION_MARK"))
-	{
-		return U_TURN_DIRECTION_MARK;
-	}
-	else if(autoware_type.compare("NO_U_TURN_DIRECTION_MARK"))
-	{
-		return NO_U_TURN_DIRECTION_MARK;
-	}
-	else
-	{
-		return UNKNOWN_ROAD_MARK;
-	}
-}
-
-ROAD_MARK_TYPE OpenDriveRoad::getAutowareRoadMarksTypeFromCode(const std::string& country_code, const std::string& type, const std::string& sub_type)
-{
-	if(p_country_signal_codes_ != nullptr)
-	{
-		for(unsigned int i=0; i < p_country_signal_codes_->size(); i++)
-		{
-			if(p_country_signal_codes_->at(i).first.compare(country_code) == 0)
-			{
-				for(unsigned int j=0; j < p_country_signal_codes_->at(i).second.size(); j++)
-				{
-					if(p_country_signal_codes_->at(i).second.at(j).id_.compare(type) == 0 &&
-							p_country_signal_codes_->at(i).second.at(j).sub_id_.compare(sub_type) == 0)
-					{
-						return getMarkTypeFromText(p_country_signal_codes_->at(i).second.at(j).gen_sub_type_);
-					}
-				}
-			}
-		}
-	}
-
-	return UNKNOWN_ROAD_MARK;
-}
-
-void OpenDriveRoad::getTrafficLights(std::vector<PlannerHNS::TrafficLight>& all_lights)
-{
-	for(unsigned int i=0; i<road_signals_.size(); i++)
-	{
-		if(getAutowareMainTypeFromCode(road_signals_.at(i).country_code_, road_signals_.at(i).type_, road_signals_.at(i).sub_type_) == TRAFFIC_LIGHT)
-		{
-			PlannerHNS::TrafficLight tl;
-			tl.id = (this->id_*1000000) + road_signals_.at(i).id_ * 10;
-			PlannerHNS::WayPoint p;
-			if(createSingleCenterPoint(road_signals_.at(i).s_, p))
-			{
-				double a = p.pos.a+M_PI_2;
-				p.pos.x += road_signals_.at(i).t_ * cos(a);
-				p.pos.y += road_signals_.at(i).t_ * sin(a);
-				tl.pos = p.pos;
-				tl.laneIds = road_signals_.at(i).valid_lanes_ids_;
-				all_lights.push_back(tl);
-			}
-		}
-	}
-}
-
-void OpenDriveRoad::getTrafficSigns(std::vector<PlannerHNS::TrafficSign>& all_signs)
-{
-//	for(unsigned int i=0; i<road_signals_.size(); i++)
-//	{
-//		PlannerHNS::TrafficLight tl;
-//		tl.id = (this->id_*1000000) + road_signals_.at(i).id_ * 10;
-//		PlannerHNS::WayPoint p;
-//		if(CreateSingleCenterPoint(road_signals_.at(i).s_, p))
-//		{
-//			double a = p.pos.a+M_PI_2;
-//			p.pos.x += road_signals_.at(i).t_ * cos(a);
-//			p.pos.y += road_signals_.at(i).t_ * sin(a);
-//			tl.pos = p.pos;
-//			tl.laneIds = road_signals_.at(i).valid_lanes_ids_;
-//			all_lights.push_back(tl);
-//		}
-//	}
-}
-
-void OpenDriveRoad::getStopLines(std::vector<PlannerHNS::StopLine>& all_stop_lines)
-{
-	for(unsigned int i=0; i<road_signals_.size(); i++)
-	{
-		if(getAutowareMainTypeFromCode(road_signals_.at(i).country_code_, road_signals_.at(i).type_, road_signals_.at(i).sub_type_) ==  ROAD_MARK)
-		{
-			PlannerHNS::StopLine sl;
-			sl.id = (this->id_*1000000) + road_signals_.at(i).id_ * 10;
-			PlannerHNS::WayPoint p, p1, p2;
-			if(createSingleCenterPoint(road_signals_.at(i).s_, p))
-			{
-				//assume fixed 2 meter stop line length
-				p1 = p;
-				double a = p.pos.a+M_PI_2;
-				p1.pos.x += (road_signals_.at(i).t_ - 1.0 ) * cos(a);
-				p1.pos.y += (road_signals_.at(i).t_ - 1.0 ) * sin(a);
-
-				p2 = p;
-				p2.pos.x += (road_signals_.at(i).t_ + 1.0 ) * cos(a);
-				p2.pos.y += (road_signals_.at(i).t_ + 1.0 ) * sin(a);
-				sl.points.push_back(p1.pos);
-				sl.points.push_back(p2.pos);
-
-				if(road_signals_.at(i).valid_lanes_ids_.size() > 0)
-
-				for(unsigned int j=0; j<road_signals_.at(i).valid_lanes_ids_.size(); j++)
-				{
-					sl.laneId = road_signals_.at(i).valid_lanes_ids_.at(j);
-					all_stop_lines.push_back(sl);
-					sl.id += 1;
-				}
-			}
-		}
-	}
-}
-}

src/ros/catkin/src/vector_map_converter/src/opendrive2autowaremap_core/xml_helpers.cpp

@@ -1,72 +0,0 @@
-/*
- * opendrive2op_map_converter_core.cpp
- *
- *  Created on: Feb 11, 2019
- *      Author: hatem
- */
-
-#include "opendrive2autoware_converter/xml_helpers.h"
-#include <fstream>
-
-namespace opendrive_converter
-{
-void XmlHelpers::findElements(std::string name, TiXmlElement* parent_element, std::vector<TiXmlElement*>& element_list)
-{
-	if(parent_element == nullptr)
-		return;
-	else if(name.compare(parent_element->Value()) == 0)
-	{
-		element_list.push_back(parent_element);
-	}
-
-	findElements(name, parent_element->FirstChildElement(), element_list);
-	findElements(name, parent_element->NextSiblingElement(), element_list);
-
-}
-
-void XmlHelpers::findFirstElement(std::string name, TiXmlElement* parent_element, std::vector<TiXmlElement*>& element_list)
-{
-	if(parent_element == nullptr  || element_list.size()>0)
-		return;
-	else if(name.compare(parent_element->Value()) == 0)
-	{
-		element_list.push_back(parent_element);
-		return;
-	}
-
-	findFirstElement(name, parent_element->FirstChildElement(), element_list);
-	findFirstElement(name, parent_element->NextSiblingElement(), element_list);
-}
-
-int XmlHelpers::getIntAttribute(TiXmlElement* p_elem, std::string name, int def_val )
-{
-	if(p_elem != nullptr && p_elem->Attribute(name) != nullptr)
-		return strtol(p_elem->Attribute(name.c_str()), NULL, 10);
-	else
-		return def_val;
-}
-
-double XmlHelpers::getDoubleAttribute(TiXmlElement* p_elem, std::string name, double def_val )
-{
-	if(p_elem != nullptr && p_elem->Attribute(name) != nullptr)
-		return strtod(p_elem->Attribute(name.c_str()), NULL);
-	else
-		return def_val;
-}
-
-std::string XmlHelpers::getStringAttribute(TiXmlElement* p_elem, std::string name, std::string def_val)
-{
-	if(p_elem != nullptr && p_elem->Attribute(name) != nullptr)
-		return std::string(p_elem->Attribute(name.c_str()));
-	else
-		return def_val;
-}
-
-std::string XmlHelpers::getStringValue(TiXmlElement* p_elem, std::string def_val)
-{
-	if(p_elem != nullptr && p_elem->Value() != nullptr)
-		return p_elem->ValueStr();
-	else
-		return def_val;
-}
-}

src/ros/catkin/src/vtd_ros/CMakeLists.txt

@@ -0,0 +1,87 @@
+cmake_minimum_required(VERSION 2.8.11)
+project(vtd_ros)
+
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  pluginlib
+  sensor_msgs
+)
+find_package(Boost REQUIRED)
+find_package(Protobuf REQUIRED)
+
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_AUTOUIC ON)
+set(CMAKE_AUTORCC ON)
+
+
+if(CMAKE_COMPILER_IS_GNUCXX)
+    set(CMAKE_CXX_FLAGS "-fPIC ${CMAKE_CXX_FLAGS}")
+    message(STATUS "optional:-fPIC")   
+endif(CMAKE_COMPILER_IS_GNUCXX)
+
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+#find_package(Qt5Core  REQUIRED)
+#qt5_wrap_cpp(MOC src/qt_ros_test.h)
+#qt5_wrap_ui(UIC src/qt_ros_test.ui)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+##   INCLUDE_DIRS include
+#  LIBRARIES client_plugin
+   CATKIN_DEPENDS 
+    roscpp std_msgs 
+    sensor_msgs pluginlib
+  DEPENDS
+    Boost
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+##  INCLUDE_DIRS include
+  ${CMAKE_CURRENT_BINARY_DIR}/..
+  ${catkin_INCLUDE_DIRS} include
+  src/VTD
+  ${Boost_INCLUDE_DIR}
+  ${autoware_msgs_INCLUDE_DIRS}
+)
+
+## Declare a C++ executable
+add_executable(vtd_ros src/main.cpp src/rdbconn.cpp  src/VTD/RDBHandler.cc )
+target_link_libraries(vtd_ros ${catkin_LIBRARIES}  )
+
+
+install(
+  TARGETS vtd_ros
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

src/ros/catkin/src/vtd_ros/package.xml

@@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+<package>
+  <name>vtd_ros</name>
+  <version>0.0.0</version>
+  <description>ros module  connected to vtd</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="yuchuli@catarc.ac.cn">yuchuli</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/client_plugin</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+  <build_depend>pluginlib</build_depend>
+  <run_depend>pluginlib</run_depend>
+
+  <build_depend>sensor_msgs</build_depend>
+  <run_depend>sensor_msgs</run_depend>
+
+
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/ros/catkin/src/vtd_ros/src/VTD/RDBHandler.cc

@@ -0,0 +1,3587 @@
+/* ===================================================
+ *  file:       RDBHandler.cc
+ * ---------------------------------------------------
+ *  purpose:	collection of RDB routines
+ * ---------------------------------------------------
+ *  first edit:	24.01.2012 by M. Dupuis @ VIRES GmbH
+ *  last mod.:  24.01.2012 by M. Dupuis @ VIRES GmbH
+ * ===================================================
+ */
+/* ====== INCLUSIONS ====== */
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "RDBHandler.hh"
+
+namespace Framework 
+{
+    
+size_t
+RDBHandler::pkgId2size( unsigned int pkgId, bool extended )
+{
+    switch( pkgId )
+    {
+        case RDB_PKG_ID_START_OF_FRAME: 
+        case RDB_PKG_ID_END_OF_FRAME:
+            return 0;
+            
+        case RDB_PKG_ID_COORD_SYSTEM:
+            return sizeof( RDB_COORD_SYSTEM_t );
+            
+        case RDB_PKG_ID_COORD:
+            return sizeof( RDB_COORD_t );
+            
+        case RDB_PKG_ID_ROAD_POS:
+            return sizeof( RDB_ROAD_POS_t );
+            
+        case RDB_PKG_ID_LANE_INFO:
+            return sizeof( RDB_LANE_INFO_t );
+            
+        case RDB_PKG_ID_ROADMARK:
+            return sizeof( RDB_ROADMARK_t );
+            
+        case RDB_PKG_ID_OBJECT_CFG:
+            return sizeof( RDB_OBJECT_CFG_t );
+            
+        case RDB_PKG_ID_OBJECT_STATE:
+            return ( extended ? sizeof( RDB_OBJECT_STATE_t ) : sizeof ( RDB_OBJECT_STATE_BASE_t ) );
+            
+        case RDB_PKG_ID_VEHICLE_SYSTEMS:
+            return sizeof( RDB_VEHICLE_SYSTEMS_t );
+            
+        case RDB_PKG_ID_VEHICLE_SETUP:
+            return sizeof( RDB_VEHICLE_SETUP_t );
+            
+        case RDB_PKG_ID_ENGINE:
+            return ( extended ? sizeof( RDB_ENGINE_t ) : sizeof( RDB_ENGINE_BASE_t ) );
+            
+        case RDB_PKG_ID_DRIVETRAIN:
+            return ( extended ? sizeof( RDB_DRIVETRAIN_t ) : sizeof( RDB_DRIVETRAIN_BASE_t ) );
+            
+        case RDB_PKG_ID_WHEEL:
+            return ( extended ? sizeof( RDB_WHEEL_t ) : sizeof( RDB_WHEEL_BASE_t ) );
+            
+        case RDB_PKG_ID_PED_ANIMATION:
+            return sizeof( RDB_PED_ANIMATION_t );
+            
+        case RDB_PKG_ID_SENSOR_STATE:
+            return sizeof( RDB_SENSOR_STATE_t );
+            
+        case RDB_PKG_ID_SENSOR_OBJECT:
+            return sizeof( RDB_SENSOR_OBJECT_t );
+            
+        case RDB_PKG_ID_CAMERA:
+            return sizeof( RDB_CAMERA_t );
+            
+        case RDB_PKG_ID_CONTACT_POINT:
+            return sizeof( RDB_CONTACT_POINT_t );
+            
+        case RDB_PKG_ID_TRAFFIC_SIGN:
+            return sizeof( RDB_TRAFFIC_SIGN_t );
+            
+        case RDB_PKG_ID_ROAD_STATE:
+            return sizeof( RDB_ROAD_STATE_t );
+            
+        case RDB_PKG_ID_IMAGE:
+        case RDB_PKG_ID_LIGHT_MAP:
+        case RDB_PKG_ID_OCCLUSION_MATRIX:
+            return sizeof( RDB_IMAGE_t );
+            
+        case RDB_PKG_ID_LIGHT_SOURCE:
+            return ( extended ? sizeof( RDB_LIGHT_SOURCE_t ) : sizeof( RDB_LIGHT_SOURCE_BASE_t ) );
+            
+        case RDB_PKG_ID_ENVIRONMENT:
+            return sizeof( RDB_ENVIRONMENT_t );
+            
+        case RDB_PKG_ID_TRIGGER:
+            return sizeof( RDB_TRIGGER_t );
+            
+        case RDB_PKG_ID_DRIVER_CTRL:
+            return sizeof( RDB_DRIVER_CTRL_t );
+            
+        case RDB_PKG_ID_TRAFFIC_LIGHT:
+            return ( extended ? sizeof( RDB_TRAFFIC_LIGHT_t ) : sizeof( RDB_TRAFFIC_LIGHT_BASE_t ) );
+            
+        case RDB_PKG_ID_SYNC:
+            return sizeof( RDB_SYNC_t );
+            
+        case RDB_PKG_ID_DRIVER_PERCEPTION:
+            return sizeof( RDB_DRIVER_PERCEPTION_t );
+            
+        case RDB_PKG_ID_TONE_MAPPING:
+            return sizeof( RDB_FUNCTION_t );            
+            
+        case RDB_PKG_ID_ROAD_QUERY:
+            return sizeof( RDB_ROAD_QUERY_t );
+            
+        case RDB_PKG_ID_SCP:
+            return sizeof( RDB_SCP_t );
+            
+        case RDB_PKG_ID_TRAJECTORY:
+            return sizeof( RDB_TRAJECTORY_t );
+            
+        case RDB_PKG_ID_DYN_2_STEER:
+            return sizeof( RDB_DYN_2_STEER_t );
+            
+        case RDB_PKG_ID_STEER_2_DYN:
+            return sizeof( RDB_STEER_2_DYN_t );
+            
+        case RDB_PKG_ID_PROXY:
+            return sizeof( RDB_PROXY_t );
+            
+        case RDB_PKG_ID_MOTION_SYSTEM:
+            return sizeof( RDB_MOTION_SYSTEM_t );
+            
+        case RDB_PKG_ID_FREESPACE:
+            return sizeof( RDB_FREESPACE_t );
+            
+        case RDB_PKG_ID_DYN_EL_SWITCH:
+            return sizeof( RDB_DYN_EL_SWITCH_t );
+            
+        case RDB_PKG_ID_DYN_EL_DOF:
+            return sizeof( RDB_DYN_EL_DOF_t );
+            
+        case RDB_PKG_ID_CUSTOM_SCORING:
+            return sizeof( RDB_CUSTOM_SCORING_t );
+            
+        case RDB_PKG_ID_IG_FRAME:
+            return sizeof( RDB_IG_FRAME_t );
+            
+        case RDB_PKG_ID_RT_PERFORMANCE:
+            return sizeof( RDB_RT_PERFORMANCE_t );
+            
+        case RDB_PKG_ID_RAY:
+            return sizeof( RDB_RAY_t );
+            
+        case RDB_PKG_ID_CUSTOM_OBJECT_CTRL_TRACK:
+            return sizeof( RDB_CUSTOM_OBJECT_CTRL_TRACK_t );
+            
+        default:
+            fprintf( stderr, "RDBHandler::pkgId2size: request for size of unknown package <%d>. Returning zero", pkgId );
+            return 0;
+    }
+}
+
+std::string
+RDBHandler::pkgId2string( unsigned int pkgId )
+{
+    switch( pkgId )
+    {
+        case RDB_PKG_ID_START_OF_FRAME: 
+            return std::string( "RDB_PKG_ID_START_OF_FRAME" );
+
+        case RDB_PKG_ID_END_OF_FRAME:
+            return std::string( "RDB_PKG_ID_END_OF_FRAME" );
+            
+        case RDB_PKG_ID_COORD_SYSTEM:
+            return std::string( "RDB_PKG_ID_COORD_SYSTEM" );
+            
+        case RDB_PKG_ID_COORD:
+            return std::string( "RDB_PKG_ID_COORD" );
+            
+        case RDB_PKG_ID_ROAD_POS:
+            return std::string( "RDB_PKG_ID_ROAD_POS" );
+            
+        case RDB_PKG_ID_LANE_INFO:
+            return std::string( "RDB_PKG_ID_LANE_INFO" );
+            
+        case RDB_PKG_ID_ROADMARK:
+            return std::string( "RDB_PKG_ID_ROADMARK" );
+            
+        case RDB_PKG_ID_OBJECT_CFG:
+            return std::string( "RDB_PKG_ID_OBJECT_CFG" );
+            
+        case RDB_PKG_ID_OBJECT_STATE:
+            return std::string( "RDB_PKG_ID_OBJECT_STATE" );
+            
+        case RDB_PKG_ID_VEHICLE_SYSTEMS:
+            return std::string( "RDB_PKG_ID_VEHICLE_SYSTEMS" );
+            
+        case RDB_PKG_ID_VEHICLE_SETUP:
+            return std::string( "RDB_PKG_ID_VEHICLE_SETUP" );
+            
+        case RDB_PKG_ID_ENGINE:
+            return std::string( "RDB_PKG_ID_ENGINE" );
+            
+        case RDB_PKG_ID_DRIVETRAIN:
+            return std::string( "RDB_PKG_ID_DRIVETRAIN" );
+            
+        case RDB_PKG_ID_WHEEL:
+            return std::string( "RDB_PKG_ID_WHEEL" );
+            
+        case RDB_PKG_ID_PED_ANIMATION:
+            return std::string( "RDB_PKG_ID_PED_ANIMATION" );
+            
+        case RDB_PKG_ID_SENSOR_STATE:
+            return std::string( "RDB_PKG_ID_SENSOR_STATE" );
+            
+        case RDB_PKG_ID_SENSOR_OBJECT:
+            return std::string( "RDB_PKG_ID_SENSOR_OBJECT" );
+            
+        case RDB_PKG_ID_CAMERA:
+            return std::string( "RDB_PKG_ID_CAMERA" );
+            
+        case RDB_PKG_ID_CONTACT_POINT:
+            return std::string( "RDB_PKG_ID_CONTACT_POINT" );
+            
+        case RDB_PKG_ID_TRAFFIC_SIGN:
+            return std::string( "RDB_PKG_ID_TRAFFIC_SIGN" );
+            
+        case RDB_PKG_ID_ROAD_STATE:
+            return std::string( "RDB_PKG_ID_ROAD_STATE" );
+            
+        case RDB_PKG_ID_IMAGE:
+            return std::string( "RDB_PKG_ID_IMAGE" );
+            
+        case RDB_PKG_ID_LIGHT_SOURCE:
+             return std::string( "RDB_PKG_ID_LIGHT_SOURCE" );
+            
+        case RDB_PKG_ID_ENVIRONMENT:
+            return std::string( "RDB_PKG_ID_ENVIRONMENT" );
+            
+        case RDB_PKG_ID_TRIGGER:
+            return std::string( "RDB_PKG_ID_TRIGGER" );
+            
+        case RDB_PKG_ID_DRIVER_CTRL:
+            return std::string( "RDB_PKG_ID_DRIVER_CTRL" );
+            
+        case RDB_PKG_ID_TRAFFIC_LIGHT:
+            return std::string( "RDB_PKG_ID_TRAFFIC_LIGHT" );
+            
+        case RDB_PKG_ID_SYNC:
+            return std::string( "RDB_PKG_ID_SYNC" );
+            
+        case RDB_PKG_ID_DRIVER_PERCEPTION:
+            return std::string( "RDB_PKG_ID_DRIVER_PERCEPTION" );
+            
+        case RDB_PKG_ID_LIGHT_MAP:
+            return std::string( "RDB_PKG_ID_LIGHT_MAP" );
+            
+        case RDB_PKG_ID_TONE_MAPPING:
+            return std::string( "RDB_PKG_ID_TONE_MAPPING" );
+            
+        case RDB_PKG_ID_ROAD_QUERY:
+            return std::string( "RDB_PKG_ROAD_QUERY" );
+            
+        case RDB_PKG_ID_SCP:
+            return std::string( "RDB_PKG_ID_SCP" );
+            
+        case RDB_PKG_ID_TRAJECTORY:
+            return std::string( "RDB_PKG_ID_TRAJECTORY" );
+            
+        case RDB_PKG_ID_DYN_2_STEER:
+            return std::string( "RDB_PKG_ID_DYN_2_STEER" );
+            
+        case RDB_PKG_ID_STEER_2_DYN:
+            return std::string( "RDB_PKG_ID_STEER_2_DYN" );
+            
+        case RDB_PKG_ID_PROXY:
+            return std::string( "RDB_PKG_ID_PROXY" );
+
+        case RDB_PKG_ID_MOTION_SYSTEM:
+            return std::string( "RDB_PKG_ID_MOTION_SYSTEM" );
+            
+        case RDB_PKG_ID_OCCLUSION_MATRIX:
+            return std::string( "RDB_PKG_ID_OCCLUSION_MATRIX" );
+            
+        case RDB_PKG_ID_FREESPACE:
+            return std::string( "RDB_PKG_ID_FREESPACE" );
+            
+        case RDB_PKG_ID_DYN_EL_SWITCH:
+            return std::string( "RDB_PKG_ID_DYN_EL_SWITCH" );
+            
+        case RDB_PKG_ID_DYN_EL_DOF:
+            return std::string( "RDB_PKG_ID_DYN_EL_DOF" );
+            
+        case RDB_PKG_ID_IG_FRAME:
+            return std::string( "RDB_PKG_ID_IG_FRAME" );
+            
+        case RDB_PKG_ID_RT_PERFORMANCE:
+            return std::string( "RDB_PKG_ID_RT_PERFORMANCE" );
+            
+        case RDB_PKG_ID_RAY:
+            return std::string( "RDB_PKG_ID_RAY" );
+            
+        case RDB_PKG_ID_CUSTOM_SCORING:
+            return std::string( "RDB_PKG_ID_CUSTOM_SCORING" );
+            
+        case RDB_PKG_ID_CUSTOM_OBJECT_CTRL_TRACK:
+             return std::string( "RDB_PKG_ID_CUSTOM_OBJECT_CTRL_TRACK" );
+            
+        case RDB_PKG_ID_CUSTOM_AUDI_FORUM:
+            return std::string( "RDB_PKG_ID_CUSTOM_AUDI_FORUM" );
+            
+        case RDB_PKG_ID_CUSTOM_OPTIX_START:
+            return std::string( "RDB_PKG_ID_CUSTOM_OPTIX_START" );
+            
+        case RDB_PKG_ID_CUSTOM_OPTIX_END:
+            return std::string( "RDB_PKG_ID_CUSTOM_OPTIX_END" );
+            
+        default:
+            return std::string( "unknown" );
+    }
+}
+
+std::string
+RDBHandler::coordType2string( unsigned int type )
+{
+    switch( type )
+    {
+        case RDB_COORD_TYPE_INERTIAL: 
+            return std::string( "RDB_COORD_TYPE_INERTIAL" );
+
+        case RDB_COORD_TYPE_RESERVED_1:
+            return std::string( "RDB_COORD_TYPE_RESERVED_1" );
+            
+        case RDB_COORD_TYPE_PLAYER:
+            return std::string( "RDB_COORD_TYPE_PLAYER" );
+            
+        case RDB_COORD_TYPE_SENSOR:
+            return std::string( "RDB_COORD_TYPE_SENSOR" );
+            
+        case RDB_COORD_TYPE_USK:
+            return std::string( "RDB_COORD_TYPE_USK" );
+            
+        case RDB_COORD_TYPE_USER:
+            return std::string( "RDB_COORD_TYPE_USER" );
+            
+        case RDB_COORD_TYPE_WINDOW:
+            return std::string( "RDB_COORD_TYPE_WINDOW" );
+            
+        case RDB_COORD_TYPE_TEXTURE:
+            return std::string( "RDB_COORD_TYPE_TEXTURE" );
+            
+        case RDB_COORD_TYPE_RELATIVE_START:
+            return std::string( "RDB_COORD_TYPE_RELATIVE_START" );
+            
+        case RDB_COORD_TYPE_GEO:
+            return std::string( "RDB_COORD_TYPE_GEO" );
+           
+        default:
+            return std::string( "unknown" );
+    }
+}
+
+std::string
+RDBHandler::objectCategory2string( unsigned int id )
+{
+    switch( id )
+    {
+        case RDB_OBJECT_CATEGORY_NONE: 
+            return std::string( "RDB_OBJECT_CATEGORY_NONE" );
+
+        case RDB_OBJECT_CATEGORY_PLAYER:
+            return std::string( "RDB_OBJECT_CATEGORY_PLAYER" );
+            
+        case RDB_OBJECT_CATEGORY_SENSOR:
+            return std::string( "RDB_OBJECT_CATEGORY_SENSOR" );
+            
+        case RDB_OBJECT_CATEGORY_CAMERA:
+            return std::string( "RDB_OBJECT_CATEGORY_CAMERA" );
+            
+        case RDB_OBJECT_CATEGORY_LIGHT_POINT:
+            return std::string( "RDB_OBJECT_CATEGORY_LIGHT_POINT" );
+            
+        case RDB_OBJECT_CATEGORY_COMMON:
+            return std::string( "RDB_OBJECT_CATEGORY_COMMON" );
+            
+        case RDB_OBJECT_CATEGORY_OPENDRIVE:
+            return std::string( "RDB_OBJECT_CATEGORY_OPENDRIVE" );
+            
+        default:
+            return std::string( "unknown" );
+    }
+}
+
+unsigned int
+RDBHandler::objectString2category( const std::string & name )
+{
+    if ( name == std::string( "RDB_OBJECT_CATEGORY_NONE" ) )
+        return RDB_OBJECT_CATEGORY_NONE;
+
+    if ( name == std::string( "RDB_OBJECT_CATEGORY_PLAYER" ) )
+        return RDB_OBJECT_CATEGORY_PLAYER;
+
+    if ( name == std::string( "RDB_OBJECT_CATEGORY_SENSOR" ) )
+        return RDB_OBJECT_CATEGORY_SENSOR;
+
+    if ( name == std::string( "RDB_OBJECT_CATEGORY_CAMERA" ) )
+        return RDB_OBJECT_CATEGORY_CAMERA;
+
+    if ( name == std::string( "RDB_OBJECT_CATEGORY_LIGHT_POINT" ) )
+        return RDB_OBJECT_CATEGORY_LIGHT_POINT;
+
+    if ( name == std::string( "RDB_OBJECT_CATEGORY_COMMON" ) )
+        return RDB_OBJECT_CATEGORY_COMMON;
+
+    if ( name == std::string( "RDB_OBJECT_CATEGORY_OPENDRIVE" ) )
+        return RDB_OBJECT_CATEGORY_OPENDRIVE;
+    
+    return RDB_OBJECT_CATEGORY_NONE;
+}
+
+std::string
+RDBHandler::objectType2string( unsigned int id )
+{
+    switch( id )
+    {
+        case RDB_OBJECT_TYPE_PLAYER_NONE:
+            return std::string( "RDB_OBJECT_TYPE_PLAYER_NONE" );
+            
+        case RDB_OBJECT_TYPE_PLAYER_CAR:
+            return std::string( "RDB_OBJECT_TYPE_PLAYER_CAR" );
+            
+        case RDB_OBJECT_TYPE_PLAYER_TRUCK:
+            return std::string( "RDB_OBJECT_TYPE_PLAYER_TRUCK" );
+            
+        case RDB_OBJECT_TYPE_PLAYER_VAN:
+            return std::string( "RDB_OBJECT_TYPE_PLAYER_VAN" );
+            
+        case RDB_OBJECT_TYPE_PLAYER_BIKE:
+            return std::string( "RDB_OBJECT_TYPE_PLAYER_BIKE" );
+            
+        case RDB_OBJECT_TYPE_PLAYER_PEDESTRIAN:
+            return std::string( "RDB_OBJECT_TYPE_PLAYER_PEDESTRIAN" );
+            
+        case RDB_OBJECT_TYPE_PLAYER_PED_GROUP:
+            return std::string( "RDB_OBJECT_TYPE_PLAYER_PED_GROUP" );
+            
+        case RDB_OBJECT_TYPE_POLE:
+            return std::string( "RDB_OBJECT_TYPE_POLE" );
+            
+        case RDB_OBJECT_TYPE_TREE:
+            return std::string( "RDB_OBJECT_TYPE_TREE" );
+            
+        case RDB_OBJECT_TYPE_BARRIER:
+            return std::string( "RDB_OBJECT_TYPE_BARRIER" );
+            
+        case RDB_OBJECT_TYPE_OPT1:
+            return std::string( "RDB_OBJECT_TYPE_OPT1" );
+            
+        case RDB_OBJECT_TYPE_OPT2:
+            return std::string( "RDB_OBJECT_TYPE_OPT2" );
+            
+        case RDB_OBJECT_TYPE_OPT3:
+            return std::string( "RDB_OBJECT_TYPE_OPT3" );
+            
+        case RDB_OBJECT_TYPE_PLAYER_MOTORBIKE:
+            return std::string( "RDB_OBJECT_TYPE_PLAYER_MOTORBIKE" );
+            
+        case RDB_OBJECT_TYPE_PLAYER_BUS:
+            return std::string( "RDB_OBJECT_TYPE_PLAYER_BUS" );
+            
+        case RDB_OBJECT_TYPE_STREET_LAMP:
+            return std::string( "RDB_OBJECT_TYPE_STREET_LAMP" );
+            
+        case RDB_OBJECT_TYPE_TRAFFIC_SIGN:
+            return std::string( "RDB_OBJECT_TYPE_TRAFFIC_SIGN" );
+            
+        case RDB_OBJECT_TYPE_HEADLIGHT:
+            return std::string( "RDB_OBJECT_TYPE_HEADLIGHT" );
+            
+        case RDB_OBJECT_TYPE_PLAYER_TRAILER:
+            return std::string( "RDB_OBJECT_TYPE_PLAYER_TRAILER" );
+            
+        case RDB_OBJECT_TYPE_BUILDING:
+            return std::string( "RDB_OBJECT_TYPE_BUILDING" );
+            
+        case RDB_OBJECT_TYPE_PARKING_SPACE:
+            return std::string( "RDB_OBJECT_TYPE_PARKING_SPACE" );
+            
+        case RDB_OBJECT_TYPE_ROAD_WORKS:
+            return std::string( "RDB_OBJECT_TYPE_ROAD_WORKS" );
+            
+        case RDB_OBJECT_TYPE_ROAD_MISC:
+            return std::string( "RDB_OBJECT_TYPE_ROAD_MISC" );
+            
+        case RDB_OBJECT_TYPE_TUNNEL:
+            return std::string( "RDB_OBJECT_TYPE_TUNNEL" );
+            
+        case RDB_OBJECT_TYPE_LEGACY:
+            return std::string( "RDB_OBJECT_TYPE_LEGACY" );
+            
+        case RDB_OBJECT_TYPE_VEGETATION:
+            return std::string( "RDB_OBJECT_TYPE_VEGETATION" );
+            
+        case RDB_OBJECT_TYPE_MISC_MOTORWAY:
+            return std::string( "RDB_OBJECT_TYPE_MISC_MOTORWAY" );
+            
+        case RDB_OBJECT_TYPE_MISC_TOWN:
+            return std::string( "RDB_OBJECT_TYPE_MISC_TOWN" );
+            
+        case RDB_OBJECT_TYPE_PATCH:
+            return std::string( "RDB_OBJECT_TYPE_PATCH" );
+            
+        case RDB_OBJECT_TYPE_OTHER:
+            return std::string( "RDB_OBJECT_TYPE_OTHER" );
+            
+        case RDB_OBJECT_PLAYER_SEMI_TRAILER:
+            return std::string( "RDB_OBJECT_PLAYER_SEMI_TRAILER" );
+            
+        case RDB_OBJECT_PLAYER_RAILCAR:
+            return std::string( "RDB_OBJECT_PLAYER_RAILCAR" );
+            
+        case RDB_OBJECT_PLAYER_RAILCAR_SEMI_HEAD:
+            return std::string( "RDB_OBJECT_PLAYER_RAILCAR_SEMI_HEAD" );
+            
+        case RDB_OBJECT_PLAYER_RAILCAR_SEMI_BACK:
+            return std::string( "RDB_OBJECT_PLAYER_RAILCAR_SEMI_BACK" );
+            
+        default:
+            return std::string( "unknown" );
+    }
+}
+
+unsigned int
+RDBHandler::objectString2type( const std::string & name )
+{
+    if ( name == std::string( "RDB_OBJECT_TYPE_PLAYER_NONE" ) )
+        return RDB_OBJECT_TYPE_PLAYER_NONE;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_PLAYER_CAR" ) )
+        return RDB_OBJECT_TYPE_PLAYER_CAR;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_PLAYER_TRUCK" ) )
+        return RDB_OBJECT_TYPE_PLAYER_TRUCK;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_PLAYER_VAN" ) )
+        return RDB_OBJECT_TYPE_PLAYER_VAN;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_PLAYER_BIKE" ) )
+        return RDB_OBJECT_TYPE_PLAYER_BIKE;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_PLAYER_PEDESTRIAN" ) )
+        return RDB_OBJECT_TYPE_PLAYER_PEDESTRIAN;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_PLAYER_PED_GROUP" ) )
+        return RDB_OBJECT_TYPE_PLAYER_PED_GROUP;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_POLE" ) )
+        return RDB_OBJECT_TYPE_POLE;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_TREE" ) )
+        return RDB_OBJECT_TYPE_TREE;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_BARRIER" ) )
+        return RDB_OBJECT_TYPE_BARRIER;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_OPT1" ) )
+        return RDB_OBJECT_TYPE_OPT1;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_OPT2" ) )
+        return RDB_OBJECT_TYPE_OPT2;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_OPT3" ) )
+        return RDB_OBJECT_TYPE_OPT3;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_PLAYER_MOTORBIKE" ) )
+        return RDB_OBJECT_TYPE_PLAYER_MOTORBIKE;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_PLAYER_BUS" ) )
+        return RDB_OBJECT_TYPE_PLAYER_BUS;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_STREET_LAMP" ) )
+        return RDB_OBJECT_TYPE_STREET_LAMP;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_TRAFFIC_SIGN" ) )
+        return RDB_OBJECT_TYPE_TRAFFIC_SIGN;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_HEADLIGHT" ) )
+        return RDB_OBJECT_TYPE_HEADLIGHT;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_PLAYER_TRAILER" ) )
+        return RDB_OBJECT_TYPE_PLAYER_TRAILER;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_BUILDING" ) )
+        return RDB_OBJECT_TYPE_BUILDING;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_PARKING_SPACE" ) )
+        return RDB_OBJECT_TYPE_PARKING_SPACE;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_ROAD_WORKS" ) )
+        return RDB_OBJECT_TYPE_ROAD_WORKS;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_ROAD_MISC" ) )
+        return RDB_OBJECT_TYPE_ROAD_MISC;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_TUNNEL" ) )
+        return RDB_OBJECT_TYPE_TUNNEL;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_LEGACY" ) )
+        return RDB_OBJECT_TYPE_LEGACY;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_VEGETATION" ) )
+        return RDB_OBJECT_TYPE_VEGETATION;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_MISC_MOTORWAY" ) )
+        return RDB_OBJECT_TYPE_MISC_MOTORWAY;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_MISC_TOWN" ) )
+        return RDB_OBJECT_TYPE_MISC_TOWN;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_PATCH" ) )
+        return RDB_OBJECT_TYPE_PATCH;
+    
+    if ( name == std::string( "RDB_OBJECT_TYPE_OTHER" ) )
+        return RDB_OBJECT_TYPE_OTHER;
+    
+    if ( name == std::string( "RDB_OBJECT_PLAYER_SEMI_TRAILER" ) )
+        return RDB_OBJECT_PLAYER_SEMI_TRAILER;
+    
+    if ( name == std::string( "RDB_OBJECT_PLAYER_RAILCAR" ) )
+        return RDB_OBJECT_PLAYER_RAILCAR;
+    
+    if ( name == std::string( "RDB_OBJECT_PLAYER_RAILCAR_SEMI_HEAD" ) )
+        return RDB_OBJECT_PLAYER_RAILCAR_SEMI_HEAD;
+    
+    if ( name == std::string( "RDB_OBJECT_PLAYER_RAILCAR_SEMI_BACK" ) )
+        return RDB_OBJECT_PLAYER_RAILCAR_SEMI_BACK;
+    
+    return RDB_OBJECT_TYPE_PLAYER_NONE;
+}
+
+void
+RDBHandler::printMessage( RDB_MSG_t* msg, bool details, bool binDump, bool csv, bool csvHeader )
+{
+    if ( !msg )
+    {
+        fprintf( stderr, "RDBHandler::printMessage: no message available\n" );
+        return;
+    }
+    
+    if ( !csv && !csvHeader )
+    {
+        fprintf( stderr, "\nRDBHandler::printMessage: ---- %s ----- BEGIN\n", details ? "full info" : "short info" );
+        fprintf( stderr, "  message: version = 0x%04x, simTime = %.3f, simFrame = %d, headerSize = %d, dataSize = %d\n",
+                                       msg->hdr.version, msg->hdr.simTime, msg->hdr.frameNo, msg->hdr.headerSize, msg->hdr.dataSize );
+    }
+                               
+    if ( !msg->hdr.dataSize )
+        return;
+    
+    RDB_MSG_ENTRY_HDR_t* entry = ( RDB_MSG_ENTRY_HDR_t* ) ( ( ( char* ) msg ) + msg->hdr.headerSize );
+    uint32_t remainingBytes    = msg->hdr.dataSize;
+        
+    while ( remainingBytes )
+    {
+        if ( entry->pkgId == RDB_PKG_ID_START_OF_FRAME )
+        {
+            if ( csvHeader )
+                fprintf( stderr, "%23s,%23s,", "simTime", "simFrame" );
+            else if ( csv ) 
+                fprintf( stderr, "%+.16e,%23d,", msg->hdr.simTime, msg->hdr.frameNo );
+        }
+        
+        printMessageEntry( entry, details, csv, csvHeader );
+
+        remainingBytes -= ( entry->headerSize + entry->dataSize );
+        
+        if ( remainingBytes )
+          entry = ( RDB_MSG_ENTRY_HDR_t* ) ( ( ( ( char* ) entry ) + entry->headerSize + entry->dataSize ) );
+    }
+    
+    // create a binary dump?
+    if ( binDump )
+    {
+        fprintf( stderr, "RDBHandler::printMessage: ---- binary dump ----- \n" );
+        
+        uint32_t remainingBytes = msg->hdr.dataSize + msg->hdr.headerSize;
+        unsigned char* dataPtr = ( unsigned char* ) msg;
+        
+        for ( unsigned int i = 1; i <= remainingBytes; i++ )
+        {
+            fprintf( stderr, "%02x ", *dataPtr );
+            
+            dataPtr++;
+            
+            if ( !( i % 16 ) )
+                fprintf( stderr, "\n" );
+        }
+        fprintf( stderr, "\n" );
+    }
+    
+    if ( !csv )
+        fprintf( stderr, "RDBHandler::printMessage: ---- %s ----- END\n", details ? "full info" : "short info" );
+}
+
+void
+RDBHandler::printMessageEntry( RDB_MSG_ENTRY_HDR_t* entryHdr, bool details, bool csv, bool csvHeader )
+{
+    if ( !entryHdr )
+        return;
+    
+    int noElements = entryHdr->elementSize ? ( entryHdr->dataSize / entryHdr->elementSize ) : 0;
+    
+    if ( !csv && !csvHeader )
+    {
+        fprintf( stderr, "    entry: pkgId = %2d (%s), headersize = %d, dataSize = %d, elementSize = %d, noElements = %d, flags = 0x%x\n",
+                         entryHdr->pkgId, pkgId2string( entryHdr->pkgId ).c_str(), entryHdr->headerSize, 
+                         entryHdr->dataSize, entryHdr->elementSize, noElements, entryHdr->flags );
+    }
+    else if ( entryHdr->pkgId == RDB_PKG_ID_END_OF_FRAME )
+        fprintf( stderr, "\n" );
+    
+    if ( details )
+    {
+        unsigned char ident   = 6;
+        char*         dataPtr = ( char* ) entryHdr;
+        
+        dataPtr += entryHdr->headerSize;
+        
+        while ( noElements-- )
+        {
+            bool printedMsg = true;
+            
+            switch ( entryHdr->pkgId )
+            {
+                case RDB_PKG_ID_COORD_SYSTEM:
+                    print( *( ( RDB_COORD_SYSTEM_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_COORD:
+                    print( *( ( RDB_COORD_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_ROAD_POS:
+                    print( *( ( RDB_ROAD_POS_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_LANE_INFO:
+                    print( *( ( RDB_LANE_INFO_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_ROADMARK:
+                    print( *( ( RDB_ROADMARK_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_OBJECT_CFG:
+                    print( *( ( RDB_OBJECT_CFG_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_OBJECT_STATE:
+                    print( *( ( RDB_OBJECT_STATE_t* ) dataPtr ), entryHdr->flags & RDB_PKG_FLAG_EXTENDED, ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_VEHICLE_SYSTEMS:
+                    print( *( ( RDB_VEHICLE_SYSTEMS_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_VEHICLE_SETUP:
+                    print( *( ( RDB_VEHICLE_SETUP_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_ENGINE:
+                    print( *( ( RDB_ENGINE_t* ) dataPtr ), entryHdr->flags & RDB_PKG_FLAG_EXTENDED, ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_DRIVETRAIN:
+                    print( *( ( RDB_DRIVETRAIN_t* ) dataPtr ), entryHdr->flags & RDB_PKG_FLAG_EXTENDED, ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_WHEEL:
+                    print( *( ( RDB_WHEEL_t* ) dataPtr ), entryHdr->flags & RDB_PKG_FLAG_EXTENDED, ident, csv, csvHeader );
+                    break;
+
+                case RDB_PKG_ID_PED_ANIMATION:
+                    print( *( ( RDB_PED_ANIMATION_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+
+                case RDB_PKG_ID_SENSOR_STATE:
+                    print( *( ( RDB_SENSOR_STATE_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+
+                case RDB_PKG_ID_SENSOR_OBJECT:
+                    print( *( ( RDB_SENSOR_OBJECT_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_CAMERA:
+                    print( *( ( RDB_CAMERA_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_CONTACT_POINT:
+                    print( *( ( RDB_CONTACT_POINT_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_TRAFFIC_SIGN:
+                    print( *( ( RDB_TRAFFIC_SIGN_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_ROAD_STATE:
+                    print( *( ( RDB_ROAD_STATE_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_IMAGE:
+                case RDB_PKG_ID_LIGHT_MAP:
+                    print( *( ( RDB_IMAGE_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_OCCLUSION_MATRIX:
+                    {
+                        RDB_IMAGE_t* pImage = ( RDB_IMAGE_t* ) dataPtr; 
+                        print( *pImage, ident );
+                        
+                        // print also the contents?
+                        if ( pImage->pixelSize == 32 ) // works only for 32bit integer!
+                            printMatrix( ( int * ) ( dataPtr + sizeof( RDB_IMAGE_t ) ), pImage->width, pImage->height, csv, csvHeader );
+                    }
+                    break;
+                    
+                case RDB_PKG_ID_LIGHT_SOURCE:
+                    print( *( ( RDB_LIGHT_SOURCE_t* ) dataPtr ), entryHdr->flags & RDB_PKG_FLAG_EXTENDED, ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_ENVIRONMENT:
+                    print( *( ( RDB_ENVIRONMENT_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_TRIGGER:
+                    print( *( ( RDB_TRIGGER_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_DRIVER_CTRL:
+                    print( *( ( RDB_DRIVER_CTRL_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_TRAFFIC_LIGHT:
+                    print( *( ( RDB_TRAFFIC_LIGHT_t* ) dataPtr ), entryHdr->flags & RDB_PKG_FLAG_EXTENDED, ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_SYNC:
+                    print( *( ( RDB_SYNC_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_DRIVER_PERCEPTION:
+                    print( *( ( RDB_DRIVER_PERCEPTION_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_TONE_MAPPING:
+                    print( *( ( RDB_FUNCTION_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_ROAD_QUERY:
+                    print( *( ( RDB_ROAD_QUERY_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+                    
+                case RDB_PKG_ID_TRAJECTORY:
+                    print( *( ( RDB_TRAJECTORY_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+
+                case RDB_PKG_ID_DYN_2_STEER:
+                    print( *( ( RDB_DYN_2_STEER_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+
+                case RDB_PKG_ID_STEER_2_DYN:
+                    print( *( ( RDB_STEER_2_DYN_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+
+                case RDB_PKG_ID_PROXY:
+                    print( *( ( RDB_PROXY_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+
+                case RDB_PKG_ID_MOTION_SYSTEM:
+                    print( *( ( RDB_MOTION_SYSTEM_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+
+                case RDB_PKG_ID_FREESPACE:
+                    print( *( ( RDB_FREESPACE_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+
+                case RDB_PKG_ID_DYN_EL_SWITCH:
+                    print( *( ( RDB_DYN_EL_SWITCH_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+
+                case RDB_PKG_ID_DYN_EL_DOF:
+                    print( *( ( RDB_DYN_EL_DOF_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+
+                case RDB_PKG_ID_IG_FRAME:
+                    print( *( ( RDB_IG_FRAME_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+
+                case RDB_PKG_ID_RT_PERFORMANCE:
+                    print( *( ( RDB_RT_PERFORMANCE_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+
+                case RDB_PKG_ID_CUSTOM_SCORING:
+                    print( *( ( RDB_CUSTOM_SCORING_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+
+                case RDB_PKG_ID_CUSTOM_OBJECT_CTRL_TRACK:
+                    print( *( ( RDB_CUSTOM_OBJECT_CTRL_TRACK_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+
+                case RDB_PKG_ID_RAY:
+                    // to be implemented
+                    //print( *( ( RDB_RAY_t* ) dataPtr ), ident, csv, csvHeader );
+                    break;
+
+                default:
+                    printedMsg = false;
+                    break;
+            }
+            dataPtr += entryHdr->elementSize;
+            
+            if ( noElements && printedMsg && !csv )
+                fprintf( stderr, "\n" );
+        }            
+    }
+}
+
+void*
+RDBHandler::addPackage( RDB_MSG_t* & msg,   const double & simTime, const unsigned int & simFrame, 
+                        unsigned int pkgId, unsigned int noElements, bool extended, size_t trailingData, 
+                        bool isCustom )
+{
+    if ( !noElements )
+        return 0;
+    
+    bool autoExtend = true;
+    bool newEntry   = true;
+    
+    // get current size
+    uint32_t dataSize      = msg ? msg->hdr.dataSize : 0;
+    uint32_t totalSize     = dataSize + sizeof( RDB_MSG_HDR_t );
+    uint32_t lastEntrySize = 0;  // current size of element if it already exists
+
+    bool newMsg = ( !msg );
+    
+    uint32_t elemSize      = ( isCustom ? 0 : pkgId2size( pkgId, extended ) ) + trailingData;
+    uint32_t addOnDataSize = noElements * elemSize;
+    uint32_t addOnSize     = addOnDataSize;
+    
+    // is the package type and size the same as the last one? If so, extend the previous package instead
+    // of including another entry headerSize
+    if ( autoExtend && !newMsg )
+    {
+        char*                dataPtr   = ( ( char* ) msg ) + msg->hdr.headerSize;
+        RDB_MSG_ENTRY_HDR_t* lastEntry = ( RDB_MSG_ENTRY_HDR_t* ) ( dataPtr );
+        lastEntrySize                  = lastEntry->headerSize + lastEntry->dataSize;
+        uint32_t remainingBytes        = msg->hdr.dataSize - lastEntrySize;
+        
+        while ( remainingBytes )
+        {
+            dataPtr        += lastEntrySize;
+            lastEntry       = ( RDB_MSG_ENTRY_HDR_t* ) ( dataPtr );
+            lastEntrySize   = lastEntry->headerSize + lastEntry->dataSize;
+            remainingBytes -= lastEntrySize;
+        }
+        
+        newEntry = ( lastEntry->pkgId != pkgId ) || ( lastEntry->elementSize != elemSize );
+    }
+    
+    // a new header is required in-between
+    if ( newEntry )
+        addOnSize += sizeof( RDB_MSG_ENTRY_HDR_t );
+    
+    RDB_MSG_t* pNewMsg = ( RDB_MSG_t* ) ( realloc( msg, totalSize + addOnSize ) );
+        
+    if ( !pNewMsg )
+    {
+        fprintf( stderr, "RDBHandler::RDBaddPackage: out of memory.\n" );
+        return 0;
+    }
+    
+    //fprintf( stderr, "RDBHandler::RDBaddPackage: msg = %p, pNewMsg = %p\n", msg, pNewMsg );
+    // now, message can be found at the new location
+    msg = pNewMsg;
+    
+    // set header info (it might be new)
+    msg->hdr.dataSize = dataSize + addOnSize;
+    
+    if ( newMsg )
+    {
+        msg->hdr.headerSize = sizeof( RDB_MSG_HDR_t );
+        msg->hdr.version    = RDB_VERSION;
+        msg->hdr.magicNo    = RDB_MAGIC_NO;
+        msg->hdr.frameNo    = simFrame;
+        msg->hdr.simTime    = simTime;
+    }
+    
+    // now initialize the package itself
+    char* dataPtr = ( char* ) msg;
+    dataPtr += sizeof( RDB_MSG_HDR_t ) + dataSize;
+    
+    if ( !newEntry )
+        dataPtr -= lastEntrySize;
+        
+    RDB_MSG_ENTRY_HDR_t* pEntry = ( RDB_MSG_ENTRY_HDR_t* ) ( dataPtr );
+        
+    // set entry parameters
+    pEntry->headerSize  = sizeof( RDB_MSG_ENTRY_HDR_t );
+    pEntry->dataSize    = newEntry ? addOnDataSize : ( pEntry->dataSize + addOnDataSize );
+    pEntry->elementSize = elemSize;
+    pEntry->pkgId       = pkgId;
+    pEntry->flags       = extended ? RDB_PKG_FLAG_EXTENDED : RDB_PKG_FLAG_NONE;
+
+    // initialize the trailing data
+    dataPtr += newEntry ? pEntry->headerSize : lastEntrySize;
+    
+    if ( addOnDataSize )
+        memset( dataPtr, 0, addOnDataSize );
+    
+    // compute new pointer for insertion of data
+    return dataPtr;
+}
+
+void*
+RDBHandler::addCustomPackage( RDB_MSG_t* & msg,   const double & simTime, const unsigned int & simFrame, 
+                              unsigned int pkgId, unsigned int noElements, size_t elementSize )
+{
+    return addPackage( msg, simTime, simFrame, pkgId, noElements, false, elementSize, true );
+}
+
+void*
+RDBHandler::getFirstEntry( RDB_MSG_t* msg, unsigned int pkgId, unsigned int & noElements, bool extended )
+{
+    RDB_MSG_ENTRY_HDR_t* entryHdr = getEntryHdr( msg, pkgId, extended );
+    
+    if ( !entryHdr )
+        return 0;
+    
+    noElements = entryHdr->elementSize ? ( entryHdr->dataSize / entryHdr->elementSize ) : 0;
+                
+    if ( !noElements && ( pkgId != RDB_PKG_ID_END_OF_FRAME ) && ( pkgId != RDB_PKG_ID_START_OF_FRAME ) )
+        return 0;
+                
+    return ( ( char* ) entryHdr ) + entryHdr->headerSize;
+}
+
+RDB_MSG_ENTRY_HDR_t*
+RDBHandler::getEntryHdr( RDB_MSG_t* msg, unsigned int pkgId, bool extended )
+{
+    if ( !msg )
+        return 0;
+
+    size_t remainingBytes = msg->hdr.dataSize;
+    char* dataPtr = ( char* ) &( msg->entryHdr );
+    
+    while ( remainingBytes )
+    {
+        RDB_MSG_ENTRY_HDR_t* entryHdr = ( RDB_MSG_ENTRY_HDR_t* ) dataPtr;
+
+        if ( entryHdr->pkgId == pkgId )
+        {
+            if ( ( !extended && !( entryHdr->flags & RDB_PKG_FLAG_EXTENDED ) ) || ( extended && ( entryHdr->flags & RDB_PKG_FLAG_EXTENDED ) ) )
+                return entryHdr;
+        }
+        
+        dataPtr += ( entryHdr->headerSize + entryHdr->dataSize );
+        remainingBytes -= ( entryHdr->headerSize + entryHdr->dataSize );
+    }
+    
+    return 0;
+}
+
+char*
+RDBHandler::getIdentString( unsigned char ident )
+{
+    unsigned int lastIdent = 255;
+    static char  sIdentStr[256];
+
+    if ( ident != lastIdent )
+    {
+        memset( sIdentStr, 0, 256 );
+        memset( sIdentStr, ' ', ident );
+    }
+    
+    return sIdentStr;
+}
+
+void
+RDBHandler::print( const RDB_GEOMETRY_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,", "dimX", "dimY", "dimZ", "offX", "offY", "offZ" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,", info.dimX, info.dimY, info.dimZ, info.offX, info.offY, info.offZ );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%sdim x / y / z = %.3f / %.3f / %.3f\n", identStr, info.dimX, info.dimY, info.dimZ ); 
+    fprintf( stderr, "%soff x / y / z = %.3f / %.3f / %.3f\n", identStr, info.offX, info.offY, info.offZ ); 
+}
+
+void
+RDBHandler::print( const RDB_COORD_SYSTEM_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,", "id" );
+        print( info.pos, ident + 4, csv, csvHeader );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,", info.id );
+        print( info.pos, ident + 4, csv, csvHeader );
+        return;
+    }
+
+    fprintf( stderr, "%sid = %d\n", getIdentString( ident ), info.id ); 
+    fprintf( stderr, "%sposition\n", getIdentString( ident ) ); 
+    print( info.pos, ident + 4 );
+}
+
+void
+RDBHandler::print( const RDB_COORD_t & info, unsigned char ident, bool csv, bool csvHeader )
+{ 
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,", "x", "y", "z", "h", "p", "r", "flags", "type", "system" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%#23x,%23d,%23d,", info.x, info.y, info.z, 
+                                                                                        info.h, info.p, info.r,
+                                                                                        info.flags, info.type, info.system );
+        return;
+    }
+
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%sx/y/z = %.3f / %.3f / %.3f\n", identStr, info.x, info.y, info.z ); 
+    fprintf( stderr, "%sh/p/r = %.3f / %.3f / %.3f\n", identStr, info.h, info.p, info.r ); 
+    fprintf( stderr, "%sflags = 0x%x, type = %s, system = %d\n", identStr, info.flags, coordType2string( info.type ).c_str(), info.system ); 
+}
+
+void
+RDBHandler::print( const RDB_ROAD_POS_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,", 
+                         "playerId", "roadId", "laneId", "flags", "roadS", "roadT", 
+                         "laneOffset", "hdgRel", "pitchRel", "rollRel", "roadType", "pathS" );        
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%23d,%23d,%#23x,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%23d,%+.16e,", 
+                         info.playerId, info.roadId, info.laneId, info.flags, info.roadS, info.roadT, 
+                         info.laneOffset, info.hdgRel, info.pitchRel, info.rollRel, info.roadType, info.pathS );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+
+    fprintf( stderr, "%splayerId roadId laneId flags      roadS       roadT   laneOffset     hdgRel   pitchRel    rollRel roadType      pathS\n", identStr );
+    fprintf( stderr, "%s%8d %6d %6d  0x%02x %+10.3f %+10.3f   %+10.3f %+10.3f %+10.3f %+10.3f %9d %+10.3f\n", 
+                      identStr, info.playerId, info.roadId, info.laneId, info.flags, 
+                      info.roadS, info.roadT, info.laneOffset, info.hdgRel, info.pitchRel, 
+                      info.rollRel, info.roadType, info.pathS ); 
+/*
+    fprintf( stderr, "%splayerId   = %d\n",    identStr, info.playerId   ); 
+    fprintf( stderr, "%sroadId     = %d\n",    identStr, info.roadId     ); 
+    fprintf( stderr, "%slaneId     = %d\n",    identStr, info.laneId     ); 
+    fprintf( stderr, "%sflags      = 0x%x\n",  identStr, info.flags      ); 
+    fprintf( stderr, "%sroadS      = %.3f\n",  identStr, info.roadS      ); 
+    fprintf( stderr, "%sroadT      = %.3f\n",  identStr, info.roadT      ); 
+    fprintf( stderr, "%slaneOffset = %.3f\n",  identStr, info.laneOffset ); 
+    fprintf( stderr, "%shdgRel     = %.3f\n",  identStr, info.hdgRel     ); 
+    fprintf( stderr, "%spitchRel   = %.3f\n",  identStr, info.pitchRel   ); 
+    fprintf( stderr, "%srollRel    = %.3f\n",  identStr, info.rollRel    ); 
+    fprintf( stderr, "%sroadType   = %d\n",    identStr, info.roadType   ); 
+    fprintf( stderr, "%spathS      = %.3f\n",  identStr, info.pathS      ); 
+*/
+}
+
+void
+RDBHandler::print( const RDB_LANE_INFO_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,", 
+                         "roadId", "id", "neighborMask", "leftLaneId", "rightLaneId", "borderType", "material", 
+                         "status", "width", "curvVert", "curvVertDot", "curvHor", "curvHorDot", "playerId" );        
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%23d,%#23x,%23d,%23d,%23d,%23d,%23d,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%23d,", 
+                         info.roadId, info.id, info.neighborMask, info.leftLaneId, info.rightLaneId, info.borderType, info.material, 
+                         info.status, info.width, info.curvVert, info.curvVertDot, info.curvHor, info.curvHorDot, info.playerId );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%splayerId roadId     id neighborMask leftLaneId rightLaneId   borderType material   status width    curvVert curvVertDot      curvHor  curvHorDot    type\n", identStr );
+    fprintf( stderr, "%s%8d %6d %6d  0x%02x %+10d %+10d %+8d %+8d %+8d %+10.3f %+10.6f %+10.6f %+10.6f %+10.6f %+8d\n", 
+                     identStr, info.playerId, info.roadId, info.id, info.neighborMask, info.leftLaneId, info.rightLaneId, info.borderType, 
+                     info.material, info.status, info.width, info.curvVert, info.curvVertDot, info.curvHor, info.curvHorDot, info.type );
+
+/*
+    fprintf( stderr, "%sroadId       = %d\n",    identStr, info.roadId       ); 
+    fprintf( stderr, "%sid           = %d\n",    identStr, info.id           ); 
+    fprintf( stderr, "%sneighborMask = 0x%x\n",  identStr, info.neighborMask ); 
+    fprintf( stderr, "%sleftLaneId   = %d\n",    identStr, info.leftLaneId   ); 
+    fprintf( stderr, "%srightLaneId  = %d\n",    identStr, info.rightLaneId  ); 
+    fprintf( stderr, "%sborderType   = %d\n",    identStr, info.borderType   ); 
+    fprintf( stderr, "%smaterial     = %d\n",    identStr, info.material     ); 
+    fprintf( stderr, "%sstatus       = %d\n",    identStr, info.status       ); 
+    fprintf( stderr, "%swidth        = %.3f\n",  identStr, info.width        ); 
+    fprintf( stderr, "%scurvVert     = %.6lf\n", identStr, info.curvVert     ); 
+    fprintf( stderr, "%scurvVertDot  = %.6lf\n", identStr, info.curvVertDot  ); 
+    fprintf( stderr, "%scurvHor      = %.6lf\n", identStr, info.curvHor      ); 
+    fprintf( stderr, "%scurvHorDot   = %.6lf\n", identStr, info.curvHorDot   ); 
+    fprintf( stderr, "%splayerId     = %d\n",    identStr, info.playerId     );
+    */
+}
+
+void
+RDBHandler::print( const RDB_ROADMARK_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,", 
+                         "playerId", "id", "prevId", "nextId", "lateralDist", "yawRel", "curvHor", "curvHorDot", 
+                         "startDx", "previewDx", "width", "height", "curvVert", "curvVertDot", "type", "color" );        
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%23d,%23d,%23d,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%23d,%23d,", 
+                         info.playerId, info.id, info.prevId, info.nextId, info.lateralDist, info.yawRel, info.curvHor, info.curvHorDot, 
+                         info.startDx, info.previewDx, info.width, info.height, info.curvVert, info.curvVertDot,  info.type, info.color );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%splayerId     = %d\n",    identStr, info.playerId     ); 
+    fprintf( stderr, "%sid           = %d\n",    identStr, info.id           ); 
+    fprintf( stderr, "%sprevId       = %d\n",    identStr, info.prevId       ); 
+    fprintf( stderr, "%snextId       = %d\n",    identStr, info.nextId       ); 
+    fprintf( stderr, "%slateralDist  = %.3f\n",  identStr, info.lateralDist  ); 
+    fprintf( stderr, "%syawRel       = %.3f\n",  identStr, info.yawRel       ); 
+    fprintf( stderr, "%scurvHor      = %.6lf\n", identStr, info.curvHor      ); 
+    fprintf( stderr, "%scurvHorDot   = %.6lf\n", identStr, info.curvHorDot   ); 
+    fprintf( stderr, "%sstartDx      = %.3f\n",  identStr, info.startDx      ); 
+    fprintf( stderr, "%spreviewDx    = %.3f\n",  identStr, info.previewDx    ); 
+    fprintf( stderr, "%swidth        = %.3f\n",  identStr, info.width        ); 
+    fprintf( stderr, "%sheight       = %.3f\n",  identStr, info.height       ); 
+    fprintf( stderr, "%scurvVert     = %.6lf\n", identStr, info.curvVert     ); 
+    fprintf( stderr, "%scurvVertDot  = %.6lf\n", identStr, info.curvVertDot  ); 
+    fprintf( stderr, "%stype         = %d\n",    identStr, info.type         ); 
+    fprintf( stderr, "%scolor        = %d\n",    identStr, info.color        ); 
+    fprintf( stderr, "%sroadId       = %d\n",    identStr, info.roadId       ); 
+    fprintf( stderr, "%slaneId       = %d\n",    identStr, info.laneId       ); 
+    fprintf( stderr, "%snoDataPoints = %d\n",    identStr, info.noDataPoints );
+
+    if ( info.noDataPoints )
+    {
+        RDB_POINT_t* pt = ( RDB_POINT_t* ) ( ( ( unsigned char* ) ( &info ) ) + sizeof( RDB_ROADMARK_t ) );
+                            
+        for ( int i = 0; i < info.noDataPoints; i++ )
+            print( pt[i], ident + 4 );
+    }
+}
+
+void
+RDBHandler::print( const RDB_OBJECT_CFG_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,", 
+                         "id", "category", "type", "modelId", "name", "modelName", "fileName", "flags" );        
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%23d,%23d,%23d,%23s,%23s,%23s,%#23x,", 
+                         info.id, info.category, info.type, info.modelId, info.name, info.modelName, info.fileName, info.flags );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%sid        = %d\n",    identStr, info.id        ); 
+    fprintf( stderr, "%scategory  = %s\n",    identStr, objectCategory2string( info.category ).c_str() ); 
+    fprintf( stderr, "%stype      = %s\n",    identStr, objectType2string( info.type ).c_str()         ); 
+    fprintf( stderr, "%smodelId   = %d\n",    identStr, info.modelId   ); 
+    fprintf( stderr, "%sname      = %s\n",    identStr, info.name      ); 
+    fprintf( stderr, "%smodelName = %s\n",    identStr, info.modelName ); 
+    fprintf( stderr, "%sfileName  = %s\n",    identStr, info.fileName  ); 
+    fprintf( stderr, "%sflags     = 0x%x\n",  identStr, info.flags     ); 
+}
+
+void
+RDBHandler::print( const RDB_OBJECT_STATE_t & state, bool extended, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,", "objectId", "category", "type", "vismask", "name" );
+        
+        print( state.base.geo, ident + 4,  csv, csvHeader );
+        print( state.base.pos, ident + 4,  csv, csvHeader );
+
+        if ( !extended )
+            return;
+        
+        print( state.ext.speed, ident + 4, csv, csvHeader );
+        print( state.ext.accel, ident + 4, csv, csvHeader );
+        
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%+23d,%23d,%23d,%#23x,%23s,", state.base.id, state.base.category, state.base.type,
+                                                       state.base.visMask, state.base.name);
+        print( state.base.geo, ident + 4,  csv, csvHeader );
+        print( state.base.pos, ident + 4,  csv, csvHeader );
+
+        if ( !extended )
+            return;
+        
+        print( state.ext.speed, ident + 4, csv, csvHeader );
+        print( state.ext.accel, ident + 4, csv, csvHeader );
+
+        return;
+    }
+
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%sid       = %d\n",   identStr, state.base.id       ); 
+    fprintf( stderr, "%scategory = %s\n",   identStr, objectCategory2string( state.base.category ).c_str() ); 
+    fprintf( stderr, "%stype     = %s\n",   identStr, objectType2string( state.base.type ).c_str()         ); 
+    fprintf( stderr, "%svisMask  = 0x%x\n", identStr, state.base.visMask  ); 
+    fprintf( stderr, "%sname     = %s\n",   identStr, state.base.name     ); 
+    
+    fprintf( stderr, "%sgeometry\n", getIdentString( ident ) ); 
+    print( state.base.geo, ident + 4 );
+    
+    fprintf( stderr, "%sposition\n", getIdentString( ident ) ); 
+    print( state.base.pos, ident + 4 );
+    
+    identStr = getIdentString( ident );
+    fprintf( stderr, "%sparent     = %d\n",   identStr, state.base.parent     ); 
+    fprintf( stderr, "%scfgFlags   = 0x%x\n", identStr, state.base.cfgFlags   ); 
+    fprintf( stderr, "%scfgModelId = %d\n",   identStr, state.base.cfgModelId ); 
+    
+    if ( !extended )
+        return;
+
+    fprintf( stderr, "%sspeed\n", getIdentString( ident ) ); 
+    print( state.ext.speed, ident + 4 );
+    
+    fprintf( stderr, "%sacceleration\n", getIdentString( ident ) ); 
+    print( state.ext.accel, ident + 4 );
+    
+    fprintf( stderr, "%sdistance = %.3f\n", getIdentString( ident ), state.ext.traveledDist ); 
+}
+
+void
+RDBHandler::print( const RDB_VEHICLE_SYSTEMS_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,", 
+                         "playerId", "lightMask", "steering", "steeringWheelTorque" );        
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%#23x,%+.16e,%+.16e,", 
+                         info.playerId, info.lightMask, info.steering, info.steeringWheelTorque );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%splayerId            = %d\n",   identStr, info.playerId            ); 
+    fprintf( stderr, "%slightMask           = 0x%x\n", identStr, info.lightMask           ); 
+    fprintf( stderr, "%ssteering            = %.3f\n", identStr, info.steering            ); 
+    fprintf( stderr, "%ssteeringWheelTorque = %.3f\n", identStr, info.steeringWheelTorque ); 
+}
+
+void
+RDBHandler::print( const RDB_VEHICLE_SETUP_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,", 
+                         "playerId", "mass", "wheelBase" );        
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%+.16e,%+.16e,", 
+                         info.playerId, info.mass, info.wheelBase );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%splayerId  = %d\n",   identStr, info.playerId  ); 
+    fprintf( stderr, "%smass      = %.3f\n", identStr, info.mass      ); 
+    fprintf( stderr, "%swheelBase = %.3f\n", identStr, info.wheelBase ); 
+}
+
+void
+RDBHandler::print( const RDB_ENGINE_t & info, bool extended, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,", 
+                         "playerId", "rps", "load" );
+        
+        if ( !extended )
+            return;
+        
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,%23s,", 
+                         "rpsStart", "torque", "torqueInner", "torqueMax", "torqueFriction", "fuelCurrent", "fuelAverage" );
+
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%+.16e,%+.16e,", 
+                         info.base.playerId, info.base.rps, info.base.load );
+
+        if ( !extended )
+            return;
+        
+        fprintf( stderr, "%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,", 
+                         info.ext.rpsStart, info.ext.torque, info.ext.torqueInner, info.ext.torqueMax, info.ext.torqueFriction, 
+                         info.ext.fuelCurrent, info.ext.fuelAverage );
+
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%splayerId        = %d\n",   identStr, info.base.playerId ); 
+    fprintf( stderr, "%srps             = %.3f\n", identStr, info.base.rps      ); 
+    fprintf( stderr, "%sload            = %.3f\n", identStr, info.base.load     );
+    
+    if ( !extended )
+        return;
+
+    fprintf( stderr, "%srpsStart        = %.3f\n", identStr, info.ext.rpsStart       ); 
+    fprintf( stderr, "%storque          = %.3f\n", identStr, info.ext.torque         );
+    fprintf( stderr, "%storqueInner     = %.3f\n", identStr, info.ext.torqueInner    ); 
+    fprintf( stderr, "%storqueMax       = %.3f\n", identStr, info.ext.torqueMax      );
+    fprintf( stderr, "%storqueFriction  = %.3f\n", identStr, info.ext.torqueFriction ); 
+    fprintf( stderr, "%sfuelCurrent     = %.3f\n", identStr, info.ext.fuelCurrent    );
+    fprintf( stderr, "%sfuelAverage     = %.3f\n", identStr, info.ext.fuelAverage    ); 
+}
+
+void
+RDBHandler::print( const RDB_DRIVETRAIN_t & info, bool extended, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,", 
+                         "playerId", "gearBoxType", "driveTrainType", "gear" );
+        
+        if ( !extended )
+            return;
+        
+        fprintf( stderr, "%23s,%23s,%23s,", 
+                         "torqueGearBoxIn", "torqueCenterDiffOut", "torqueShaft" );
+
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%23d,%23d,%23d,", 
+                         info.base.playerId, info.base.gearBoxType, info.base.driveTrainType, info.base.gear );
+
+        if ( !extended )
+            return;
+        
+        fprintf( stderr, "%+.16e,%+.16e,%+.16e,", 
+                         info.ext.torqueGearBoxIn, info.ext.torqueCenterDiffOut, info.ext.torqueShaft );
+
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%splayerId            = %d\n",   identStr, info.base.playerId       ); 
+    fprintf( stderr, "%sgearBoxType         = %d\n",   identStr, info.base.gearBoxType    ); 
+    fprintf( stderr, "%sdriveTrainType      = %d\n",   identStr, info.base.driveTrainType ); 
+    fprintf( stderr, "%sgear                = %d\n",   identStr, info.base.gear           ); 
+
+    if ( !extended )
+        return;
+
+    fprintf( stderr, "%storqueGearBoxIn     = %.3f\n", identStr, info.ext.torqueGearBoxIn     ); 
+    fprintf( stderr, "%storqueCenterDiffOut = %.3f\n", identStr, info.ext.torqueCenterDiffOut );
+    fprintf( stderr, "%storqueShaft         = %.3f\n", identStr, info.ext.torqueShaft         ); 
+}
+
+void
+RDBHandler::print( const RDB_WHEEL_t & info, bool extended, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,", 
+                         "playerId", "id", "flags", "radiusStatic", "springCompression", "rotAngle", "slip", "steeringAngle" );
+        
+        if ( !extended )
+            return;
+        
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,", 
+                         "vAngular", "forceZ", "forceLat", "forceLong", "forceTireWheelX", "forceTireWheelY", "forceTireWheelZ", 
+                         "radiusDynamic", "brakePressure", "torqueDriveShaft", "damperSpeed", "vAngular", "forceZ", "forceLat", "forceLong" );
+
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%23d,%#23x,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,", 
+                         info.base.playerId, info.base.id, info.base.flags, info.base.radiusStatic, 
+                         info.base.springCompression, info.base.rotAngle, info.base.slip, info.base.steeringAngle );
+
+        if ( !extended )
+            return;
+        
+        fprintf( stderr, "%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,", 
+                         info.ext.vAngular, info.ext.forceZ, info.ext.forceLat, info.ext.forceLong, info.ext.forceTireWheelXYZ[0], 
+                         info.ext.forceTireWheelXYZ[1], info.ext.forceTireWheelXYZ[2], info.ext.radiusDynamic, info.ext.brakePressure, info.ext.torqueDriveShaft, 
+                         info.ext.damperSpeed, info.ext.vAngular, info.ext.forceZ, info.ext.forceLat, info.ext.forceLong );
+
+        return;
+    }
+
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%splayerId             = %d\n",   identStr, info.base.playerId          ); 
+    fprintf( stderr, "%sid                   = %d\n",   identStr, info.base.id                ); 
+    fprintf( stderr, "%sflags                = 0x%x\n", identStr, info.base.flags             ); 
+    fprintf( stderr, "%sradiusStatic         = %.3f\n", identStr, info.base.radiusStatic      ); 
+    fprintf( stderr, "%sspringCompression    = %.3f\n", identStr, info.base.springCompression ); 
+    fprintf( stderr, "%srotAngle             = %.3f\n", identStr, info.base.rotAngle          ); 
+    fprintf( stderr, "%sslip                 = %.3f\n", identStr, info.base.slip              ); 
+    fprintf( stderr, "%ssteeringAngle        = %.3f\n", identStr, info.base.steeringAngle     ); 
+
+    if ( !extended )
+        return;
+
+    fprintf( stderr, "%svAngular             = %.3f\n", identStr, info.ext.vAngular             ); 
+    fprintf( stderr, "%sforceZ               = %.3f\n", identStr, info.ext.forceZ               ); 
+    fprintf( stderr, "%sforceLat             = %.3f\n", identStr, info.ext.forceLat             ); 
+    fprintf( stderr, "%sforceLong            = %.3f\n", identStr, info.ext.forceLong            ); 
+    fprintf( stderr, "%sforceTireWheelXYZ[0] = %.3f\n", identStr, info.ext.forceTireWheelXYZ[0] ); 
+    fprintf( stderr, "%sforceTireWheelXYZ[1] = %.3f\n", identStr, info.ext.forceTireWheelXYZ[1] ); 
+    fprintf( stderr, "%sforceTireWheelXYZ[2] = %.3f\n", identStr, info.ext.forceTireWheelXYZ[2] ); 
+    fprintf( stderr, "%sradiusDynamic        = %.3f\n", identStr, info.ext.radiusDynamic        ); 
+    fprintf( stderr, "%sbrakePressure        = %.3f\n", identStr, info.ext.brakePressure        ); 
+    fprintf( stderr, "%storqueDriveShaft     = %.3f\n", identStr, info.ext.torqueDriveShaft     ); 
+    fprintf( stderr, "%sdamperSpeed          = %.3f\n", identStr, info.ext.damperSpeed          ); 
+    fprintf( stderr, "%svAngular             = %.3f\n", identStr, info.ext.vAngular             ); 
+    fprintf( stderr, "%sforceZ               = %.3f\n", identStr, info.ext.forceZ               ); 
+    fprintf( stderr, "%sforceLat             = %.3f\n", identStr, info.ext.forceLat             ); 
+    fprintf( stderr, "%sforceLong            = %.3f\n", identStr, info.ext.forceLong            ); 
+}
+
+void
+RDBHandler::print( const RDB_PED_ANIMATION_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s", "pedAnimation" );
+        return;
+    }
+
+    if ( csv )
+    {
+        fprintf( stderr, "%23s", "n/a" );
+        return;
+    }
+
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%splayerId = %d\n",   identStr, info.playerId ); 
+
+    fprintf( stderr, "%sposition\n", identStr ); 
+    print( info.pos, ident + 4 );
+    
+    fprintf( stderr, "%snoCoords = %d\n", getIdentString( ident ), info.noCoords ); 
+    fprintf( stderr, "%sdataSize = %d\n", getIdentString( ident ), info.dataSize ); 
+}
+
+void
+RDBHandler::print( const RDB_SENSOR_STATE_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,", 
+                         "id", "type", "hostCategory", "name", "fovH", "fovV", "clipNear", "clipFar" );
+        print( info.pos,            ident + 4, csv, csvHeader );
+        print( info.originCoordSys, ident + 4, csv, csvHeader );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%23d,%23d,%23s,%+.16e,%+.16e,%+.16e,%+.16e,", 
+                 info.id, info.type, info.hostCategory, info.name, info.fovHV[0], info.fovHV[1], info.clipNF[0], info.clipNF[1] );
+        print( info.pos,            ident + 4, csv, csvHeader );
+        print( info.originCoordSys, ident + 4, csv, csvHeader );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%sid           = %d\n",   identStr, info.id           ); 
+    fprintf( stderr, "%stype         = %d\n",   identStr, info.type         ); 
+    fprintf( stderr, "%shostCategory = %d\n",   identStr, info.hostCategory ); 
+    fprintf( stderr, "%sname         = %s\n",   identStr, info.name         ); 
+    fprintf( stderr, "%sfovHV[0]     = %.3f\n", identStr, info.fovHV[0]     ); 
+    fprintf( stderr, "%sfovHV[1]     = %.3f\n", identStr, info.fovHV[1]     ); 
+    fprintf( stderr, "%sfovOffHV[0]  = %.3f\n", identStr, info.fovOffHV[0]  ); 
+    fprintf( stderr, "%sfovOffHV[1]  = %.3f\n", identStr, info.fovOffHV[1]  ); 
+    fprintf( stderr, "%sclipNF[0]    = %.3f\n", identStr, info.clipNF[0]    ); 
+    fprintf( stderr, "%sclipNF[1]    = %.3f\n", identStr, info.clipNF[1]    ); 
+
+    fprintf( stderr, "%sposition\n", identStr ); 
+    print( info.pos, ident + 4 );
+    
+    fprintf( stderr, "%soriginCoordSys\n", getIdentString( ident ) ); 
+    print( info.originCoordSys, ident + 4 );
+}
+
+void
+RDBHandler::print( const RDB_SENSOR_OBJECT_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,%23s,", 
+                         "category", "type", "flags", "id", "sensorId", "dist", "occlusion" );
+        print( info.sensorPos, ident + 4, csv, csvHeader );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%23d,%#23x,%23d,%23d,%+.16e,%23d,", 
+                 info.category, info.type, info.flags, info.id, info.sensorId, info.dist, info.occlusion );
+        print( info.sensorPos, ident + 4, csv, csvHeader );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%scategory = %d\n",    identStr, info.category ); 
+    fprintf( stderr, "%stype     = %d\n",    identStr, info.type     ); 
+    fprintf( stderr, "%sflags    = 0x%x\n",  identStr, info.flags    ); 
+    fprintf( stderr, "%sid       = %d\n",    identStr, info.id       ); 
+    fprintf( stderr, "%ssensorId = %d\n",    identStr, info.sensorId ); 
+    fprintf( stderr, "%sdist     = %.3lf\n", identStr, info.dist     ); 
+
+    fprintf( stderr, "%ssensorPos\n", identStr ); 
+    print( info.sensorPos, ident + 4 );
+    
+    fprintf( stderr, "%socclusion = %d\n", getIdentString( ident ), info.occlusion ); 
+}
+
+void
+RDBHandler::print( const RDB_CAMERA_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,", 
+                         "id", "width", "height", "clipNear", "clipFar", "focalX", "focalY", "principalX", "principalY" );
+        print( info.pos, ident + 4, csv, csvHeader );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%23d,%23d,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,", 
+                 info.id, info.width, info.height, info.clipNear, info.clipFar, info.focalX, info.focalY, info.principalX, info.principalY );
+        print( info.pos, ident + 4, csv, csvHeader );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%sid         = %d\n",   identStr, info.id         );
+    fprintf( stderr, "%swidth      = %d\n",   identStr, info.width      );
+    fprintf( stderr, "%sheight     = %d\n",   identStr, info.height     );
+    fprintf( stderr, "%sclipNear   = %.3f\n", identStr, info.clipNear   );
+    fprintf( stderr, "%sclipFar    = %.3f\n", identStr, info.clipFar    );
+    fprintf( stderr, "%sfocalX     = %.3f\n", identStr, info.focalX     );
+    fprintf( stderr, "%sfocalY     = %.3f\n", identStr, info.focalY     );
+    fprintf( stderr, "%sprincipalX = %.3f\n", identStr, info.principalX );
+    fprintf( stderr, "%sprincipalY = %.3f\n", identStr, info.principalY );
+
+    fprintf( stderr, "%sposition\n", identStr ); 
+    print( info.pos, ident + 4 );
+}
+
+void
+RDBHandler::print( const RDB_CONTACT_POINT_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,", "id", "flags" );
+        print( info.roadDataIn, ident + 4, csv, csvHeader );
+        fprintf( stderr, "%23s,%23s,", "friction", "playerId" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%#23x,", info.id, info.flags );
+        print( info.roadDataIn, ident + 4, csv, csvHeader );
+        fprintf( stderr, "%+.16e,%23d,", info.friction, info.playerId );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%sid        = %d\n",   identStr, info.id    );
+    fprintf( stderr, "%sflags     = 0x%x\n", identStr, info.flags );
+
+    fprintf( stderr, "%sroadDataIn\n", identStr ); 
+    print( info.roadDataIn, ident + 4 );
+
+    fprintf( stderr, "%sfriction  = %.3f\n", getIdentString( ident ), info.friction );
+    fprintf( stderr, "%splayerId  = %d\n",   getIdentString( ident ), info.playerId );
+}
+
+void
+RDBHandler::print( const RDB_TRAFFIC_SIGN_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,", "id", "playerId", "roadDist" );
+        print( info.pos, ident + 4, csv, csvHeader );
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,", "type", "subType", "value", "state", 
+                                                                          "readability", "occlusion", "addOnId",
+                                                                          "minLane", "maxLane" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%23d,%+.16e,", info.id, info.playerId, info.roadDist );
+        print( info.pos, ident + 4, csv, csvHeader );
+        fprintf( stderr, "%23d,%23d,%+.16e,%23d,%23d,%23d,%23d,%23d,%23d,", 
+                         info.type, info.subType, info.value, info.state, info.readability, 
+                         info.occlusion, info.addOnId, info.minLane, info.maxLane );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%sid          = %d\n",   identStr, info.id       );
+    fprintf( stderr, "%splayerId    = %d\n",   identStr, info.playerId );
+    fprintf( stderr, "%sroadDist    = %.3f\n", identStr, info.roadDist );
+
+    fprintf( stderr, "%sposition\n", identStr ); 
+    print( info.pos, ident + 4 );
+    
+    identStr = getIdentString( ident );
+    fprintf( stderr, "%stype        = %d\n",   identStr, info.type        );
+    fprintf( stderr, "%ssubType     = %d\n",   identStr, info.subType     );
+    fprintf( stderr, "%svalue       = %.3f\n", identStr, info.value       );
+    fprintf( stderr, "%sstate       = %d\n",   identStr, info.state       );
+    fprintf( stderr, "%sreadability = %d\n",   identStr, info.readability );
+    fprintf( stderr, "%socclusion   = %d\n",   identStr, info.occlusion   );
+    fprintf( stderr, "%saddOnId     = %d\n",   identStr, info.addOnId     );
+    fprintf( stderr, "%sminLane     = %d\n",   identStr, info.minLane     );
+    fprintf( stderr, "%smaxLane     = %d\n",   identStr, info.maxLane     );
+}
+
+void
+RDBHandler::print( const RDB_ROAD_STATE_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,", "playerId", "wheelId", "roadId", "defaultSpeed", "waterLevel", "eventMask" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%23d,%23d,%+.16e,%+.16e,%#23x,", info.playerId, info.wheelId, info.roadId, info.defaultSpeed, info.waterLevel, info.eventMask );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%splayerId     = %d\n",   identStr, info.playerId     );
+    fprintf( stderr, "%swheelId      = %d\n",   identStr, info.wheelId      );
+    fprintf( stderr, "%sroadId       = %d\n",   identStr, info.roadId       );
+    fprintf( stderr, "%sdefaultSpeed = %.3f\n", identStr, info.defaultSpeed );
+    fprintf( stderr, "%swaterLevel   = %.3f\n", identStr, info.waterLevel   );
+    fprintf( stderr, "%seventMask    = 0x%x\n", identStr, info.eventMask    );
+}
+void
+RDBHandler::print( const RDB_IMAGE_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,", 
+                         "id", "width", "height", "pixelSize", "pixelFormat", 
+                         "cameraId", "imgSize", "colorR", "colorG", "colorB", "colorA" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%23d,%23d,%23d,%23d,%23d,%23d,%23d,%23d,%23d,%23d,", 
+                         info.id, info.width, info.height, info.pixelSize, info.pixelFormat, info.cameraId, 
+                         info.imgSize, info.color[0], info.color[1], info.color[2], info.color[3] );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%sid               = %d\n",          identStr, info.id          ); 
+    fprintf( stderr, "%swidth            = %d\n",          identStr, info.width       ); 
+    fprintf( stderr, "%sheight           = %d\n",          identStr, info.height      ); 
+    fprintf( stderr, "%spixelSize        = %d\n",          identStr, info.pixelSize   ); 
+    fprintf( stderr, "%spixelFormat      = %d\n",          identStr, info.pixelFormat ); 
+    fprintf( stderr, "%scameraId         = %d\n",          identStr, info.cameraId    ); 
+    fprintf( stderr, "%simgSize          = %d\n",          identStr, info.imgSize     ); 
+    fprintf( stderr, "%scolor            = %d/%d/%d/%d\n", identStr, info.color[0], info.color[1], info.color[2], info.color[3] ); 
+}
+
+void
+RDBHandler::print( const RDB_LIGHT_SOURCE_t & info, bool extended, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,", 
+                         "id", "templateId", "state", "playerId", "flags" );
+
+        print( info.base.pos, ident + 4, csv, csvHeader );
+        
+        if ( !extended )
+            return;
+    
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,", 
+                         "near", "far", "left", "right", "bottom", "top", "int1", "int2", "int3", "atten0", "atten1", "atten2" );
+
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%23d,%23d,%23d,%23d,", 
+                         info.base.id, info.base.templateId, info.base.state, info.base.playerId, info.base.flags );
+        
+        print( info.base.pos, ident + 4, csv, csvHeader );
+
+        if ( !extended )
+            return;
+    
+        fprintf( stderr, "%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,", 
+                         info.ext.nearFar[0], info.ext.nearFar[1], info.ext.frustumLRBT[0], info.ext.frustumLRBT[1], info.ext.frustumLRBT[2], info.ext.frustumLRBT[3],
+                         info.ext.intensity[0], info.ext.intensity[1], info.ext.intensity[2], info.ext.atten[0], info.ext.atten[1], info.ext.atten[2] );
+        
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%sid         = %d\n", identStr, info.base.id         ); 
+    fprintf( stderr, "%stemplateId = %d\n", identStr, info.base.templateId ); 
+    fprintf( stderr, "%sstate      = %d\n", identStr, info.base.state      ); 
+    fprintf( stderr, "%splayerId   = %d\n", identStr, info.base.playerId   ); 
+
+    fprintf( stderr, "%sposition\n", identStr ); 
+    print( info.base.pos, ident + 4 );
+    
+    identStr = getIdentString( ident );
+    fprintf( stderr, "%sflags      = 0x%x\n", identStr, info.base.flags );
+    
+    if ( !extended )
+        return;
+    
+    fprintf( stderr, "%snearFar     = %.3f / %.3f\n",               identStr, info.ext.nearFar[0], info.ext.nearFar[1] ); 
+    fprintf( stderr, "%sfrustumLRBT = %.3f / %.3f / %.3f / %.3f\n", identStr, info.ext.frustumLRBT[0], info.ext.frustumLRBT[1], info.ext.frustumLRBT[2], info.ext.frustumLRBT[3] ); 
+    fprintf( stderr, "%sintensity   = %.3f / %.3f / %.3f\n",        identStr, info.ext.intensity[0], info.ext.intensity[1], info.ext.intensity[2] ); 
+    fprintf( stderr, "%satten       = %.3f / %.3f / %.3f\n",        identStr, info.ext.atten[0], info.ext.atten[1], info.ext.atten[2] );                                         
+}                                                                                                                                                                                
+
+void
+RDBHandler::print( const RDB_ENVIRONMENT_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,", 
+                         "visibility", "timeOfDay", "brightness", "precipitation", "cloudState", "flags" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%+.16e,%23d,%+.16e,%23d,%23d,%#23x,", 
+                         info.visibility, info.timeOfDay, info.brightness, info.precipitation, info.cloudState, info.flags );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%svisibility    = %.3f\n", identStr, info.visibility    ); 
+    fprintf( stderr, "%stimeOfDay     = %d\n",   identStr, info.timeOfDay     ); 
+    fprintf( stderr, "%sbrightness    = %.3f\n", identStr, info.brightness    ); 
+    fprintf( stderr, "%sprecipitation = %d\n",   identStr, info.precipitation ); 
+    fprintf( stderr, "%scloudState    = %d\n",   identStr, info.cloudState    ); 
+    fprintf( stderr, "%sflags         = 0x%x\n", identStr, info.flags         ); 
+}
+
+void
+RDBHandler::print( const RDB_TRIGGER_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,", "deltaT", "frameNo" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%+.16e,%23d,", info.deltaT, info.frameNo );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%sdeltaT  = %.3f\n", identStr, info.deltaT  ); 
+    fprintf( stderr, "%sframeNo = %d\n",   identStr, info.frameNo ); 
+}
+
+void
+RDBHandler::print( const RDB_DRIVER_CTRL_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s",
+                         "playerId", "steeringWheelV", "steeringWheel", "steeringSpeedV", "steeringSpeed",  
+                         "throttlePedalV", "throttlePedal", "brakePedalV", "brakePedal",
+                         "clutchPedalV", "clutchPedal", "accelTgtV", "accelTgt",
+                         "steeringTgtV", "steeringTgt", "curvatureTgtV", "curvatureTgt",
+                         "steeringTorqueV", "steeringTorque", "engineTorqueTgtV", "engineTorqueTgt",
+                         "speedTgtV", "speedTgt", "gearV", "gear", "flagsV", "flags",
+                         "sourceId", "validityFlags" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%23d,%+.16e,%23d,%+.16e,%23d,%+.16e,%23d,%+.16e,%23d,%+.16e,%23d,%+.16e,%23d,%+.16e,%23d,%+.16e,%23d,%+.16e,%23d,%+.16e,%23d,%+.16e,%23d,%23d,%23d,%#23x,%23d,%#23x,",
+                 info.playerId, 
+                 ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_STEERING_WHEEL )  ? 1 : 0, info.steeringWheel,
+                 ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_STEERING_SPEED )  ? 1 : 0, info.steeringSpeed, 
+                 ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_THROTTLE )        ? 1 : 0, info.throttlePedal, 
+                 ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_BRAKE )           ? 1 : 0, info.brakePedal, 
+                 ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_CLUTCH )          ? 1 : 0, info.clutchPedal, 
+                 ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_TGT_ACCEL )       ? 1 : 0, info.accelTgt, 
+                 ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_TGT_STEERING )    ? 1 : 0, info.steeringTgt, 
+                 ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_CURVATURE )       ? 1 : 0, info.curvatureTgt, 
+                 ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_STEERING_TORQUE ) ? 1 : 0, info.steeringTorque, 
+                 ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_ENGINE_TORQUE )   ? 1 : 0, info.engineTorqueTgt, 
+                 ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_TGT_SPEED )       ? 1 : 0, info.speedTgt, 
+                 ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_GEAR )            ? 1 : 0, info.gear, 
+                 ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_FLAGS )           ? 1 : 0, info.flags, info.sourceId, info.validityFlags ); 
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%splayerId        = %d\n",       identStr, info.playerId ); 
+    fprintf( stderr, "%ssteeringWheel   = %d, %.3f\n", identStr, ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_STEERING_WHEEL )  ? 1 : 0, info.steeringWheel   );     
+    fprintf( stderr, "%ssteeringSpeed   = %d, %.3f\n", identStr, ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_STEERING_SPEED )  ? 1 : 0, info.steeringSpeed   );     
+    fprintf( stderr, "%sthrottlePedal   = %d, %.3f\n", identStr, ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_THROTTLE )        ? 1 : 0, info.throttlePedal   );     
+    fprintf( stderr, "%sbrakePedal      = %d, %.3f\n", identStr, ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_BRAKE )           ? 1 : 0, info.brakePedal      );     
+    fprintf( stderr, "%sclutchPedal     = %d, %.3f\n", identStr, ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_CLUTCH )          ? 1 : 0, info.clutchPedal     );     
+    fprintf( stderr, "%saccelTgt        = %d, %.3f\n", identStr, ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_TGT_ACCEL )       ? 1 : 0, info.accelTgt        );     
+    fprintf( stderr, "%ssteeringTgt     = %d, %.3f\n", identStr, ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_TGT_STEERING )    ? 1 : 0, info.steeringTgt     ); 
+    fprintf( stderr, "%scurvatureTgt    = %d, %.3f\n", identStr, ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_CURVATURE )       ? 1 : 0, info.curvatureTgt    ); 
+    fprintf( stderr, "%ssteeringTorque  = %d, %.3f\n", identStr, ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_STEERING_TORQUE ) ? 1 : 0, info.steeringTorque  ); 
+    fprintf( stderr, "%sengineTorqueTgt = %d, %.3f\n", identStr, ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_ENGINE_TORQUE )   ? 1 : 0, info.engineTorqueTgt ); 
+    fprintf( stderr, "%sspeedTgt        = %d, %.3f\n", identStr, ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_TGT_SPEED )       ? 1 : 0, info.speedTgt        ); 
+    fprintf( stderr, "%sgear            = %d, %d\n",   identStr, ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_GEAR )            ? 1 : 0, info.gear            ); 
+    fprintf( stderr, "%sflags           = %d, 0x%x\n", identStr, ( info.validityFlags & RDB_DRIVER_INPUT_VALIDITY_FLAGS )           ? 1 : 0, info.flags           ); 
+    fprintf( stderr, "%ssourceId        = %d\n",       identStr, info.sourceId     ); 
+    fprintf( stderr, "%svalidityFlags   = 0x%x\n",     identStr, info.validityFlags     ); 
+}
+
+void
+RDBHandler::print( const RDB_TRAFFIC_LIGHT_t & info, bool extended, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,", "id", "state", "stateMask" );
+
+        if ( !extended )
+            return;
+    
+        fprintf( stderr, "%23s,%23s,%23s,", "ctrlId", "cycleTime", "noPhases" );
+
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%+.16e,%#23x,", info.base.id, info.base.state, info.base.stateMask );
+        
+        if ( !extended )
+            return;
+    
+        fprintf( stderr, "%23d,%+.16e,%23d,", info.ext.ctrlId, info.ext.cycleTime, info.ext.noPhases );
+        
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%sid        = %d\n",   identStr, info.base.id        ); 
+    fprintf( stderr, "%sstate     = %.3f\n", identStr, info.base.state     ); 
+    fprintf( stderr, "%sstateMask = 0x%x\n", identStr, info.base.stateMask );
+    
+    if ( !extended )
+        return;
+
+    fprintf( stderr, "%sctrlId    = %d\n",   identStr, info.ext.ctrlId    ); 
+    fprintf( stderr, "%scycleTime = %.3f\n", identStr, info.ext.cycleTime ); 
+    fprintf( stderr, "%snoPhases  = %d\n",   identStr, info.ext.noPhases  ); 
+    fprintf( stderr, "%sdataSize  = %d\n",   identStr, info.ext.dataSize  ); 
+
+    RDB_TRAFFIC_LIGHT_PHASE_t* phasePtr = ( RDB_TRAFFIC_LIGHT_PHASE_t* ) ( ( ( char* ) &info ) + sizeof( RDB_TRAFFIC_LIGHT_t ) );
+
+    for ( unsigned int j = 0; j < info.ext.noPhases; j++ )
+    {
+        fprintf( stderr, "   phase  %d, duration = %.3f, type = %d\n", j, phasePtr->duration, phasePtr->type );
+        phasePtr++;
+    }
+}
+
+void
+RDBHandler::print( const RDB_SYNC_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,", "mask", "cmdMask", "systemTime" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%#23x,%#23x,%+.16e,", info.mask, info.cmdMask, info.systemTime );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%smask       = 0x%x\n",  identStr, info.mask       );
+    fprintf( stderr, "%scmdMask    = 0x%x\n",  identStr, info.cmdMask    );
+    fprintf( stderr, "%ssystemTime = %.4lf\n", identStr, info.systemTime );
+}
+
+void
+RDBHandler::print( const RDB_DRIVER_PERCEPTION_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,", "playerId", "speedFromRules", "distToSpeed", "flags" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%+.16e,%+.16e,%#23x,", info.playerId, info.speedFromRules, info.distToSpeed, info.flags );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+
+    fprintf( stderr, "%splayerId       = %d\n",   identStr, info.playerId       );
+    fprintf( stderr, "%sspeedFromRules = %.3f\n", identStr, info.speedFromRules );
+    fprintf( stderr, "%sdistToSpeed    = %.3f\n", identStr, info.distToSpeed    );
+    fprintf( stderr, "%sflags          = 0x%x\n", identStr, info.flags          );
+    
+}
+
+void
+RDBHandler::print( const RDB_FUNCTION_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s", "function" );
+        return;
+    }
+
+    if ( csv )
+    {
+        fprintf( stderr, "%23s", "n/a" );
+        return;
+    }
+
+    char* identStr = getIdentString( ident );
+
+    fprintf( stderr, "%sid        = %d\n", identStr, info.id        );
+    fprintf( stderr, "%stype      = %d\n", identStr, info.type      );
+    fprintf( stderr, "%sdimension = %d\n", identStr, info.dimension );
+    fprintf( stderr, "%sdataSize  = %d\n", identStr, info.dataSize  );
+}
+
+void
+RDBHandler::print( const RDB_ROAD_QUERY_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,", "id", "flags", "x", "y" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%#23x,%+.16e,%+.16e,", info.id, info.flags, info.x, info.y );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%sid    = %d\n",    identStr, info.id    );
+    fprintf( stderr, "%sflags = 0x%x\n",  identStr, info.flags );
+    fprintf( stderr, "%sx     = %.3lf\n", identStr, info.x     );
+    fprintf( stderr, "%sy     = %.3lf\n", identStr, info.y     );
+}
+
+void
+RDBHandler::print( const RDB_POINT_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,", "x", "y", "z", "flags", "type", "system" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%+.16e,%+.16e,%+.16e,%#23x,%23d,%23d,", info.x, info.y, info.z, info.flags, info.type, info.system );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%sx/y/z = %.3f / %.3f / %.3f\n", identStr, info.x, info.y, info.z ); 
+    fprintf( stderr, "%sflags = 0x%x, type = %s, system = %d\n", identStr, info.flags, coordType2string( info.type ).c_str(), info.system ); 
+}
+
+void
+RDBHandler::print( const RDB_TRAJECTORY_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,", "playerId", "spacing", "flags", "noDataPoints" );
+        
+        if ( info.noDataPoints )
+        {
+            RDB_POINT_t* pt = ( RDB_POINT_t* ) ( ( ( unsigned char* ) ( &info ) ) + sizeof( RDB_TRAJECTORY_t ) );
+                                
+            for ( int i = 0; i < info.noDataPoints; i++ )
+                print( pt[i], ident + 4, csv, csvHeader );
+        }
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%+.16e,%#23x,%23d,", info.playerId, info.spacing, info.flags, info.noDataPoints );
+        
+        if ( info.noDataPoints )
+        {
+            RDB_POINT_t* pt = ( RDB_POINT_t* ) ( ( ( unsigned char* ) ( &info ) ) + sizeof( RDB_TRAJECTORY_t ) );
+                                
+            for ( int i = 0; i < info.noDataPoints; i++ )
+                print( pt[i], ident + 4, csv, csvHeader );
+        }
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%splayerId     = %d\n",    identStr, info.playerId     ); 
+    fprintf( stderr, "%sspacing      = %.3f\n",  identStr, info.spacing      ); 
+    fprintf( stderr, "%sflags        = 0x%x\n",  identStr, info.flags        ); 
+    fprintf( stderr, "%snoDataPoints = %d\n",    identStr, info.noDataPoints );
+
+    if ( info.noDataPoints )
+    {
+        RDB_POINT_t* pt = ( RDB_POINT_t* ) ( ( ( unsigned char* ) ( &info ) ) + sizeof( RDB_TRAJECTORY_t ) );
+                            
+        for ( int i = 0; i < info.noDataPoints; i++ )
+            print( pt[i], ident + 4 );
+    }
+}
+
+void
+RDBHandler::print( const RDB_DYN_2_STEER_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,", 
+                 "playerId", "state", "cmd", "effects", "torque", "friction", "damping", "stiffness", "velocity", "angle" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%#23x,%#23x,%#23x,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,", 
+                 info.playerId, info.state, info.cmd, info.effects, info.torque, info.friction, info.damping, info.stiffness, info.velocity, info.angle );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+
+    fprintf( stderr, "%splayerId         = %d\n",   identStr, info.playerId         ); 
+    fprintf( stderr, "%sstate            = 0x%x\n", identStr, info.state            ); 
+    fprintf( stderr, "%scmd              = 0x%x\n", identStr, info.cmd              ); 
+    fprintf( stderr, "%seffects          = 0x%x\n", identStr, info.effects          ); 
+    fprintf( stderr, "%storque           = %.6f\n", identStr, info.torque           );
+    fprintf( stderr, "%sfriction         = %.6f\n", identStr, info.friction         );
+    fprintf( stderr, "%sdamping          = %.6f\n", identStr, info.damping          );
+    fprintf( stderr, "%sstiffness        = %.6f\n", identStr, info.stiffness        );
+    fprintf( stderr, "%svelocity         = %.6f\n", identStr, info.velocity         );
+    fprintf( stderr, "%sangle            = %.6f\n", identStr, info.angle            );
+    fprintf( stderr, "%sneutralPos       = %.6f\n", identStr, info.neutralPos       );
+    fprintf( stderr, "%sdampingMaxTorque = %.6f\n", identStr, info.dampingMaxTorque );
+}
+
+void
+RDBHandler::print( const RDB_STEER_2_DYN_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,", "playerId", "state", "angle", "rev", "torque" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%#23x,%+.16e,%+.16e,%+.16e,", 
+                 info.playerId, info.state, info.angle, info.rev, info.torque );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%splayerId  = %d\n",   identStr, info.playerId ); 
+    fprintf( stderr, "%sstate     = 0x%x\n", identStr, info.state    ); 
+    fprintf( stderr, "%sangle     = %.3f\n", identStr, info.angle    );
+    fprintf( stderr, "%srev       = %.3f\n", identStr, info.rev      );
+    fprintf( stderr, "%storque    = %.3f\n", identStr, info.torque   );
+}
+
+void
+RDBHandler::print( const RDB_PROXY_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,", "protocol", "pkgId", "dataSize" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%23d,%23d,", info.protocol, info.pkgId, info.dataSize );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%sprotocol = %d\n", identStr, info.protocol ); 
+    fprintf( stderr, "%spkgId    = %d\n", identStr, info.pkgId    ); 
+    fprintf( stderr, "%sdataSize = %d\n", identStr, info.dataSize );
+}
+
+void
+RDBHandler::print( const RDB_MOTION_SYSTEM_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,", "playerId", "flags" );
+        print( info.pos, ident + 4, csv, csvHeader );
+        print( info.speed, ident + 4, csv, csvHeader );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%#23x,", info.playerId, info.flags );
+        print( info.pos, ident + 4, csv, csvHeader );
+        print( info.speed, ident + 4, csv, csvHeader );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+ 
+    fprintf( stderr, "%splayerId  = %d\n",   identStr, info.playerId ); 
+    fprintf( stderr, "%sflags     = 0x%x\n", identStr, info.flags    ); 
+   
+    fprintf( stderr, "%spos:\n", identStr ); 
+    print( info.pos, ident + 4 );
+
+    fprintf( stderr, "%sspeed:\n", identStr ); 
+    print( info.speed, ident + 4 );
+}
+
+void
+RDBHandler::print( const RDB_CUSTOM_SCORING_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,%23s,", "playerId", "pathS", "roadS", "fuelCurrent", "fuelAverage", "stateFlags", "slip" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%+.16e,%+.16e,%+.16e,%+.16e,%#23x,%+.16e,", 
+                 info.playerId, info.pathS,  info.roadS, info.fuelCurrent, info.fuelAverage, info.stateFlags, info.slip );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+ 
+    fprintf( stderr, "%splayerId    = %d\n",   identStr, info.playerId    ); 
+    fprintf( stderr, "%spathS       = %.3f\n", identStr, info.pathS       ); 
+    fprintf( stderr, "%sroadS       = %.3f\n", identStr, info.roadS       ); 
+    fprintf( stderr, "%sfuelCurrent = %.3f\n", identStr, info.fuelCurrent ); 
+    fprintf( stderr, "%sfuelAverage = %.3f\n", identStr, info.fuelAverage ); 
+    fprintf( stderr, "%sstateFlags  = 0x%x\n", identStr, info.stateFlags  ); 
+    fprintf( stderr, "%sslip        = %.3f\n", identStr, info.slip        ); 
+}
+
+void
+RDBHandler::print( const RDB_CUSTOM_OBJECT_CTRL_TRACK_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,%23s,", "playerId", "flags", "posType", "dir", "roadId", "initialRoadDeltaS", "targetRoadT" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%#23x,%23d,%23d,%23d,%+.16e,%+.16e,", 
+                 info.playerId, info.flags, info.posType, info.dir, info.roadId, info.initialRoadDeltaS, info.targetRoadT );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+ 
+    fprintf( stderr, "%splayerId          = %d\n",   identStr, info.playerId          ); 
+    fprintf( stderr, "%sflags             = 0x%x\n", identStr, info.flags             ); 
+    fprintf( stderr, "%sposType           = %d\n",   identStr, info.posType           ); 
+    fprintf( stderr, "%sdir               = %d\n",   identStr, info.dir               ); 
+    fprintf( stderr, "%sroadId            = %d\n",   identStr, info.roadId            ); 
+    fprintf( stderr, "%sinitialRoadDeltaS = %.3f\n", identStr, info.initialRoadDeltaS ); 
+    fprintf( stderr, "%stargetRoadT       = %.3f\n", identStr, info.targetRoadT       ); 
+    fprintf( stderr, "%sspeedTgtS         = %.3f\n", identStr, info.speedTgtS         ); 
+    fprintf( stderr, "%sminAccelS         = %.3f\n", identStr, info.minAccelS         ); 
+    fprintf( stderr, "%smaxAccelS         = %.3f\n", identStr, info.maxAccelS         ); 
+    fprintf( stderr, "%saccelTgt          = %.3f\n", identStr, info.accelTgt          ); 
+    fprintf( stderr, "%svalidityFlags     = 0x%x\n", identStr, info.validityFlags     );         
+}
+
+void
+RDBHandler::print( const RDB_SCP_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s", "scp" );
+        return;
+    }
+
+    if ( csv )
+    {
+        fprintf( stderr, "%23s", "n/a" );
+        return;
+    }
+
+    char* identStr = getIdentString( ident );
+
+    fprintf( stderr, "%sversion  = %d\n", identStr, info.version  ); 
+    fprintf( stderr, "%ssender   = %s\n", identStr, info.sender   ); 
+    fprintf( stderr, "%sreceiver = %s\n", identStr, info.receiver ); 
+    fprintf( stderr, "%sdataSize = %d\n", identStr, info.dataSize ); 
+}
+
+void
+RDBHandler::print( const RDB_FREESPACE_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,%23s,", 
+                         "playerId", "distance", "angleLeft", "angleRight", "distanceLeft", "distanceRight", 
+                         "stateLeft", "stateRight", "stateDistance", "noVisibleObjects" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%+.16e,%+.16e,%+.16e,%+.16e,%+.16e,%23d,%23d,%23d,%23d,", 
+                         info.playerId, info.distance, info.angleLeft, info.angleRight, info.distanceLeft, info.distanceRight, 
+                         info.stateLeft, info.stateRight, info.stateDistance, info.noVisibleObjects );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%splayerId         = %d\n",   identStr, info.playerId         ); 
+    fprintf( stderr, "%sdistance         = %.3f\n", identStr, info.distance         ); 
+    fprintf( stderr, "%sangleLeft;       = %.3f\n", identStr, info.angleLeft        ); 
+    fprintf( stderr, "%sangleRight       = %.3f\n", identStr, info.angleRight       ); 
+    fprintf( stderr, "%sdistanceLeft     = %.3f\n", identStr, info.distanceLeft     ); 
+    fprintf( stderr, "%sdistanceRight    = %.3f\n", identStr, info.distanceRight    ); 
+    fprintf( stderr, "%sstateLeft        = %d\n",   identStr, info.stateLeft        ); 
+    fprintf( stderr, "%sstateRight       = %d\n",   identStr, info.stateRight       ); 
+    fprintf( stderr, "%sstateDistance    = %d\n",   identStr, info.stateDistance    ); 
+    fprintf( stderr, "%snoVisibleObjects = %d\n",   identStr, info.noVisibleObjects ); 
+}
+
+void
+RDBHandler::print( const RDB_DYN_EL_SWITCH_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,", "objectId", "elementId", "scope", "state" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%23d,%23d,%#23x,", 
+                         info.objectId, info.elementId, info.scope, info.state );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%sobjectId  = %d\n",   identStr, info.objectId  ); 
+    fprintf( stderr, "%selementId = %d\n",   identStr, info.elementId ); 
+    fprintf( stderr, "%sscope     = %d\n",   identStr, info.scope     ); 
+    fprintf( stderr, "%sstate     = 0x%x\n", identStr, info.state     ); 
+}
+
+void
+RDBHandler::print( const RDB_DYN_EL_DOF_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,", "objectId", "elementId", "scope", "validity", "nValues" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%23d,%23d,%23d,%23d,", 
+                         info.objectId, info.elementId, info.scope, info.validity, info.nValues );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%sobjectId  = %d\n",   identStr, info.objectId  ); 
+    fprintf( stderr, "%selementId = %d\n",   identStr, info.elementId ); 
+    fprintf( stderr, "%sscope     = %d\n",   identStr, info.scope     ); 
+    fprintf( stderr, "%svalidity  = %d\n",   identStr, info.validity  ); 
+    fprintf( stderr, "%snValues   = %d\n",   identStr, info.nValues   ); 
+}
+
+void
+RDBHandler::print( const RDB_IG_FRAME_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,", "deltaT", "frameNo" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%+.16e,%23d,", info.deltaT, info.frameNo );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%sdeltaT  = %.3f\n", identStr, info.deltaT  ); 
+    fprintf( stderr, "%sframeNo = %d\n",   identStr, info.frameNo ); 
+}
+
+void
+RDBHandler::print( const RDB_RT_PERFORMANCE_t & info, unsigned char ident, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+    {
+        fprintf( stderr, "%23s,%23s,%23s,%23s,%23s,%23s,", "noOverruns", "noUnderruns", "noMeasurements", "tolerance", "nominalFrameTime", "actualFrameTime" );
+        return;
+    }
+    
+    if ( csv )
+    {
+        fprintf( stderr, "%23d,%23d,%23d,%+.16e,%+.16e,%+.16e,", info.noOverruns, info.noUnderruns, info.noMeasurements, info.tolerance, info.nominalFrameTime, info.actualFrameTime );
+        return;
+    }
+    
+    char* identStr = getIdentString( ident );
+    
+    fprintf( stderr, "%snoMeasurements   = %d\n",     identStr, info.noMeasurements ); 
+    fprintf( stderr, "%snoUnderruns      = %d\n",     identStr, info.noUnderruns ); 
+    fprintf( stderr, "%snoOverruns       = %d\n",     identStr, info.noOverruns ); 
+    fprintf( stderr, "%stolerance        = %.3f\n",   identStr, info.tolerance ); 
+    fprintf( stderr, "%snominalFrameTime = %.3fms\n", identStr, info.nominalFrameTime * 1000.0f ); 
+    fprintf( stderr, "%sactualFrameTime  = %.3fms\n", identStr, info.actualFrameTime * 1000.0f ); 
+}
+
+void
+RDBHandler::printMatrix( int *pData, unsigned int width, unsigned int height, bool csv, bool csvHeader )
+{
+    if ( csvHeader )
+        return;
+
+    if ( csv )
+        return;
+
+    for ( unsigned int h = 0; h < height; h++ )
+    {
+        for ( unsigned int w = 0; w < width; w++ )
+        {
+            fprintf( stderr, "%5d", *pData );
+            pData++;
+        }
+        fprintf( stderr, "\n" );
+    }
+}
+        
+RDBHandler::RDBHandler() : mMsg( 0 ),
+                           mShmHdr( 0 )                          
+{
+     //std::cerr << "RDBHandler::RDBHandler: CTOR called, this=" << this << std::endl;
+}
+
+RDBHandler::~RDBHandler()
+{
+     //std::cerr << "RDBHandler::~RDBHandler: DTOR called, this=" << this << std::endl;
+     if ( mMsg )
+         free( mMsg );
+}
+
+void
+RDBHandler::initMsg()
+{
+    if ( mMsg )
+        free( mMsg );
+    
+    mMsg = 0;
+}
+
+void*
+RDBHandler::addPackage( const double & simTime, const unsigned int & simFrame, 
+                        unsigned int pkgId,     unsigned int noElements,       
+                        bool extended, size_t trailingData, bool isCustom )
+{
+    // extend the internal message if no other is given
+    return addPackage( mMsg, simTime, simFrame, pkgId, noElements, extended, trailingData, isCustom );
+}
+
+void*
+RDBHandler::addCustomPackage( const double & simTime, const unsigned int & simFrame, 
+                              unsigned int pkgId, unsigned int noElements, size_t elementSize )
+{
+    // extend the internal message if no other is given
+    return addCustomPackage( mMsg, simTime, simFrame, pkgId, noElements, elementSize );
+}
+
+RDB_MSG_t*
+RDBHandler::getMsg()
+{
+    return mMsg;
+}
+
+RDB_MSG_HDR_t*
+RDBHandler::getMsgHdr()
+{
+    if ( !mMsg )
+        return 0;
+    
+    return &( mMsg->hdr );
+}
+
+size_t
+RDBHandler::getMsgTotalSize()
+{
+    if ( !mMsg )
+        return 0;
+    
+    return mMsg->hdr.headerSize + mMsg->hdr.dataSize;
+}
+
+void*
+RDBHandler::getFirstEntry( unsigned int pkgId, unsigned int & noElements, bool extended )
+{
+    return getFirstEntry( mMsg, pkgId, noElements, extended );
+}
+
+RDB_MSG_ENTRY_HDR_t*
+RDBHandler::getEntryHdr( unsigned int pkgId, bool extended )
+{
+    return getEntryHdr( mMsg, pkgId, extended );
+}
+
+bool
+RDBHandler::shmConfigure( void *startPtr, unsigned int noBuffers, unsigned int totalSize )
+{
+    if ( !startPtr || !noBuffers )
+        return false;
+    
+    mShmHdr = ( RDB_SHM_HDR_t* ) ( startPtr );
+    
+    memset( mShmHdr, 0, totalSize );
+    
+    mShmHdr->headerSize = sizeof( RDB_SHM_HDR_t );
+    mShmHdr->noBuffers  = noBuffers;
+
+    RDB_SHM_BUFFER_INFO_t* info  = ( RDB_SHM_BUFFER_INFO_t* ) ( ( ( char* ) mShmHdr ) + mShmHdr->headerSize );
+    
+    // compute the size of a single buffer
+    unsigned int bufferSize = 0;
+    
+    if ( totalSize > 0 )
+    {
+        if ( totalSize <= ( mShmHdr->noBuffers * sizeof( RDB_SHM_BUFFER_INFO_t ) + mShmHdr->headerSize ) )
+            fprintf( stderr, "RDBHandler::shmConfigure: insufficient data size for shared memory!\n" );
+        else
+            bufferSize = ( totalSize - ( mShmHdr->noBuffers * sizeof( RDB_SHM_BUFFER_INFO_t ) + mShmHdr->headerSize ) ) / mShmHdr->noBuffers;
+
+    }
+    
+    for ( int i = 0; i < mShmHdr->noBuffers; i++ )
+    {
+        //fprintf( stderr, "RDBHandler::shmConfigure: configuring buffer %d with %d bytes!\n", i, bufferSize );
+    
+        info[i].thisSize   = sizeof( RDB_SHM_BUFFER_INFO_t );
+        info[i].bufferSize = bufferSize;
+        info[i].offset     = mShmHdr->noBuffers * sizeof( RDB_SHM_BUFFER_INFO_t ) + mShmHdr->headerSize + i * bufferSize;
+    }
+    
+    return true;
+}
+
+bool
+RDBHandler::shmSetAddress( void* shmAddr )
+{
+    mShmHdr = ( RDB_SHM_HDR_t* ) shmAddr;
+    
+    return mShmHdr != 0;
+}
+
+RDB_SHM_HDR_t*
+RDBHandler::shmGetHdr()
+{
+    return mShmHdr;
+}
+
+RDB_SHM_BUFFER_INFO_t*
+RDBHandler::shmBufferGetInfo( unsigned int index )
+{
+    if ( !mShmHdr )
+        return 0;
+    
+    if ( index >= mShmHdr->noBuffers )
+        return 0;
+    
+    RDB_SHM_BUFFER_INFO_t* info = ( RDB_SHM_BUFFER_INFO_t* ) ( ( ( char* ) mShmHdr ) + mShmHdr->headerSize );
+    
+    return &( info[index] );
+}
+
+void*
+RDBHandler::shmBufferGetPtr( unsigned int index )
+{
+    RDB_SHM_BUFFER_INFO_t* info = shmBufferGetInfo( index );
+    
+    if ( !info )
+        return 0;
+    
+    return ( ( ( char* ) mShmHdr ) + info->offset );
+}
+
+void
+RDBHandler::shmHdrUpdate()
+{
+    if ( !mShmHdr )
+        return;
+    
+    // access the first buffer
+    RDB_SHM_BUFFER_INFO_t* info     = shmBufferGetInfo( 0 );
+    RDB_SHM_BUFFER_INFO_t* prevInfo = 0;
+    
+    if ( !info )
+        return;
+
+    // compute total data size of the shared memory buffers
+    unsigned int totalSize = 0;
+    
+    // we assume that one buffer is written after another so that a buffer's offset
+    // can be computed from the predecessor's offset and its size
+    
+    for ( int i = 0; i < mShmHdr->noBuffers; i++ )
+    {
+        totalSize += info->thisSize + info->bufferSize;
+        
+        if ( prevInfo )
+            info->offset = prevInfo->offset + prevInfo->bufferSize;
+            
+        prevInfo = info;
+        info++;
+    }
+    mShmHdr->dataSize = totalSize;    
+}
+
+void
+RDBHandler::shmBufferSetSize( unsigned int index, size_t size )
+{
+    RDB_SHM_BUFFER_INFO_t* info = shmBufferGetInfo( index );
+    
+    if ( !info )
+        return;
+    
+    info->bufferSize = size;
+}
+
+size_t
+RDBHandler::shmBufferGetSize( unsigned int index )
+{
+    RDB_SHM_BUFFER_INFO_t* info = shmBufferGetInfo( index );
+    
+    if ( !info )
+        return 0;
+    
+    return info->bufferSize;
+}
+
+void
+RDBHandler::shmBufferSetId( unsigned int index, unsigned int id )
+{
+    RDB_SHM_BUFFER_INFO_t* info = shmBufferGetInfo( index );
+    
+    if ( !info )
+        return;
+    
+    info->id = id;
+}
+
+void
+RDBHandler::shmBufferSetFlags( unsigned int index, unsigned int flags )
+{
+    RDB_SHM_BUFFER_INFO_t* info = shmBufferGetInfo( index );
+    
+    if ( !info )
+        return;
+    
+    info->flags = flags;
+}
+
+void
+RDBHandler::shmBufferAddFlags( unsigned int index, unsigned int flags )
+{
+    RDB_SHM_BUFFER_INFO_t* info = shmBufferGetInfo( index );
+    
+    if ( !info )
+        return;
+    
+    info->flags |= flags;
+}
+
+void
+RDBHandler::shmBufferReleaseFlags( unsigned int index, unsigned int flags )
+{
+    RDB_SHM_BUFFER_INFO_t* info = shmBufferGetInfo( index );
+    
+    if ( !info )
+        return;
+    
+    info->flags &= ~flags;
+}
+
+unsigned int
+RDBHandler::shmBufferGetFlags( unsigned int index )
+{
+    RDB_SHM_BUFFER_INFO_t* info = shmBufferGetInfo( index );
+    
+    if ( !info )
+        return 0;
+    
+    return info->flags;
+}
+
+bool
+RDBHandler::shmBufferHasFlags( unsigned int index, unsigned int mask )
+{
+    RDB_SHM_BUFFER_INFO_t* info = shmBufferGetInfo( index );
+    
+    if ( !info )
+        return false;
+    
+    return ( info->flags & mask ) == mask;
+}
+
+bool
+RDBHandler::mapMsgToShm( unsigned int index, bool relocateBuffers )
+{
+    void* tgt = shmBufferGetPtr( index );
+    
+    if ( !tgt || !mMsg )
+        return false;
+
+    if ( relocateBuffers )
+    {
+        // total size of buffer is determined by message size 
+        shmBufferSetSize( index, getMsgTotalSize() );
+    
+        // buffer size has changed, so compute the header information again
+        shmHdrUpdate();
+    }
+
+    // copy the local message data to the target location
+    if ( shmBufferGetSize( index ) >= getMsgTotalSize() )
+        memcpy( tgt, getMsg(), getMsgTotalSize() );
+    
+    return true;
+}
+
+bool
+RDBHandler::addMsgToShm( unsigned int index, RDB_MSG_t* msg )
+{
+    char* tgt = ( char* ) shmBufferGetPtr( index );
+    
+    if ( !tgt || !msg )
+        return false;
+    
+    unsigned int msgTotalSize = msg->hdr.headerSize + msg->hdr.dataSize;
+    
+    // get the current usage of the buffer
+    unsigned int usedSize = shmBufferGetUsedSize( index );
+
+    // copy the local message data to the target location
+    if ( shmBufferGetSize( index ) < ( usedSize + msgTotalSize ) )
+        return false;
+    
+    memcpy( ( tgt + usedSize ), msg, msgTotalSize );
+    
+    return true;
+}
+
+unsigned int
+RDBHandler::shmBufferGetUsedSize( unsigned int index )
+{
+    char* tgt = ( char* ) shmBufferGetPtr( index );
+    
+    if ( !tgt )
+        return 0;
+    
+    unsigned int noBytes = 0;
+    
+    while ( 1 )
+    {
+        RDB_MSG_t* msg = ( RDB_MSG_t* ) tgt;
+        
+        if ( msg->hdr.magicNo != RDB_MAGIC_NO )
+            break;
+        
+        unsigned int msgSize = msg->hdr.headerSize + msg->hdr.dataSize;
+        
+        noBytes += msgSize;
+        tgt     += msgSize;
+    }
+    
+    return noBytes;
+}
+
+bool
+RDBHandler::shmBufferClear( unsigned int index, bool force )
+{
+    // check if buffer is locked
+    if ( !force && shmBufferIsLocked( index ) )
+        return false;
+    
+    char* tgt = ( char* ) shmBufferGetPtr( index );
+    
+    if ( !tgt )
+        return false;
+    
+    memset( tgt, 0, shmBufferGetSize( index ) );
+    
+    return true;
+}
+
+bool
+RDBHandler::shmBufferIsLocked( unsigned int index )
+{
+    // check if buffer is locked
+    RDB_SHM_BUFFER_INFO_t* info = shmBufferGetInfo( index );
+    
+    if ( !info )
+        return true;
+    
+    //fprintf( stderr, "RDBHandler::shmBufferIsLocked: buffer %d, flags = 0x%x, isLocked = 0x%x\n", 
+    //                 index, info->flags, info->flags & RDB_SHM_BUFFER_FLAG_LOCK );
+
+    return ( ( info->flags & RDB_SHM_BUFFER_FLAG_LOCK ) != 0 );
+}
+
+bool
+RDBHandler::shmBufferLock( unsigned int index )
+{
+    RDB_SHM_BUFFER_INFO_t* info = shmBufferGetInfo( index );
+    
+    if ( !info )
+        return false;
+    
+    info->flags |= RDB_SHM_BUFFER_FLAG_LOCK;
+
+    //fprintf( stderr, "RDBHandler::shmBufferLock: mShmHdr %p locking buffer %d, flags = 0x%x\n", mShmHdr, index, info->flags );
+    
+    return true;
+}
+
+bool
+RDBHandler::shmBufferRelease( unsigned int index )
+{
+    RDB_SHM_BUFFER_INFO_t* info = shmBufferGetInfo( index );
+    
+    if ( !info )
+        return false;
+ 
+    info->flags &= ~RDB_SHM_BUFFER_FLAG_LOCK;
+    
+    //fprintf( stderr, "RDBHandler::shmBufferRelease: mShmHdr %p releasing buffer %d, flags = 0x%x\n", mShmHdr, index, info->flags );
+ 
+    return true;
+}
+
+
+unsigned int
+RDBHandler::shmGetNoBuffers()
+{
+    if ( !mShmHdr )
+        return 0;
+    
+    return mShmHdr->noBuffers;
+}
+
+void
+RDBHandler::printPackageSizes()
+{
+    fprintf( stderr, "RDBHandler::printPackageSizes: sizes of all known packages\n" );
+    
+    for ( int i = RDB_PKG_ID_START_OF_FRAME; i <= RDB_PKG_ID_TONE_MAPPING; i++ )
+    {
+        bool hasExtended = pkgId2size( i ) != pkgId2size( i, true );
+        
+        fprintf( stderr, "%40s, size = %4d", pkgId2string( i ).c_str(), ( int ) pkgId2size( i ) );
+
+        if ( hasExtended )
+            fprintf( stderr, " ( %4d )", ( int ) pkgId2size( i, true ) );
+
+        fprintf( stderr, "\n" );
+    }
+}
+
+void
+RDBHandler::parseMessage( RDB_MSG_t* msg )
+{
+    if ( !msg )
+        return;
+
+    if ( !msg->hdr.dataSize )
+        return;
+    
+    RDB_MSG_ENTRY_HDR_t* entry = ( RDB_MSG_ENTRY_HDR_t* ) ( ( ( char* ) msg ) + msg->hdr.headerSize );
+    uint32_t remainingBytes    = msg->hdr.dataSize;
+        
+    while ( 1 )
+    {
+        parseMessageEntry( entry, msg->hdr.simTime, msg->hdr.frameNo );
+
+        remainingBytes -= ( entry->headerSize + entry->dataSize );
+        
+        if ( !remainingBytes )
+            return;
+        
+        entry = ( RDB_MSG_ENTRY_HDR_t* ) ( ( ( ( char* ) entry ) + entry->headerSize + entry->dataSize ) );
+    }
+}
+
+void
+RDBHandler::parseMessageEntry( RDB_MSG_ENTRY_HDR_t* entryHdr, const double & simTime, const unsigned int & simFrame )
+{
+    if ( !entryHdr )
+        return;
+    
+    unsigned int noElements = entryHdr->elementSize ? ( entryHdr->dataSize / entryHdr->elementSize ) : 0;
+    
+    // some packages may not have an active element
+    if ( !noElements )
+    {
+        switch ( entryHdr->pkgId )
+        {
+            case RDB_PKG_ID_START_OF_FRAME:
+                parseStartOfFrame( simTime, simFrame );
+                break;
+                
+            case RDB_PKG_ID_END_OF_FRAME:
+                parseEndOfFrame( simTime, simFrame );
+                break;
+        }        
+        return;
+    }
+        
+    char* dataPtr = ( char* ) entryHdr;
+    dataPtr += entryHdr->headerSize;
+        
+    for ( unsigned int i = 0; i < noElements; i++ )
+    {
+        switch ( entryHdr->pkgId )
+        {
+            case RDB_PKG_ID_START_OF_FRAME:
+                parseStartOfFrame( simTime, simFrame );
+                break;
+                
+            case RDB_PKG_ID_END_OF_FRAME:
+                parseEndOfFrame( simTime, simFrame );
+                break;
+                
+            case RDB_PKG_ID_COORD_SYSTEM:
+                parseEntry( ( RDB_COORD_SYSTEM_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_COORD:
+                parseEntry( ( RDB_COORD_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_ROAD_POS:
+                parseEntry( ( RDB_ROAD_POS_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_LANE_INFO:
+                parseEntry( ( RDB_LANE_INFO_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_ROADMARK:
+                parseEntry( ( RDB_ROADMARK_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_OBJECT_CFG:
+                parseEntry( ( RDB_OBJECT_CFG_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_OBJECT_STATE:
+                parseEntry( ( RDB_OBJECT_STATE_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_VEHICLE_SYSTEMS:
+                parseEntry( ( RDB_VEHICLE_SYSTEMS_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_VEHICLE_SETUP:
+                parseEntry( ( RDB_VEHICLE_SETUP_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_ENGINE:
+                parseEntry( ( RDB_ENGINE_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_DRIVETRAIN:
+                parseEntry( ( RDB_DRIVETRAIN_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_WHEEL:
+                parseEntry( ( RDB_WHEEL_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+    
+            case RDB_PKG_ID_PED_ANIMATION:
+                parseEntry( ( RDB_PED_ANIMATION_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+    
+            case RDB_PKG_ID_SENSOR_STATE:
+                parseEntry( ( RDB_SENSOR_STATE_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+    
+            case RDB_PKG_ID_SENSOR_OBJECT:
+                parseEntry( ( RDB_SENSOR_OBJECT_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_CAMERA:
+                parseEntry( ( RDB_CAMERA_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_CONTACT_POINT:
+                parseEntry( ( RDB_CONTACT_POINT_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_TRAFFIC_SIGN:
+                parseEntry( ( RDB_TRAFFIC_SIGN_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_ROAD_STATE:
+                parseEntry( ( RDB_ROAD_STATE_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_IMAGE:
+            case RDB_PKG_ID_LIGHT_MAP:
+            case RDB_PKG_ID_OCCLUSION_MATRIX:
+                parseEntry( ( RDB_IMAGE_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_LIGHT_SOURCE:
+                parseEntry( ( RDB_LIGHT_SOURCE_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_ENVIRONMENT:
+                parseEntry( ( RDB_ENVIRONMENT_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_TRIGGER:
+                parseEntry( ( RDB_TRIGGER_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_DRIVER_CTRL:
+                parseEntry( ( RDB_DRIVER_CTRL_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_TRAFFIC_LIGHT:
+                parseEntry( ( RDB_TRAFFIC_LIGHT_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_SYNC:
+                parseEntry( ( RDB_SYNC_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_DRIVER_PERCEPTION:
+                parseEntry( ( RDB_DRIVER_PERCEPTION_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_TONE_MAPPING:
+                parseEntry( ( RDB_FUNCTION_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_ROAD_QUERY:
+                parseEntry( ( RDB_ROAD_QUERY_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_TRAJECTORY:
+                parseEntry( ( RDB_TRAJECTORY_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+
+            case RDB_PKG_ID_CUSTOM_SCORING:
+                parseEntry( ( RDB_CUSTOM_SCORING_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+
+            case RDB_PKG_ID_DYN_2_STEER:
+                parseEntry( ( RDB_DYN_2_STEER_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+
+            case RDB_PKG_ID_SCP:
+                parseEntry( ( RDB_SCP_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+
+            case RDB_PKG_ID_STEER_2_DYN:
+                parseEntry( ( RDB_STEER_2_DYN_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_PROXY:
+                parseEntry( ( RDB_PROXY_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+
+            case RDB_PKG_ID_MOTION_SYSTEM:
+                parseEntry( ( RDB_MOTION_SYSTEM_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_FREESPACE:
+                parseEntry( ( RDB_FREESPACE_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_DYN_EL_SWITCH:
+                parseEntry( ( RDB_DYN_EL_SWITCH_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_DYN_EL_DOF:
+                parseEntry( ( RDB_DYN_EL_DOF_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_IG_FRAME:
+                parseEntry( ( RDB_IG_FRAME_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_RT_PERFORMANCE:
+                parseEntry( ( RDB_RT_PERFORMANCE_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_CUSTOM_OBJECT_CTRL_TRACK:
+                parseEntry( ( RDB_CUSTOM_OBJECT_CTRL_TRACK_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+                
+            case RDB_PKG_ID_RAY:
+                // to be implemented
+                //parseEntry( ( RDB_RAY_t* ) dataPtr, simTime, simFrame, entryHdr->pkgId, entryHdr->flags, i, noElements );
+                break;
+            default:
+                fprintf( stderr, "RDBHandler::parseMessageEntry: unhandled pkgId = %d\n", entryHdr->pkgId );
+                break;
+        }
+        dataPtr += entryHdr->elementSize;
+    }
+}
+        
+void
+RDBHandler::parseMessageEntryInfo( const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    fprintf( stderr, "RDBHandler::parseMessageEntryInfo: simTime = %.3f, simFrame = %d, pkgId = %d, flags = 0x%x, elemId = %d, totalElem = %d\n",
+                     simTime, simFrame, pkgId, flags, elemId, totalElem );
+}
+
+void
+RDBHandler::parseStartOfFrame( const double & simTime, const unsigned int & simFrame )
+{
+    fprintf( stderr, "RDBHandler::parseStartOfFrame: simTime = %.3f, simFrame = %d\n", simTime, simFrame );
+}
+
+void
+RDBHandler::parseEndOfFrame( const double & simTime, const unsigned int & simFrame )
+{
+    fprintf( stderr, "RDBHandler::parseEndOfFrame: simTime = %.3f, simFrame = %d\n", simTime, simFrame );
+}
+
+void
+RDBHandler::parseEntry( RDB_GEOMETRY_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_COORD_SYSTEM_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_COORD_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_ROAD_POS_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_LANE_INFO_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_ROADMARK_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_OBJECT_CFG_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_OBJECT_STATE_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_VEHICLE_SYSTEMS_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_VEHICLE_SETUP_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_ENGINE_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_DRIVETRAIN_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_WHEEL_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_PED_ANIMATION_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_SENSOR_STATE_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_SENSOR_OBJECT_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_CAMERA_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_CONTACT_POINT_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_TRAFFIC_SIGN_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_ROAD_STATE_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_IMAGE_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_LIGHT_SOURCE_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_ENVIRONMENT_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_TRIGGER_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_DRIVER_CTRL_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_TRAFFIC_LIGHT_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_SYNC_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_DRIVER_PERCEPTION_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_FUNCTION_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_ROAD_QUERY_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_POINT_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_TRAJECTORY_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_CUSTOM_SCORING_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_DYN_2_STEER_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_SCP_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_STEER_2_DYN_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_PROXY_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_MOTION_SYSTEM_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_FREESPACE_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_DYN_EL_SWITCH_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_DYN_EL_DOF_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_IG_FRAME_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_RT_PERFORMANCE_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+void
+RDBHandler::parseEntry( RDB_CUSTOM_OBJECT_CTRL_TRACK_t *data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem )
+{
+    parseMessageEntryInfo( simTime, simFrame, pkgId, flags, elemId, totalElem );
+    print( *data, 4 );
+}
+
+
+} // namespace Framework

src/ros/catkin/src/vtd_ros/src/VTD/RDBHandler.hh

@@ -0,0 +1,895 @@
+/* ===================================================
+ *  file:       RDBHandler.hh
+ * ---------------------------------------------------
+ *  purpose:	collection of RDB routines
+ * ---------------------------------------------------
+ *  first edit:	24.01.2012 by M. Dupuis @ VIRES GmbH
+ *  last mod.:  24.01.2012 by M. Dupuis @ VIRES GmbH
+ * ===================================================
+ */
+#ifndef _FRAMEWORK_RDB_HANDLER_HH
+#define _FRAMEWORK_RDB_HANDLER_HH
+
+/* ====== INCLUSIONS ====== */
+#include <string>
+#include <vector>
+#include "viRDBIcd.h"
+
+namespace Framework
+{
+class RDBHandler 
+{
+    public:
+        /**
+        * convert a package ID to the corresponding element size
+        * @param  pkgId          id of the package whose size is to be determined
+        * @param  extended       true if the size of the extended package is to be determined
+        * @return size of the package
+        */
+        static size_t pkgId2size( unsigned int pkgId, bool extended = false );
+
+        /**
+        * convert a package ID to the corresponding name
+        * @param  pkgId          id of the package whose name is to be determined
+        * @return name of the package
+        */
+        static std::string pkgId2string( unsigned int pkgId );
+        
+        /**
+        * convert a coord type to the corresponding name
+        * @param  type          id of the coordinate type whose name is to be determined
+        * @return name of the package
+        */
+        static std::string coordType2string( unsigned int type );
+        
+        /**
+        * convert an object category into a string
+        * @param  id          id of the category
+        * @return name of the category
+        */
+        static std::string objectCategory2string( unsigned int id );
+        
+        /**
+        * convert an object category string into the numeric category
+        * @param  name    name of the category 
+        * @return id of the category
+        */
+        static unsigned int objectString2category( const std::string & name );
+        
+        /**
+        * convert an object type into a string
+        * @param  id          id of the type
+        * @return name of the type
+        */
+        static std::string objectType2string( unsigned int id );
+        
+        /**
+        * convert an object type string into the numeric type
+        * @param  name    name of the type 
+        * @return id of the type
+        */
+        static unsigned int objectString2type( const std::string & name );
+        
+        /**
+        * print the contents of an RDB message
+        * @param msg       pointer to the message which is to be printed; 0 for current internal message
+        * @param details   if true, print the details, not only the headers
+        * @param binDump   create a binary dump of the message
+        * @param csv       print CSV version of the message
+        * @param csvHeader print CSV header information only
+        */
+        static void printMessage( RDB_MSG_t* msg = 0, bool details = false, bool binDump = false, bool csv = false, bool csvHeader = false );
+
+        /**
+        * print the contents of an RDB message entry
+        * @param entryHdr pointer to the entry header whose contents are to be printed
+        * @param details  if true, print the details, not only the headers
+        * @param csv       print CSV version of the entry
+        * @param csvHeader print CSV header information only
+        */
+        static void printMessageEntry( RDB_MSG_ENTRY_HDR_t* entryHdr, bool details = false, bool csv = false, bool csvHeader = false );
+
+        /**
+        * add a packet or series of packets to an RDB message
+        * @param  msg            pointer to the message that is to be extended / composed (0 for new message); may be altered!
+        * @param  simTime        simulation time for which to compose the package
+        * @param  simFrame       simulation frame for which to compose the package
+        * @param  pkgId          id of the package that is to be added to the message 
+        * @param  noElements     number of elements of given package ID type that are to be added
+        * @param  extended       true if an extended element is to be inserted
+        * @param  trailingData   size of trailing data of each element
+        * @param  isCustom       if true, message is considered a custom message and size per element will be derived from argument "trailingData"
+        * @return pointer where to start inserting the data, otherwise 0; message pointer may be altered!
+        */
+        static void* addPackage( RDB_MSG_t* & msg,   const double & simTime, const unsigned int & simFrame, 
+                                 unsigned int pkgId, unsigned int noElements, bool extended, size_t trailingData, bool isCustom = false );
+                
+        /**
+        * add a custom packet or series of packets to an RDB message
+        * @param  msg            pointer to the message that is to be extended / composed (0 for new message); may be altered!
+        * @param  simTime        simulation time for which to compose the package
+        * @param  simFrame       simulation frame for which to compose the package
+        * @param  pkgId          id of the package that is to be added to the message 
+        * @param  noElements     number of elements of given package ID type that are to be added
+        * @param  elementSize    data size of each element
+        * @return pointer where to start inserting the data, otherwise 0; message pointer may be altered!
+        */
+        static void* addCustomPackage( RDB_MSG_t* & msg, const double & simTime, const unsigned int & simFrame, 
+                                       unsigned int pkgId, unsigned int noElements, size_t elementSize );
+                
+        /**
+        * retrieve the pointer to the first entry of a given type from a given message
+        * @param  msg            pointer to the message that is to be searched for the entry
+        * @param  pkgId          id (i.e. type) of the entry that is to be retrieved 
+        * @param  noElements     number of elements of given entry type that have been found (will be altered)
+        * @param  extended       true if an extended element is to be retrieved
+        * @return pointer to first element of the requested entry type or 0 if none has been found.
+        */
+        static void* getFirstEntry( RDB_MSG_t* msg, unsigned int pkgId, unsigned int & noElements, bool extended );
+
+        /**
+        * retrieve the pointer to the first entry header of a given type from a given message
+        * @param  msg            pointer to the message that is to be searched for the entry
+        * @param  pkgId          id (i.e. type) of the entry that is to be retrieved 
+        * @param  extended       true if an extended element is to be retrieved
+        * @return pointer to first entry header of the requested entry type or 0 if none has been found.
+        */
+        static RDB_MSG_ENTRY_HDR_t* getEntryHdr( RDB_MSG_t* msg, unsigned int pkgId, bool extended );
+
+        /**
+        * create an ident string of given length
+        * @param ident number of spaces to use for identing
+        * @return pointer to ident character string
+        */
+        static char* getIdentString( unsigned char ident );
+                
+        /**
+        * print a geometry info
+        * @param info      the info which is to be printed
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_GEOMETRY_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+                
+        /**
+        * print a coordinate system
+        * @param info      the info which is to be printed
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_COORD_SYSTEM_t & state, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+                
+        /**
+        * print a co-ordinate
+        * @param info      the info which is to be printed
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_COORD_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+                
+        /**
+        * print the whole bunch of road information
+        * @param info      reference to the road information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_ROAD_POS_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print information about lanes
+        * @param info      reference to the lane information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_LANE_INFO_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print the whole bunch of road mark information
+        * @param info      reference to the road mark information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_ROADMARK_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print an object configuration
+        * @param info      the configuration which is to be printed
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_OBJECT_CFG_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+                
+        /**
+        * print an object state
+        * @param state     the state which is to be printed
+        * @param extended  true if extened object state is to be printed
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_OBJECT_STATE_t & state, bool extended = false, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+                
+        /**
+        * print information about vehicle systems
+        * @param info      reference to the data object
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_VEHICLE_SYSTEMS_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print a vehicle's setup
+        * @param info      the info which is to be printed
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_VEHICLE_SETUP_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+                
+        /**
+        * print engine information
+        * @param info      the info which is to be printed
+        * @param extended  true if extened object state is to be printed
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_ENGINE_t & info, bool extended = false, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+                
+        /**
+        * print drivetrain information
+        * @param info      the info which is to be printed
+        * @param extended  true if extened object state is to be printed
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_DRIVETRAIN_t & info, bool extended = false, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+                
+        /**
+        * print wheel information
+        * @param info      the info which is to be printed
+        * @param extended  true if extened object state is to be printed
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_WHEEL_t & info, bool extended = false, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+                
+        /**
+        * print pedestrian animation information
+        * @param info      the info which is to be printed
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_PED_ANIMATION_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+                
+        /**
+        * print information about a sensor
+        * @param info  reference to the sensor state
+        * @param ident number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_SENSOR_STATE_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print information about sensor objects
+        * @param info      reference to the sensor object
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_SENSOR_OBJECT_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print the whole bunch of camera information
+        * @param info  reference to the camera information
+        * @param ident number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_CAMERA_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print the whole bunch of contact point information
+        * @param info      reference to the contact point information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_CONTACT_POINT_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print information about a single traffic sign
+        * @param info      reference to the traffic sign's information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_TRAFFIC_SIGN_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print information about a single road state
+        * @param info      reference to the road state's information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_ROAD_STATE_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print information about an image data block
+        * @param info      reference to the data object
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_IMAGE_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print information about an light source
+        * @param info  reference to the data object
+        * @param ident number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_LIGHT_SOURCE_t & info, bool extended = false, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print information about environment package
+        * @param info      reference to the information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_ENVIRONMENT_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print information about trigger package
+        * @param info      reference to the information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_TRIGGER_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print information about driver controls
+        * @param info      reference to the driver control information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_DRIVER_CTRL_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+
+        /**
+        * print information about a single traffic light
+        * @param info      reference to the traffic light's information
+        * @param extended  if true, print extended state
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_TRAFFIC_LIGHT_t & info, bool extended = false, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print information about sync package
+        * @param info      reference to the information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_SYNC_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print information about driver perception package
+        * @param info      reference to the information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_DRIVER_PERCEPTION_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print information about tone mapping package
+        * @param info      reference to the information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_FUNCTION_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print information about road query package
+        * @param info      reference to the information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_ROAD_QUERY_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print point information
+        * @param info      reference to the information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_POINT_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+
+        /**
+        * print the a trajectory information
+        * @param info      reference to the trajectory information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_TRAJECTORY_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+
+        /**
+        * print the a information from dynamics to steering
+        * @param info      reference to the information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_DYN_2_STEER_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+
+        /**
+        * print the a information from steering to dynamics
+        * @param info      reference to the information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_STEER_2_DYN_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+
+        /**
+        * print the a proxy information
+        * @param info      reference to the information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_PROXY_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print the a motion system information
+        * @param info      reference to the information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_MOTION_SYSTEM_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print the a scoring information
+        * @param info      reference to the information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_CUSTOM_SCORING_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print the a track control information
+        * @param info      reference to the information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_CUSTOM_OBJECT_CTRL_TRACK_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print the an SCP information
+        * @param info      reference to the information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_SCP_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print the a FREESPACE information
+        * @param info      reference to the information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_FREESPACE_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print the a DYN_EL_SWITCH information
+        * @param info      reference to the information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_DYN_EL_SWITCH_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print the a DYN_EL_DOF information
+        * @param info      reference to the information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_DYN_EL_DOF_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print the a IG_FRAME information
+        * @param info      reference to the information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_IG_FRAME_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print the a real-time performance information
+        * @param info      reference to the information
+        * @param ident     number of spaces to use for identing
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void print( const RDB_RT_PERFORMANCE_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false );
+        
+        /**
+        * print information about a matrix
+        * @param pData     pointer to the data structure
+        * @param width     width of the matrix
+        * @param height    height of the matrix
+        * @param csv       print CSV version of the data
+        * @param csvHeader print CSV header information only
+        */
+        static void printMatrix( int *pData, unsigned int width, unsigned int height, bool csv = false, bool csvHeader = false );
+        
+    public:
+        /**
+        * constructor
+        */
+        explicit RDBHandler();
+
+        /**
+        * Destroy the class. 
+        */
+        virtual ~RDBHandler();
+        
+        /**
+        * (re-) initialize the RDB message, i.e. internally held data
+        */
+        void initMsg();
+        
+        /**
+        * add a packet or series of packets to an RDB message
+        * @param  simTime        simulation time for which to compose the package
+        * @param  simFrmae       simulation frame for which to compose the package
+        * @param  pkgId          id of the package that is to be added to the message 
+        * @param  noElements     number of elements of given package ID type that are to be added
+        * @param  extended       true if an extended element is to be inserted
+        * @param  trailingData   size of trailing data of each element
+        * @param  isCustom       if true, package is considered a custom package and size per element will be derived from argument "trailingData"
+        * @return pointer where to start inserting the data, otherwise 0
+        */
+        void* addPackage( const double & simTime, const unsigned int & simFrame, 
+                          unsigned int pkgId, unsigned int noElements = 1, bool extended = false, 
+                          size_t trailingData = 0, bool isCustom = false );
+        
+        /**
+        * add a custom packet or series of packets to an RDB message
+        * @param  simTime        simulation time for which to compose the package
+        * @param  simFrmae       simulation frame for which to compose the package
+        * @param  pkgId          id of the package that is to be added to the message 
+        * @param  noElements     number of elements of given package ID type that are to be added
+        * @param  elementSize    size of each element [byte]
+        * @return pointer where to start inserting the data, otherwise 0
+        */
+        void* addCustomPackage( const double & simTime, const unsigned int & simFrame, 
+                                unsigned int pkgId, unsigned int noElements = 1, size_t elementSize = 0 );
+        
+        /**
+        * get a pointer to the message that is currently being composed
+        * @return pointer to current RDB message
+        */
+        RDB_MSG_t* getMsg();
+       
+        /**
+        * get a pointer to the message header that is currently being composed
+        * @return pointer to current RDB message header
+        */
+        RDB_MSG_HDR_t* getMsgHdr();
+        
+        /**
+        * get the total size of the current message
+        * @return total size of the message which is currently being composed
+        */
+        size_t getMsgTotalSize();
+        
+        /**
+        * retrieve the pointer to the first entry of a given type from the internally held message
+        * @param  pkgId          id (i.e. type) of the entry that is to be retrieved 
+        * @param  noElements     number of elements of given entry type that have been found (will be altered)
+        * @param  extended       true if an extended element is to be retrieved
+        * @return pointer to first element of the requested entry type or 0 if none has been found.
+        */
+        void* getFirstEntry( unsigned int pkgId, unsigned int & noElements, bool extended );
+
+        /**
+        * retrieve the pointer to the first entry header of a given type from the internally held message
+        * @param  pkgId          id (i.e. type) of the entry that is to be retrieved 
+        * @param  extended       true if an extended element is to be retrieved
+        * @return pointer to first entry header of the requested entry type or 0 if none has been found.
+        */
+        RDB_MSG_ENTRY_HDR_t* getEntryHdr( unsigned int pkgId, bool extended );
+
+        /**
+        * configure a shared memory segment for the use with RDB
+        * @param startPtr   pointer to the start of the shared memory segment
+        * @param noBuffers  number of buffers which are to be placed within the segment
+        * @param totalSize  total size of SHM segment
+        * @return true if successful
+        */
+        bool shmConfigure( void *startPtr, unsigned int noBuffers, unsigned int totalSize = 0 );
+            
+        /**
+        * set the shared memory pointer
+        * @param shmAddr address of the shared memory segment
+        * @return true if successful
+        */
+        bool shmSetAddress( void* shmAddr );
+            
+        /**
+        * get the pointer to the shared memory header
+        */
+        RDB_SHM_HDR_t* shmGetHdr();
+            
+        /**
+        * get the pointer to a shared memory buffer's info segment
+        * @param index  index of the buffer
+        * @return pointer to the info segment of the respective shared memory buffer
+        */
+        RDB_SHM_BUFFER_INFO_t* shmBufferGetInfo( unsigned int index );
+        
+        /**
+        * get the pointer to a shared memory buffer's data segment
+        * @param index  index of the buffer
+        * @return pointer to the data segment of the respective shared memory buffer
+        */
+        void* shmBufferGetPtr( unsigned int index );
+        
+        /**
+        * update the header information of the shared memory according to buffer data
+        */
+        void shmHdrUpdate();
+                
+        /**
+        * set the size of a shared memory buffer's data segment
+        * @param index  index of the buffer
+        * @param size   size of the buffer's data segment
+        */
+        void shmBufferSetSize( unsigned int index, size_t size );
+                
+        /**
+        * get the size of a shared memory buffer's data segment
+        * @param index  index of the buffer
+        */
+        size_t shmBufferGetSize( unsigned int index );
+                
+        /**
+        * set the id of a shared memory buffer
+        * @param index  index of the buffer
+        * @param id   id of the buffer
+        */
+        void shmBufferSetId( unsigned int index, unsigned int id );
+                
+        /**
+        * set the flags of a shared memory buffer
+        * @param index  index of the buffer
+        * @param flags   flags of the buffer
+        */
+        void shmBufferSetFlags( unsigned int index, unsigned int flags );
+        
+        /**
+        * add the flags of a shared memory buffer
+        * @param index  index of the buffer
+        * @param flags   flags of the buffer
+        */
+        void shmBufferAddFlags( unsigned int index, unsigned int flags );
+        
+        /**
+        * release the flags of a shared memory buffer
+        * @param index  index of the buffer
+        * @param flags   flags of the buffer
+        */
+        void shmBufferReleaseFlags( unsigned int index, unsigned int flags );
+        
+        /**
+        * get the flags of a shared memory buffer
+        * @param index  index of the buffer
+        * @return  flags of the buffer
+        */
+        unsigned int shmBufferGetFlags( unsigned int index );
+        
+        /**
+        * check the flags of a shared memory buffer for a given mask
+        * @param index  index of the buffer
+        * @param mask   mask against which to check
+        * @return true if mask is contained in shm flags
+        */
+        bool shmBufferHasFlags( unsigned int index, unsigned int mask );
+        
+        /**
+        * copy the current message to the shared memory, replacing existing data
+        * @param index  index of the buffer to which the message shall be copied
+        * @param relocateBuffers  if true, buffer locations (i.e. offsets) will be adjusted to size of copied data; this requires buffers to be filled sequentially!
+        * @return true if successful
+        */
+        bool mapMsgToShm( unsigned int index, bool relocateBuffers = true );
+        
+        /**
+        * add a message to the shared memory, extending existing data
+        * @param index  index of the buffer to which the message shall be copied
+        * @param msg    pointer to the message that is to be transferred to SHM
+        * @return true if successful
+        */
+        bool addMsgToShm( unsigned int index, RDB_MSG_t* msg );
+        
+        /**
+        * get the usage of an SHM buffer
+        * @param index  index of the buffer which shall be queried
+        * @return number of bytes occupied in the buffer
+        */
+        unsigned int shmBufferGetUsedSize( unsigned int index );
+        
+        /**
+        * clear the given SHM buffer
+        * @param index  index of the buffer to which is to be cleared
+        * @param force  force clearing of locked buffers
+        * @return true if successful
+        */
+        bool shmBufferClear( unsigned int index, bool force = false );
+        
+        /**
+        * check if a given SHM buffer is locked
+        * @param index  index of the buffer to which is to be checked
+        * @return true if buffer is locked
+        */
+        bool shmBufferIsLocked( unsigned int index );
+        
+        /**
+        * lock a given SHM buffer 
+        * @param index  index of the buffer to which is to be locked
+        * @return true if buffer could be locked
+        */
+        bool shmBufferLock( unsigned int index );
+        
+        /**
+        * release the lock of a given SHM buffer 
+        * @param index  index of the buffer to which is to be released
+        * @return true if buffer could be released
+        */
+        bool shmBufferRelease( unsigned int index );
+        
+        /**
+        * get the number of buffers in the SHM segment
+        * @return number of buffers in SHM segment
+        */
+        unsigned int shmGetNoBuffers();
+        
+        /**
+        * print sizes of all structures
+        */
+        void printPackageSizes();
+        
+        /**
+        * parse an RDB message
+        * @param msg      pointer to the message which is to be parsed;
+        */
+        virtual void parseMessage( RDB_MSG_t* msg );
+        
+        /**
+        * parse an RDB message entry
+        * @param entry      pointer to the message entry which is to be parsed;
+        * @param simTime    simulation time of the message
+        * @param simFrame   simulation frame of the message
+        */
+        virtual void parseMessageEntry( RDB_MSG_ENTRY_HDR_t* entryHdr, const double & simTime, const unsigned int & simFrame );
+        
+    protected:
+        /**
+        * parse the information of a message entry
+        * @param simTime    simulation time of the message
+        * @param simFrame   simulation frame of the message
+        * @param pkgId      id of the package
+        * @param flags      flags of the message entry (e.g. EXTENDED message entry)
+        * @param elemId     id (index) of the current element in the vector of elements of this specific type as contained in the message
+        * @param totalElem  total number of elements in the vector of this specific type as contained in the message
+        */
+        virtual void parseMessageEntryInfo( const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        
+        /**
+        * parse routines for the frame limits
+        * @param simTime    simulation time of the message
+        * @param simFrame   simulation frame of the message
+        */
+        virtual void parseStartOfFrame( const double & simTime, const unsigned int & simFrame );
+        virtual void parseEndOfFrame(   const double & simTime, const unsigned int & simFrame );
+        
+        /**
+        * parse routines for the various message types
+        * @param msg        pointer to the message which is to be parsed;
+        * @param data       pointer to the entry data that is to be parsed
+        * @param simTime    simulation time of the message
+        * @param simFrame   simulation frame of the message
+        * @param pkgId      id of the package
+        * @param flags      flags of the message entry (e.g. EXTENDED message entry)
+        * @param elemId     id (index) of the current element in the vector of elements of this specific type as contained in the message
+        * @param totalElem  total number of elements in the vector of this specific type as contained in the message
+        */
+        virtual void parseEntry( RDB_GEOMETRY_t *                 data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_COORD_SYSTEM_t *             data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_COORD_t *                    data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_ROAD_POS_t *                 data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_LANE_INFO_t *                data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_ROADMARK_t *                 data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_OBJECT_CFG_t *               data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_OBJECT_STATE_t *             data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_VEHICLE_SYSTEMS_t *          data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_VEHICLE_SETUP_t *            data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_ENGINE_t *                   data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_DRIVETRAIN_t *               data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_WHEEL_t *                    data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_PED_ANIMATION_t *            data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_SENSOR_STATE_t *             data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_SENSOR_OBJECT_t *            data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_CAMERA_t *                   data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_CONTACT_POINT_t *            data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_TRAFFIC_SIGN_t *             data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_ROAD_STATE_t *               data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_IMAGE_t *                    data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_LIGHT_SOURCE_t *             data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_ENVIRONMENT_t *              data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_TRIGGER_t *                  data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_DRIVER_CTRL_t *              data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_TRAFFIC_LIGHT_t *            data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_SYNC_t *                     data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_DRIVER_PERCEPTION_t *        data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_FUNCTION_t *                 data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_ROAD_QUERY_t *               data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_POINT_t *                    data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_TRAJECTORY_t *               data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_CUSTOM_SCORING_t *           data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_DYN_2_STEER_t *              data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_SCP_t *                      data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_STEER_2_DYN_t *              data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_PROXY_t *                    data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_MOTION_SYSTEM_t *            data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_FREESPACE_t *                data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_DYN_EL_SWITCH_t *            data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_DYN_EL_DOF_t *               data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_IG_FRAME_t *                 data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_RT_PERFORMANCE_t *           data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+        virtual void parseEntry( RDB_CUSTOM_OBJECT_CTRL_TRACK_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem );
+    private:
+        /**
+        * the actual RDB message that is composed
+        */
+        RDB_MSG_t* mMsg;
+        
+        /**
+        * pointer to the start of the shared memory segment
+        */
+        RDB_SHM_HDR_t* mShmHdr;
+};
+} // namespace Framework
+#endif /* _FRAMEWORK_RDB_HANDLER_HH */

src/ros/catkin/src/vtd_ros/src/VTD/viRDBIcd.h

@@ -0,0 +1,2199 @@
+/*******************************************************
+ * @file
+ * ICD of the Runtime Data Bus (RDB)
+ *
+ * (c) VIRES GmbH
+ * @author Marius Dupuis
+ ********************************************************/
+
+/*****************************************************/
+/**
+   @page RDB_CHANGE_LOG RDB Change Log
+-  27.09.2016  version 0x011D
+               introduced RDB_PKG_ID_RT_PERFORMANCE and RDB_RT_PERFORMANCE_t
+               introduced RDB_PKG_ID_CUSTOM_LIGHT_GROUP_B and RDB_CUSTOM_LIGHT_GROUP_B_t
+               introduced RDB_ROAD_QUERY_FLAG
+-  03.06.2016  version 0x011C
+               introducing ray casting message RDB_PKG_ID_RAY and structure RDB_RAY_t
+               introduced range B of custom messages between RDB_PKG_ID_CUSTOM_USER_B_START and  RDB_PKG_ID_CUSTOM_USER_B_END  
+-  11.03.2016  version 0x011B
+               introducing RDB_DYN_EL_SWITCH_t
+               and RDB_PKG_ID_DYN_EL_SWITCH
+               introducing RDB_DYN_EL_DOF_t
+               and RDB_PKG_ID_DYN_EL_DOF
+               introducing road position flag RDB_ROAD_POS_FLAG_OFFROAD
+               added FOV offsets for sensor information RDB_SENSOR_STATE_t (converted two spares)
+               added RDB_PKG_ID_IG_FRAME and RDB_IG_FRAME_t
+               converted first half of spare in RDB_TRIGGER_t into member "features" to limit update to certain subset of the modules
+-  09.12.2015  version 0x011A
+               added RDB_FREESPACE_t and 
+               RDB_PKG_ID_FREESPACE and
+               RDB_FREESPACE_STATE
+               converted one spare in OBJECT_STATE_EXT to "traveledDist"
+               added RDB_DRIVER_INPUT_VALIDITY_MODIFIED to tag a driver control command which is a combination of
+               an original command and an add_on command
+               converted spares in ROADMARK info to "laneId" and "roadId"
+-  07.12.2015  still version 0x0119
+               added RDB_CUSTOM_TRACK_CTRL_VALIDITY_STEERING_TPOS
+-  25.11.2015  still version 0x0119
+               added RDB_PKG_ID_CUSTOM_LIGHT_B
+               added RDB_CUSTOM_TRACK_CTRL_FLAG_VIS_SENSOR_A 
+               added RDB_CUSTOM_TRACK_CTRL_FLAG_VIS_SENSOR_B 
+               added RDB_CUSTOM_TRACK_CTRL_FLAG_VIS_SENSOR_C 
+               added RDB_CUSTOM_TRACK_CTRL_FLAG_VIS_SENSOR_D
+               added RDB_CUSTOM_TRACK_CTRL_VALIDITY_DEFAULT  
+               added RDB_CUSTOM_TRACK_CTRL_VALIDITY_TGT_ACCEL
+               added RDB_OBJECT_TYPE_VEH_CABIN
+-  09.11.2015  still version 0x0119
+               added RDB_PKG_ID_CUSTOM_LIGHT_B
+               added RDB_PKG_ID_CUSTOM_LIGHT_A
+-  15.09.2015  version 0x0119
+               added RDB_VEHICLE_LIGHT_FORCE
+               added "temperature" to RDB_ENVIRONMENT_t (traded one spare for it)
+               added "temperature" and "oilTemperature" to RDB_ENGINE_EXT_t (traded remaining spares)
+               added "fuelGauge" to VEHICLE_SYSTEMS_t
+               introduced new gear box positions
+               RDB_GEAR_BOX_POS_C  
+               RDB_GEAR_BOX_POS_MS 
+               RDB_GEAR_BOX_POS_CS 
+               RDB_GEAR_BOX_POS_PS 
+               RDB_GEAR_BOX_POS_RS 
+               RDB_GEAR_BOX_POS_NS 
+               RDB_GEAR_BOX_POS_DS 
+               converted spare in RDB_OBJECT_STATE_BASE_t into two configuration parameters, 
+               which will render sending the "heavy" package RDB_OBJECT_CFG_t unnecessary in
+               certain cases
+                   cfgFlags
+                   cfgModelId
+               in order to guarantee backward compatibility with existing implementations, the
+               flag RDB_OBJECT_CFG_FLAG_MODEL_ID_VALID has to be set if the member "cfgModelId"
+               is to be interpreted
+               introduced RDB_COORD_TYPE_TRACK      
+               introduced RDB_PKG_ID_CUSTOM_OBJECT_CTRL_TRACK and associated structure
+               introduced members "neutralPos" and "dampingMaxTorque" in package RDB_DYN_2_STEER_t
+               added further vehicle light identifiers "RDB_OBJECT_TYPE_VEH_LIGHT_xxx"
+               introduced "RDB_CUSTOM_TRACK_CTRL_FLAG" class for custom use case (temporary solution only!!!)
+-  18.02.2015  version 0x0118
+               converted spares in RDB_SYNC_t to systemTime
+               added coordinate type RDB_COORD_TYPE_GEO
+-  05.01.2015  version 0x0117
+               added package RDB_PKG_ID_OCCLUSION_MATRIX (using structure RDB_IMAGE_t)
+-  26.09.2014  version 0x0116
+               added package RDB_PKG_ID_MOTION_SYSTEM
+               added structure RDB_MOTION_SYSTEM_t
+-  12.05.2014  version 0x0115
+               converted unsigned short "spare" in RDB_IMAGE_t into identifier for camera 
+               relative yaw angle in roadmarks is limited to range [-PI;PI]
+-  13.02.2014  still version 0x0114
+               added various player type definitions 
+-  15.11.2013  version 0x0114
+               added "flags" in RDB_WHEEL_BASE_t and associated definitions
+-  13.11.2013  added MATLAB_MEX_FILE conditions
+-  31.10.2013  version 0x0113
+               added further road mark colors
+-  06.09.2013  version 0x0112
+               added proxy packages
+               RDB_PROXY
+               renamed RDB_PKG_ID_RDB_DYN_2_STEER to RDB_PKG_ID_DYN_2_STEER
+               renamed RDB_PKG_ID_RDB_STEER_2_DYN to RDB_PKG_ID_STEER_2_DYN
+               added object types:
+               RDB_OBJECT_TYPE_BUILDING
+               RDB_OBJECT_TYPE_PARKING_SPACE
+               RDB_OBJECT_TYPE_ROAD_WORKS   
+               RDB_OBJECT_TYPE_ROAD_MISC    
+               RDB_OBJECT_TYPE_TUNNEL       
+               RDB_OBJECT_TYPE_LEGACY       
+-  05.08.2013  version 0x0111
+               added steering wheel control structures and definitions
+               RDB_DYN_2_STEER_STATE
+               RDB_DYN_2_STEER_CMD
+               RDB_DYN_2_STEER_EFFECTS
+               RDB_STEER_2_DYN_STATE
+               RDB_STEER_2_DYN_t
+               RDB_DYN_2_STEER_t
+               RDB_SHM_ID_DYN_2_STEER
+               RDB_SHM_ID_STEER_2_DYN
+               RDB_PKG_ID_RDB_DYN_2_STEER
+               RDB_PKG_ID_RDB_STEER_2_DYN
+               added indicator lamp state
+               RDB_VEHICLE_LIGHT_INDICATOR_LAMP_ON
+-  26.07.2013  corrected version definition to 0x0110
+-  10.07.2013  version still 0x0110
+               added RDB_PKG_ID_TRAJECTORY and accompanying definitions
+-  08.07.2013  version 0x0110
+               added OpenGL conform RDB image pix formats
+               old formats now deprecated
+-  02.07.2013  version 0x0110
+               added RDB_OBJECT_VIS_FLAG_RECORDER
+               added RDB_COORD_TYPE_RELATIVE_START
+               added empty or cast definitions for
+                   RDB_END_OF_FRAME_t
+                   RDB_START_OF_FRAME_t
+               added RDB_OBJECT_CATEGORY_OPENDRIVE
+-  22.04.2013  version 0x010F
+               added RDB_DRIVER_SOURCE definitions
+               added RDB_SHM_ID_TC_IN
+               added RDB_SHM_ID_TC_OUT
+               added RDB_SHM_SIZE_TC
+-  13.02.2013  version still 0x010E
+               added shared memory identifier RDB_SHM_ID_CONTROL_GENERATOR_IN
+               added control commands for sync message (RDB_SYNC_CMD_xxx)
+               used a spare of RDB_SYNC_t
+-  22.01.2013  version 0x010E
+               added path s-coordinate to RDB_ROAD_POS_t
+               added RDB_PKG_FLAG_HIDDEN
+-  27.12.2012  version 0x010D
+               converted spare variable to "type" in structure RDB_LANE_INFO_t
+               extended range of road mark IDs
+-  18.12.2012  version still 0x010C (by H.H.)
+               added message type RDB_PKG_ID_OPTIX_BUFFER ( RDBInterface plugin )
+-  03.12.2012: version 0x010C
+-              added package type RDB_PKG_ID_SCP and associated structure
+-  15.11.2012: version still 0x010B
+-              improved description of occlusion
+-  10.10.2012: version still 0x010B
+-              added RDB_ROAD_TYPE definitions and member "type" in RDB_ROAD_POS_t
+-  02.10.2012: version still 0x010B
+-              introduced RDB_LIGHT_SOURCE_FLAG_STENCIL
+-              introduced RDB_COORD_TYPE_TEXTURE
+-  01.10.2012: version set to 0x010B
+-              activated one spare parameter in RDB_VEHICLE_SYSTEMS_t
+-  18.09.2012: version set to 0x010A
+-              added further gearbox positions
+-              added flags for mockup inputs: RDB_MOCKUP_INPUT0_, RDB_MOCKUP_INPUT1_, RDB_MOCKUP_INPUT2_
+-              introduced mockup input flags in RDB_DRIVER_CTRL_t
+-  06.09.2012: version set to 0x0109
+-              corrected frameNo type mismatch
+-  27.08.2012: version set to 0x0108
+-              added RDB_OBJECT_TYPE_PLAYER_TRAILER
+-              added "spare0" in RDB_LANE_INFO_t between "status" and "width"
+-              to comply with 4-byte alignment. The space has been there before
+-              due to the compiler and is now adressable. So, don't panic! Your
+-              "old" code should still work.
+-  06.08.2012: version set to 0x0107
+-              added RDB_PKG_ID_ROAD_QUERY and associated structures
+-  04.07.2012: added definition RDB_PIX_FORMAT_BW_32
+               added definition RDB_PIX_FORMAT_RGB_32
+               added definition RDB_PIX_FORMAT_RGBA_32
+-              version set to 0x0106
+-  01.05.2012: added some driver control flags (headlight etc.)
+-              version set to 0x0105
+-  17.04.2012: corrected comment in RDB_OBJECT_CFG_t
+-  12.04.2012: introduced RDB_DRIVER_INPUT_VALIDITY_ADD_ON
+-  26.03.2012: set version to 0x0104
+-              added sourceId in structure RDB_DRIVER_CTRL_t (converted one spare uint8 for this)
+-  28.02.2012: set version to 0x0103
+-              set occlusion in SENSOR_OBJECT_t to type int8_t instead of uint8_t
+-              introduced RDB_OBJECT_CATEGORY_COMMON
+-              introduced RDB_OBJECT_TYPE_NONE
+-  09.02.2012: introduced RDB_ENV_FLAG_STREET_LAMPS
+-  31.01.2012: introduced RDB_LIGHT_SOURCE_FLAG_PERSISTENT
+-  22.01.2012: set version to 0x0102
+-              introduced RDB_CONTACT_POINT_FLAG_...
+-  10.12.2011: set version to 0x0101
+-              modified definition of co-ordinate systems
+-              RDB_COORD_TYPE_DEFAULT  has become    RDB_COORD_TYPE_INERTIAL
+-              RDB_COORD_TYPE_INERTIAL has become    RDB_COORD_TYPE_RESERVED_1
+-  17.11.2011: introduced RDB_COORD_TYPE_WINDOW
+-  03.11.2011: converted two spares in RDB_ENGINE_EXT_t into variables
+-              for current and average fuel consumptions
+-  10.10.2011: inserted RDB_GEAR_BOX_POS_5
+-  15.09.2011: added definition RDB_PIX_FORMAT_DEPTH_32
+-  16.08.2011: introduced "parent" as member in OBJECT_STATE_BASE_t
+-              introduced RDB_OBJECT_CATEGORY_LIGHT_POINT
+-  02.08.2011: added flags for direction info to RDB_ROAD_POS_t
+-  14.07.2011: added playerId to LANE_INFO
+-  10.07.2011: added steeringWheelTorque to VEHICLE_SYSTEMS
+-  08.07.2011: introduced RDB_DRIVER_INPUT_VALIDITY_INFO_ONLY
+-  07.07.2011: introduced visMask in RDB_OBJECT_STATE_t
+-  28.06.2011: introduced structure RDB_FUNCTION_t
+-              introduced packages RDB_PKG_ID_LIGHT_MAP and
+-              RDB_PKG_ID_TONE_MAPPING
+-  30.05.2011: introduced member "material" in RDB_LANE_INFO_t
+-              introduced member "addOnId" in RDB_TRAFFIC_SIGN_t; this shall
+-              point to a sign which extends the original sign; 0 for no extension
+-  20.05.2011: fixed a GSI typo, added RDB_PIX_FORMAT_CUSTOM_01
+-  17.05.2011: added some picture formats
+-  18.04.2011: introduced RDB_PKG_ID_DRIVER_PERCEPTION
+-  31.03.2011: introduced RDB_PKG_ID_SYNC
+-              introduced RDB_OBJECT_CFG_FLAG
+-  14.03.2011: updated comment fields of @unit
+-              added shared memory header
+-  07.03.2011: deleted "throttlePos" from RDB_ENGINE_EXT_t since
+-              "load" is already in BASE package
+-              introduced "RDB_DRIVER_INPUT_VALIDITY_CURVATURE" and
+-              curvatureTgt in "RDB_DRIVER_CTRL_t"
+-              introduced "RDB_DRIVER_INPUT_VALIDITY_STEERING_TORQUE" and
+-              steeringTorque in "RDB_DRIVER_CTRL_t"
+-              introduced "RDB_DRIVER_INPUT_VALIDITY_ENGINE_TORQUE" and
+-              engineTorqueTgt in "RDB_DRIVER_CTRL_t"
+-              introduced "RDB_DRIVER_INPUT_VALIDITY_SPEED" and
+-              speedTgt in "RDB_DRIVER_CTRL_t"
+-              increased "validityFlags" in "RDB_DRIVER_CTRL_t" from
+-              16 to 32 bit.
+-              RDB_WHEEL_BASE_t: replaced "deflZ" with "springCompression"
+-              RDB_WHEEL_EXT_t: deleted "springCompression"
+-  09.02.2011: initial version will be 0x0100
+-              01 = major version
+-              00 = minor version
+-              major version will change when binary
+-              incompatibility is imminent, otherwise
+-              minor version will change
+-  08.02.2011: fixed minor bug in wheel structure
+-              set initial revision to 0x000E
+-  14.01.2011: replaced "class" with "category"
+-  18.11.2010: added feedback from review
+-              modified general remarks
+-  28.10.2010: added feedback from review
+-              added general remarks
+-  17.08.2010: draft version 0x0100
+-  03.08.2010: derived from GSI version 0x000D++
+*********************************************************/
+
+/**
+* <h2> General Remarks: </h2>
+*
+* <h3> Introduction: </h3>
+*
+* RDB contains a wide variety of packages, most of which
+* are (potentially) well suited for most applications. 
+* Nevertheless, several things have to be noted:
+* - not all applications will use or provide
+*   all sorts of packages
+* - when a package is provided all of its elements must
+*   be filled with valid data (in case of doubt, initialize
+*   the packages with 0 before filling the respective data)
+* - some packages are specialized so that they work
+*   well only in certain environments; these packages
+*   bear the string "CUSTOM" in their name and have
+*   IDs in the range 10000-19999
+*
+*
+* <h3> Packages with Trailing Data: </h3>
+*
+* Some packages may be followed by trailing data of flexible size.
+* In this case, a package itself contains information about the
+* number of bytes that will follow the original package. The 
+* type and content of the trailing data depend on the package
+* type.
+*
+*
+* <h3> Extended Information: </h3>
+*
+* Packages are defined for maximum flexibility. This means
+* that for the complete description of a complex object more
+* data may be required than for a simple object of comparable
+* type.
+*
+* Therefore, the information about an object may be contained
+* in a basic structure followed immediately - within the same
+* package - by an additional information block (so-called extension).
+* In the respective package's header (of type RDB_MSG_ENTRY_HDR_t) 
+* the member "flags" will have the flag RDB_PKG_FLAG_EXTENDED set 
+* in case of an extended package. 
+* 
+* Note that if multiple elements of the same package type are
+* packed in a package vector, all of these elements must be
+* either of basic type or of extended type. Mixing these types
+* is not allowed.
+*
+* For easier casting of basic and extended packages, containers
+* are provided which provide one member of the basic information
+* followed by one member holding the extended information.
+*
+* The following package extensions are possible:
+*
+* <h4> A) OBJECTS (PKG ID = RDB_PKG_ID_OBJECT_STATE) </h4>
+*
+* information about a static object (e.g. obstacle):
+*   package contains only 
+*       RDB_OBJECT_STATE_BASE_t
+*
+* information about a dynamic object (e.g. vehicle)
+*   package contains  
+*       RDB_OBJECT_STATE_BASE_t
+*   followed by
+*       RDB_OBJECT_STATE_EXT_t
+*
+* casting structure:
+* @code
+* {
+*   RDB_OBJECT_STATE_BASE_t base;
+*   RDB_OBJECT_STATE_EXT_t  ext;
+* } RDB_OBJECT_STATE_t;
+* @endcode
+*
+* <h4> B) WHEELS (PKG ID = RDB_PKG_ID_WHEEL) </h4>
+*
+* basic information about a wheel:
+*   package contains only 
+*       RDB_WHEEL_BASE_t
+*
+* extended information about a wheel
+*   package contains  
+*       RDB_WHEEL_BASE_t
+*   followed by
+*       RDB_WHEEL_EXT_t
+*
+* casting structure:
+* @code
+* {
+*   RDB_WHEEL_BASE_t base;
+*   RDB_WHEEL_EXT_t  ext;
+* } RDB_WHEEL_t;
+* @endcode
+*
+*
+* <h4> C) ENGINE (PKG ID = RDB_PKG_ID_ENGINE) </h4>
+*
+* basic information about an engine:
+*   package contains only 
+*       RDB_ENGINE_BASE_t
+*
+* extended information about an engine
+*   package contains  
+*       RDB_ENGINE_BASE_t
+*   followed by
+*       RDB_ENGINE_EXT_t
+*
+* casting structure:
+* @code
+* {
+*   RDB_ENGINE_BASE_t base;
+*   RDB_ENGINE_EXT_t  ext;
+* } RDB_ENGINE_t;
+* @endcode
+*
+*
+* <h4> D) DRIVETRAIN (PKG ID = RDB_PKG_ID_DRIVETRAIN) </h4>
+*
+* basic information about a drivetrain:
+*   package contains only 
+*       RDB_DRIVETRAIN_BASE_t
+*
+* extended information about a drivetrain
+*   package contains  
+*       RDB_DRIVETRAIN_BASE_t
+*   followed by
+*       RDB_DRIVETRAIN_EXT_t
+*
+* casting structure:
+* @code
+* {
+*   RDB_DRIVETRAIN_BASE_t base;
+*   RDB_DRIVETRAIN_EXT_t  ext;
+* } RDB_DRIVETRAIN_t;
+* @endcode
+*
+*
+* <h4> E) TRAFFIC LIGHT (PKG ID = RDB_PKG_ID_TRAFFIC_LIGHT) </h4>
+*
+* basic information about a traffic light:
+*   package contains only 
+*       RDB_TRAFFIC_LIGHT_BASE_t
+*
+* extended information about a traffic light
+*   package contains  
+*       RDB_TRAFFIC_LIGHT_BASE_t
+*   followed by
+*       RDB_TRAFFIC_LIGHT_EXT_t
+*
+* casting structure:
+* @code
+* {
+*   RDB_TRAFFIC_LIGHT_BASE_t base;
+*   RDB_TRAFFIC_LIGHT_EXT_t  ext;
+* } RDB_TRAFFIC_LIGHT_t;
+* @endcode
+*/
+
+#pragma pack (push, 4)
+
+#ifndef _VIRES_RDB_ICD_H
+#define _VIRES_RDB_ICD_H
+
+/* includes for 64bit compatibility */
+#ifdef MATLAB_MEX_FILE
+    #include <sys/types.h>
+    #include "viRDBTypes.h"
+#else
+    #include <stdint.h>
+#endif
+
+
+/** @addtogroup GENERAL_DEFINITIONS
+ *  @{
+ */
+#define RDB_DEFAULT_PORT   48190       /**< default port for RDB communication      @version 0x0100 */
+#define RDB_FEEDBACK_PORT  48191       /**< port for RDB feedback to taskControl    @version 0x0100 */
+#define RDB_IMAGE_PORT     48192       /**< port for RDB image data                 @version 0x0100 */
+#define RDB_MAGIC_NO       35712       /**< magic number                            @version 0x0100 */
+#define RDB_VERSION       0x011D       /**< upper byte = major, lower byte = minor  @version 0x011D */
+/** @} */
+
+/** @addtogroup ARRAY_SIZES
+ *  ------ array sizes ------
+ *  @{
+ */
+#define RDB_SIZE_OBJECT_NAME       32       /**< maximum length of an object's name                 @version 0x0100 */
+#define RDB_SIZE_SCP_NAME          64       /**< maximum length of an SCP sender / receiver         @version 0x010C */
+#define RDB_SIZE_FILENAME        1024       /**< number of bytes in a filename (may include path)   @version 0x0100 */
+#define RDB_SIZE_TRLIGHT_PHASES     8       /**< maximum number of phases for a traffic light       @version 0x0100 */
+/** @} */
+
+
+/** @addtogroup ENUM_DEFINITIONS
+ *  ------ Enum Definitions ------
+ *  @{
+ */
+/** @addtogroup RDB_PKG_ID
+ *  ------ message types ------
+ *  @{
+ */
+#define RDB_PKG_ID_START_OF_FRAME               1     /**< sent as first package of a simulation frame                      @version 0x0100 */
+#define RDB_PKG_ID_END_OF_FRAME                 2     /**< sent as last package of a simulation frame                       @version 0x0100 */
+#define RDB_PKG_ID_COORD_SYSTEM                 3     /**< defines a (custom) co-ordinate system                            @version 0x0100 */
+#define RDB_PKG_ID_COORD                        4     /**< single co-ordinate extending previous object information         @version 0x0100 */
+#define RDB_PKG_ID_ROAD_POS                     5     /**< detailed road position of a given entity                         @version 0x0100 */
+#define RDB_PKG_ID_LANE_INFO                    6     /**< lane information for a given entity                              @version 0x0100 */
+#define RDB_PKG_ID_ROADMARK                     7     /**< road mark information of a player (typically EGO)                @version 0x0100 */
+#define RDB_PKG_ID_OBJECT_CFG                   8     /**< object configuration information                                 @version 0x0100 */
+#define RDB_PKG_ID_OBJECT_STATE                 9     /**< state of a standard (static) object                              @version 0x0100 */
+#define RDB_PKG_ID_VEHICLE_SYSTEMS             10     /**< vehicle systems' states (lights etc.)                            @version 0x0100 */
+#define RDB_PKG_ID_VEHICLE_SETUP               11     /**< basic information about a vehicle (mass etc.)                    @version 0x0100 */
+#define RDB_PKG_ID_ENGINE                      12     /**< info about a vehicle's engine                                    @version 0x0100 */
+#define RDB_PKG_ID_DRIVETRAIN                  13     /**< info about a vehicle's drivetrain                                @version 0x0100 */
+#define RDB_PKG_ID_WHEEL                       14     /**< info about a wheel of a player                                   @version 0x0100 */
+#define RDB_PKG_ID_PED_ANIMATION               15     /**< pedestrian animation details (joint angles etc.)                 @version 0x0100 */
+#define RDB_PKG_ID_SENSOR_STATE                16     /**< state (position etc.) of a sensor                                @version 0x0100 */
+#define RDB_PKG_ID_SENSOR_OBJECT               17     /**< information about an object registered within a sensor           @version 0x0100 */
+#define RDB_PKG_ID_CAMERA                      18     /**< camera parameters corresponding to video image                   @version 0x0100 */
+#define RDB_PKG_ID_CONTACT_POINT               19     /**< road data at a given contact point                               @version 0x0100 */
+#define RDB_PKG_ID_TRAFFIC_SIGN                20     /**< info about traffic signs objects                                 @version 0x0100 */
+#define RDB_PKG_ID_ROAD_STATE                  21     /**< road state information for a given player                        @version 0x0100 */
+#define RDB_PKG_ID_IMAGE                       22     /**< video image                                                      @version 0x0100 */
+#define RDB_PKG_ID_LIGHT_SOURCE                23     /**< light source information                                         @version 0x0100 */
+#define RDB_PKG_ID_ENVIRONMENT                 24     /**< environment information                                          @version 0x0100 */
+#define RDB_PKG_ID_TRIGGER                     25     /**< trigger info for next simulation frame                           @version 0x0100 */
+#define RDB_PKG_ID_DRIVER_CTRL                 26     /**< info about mockup or from driver module as input for dynamics    @version 0x0100 */
+#define RDB_PKG_ID_TRAFFIC_LIGHT               27     /**< information about a traffic lights and their states              @version 0x0100 */
+#define RDB_PKG_ID_SYNC                        28     /**< synchronization with external RDB client                         @version 0x0100 */
+#define RDB_PKG_ID_DRIVER_PERCEPTION           29     /**< driver perception                                                @version 0x0100 */
+#define RDB_PKG_ID_LIGHT_MAP                   30     /**< light map for a headlight                                        @version 0x0100 */
+#define RDB_PKG_ID_TONE_MAPPING                31     /**< tone mapping function                                            @version 0x0100 */
+#define RDB_PKG_ID_ROAD_QUERY                  32     /**< co-ordinates of an explicit ODR road query                       @version 0x0107 */
+#define RDB_PKG_ID_SCP                         33     /**< SCP package via RDB                                              @version 0x010C */
+#define RDB_PKG_ID_TRAJECTORY                  34     /**< trajectory for path planning                                     @version 0x0110 */
+#define RDB_PKG_ID_DYN_2_STEER                 35     /**< information from dynamics to steering wheel                      @version 0x0111 */
+#define RDB_PKG_ID_STEER_2_DYN                 36     /**< information from steering wheel to dynamics                      @version 0x0111 */
+#define RDB_PKG_ID_PROXY                       37     /**< proxy package handler                                            @version 0x0112 */
+#define RDB_PKG_ID_MOTION_SYSTEM               38     /**< information about motion system                                  @version 0x0116 */
+#define RDB_PKG_ID_OCCLUSION_MATRIX            39     /**< occlusion matrix of a sensor object                              @version 0x0117 */
+#define RDB_PKG_ID_FREESPACE                   40     /**< description of a single freespace object                         @version 0x0119 */
+#define RDB_PKG_ID_DYN_EL_SWITCH               41     /**< control of a dynamic switch element                              @version 0x011B */
+#define RDB_PKG_ID_DYN_EL_DOF                  42     /**< control of a dynamic DOF element                                 @version 0x011B */
+#define RDB_PKG_ID_IG_FRAME                    43     /**< info about frame received by IG                                  @version 0x011B */
+#define RDB_PKG_ID_RAY                         44     /**< information about a (sensor) ray                                 @version 0x011C */
+#define RDB_PKG_ID_RT_PERFORMANCE              45     /**< real-time performance monitorng                                  @version 0x011D */
+
+
+/** @} */
+
+/** @addtogroup RDB_PKG_ID_CUSTOM
+ *  ------ custom message types, not to be expected in "standard" simulations ------
+ *  @{
+ */
+#define RDB_PKG_ID_CUSTOM_SCORING           10000     /**< parameters for driver scoring (efficiency, safety, race etc.)    @version 0x0100 */
+#define RDB_PKG_ID_CUSTOM_OBJECT_CTRL_TRACK 10001     /**< control of an object using track information                     @version 0x0119 */
+#define RDB_PKG_ID_CUSTOM_LIGHT_B           10002     /**< custom light control package                                     @version 0x0119 */
+#define RDB_PKG_ID_CUSTOM_LIGHT_A           10003     /**< custom light control package                                     @version 0x0119 */
+#define RDB_PKG_ID_CUSTOM_LIGHT_GROUP_B     10004     /**< custom light control package                                     @version 0x011D */
+#define RDB_PKG_ID_CUSTOM_AUDI_FORUM        12000     /**< parameters for AUDI Forum                                        @version 0x0100 */
+#define RDB_PKG_ID_CUSTOM_OPTIX_START       12100     /**< start of custom packages for OPTIX applications                  @version 0x0100 */
+#define RDB_PKG_ID_OPTIX_BUFFER             12101     /**< custom optix buffer for RDBInterface Plugin                      @version 0x010C */
+#define RDB_PKG_ID_CUSTOM_OPTIX_END         12149     /**< end of custom packages for OPTIX applications                    @version 0x0100 */
+#define RDB_PKG_ID_CUSTOM_USER_A_START      12150     /**< start of custom packages for user A                              @version 0x0106 */
+#define RDB_PKG_ID_CUSTOM_USER_A_END        12174     /**< end of custom packages for user A                                @version 0x0106 */
+#define RDB_PKG_ID_CUSTOM_USER_B_START      12175     /**< start of custom packages for user B                              @version 0x011C */
+#define RDB_PKG_ID_CUSTOM_USER_B_END        12200     /**< end of custom packages for user B                                @version 0x011C */
+/** @} */
+
+/** @addtogroup RDB_OBJECT_CATEGORY
+ *  ------ object types ------
+ *  @{
+ */
+#define RDB_OBJECT_CATEGORY_NONE           0    /**< no category defined           @version 0x0100 */
+#define RDB_OBJECT_CATEGORY_PLAYER         1    /**< category is player            @version 0x0100 */
+#define RDB_OBJECT_CATEGORY_SENSOR         2    /**< category is sensor            @version 0x0100 */
+#define RDB_OBJECT_CATEGORY_CAMERA         3    /**< category is camera            @version 0x0100 */
+#define RDB_OBJECT_CATEGORY_LIGHT_POINT    4    /**< category is light point       @version 0x0100 */
+#define RDB_OBJECT_CATEGORY_COMMON         5    /**< category is common object     @version 0x0100 */
+#define RDB_OBJECT_CATEGORY_OPENDRIVE      6    /**< category is OpenDRIVE object  @version 0x0110 */
+/** @} */
+
+/** @addtogroup RDB_OBJECT_TYPE
+ *  ------ object types ------
+ *  @{
+ */
+#define RDB_OBJECT_TYPE_NONE                   0    /**< undefined object type for categories other than player         @version 0x0100 */
+#define RDB_OBJECT_TYPE_PLAYER_NONE            0    /**< undefined player type             			 					@version 0x0100 */
+#define RDB_OBJECT_TYPE_PLAYER_CAR             1    /**< player of type car                                             @version 0x0100 */
+#define RDB_OBJECT_TYPE_PLAYER_TRUCK           2    /**< player of type truck                                           @version 0x0100 */
+#define RDB_OBJECT_TYPE_PLAYER_VAN             3    /**< player of type van                                             @version 0x0100 */
+#define RDB_OBJECT_TYPE_PLAYER_BIKE            4    /**< player of type bicycle                                         @version 0x0100 */
+#define RDB_OBJECT_TYPE_PLAYER_PEDESTRIAN      5    /**< player of type pedestrian                                      @version 0x0100 */
+#define RDB_OBJECT_TYPE_PLAYER_PED_GROUP       6    /**< player of type pedestrian group                                @version 0x0100 */
+#define RDB_OBJECT_TYPE_POLE                   7    /**< pole                                                           @version 0x0100 */
+#define RDB_OBJECT_TYPE_TREE                   8    /**< tree                                                           @version 0x0100 */
+#define RDB_OBJECT_TYPE_BARRIER                9    /**< barrier                                                        @version 0x0100 */
+#define RDB_OBJECT_TYPE_OPT1                  10    /**< optional user type 1                                           @version 0x0100 */
+#define RDB_OBJECT_TYPE_OPT2                  11    /**< optional user type 2                                           @version 0x0100 */
+#define RDB_OBJECT_TYPE_OPT3                  12    /**< optional user type 3                                           @version 0x0100 */
+#define RDB_OBJECT_TYPE_PLAYER_MOTORBIKE      13    /**< player of type motorbike                                       @version 0x0100 */
+#define RDB_OBJECT_TYPE_PLAYER_BUS            14    /**< player of type bus                                             @version 0x0100 */
+#define RDB_OBJECT_TYPE_STREET_LAMP           15    /**< street lamp                                                    @version 0x0100 */
+#define RDB_OBJECT_TYPE_TRAFFIC_SIGN          16    /**< traffic sign                                                   @version 0x0100 */
+#define RDB_OBJECT_TYPE_HEADLIGHT             17    /**< headlights                                                     @version 0x0100 */
+#define RDB_OBJECT_TYPE_PLAYER_TRAILER        18    /**< player of type trailer                                         @version 0x0108 */
+#define RDB_OBJECT_TYPE_BUILDING              19    /**< object of type building                                        @version 0x0108 */
+#define RDB_OBJECT_TYPE_PARKING_SPACE         20    /**< object of type parking space                                   @version 0x0112 */
+#define RDB_OBJECT_TYPE_ROAD_WORKS            21    /**< object for road works                                          @version 0x0112 */
+#define RDB_OBJECT_TYPE_ROAD_MISC             22    /**< miscellaneous road object                                      @version 0x0112 */
+#define RDB_OBJECT_TYPE_TUNNEL                23    /**< object for tunnel environment                                  @version 0x0112 */
+#define RDB_OBJECT_TYPE_LEGACY                24    /**< legacy object (not to be used)                                 @version 0x0112 */
+#define RDB_OBJECT_TYPE_VEGETATION            25    /**< common vegetation object                                       @version 0x0114 */
+#define RDB_OBJECT_TYPE_MISC_MOTORWAY         26    /**< common motorway object                                         @version 0x0114 */
+#define RDB_OBJECT_TYPE_MISC_TOWN             27    /**< common town object                                             @version 0x0114 */
+#define RDB_OBJECT_TYPE_PATCH                 28    /**< patch on road                                                  @version 0x0114 */
+#define RDB_OBJECT_TYPE_OTHER                 29    /**< any other unspecified object                                   @version 0x0114 */
+#define RDB_OBJECT_PLAYER_SEMI_TRAILER        30    /**< player of type semi trailer                                    @version 0x0114 */
+#define RDB_OBJECT_PLAYER_RAILCAR             31    /**< player of type rail car                                        @version 0x0114 */
+#define RDB_OBJECT_PLAYER_RAILCAR_SEMI_HEAD   32    /**< player of type rail car semi, head                             @version 0x0114 */
+#define RDB_OBJECT_PLAYER_RAILCAR_SEMI_BACK   33    /**< player of type rail car semi, back                             @version 0x0114 */
+#define RDB_OBJECT_TYPE_VEH_LIGHT_FRONT_LEFT  34    /**< headlight front left                                           @version 0x0119 */
+#define RDB_OBJECT_TYPE_VEH_LIGHT_FRONT_RIGHT 35    /**< headlight front right                                          @version 0x0119 */
+#define RDB_OBJECT_TYPE_VEH_LIGHT_REAR_LEFT   36    /**< tail light left                                                @version 0x0119 */
+#define RDB_OBJECT_TYPE_VEH_LIGHT_REAR_RIGHT  37    /**< tail light right                                               @version 0x0119 */
+#define RDB_OBJECT_TYPE_VEH_CABIN             38    /**< articulated cabin (e.g. for trucks), must have parent          @version 0x0119 */
+/** @} */
+
+/** @addtogroup RDB_LANE_BORDER
+ *  ------ lane border types ------
+ *  @{
+ */
+#define RDB_LANE_BORDER_UNKNOWN          0      /**< unknown border type    @version 0x0100 */
+#define RDB_LANE_BORDER_NONE             1      /**< no border              @version 0x0100 */
+#define RDB_LANE_BORDER_POLE             2      /**< pole border            @version 0x0100 */
+#define RDB_LANE_BORDER_BARRIER          3      /**< barrier border         @version 0x0100 */
+#define RDB_LANE_BORDER_SOFT_SHOULDER    4      /**< soft shoulder border   @version 0x0100 */
+#define RDB_LANE_BORDER_HARD_SHOULDER    5      /**< hard shoulder border   @version 0x0100 */
+#define RDB_LANE_BORDER_CURB             6      /**< curb border */
+/** @} */
+
+/** @addtogroup RDB_ROADMARK_TYPE
+ *  ------ road mark types ------
+ *  @{
+ */
+#define RDB_ROADMARK_TYPE_NONE           0      /**< no roadmark defined    @version 0x0100 */
+#define RDB_ROADMARK_TYPE_SOLID          1      /**< solid marks            @version 0x0100 */
+#define RDB_ROADMARK_TYPE_BROKEN         2      /**< broken marks           @version 0x0100 */
+#define RDB_ROADMARK_TYPE_CURB           3      /**< curb                   @version 0x0100 */
+#define RDB_ROADMARK_TYPE_GRASS          4      /**< grass                  @version 0x0100 */
+#define RDB_ROADMARK_TYPE_BOTDOT         5      /**< Botts' dots            @version 0x0100 */
+#define RDB_ROADMARK_TYPE_OTHER          6      /**< something else         @version 0x0100 */
+/** @} */
+
+/** @addtogroup RDB_ROADMARK_COLOR
+ *  ------ road mark colors ------
+ *  @{
+ */
+#define RDB_ROADMARK_COLOR_NONE          0      /**< no color defined   @version 0x0100 */
+#define RDB_ROADMARK_COLOR_WHITE         1      /**< white color        @version 0x0100 */
+#define RDB_ROADMARK_COLOR_RED           2      /**< red color          @version 0x0100 */
+#define RDB_ROADMARK_COLOR_YELLOW        3      /**< yellow color       @version 0x0100 */
+#define RDB_ROADMARK_COLOR_OTHER         4      /**< other color        @version 0x0100 */
+#define RDB_ROADMARK_COLOR_BLUE          5      /**< blue color         @version 0x0113 */
+#define RDB_ROADMARK_COLOR_GREEN         6      /**< green color        @version 0x0113 */
+/** @} */
+
+/** @addtogroup RDB_WHEEL_ID
+ *  ------ wheel indices ------
+ *  @{
+ */
+#define RDB_WHEEL_ID_FRONT_LEFT          0      /**< increase ID clockwise; next one is front right wheel (not on trikes)   @version 0x0100 */
+/** @} */
+
+/** @addtogroup RDB_GEAR_BOX_TYPE
+ *  ------ gear box types ------
+ *  @{
+ */
+#define RDB_GEAR_BOX_TYPE_AUTOMATIC      0      /**< automatic gear shift   @version 0x0100 */
+#define RDB_GEAR_BOX_TYPE_MANUAL         1      /**< manual gear shift      @version 0x0100 */
+/** @} */
+
+/** @addtogroup RDB_GEAR_BOX_POS
+ *  ------ gear box positions ------
+ *  @{
+ */
+#define RDB_GEAR_BOX_POS_AUTO             0     /**< gear set to automatic                 @version 0x0100 */
+#define RDB_GEAR_BOX_POS_P                1     /**< park                                  @version 0x0100 */
+#define RDB_GEAR_BOX_POS_R                2     /**< reverse                               @version 0x0100 */
+#define RDB_GEAR_BOX_POS_N                3     /**< neutral                               @version 0x0100 */
+#define RDB_GEAR_BOX_POS_D                4     /**< drive                                 @version 0x0100 */
+#define RDB_GEAR_BOX_POS_1                5     /**< 1st gear                              @version 0x0100 */
+#define RDB_GEAR_BOX_POS_2                6     /**< 2nd gear                              @version 0x0100 */
+#define RDB_GEAR_BOX_POS_3                7     /**< 3rd gear                              @version 0x0100 */
+#define RDB_GEAR_BOX_POS_4                8     /**< 4th gear                              @version 0x0100 */
+#define RDB_GEAR_BOX_POS_5                9     /**< 5th gear                              @version 0x0100 */
+#define RDB_GEAR_BOX_POS_6               10     /**< 6th gear                              @version 0x0100 */
+#define RDB_GEAR_BOX_POS_7               11     /**< 7th gear                              @version 0x0100 */
+#define RDB_GEAR_BOX_POS_8               12     /**< 8th gear                              @version 0x0100 */
+#define RDB_GEAR_BOX_POS_9               13     /**< 9th gear                              @version 0x0100 */
+#define RDB_GEAR_BOX_POS_10              14     /**< 10th gear                             @version 0x0100 */
+#define RDB_GEAR_BOX_POS_11              15     /**< 11th gear                             @version 0x0100 */
+#define RDB_GEAR_BOX_POS_12              16     /**< 12th gear                             @version 0x0100 */
+#define RDB_GEAR_BOX_POS_13              17     /**< 13th gear                             @version 0x0100 */
+#define RDB_GEAR_BOX_POS_14              18     /**< 14th gear                             @version 0x0100 */
+#define RDB_GEAR_BOX_POS_15              19     /**< 15th gear                             @version 0x0100 */
+#define RDB_GEAR_BOX_POS_16              20     /**< 16th gear                             @version 0x0100 */
+#define RDB_GEAR_BOX_POS_R1              21     /**< 1st reverse gear                      @version 0x0100 */
+#define RDB_GEAR_BOX_POS_R2              22     /**< 2nd reverse gear                      @version 0x0100 */
+#define RDB_GEAR_BOX_POS_R3              23     /**< 3rd reverse gear                      @version 0x0100 */
+#define RDB_GEAR_BOX_POS_M               24     /**< manual shift position (tiptronic)     @version 0x010A */
+#define RDB_GEAR_BOX_POS_M_UP            25     /**< manual shift up                       @version 0x010A */
+#define RDB_GEAR_BOX_POS_M_DOWN          26     /**< manual shift down                     @version 0x010A */
+#define RDB_GEAR_BOX_POS_C               27     /**< position C                            @version 0x0119 */
+#define RDB_GEAR_BOX_POS_MS              28     /**< position MS                           @version 0x0119 */
+#define RDB_GEAR_BOX_POS_CS              29     /**< position CS                           @version 0x0119 */
+#define RDB_GEAR_BOX_POS_PS              30     /**< position PS                           @version 0x0119 */
+#define RDB_GEAR_BOX_POS_RS              31     /**< position RS                           @version 0x0119 */
+#define RDB_GEAR_BOX_POS_NS              32     /**< position NS                           @version 0x0119 */
+#define RDB_GEAR_BOX_POS_DS              33     /**< position DS                           @version 0x0119 */
+/** @} */
+
+/** @addtogroup RDB_DRIVETRAIN_TYPE
+ *  ------ drivetrain types ------
+ *  @{
+ */
+#define RDB_DRIVETRAIN_TYPE_FRONT        0      /**< front-wheel drive  @version 0x0100 */
+#define RDB_DRIVETRAIN_TYPE_REAR         1      /**< rear-wheel drive   @version 0x0100 */
+#define RDB_DRIVETRAIN_TYPE_AWD          2      /**< all-wheel drive    @version 0x0100 */
+/** @} */
+
+/** @addtogroup RDB_PIX_FORMAT
+ *  ------ image pixel formats ------
+ *  @{
+ */
+#define RDB_PIX_FORMAT_RGB              0  /**< deprecated: use RDB_PIX_FORMAT_R3_G2_B2        @version 0x0100 */
+#define RDB_PIX_FORMAT_RGB_16           1  /**< deprecated: use RDB_PIX_FORMAT_R5_G6_B5        @version 0x0100 */
+#define RDB_PIX_FORMAT_RGB_24           2  /**< deprecated: use RDB_PIX_FORMAT_RGB8            @version 0x0100 */
+#define RDB_PIX_FORMAT_RGBA             3  /**< deprecated: use RDB_PIX_FORMAT_R3_G2_B2_A8     @version 0x0100 */
+#define RDB_PIX_FORMAT_RGBA_16          4  /**< deprecated: use RDB_PIX_FORMAT_R5_G6_B5_A16    @version 0x0100 */
+#define RDB_PIX_FORMAT_RGBA_24          5  /**< deprecated: use RDB_PIX_FORMAT_RGB8_A24        @version 0x0100 */
+#define RDB_PIX_FORMAT_BW_8             6  /**< deprecated: use RDB_PIX_FORMAT_RED8            @version 0x0100 */
+#define RDB_PIX_FORMAT_BW_16            7  /**< deprecated: use RDB_PIX_FORMAT_RED16           @version 0x0100 */
+#define RDB_PIX_FORMAT_BW_24            8  /**< deprecated: use RDB_PIX_FORMAT_RED24           @version 0x0100 */
+#define RDB_PIX_FORMAT_DEPTH_8          9  /**< deprecated: use RDB_PIX_FORMAT_DEPTH8          @version 0x0100 */
+#define RDB_PIX_FORMAT_DEPTH_16        10  /**< deprecated: use RDB_PIX_FORMAT_DEPTH16         @version 0x0100 */
+#define RDB_PIX_FORMAT_DEPTH_24        11  /**< deprecated: use RDB_PIX_FORMAT_DEPTH24         @version 0x0100 */
+#define RDB_PIX_FORMAT_RGB_32_F        12  /**< deprecated: use RDB_PIX_FORMAT_RGB32F          @version 0x0100 */
+#define RDB_PIX_FORMAT_RGBA_32_F       13  /**< deprecated: use RDB_PIX_FORMAT_RGBA32F         @version 0x0100 */
+#define RDB_PIX_FORMAT_LUM_32_F        14  /**< deprecated: use RDB_PIX_FORMAT_RED32F          @version 0x0100 */
+#define RDB_PIX_FORMAT_LUMA_32_F       15  /**< deprecated: use RDB_PIX_FORMAT_RG32F           @version 0x0100 */
+#define RDB_PIX_FORMAT_RGB_16_F        16  /**< deprecated: use RDB_PIX_FORMAT_RGB16F          @version 0x0100 */
+#define RDB_PIX_FORMAT_RGBA_16_F       17  /**< deprecated: use RDB_PIX_FORMAT_RGBA16F         @version 0x0100 */
+#define RDB_PIX_FORMAT_LUM_16_F        18  /**< deprecated: use RDB_PIX_FORMAT_RED16F          @version 0x0100 */
+#define RDB_PIX_FORMAT_LUMA_16_F       19  /**< deprecated: use RDB_PIX_FORMAT_RG16F           @version 0x0100 */
+#define RDB_PIX_FORMAT_DEPTH_32        20  /**< deprecated: use RDB_PIX_FORMAT_DEPTH32         @version 0x0101 */
+#define RDB_PIX_FORMAT_BW_32           21  /**< deprecated: use RDB_PIX_FORMAT_RED32           @version 0x0106 */
+#define RDB_PIX_FORMAT_RGB_32          22  /**< 32 bit RGB data                                @version 0x0106 */
+#define RDB_PIX_FORMAT_RGBA_32         23  /**< 32 bit RGB data + 32 bit alpha                 @version 0x0106 */
+
+#define RDB_PIX_FORMAT_R3_G2_B2        24  /**<   8 bit RGB data                          @version 0x010F */
+#define RDB_PIX_FORMAT_R3_G2_B2_A8     25  /**<   8 bit RGB data + 8 bit alpha            @version 0x010F */
+#define RDB_PIX_FORMAT_R5_G6_B5        26  /**<  16 bit RGB data                          @version 0x010F */
+#define RDB_PIX_FORMAT_R5_G6_B5_A16    27  /**<  16 bit RGB data + 16 bit alpha           @version 0x010F */
+#define RDB_PIX_FORMAT_RED8            28  /**<   8 bit RED data                          @version 0x010F */
+#define RDB_PIX_FORMAT_RED16           29  /**<  16 bit RED data                          @version 0x010F */
+#define RDB_PIX_FORMAT_RED16F          30  /**<  16 bit RED floating point data           @version 0x010F */
+#define RDB_PIX_FORMAT_RED24           31  /**<  24 bit RED data                          @version 0x010F */
+#define RDB_PIX_FORMAT_RED32           32  /**<  32 bit RED data                          @version 0x010F */
+#define RDB_PIX_FORMAT_RED32F          33  /**<  32 bit RED floating point data           @version 0x010F */
+#define RDB_PIX_FORMAT_RG8             34  /**<  16 bit RED+GREEN data                    @version 0x010F */
+#define RDB_PIX_FORMAT_RG16            35  /**<  32 bit RED+GREEN data                    @version 0x010F */
+#define RDB_PIX_FORMAT_RG16F           36  /**<  32 bit RED+GREEN floating point data     @version 0x010F */
+#define RDB_PIX_FORMAT_RG32            37  /**<  64 bit RED+GREEN floating point data     @version 0x010F */
+#define RDB_PIX_FORMAT_RG32F           38  /**<  64 bit RED+GREEN floating point data     @version 0x010F */
+#define RDB_PIX_FORMAT_RGB8            39  /**<  24 bit RGB data                          @version 0x010F */
+#define RDB_PIX_FORMAT_RGBA8           40  /**<  24 bit RGB data + 8 bit alpha            @version 0x010F */
+#define RDB_PIX_FORMAT_RGB8_A24        41  /**<  24 bit RGB data + 24 bit alpha           @version 0x010F */
+#define RDB_PIX_FORMAT_RGB16           42  /**<  48 bit RGB data                          @version 0x010F */
+#define RDB_PIX_FORMAT_RGB16F          43  /**<  48 bit RGB floating point data           @version 0x010F */
+#define RDB_PIX_FORMAT_RGBA16          44  /**<  64 bit RGBA data                         @version 0x010F */
+#define RDB_PIX_FORMAT_RGBA16F         45  /**<  64 bit RGBA floating point data          @version 0x010F */
+#define RDB_PIX_FORMAT_RGB32           46  /**<  96 bit RGB data                          @version 0x010F */
+#define RDB_PIX_FORMAT_RGB32F          47  /**<  96 bit RGB floating point data           @version 0x010F */
+#define RDB_PIX_FORMAT_RGBA32          48  /**< 128 bit RGBA data                         @version 0x010F */
+#define RDB_PIX_FORMAT_RGBA32F         49  /**< 128 bit RGBA floating point data          @version 0x010F */
+#define RDB_PIX_FORMAT_DEPTH8          50  /**<   8 bit DEPTH data                        @version 0x010F */
+#define RDB_PIX_FORMAT_DEPTH16         51  /**<  16 bit DEPTH data                        @version 0x010F */
+#define RDB_PIX_FORMAT_DEPTH24         52  /**<  24 bit DEPTH data                        @version 0x010F */
+#define RDB_PIX_FORMAT_DEPTH32         53  /**<  32 bit DEPTH data                        @version 0x010F */
+
+/** @} */
+
+/** @addtogroup RDB_PIX_FORMAT_CUSTOM
+ *  ------ custom pixel formats (any ID above 150 up to 255) may be used ------
+ *  @{
+ */
+#define RDB_PIX_FORMAT_CUSTOM_01           151     /**< custom image format 01    @version 0x0100 */
+#define RDB_PIX_FORMAT_CUSTOM_02           152     /**< custom image format 02    @version 0x0100 */
+#define RDB_PIX_FORMAT_CUSTOM_03           153     /**< custom image format 03    @version 0x0100 */
+/** @} */
+
+/** @addtogroup RDB_SENSOR_TYPE
+ *  ------ sensor types ------
+ *  @{
+ */
+#define RDB_SENSOR_TYPE_NONE            0  /**< unknown sensor type                      @version 0x0100 */
+#define RDB_SENSOR_TYPE_RADAR           1  /**< radar sensor                             @version 0x0100 */
+#define RDB_SENSOR_TYPE_VIDEO           2  /**< video sensor                             @version 0x0100 */
+/** @} */
+
+/** @addtogroup RDB_TRLIGHT_PHASE
+ *  ------ traffic light phases ------
+ *  @{
+ */
+#define RDB_TRLIGHT_PHASE_OFF           0   /**< traffic light is off                    @version 0x0100 */
+#define RDB_TRLIGHT_PHASE_STOP          1   /**< traffic indicates STOP                  @version 0x0100 */
+#define RDB_TRLIGHT_PHASE_STOP_ATTN     2   /**< traffic indicates STOP/ATTENTION        @version 0x0100 */
+#define RDB_TRLIGHT_PHASE_GO            3   /**< traffic indicates GO                    @version 0x0100 */
+#define RDB_TRLIGHT_PHASE_GO_EXCL       4   /**< traffic indicates GO EXCLUSIVELY        @version 0x0100 */
+#define RDB_TRLIGHT_PHASE_ATTN          5   /**< traffic indicates ATTENTION             @version 0x0100 */
+#define RDB_TRLIGHT_PHASE_BLINK         6   /**< traffic indicates BLINK                 @version 0x0100 */
+#define RDB_TRLIGHT_PHASE_UNKNOWN       7   /**< traffic indicates an unknown state      @version 0x0100 */
+/** @} */
+
+/** @addtogroup RDB_COORD_TYPE
+ *  ------ co-ordinate type identifiers ------
+ *  @{
+ */
+#define RDB_COORD_TYPE_INERTIAL         0  /**< inertial co-ordinate system              @version 0x0101 */
+#define RDB_COORD_TYPE_RESERVED_1       1  /**< reserved for future use                  @version 0x0101 */
+#define RDB_COORD_TYPE_PLAYER           2  /**< player co-ordinate system                @version 0x0100 */
+#define RDB_COORD_TYPE_SENSOR           3  /**< sensor-specific co-ordinate system       @version 0x0100 */
+#define RDB_COORD_TYPE_USK              4  /**< universal sensor co-ordinate system      @version 0x0100 */
+#define RDB_COORD_TYPE_USER             5  /**< relative to a user co-ordinate system    @version 0x0100 */
+#define RDB_COORD_TYPE_WINDOW           6  /**< window co-ordinates [pixel]              @version 0x0100 */
+#define RDB_COORD_TYPE_TEXTURE          7  /**< texture co-ordinates [normalized]        @version 0x010C */
+#define RDB_COORD_TYPE_RELATIVE_START   8  /**< co-ordinate relative to start pos.       @version 0x0110 */
+#define RDB_COORD_TYPE_GEO              9  /**< geographic co-ordinate                   @version 0x0118 */
+#define RDB_COORD_TYPE_TRACK           10  /**< track co-ordinate (x=s, y=t )            @version 0x0119 */
+/** @} */
+
+/** @addtogroup RDB_ENV_CLOUD_STATE
+ *  ------ cloud states ------
+ *  @{
+ */
+#define RDB_ENV_CLOUD_STATE_OFF         0  /**< sky is disabled                          @version 0x0100 */
+#define RDB_ENV_CLOUD_STATE_0_8         1  /**< blue sky                                 @version 0x0100 */
+#define RDB_ENV_CLOUD_STATE_4_8         2  /**< cloudy sky                               @version 0x0100 */
+#define RDB_ENV_CLOUD_STATE_6_8         3  /**< overcast sky                             @version 0x0100 */
+#define RDB_ENV_CLOUD_STATE_8_8         4  /**< rainy sky                                @version 0x0100 */
+/** @} */
+
+/** @addtogroup RDB_FUNCTION_TYPE
+ *  ------ function types ------
+ *  @{
+ */
+#define RDB_FUNCTION_TYPE_NONE          0  /**< unknown                                  @version 0x0100 */
+#define RDB_FUNCTION_TYPE_TONE_MAPPING  1  /**< tone mapping function                    @version 0x0100 */
+/** @} */
+
+/** @addtogroup RDB_ROAD_TYPE
+ *  ------ road types ------
+ *  @{
+ */
+#define RDB_ROAD_TYPE_UNKNOWN          0  /**< unknown                                  @version 0x010A */
+#define RDB_ROAD_TYPE_RURAL            1  /**< rural road (100km/h in Germany)          @version 0x010A */
+#define RDB_ROAD_TYPE_MOTORWAY         2  /**< motorway (no limit in Germany)           @version 0x010A */
+#define RDB_ROAD_TYPE_TOWN             3  /**< town (50km/h in Germany)                 @version 0x010A */
+#define RDB_ROAD_TYPE_LOW_SPEED        4  /**< low speed zone (30km/h in Germany)       @version 0x010A */
+#define RDB_ROAD_TYPE_PEDESTRIAN       5  /**< sidewalk (slow, worldwide)               @version 0x010A */
+/** @} */
+
+/** @addtogroup RDB_DRIVER_SOURCE
+ *  ------ sources of driver commands ------
+ *  @{
+ */
+#define RDB_DRIVER_SOURCE_UNKNOWN          0  /**< unknown                                  @version 0x010F */
+#define RDB_DRIVER_SOURCE_GHOSTDRIVER      1  /**< ghostdriver                              @version 0x010F */
+/** @} */
+
+/** @addtogroup RDB_SHM_SIZE
+ *  ------ shared memory sizes ------
+ *  @{
+ */
+#define RDB_SHM_SIZE_TC              5242880  /**< total SHM size for TC I/O  (5 MB)        @version 0x010F */
+/** @} */
+
+/** @addtogroup RDB_FREESPACE_STATE
+ *  ------ freespace state categories ------
+ *  @{
+ */
+#define RDB_FREESPACE_STATE_OBJECT_NONE      0  /**< no object                                @version 0x0119 */
+#define RDB_FREESPACE_STATE_OBJECT_SAME_DIR  1  /**< object in same direction                 @version 0x0119 */
+#define RDB_FREESPACE_STATE_OBJECT_ONCOMING  2  /**< oncoming object                          @version 0x0119 */
+/** @} */
+
+
+/** @addtogroup RDB_DYN_EL_SCOPE
+ *  ------ scope of the target of a dynamic element ------
+ *  @{
+ */
+#define RDB_DYN_EL_SCOPE_UNKNOWN            0       /**< scope of a dynamic element definition in unknown                           @version 0x011B */
+#define RDB_DYN_EL_SCOPE_STATIC_DB          1       /**< scope of a dynamic element definition in the static database               @version 0x011B */
+#define RDB_DYN_EL_SCOPE_DYN_OBJECT         2       /**< scope of a dynamic element definition in a dynamic object                  @version 0x011B */
+#define RDB_DYN_EL_SCOPE_ANY                3       /**< scope of a dynamic element definition in any object in the data tree       @version 0x011B */
+#define RDB_DYN_EL_SCOPE_FIRST              4       /**< scope of a dynamic element definition in the first object in the data tree @version 0x011B */
+#define RDB_DYN_EL_SCOPE_STATIC_DB_SIGNAL   5       /**< scope of a signal definition in the static database                        @version 0x011B */
+#define RDB_DYN_EL_SCOPE_STATIC_DB_SWITCH   6       /**< scope of a switch definition in the static database                        @version 0x011B */
+/** @} */
+
+/** @addtogroup RDB_RAY_TYPE
+ *  ------ type of ray contained in ray message ------
+ *  @{
+ */
+#define RDB_RAY_TYPE_UNKNOWN            0       /**< scope of a dynamic element definition in unknown                           @version 0x011C */
+#define RDB_RAY_TYPE_EMIT               1       /**< emitted ray                                                                @version 0x011C */
+#define RDB_RAY_TYPE_HIT                2       /**< hit point information                                                      @version 0x011C */
+/** @} */
+
+
+/** @} --END GROUP ENUM_DEFINITIONS--*/
+
+/** @addtogroup BITMASK_DEFINITIONS
+ *  ------ Bitmask Definitions ------
+ *  @{
+ */
+/** @addtogroup RDB_PKG_FLAG
+ *  ------ basic package flags ------
+ *  @{
+ */
+#define RDB_PKG_FLAG_NONE                        0x0000      /**< no flags                                                @version 0x0100 */
+#define RDB_PKG_FLAG_EXTENDED                    0x0001      /**< package contains extended information                   @version 0x0100 */
+#define RDB_PKG_FLAG_HIDDEN                      0x0002      /**< package contains hidden information (not for public)    @version 0x010E */
+/** @} */
+
+/** @addtogroup RDB_OBJECT_VIS_FLAG
+ *  ------ object visibility flags ------
+ *  @{
+ */
+#define RDB_OBJECT_VIS_FLAG_ALL                      0xffff  /**< all visibility flags set                                @version 0x0100 */
+#define RDB_OBJECT_VIS_FLAG_NONE                     0x0000  /**< no visibility flags set                                 @version 0x0100 */
+#define RDB_OBJECT_VIS_FLAG_GFX                      0x0001  /**< object is visible in graphics                           @version 0x0100 */
+#define RDB_OBJECT_VIS_FLAG_TRAFFIC                  0x0002  /**< object is visible for traffic                           @version 0x0100 */
+#define RDB_OBJECT_VIS_FLAG_RECORDER                 0x0004  /**< object is visible on data recorder                      @version 0x0110 */
+/** @} */
+
+/** @addtogroup RDB_VEHICLE_LIGHT
+ *  ------ light states ------
+ *  @{
+ */
+#define RDB_VEHICLE_LIGHT_OFF                    0x00000000  /**< all lights OFF                                          @version 0x0100 */
+#define RDB_VEHICLE_LIGHT_PARK                   0x00000001  /**< front and rear lights are ON                            @version 0x0100 */
+#define RDB_VEHICLE_LIGHT_LOW_BEAM               0x00000002  /**< front and rear lights are ON                            @version 0x0100 */
+#define RDB_VEHICLE_LIGHT_HIGH_BEAM              0x00000004  /**< front lights are ON                                     @version 0x0100 */
+#define RDB_VEHICLE_LIGHT_REAR_BRAKE             0x00000008  /**< only rear lights are ON                                 @version 0x0100 */
+#define RDB_VEHICLE_LIGHT_REAR_DRIVE             0x00000010  /**< only rear lights are ON                                 @version 0x0100 */
+#define RDB_VEHICLE_LIGHT_INDICATOR_L            0x00000020  /**< left indicator lights are ON                            @version 0x0100 */
+#define RDB_VEHICLE_LIGHT_INDICATOR_R            0x00000040  /**< right indicator lights are ON                           @version 0x0100 */
+#define RDB_VEHICLE_LIGHT_FLASH                  0x00000080  /**< special light for police forces etc.                    @version 0x0100 */
+#define RDB_VEHICLE_LIGHT_FRONT_FOG              0x00000100  /**< only front lights are ON                                @version 0x0100 */
+#define RDB_VEHICLE_LIGHT_REAR_FOG               0x00000200  /**< only rear lights are ON                                 @version 0x0100 */
+#define RDB_VEHICLE_LIGHT_VIRES1                 0x00000400  /**< used internally by VIRES                                @version 0x0100 */
+#define RDB_VEHICLE_LIGHT_DRL                    0x00000800  /**< daytime running light                                   @version 0x0100 */
+#define RDB_VEHICLE_LIGHT_DRL_LEFT_LOW           0x00001000  /**< dimmed state of left front DRLs                         @version 0x0100 */
+#define RDB_VEHICLE_LIGHT_DRL_RIGHT_LOW          0x00002000  /**< dimmed state of right front DRLs                        @version 0x0100 */
+#define RDB_VEHICLE_LIGHT_EMERGENCY              0x00004000  /**< emergency indicator lights                              @version 0x0100 */
+#define RDB_VEHICLE_LIGHT_INDICATOR_LAMP_ON      0x00008000  /**< true if an indicator lamp is                            @version 0x0111 */
+#define RDB_VEHICLE_LIGHT_FORCE                  0x00010000  /**< if received via RDB, RDB will be the only source for the light mask  @version 0x0111 */
+/** @} */
+
+/** @addtogroup RDB_VEHICLE_ACC_FLAG
+ *  ------ acc states, 1 byte ------
+ *  @{
+ */
+#define RDB_VEHICLE_ACC_FLAG_OFF             0x00  /**< ACC is OFF                                              @version 0x010A */
+#define RDB_VEHICLE_ACC_FLAG_DIST_1          0x01  /**< ACC distance setting 1                                  @version 0x010A */
+#define RDB_VEHICLE_ACC_FLAG_DIST_2          0x02  /**< ACC distance setting 2                                  @version 0x010A */
+#define RDB_VEHICLE_ACC_FLAG_DIST_3          0x03  /**< ACC distance setting 3                                  @version 0x010A */
+#define RDB_VEHICLE_ACC_FLAG_TARGET          0x04  /**< show ACC target vehicle                                 @version 0x010A */
+#define RDB_VEHICLE_ACC_FLAG_SPEED           0x08  /**< show ACC speed                                          @version 0x010A */
+/** @} */
+
+/** @addtogroup RDB_VEHICLE_DISPLAY_LIGHT
+ *  ------ display lights, 2 bytes ------
+ *  @{
+ */
+#define RDB_VEHICLE_DISPLAY_LIGHT_OFF        0x0000  /**< all lights OFF in driver display                         @version 0x010A */
+#define RDB_VEHICLE_DISPLAY_LIGHT_01         0x0001  /**< light 01 ON in driver display                            @version 0x010A */
+#define RDB_VEHICLE_DISPLAY_LIGHT_02         0x0002  /**< light 02 ON in driver display                            @version 0x010A */
+#define RDB_VEHICLE_DISPLAY_LIGHT_03         0x0004  /**< light 03 ON in driver display                            @version 0x010A */
+#define RDB_VEHICLE_DISPLAY_LIGHT_04         0x0008  /**< light 04 ON in driver display                            @version 0x010A */
+#define RDB_VEHICLE_DISPLAY_LIGHT_05         0x0010  /**< light 05 ON in driver display                            @version 0x010A */
+#define RDB_VEHICLE_DISPLAY_LIGHT_06         0x0020  /**< light 06 ON in driver display                            @version 0x010A */
+#define RDB_VEHICLE_DISPLAY_LIGHT_07         0x0040  /**< light 07 ON in driver display                            @version 0x010A */
+#define RDB_VEHICLE_DISPLAY_LIGHT_08         0x0080  /**< light 08 ON in driver display                            @version 0x010A */
+#define RDB_VEHICLE_DISPLAY_LIGHT_09         0x0100  /**< light 09 ON in driver display                            @version 0x010A */
+#define RDB_VEHICLE_DISPLAY_LIGHT_10         0x0200  /**< light 10 ON in driver display                            @version 0x010A */
+#define RDB_VEHICLE_DISPLAY_LIGHT_11         0x0400  /**< light 11 ON in driver display                            @version 0x010A */
+#define RDB_VEHICLE_DISPLAY_LIGHT_12         0x0800  /**< light 12 ON in driver display                            @version 0x010A */
+#define RDB_VEHICLE_DISPLAY_LIGHT_13         0x1000  /**< light 13 ON in driver display                            @version 0x010A */
+#define RDB_VEHICLE_DISPLAY_LIGHT_14         0x2000  /**< light 14 ON in driver display                            @version 0x010A */
+#define RDB_VEHICLE_DISPLAY_LIGHT_15         0x4000  /**< light 15 ON in driver display                            @version 0x010A */
+#define RDB_VEHICLE_DISPLAY_LIGHT_16         0x8000  /**< light 16 ON in driver display                            @version 0x010A */
+/** @} */
+
+
+/** @addtogroup RDB_LANE_EXISTS
+ *  ------ lane existence masks ------
+ *  @{
+ */
+#define RDB_LANE_EXISTS_OWN                      0x01        /**< own lane exists                                         @version 0x0100 */
+#define RDB_LANE_EXISTS_LEFT                     0x02        /**< left lane exists                                        @version 0x0100 */
+#define RDB_LANE_EXISTS_RIGHT                    0x04        /**< right lane exists                                       @version 0x0100 */
+/** @} */
+
+/** @addtogroup RDB_LANE_STATUS
+ *  ------ lane status masks ------
+ *  @{
+ */
+#define RDB_LANE_STATUS_NONE                     0x0000      /**< nothing special                                         @version 0x0100 */
+#define RDB_LANE_STATUS_ROADWORKS                0x0001      /**< road works                                              @version 0x0100 */
+#define RDB_LANE_STATUS_EXIT                     0x0002      /**< motorway exit                                           @version 0x0100 */
+#define RDB_LANE_STATUS_ENTRY                    0x0004      /**< motorway entry                                          @version 0x0100 */
+#define RDB_LANE_STATUS_LINKED                   0x0008      /**< linked lanes                                            @version 0x0100 */
+#define RDB_LANE_STATUS_WET                      0x0010      /**< wet lane (from rain)                                    @version 0x0100 */
+#define RDB_LANE_STATUS_SNOW                     0x0020      /**< snow-covered lane                                       @version 0x0100 */
+/** @} */
+
+/** @addtogroup RDB_DRIVER_FLAG
+ *  ------ dynamics input flags ------
+ *  @{
+ */
+#define RDB_DRIVER_FLAG_NONE                     0x00000000  /**< default                                                 @version 0x0100 */
+#define RDB_DRIVER_FLAG_INDICATOR_L              0x00000001  /**< driver activated left indicator                         @version 0x0100 */
+#define RDB_DRIVER_FLAG_INDICATOR_R              0x00000002  /**< driver activated right indicator                        @version 0x0100 */
+#define RDB_DRIVER_FLAG_PARKING_BRAKE            0x00000004  /**< driver activated parking brake                          @version 0x0100 */
+#define RDB_DRIVER_FLAG_LIGHT_LOW_BEAM           0x00000008  /**< driver activated low beam headlights                    @version 0x0105 */
+#define RDB_DRIVER_FLAG_LIGHT_HIGH_BEAM          0x00000010  /**< driver activated high beam headlights                   @version 0x0105 */
+#define RDB_DRIVER_FLAG_LIGHT_FOG_FRONT          0x00000020  /**< driver activated front fog light                        @version 0x0105 */
+#define RDB_DRIVER_FLAG_LIGHT_FOG_REAR           0x00000040  /**< driver activated rear fog light                         @version 0x0105 */
+#define RDB_DRIVER_FLAG_LIGHT_EMERGENCY          0x00000080  /**< driver activated emergency light (indic. L&R)           @version 0x0105 */
+#define RDB_DRIVER_FLAG_LIGHT_PRIORITY           0x00000100  /**< driver activated priority light (blue flashlight)       @version 0x0105 */
+#define RDB_DRIVER_FLAG_COLLISION                0x00000200  /**< driver noticed a collision with his vehicle             @version 0x0119 */
+/** @} */
+
+/** @addtogroup RDB_MOCKUP_INPUT0
+ *  ------ mockup input flags, part 1 ------
+ *  @{
+ */
+/* INPUT: MFL and SZL bits */
+#define RDB_MOCKUP_INPUT0_MFL_PLUS           0x00000001  /**< input no. 0 for MFL_PLUS               @version 0x010B */
+#define RDB_MOCKUP_INPUT0_MFL_MINUS          0x00000002  /**< input no. 0 for MFL_MINUS              @version 0x010B */
+#define RDB_MOCKUP_INPUT0_MFL_PHONE          0x00000004  /**< input no. 0 for MFL_PHONE              @version 0x010B */
+#define RDB_MOCKUP_INPUT0_MFL_VOICE          0x00000008  /**< input no. 0 for MFL_VOICE              @version 0x010B */
+#define RDB_MOCKUP_INPUT0_MFL_UP             0x00000010  /**< input no. 0 for MFL_UP                 @version 0x010B */
+#define RDB_MOCKUP_INPUT0_MFL_DOWN           0x00000020  /**< input no. 0 for MFL_DOWN               @version 0x010B */
+#define RDB_MOCKUP_INPUT0_MFL_DIAMOND        0x00000040  /**< input no. 0 for MFL_DIAMOND            @version 0x010B */
+#define RDB_MOCKUP_INPUT0_MFL_STAR           0x00000080  /**< input no. 0 for MFL_STAR               @version 0x010B */
+#define RDB_MOCKUP_INPUT0_TURN_UP            0x00000100  /**< input no. 0 for TURN_UP                @version 0x010B */
+#define RDB_MOCKUP_INPUT0_TURN_UP_2          0x00000200  /**< input no. 0 for TURN_UP_2              @version 0x010B */
+#define RDB_MOCKUP_INPUT0_TURN_DOWN          0x00000400  /**< input no. 0 for TURN_DOWN              @version 0x010B */
+#define RDB_MOCKUP_INPUT0_TURN_DOWN_2        0x00000800  /**< input no. 0 for TURN_DOWN_2            @version 0x010B */
+#define RDB_MOCKUP_INPUT0_TURN_FLASHER       0x00001000  /**< input no. 0 for TURN_FLASHER           @version 0x010B */
+#define RDB_MOCKUP_INPUT0_TURN_HIGHBEAM      0x00002000  /**< input no. 0 for TURN_HIGHBEAM          @version 0x010B */
+#define RDB_MOCKUP_INPUT0_TURN_CHECK         0x00004000  /**< input no. 0 for TURN_CHECK             @version 0x010B */
+#define RDB_MOCKUP_INPUT0_TURN_BC            0x00008000  /**< input no. 0 for TURN_BC                @version 0x010B */
+#define RDB_MOCKUP_INPUT0_ACC_BACK           0x00010000  /**< input no. 0 for ACC_BACK               @version 0x010B */
+#define RDB_MOCKUP_INPUT0_ACC_BACK_2         0x00020000  /**< input no. 0 for ACC_BACK_2             @version 0x010B */
+#define RDB_MOCKUP_INPUT0_ACC_FWD            0x00040000  /**< input no. 0 for ACC_FWD                @version 0x010B */
+#define RDB_MOCKUP_INPUT0_ACC_FWD_2          0x00080000  /**< input no. 0 for ACC_FWD_2              @version 0x010B */
+#define RDB_MOCKUP_INPUT0_ACC_UP             0x00100000  /**< input no. 0 for ACC_UP                 @version 0x010B */
+#define RDB_MOCKUP_INPUT0_ACC_DOWN           0x00200000  /**< input no. 0 for ACC_DOWN               @version 0x010B */      
+#define RDB_MOCKUP_INPUT0_ACC_SET            0x00400000  /**< input no. 0 for ACC_SET                @version 0x010B */ 
+#define RDB_MOCKUP_INPUT0_HORN               0x00800000  /**< input no. 0 for HORN                   @version 0x010B */                
+#define RDB_MOCKUP_INPUT0_WIPER_INTERVAL     0x03000000  /**< input no. 0 for WIPER_INTERVAL         @version 0x010B */
+#define RDB_MOCKUP_INPUT0_WIPER_INTERVAL_1   0x01000000  /**< input no. 0 for WIPER_INTERVAL_1       @version 0x010B */
+#define RDB_MOCKUP_INPUT0_WIPER_INTERVAL_2   0x02000000  /**< input no. 0 for WIPER_INTERVAL_2       @version 0x010B */
+#define RDB_MOCKUP_INPUT0_WIPER_INTERVAL_3   0x03000000  /**< input no. 0 for WIPER_INTERVAL_3       @version 0x010B */
+#define RDB_MOCKUP_INPUT0_WIPER_AUTO         0x04000000  /**< input no. 0 for WIPER_AUTO             @version 0x010B */
+#define RDB_MOCKUP_INPUT0_WIPER_BACK         0x08000000  /**< input no. 0 for WIPER_BACK             @version 0x010B */
+#define RDB_MOCKUP_INPUT0_WIPER_UP           0x10000000  /**< input no. 0 for WIPER_UP               @version 0x010B */
+#define RDB_MOCKUP_INPUT0_WIPER_UP_2         0x20000000  /**< input no. 0 for WIPER_UP_2             @version 0x010B */         
+#define RDB_MOCKUP_INPUT0_WIPER_DOWN         0x40000000  /**< input no. 0 for WIPER_DOWN             @version 0x010B */                                                              
+/** @} */
+                                                                                                                                     
+/** @addtogroup RDB_MOCKUP_INPUT1
+ *  ------ mockup input flags, part 2 ------                                                                                           
+ *  @{
+ */
+/* INPUT: ZBE and GWS bits */
+#define RDB_MOCKUP_INPUT1_ZBE_COUNTER      0x0000FFFF   /**< input no. 1 for ZBE_COUNTER            @version 0x010B */
+#define RDB_MOCKUP_INPUT1_ZBE_FWD          0x00010000   /**< input no. 1 for ZBE_FWD                @version 0x010B */
+#define RDB_MOCKUP_INPUT1_ZBE_BACK         0x00020000   /**< input no. 1 for ZBE_BACK               @version 0x010B */
+#define RDB_MOCKUP_INPUT1_ZBE_LEFT         0x00040000   /**< input no. 1 for ZBE_LEFT               @version 0x010B */
+#define RDB_MOCKUP_INPUT1_ZBE_RIGHT        0x00080000   /**< input no. 1 for ZBE_RIGHT              @version 0x010B */
+#define RDB_MOCKUP_INPUT1_ZBE_PRESS        0x00100000   /**< input no. 1 for ZBE_PRESS              @version 0x010B */
+#define RDB_MOCKUP_INPUT1_ZBE_MENU         0x00200000   /**< input no. 1 for ZBE_MENU               @version 0x010B */
+#define RDB_MOCKUP_INPUT1_GWS_P            0x00400000   /**< input no. 1 for GWS_P                  @version 0x010B */
+#define RDB_MOCKUP_INPUT1_GWS_LOCK         0x00800000   /**< input no. 1 for GWS_LOCK               @version 0x010B */
+#define RDB_MOCKUP_INPUT1_GWS_FWD          0x01000000   /**< input no. 1 for GWS_FWD                @version 0x010B */
+#define RDB_MOCKUP_INPUT1_GWS_FWD_2        0x02000000   /**< input no. 1 for GWS_FWD_2              @version 0x010B */
+#define RDB_MOCKUP_INPUT1_GWS_BACK         0x04000000   /**< input no. 1 for GWS_BACK               @version 0x010B */
+#define RDB_MOCKUP_INPUT1_GWS_BACK_2       0x08000000   /**< input no. 1 for GWS_BACK_2             @version 0x010B */
+#define RDB_MOCKUP_INPUT1_GWS_AUTO_N       0x10000000   /**< input no. 1 for GWS_AUTO_N             @version 0x010B */
+#define RDB_MOCKUP_INPUT1_GWS_MAN_N        0x20000000   /**< input no. 1 for GWS_MAN_N              @version 0x010B */
+#define RDB_MOCKUP_INPUT1_GWS_MAN_PLUS     0x40000000   /**< input no. 1 for GWS_MAN_PLUS           @version 0x010B */
+#define RDB_MOCKUP_INPUT1_GWS_MAN_MINUS    0x80000000   /**< input no. 1 for GWS_MAN_MINUS          @version 0x010B */
+/** @} */
+                                                                                                                     
+/** @addtogroup RDB_MOCKUP_INPUT2
+ *  ------ mockup input flags, part 3 ------
+ *  @{
+ */
+#define RDB_MOCKUP_INPUT2_LSZ_POTI              0x000000FF /**< input no. 2 for LSZ_POTI                 @version 0x010B */
+#define RDB_MOCKUP_INPUT2_LSZ_PARKING           0x00000100 /**< input no. 2 for LSZ_PARKING              @version 0x010B */
+#define RDB_MOCKUP_INPUT2_LSZ_DRIVING           0x00000200 /**< input no. 2 for LSZ_DRIVING              @version 0x010B */
+#define RDB_MOCKUP_INPUT2_LSZ_AUTO              0x00000300 /**< input no. 2 for LSZ_AUTO                 @version 0x010B */
+#define RDB_MOCKUP_INPUT2_LSZ_FOG_FRONT         0x00000400 /**< input no. 2 for LSZ_FOG_FRONT            @version 0x010B */
+#define RDB_MOCKUP_INPUT2_LSZ_FOG_REAR          0x00000800 /**< input no. 2 for LSZ_FOG_REAR             @version 0x010B */
+#define RDB_MOCKUP_INPUT2_DB_DTC                0x00001000 /**< input no. 2 for DB_DTC                   @version 0x010B */
+#define RDB_MOCKUP_INPUT2_DB_PDC                0x00002000 /**< input no. 2 for DB_PDC                   @version 0x010B */
+#define RDB_MOCKUP_INPUT2_DB_SEAT_HEAT_L        0x00004000 /**< input no. 2 for DB_SEAT_HEAT_L           @version 0x010B */
+#define RDB_MOCKUP_INPUT2_DB_SEAT_HEAT_R        0x00008000 /**< input no. 2 for DB_SEAT_HEAT_R           @version 0x010B */
+#define RDB_MOCKUP_INPUT2_DB_STARTER            0x00010000 /**< input no. 2 for DB_STARTER               @version 0x010B */
+#define RDB_MOCKUP_INPUT2_DB_HAZARD_LIGHTS      0x00020000 /**< input no. 2 for DB_HAZARD_LIGHTS         @version 0x010B */
+#define RDB_MOCKUP_INPUT2_DB_LOCK               0x00040000 /**< input no. 2 for DB_LOCK                  @version 0x010B */
+                                                                                
+#define RDB_MOCKUP_INPUT2_DB_STEER_ADJUST_FWD   0x00100000 /**< input no. 2 for DB_STEER_ADJUST_FWD      @version 0x010B */
+#define RDB_MOCKUP_INPUT2_DB_STEER_ADJUST_BACK  0x00200000 /**< input no. 2 for DB_STEER_ADJUST_BACK     @version 0x010B */
+#define RDB_MOCKUP_INPUT2_DB_STEER_ADJUST_UP    0x00400000 /**< input no. 2 for DB_STEER_ADJUST_UP       @version 0x010B */
+#define RDB_MOCKUP_INPUT2_DB_STEER_ADJUST_DOWN  0x00800000 /**< input no. 2 for DB_STEER_ADJUST_DOWN     @version 0x010B */
+/** @} */                                                                                                              
+
+/** @addtogroup RDB_DRIVER_PERCEPTION_FLAG
+ *  ------ driver persecption flags ------
+ *  @{
+ */
+#define RDB_DRIVER_PERCEPTION_FLAG_NONE          0x00000000  /**< default                                                 @version 0x0100 */
+#define RDB_DRIVER_PERCEPTION_FLAG_TURN_L        0x00000001  /**< turn left at next intersection                          @version 0x0100 */
+#define RDB_DRIVER_PERCEPTION_FLAG_TURN_R        0x00000002  /**< turn right at next intersection                         @version 0x0100 */
+/** @} */
+
+/** @addtogroup RDB_DRIVER_INPUT_VALIDITY
+ *  ------ dynamics input flags ------
+ *  @{
+ */
+#define RDB_DRIVER_INPUT_VALIDITY_NONE            0x00000000 /**< default                                                @version 0x0100 */
+#define RDB_DRIVER_INPUT_VALIDITY_STEERING_WHEEL  0x00000001 /**< steering wheel is valid                                @version 0x0100 */
+#define RDB_DRIVER_INPUT_VALIDITY_STEERING_SPEED  0x00000002 /**< steering speed is valid                                @version 0x0100 */
+#define RDB_DRIVER_INPUT_VALIDITY_THROTTLE        0x00000004 /**< throttle is valid                                      @version 0x0100 */
+#define RDB_DRIVER_INPUT_VALIDITY_BRAKE           0x00000008 /**< brake is valid                                         @version 0x0100 */
+#define RDB_DRIVER_INPUT_VALIDITY_CLUTCH          0x00000010 /**< clutch is valid                                        @version 0x0100 */
+#define RDB_DRIVER_INPUT_VALIDITY_TGT_ACCEL       0x00000020 /**< target acceleration is valid                           @version 0x0100 */
+#define RDB_DRIVER_INPUT_VALIDITY_TGT_STEERING    0x00000040 /**< target steering is valid                               @version 0x0100 */
+#define RDB_DRIVER_INPUT_VALIDITY_GEAR            0x00000080 /**< gear selection is valid                                @version 0x0100 */
+#define RDB_DRIVER_INPUT_VALIDITY_CURVATURE       0x00000100 /**< curvature is valid for lateral control                 @version 0x0100 */
+#define RDB_DRIVER_INPUT_VALIDITY_STEERING_TORQUE 0x00000200 /**< torque at steering wheel is valid                      @version 0x0100 */
+#define RDB_DRIVER_INPUT_VALIDITY_ENGINE_TORQUE   0x00000400 /**< target torque of engine is valid                       @version 0x0100 */
+#define RDB_DRIVER_INPUT_VALIDITY_TGT_SPEED       0x00000800 /**< target speed is valid                                  @version 0x0100 */
+#define RDB_DRIVER_INPUT_VALIDITY_INFO_ONLY       0x00001000 /**< consider the values for info only                      @version 0x0100 */
+#define RDB_DRIVER_INPUT_VALIDITY_ADD_ON          0x00002000 /**< consider the values as an addOn for existing values    @version 0x0104 */
+#define RDB_DRIVER_INPUT_VALIDITY_FLAGS           0x00004000 /**< member "flags" of RDB_DRIVER_CTRL_t is valid           @version 0x0107 */
+#define RDB_DRIVER_INPUT_VALIDITY_MOCKUP_INPUT0   0x00008000 /**< member "mockupInput0" of RDB_DRIVER_CTRL_t is valid    @version 0x010A */
+#define RDB_DRIVER_INPUT_VALIDITY_MOCKUP_INPUT1   0x00010000 /**< member "mockupInput1" of RDB_DRIVER_CTRL_t is valid    @version 0x010A */
+#define RDB_DRIVER_INPUT_VALIDITY_MOCKUP_INPUT2   0x00020000 /**< member "mockupInput2" of RDB_DRIVER_CTRL_t is valid    @version 0x010A */
+#define RDB_DRIVER_INPUT_VALIDITY_STEERING_TPOS   0x00040000 /**< interpret steeringTgt as t position                    @version 0x0119 */
+#define RDB_DRIVER_INPUT_VALIDITY_MODIFIED        0x00080000 /**< set if driver ctrl command is combination of original command and add_on command    @version 0x011a */
+/** @} */
+
+
+/** @addtogroup RDB_SCORING_FLAG
+ *  ------ arbitrary flags influencing the scoring ------
+ *  @{
+ */
+#define RDB_SCORING_FLAG_NONE                     0x00000000  /**< default                                                @version 0x0100 */
+#define RDB_SCORING_FLAG_COLLISION                0x00000001  /**< driver caused collision                                @version 0x0100 */
+#define RDB_SCORING_FLAG_OFF_ROAD                 0x00000002  /**< driver went off road                                   @version 0x0100 */
+/** @} */
+
+/** @addtogroup RDB_COORD_FLAG
+ *  ------ co-ordinate flags ------
+ *  @{
+ */
+#define RDB_COORD_FLAG_NONE                       0x00        /**< co-ordinate has no flags (state unknown or invalid)    @version 0x0100 */
+#define RDB_COORD_FLAG_POINT_VALID                0x01        /**< point co-ordinate is valid                             @version 0x0100 */
+#define RDB_COORD_FLAG_ANGLES_VALID               0x02        /**< angles are valid                                       @version 0x0100 */
+/** @} */
+
+/** @addtogroup RDB_LIGHT_SOURCE_FLAG
+ *  ------ light source flags ------
+ *  @{
+ */
+#define RDB_LIGHT_SOURCE_FLAG_NONE                0x0000      /**< light source has no flags (state unknown)                  @version 0x0100 */
+#define RDB_LIGHT_SOURCE_FLAG_USE_FRUSTUM         0x0001      /**< use frustum information of light source                    @version 0x0100 */
+#define RDB_LIGHT_SOURCE_FLAG_PERSISTENT          0x0002      /**< keep the light source even after initialization            @version 0x0100 */
+#define RDB_LIGHT_SOURCE_FLAG_STENCIL             0x0004      /**< use this definition for the stencil mask of a light source @version 0x010C */
+/** @} */
+
+/** @addtogroup RDB_SENSOR_OBJECT_FLAG
+ *  ------ sensor object flags ------
+ *  @{
+ */
+#define RDB_SENSOR_OBJECT_FLAG_NONE               0x0000      /**< object has no flags                                    @version 0x0100 */
+#define RDB_SENSOR_OBJECT_FLAG_CRITICALITY_LOW    0x0001      /**< criticality of object is low                           @version 0x0100 */
+#define RDB_SENSOR_OBJECT_FLAG_CRITICALITY_MEDIUM 0x0002      /**< criticality of object is medium                        @version 0x0100 */
+#define RDB_SENSOR_OBJECT_FLAG_CRITICALITY_HIGH   0x0003      /**< criticality of object is high                          @version 0x0100 */
+/** @} */
+
+/** @addtogroup RDB_ROAD_EVENT
+ *  ------ road events ------
+ *  @{
+ */
+#define RDB_ROAD_EVENT_NONE                       0x00000000  /**< no event is triggered                                  @version 0x0100 */
+#define RDB_ROAD_EVENT_POTHOLE                    0x00000001  /**< contact point is in a pothole                          @version 0x0100 */
+/** @} */
+
+/** @addtogroup RDB_ENV_FLAG
+ *  ------ road events ------
+ *  @{
+ */
+#define RDB_ENV_FLAG_NONE                         0x0000  /**< no environment flag is set                                  @version 0x0100 */
+#define RDB_ENV_FLAG_PRECIPITATION_SNOW           0x0001  /**< precipitation is snow, not rain                             @version 0x0100 */
+#define RDB_ENV_FLAG_PRECIPITATION_HAIL           0x0002  /**< precipitation is hail, not rain                             @version 0x0100 */
+#define RDB_ENV_FLAG_ROAD_SURFACE_WET             0x0004  /**< wet road surface                                            @version 0x0100 */
+#define RDB_ENV_FLAG_STREET_LAMPS                 0x0008  /**< street lamps are ON                                         @version 0x0102 */
+/** @} */
+
+/** @addtogroup RDB_SHM_ID
+ *  ------ shared memory identifiers ------
+ *  @{
+ */
+#define RDB_SHM_ID_IMG_GENERATOR_OUT              0x0816a   /**< shared memory key of image generator                      @version 0x0100 */
+#define RDB_SHM_ID_IMG_GENERATOR_IN               0x0817a   /**< shared memory key for light maps                          @version 0x0100 */
+#define RDB_SHM_ID_CONTROL_GENERATOR_IN           0x0817b   /**< shared memory key commands                                @version 0x010E */
+#define RDB_SHM_ID_CUSTOM_01                      0x0818a   /**< shared memory key of custom data block                    @version 0x0100 */
+#define RDB_SHM_ID_TC_IN                          0x08200   /**< shared memory key of taskControl input                    @version 0x010F */
+#define RDB_SHM_ID_TC_OUT                         0x08201   /**< shared memory key of taskControl output                   @version 0x010F */
+#define RDB_SHM_ID_DYN_2_STEER                    0x08210   /**< shared memory key of dynamics to steering                 @version 0x0111 */
+#define RDB_SHM_ID_STEER_2_DYN                    0x08211   /**< shared memory key of steering to dynamics                 @version 0x0111 */
+/** @} */
+
+/** @addtogroup RDB_SHM_BUFFER_FLAG
+ *  ------ shared memory identifiers ------
+ *  @{
+ */
+#define RDB_SHM_BUFFER_FLAG_NONE                    0x00000000      /**< no bits set, buffer may be overwritten            @version 0x0100 */
+#define RDB_SHM_BUFFER_FLAG_LOCK                    0x00000001      /**< buffer is locked by producer                      @version 0x0100 */
+#define RDB_SHM_BUFFER_FLAG_TC                      0x00000002      /**< buffer is to be processed by TC                   @version 0x0100 */
+#define RDB_SHM_BUFFER_FLAG_IG                      0x00000004      /**< buffer is to be processed by IG                   @version 0x0100 */
+/** @} */
+
+/** @addtogroup RDB_OBJECT_CFG_FLAG
+ *  ------ object configuration flags ------
+ *  @{
+ */
+#define RDB_OBJECT_CFG_FLAG_NONE                  0x0000 /**< no configuration flag set                                       @version 0x0100 */
+#define RDB_OBJECT_CFG_FLAG_CTRL_EXTERN           0x0001 /**< object is controlled externally                                 @version 0x0100 */
+#define RDB_OBJECT_CFG_FLAG_MODEL_ID_VALID        0x0002 /**< model ID given in object state message is valid                 @version 0x0119 */
+/** @} */
+
+/** @addtogroup RDB_ROAD_POS_FLAG
+ *  ------ road position flags ------
+ *  @{
+ */
+#define RDB_ROAD_POS_FLAG_NONE                    0x00 /**< no flag set                                                 @version 0x0100 */
+#define RDB_ROAD_POS_FLAG_DIR_FWD                 0x01 /**< player orientation is in forward road direction             @version 0x0100 */
+#define RDB_ROAD_POS_FLAG_DIR_REAR                0x02 /**< player orientation is in rearward road direction            @version 0x0100 */
+#define RDB_ROAD_POS_FLAG_OFFROAD                 0x04 /**< player is considered "offroad", i.e. not on a driving lane  @version 0x011B */
+/** @} */
+
+/** @addtogroup RDB_CONTACT_POINT_FLAG
+ *  ------ contact point flags ------
+ *  @{
+ */
+#define RDB_CONTACT_POINT_FLAG_NONE                0x0000      /**< contact point has no flags (state unknown)             @version 0x0102 */
+#define RDB_CONTACT_POINT_FLAG_PLAYER_VALID        0x0001      /**< player ID within CP structure is valid                 @version 0x0102 */
+/** @} */
+
+/** @addtogroup RDB_SYNC_CMD
+ *  ------ commands distributed with sync messages ------
+ *  @{
+ */
+#define RDB_SYNC_CMD_RENDER_CONTINUOUS          0x00000080      /**< target will render as usual                            @version 0x010E */
+#define RDB_SYNC_CMD_RENDER_PAUSE               0x00000100      /**< target will pause rendering completely                 @version 0x010E */
+#define RDB_SYNC_CMD_RENDER_SINGLE_FRAME        0x00000200      /**< target will render a single frame and pause then       @version 0x010E */
+/** @} */
+
+/** @addtogroup RDB_TRAJECTORY_FLAG
+ *  ------ details about a trajectory definiton  ------
+ *  @{
+ */
+#define RDB_TRAJECTORY_FLAG_NONE                0x0000          /**< no special settings                                    @version 0x0110 */
+#define RDB_TRAJECTORY_FLAG_TIME_DOMAIN         0x0001          /**< trajectory is in time domain                           @version 0x0110 */
+/** @} */
+
+/** @addtogroup RDB_DYN_2_STEER_STATE
+ *  ------ dynamics model state ------
+ *  @{
+ */
+#define RDB_DYN_2_STEER_STATE_NONE                  0x0000          /**< no bits set, STOP or not connected                     @version 0x0100 */
+#define RDB_DYN_2_STEER_STATE_PAUSE                 0x0001          /**< PAUSE                                                  @version 0x0100 */
+#define RDB_DYN_2_STEER_STATE_RUN                   0x0002          /**< RUN                                                    @version 0x0100 */
+#define RDB_DYN_2_STEER_STATE_ERROR                 0x0004          /**< ERROR                                                  @version 0x0100 */
+/** @} */
+
+
+/** @addtogroup RDB_DYN_2_STEER_CMD
+ *  ------ commands to the steering device ------
+ *  @{
+ */
+#define RDB_DYN_2_STEER_CMD_NONE                    0x0000          /**< no bits set, no command                                @version 0x0100 */
+#define RDB_DYN_2_STEER_CMD_RESET                   0x0001          /**< RESET steering device                                  @version 0x0100 */
+/** @} */
+
+
+/** @addtogroup RDB_DYN_2_STEER_EFFECTS
+ *  ------ steering device state ------
+ *  @{
+ */
+#define RDB_DYN_2_STEER_EFFECT_NONE                 0x00000000      /**< no bits set, no effect active                          @version 0x0100 */
+#define RDB_DYN_2_STEER_EFFECT_TIRE_MODEL           0x00000001      /**< steering device calculates own tire model              @version 0x0100 */
+#define RDB_DYN_2_STEER_EFFECT_VIBRATION_10HZ       0x00000002      /**< steering device is initializing                        @version 0x0100 */
+/** @} */
+
+/** @addtogroup RDB_STEER_2_DYN_STATE
+ *  ------ steering device state ------
+ *  @{
+ */
+#define RDB_STEER_2_DYN_STATE_OFF                   0x00000000      /**< no bits set, steering not connected or OFF     @version 0x0100 */
+#define RDB_STEER_2_DYN_STATE_INIT                  0x00000001      /**< steering device is initializing                @version 0x0100 */
+#define RDB_STEER_2_DYN_STATE_FAIL                  0x00000002      /**< steering device is failed                      @version 0x0100 */
+#define RDB_STEER_2_DYN_STATE_RUN                   0x00000004      /**< steering device is running                     @version 0x0100 */
+#define RDB_STEER_2_DYN_STATE_OVER_LIMITS           0x00000008      /**< input values out of limits                     @version 0x0100 */
+/** @} */
+
+/** @addtogroup RDB_WHEEL_FLAG
+ *  ------ wheel status flags ------
+ *  @{
+ */
+#define RDB_WHEEL_FLAG_NONE                         0x0000          /**< no bits set,                                   @version 0x0114 */
+#define RDB_WHEEL_FLAG_ON_ROADMARK                  0x0001          /**< wheel is on road mark                          @version 0x0114 */
+/** @} */
+
+/** @addtogroup RDB_MOTION_SYSTEM_FLAG
+ *  ------ commands distributed with sync messages ------
+ *  @{
+ */
+#define RDB_MOTION_SYSTEM_FLAG_NONE             0x0000          /**< no special settings                                    @version 0x0116 */
+#define RDB_MOTION_SYSTEM_FLAG_ACTIVE           0x0001          /**< motion system is active (ON)                           @version 0x0116 */
+#define RDB_MOTION_SYSTEM_FLAG_ERROR            0x0002          /**< motion system has errors                               @version 0x0116 */
+/** @} */
+
+/** @addtogroup RDB_CUSTOM_TRACK_CTRL_FLAG
+ *  ------ commands distributed with sync messages ------
+ *  @{
+ */
+#define RDB_CUSTOM_TRACK_CTRL_FLAG_VIS_SENSOR_A  0x0001  /**< object is visible in sensor A                                   @version 0x0119 */
+#define RDB_CUSTOM_TRACK_CTRL_FLAG_VIS_SENSOR_B  0x0002  /**< object is visible in sensor B                                   @version 0x0119 */
+#define RDB_CUSTOM_TRACK_CTRL_FLAG_VIS_GFX       0x0004  /**< object is visible in Ig                                         @version 0x0119 */
+#define RDB_CUSTOM_TRACK_CTRL_FLAG_VIS_SENSOR_C  0x0008  /**< object is visible in sensor C                                   @version 0x0119 */
+#define RDB_CUSTOM_TRACK_CTRL_FLAG_VIS_SENSOR_D  0x0010  /**< object is visible in sensor D                                   @version 0x0119 */
+#define RDB_CUSTOM_TRACK_CTRL_FLAG_NAME_BY_ID    0x0100  /**< model name  is derived from ID and model is addressed via name  @version 0x0119 */
+#define RDB_CUSTOM_TRACK_CTRL_FLAG_PLAYER_ACTIVE 0x0200  /**< if set, player is active                                        @version 0x0119 */
+/** @} */
+
+/** @addtogroup RDB_CUSTOM_TRACK_CTRL_VALIDITY
+ *  ------ dynamics input flags ------
+ *  @{
+ */
+#define RDB_CUSTOM_TRACK_CTRL_VALIDITY_DEFAULT         0x00000000 /**< default                                                @version 0x0119 */
+#define RDB_CUSTOM_TRACK_CTRL_VALIDITY_TGT_ACCEL       0x00000001 /**< target acceleration is valid                           @version 0x0119 */
+#define RDB_CUSTOM_TRACK_CTRL_VALIDITY_STEERING_TPOS   0x00000002 /**< t-pos is valid                                         @version 0x0119 */
+/** @} */
+
+/** @addtogroup RDB_ROAD_QUERY_FLAG
+ *  ------ dynamics input flags ------
+ *  @{
+ */
+#define RDB_ROAD_QUERY_FLAG_NONE                       0x0000 /**< default                                                @version 0x011D */
+#define RDB_ROAD_QUERY_FLAG_RELATIVE_POS               0x0001 /**< use relative positioning                               @version 0x011D */
+/** @} */
+
+/** @} --END GROUP BITMASK_DEFINITIONS-- */
+
+/** ------ generic point structure --- */
+typedef struct
+{
+    double x;         /**< x position                                                @unit m                                @version 0x0100 */
+    double y;         /**< y position                                                @unit m                                @version 0x0100 */
+    double z;         /**< z position                                                @unit m                                @version 0x0100 */
+    uint8_t  flags;   /**< co-ordinate flags                                         @unit @link RDB_COORD_FLAG @endlink    @version 0x0100 */
+    uint8_t  type;    /**< co-ordinate system type identifier                        @unit @link RDB_COORD_TYPE @endlink    @version 0x0100 */
+    uint16_t system;  /**< unique ID of the corresponding (user) co-ordinate system  @unit _                                @version 0x0100 */
+} RDB_POINT_t;
+
+/** ------ generic co-ordinate structure --- */
+typedef struct
+{
+    double   x;       /**< x position                                                @unit m                                @version 0x0100 */
+    double   y;       /**< y position                                                @unit m                                @version 0x0100 */
+    double   z;       /**< z position                                                @unit m                                @version 0x0100 */
+    float    h;       /**< heading angle                                             @unit rad                              @version 0x0100 */
+    float    p;       /**< pitch angle                                               @unit rad                              @version 0x0100 */
+    float    r;       /**< roll angle                                                @unit rad                              @version 0x0100 */
+    uint8_t  flags;   /**< co-ordinate flags                                         @unit @link RDB_COORD_FLAG @endlink    @version 0x0100 */
+    uint8_t  type;    /**< co-ordinate system type identifier                        @unit @link RDB_COORD_TYPE @endlink    @version 0x0100 */
+    uint16_t system;  /**< unique ID of the corresponding (user) co-ordinate system  @unit _                                @version 0x0100 */
+} RDB_COORD_t;
+
+/** ------ definition / position of a user co-ordinate system --- */
+typedef struct
+{
+    uint16_t     id;                     /**< unique ID of the co-ordinate system                                    @unit _                    @version 0x0100 */
+    uint16_t     spare;                  /**< spare for future use                                                   @unit _                    @version 0x0100 */
+    RDB_COORD_t  pos;                    /**< origin and orientation of x axis of the co-ordinate system             @unit m,m,m,rad,rad,rad    @version 0x0100 */
+} RDB_COORD_SYSTEM_t;
+
+/** ------ road position and associated properties ------ */
+typedef struct
+{
+    uint32_t         playerId;           /**< id of the player to which road position belongs                        @unit _                                @version 0x0100 */
+    uint16_t         roadId;             /**< unique road ID                                                         @unit _                                @version 0x0100 */
+    int8_t           laneId;             /**< lane ID                                                                @unit _                                @version 0x0100 */
+    uint8_t          flags;              /**< road position flags, further info                                      @unit @link RDB_ROAD_POS_FLAG @endlink @version 0x0100 */
+    float            roadS;              /**< s-coordinate along road's reference line                               @unit m                                @version 0x0100 */
+    float            roadT;              /**< t-coordinate perpendicular to road's reference line                    @unit m                                @version 0x0100 */
+    float            laneOffset;         /**< offset from lane center in road co-ordinates                           @unit m                                @version 0x0100 */
+    float            hdgRel;             /**< heading angle relative to lane tangent dir                             @unit rad                              @version 0x0100 */
+    float            pitchRel;           /**< pitch angle relative to road tangent plane                             @unit rad                              @version 0x0100 */
+    float            rollRel;            /**< roll angle relative to road tangent plane                              @unit rad                              @version 0x0100 */
+    uint8_t          roadType;           /**< type of the road, corresponding to OpenDRIVE                           @unit @link RDB_ROAD_TYPE @endlink     @version 0x010A */
+    uint8_t          spare1;             /**< for future use                                                         @unit _                                @version 0x010A */
+    uint16_t         spare2;             /**< for future use                                                         @unit _                                @version 0x010A */
+    float            pathS;              /**< longitudinal path co-ordinate                                          @unit _                                @version 0x010E */
+} RDB_ROAD_POS_t;
+
+/** ------ road mark information ------
+ * @note this package is immediately followed by "noDataPoints" entries of type RDB_POINT_t
+ */
+typedef struct
+{
+    uint32_t    playerId;                /**< id of the player to which roadmark belongs                             @unit _                                    @version 0x0100 */
+    int8_t      id;                      /**< id of this road mark                                                   @unit [0..127]                             @version 0x010D */
+    int8_t      prevId;                  /**< id of predecessor                                                      @unit [-1, 0..127]                         @version 0x010D */
+    int8_t      nextId;                  /**< id of successor                                                        @unit [-1, 0..127]                         @version 0x010D */
+    int8_t      laneId;                  /**< id of the lane to which the roadmark belongs                           @unit _                                    @version 0x011a */
+    float       lateralDist;             /**< lateral distance to vehicle ref. point and dir                         @unit m                                    @version 0x0100 */
+    float       yawRel;                  /**< yaw angle relative to vehicle dir                                      @unit rad [-PI;PI]                         @version 0x0100 */
+    double      curvHor;                 /**< horizontal curvature                                                   @unit 1/m                                  @version 0x0100 */
+    double      curvHorDot;              /**< change of horizontal curvature                                         @unit 1/m2                                 @version 0x0100 */
+    float       startDx;                 /**< start of road mark in driving dir                                      @unit m                                    @version 0x0100 */
+    float       previewDx;               /**< distance of last valid measurement                                     @unit m                                    @version 0x0100 */
+    float       width;                   /**< width of road mark                                                     @unit m                                    @version 0x0100 */
+    float       height;                  /**< height of road mark                                                    @unit m                                    @version 0x0100 */
+    double      curvVert;                /**< vertical curvature                                                     @unit 1/m                                  @version 0x0100 */
+    double      curvVertDot;             /**< change of vertical curvature                                           @unit 1/m2                                 @version 0x0100 */
+    uint8_t     type;                    /**< type of road mark                                                      @unit @link RDB_ROADMARK_TYPE  @endlink    @version 0x0100 */
+    uint8_t     color;                   /**< color of road mark                                                     @unit @link RDB_ROADMARK_COLOR @endlink    @version 0x0100 */
+    uint16_t    noDataPoints;            /**< number of tesselation points following this package                    @unit _                                    @version 0x0100 */
+    uint32_t    roadId;                  /**< id of the road to which the roadmark belongs                           @unit _                                    @version 0x011a */
+    uint32_t    spare1;                  /**< for future use                                                         @unit _                                    @version 0x0100 */
+} RDB_ROADMARK_t;
+
+/** ------ lane information ------ */
+typedef struct
+{
+    uint16_t    roadId;                  /**< unique road ID                                                         @unit _                                @version 0x0100 */
+    int8_t      id;                      /**< lane ID according to OpenDRIVE                                         @unit [-127..127]                      @version 0x0100 */
+    uint8_t     neighborMask;            /**< existence mask for adjacent lanes                                      @unit @link RDB_LANE_EXISTS @endlink   @version 0x0100 */
+    int8_t      leftLaneId;              /**< ID of lane left of current lane                                        @unit [-127..127]                      @version 0x0100 */
+    int8_t      rightLaneId;             /**< ID of lane right of current lane                                       @unit [-127..127]                      @version 0x0100 */
+    uint8_t     borderType;              /**< type of lane border                                                    @unit @link RDB_LANE_BORDER @endlink   @version 0x0100 */
+    uint8_t     material;                /**< type of lane material                                                  @unit [0..255]                         @version 0x0100 */
+    uint16_t    status;                  /**< status mask of lane                                                    @unit @link RDB_LANE_STATUS @endlink   @version 0x0100 */
+    uint16_t    type;                    /**< enumerated lane type according to OpenDRIVE (0=none, 1=driving...)     @unit _                                @version 0x010D */
+    float       width;                   /**< lane width                                                             @unit m                                @version 0x0100 */
+    double      curvVert;                /**< vertical curvature in lane center                                      @unit 1/m                              @version 0x0100 */
+    double      curvVertDot;             /**< change of vertical curvature in lane center                            @unit 1/m2                             @version 0x0100 */
+    double      curvHor;                 /**< horizontal curvature in lane center                                    @unit 1/m                              @version 0x0100 */
+    double      curvHorDot;              /**< change of horizontal curvature in lane center                          @unit 1/m2                             @version 0x0100 */
+    uint32_t    playerId;                /**< id of the player to which this info belongs                            @unit _                                @version 0x0100 */
+    uint32_t    spare1;                  /**< for future use                                                         @unit _                                @version 0x0100 */
+} RDB_LANE_INFO_t;
+
+/** ------ configuration of an object (sent at start of sim and when triggered via SCP) ------ */
+typedef struct
+{
+    uint32_t id;                                    /**< unique object ID                                              @unit _                                  @version 0x0100 */
+    uint8_t  category;                              /**< object category                                               @unit @link RDB_OBJECT_CATEGORY @endlink @version 0x0100 */
+    uint8_t  type;                                  /**< object type                                                   @unit @link RDB_OBJECT_TYPE     @endlink @version 0x0100 */
+    int16_t  modelId;                               /**< visual model ID                                               @unit _                                  @version 0x0100 */
+    char     name[RDB_SIZE_OBJECT_NAME];            /**< symbolic name                                                 @unit _                                  @version 0x0100 */
+    char     modelName[RDB_SIZE_OBJECT_NAME];       /**< model name associated to an object                            @unit _                                  @version 0x0100 */
+    char     fileName[RDB_SIZE_FILENAME];           /**< filename associated to an object                              @unit _                                  @version 0x0100 */
+    uint16_t flags;                                 /**< object configuration flags                                    @unit @link RDB_OBJECT_CFG_FLAG @endlink @version 0x0100 */
+    uint16_t spare0;                                /**< reserved for future use                                       @unit _                                  @version 0x0100 */
+    uint32_t spare1;                                /**< reserved for future use                                       @unit _                                  @version 0x0100 */
+} RDB_OBJECT_CFG_t;
+
+/** ------ geometry information for an object --- */
+typedef struct
+{
+    float dimX;        /**< x dimension in object co-ordinates (length)                                               @unit m                                  @version 0x0100 */
+    float dimY;        /**< y dimension in object co-ordinates (width)                                                @unit m                                  @version 0x0100 */
+    float dimZ;        /**< z dimension in object co-ordinates (height)                                               @unit m                                  @version 0x0100 */
+    float offX;        /**< x distance from ref. point to center of geometry, object co-ordinate system               @unit m                                  @version 0x0100 */
+    float offY;        /**< y distance from ref. point to center of geometry, object co-ordinate system               @unit m                                  @version 0x0100 */
+    float offZ;        /**< z distance from ref. point to center of geometry, object co-ordinate system               @unit m                                  @version 0x0100 */
+} RDB_GEOMETRY_t;
+
+/** ------ state of an object (may be extended by the next structure) ------- */
+typedef struct
+{
+    uint32_t            id;                         /**< unique object ID                                              @unit _                                  @version 0x0100 */
+    uint8_t             category;                   /**< object category                                               @unit @link RDB_OBJECT_CATEGORY @endlink @version 0x0100 */
+    uint8_t             type;                       /**< object type                                                   @unit @link RDB_OBJECT_TYPE     @endlink @version 0x0100 */
+    uint16_t            visMask;                    /**< visibility mask                                               @unit @link RDB_OBJECT_VIS_FLAG @endlink @version 0x0100 */
+    char                name[RDB_SIZE_OBJECT_NAME]; /**< symbolic name                                                 @unit _                                  @version 0x0100 */
+    RDB_GEOMETRY_t      geo;                        /**< info about object's geometry                                  @unit m,m,m,m,m,m                        @version 0x0100 */
+    RDB_COORD_t         pos;                        /**< position and orientation of object's reference point          @unit m,m,m,rad,rad,rad                  @version 0x0100 */
+    uint32_t            parent;                     /**< unique ID of parent object                                    @unit _                                  @version 0x0100 */
+    uint16_t            cfgFlags;                   /**< configuration flags                                           @unit @link RDB_OBJECT_CFG_FLAG @endlink @version 0x0100 */
+    int16_t             cfgModelId;                 /**< visual model ID (configuration parameter)                     @unit _                                  @version 0x0100 */
+} RDB_OBJECT_STATE_BASE_t;
+
+/** ------ extended object data (e.g. for dynamic objects) ------- */
+typedef struct
+{
+    RDB_COORD_t         speed;                      /**< speed and rates                                               @unit m/s,m/s,m/s,rad/s,rad/s,rad/s          @version 0x0100 */
+    RDB_COORD_t         accel;                      /**< acceleration                                                  @unit m/s2,m/s2,m/s2,rad/s2,rad/s2/rad/s2    @version 0x0100 */
+    float               traveledDist;               /**< traveled distance                                             @unit m                                      @version 0x011a */
+    uint32_t            spare[3];                   /**< reserved for future use                                       @unit _                                      @version 0x0100 */
+} RDB_OBJECT_STATE_EXT_t;
+
+/** ------ complete object data (basic and extended info) ------- */
+typedef struct
+{
+    RDB_OBJECT_STATE_BASE_t base;           /**< state of an object     @unit RDB_OBJECT_STATE_BASE_t   @version 0x0100 */
+    RDB_OBJECT_STATE_EXT_t  ext;            /**< extended object data   @unit RDB_OBJECT_STATE_EXT_t    @version 0x0100 */
+} RDB_OBJECT_STATE_t;
+
+/** ------ standard engine information ------ */
+typedef struct
+{
+    uint32_t playerId;                    /**< unique ID of the player                          @unit _                         @version 0x0100 */
+    float    rps;                         /**< current rotation speed                           @unit 1/s                       @version 0x0100 */
+    float    load;                        /**< engine load (throttle position)                  @unit [0.0..1.0]                @version 0x0100 */
+    uint32_t spare1[2];                   /**< reserved for future use                          @unit _                         @version 0x0100 */
+} RDB_ENGINE_BASE_t;
+
+/** ------ extension of standard engine information ------ */
+typedef struct
+{
+    float    rpsStart;                    /**< start speed                                      @unit 1/s                       @version 0x0100 */
+    float    torque;                      /**< torque                                           @unit Nm                        @version 0x0100 */
+    float    torqueInner;                 /**< inner torque                                     @unit Nm                        @version 0x0100 */
+    float    torqueMax;                   /**< maximum torque                                   @unit Nm                        @version 0x0100 */
+    float    torqueFriction;              /**< friction torque                                  @unit Nm                        @version 0x0100 */
+    float    fuelCurrent;                 /**< current fuel consumption                         @unit l/100km                   @version 0x0100 */
+    float    fuelAverage;                 /**< average fuel consumption                         @unit l/100km                   @version 0x0100 */
+    float    oilTemperature;              /**< oil temperature                                  @unit deg                       @version 0x0119 */
+    float    temperature;                 /**< engine core temperature                          @unit deg                       @version 0x0119 */
+} RDB_ENGINE_EXT_t;
+
+/** ------ complete engine data (basic and extended info) ------- */
+typedef struct
+{
+    RDB_ENGINE_BASE_t base;             /**< standard engine information                @unit RDB_ENGINE_BASE_t @version 0x0100 */
+    RDB_ENGINE_EXT_t  ext;              /**< extension of standard engine information   @unit RDB_ENGINE_EXT_t  @version 0x0100 */
+} RDB_ENGINE_t;
+
+/** ------ standard drivetrain information ------ */
+typedef struct
+{
+    uint32_t playerId;                    /**< unique ID of the player                          @unit _                                     @version 0x0100 */
+    uint8_t  gearBoxType;                 /**< type of gear box                                 @unit @link RDB_GEAR_BOX_TYPE   @endlink    @version 0x0100 */
+    uint8_t  driveTrainType;              /**< type of drivetrain                               @unit @link RDB_DRIVETRAIN_TYPE @endlink    @version 0x0100 */
+    uint8_t  gear;                        /**< current gear position                            @unit @link RDB_GEAR_BOX_POS    @endlink    @version 0x0100 */
+    uint8_t  spare0;                      /**< reserved for future use                          @unit _                                     @version 0x0100 */
+    uint32_t spare1[2];                   /**< reserved for future use                          @unit _                                     @version 0x0100 */
+} RDB_DRIVETRAIN_BASE_t;
+
+/** ------ extension of standard drivetrain information ------ */
+typedef struct
+{
+    float    torqueGearBoxIn;             /**< torque at entry of gearbox                       @unit Nm                        @version 0x0100 */
+    float    torqueCenterDiffOut;         /**< torque at exit of center differential            @unit Nm                        @version 0x0100 */
+    float    torqueShaft;                 /**< torque at shaft                                  @unit Nm                        @version 0x0100 */
+    uint32_t spare1[2];                   /**< reserved for future use                          @unit _                         @version 0x0100 */
+} RDB_DRIVETRAIN_EXT_t;
+
+/** ------ complete engine data (basic and extended info) ------- */
+typedef struct
+{
+    RDB_DRIVETRAIN_BASE_t base;             /**< standard drivetrain information                @unit RDB_DRIVETRAIN_BASE_t @version 0x0100 */
+    RDB_DRIVETRAIN_EXT_t  ext;              /**< extension of standard drivetrain information   @unit RDB_DRIVETRAIN_EXT_t  @version 0x0100 */
+} RDB_DRIVETRAIN_t;
+
+/** ------ standard wheel information ------ */
+typedef struct
+{
+    uint32_t playerId;                    /**< ID of the player to which the wheel belongs      @unit _                             @version 0x0100 */
+    uint8_t  id;                          /**< ID of the wheel within the player                @unit @link RDB_WHEEL_ID @endlink   @version 0x0100 */
+    uint8_t  flags;                       /**< wheel status flags (e.g. for sound )             @unit @link RDB_WHEEL_FLAG @endlink @version 0x0114 */
+    uint8_t  spare0[2];                   /**< reserved for future use                          @unit _                             @version 0x0100 */
+    float    radiusStatic;                /**< static tire radius                               @unit m                             @version 0x0100 */
+    float    springCompression;           /**< compression of spring                            @unit m                             @version 0x0100 */
+    float    rotAngle;                    /**< angle of rotation                                @unit rad                           @version 0x0100 */
+    float    slip;                        /**< slip factor [0.0..1.0]                           @unit _                             @version 0x0100 */
+    float    steeringAngle;               /**< steering angle                                   @unit rad                           @version 0x0100 */
+    uint32_t spare1[4];                   /**< reserved for future use                          @unit _                             @version 0x0100 */
+} RDB_WHEEL_BASE_t;
+
+/** ------ extension of standard wheel information ------ */
+typedef struct
+{
+    float    vAngular;                    /**< angular velocity                                 @unit rad/s                     @version 0x0100 */
+    float    forceZ;                      /**< wheel contact force                              @unit N                         @version 0x0100 */
+    float    forceLat;                    /**< lateral force                                    @unit N                         @version 0x0100 */
+    float    forceLong;                   /**< longitudinal force                               @unit N                         @version 0x0100 */
+    float    forceTireWheelXYZ[3];        /**< force of tire on wheel                           @unit N                         @version 0x0100 */
+    float    radiusDynamic;               /**< dynamic tire radius                              @unit m                         @version 0x0100 */
+    float    brakePressure;               /**< brake pressure at wheel                          @unit Pa                        @version 0x0100 */
+    float    torqueDriveShaft;            /**< torque at drive shaft                            @unit Nm                        @version 0x0100 */
+    float    damperSpeed;                 /**< speed of damper                                  @unit m/s                       @version 0x0100 */
+    uint32_t spare2[4];                   /**< reserved for future use                          @unit _                         @version 0x0100 */
+} RDB_WHEEL_EXT_t;
+
+/** ------ complete wheel data (basic and extended info) ------- */
+typedef struct
+{
+    RDB_WHEEL_BASE_t base;              /**< standard wheel information                 @unit RDB_WHEEL_BASE_t  @version 0x0100 */
+    RDB_WHEEL_EXT_t  ext;               /**< extension of standard wheel information    @unit RDB_WHEEL_EXT_t   @version 0x0100 */
+} RDB_WHEEL_t;
+
+/** ------ vehicle system information ------ */
+typedef struct
+{
+    uint32_t playerId;                /**< ID of the player to which the data belongs                               @unit _                                        @version 0x0100 */
+    uint32_t lightMask;               /**< mask of active light systems                                             @unit @link RDB_VEHICLE_LIGHT @endlink         @version 0x0100 */
+    float    steering;                /**< front wheel steering angle (NOT: steering wheel angle)                   @unit rad                                      @version 0x0100 */
+    float    steeringWheelTorque;     /**< torque at the steering wheel                                             @unit Nm                                       @version 0x0100 */
+    uint8_t  accMask;                 /**< acc state mask                                                           @unit @link RDB_VEHICLE_ACC_FLAG @endlink      @version 0x010A */
+    uint8_t  accSpeed;                /**< acc speed indication                                                     @unit km/h [0..255]                            @version 0x010A */
+    uint8_t  batteryState;            /**< battery charge state                                                     @unit [0..255]                                 @version 0x010A */
+    int8_t   batteryRate;             /**< battery discharge / charge rate                                          @unit [-127..127]                              @version 0x010A */
+    uint16_t displayLightMask;        /**< lightmask for driver display (16 lights)                                 @unit @link RDB_VEHICLE_DISPLAY_LIGHT @endlink @version 0x010A */
+    uint16_t fuelGauge;               /**< fuel gauge [0.0..1.0]                                                    @unit [0..65535] = [0.0..1.0]                  @version 0x0119 */
+    uint32_t spare[5];                /**< spares for future use                                                    @unit _                                        @version 0x0100 */
+} RDB_VEHICLE_SYSTEMS_t;
+
+/** ------ vehicle setup information ------ */
+typedef struct
+{
+    uint32_t playerId;       /**< ID of the player to which the data belongs                                       @unit _                     @version 0x0100 */
+    float    mass;           /**< vehicle mass                                                                     @unit kg                    @version 0x0100 */
+    float    wheelBase;      /**< wheel base                                                                       @unit m                     @version 0x0100 */
+    int32_t  spare[4];       /**< for future use                                                                   @unit _                     @version 0x0100 */
+} RDB_VEHICLE_SETUP_t;
+
+/** ------ image information ------
+*   @note to be followed by actual image data
+*/
+typedef struct
+{
+    uint32_t id;                          /**< unique ID of the image (e.g. frame count)                            @unit _                             @version 0x0100 */
+    uint16_t width;                       /**< width of the image                                                   @unit pixel                         @version 0x0100 */
+    uint16_t height;                      /**< height of the image                                                  @unit pixel                         @version 0x0100 */
+    uint8_t  pixelSize;                   /**< memory size of a pixel                                               @unit bit                           @version 0x0100 */
+    uint8_t  pixelFormat;                 /**< format of a pixel                                                    @unit @link RDB_PIX_FORMAT @endlink @version 0x0100 */
+    uint16_t cameraId;                    /**< id of the camera which created the image (0=uninitialized)           @unit _                             @version 0x0115 */
+    uint32_t imgSize;                     /**< total size of image                                                  @unit byte                          @version 0x0100 */
+    uint8_t  color[4];                    /**< color for additional colorizing, if required                         @unit _                             @version 0x0100 */
+    uint32_t spare1[3];                   /**< for future use                                                       @unit _                             @version 0x0100 */
+} RDB_IMAGE_t;
+
+/** ------ custom light source information ------
+*   @note to be followed by actual light source intensity data
+*/
+typedef struct
+{
+    uint16_t lightElementId;              /**< unique ID of the light element                                       @unit _                             @version 0x0119 */
+    uint16_t width;                       /**< width of the light element control data                              @unit pixel                         @version 0x0119 */
+    uint16_t height;                      /**< height of the light element control data                             @unit pixel                         @version 0x0119 */
+    uint16_t spare0;                      /**< for future use                                                       @unit _                             @version 0x0119 */
+    uint32_t dataSize;                    /**< size of intensity data                                               @unit byte                          @version 0x0119 */
+    uint32_t spare1[3];                   /**< for future use                                                       @unit _                             @version 0x0119 */
+} RDB_CUSTOM_LIGHT_B_t;
+
+/** ------ custom light source information for light groups ------
+*/
+typedef struct
+{
+    uint16_t lightElementId;              /**< unique ID of the light element                                       @unit _                             @version 0x011D */
+    uint16_t groupId;                     /**< unique ID of the group within the light element                      @unit _                             @version 0x011D */
+    float    intensity;                   /**< intensity scale for the whole group                                  @unit _                             @version 0x011D */
+    float    hOffset;                     /**< heading offset of whole groups                                       @unit _                             @version 0x011D */
+    float    pOffset;                     /**< pitch offset of whole groups                                         @unit _                             @version 0x011D */
+    uint32_t spare[4];                    /**< for future use                                                       @unit _                             @version 0x011D */
+} RDB_CUSTOM_LIGHT_GROUP_B_t;
+
+
+/** ------ arbitrary x/y function ------
+*   @note to be followed by actual function points (each 2 or 3 doubles)
+*/
+typedef struct
+{
+    uint32_t id;                          /**< unique ID of the function                                            @unit _                                 @version 0x0100 */
+    uint8_t  type;                        /**< type of the function                                                 @unit @link RDB_FUNCTION_TYPE @endlink  @version 0x0100 */
+    uint8_t  dimension;                   /**< dimension of the function (xy=2, xyz=3)                              @unit _                                 @version 0x0100 */
+    uint16_t spare;                       /**< for future use                                                       @unit _                                 @version 0x0100 */
+    uint32_t dataSize;                    /**< total size of following data                                         @unit byte                              @version 0x0100 */
+    uint32_t spare1[4];                   /**< for future use                                                       @unit _                                 @version 0x0100 */
+} RDB_FUNCTION_t;
+
+/** ------ sensor definition and state ------ */
+typedef struct
+{
+    uint32_t    id;                          /**< id of the sensor                                      @unit _                                     @version 0x0100 */
+    uint8_t     type;                        /**< type of the sensor                                    @unit @link RDB_SENSOR_TYPE     @endlink    @version 0x0100 */
+    uint8_t     hostCategory;                /**< category of the object hosting the sensor             @unit @link RDB_OBJECT_CATEGORY @endlink    @version 0x0100 */
+    uint16_t    spare0;                      /**< for future use                                        @unit _                                     @version 0x0100 */
+    uint32_t    hostId;                      /**< unique id of the sensor's host                        @unit _                                     @version 0x0100 */
+    char        name[RDB_SIZE_OBJECT_NAME];  /**< name of the sensor                                    @unit _                                     @version 0x0100 */
+    float       fovHV[2];                    /**< field-of-view (horizontal/vertical)                   @unit rad,rad                               @version 0x0100 */
+    float       clipNF[2];                   /**< clipping ranges (near/far)                            @unit m,m                                   @version 0x0100 */
+    RDB_COORD_t pos;                         /**< position and orientation of sensor's reference point  @unit m,m,m,rad,rad,rad                     @version 0x0100 */
+    RDB_COORD_t originCoordSys;              /**< position and orientation of sensor's coord origin     @unit m,m,m,rad,rad,rad                     @version 0x0100 */ 
+    float       fovOffHV[2];                 /**< field-of-view offset (horizontal/vertical)            @unit rad, rad                              @version 0x011B */
+    int32_t     spare[2];                    /**< for future use                                        @unit _                                     @version 0x0100 */
+} RDB_SENSOR_STATE_t;
+
+/** ------ information about an object registered within a sensor ------ */
+typedef struct
+{
+    uint8_t     category;    /**< object category                                                                @unit @link RDB_OBJECT_CATEGORY    @endlink  @version 0x0100 */
+    uint8_t     type;        /**< object type                                                                    @unit @link RDB_OBJECT_TYPE        @endlink  @version 0x0100 */
+    uint16_t    flags;       /**< sensor object flags                                                            @unit @link RDB_SENSOR_OBJECT_FLAG @endlink  @version 0x0100 */
+    uint32_t    id;          /**< id of the object                                                               @unit _                                      @version 0x0100 */
+    uint32_t    sensorId;    /**< id of the detecting sensor                                                     @unit _                                      @version 0x0100 */
+    double      dist;        /**< distance between object and referring device                                   @unit m                                      @version 0x0100 */
+    RDB_COORD_t sensorPos;   /**< position and orientation of object in sensor coord                             @unit m,m,m,rad,rad,rad                      @version 0x0100 */
+    int8_t      occlusion;   /**< degree of occlusion for viewer (-1 = not valid, 0..127 = 0..100% occlusion)    @unit [-1, 0..127]                           @version 0x0100 */
+    uint8_t     spare0[3];   /**< for future use                                                                 @unit _                                      @version 0x0100 */
+    uint32_t    spare[3];    /**< for future use                                                                 @unit _                                      @version 0x0100 */
+} RDB_SENSOR_OBJECT_t;
+
+/** ------ camera information ------ */
+typedef struct
+{
+    uint16_t    id;                         /**< unique ID of the camera                                @unit _                        @version 0x0100 */
+    uint16_t    width;                      /**< width of viewport                                      @unit pixel                    @version 0x0100 */
+    uint16_t    height;                     /**< height of viewport                                     @unit pixel                    @version 0x0100 */
+    uint16_t    spare0;                     /**< for future use                                         @unit _                        @version 0x0100 */
+    float       clipNear;                   /**< near clipping plane                                    @unit m                        @version 0x0100 */
+    float       clipFar;                    /**< far clipping plane                                     @unit m                        @version 0x0100 */
+    float       focalX;                     /**< focal length in x direction                            @unit pixel                    @version 0x0100 */
+    float       focalY;                     /**< focal length in y direction                            @unit pixel                    @version 0x0100 */
+    float       principalX;                 /**< position of principal point in x direction             @unit pixel                    @version 0x0100 */
+    float       principalY;                 /**< position of principal point in y direction             @unit pixel                    @version 0x0100 */
+    RDB_COORD_t pos;                        /**< position and orientation                               @unit m,m,m,rad,rad,rad        @version 0x0100 */
+    uint32_t    spare1[4];                  /**< for future use                                         @unit _                        @version 0x0100 */
+} RDB_CAMERA_t;
+
+/** ------ basic info about a light illuminating the scene ------ */
+typedef struct
+{
+    uint16_t     id;                      /**< unique ID of the light source                                    @unit _                                     @version 0x0100 */
+    int8_t       templateId;              /**< template definition of light source (-1 deletes light source)    @unit _                                     @version 0x0100 */
+    uint8_t      state;                   /**< state of light source                                            @unit _                                     @version 0x0100 */
+    int32_t      playerId;                /**< ID of the player to which light source shall be linked           @unit _                                     @version 0x0100 */
+    RDB_COORD_t  pos;                     /**< position and orientation of light source                         @unit m,m,m,rad,rad,rad                     @version 0x0100 */
+    uint16_t     flags;                   /**< additional flags                                                 @unit @link RDB_LIGHT_SOURCE_FLAG @endlink  @version 0x0100 */
+    uint16_t     spare0;                  /**< just another spare                                               @unit _                                     @version 0x0100 */
+    int32_t      spare1[2];               /**< yet another spare                                                @unit _                                     @version 0x0100 */
+} RDB_LIGHT_SOURCE_BASE_t;
+
+/** ------ extended info about a light illuminating the scene ------ */
+typedef struct
+{
+    float        nearFar[2];              /**< near and far clip of light soure                                   @unit m,m                          @version 0x0100 */
+    float        frustumLRBT[4];          /**< frustum left / right / bottom / top                                @unit m,m,m,m                      @version 0x0100 */
+    float        intensity[3];            /**< intensity of the light (ambient, diffuse, specular)                @unit _,_,_                        @version 0x0100 */
+    float        atten[3];                /**< attenuation (constant, linear, quadratic)                          @unit _,_,_                        @version 0x0100 */
+    int32_t      spare1[3];               /**< yet another spare                                                  @unit _                            @version 0x0100 */
+} RDB_LIGHT_SOURCE_EXT_t;
+
+/** ------ complete light source data (basic and extended info) ------- */
+typedef struct
+{
+    RDB_LIGHT_SOURCE_BASE_t base;       /**< basic info about a light illuminating the scene    @unit RDB_LIGHT_SOURCE_BASE_t   @version 0x0100 */
+    RDB_LIGHT_SOURCE_EXT_t  ext;        /**< extended info about a light illuminating the scene @unit RDB_LIGHT_SOURCE_EXT_t    @version 0x0100 */
+} RDB_LIGHT_SOURCE_t;
+
+/** ------ info about an arbitrary contact point ------ */
+typedef struct
+{
+    uint16_t     id;             /**< unique ID of the contact point                                                                @unit _                                           @version 0x0100 */
+    uint16_t     flags;          /**< various flags with contact point options                                                      @unit @link RDB_CONTACT_POINT_FLAG @endlink       @version 0x0102 */
+    RDB_COORD_t  roadDataIn;     /**< inertial position of contact point; heading=0; pitch and roll relative to vehicle axis        @unit m,m,m,rad,rad,rad                           @version 0x0100 */
+    float        friction;       /**< road friction at contact point                                                                @unit _                                           @version 0x0100 */
+    int32_t      playerId;       /**< ID of the player to which CP belongs; only valid if corresponding flag is set                 @unit _                                           @version 0x0102 */
+    int32_t      spare1;         /**< just another spare                                                                            @unit _                                           @version 0x0100 */
+} RDB_CONTACT_POINT_t;
+
+/** ------ signal / sign info for a given vehicle ------ */
+typedef struct
+{
+    uint32_t     id;              /**< ID of the signal                                                                     @unit _                                           @version 0x0100 */
+    uint32_t     playerId;        /**< ID of the player who "detected" the signal                                           @unit _                                           @version 0x0100 */
+    float        roadDist;        /**< distance to signal along road                                                        @unit m                                           @version 0x0100 */
+    RDB_COORD_t  pos;             /**< inertial position of signal                                                          @unit m,m,m,rad,rad,rad                           @version 0x0100 */
+    int32_t      type;            /**< signal type according to OpenDRIVE                                                   @unit _                                           @version 0x0100 */
+    int32_t      subType;         /**< signal sub-type according to OpenDRIVE                                               @unit _                                           @version 0x0100 */
+    float        value;           /**< value associated with signal (e.g. speed, mass, validity)                            @unit depending on signal type, e.g. [m/s, kg, h] @version 0x0100 */
+    uint32_t     state;           /**< traffic sign's state (if dynamic)                                                    @unit _                                           @version 0x0100 */
+    int8_t       readability;     /**< sign's readability (-1 = not valid, 0..127 = 0..100% readability)                    @unit [-1, 0..127]                                @version 0x0100 */
+    int8_t       occlusion;       /**< degree of occlusion for viewer (-1 = not valid, 0..127 = 0..100% occlusion)          @unit [-1, 0..127]                                @version 0x0100 */
+    uint16_t     spare0;          /**< some more spare                                                                      @unit _                                           @version 0x0100 */
+    uint32_t     addOnId;         /**< ID of additional sign which extends this sign                                        @unit _                                           @version 0x0100 */
+    int8_t       minLane;         /**< ID minimum lane for which sign is valid                                              @unit [-127..127]                                 @version 0x0100 */
+    int8_t       maxLane;         /**< ID maximum lane for which sign is valid                                              @unit [-127..127]                                 @version 0x0100 */
+    uint16_t     spare;           /**< spare for upcoming info                                                              @unit _                                           @version 0x0100 */
+} RDB_TRAFFIC_SIGN_t;
+
+/** ------ road state for a given vehicle ------ */
+typedef struct
+{
+    uint32_t     playerId;        /**< ID of the player for which the state applies                            @unit _                              @version 0x0100 */
+    int8_t       wheelId;         /**< unique ID of the player's wheel for which state is valid (-1 for all)   @unit @link RDB_WHEEL_ID @endlink    @version 0x0100 */
+    uint8_t      spare0;          /**< yet another spare                                                       @unit _                              @version 0x0100 */
+    uint16_t     spare1;          /**< yet another spare                                                       @unit _                              @version 0x0100 */
+    uint32_t     roadId;          /**< unique ID of the road                                                   @unit _                              @version 0x0100 */
+    float        defaultSpeed;    /**< default speed of the road                                               @unit m/s                            @version 0x0100 */
+    float        waterLevel;      /**< rain level on road                                                      @unit [0.0..1.0]                     @version 0x0100 */
+    uint32_t     eventMask;       /**< road events                                                             @unit @link RDB_ROAD_EVENT @endlink  @version 0x0100 */
+    int32_t      spare2[12];      /**< for future use                                                          @unit _                              @version 0x0100 */
+} RDB_ROAD_STATE_t;
+
+/** ------ information about the environment state ------ */
+typedef struct
+{
+    float          visibility;    /**< visibility range                                                        @unit m                                  @version 0x0100 */
+    uint32_t       timeOfDay;     /**< time of day at sim start                                                @unit s                                  @version 0x0100 */
+    float          brightness;    /**< brightness of ambient light                                             @unit [0.0..1.0]                         @version 0x0100 */
+    uint8_t        precipitation; /**< intensity of precipitation                                              @unit [0..255]                           @version 0x0100 */
+    uint8_t        cloudState;    /**< cloud state                                                             @unit @link RDB_ENV_CLOUD_STATE @endlink @version 0x0100 */
+    uint16_t       flags;         /**< a series of environment flags                                           @unit @link RDB_ENV_FLAG        @endlink @version 0x0100 */
+    float          temperature;   /**< ambient temperature                                                     @unit [deg]                              @version 0x0119 */
+    uint32_t       spare1[7];     /**< yet another spare                                                       @unit _                                  @version 0x0100 */
+} RDB_ENVIRONMENT_t;
+
+/** ------ pedestrians animation data ------
+* @note this package is followed immediately by "dataSize" bytes of data, containing hinge information etc.
+*/
+typedef struct
+{
+    uint32_t          playerId;   /**< unique player ID                                                        @unit _                          @version 0x0100 */
+    RDB_COORD_t       pos;        /**< real-world position and orientation of reference point32_t              @unit m,m,m,rad,rad,rad          @version 0x0100 */
+    uint32_t          spare[4];   /**< some more spares                                                        @unit _                          @version 0x0100 */
+    uint32_t          noCoords;   /**< number of valid co-ordinates in coord array                             @unit _                          @version 0x0100 */
+    uint32_t          dataSize;   /**< size of the following data containing the actual co-ordinates           @unit byte                       @version 0x0100 */
+} RDB_PED_ANIMATION_t;
+
+/** ------ scoring information (for racing applications) ------ */
+typedef struct
+{
+    uint32_t playerId;         /**< unique player ID                                     @unit _                                    @version 0x0100 */
+    float    pathS;            /**< path position (negative if no path is available)     @unit m                                    @version 0x0100 */
+    float    roadS;            /**< road position (negative if no road is available)     @unit m                                    @version 0x0100 */
+    float    fuelCurrent;      /**< current fuel consumption                             @unit l/100km                              @version 0x0100 */
+    float    fuelAverage;      /**< average fuel consumption                             @unit l/100km                              @version 0x0100 */
+    uint32_t stateFlags;       /**< arbitrary state information                          @unit @link RDB_SCORING_FLAG @endlink      @version 0x0100 */
+    float    slip;             /**< slip factor [0.0..1.0]                               @unit _                                    @version 0x0100 */
+    uint32_t spare[4];         /**< we'll certainly have some more ideas....             @unit _                                    @version 0x0100 */
+} RDB_CUSTOM_SCORING_t;
+
+/** ------ simulation frame trigger information ------ */
+typedef struct
+{
+    float    deltaT;           /**< delta time by which to advance the simulation        @unit s                                    @version 0x0100 */
+    uint32_t frameNo;          /**< number of the simulation frame which is triggered    @unit _                                    @version 0x0100 */
+    uint16_t features;         /**< mask of features that shall be computed              @unit _                                    @version 0x011B */
+    int16_t  spare0;           /**< spare for future use                                 @unit _                                    @version 0x011B */
+} RDB_TRIGGER_t;
+
+/** ------ image generator trigger (live counter) information ------ */
+typedef struct
+{
+    float    deltaT;           /**< delta time provided by IG                            @unit s                                    @version 0x011B */
+    uint32_t frameNo;          /**< number of the IG frame which is triggering           @unit _                                    @version 0x011B */
+    uint32_t spare[2];         /**< spares for future use                                @unit _                                    @version 0x011B */
+} RDB_IG_FRAME_t;
+
+/** ------ information about driver control inputs (may be used e.g. for dynamics input) ------ */
+typedef struct
+{
+    uint32_t playerId;         /**< unique player ID to which the controls apply         @unit _                                        @version 0x0100 */
+    float    steeringWheel;    /**< steering wheel angle                                 @unit rad                                      @version 0x0100 */
+    float    steeringSpeed;    /**< steering speed                                       @unit rad/s                                    @version 0x0100 */
+    float    throttlePedal;    /**< throttle pedal position                              @unit [0.0..1.0]                               @version 0x0100 */
+    float    brakePedal;       /**< brake pedal position                                 @unit [0.0..1.0]                               @version 0x0100 */
+    float    clutchPedal;      /**< clutch pedal position                                @unit [0.0..1.0]                               @version 0x0100 */
+    float    accelTgt;         /**< desired acceleration                                 @unit m/s2                                     @version 0x0100 */
+    float    steeringTgt;      /**< desired steering angle at wheels                     @unit rad                                      @version 0x0100 */        /* TODO: maybe this should become the turn rate */
+    double   curvatureTgt;     /**< desired resulting curvature of the lateral motion    @unit 1/m                                      @version 0x0100 */
+    float    steeringTorque;   /**< torque at steering wheel                             @unit Nm                                       @version 0x0100 */ 
+    float    engineTorqueTgt;  /**< target engine torque                                 @unit Nm                                       @version 0x0100 */ 
+    float    speedTgt;         /**< target speed                                         @unit m/s                                      @version 0x0100 */ 
+    uint8_t  gear;             /**< desired gear box position                            @unit @link RDB_GEAR_BOX_POS          @endlink @version 0x0100 */
+    uint8_t  sourceId;         /**< unique number of the source providing this input     @unit @link RDB_DRIVER_SOURCE         @endlink @version 0x010E */
+    uint8_t  spare0[2];        /**< some spares for future use                           @unit _                                        @version 0x0100 */
+    uint32_t validityFlags;    /**< flags which of the above inputs are valid            @unit @link RDB_DRIVER_INPUT_VALIDITY @endlink @version 0x0100 */
+    uint32_t flags;            /**< input flags (indicator etc.)                         @unit @link RDB_DRIVER_FLAG           @endlink @version 0x0100 */
+    uint32_t mockupInput0;     /**< flags resulting from mockup buttons, part 1          @unit @link RDB_MOCKUP_INPUT0         @endlink @version 0x010A */
+    uint32_t mockupInput1;     /**< flags resulting from mockup buttons, part 2          @unit @link RDB_MOCKUP_INPUT1         @endlink @version 0x010A */
+    uint32_t mockupInput2;     /**< flags resulting from mockup buttons, part 3          @unit @link RDB_MOCKUP_INPUT2         @endlink @version 0x010A */
+    uint32_t spare;            /**< some spare  for future use                           @unit _                                        @version 0x010A */
+} RDB_DRIVER_CTRL_t;
+
+/** ------ information about driver control inputs (may be used e.g. for dynamics input) ------ */
+typedef struct
+{
+    uint32_t playerId;         /**< unique player ID to which the controls apply         @unit _                                            @version 0x0100 */
+    float    speedFromRules;   /**< speed from rules (i.e. road, signs etc.)             @unit m/s                                          @version 0x0100 */
+    float    distToSpeed;      /**< distance to next speed from rules (-1.0 to disable)  @unit m                                            @version 0x0100 */
+    float    spare0[4];        /**< just some spares                                     @unit _                                            @version 0x0100 */
+    uint32_t flags;            /**< input flags (turn dir etc.)                          @unit @link RDB_DRIVER_PERCEPTION_FLAG @endlink    @version 0x0100 */
+    uint32_t spare[4];         /**< some spares for future use                           @unit _                                            @version 0x0100 */
+} RDB_DRIVER_PERCEPTION_t;
+
+/** ------ information about a traffic light (state) ------ */
+typedef struct
+{
+   int32_t                   id;                             /**< unique ID of the traffic light                           @unit _                           @version 0x0100 */
+   float                     state;                          /**< current state (normalized)                               @unit [0.0..1.0]                  @version 0x0100 */
+   uint32_t                  stateMask;                      /**< current state mask (light mask, e.g. for gfx)            @unit _                           @version 0x0100 */
+} RDB_TRAFFIC_LIGHT_BASE_t;
+
+/** ------ traffic light phase information entry ------ */
+typedef struct
+{
+   float   duration;                        /**< normalized duration of the phase, invalid phases will have duration 0.0   @unit [0.0..1.0]                         @version 0x0100 */
+   uint8_t type;                            /**< type of the phase                                                         @unit @link RDB_TRLIGHT_PHASE @endlink   @version 0x0100 */
+   uint8_t spare[3];                        /**< spares for future use                                                     @unit _                                  @version 0x0100 */
+} RDB_TRAFFIC_LIGHT_PHASE_t;
+
+/** ------ extended information about a traffic light (phases, state details) ------
+ * @note this package is followed immediately by "dataSize" bytes of data, containing "noPhases" phase information entries RDB_TRAFFIC_LIGHT_PHASE_t */
+typedef struct
+{
+   int32_t                   ctrlId;                         /**< ID of the traffic light's controller                     @unit _                           @version 0x0100 */
+   float                     cycleTime;                      /**< duration of a complete cycle of all phases               @unit s                           @version 0x0100 */
+   uint16_t                  noPhases;                       /**< number of phases provided by this traffic light          @unit _                           @version 0x0100 */
+   uint32_t                  dataSize;                       /**< total size of phase data following the package           @unit _                           @version 0x0100 */
+} RDB_TRAFFIC_LIGHT_EXT_t;
+
+/** ------ complete traffic light data (basic and extended info) ------- */
+typedef struct
+{
+    RDB_TRAFFIC_LIGHT_BASE_t base;          /**< information about a traffic light state    @unit RDB_TRAFFIC_LIGHT_BASE_t  @version 0x0100 */
+    RDB_TRAFFIC_LIGHT_EXT_t  ext;           /**< extended information about a traffic light @unit RDB_TRAFFIC_LIGHT_EXT_t   @version 0x0100 */
+} RDB_TRAFFIC_LIGHT_t;
+
+/** ------ synchronization package ------- */
+typedef struct
+{
+    uint32_t mask;          /**< mask of required sync sources which are fulfilled by this sender            @unit _                        @version 0x0100 */
+    uint32_t cmdMask;       /**< mask of commands included in the sync message                               @unit RDB_SYNC_CMD             @version 0x010E */
+    double   systemTime;    /**< system time                                                                 @unit s                        @version 0x0118 */
+} RDB_SYNC_t;       
+
+/** ------ road query package ------- */
+typedef struct
+{
+    uint16_t id;            /**< unique ID of the road query point (reflected in answer)                     @unit _                        @version 0x0107 */
+    uint16_t flags;         /**< query flags                                                                 @unit RDB_ROAD_QUERY_FLAG      @version 0x0107 */
+    uint16_t spare[2];      /**< spare for future use                                                        @unit _                        @version 0x0107 */
+    double   x;             /**< x co-ordinate (inertial) of query location                                  @unit m                        @version 0x0107 */
+    double   y;             /**< y co-ordinate (inertial) of query location                                  @unit m                        @version 0x0107 */
+} RDB_ROAD_QUERY_t;       
+
+/** ------ wrapper for SCP messages ------- 
+ * @note this package is followed immediately by "dataSize" bytes of data, containing the actual SCP command string */
+typedef struct
+{
+    uint16_t  version;                     /**< upper byte = major, lower byte = minor                       @unit _                        @version 0x010C */
+    uint16_t  spare;                       /**< just another spare                                           @unit _                        @version 0x010C */
+    char      sender[RDB_SIZE_SCP_NAME];   /**< name of the sender as text                                   @unit _                        @version 0x010C */
+    char      receiver[RDB_SIZE_SCP_NAME]; /**< name of the receiver as text                                 @unit _                        @version 0x010C */
+    uint32_t  dataSize;                    /**< number of data bytes following this entry                    @unit _                        @version 0x010C */
+} RDB_SCP_t;
+
+/** ------ wrapper for forwarded messages ------- 
+ * @note this package is followed immediately by "dataSize" bytes of data, containing the actual forwarded message */
+typedef struct
+{
+    uint16_t  protocol;                    /**< protocol identifier of the wrapped package                   @unit _                        @version 0x0112 */
+    uint16_t  pkgId;                       /**< unique pkg id                                                @unit _                        @version 0x0112 */
+    uint32_t  spare[6];                    /**< some spares                                                  @unit _                        @version 0x0112 */
+    uint32_t  dataSize;                    /**< number of data bytes following this entry                    @unit _                        @version 0x0112 */
+} RDB_PROXY_t;
+
+/** ------ trajectory planning points of a player ------- 
+ * @note this package is immediately followed by "noDataPoints" entries of type RDB_POINT_t
+*/
+typedef struct
+{
+    uint32_t  playerId;                    /**< unique player / object ID                                    @unit _                                       @version 0x0110 */
+    double    spacing;                     /**< spacing of data points, either in space or time domain       @unit m, s                                    @version 0x0110 */
+    uint16_t  flags;                       /**< flags for treatment of trajectory                            @unit @link RDB_TRAJECTORY_FLAG @endlink      @version 0x0110 */
+    uint16_t  noDataPoints;                /**< number of trajectory points following this package           @unit _                                       @version 0x0110 */
+    uint32_t  spare[4];                    /**< some more spares                                             @unit _                                       @version 0x0110 */
+} RDB_TRAJECTORY_t;
+
+/** ------ state of the  motion system ------- */
+typedef struct
+{
+    uint32_t            playerId;                   /**< unique player / object ID to which the information applies    @unit _                                      @version 0x0116 */
+    uint32_t            flags;                      /**< flags for state monitoring etc.                               @unit @link RDB_MOTION_SYSTEM_FLAG  @endlink @version 0x0116 */
+    RDB_COORD_t         pos;                        /**< position and orientation of eyepoint                          @unit m,m,m,rad,rad,rad                      @version 0x0116 */
+    RDB_COORD_t         speed;                      /**< transversal and angular speed of eyepoint                     @unit m,m,m,rad,rad,rad                      @version 0x0116 */
+    uint32_t            spare[6];                   /**< reserved for future use                                       @unit _                                      @version 0x0116 */
+} RDB_MOTION_SYSTEM_t;
+
+/** ------ custom object control package ------ */
+typedef struct
+{
+    uint32_t playerId;              /**< unique player ID                                                             @unit _                                                        @version 0x0119 */
+    uint16_t flags;                 /**< configuraton flags                                                           @unit @link RDB_CUSTOM_TRACK_CTRL_FLAG  @endlink               @version 0x0119 */
+    uint8_t  posType;               /**< player position type                                                         @unit 0 = default, 1 = rel. Ego, 2 = rel. path, 3 = rel. road  @version 0x0119 */
+    int8_t   dir;                   /**< direction of driving (1=with track, -1=opposite track)                       @unit [-1,1]                                                   @version 0x0119 */
+    uint32_t roadId;                /**< unique road ID                                                               @unit _                                                        @version 0x0119 */
+    double   initialRoadDeltaS;     /**< initial road position relative to Ego (negative if no road is available)     @unit m                                                        @version 0x0119 */
+    float    targetRoadT;           /**< target lateral road position; for first frame, it's the initial roadT        @unit m                                                        @version 0x0119 */
+    float    speedTgtS;             /**< target speed along road                                                      @unit m/s                                                      @version 0x0119 */
+    float    minAccelS;             /**< minimum acceleration along road                                              @unit m/s2                                                     @version 0x0119 */
+    float    maxAccelS;             /**< maximum acceleration along road                                              @unit m/s2                                                     @version 0x0119 */
+    float    accelTgt;              /**< target acceleration (vehicle system)                                         @unit m/s2                                                     @version 0x0119 */
+    uint32_t validityFlags;         /**< flags which of the above inputs are valid                                    @unit @link RDB_CUSTOM_TRACK_CTRL_VALIDITY @endlink            @version 0x0119 */
+    uint32_t spare[4];              /**< we'll certainly have some more ideas....                                     @unit _                                                        @version 0x0119 */
+} RDB_CUSTOM_OBJECT_CTRL_TRACK_t;
+
+/** ------ freespace description package ------ */
+typedef struct
+{
+     uint32_t playerId;             /**< unique player ID                                                             @unit _                                                        @version 0x011A */
+     float    distance;             /**< distance to closest object                                                   @unit m                                                        @version 0x011A */ 
+     float    angleLeft;            /**< angle to left-most boundary of all objects forming the freespace             @unit rad                                                      @version 0x011A */
+     float    angleRight;           /**< angle to right-most boundary of all objects forming the freespace            @unit rad                                                      @version 0x011A */
+     float    distanceLeft;         /**< distance to item defining left angle                                         @unit m                                                        @version 0x011A */ 
+     float    distanceRight;        /**< distance to item defining right angle                                        @unit m                                                        @version 0x011A */ 
+     uint8_t  stateLeft;            /**< state of left boundary                                                       @unit @link RDB_FREESPACE_STATE  @endlink                      @version 0x011A */
+     uint8_t  stateRight;           /**< state of right boundary                                                      @unit @link RDB_FREESPACE_STATE  @endlink                      @version 0x011A */
+     uint8_t  stateDistance;        /**< state of distance information                                                @unit @link RDB_FREESPACE_STATE  @endlink                      @version 0x011A */
+     uint8_t  noVisibleObjects;     /**< number of visible objects in range                                           @unit _                                                        @version 0x011A */
+     uint32_t spare1[3];            /**< we'll certainly have some more ideas....                                     @unit _                                                        @version 0x011A */
+} RDB_FREESPACE_t;
+
+/** ------ generit switch description package ------ */
+typedef struct
+{
+    uint32_t objectId;              /**< ID of the object which contains the switch                          @unit _                                @version 0x011B */
+    uint32_t elementId;             /**< ID of the switch itself                                             @unit _                                @version 0x011B */
+    uint8_t  scope;                 /**< scope of the dynamic element                                        @unit @link RDB_DYN_EL_SCOPE  @endlink @version 0x011B */
+    uint8_t  spare0[3];             /**< for future use                                                      @unit _                                @version 0x011B */
+    uint32_t state;                 /**< state of the switch as bit mask                                     @unit _                                @version 0x011B */
+    uint32_t spare1[2];             /**< for future use                                                      @unit _                                @version 0x011B */
+} RDB_DYN_EL_SWITCH_t;                                                                                              
+                                    
+/** ------ generic DOF description package ------ 
+ * @note this package is immediately followed by "nValues" entries of type double
+*/
+typedef struct                      
+{                                   
+    uint32_t objectId;              /**< ID of the object which contains the switch                          @unit _                                                @version 0x011B */
+    uint32_t elementId;             /**< ID of the switch itself                                             @unit _                                                @version 0x011B */
+    uint8_t  scope;                 /**< scope of the dynamic element                                        @unit @link RDB_DYN_EL_SCOPE  @endlink                 @version 0x011B */
+    uint8_t  validity;              /**< mask of elements controlled by the following double numbers         @unit [x=0x01, y=0x02, z=0x04, h=0x08, p=0x10, r=0x20] @version 0x011B */           
+    uint8_t  nValues;               /**< number of double values sent immediately after this structure       @unit _                                                @version 0x011B */
+    uint8_t  spare0;                /**< for future use                                                      @unit _                                                @version 0x011B */
+    uint32_t spare1[3];             /**< for future use                                                      @unit _                                                @version 0x011B */
+} RDB_DYN_EL_DOF_t;
+
+#ifndef MATLAB_MEX_FILE
+/** ------ empty structure for end-of-frame message (not to be used) -------  */
+typedef struct
+{
+} RDB_END_OF_FRAME_t;
+
+/** ------ empty structure for start-of-frame message (not to be used) -------  */
+typedef struct
+{
+} RDB_START_OF_FRAME_t;
+#endif
+
+/** ------ structure for SHM interface from steering wheel control to dynamics -------  */
+typedef struct
+{
+    uint32_t  playerId;     /**< unique player / object ID                           @unit _                                      @version 0x0111 */
+    uint32_t  state;        /**< steering wheel state flags                          @unit @link RDB_STEER_2_DYN_STATE @endlink   @version 0x0111 */
+    float     angle;        /**< steering wheel angle                                @unit rad                                    @version 0x0111 */
+    float     rev;          /**< steering wheel rot speed                            @unit rad/s                                  @version 0x0111 */
+    float     torque;       /**< steering wheel current torque                       @unit Nm                                     @version 0x0111 */
+    uint32_t  spare[8];     /**< some more spares                                    @unit _                                      @version 0x0111 */
+} RDB_STEER_2_DYN_t;
+
+/** ------ structure for SHM interface from dynamics to steering wheel control -------  */
+typedef struct
+{
+    uint32_t  playerId;         /**< unique player / object ID                            @unit _                                         @version 0x0111 */
+    uint16_t  state;            /**< dynamics state.                                      @unit @link RDB_DYN_2_STEER_STATE   @endlink    @version 0x0111 */
+    uint16_t  cmd;              /**< command to the steering wheel                        @unit @link RDB_DYN_2_STEER_CMD     @endlink    @version 0x0111 */
+	uint32_t  effects;          /**< special effects flag.                                @unit @link RDB_DYN_2_STEER_EFFECTS @endlink    @version 0x0111 */
+    float     torque;           /**< desired torque                                       @unit Nm                                        @version 0x0111 */
+    float     friction;         /**< friction                                             @unit Nm                                        @version 0x0111 */
+    float     damping;          /**< damping                                              @unit Nm/rad/s                                  @version 0x0111 */
+    float     stiffness;        /**< stiffness                                            @unit Nm/rad                                    @version 0x0111 */
+    float     velocity;         /**< vehicle longitudinal velocity                        @unit m/s                                       @version 0x0111 */
+    float     angle;            /**< resulting steering wheel angle (for passive mode)    @unit rad                                       @version 0x0111 */
+    float     neutralPos;       /**< neutral position of steering wheel                   @unit rad                                       @version 0x0119 */
+    float     dampingMaxTorque; /**< damping, maximum torque                              @unit Nm/                                       @version 0x0119 */
+    uint32_t  spare[6];         /**< some more spares                                     @unit _                                         @version 0x0111 */
+} RDB_DYN_2_STEER_t;
+
+/** ------ structure for ray casting -------  */
+typedef struct
+{
+    uint32_t    id;               /**< unique ray ID                                        @unit _                                         @version 0x011C */
+    uint32_t    emitterId;        /**< unique ID of the ray's emitter                       @unit _                                         @version 0x011C */
+    uint8_t     type;             /**< type of the ray information                          @unit @link RDB_RAY_TYPE @endlink               @version 0x011C */
+    uint8_t     spare0;           /**< spare (to be used later on for emitter category etc. @unit _                                         @version 0x011C */
+    uint16_t    spare2;           /**< spare (to be used later on for emitter category etc. @unit _                                         @version 0x011C */
+    float       length;           /**< (maximum) length of the ray                          @unit _                                         @version 0x011C */
+    uint32_t    spare1[3];        /**< some more spares                                     @unit _                                         @version 0x011C */
+    RDB_COORD_t ray;              /**< the ray itself (origin + direction)                  @unit m,m,m,rad,rad,rad                         @version 0x011C */
+} RDB_RAY_t;
+
+/** ------ structure for performance monitoring -------  */
+typedef struct
+{
+    uint32_t    noOverruns;       /**< number of real-time overruns                         @unit _                                         @version 0x011D */
+    uint32_t    noUnderruns;      /**< number of real-time underruns                        @unit _                                         @version 0x011D */
+    uint32_t    noMeasurements;   /**< number of measured frames                            @unit _                                         @version 0x011D */
+    float       tolerance;        /**< tolerance of real-time watchdog                      @unit [s]                                       @version 0x011D */
+    float       nominalFrameTime; /**< nominal duration of a frame                          @unit [s]                                       @version 0x011D */
+    float       actualFrameTime;  /**< actual (measured) duration of last frame             @unit [s]                                       @version 0x011D */
+    uint32_t    spare1[6];        /**< some more spares                                     @unit _                                         @version 0x011D */
+} RDB_RT_PERFORMANCE_t;
+
+/** ------ header of a complete message ------ */
+typedef struct
+{
+    uint16_t  magicNo;      /**< must be RDB_MAGIC_NO (35712)                                               @unit @link GENERAL_DEFINITIONS @endlink   @version 0x0100 */
+    uint16_t  version;      /**< upper byte = major, lower byte = minor                                     @unit _                                    @version 0x0100 */
+    uint32_t  headerSize;   /**< size of this header structure when transmitted                             @unit byte                                 @version 0x0100 */
+    uint32_t  dataSize;     /**< size of data following the header                                          @unit byte                                 @version 0x0100 */
+    uint32_t  frameNo;      /**< number of the simulation frame                                             @unit _                                    @version 0x0100 */
+    double    simTime;      /**< simulation time                                                            @unit s                                    @version 0x0100 */
+} RDB_MSG_HDR_t;                                                                                              
+
+/** ------ header of a package vector within a message ------ */
+typedef struct
+{
+    uint32_t  headerSize;   /**< size of this header structure when transmitted                              @unit byte                     @version 0x0100 */
+    uint32_t  dataSize;     /**< size of data following the header                                           @unit byte                     @version 0x0100 */
+    uint32_t  elementSize;  /**< if data following the header contains an array of elements of equal size:
+                                 size of one element in this data
+                                 (elementSize is equivalent to dataSize if only one element is transmitted)  @unit byte                         @version 0x0100 */
+    uint16_t  pkgId;        /**< package identifier                                                          @unit _                            @version 0x0100 */
+    uint16_t  flags;        /**< various flags concerning the package's contents (e.g. extension)            @unit @link RDB_PKG_FLAG @endlink  @version 0x0100 */
+} RDB_MSG_ENTRY_HDR_t;
+
+
+/** ------ the message union (use for very simplistic casting only)------ */
+typedef union
+{
+    RDB_COORD_SYSTEM_t              coordSystem;                   /**< (custom) co-ordinate system                                         @msgid RDB_PKG_ID_COORD_SYSTEM              */
+    RDB_COORD_t                     coord;                         /**< single co-ordinate extending previous object information            @msgid RDB_PKG_ID_COORD                     */
+    RDB_ROAD_POS_t                  roadPos;                       /**< detailed road position of a given entity                            @msgid RDB_PKG_ID_ROAD_POS                  */
+    RDB_LANE_INFO_t                 laneInfo;                      /**< lane information for a given entity                                 @msgid RDB_PKG_ID_LANE_INFO                 */
+    RDB_ROADMARK_t                  roadMark;                      /**< road mark information for a player                                  @msgid RDB_PKG_ID_ROADMARK                  */
+    RDB_OBJECT_CFG_t                objectCfg;                     /**< info about a (traffic) object configuration                         @msgid RDB_PKG_ID_OBJECT_CFG                */
+    RDB_OBJECT_STATE_t              objectState;                   /**< info about an arbitrary object's state                              @msgid RDB_PKG_ID_OBJECT_STATE              */
+    RDB_VEHICLE_SYSTEMS_t           vehicleSystems;                /**< vehicle systems' states (lights etc.)                               @msgid RDB_PKG_ID_VEHICLE_SYSTEMS           */
+    RDB_VEHICLE_SETUP_t             vehicleSetup;                  /**< basic information about a vehicle (mass etc.)                       @msgid RDB_PKG_ID_VEHICLE_SETUP             */
+    RDB_ENGINE_t                    engine;                        /**< info about a vehicle's engine                                       @msgid RDB_PKG_ID_ENGINE                    */
+    RDB_DRIVETRAIN_t                drivetrain;                    /**< info about a vehicle's drivetrain                                   @msgid RDB_PKG_ID_DRIVETRAIN                */
+    RDB_WHEEL_t                     wheel;                         /**< info about a wheel of a player                                      @msgid RDB_PKG_ID_WHEEL                     */
+    RDB_PED_ANIMATION_t             pedAnimation;                  /**< pedestrian animation details (joint angles etc.)                    @msgid RDB_PKG_ID_PED_ANIMATION             */
+    RDB_SENSOR_STATE_t              sensorState;                   /**< state of a sensor                                                   @msgid RDB_PKG_ID_SENSOR_STATE              */
+    RDB_SENSOR_OBJECT_t             sensorObject;                  /**< state of an object detected by a sensor                             @msgid RDB_PKG_ID_SENSOR_OBJECT             */
+    RDB_CAMERA_t                    camera;                        /**< camera settings used for video image                                @msgid RDB_PKG_ID_CAMERA                    */
+    RDB_CONTACT_POINT_t             contactPoint;                  /**< info about contact points                                           @msgid RDB_PKG_ID_CONTACT_POINT             */
+    RDB_TRAFFIC_SIGN_t              trafficSign;                   /**< info about traffic signs seen by a vehicle                          @msgid RDB_PKG_ID_TRAFFIC_SIGN              */
+    RDB_ROAD_STATE_t                roadState;                     /**< road state informaton for a given player                            @msgid RDB_PKG_ID_ROAD_STATE                */
+    RDB_IMAGE_t                     image;                         /**< video image (followed by data)                                      @msgid RDB_PKG_ID_IMAGE                     */
+    RDB_LIGHT_SOURCE_t              lightSrc;                      /**< info about a light source                                           @msgid RDB_PKG_ID_LIGHT_SOURCE              */
+    RDB_ENVIRONMENT_t               environment;                   /**< info about environment state                                        @msgid RDB_PKG_ID_ENVIRONMENT               */
+    RDB_TRIGGER_t                   trigger;                       /**< trigger information                                                 @msgid RDB_PKG_ID_TRIGGER                   */
+    RDB_DRIVER_CTRL_t               driverCtrl;                    /**< driver input (e.g. from mockup) - serves also as dynamics input     @msgid RDB_PKG_ID_DRIVER_CTRL               */
+    RDB_TRAFFIC_LIGHT_t             trafficLight;                  /**< info about a traffic light                                          @msgid RDB_PKG_ID_TRAFFIC_LIGHT             */
+    RDB_SYNC_t                      sync;                          /**< sync source message                                                 @msgid RDB_PKG_ID_SYNC                      */
+    RDB_DRIVER_PERCEPTION_t         driverPerception;              /**< perception of road/rule environment by driver                       @msgid RDB_PKG_ID_DRIVER_PERCEPTION         */
+    RDB_IMAGE_t                     lightMap;                      /**< light distribution image (followed by data)                         @msgid RDB_PKG_ID_LIGHT_MAP                 */
+    RDB_FUNCTION_t                  toneMapping;                   /**< tone mapping function                                               @msgid RDB_PKG_ID_TONE_MAPPING              */
+    RDB_PROXY_t                     proxy;                         /**< proxy package with payload                                          @msgid RDB_PKG_ID_PROXY                     */
+    RDB_MOTION_SYSTEM_t             motionSystem;                  /**< information about motion system state                               @msgid RDB_PKG_ID_MOTION_SYSTEM             */
+    RDB_IG_FRAME_t                  igFrame;                       /**< information about IG frame upon arrival                             @msgid RDB_PKG_ID_IG_FRAME                  */
+    /* custom packages here, please */
+    RDB_CUSTOM_SCORING_t            scoring;                       /**< scoring information                                                 @msgid RDB_PKG_ID_CUSTOM_SCORING            */
+} RDB_MSG_UNION_t;
+
+/** ------ now a complete message containing one element only ------ */
+typedef struct
+{
+    RDB_MSG_HDR_t       hdr;            /**< header of a complete message                   @unit RDB_MSG_HDR_t         @version 0x0100 */
+    RDB_MSG_ENTRY_HDR_t entryHdr;       /**< header of a package vector within a message    @unit RDB_MSG_ENTRY_HDR_t   @version 0x0100 */
+    RDB_MSG_UNION_t     u;              /**< the message union                              @unit RDB_MSG_UNION_t       @version 0x0100 */
+} RDB_MSG_t;
+
+/** ------ shared memory segment header, located of beginning of shared memory segment ------ */
+typedef struct
+{
+    uint32_t  thisSize;   /**< size of this info structure                               @unit byte                                 @version 0x0100 */
+    uint32_t  bufferSize; /**< size of the associated buffer                             @unit byte                                 @version 0x0100 */
+    uint16_t  id;         /**< unique ID of the buffer                                   @unit _                                    @version 0x0100 */
+    uint16_t  spare0;     /**< just some spare                                           @unit _                                    @version 0x0100 */
+    uint32_t  flags;      /**< access flags etc.                                         @unit @link RDB_SHM_BUFFER_FLAG @endlink   @version 0x0100 */
+    uint32_t  offset;     /**< offset to start of buffer from start of shared memory     @unit byte                                 @version 0x0100 */
+    uint32_t  spare1[4];  /**< just some spares                                          @unit _                                    @version 0x0100 */
+} RDB_SHM_BUFFER_INFO_t;
+
+/** ------ shared memory management header, located of beginning of shared memory ------
+* @note this entry is followed immediately by "noBuffers" bytes of type RDB_SHM_BUFFER_INFO_t, containing "noBuffers" buffer information entries
+*/
+typedef struct
+{
+    uint32_t  headerSize;     /**< size of this header structure                               @unit byte                     @version 0x0100 */
+    uint32_t  dataSize;       /**< size of data following this header                          @unit byte                     @version 0x0100 */
+    uint8_t   noBuffers;      /**< number of data buffers in the shared memory                 @unit _                        @version 0x0100 */
+} RDB_SHM_HDR_t;
+
+#endif        /* _VIRES_RDB_ICD_H */
+
+/* end of pragma 4 */
+#pragma pack(pop)

src/ros/catkin/src/vtd_ros/src/main.cpp

@@ -0,0 +1,41 @@
+#include "ros/ros.h"
+
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+//#include <asm/termios.h>
+#include <termios.h>
+
+
+
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/PointStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+
+#include <thread>
+
+static std::string _pose_topic = "/current_pose";
+static std::string _twist_topic =  "/current_velocity";
+
+
+int main(int argc, char *argv[])
+{
+	ros::init(argc, argv, "vtd_rdb_ros");
+//	ros::NodeHandle n;
+    ros::NodeHandle private_nh("~");
+	ros::Rate loop_rate(100);
+
+	    
+    private_nh.getParam("pose_topic",_pose_topic);
+	private_nh.getParam("twist_topic",_twist_topic);
+    std::cout<<"pose_topic  is "<<_pose_topic<<std::endl;
+	std::cout<<"twist_topic : "<<_twist_topic<<std::endl;
+
+
+
+ros::spin();
+
+}

src/ros/catkin/src/vtd_ros/src/rdbconn.cpp

@@ -0,0 +1,362 @@
+#include "rdbconn.h"
+
+#include <iostream>
+
+#define DEFAULT_BUFFER      204800
+
+RDBConn::RDBConn(const char * strserip,int port)
+{
+    strncpy(mstrserverip,strserip,256);
+    mnserverport = port;
+    mpthreadconn = new std::thread(&RDBConn::threadconn,this, mstrserverip,mnserverport);
+}
+
+RDBConn::~RDBConn()
+{
+    mbthreadconn = false;
+    mpthreadconn->join();
+}
+
+void RDBConn::threadsend(int nsock)
+{
+    while(mbthreadconn)
+    {
+        if(mbEgoUpdate)
+        {
+            Framework::RDBHandler myHandler;
+
+            // start a new message
+            myHandler.initMsg();
+
+            // begin with an SOF identifier
+            myHandler.addPackage( simTime, simFrame, RDB_PKG_ID_START_OF_FRAME );
+
+            // add extended package for the object state
+            RDB_OBJECT_STATE_t *objState = ( RDB_OBJECT_STATE_t* ) myHandler.addPackage( simTime, simFrame, RDB_PKG_ID_OBJECT_STATE, 1, true );
+
+            if ( !objState )
+            {
+                fprintf( stderr, "sendOwnObjectState: could not create object state\n" );
+                return;
+            }
+
+            // copy contents of internally held object state to output structure
+            memcpy( objState, &sOwnObjectState, sizeof( RDB_OBJECT_STATE_t ) );
+
+            fprintf( stderr, "sendOwnObjectState: sending pos x/y/z = %.3lf/%.3lf/%.3lf,\n", objState->base.pos.x, objState->base.pos.y, objState->base.pos.z );
+
+            // terminate with an EOF identifier
+            myHandler.addPackage( simTime, simFrame, RDB_PKG_ID_END_OF_FRAME );
+
+            int retVal = send( nsock, ( const char* ) ( myHandler.getMsg() ), myHandler.getMsgTotalSize(), 0 );
+
+            if ( !retVal )
+                fprintf( stderr, "sendOwnObjectState: could not send object state\n" );
+            mbEgoUpdate = false;
+        }
+        else
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+    }
+}
+
+void RDBConn::threadconn(char *strserip, int nport)
+{
+
+    unsigned int  bytesInBuffer = 0;
+    size_t        bufferSize    = sizeof( RDB_MSG_HDR_t );
+    unsigned char *pData        = new unsigned char[DEFAULT_BUFFER];
+
+
+
+
+    bool bConnect = false;
+    int sClient =  socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
+    if(sClient == -1)
+    {
+        std::cout<<"socket() failed."<<std::endl;
+        return;
+    }
+
+    int opt = 1;
+    setsockopt ( sClient, IPPROTO_TCP, TCP_NODELAY, &opt, sizeof( opt ) );
+
+    struct sockaddr_in server;
+    struct hostent    *host = NULL;
+    server.sin_family      = AF_INET;
+    server.sin_port        = htons(nport);
+    server.sin_addr.s_addr = inet_addr(strserip);
+
+    if ( server.sin_addr.s_addr == INADDR_NONE )
+    {
+        host = gethostbyname(strserip);
+        if ( host == NULL )
+        {
+            std::cout<<"Unable to resolve VTD server:"<<strserip<<std::endl;
+            return ;
+        }
+        memcpy( &server.sin_addr, host->h_addr_list[0], host->h_length );
+    }
+
+//    bool bMsgComplete = false;
+    int ret;
+    char* szBuffer = new char[DEFAULT_BUFFER];  // allocate on heap
+
+
+    std::thread * pthreadsend = NULL;
+
+    while(mbthreadconn)
+    {
+
+        if(bConnect == false)
+        {
+            std::cout<<"start connect server."<<std::endl;
+            if (connect( sClient, (struct sockaddr *)&server, sizeof( server ) ) == -1 )
+            {
+                std::cout<<"VTD connect() failed. Server:"<<strserip<<" port:"<<nport<< std::endl;
+                std::this_thread::sleep_for(std::chrono::seconds(1));
+                continue;
+            }
+            else
+            {
+                bConnect = true;
+                std::cout<<" connected to server."<<std::endl;
+            }
+        }
+
+        if(pthreadsend == NULL)
+        {
+            pthreadsend = new std::thread(&RDBConn::threadsend,this,sClient);
+        }
+
+        ret = recv( sClient, szBuffer, DEFAULT_BUFFER, 0 );
+
+        std::cout<<" ret: "<<ret<<std::endl;
+
+//        if(mbEgoUpdate)
+//        {
+//            Framework::RDBHandler myHandler;
+
+//            // start a new message
+//            myHandler.initMsg();
+
+//            // begin with an SOF identifier
+//            myHandler.addPackage( simTime, simFrame, RDB_PKG_ID_START_OF_FRAME );
+
+//            // add extended package for the object state
+//            RDB_OBJECT_STATE_t *objState = ( RDB_OBJECT_STATE_t* ) myHandler.addPackage( simTime, simFrame, RDB_PKG_ID_OBJECT_STATE, 1, true );
+
+//            if ( !objState )
+//            {
+//                fprintf( stderr, "sendOwnObjectState: could not create object state\n" );
+//                return;
+//            }
+
+//            // copy contents of internally held object state to output structure
+//            memcpy( objState, &sOwnObjectState, sizeof( RDB_OBJECT_STATE_t ) );
+
+//            fprintf( stderr, "sendOwnObjectState: sending pos x/y/z = %.3lf/%.3lf/%.3lf,\n", objState->base.pos.x, objState->base.pos.y, objState->base.pos.z );
+
+//            // terminate with an EOF identifier
+//            myHandler.addPackage( simTime, simFrame, RDB_PKG_ID_END_OF_FRAME );
+
+//            int retVal = send( sClient, ( const char* ) ( myHandler.getMsg() ), myHandler.getMsgTotalSize(), 0 );
+
+//            if ( !retVal )
+//                fprintf( stderr, "sendOwnObjectState: could not send object state\n" );
+//            mbEgoUpdate = false;
+//        }
+
+        if ( ret == -1 )
+        {
+            printf( "recv() failed: %s\n", strerror( errno ) );
+            break;
+        }
+
+        if ( ret != 0 )
+        {
+            // do we have to grow the buffer??
+            if ( ( bytesInBuffer + ret ) > bufferSize )
+            {
+                pData      = ( unsigned char* ) realloc( pData, bytesInBuffer + ret );
+                bufferSize = bytesInBuffer + ret;
+            }
+
+            memcpy( pData + bytesInBuffer, szBuffer, ret );
+            bytesInBuffer += ret;
+
+            // already complete messagae?
+            if ( bytesInBuffer >= sizeof( RDB_MSG_HDR_t ) )
+            {
+                RDB_MSG_HDR_t* hdr = ( RDB_MSG_HDR_t* ) pData;
+
+                // is this message containing the valid magic number?
+                if ( hdr->magicNo != RDB_MAGIC_NO )
+                {
+                    printf( "message receiving is out of sync; discarding data" );
+                    bytesInBuffer = 0;
+                }
+
+                while ( bytesInBuffer >= ( hdr->headerSize + hdr->dataSize ) )
+                {
+                    unsigned int msgSize = hdr->headerSize + hdr->dataSize;
+                    // Tianshu 20190926
+    //                bool         isImage = true;    // TS: 20190926 false;
+
+
+
+                    // now parse the message
+ //                   parseRDBMessage( ( RDB_MSG_t* ) pData, isImage );
+                    parseRDBMessage( ( RDB_MSG_t* ) pData);
+
+
+                    // remove message from queue
+                    memmove( pData, pData + msgSize, bytesInBuffer - msgSize );
+                    bytesInBuffer -= msgSize;
+                    hdr = ( RDB_MSG_HDR_t* ) pData;
+
+                    if(bytesInBuffer<sizeof(RDB_MSG_ENTRY_HDR_t))
+                    {
+                        break;
+                    }
+
+ //                   bMsgComplete = true;
+                }
+            }
+        }
+
+
+    }
+
+
+    delete szBuffer;
+}
+
+void RDBConn::parseRDBMessage(RDB_MSG_t *msg)
+{
+    if ( !msg )
+        return;
+
+    if ( !msg->hdr.dataSize )
+        return;
+
+    RDB_MSG_ENTRY_HDR_t* entry = ( RDB_MSG_ENTRY_HDR_t* ) ( ( ( char* ) msg ) + msg->hdr.headerSize );
+    uint32_t remainingBytes    = msg->hdr.dataSize;
+
+    while ( remainingBytes )
+    {
+        simTime =  msg->hdr.simTime;
+        simFrame = msg->hdr.frameNo;
+        parseRDBMessageEntry( msg->hdr.simTime, msg->hdr.frameNo, entry );
+
+
+        remainingBytes -= ( entry->headerSize + entry->dataSize );
+
+        if ( remainingBytes )
+            entry = ( RDB_MSG_ENTRY_HDR_t* ) ( ( ( ( char* ) entry ) + entry->headerSize + entry->dataSize ) );
+    }
+}
+
+void RDBConn::parseRDBMessageEntry(const double &simTime, const unsigned int &simFrame, RDB_MSG_ENTRY_HDR_t *entryHdr)
+{
+    if ( !entryHdr )
+        return;
+
+    int noElements = entryHdr->elementSize ? ( entryHdr->dataSize / entryHdr->elementSize ) : 0;
+
+    if ( !noElements )  // some elements require special treatment
+    {
+        switch ( entryHdr->pkgId )
+        {
+        case RDB_PKG_ID_START_OF_FRAME:
+            //                fprintf( stderr, "void parseRDBMessageEntry: got start of frame\n" );
+            break;
+
+        case RDB_PKG_ID_END_OF_FRAME:
+            //                fprintf( stderr, "void parseRDBMessageEntry: got end of frame\n" );
+            break;
+
+        default:
+            return;
+            break;
+        }
+        return;
+    }
+
+//    unsigned char ident   = 6;
+    char*         dataPtr = ( char* ) entryHdr;
+
+    dataPtr += entryHdr->headerSize;
+
+    while ( noElements-- )
+    {
+        iv::rdbitem xrdbitem;
+        int64_t nchronotime = std::chrono::system_clock::now().time_since_epoch().count()/1000;
+        double fchronotime = nchronotime;
+        fchronotime = fchronotime * 0.000001;
+        xrdbitem.mfMsgTime = fchronotime;
+        xrdbitem.simFrame = simFrame;
+        xrdbitem.simTime = simTime;
+        xrdbitem.mnpkgdatasize = entryHdr->elementSize;
+        xrdbitem.pkgid = entryHdr->pkgId;
+        xrdbitem.mpstr_pkgdata = std::shared_ptr<char>(new char[xrdbitem.mnpkgdatasize]);
+        memcpy(xrdbitem.mpstr_pkgdata.get(),dataPtr,xrdbitem.mnpkgdatasize);
+        mmutexdata.lock();
+        while(mvectorrdbitem.size()>rdbitembufsize)
+        {
+            std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" parseRDBMessageEntry buffer full,erase one. "<<std::endl;
+            mvectorrdbitem.erase(mvectorrdbitem.begin());
+        }
+        mvectorrdbitem.push_back(xrdbitem);
+        mmutexdata.unlock();
+        mcv.notify_all();
+
+        dataPtr += entryHdr->elementSize;
+    }
+
+}
+
+int RDBConn::ConsumeBuf(iv::rdbitem &xrdbitem, int nwaitms)
+{
+    int nrtn = 0;
+    if(mvectorrdbitem.size()>0)
+    {
+        mmutexdata.lock();
+        if(mvectorrdbitem.size()>0)
+        {
+            xrdbitem = mvectorrdbitem[0];
+            nrtn = 1;
+            mvectorrdbitem.erase(mvectorrdbitem.begin());
+        }
+        else
+        {
+            nrtn = 0;
+        }
+        mmutexdata.unlock();
+        return nrtn;
+    }
+
+    std::unique_lock<std::mutex> lk(mmutexcv);
+    if(mcv.wait_for(lk,std::chrono::milliseconds(nwaitms)) == std::cv_status::timeout)
+    {
+        lk.unlock();
+    }
+    else
+    {
+        lk.unlock();
+    }
+    mmutexdata.lock();
+    if(mvectorrdbitem.size()>0)
+    {
+        xrdbitem = mvectorrdbitem[0];
+        nrtn = 1;
+        mvectorrdbitem.erase(mvectorrdbitem.begin());
+    }
+    else
+    {
+        nrtn = 0;
+    }
+    mmutexdata.unlock();
+    return nrtn;
+}

src/ros/catkin/src/vtd_ros/src/rdbconn.h

@@ -0,0 +1,77 @@
+#ifndef RDBCONN_H
+#define RDBCONN_H
+
+#include <sstream>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <netinet/in.h>
+#include <netinet/tcp.h>
+#include <arpa/inet.h>
+#include <netdb.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+
+#include "RDBHandler.hh"
+
+#include <thread>
+#include <vector>
+#include <mutex>
+#include <condition_variable>
+
+namespace iv {
+struct rdbitem
+{
+    double mfMsgTime; //when recv this meesage,second
+    double simTime;
+    unsigned int simFrame;
+    uint16_t  pkgid;
+    std::shared_ptr<char> mpstr_pkgdata;
+    int mnpkgdatasize;
+};
+
+}
+
+class RDBConn
+{
+public:
+    RDBConn(const char * strserip,int port);
+    ~RDBConn();
+
+private:
+    char mstrserverip[256];
+    int mnserverport;
+
+    std::thread * mpthreadconn;
+    bool mbthreadconn = true;
+
+    std::mutex mmutexdata;
+    std::mutex mmutexcv;
+    std::condition_variable mcv;
+    std::vector<iv::rdbitem> mvectorrdbitem;
+    const  unsigned int rdbitembufsize = 10000;
+
+private:
+    void threadconn(char * strserip,int nport);
+
+    void threadsend(int nsock);
+
+private:
+    void parseRDBMessage( RDB_MSG_t* msg);
+    void parseRDBMessageEntry( const double & simTime, const unsigned int & simFrame, RDB_MSG_ENTRY_HDR_t* entryHdr );
+
+public:
+    int ConsumeBuf(iv::rdbitem & xrdbitem,int nwaitms = 0);
+
+public:
+    RDB_OBJECT_STATE_t sOwnObjectState;
+    bool mbEgoUpdate = false;
+    double simTime;
+    unsigned int simFrame;
+};
+
+#endif // RDBCONN_H