Commit 6cf69679 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Added xodr parser to load landmarks

parent 50ab0b01
cmake_minimum_required(VERSION 2.8.3)
project(iri_adc_landmarks_slam_solver)
ADD_DEFINITIONS(-D_HAVE_XSD)
## Find catkin macros and libraries
find_package(catkin REQUIRED)
# ********************************************************************
......@@ -12,6 +14,9 @@ find_package(catkin REQUIRED COMPONENTS iri_base_algorithm visualization_msgs ge
# find_package(Boost REQUIRED COMPONENTS system)
find_package(Eigen3 REQUIRED)
find_package(Ceres REQUIRED)
find_package(iriutils REQUIRED)
find_package(autonomous_driving_tools REQUIRED)
find_package(opendrive_road_map REQUIRED)
# ********************************************************************
# Add system and labrobotica dependencies here
......@@ -80,6 +85,9 @@ include_directories(include)
include_directories(${catkin_INCLUDE_DIRS})
include_directories(${EIGEN3_INCLUDE_DIR})
include_directories(${CERES_INCLUDE_DIRS})
include_directories(${iriutils_INCLUDE_DIRS})
include_directories(${autonomous_driving_tools_INCLUDE_DIRS})
include_directories(${opendrive_road_map_INCLUDE_DIRS})
# include_directories(${<dependency>_INCLUDE_DIRS})
## Declare a cpp library
......@@ -93,6 +101,9 @@ add_executable(${PROJECT_NAME} src/adc_landmarks_slam_solver_alg.cpp src/adc_lan
# ********************************************************************
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${CERES_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${autonomous_driving_tools_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${opendrive_road_map_LIBRARIES})
# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARIES})
# ********************************************************************
......
......@@ -70,6 +70,7 @@ ceres.add("update_problem_angle", double_t, 0, "An
ceres.add("update_problem_features_detected", int_t, 0, "Update problem when a lot of features are detected", 3, 1, 10)
ceres.add("wait_feature_detected_timeout", double_t, 0, "Timeout on a update problem event to wait for a feature detection", 0.2, 0.01, 0.5)
ceres.add("landmarks_map_file", str_t, 0, "Input txt file path", "landmarks.txt")
ceres.add("load_landmarks_from_xodr", bool_t, 0, "If the landmarks map file is an xodr", False)
ceres.add("problem_frame_window", int_t, 0, "Max number of frames to add", 30, -1, 500)
flags.add("publish_visualization", bool_t, 0, "Boolean to publish visualization markers", False)
......
......@@ -42,6 +42,17 @@
#include "landmark_residual_from_base_2D.h"
#include "sensor_pose_residual_2D.h"
#include <cmath>
#define SIGN_MARKER_ID 0
#define SEMAPHORE_TYPE "1000001"
#define SEMAPHORE_MARKER_ID 1
#define LOC_SIGN_TRIANGLE_TYPE ""
#define LOC_SIGN_TRIANGLE_MARKER_ID 2
#define LOC_SIGN_ID_OFFSET 100
#define LOC_SIGN_TRIANGLE_WIDTH 0.074
#define LOC_SIGN_TRIANGLE_ANGLE M_PI/2
//include adc_landmarks_slam_solver_alg main library
/**
......@@ -117,11 +128,24 @@ class AdcLandmarksSlamSolverAlgorithm
*
* \param _landmarks_file The route to the landmarks file to be loaded.
*
* \param _fixed If landamrks are fixed.
* \param _fixed If landmarks are fixed.
*
* \param _from_xodr If landamrks are loaded form an xodr file.
*
* \return If success.
*/
bool load_landmarks(const std::string& _landmarks_file, bool _fixed, bool _from_xodr);
/**
* \brief Function to load landmarks map from a xodr file.
*
* \param _landmarks_file The route to the landmarks file to be loaded.
*
* \param _fixed If landmarks are fixed.
*
* \return If success.
*/
bool load_landmarks(const std::string& _landmarks_file, bool _fixed);
bool load_from_xodr(const std::string& _landmarks_file, bool _fixed);
/**
* \brief Function to add a landmark.
......
#include "adc_landmarks_slam_solver_alg.h"
#include "opendrive_road.h"
#include "exceptions.h"
#include "opendrive_road_segment.h"
#include "opendrive_signal.h"
AdcLandmarksSlamSolverAlgorithm::AdcLandmarksSlamSolverAlgorithm(void)
{
......@@ -51,8 +55,10 @@ void AdcLandmarksSlamSolverAlgorithm::config_update(Config& config, uint32_t lev
}
// AdcLandmarksSlamSolverAlgorithm Public API
bool AdcLandmarksSlamSolverAlgorithm::load_landmarks(const std::string& _landmarks_file, bool _fixed)
bool AdcLandmarksSlamSolverAlgorithm::load_landmarks(const std::string& _landmarks_file, bool _fixed, bool _from_xodr)
{
if (_from_xodr)
return load_from_xodr(_landmarks_file, _fixed);
std::ifstream landmarks_file;
landmarks_file.open(_landmarks_file.c_str());
if (!landmarks_file.good())
......@@ -79,6 +85,76 @@ bool AdcLandmarksSlamSolverAlgorithm::load_landmarks(const std::string& _landmar
return true;
}
bool AdcLandmarksSlamSolverAlgorithm::load_from_xodr(const std::string& _landmarks_file, bool _fixed)
{
COpendriveRoad road;
std::string type, subtype;
TOpendriveWorldPose world;
try{
road.load(_landmarks_file);
std::map<double, LandmarkInfo>().swap(this->landmarks_);
for(unsigned int i=0;i<road.get_num_segments();i++)
{
const COpendriveRoadSegment &segment=road.get_segment(i);
for(unsigned int j=0;j<segment.get_num_signals();j++)
{
LandmarkInfo l;
l.detected = false;
l.source_frame_id = "";
l.fixed = _fixed;
const COpendriveSignal &sign = segment.get_signal(j);
sign.get_type(type,subtype);
if (type == LOC_SIGN_TRIANGLE_TYPE)
{
double loc_x, loc_y, loc_yaw, id;
l.type = LOC_SIGN_TRIANGLE_MARKER_ID;
world = sign.get_world_pose();
loc_x = -LOC_SIGN_TRIANGLE_WIDTH*std::sin(LOC_SIGN_TRIANGLE_ANGLE/2)/2;
loc_y = -LOC_SIGN_TRIANGLE_WIDTH*std::cos(LOC_SIGN_TRIANGLE_ANGLE/2)/2;
loc_yaw = M_PI + LOC_SIGN_TRIANGLE_ANGLE/2;
l.pose(0) = world.x + loc_x*std::cos(world.heading) - loc_y*std::sin(world.heading);
l.pose(1) = world.y + loc_x*std::sin(world.heading) + loc_y*std::cos(world.heading);
l.pose(2) = loc_yaw + world.heading;
id = LOC_SIGN_ID_OFFSET + (sign.get_id()%LOC_SIGN_ID_OFFSET)*2-1;
this->landmarks_[id] = l;
loc_x = LOC_SIGN_TRIANGLE_WIDTH*std::sin(LOC_SIGN_TRIANGLE_ANGLE/2)/2;
loc_y = -LOC_SIGN_TRIANGLE_WIDTH*std::cos(LOC_SIGN_TRIANGLE_ANGLE/2)/2;
loc_yaw = -LOC_SIGN_TRIANGLE_ANGLE/2;
l.pose(0) = world.x + loc_x*std::cos(world.heading) - loc_y*std::sin(world.heading);
l.pose(1) = world.y + loc_x*std::sin(world.heading) + loc_y*std::cos(world.heading);
l.pose(2) = loc_yaw + world.heading;
id = LOC_SIGN_ID_OFFSET + (sign.get_id()%LOC_SIGN_ID_OFFSET)*2;
this->landmarks_[id] = l;
}
else
{
if(type == SEMAPHORE_TYPE)
l.type = SEMAPHORE_MARKER_ID;
else
l.type = SIGN_MARKER_ID;
world = sign.get_world_pose();
l.pose(0) = world.x;
l.pose(1) = world.y;
l.pose(2) = world.heading;
this->landmarks_[(double)sign.get_id()] = l;
}
}
}
}catch (CException &e){
ROS_ERROR_STREAM("AdcLandmarksSlamSolverAlgorithm::load_from_xodr -> [Exception caught] : " << e.what());
return false;
}
return true;
}
bool AdcLandmarksSlamSolverAlgorithm::write_landmarks_file(void)
{
std::string file_name;
......@@ -188,6 +264,10 @@ double AdcLandmarksSlamSolverAlgorithm::is_a_mapped_landmark(Eigen::Vector3d _fe
double dist = mahalanobis_distance(feature_pos, _feature_r, _feature_th, landmark_pos);
// std::cout << "check dist " << dist << std::endl;
//pitch instead of yaw beacuse is a camera
while (pitch >= M_PI)
pitch -= 2*M_PI;
while (pitch < -M_PI)
pitch += 2*M_PI;
if (dist <= this->config_.landmark_mahalanobis_dist && ((!this->config_.landmarks_filter_orientation_en) || (std::fabs(pitch -_feature_pose(2)) < this->config_.landmarks_orientation_th)))
{
Eigen::Vector2d d = landmark_pos - feature_pos;
......
......@@ -78,6 +78,12 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'landmarks_map_file' not found. Setting it to \"landmarks.txt\".");
this->config_.landmarks_map_file = "landmarks.txt";
}
if(!this->private_node_handle_.getParam("load_landmarks_from_xodr", this->config_.load_landmarks_from_xodr))
{
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'load_landmarks_from_xodr' not found. Setting it to false.");
this->config_.load_landmarks_from_xodr = false;
}
load_landmarks();
}
if (!this->config_.amcl_localization)
......@@ -649,6 +655,10 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks, bool
q = tf2::Quaternion(local_feature.pose.orientation.x, local_feature.pose.orientation.y, local_feature.pose.orientation.z, local_feature.pose.orientation.w);
tf2::Matrix3x3(q).getEulerYPR(yaw_local, pitch, roll);
// feature_pose_local << local_feature.pose.position.x, local_feature.pose.position.y, yaw_local;
while (pitch >= M_PI)
pitch -= 2*M_PI;
while (pitch < -M_PI)
pitch += 2*M_PI;
feature_pose_local << local_feature.pose.position.z, local_feature.pose.position.x, pitch;//is a camera. axis rotated
(_front? this->front_features_: this->rear_features_)[i].landmark_key = this->alg_.is_a_mapped_landmark(feature_pose_local, (_front? this->front_features_: this->rear_features_)[i].r, (_front? this->front_features_: this->rear_features_)[i].th, (_front? this->front_features_: this->rear_features_)[i].type, (_front? this->front_features_frame_: this->rear_features_frame_), tf_sensor_map_msg);
......@@ -733,6 +743,10 @@ bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature, boo
double dist = this->alg_.mahalanobis_distance(feature_pos, _feature.r, _feature.th, landmark_pos);
// pitch instead of yaw beacuse is a camera
while (pitch >= M_PI)
pitch -= 2*M_PI;
while (pitch < -M_PI)
pitch += 2*M_PI;
if (dist <= this->config_.landmark_mahalanobis_dist && ((!this->config_.landmarks_filter_orientation_en) || (std::fabs(pitch -_feature_pose(2)) < this->config_.landmarks_orientation_th)))
{
Eigen::Vector2d d = landmark_pos - feature_pos;
......@@ -887,7 +901,7 @@ bool AdcLandmarksSlamSolverAlgNode::get_tf_map_odom(ros::Time _t, Eigen::Vector3
void AdcLandmarksSlamSolverAlgNode::load_landmarks(void)
{
ROS_INFO_STREAM("AdcLandmarksSlamSolverAlgNode::load_landmarks -> Loading from file " << this->config_.landmarks_map_file);
if (!this->alg_.load_landmarks(this->config_.landmarks_map_file, this->config_.landmarks_pos_fixed))
if (!this->alg_.load_landmarks(this->config_.landmarks_map_file, this->config_.landmarks_pos_fixed, this->config_.load_landmarks_from_xodr))
ROS_ERROR_STREAM("AdcLandmarksSlamSolverAlgNode::load_landmarks -> File" << this->config_.landmarks_map_file << " can't be opened");
}
......@@ -934,7 +948,7 @@ void AdcLandmarksSlamSolverAlgNode::publish_landmarks_markers()
this->landmarks_MarkerArray_msg_.markers[i].id = i;
this->landmarks_MarkerArray_msg_.markers[i].ns = "landmarks";
// this->landmarks_MarkerArray_msg_.markers[i].type = visualization_msgs::Marker::TEXT_VIEW_FACING;
this->landmarks_MarkerArray_msg_.markers[i].type = visualization_msgs::Marker::CUBE;
this->landmarks_MarkerArray_msg_.markers[i].type = visualization_msgs::Marker::ARROW;
this->landmarks_MarkerArray_msg_.markers[i].action = visualization_msgs::Marker::ADD;
// this->landmarks_MarkerArray_msg_.markers[i].lifetime = ros::Duration(0.1);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment