From 3b79b807aed25c34915756c12965a2f1a0312c01 Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Fri, 10 Nov 2023 12:57:37 +0100
Subject: [PATCH] Add set of utm_zone parameter
---
cfg/OpendriveGlobalPlanner.cfg | 1 +
.../opendrive_planner.h | 2 +-
src/opendrive_planner.cpp | 22 ++++++++++++++-----
3 files changed, 19 insertions(+), 6 deletions(-)
diff --git a/cfg/OpendriveGlobalPlanner.cfg b/cfg/OpendriveGlobalPlanner.cfg
index b04891a..fdc8420 100755
--- a/cfg/OpendriveGlobalPlanner.cfg
+++ b/cfg/OpendriveGlobalPlanner.cfg
@@ -18,6 +18,7 @@ gen.add("end_angle_tol", double_t, 0, "Angle tolerance to find start and end p
gen.add("end_dist_tol", double_t, 0, "Distance tolerance to find start and end positions on the road map", 3.0, 0.5, 10.0)
gen.add("multi_hyp", bool_t, 0, "Use multi hypothesis path search", False)
gen.add("resolution", double_t, 0, "Resolution of the generated path", 0.1, 0.01, 1.0)
+gen.add("utm_zone", int_t, 0, "UTM zone for geodesy lat/lon to UTM conversions", 31, 1, 60)
gen.add("cost_type", int_t, 0, "Cost type", 0, 0, 1, edit_method=enum_cost_type)
gen.add("debug", bool_t, 0, "Show debug messages", False)
diff --git a/include/iri_opendrive_global_planner/opendrive_planner.h b/include/iri_opendrive_global_planner/opendrive_planner.h
index 6779c90..0eeec65 100644
--- a/include/iri_opendrive_global_planner/opendrive_planner.h
+++ b/include/iri_opendrive_global_planner/opendrive_planner.h
@@ -128,7 +128,7 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
bool get_opendrive_map(iri_nav_module::get_opendrive_map::Request &req,iri_nav_module::get_opendrive_map::Response &res);
ros::ServiceServer opendrive_nodes_service;
bool get_opendrive_nodes(iri_nav_module::get_opendrive_nodes::Request &req,iri_nav_module::get_opendrive_nodes::Response &res);
- void create_opendrive_map(std::string &filename,double resolution);
+ void create_opendrive_map(std::string &filename,double resolution,int utm_zone);
nav_msgs::OccupancyGrid full_path_;
CMutex path_access;
diff --git a/src/opendrive_planner.cpp b/src/opendrive_planner.cpp
index 5c16cde..daa8b8d 100644
--- a/src/opendrive_planner.cpp
+++ b/src/opendrive_planner.cpp
@@ -88,23 +88,33 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
std::string opendrive_file;
double resolution;
int cost_type;
+ int utm_zone;
if(private_nh.getParam("resolution", resolution))
this->roadmap.set_resolution(resolution);
else
ROS_ERROR("OpendriveGlobalPlanner: Map resolution not defined");
+
+ if(private_nh.getParam("utm_zone", utm_zone))
+ {
+ //ROS_INFO("utm_zone=%d",utm_zone);
+ this->roadmap.set_utm_zone(utm_zone);
+ }
+ else
+ ROS_ERROR("OpendriveGlobalPlanner: utm zone not defined");
+
if(private_nh.getParam("opendrive_file", opendrive_file))
{
if(opendrive_file!="")
{
try{
if(opendrive_file.find("xodr")!=std::string::npos)
- roadmap.load_opendrive(opendrive_file);
+ this->roadmap.load_opendrive(opendrive_file);
else if(opendrive_file.find("osm")!=std::string::npos)
- roadmap.load_osm(opendrive_file);
+ this->roadmap.load_osm(opendrive_file);
else
ROS_ERROR("Unknown file type");
- this->create_opendrive_map(opendrive_file,resolution);
+ this->create_opendrive_map(opendrive_file,resolution, utm_zone);
}catch(CException &e){
ROS_ERROR_STREAM(e.what());
}
@@ -157,13 +167,14 @@ void OpendriveGlobalPlanner::map_connect_callback(const ros::SingleSubscriberPub
this->path_access.exit();
}
-void OpendriveGlobalPlanner::create_opendrive_map(std::string &filename,double resolution)
+void OpendriveGlobalPlanner::create_opendrive_map(std::string &filename,double resolution, int utm_zone)
{
std::vector<double> x,partial_x,y,partial_y,heading;
double max_x=std::numeric_limits<double>::min(),min_x=std::numeric_limits<double>::max(),max_y=std::numeric_limits<double>::min(),min_y=std::numeric_limits<double>::max();
CRoadMap road;
this->path_access.enter();
+ road.set_utm_zone(utm_zone);
if(filename.find("xodr")!=std::string::npos)
road.load_opendrive(filename);
else if(filename.find("osm")!=std::string::npos)
@@ -171,6 +182,7 @@ void OpendriveGlobalPlanner::create_opendrive_map(std::string &filename,double r
else
return;
road.set_resolution(resolution);
+
this->full_path_.header.frame_id = this->opendrive_frame_id_;
this->full_path_.header.stamp = ros::Time::now();
this->full_path_.info.map_load_time = ros::Time::now();
@@ -242,7 +254,7 @@ void OpendriveGlobalPlanner::reconfigureCB(iri_opendrive_global_planner::Opendri
roadmap.load_osm(new_config.opendrive_file);
else
ROS_ERROR("Unknown file type");
- this->create_opendrive_map(new_config.opendrive_file,new_config.resolution);
+ this->create_opendrive_map(new_config.opendrive_file,new_config.resolution, new_config.utm_zone);
}
this->roadmap.set_cost_type((cost_t)new_config.cost_type);
this->config=new_config;
--
GitLab