Commit 385e66fd authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Added tranformation from opendrive to map

parent dffea66a
## Description
This node provides a global localization based on landmarks. On the ADC competition, these landmarks are [ar_track_alvar](http://wiki.ros.org/ar_track_alvar) tags. This node takes the filtered ar tag detections of both cameras and optimizes a landmark based slam problem to get the robot position. A landmarks based slam problem is the problem of optimize a window of robot poses with a map of landmarks. It uses [Ceres Solver](http://ceres-solver.org/index.html) to make the optimization.
This node provides a global localization based on landmarks. On the ADC competition, these landmarks are [ar_track_alvar](http://wiki.ros.org/ar_track_alvar) tags. This node takes the filtered ar tag detections of both cameras and optimizes a landmark based slam problem to get the robot position. A landmarks based slam problem is the problem of optimize a number of robot poses with a map of landmarks. It uses [Ceres Solver](http://ceres-solver.org/index.html) to make the optimization.
The following image is a skecth of the input and output of the node.
......@@ -28,13 +28,13 @@ On [parameters_adjust.md](./doc/parameters_adjust.md) there is an small explanat
- /**tf** (tf/tfMessage): Tf to listen the transforms between odom an robot's base link and between the robot's base link and the sensors.
- ~**front_features** (iri_adc_msgs/feature_array.msg): Incoming detections from the front camera.
- ~**rear_features** (iri_adc_msgs/feature_array.msg): Incoming detections from the rear camera.
### Parameters
#####General
- ~**rate** (Double; default: 10.0; min: 0.1; max: 1000) The main node thread loop rate in Hz.
- ~**global_frame** (String; default: map) Global frame id.
- ~**odom_frame** (String; default: odom) Odometry frame id.
- ~**base_link_frame** (String; default: base_link) Robot's base link frame id.
- ~**opendriveframe** (String; default: opendrive) Opendrive frame id.
- ~**tf_timeout** (Double; default: 0.2; min: 0.1; max: 2.0) Timeout to find a transform.
- ~**old_feature_timeout** (Double; default: 0.5; min: 0.1; max: 2.0) Timeout to set a features source as old.
- ~**amcl_pose_estimated_sigma** (Double; default: 1.0; min: 0.1; max: 10.0) AMCL pose sigma when using AMCL localization.
......@@ -59,6 +59,8 @@ On [parameters_adjust.md](./doc/parameters_adjust.md) there is an small explanat
- ~**update_problem_features_detected** (Integer; default: 3; min: 1; max: 10) Update problem when a lot of features are detected.
- ~**wait_feature_detected_timeout** (Double; default: 0.2; min: 0.01; max: 0.5) Timeout on a update problem event to wait for a feature detection.
- ~**landmarks_map_file** (String; default: landmarks.txt) Landmarks map txt file path.
- ~**load_landmarks_from_xodr** (Boolean; default: False) Boolean to load landmarks from a xodr file.
- ~**landmarks_xodr_scale** (Double; default: 1.0; min: 0.0) Landmarks xodr scale factor.
- ~**problem_frame_window** (Integer; default: 120; min: 50; max: 1000) Max number of frames to add.
##### Flags
- ~**publish_visualization** (Boolean; default: False) Boolean to publish visualization markers.
......@@ -112,6 +114,7 @@ This node provides a basic launch file named **node.launch** intended to be incl
- *launch_prefix*: Node's launch prefix.
- *config_file*: Path to the configuration file.
- *landmarks_map_file*: Landmarks txt map file.
- *load_landmarks_from_xodr*: Flag to know if landmars are loaded from a xodr file.
- *initial_pose_x*: Initial robot x position.
- *initial_pose_y*: Initial robot y position.
- *initial_pose_yaw*: Initial robot orientation.
......
......@@ -47,6 +47,7 @@ gen.add("rate", double_t, 0, "Main l
gen.add("global_frame", str_t, 0, "Global frame", "map")
gen.add("odom_frame", str_t, 0, "Odom frame", "odom")
gen.add("base_link_frame", str_t, 0, "Robot base_link frame", "base_link")
gen.add("opendrive_frame", str_t, 0, "Opendrive frame", "opendrive")
gen.add("tf_timeout", double_t, 0, "Timeout to find a transform", 0.2, 0.1, 2.0)
gen.add("old_feature_timeout", double_t, 0, "Timeout to set a features source as old", 0.5, 0.1, 2.0)
gen.add("amcl_pose_estimated_sigma", double_t, 0, "AMCL pose sigma when using AMCL localization", 1, 0.1, 10)
......
......@@ -161,6 +161,13 @@ class AdcLandmarksSlamSolverAlgorithm
*/
void add_landmark(double _landmark_key, const LandmarkInfo& _landmark);
/**
* \brief Function to apply a transform to landmarks.
*
* \param _tf_map_opendrive The transform to be applied.
*/
void transform_landmarks(geometry_msgs::TransformStamped& _tf_map_opendrive);
/**
* \brief Function to delete all landmarks.
*/
......
......@@ -123,6 +123,8 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
bool last_frame_min_detections_; ///< Flag to know that last frame was because a lot of front_features seen.
bool must_transform_landmarks_;///< Flag to know if landmarks must be transformed from opendrive frame to global frame.
// [publisher attributes]
ros::Publisher frame_data_publisher_;
visualization_msgs::MarkerArray frame_data_MarkerArray_msg_;
......
......@@ -156,6 +156,32 @@ bool AdcLandmarksSlamSolverAlgorithm::load_from_xodr(const std::string& _landmar
return true;
}
void AdcLandmarksSlamSolverAlgorithm::transform_landmarks(geometry_msgs::TransformStamped& _tf_map_opendrive)
{
for (std::map<double, LandmarkInfo>::iterator it = this->landmarks_.begin(); it != this->landmarks_.end(); ++it)
{
geometry_msgs::PoseStamped land, transformed_land;
land.header.stamp = _tf_map_opendrive.header.stamp;
land.header.frame_id = _tf_map_opendrive.child_frame_id;//global_frame
land.pose.position.z = 0.0;
land.pose.position.x = it->second.pose(0);
land.pose.position.y = it->second.pose(1);
tf2::Quaternion q;
q.setRPY(0.0, 0.0, it->second.pose(2));
land.pose.orientation = tf2::toMsg(q);
tf2::doTransform(land, transformed_land, _tf_map_opendrive);
double yaw, pitch, roll;
q = tf2::Quaternion(transformed_land.pose.orientation.x, transformed_land.pose.orientation.y, transformed_land.pose.orientation.z, transformed_land.pose.orientation.w);
tf2::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
it->second.pose(0) = transformed_land.pose.position.x;
it->second.pose(1) = transformed_land.pose.position.y;
it->second.pose(2) = yaw;
}
}
bool AdcLandmarksSlamSolverAlgorithm::write_landmarks_file(void)
{
std::string file_name;
......
......@@ -89,8 +89,13 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
{
if(!this->private_node_handle_.getParam("landmarks_xodr_scale", this->config_.landmarks_xodr_scale))
{
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'landmarks_xodr_scale' not found. Setting it to false.");
this->config_.landmarks_xodr_scale = false;
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'landmarks_xodr_scale' not found. Setting it to 1.0.");
this->config_.landmarks_xodr_scale = 1.0;
}
if(!this->private_node_handle_.getParam("opendrive_frame", this->config_.opendrive_frame))
{
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'opendrive_frame' not found. Setting it to \"opendrive\".");
this->config_.opendrive_frame = "opendrive";
}
}
load_landmarks();
......@@ -571,6 +576,22 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks, bool
geometry_msgs::TransformStamped tf_map_sensor_msg, tf_sensor_map_msg;
geometry_msgs::PoseStamped local_feature, global_feature;
if (this->must_transform_landmarks_)
{
geometry_msgs::TransformStamped tf_map_opendrive_msg;
tf_map_opendrive_msg.header.frame_id = this->config_.global_frame;
tf_map_opendrive_msg.header.stamp = ros::Time::now();
tf_map_opendrive_msg.child_frame_id = this->config_.opendrive_frame;
if (!this->tf2_buffer.canTransform(tf_map_opendrive_msg.header.frame_id, tf_map_opendrive_msg.child_frame_id, tf_map_opendrive_msg.header.stamp, ros::Duration(this->config_.tf_timeout)))
{
ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode:check_landmarks -> Can't transform from " << tf_map_opendrive_msg.header.frame_id << " to " << tf_map_opendrive_msg.child_frame_id << " at " << tf_map_opendrive_msg.header.stamp);
return false;
}
tf_map_opendrive_msg = this->tf2_buffer.lookupTransform(tf_map_opendrive_msg.header.frame_id, tf_map_opendrive_msg.child_frame_id, tf_map_opendrive_msg.header.stamp);
this->alg_.transform_landmarks(tf_map_opendrive_msg);
this->must_transform_landmarks_ = false;
}
if ((_front && this->front_features_frame_ == "") || (!_front && this->rear_features_frame_ == ""))
{
ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode::check_landmarks -> " << (_front? "front ": "rear ") << "features frame id empty; AR tag filter not initialized.");
......@@ -912,6 +933,10 @@ 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, this->config_.load_landmarks_from_xodr, this->config_.landmarks_xodr_scale))
ROS_ERROR_STREAM("AdcLandmarksSlamSolverAlgNode::load_landmarks -> File" << this->config_.landmarks_map_file << " can't be opened");
if (this->config_.load_landmarks_from_xodr)
this->must_transform_landmarks_ = true;
else
this->must_transform_landmarks_ = false;
}
void AdcLandmarksSlamSolverAlgNode::merge_features()
......
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