Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
labrobotica
ros
navigation
iri_opendrive_global_planner
Commits
8e08bd5c
Commit
8e08bd5c
authored
May 10, 2021
by
Sergi Hernandez
Browse files
Added a topic (RVIZ) and a service to get the full opendrive map as an occupancy grid.
parent
9aca1907
Changes
4
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
8e08bd5c
...
@@ -14,6 +14,7 @@ find_package(catkin REQUIRED
...
@@ -14,6 +14,7 @@ find_package(catkin REQUIRED
roscpp
roscpp
tf2_geometry_msgs
tf2_geometry_msgs
tf2_ros
tf2_ros
iri_adc_msgs
)
)
find_package
(
iriutils REQUIRED
)
find_package
(
iriutils REQUIRED
)
...
@@ -36,6 +37,7 @@ catkin_package(
...
@@ -36,6 +37,7 @@ catkin_package(
pluginlib
pluginlib
roscpp
roscpp
tf2_ros
tf2_ros
iri_adc_msgs
)
)
include_directories
(
include_directories
(
...
...
include/iri_opendrive_global_planner/opendrive_planner.h
View file @
8e08bd5c
...
@@ -47,6 +47,8 @@
...
@@ -47,6 +47,8 @@
#include <iri_opendrive_global_planner/OpendriveGlobalPlannerConfig.h>
#include <iri_opendrive_global_planner/OpendriveGlobalPlannerConfig.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_listener.h>
#include <ros/single_subscriber_publisher.h>
#include <iri_adc_msgs/get_opendrive_map.h>
#include "opendrive_road_map.h"
#include "opendrive_road_map.h"
...
@@ -118,6 +120,13 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
...
@@ -118,6 +120,13 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
costmap_2d
::
Costmap2D
*
costmap_
;
costmap_2d
::
Costmap2D
*
costmap_
;
std
::
string
frame_id_
,
opendrive_frame_id_
;
std
::
string
frame_id_
,
opendrive_frame_id_
;
ros
::
Publisher
plan_pub_
;
ros
::
Publisher
plan_pub_
;
ros
::
Publisher
opendrive_map_pub_
;
void
map_connect_callback
(
const
ros
::
SingleSubscriberPublisher
&
subs
);
ros
::
ServiceServer
opendrive_map_service
;
bool
get_opendrive_map
(
iri_adc_msgs
::
get_opendrive_map
::
Request
&
req
,
iri_adc_msgs
::
get_opendrive_map
::
Response
&
res
);
void
create_opendrive_map
(
std
::
string
&
filename
,
double
resolution
,
double
scale_factor
,
double
min_road_length
);
nav_msgs
::
OccupancyGrid
full_path_
;
bool
initialized_
;
bool
initialized_
;
COpendriveRoadMap
roadmap
;
COpendriveRoadMap
roadmap
;
...
...
package.xml
View file @
8e08bd5c
...
@@ -24,6 +24,7 @@
...
@@ -24,6 +24,7 @@
<depend>
pluginlib
</depend>
<depend>
pluginlib
</depend>
<depend>
roscpp
</depend>
<depend>
roscpp
</depend>
<depend>
tf2_ros
</depend>
<depend>
tf2_ros
</depend>
<depend>
iri_adc_msgs
</depend>
<export>
<export>
<nav_core
plugin=
"${prefix}/opendrive_gp_plugin.xml"
/>
<nav_core
plugin=
"${prefix}/opendrive_gp_plugin.xml"
/>
...
...
src/opendrive_planner.cpp
View file @
8e08bd5c
...
@@ -40,6 +40,7 @@
...
@@ -40,6 +40,7 @@
#include <costmap_2d/cost_values.h>
#include <costmap_2d/cost_values.h>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/costmap_2d.h>
#include <tf2/utils.h>
#include <tf2/utils.h>
#include <limits>
#include "exceptions.h"
#include "exceptions.h"
...
@@ -83,18 +84,21 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
...
@@ -83,18 +84,21 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
try
{
try
{
std
::
string
opendrive_file
;
std
::
string
opendrive_file
;
double
value
;
double
resolution
,
scale_factor
,
min_road_length
;
int
cost_type
;
int
cost_type
;
private_nh
.
param
(
"resolution"
,
value
,
0.0
);
private_nh
.
param
(
"resolution"
,
resolution
,
0.0
);
this
->
roadmap
.
set_resolution
(
value
);
this
->
roadmap
.
set_resolution
(
resolution
);
private_nh
.
param
(
"scale_factor"
,
value
,
0.0
);
private_nh
.
param
(
"scale_factor"
,
scale_factor
,
0.0
);
this
->
roadmap
.
set_scale_factor
(
value
);
this
->
roadmap
.
set_scale_factor
(
scale_factor
);
private_nh
.
param
(
"min_road_length"
,
value
,
0.0
);
private_nh
.
param
(
"min_road_length"
,
min_road_length
,
0.0
);
this
->
roadmap
.
set_min_road_length
(
value
);
this
->
roadmap
.
set_min_road_length
(
min_road_length
);
private_nh
.
param
(
"opendrive_file"
,
opendrive_file
,
std
::
string
(
""
));
private_nh
.
param
(
"opendrive_file"
,
opendrive_file
,
std
::
string
(
""
));
if
(
opendrive_file
!=
""
)
if
(
opendrive_file
!=
""
)
{
this
->
roadmap
.
load
(
opendrive_file
);
this
->
roadmap
.
load
(
opendrive_file
);
this
->
create_opendrive_map
(
opendrive_file
,
resolution
,
scale_factor
,
min_road_length
);
}
private_nh
.
param
(
"opendrive_frame"
,
this
->
opendrive_frame_id_
,
std
::
string
(
""
));
private_nh
.
param
(
"opendrive_frame"
,
this
->
opendrive_frame_id_
,
std
::
string
(
""
));
private_nh
.
param
(
"angle_tol"
,
this
->
angle_tol
,
DEFAULT_ANGLE_THRESHOLD
);
private_nh
.
param
(
"angle_tol"
,
this
->
angle_tol
,
DEFAULT_ANGLE_THRESHOLD
);
private_nh
.
param
(
"dist_tol"
,
this
->
dist_tol
,
DEFAULT_DIST_THRESHOLD
);
private_nh
.
param
(
"dist_tol"
,
this
->
dist_tol
,
DEFAULT_DIST_THRESHOLD
);
...
@@ -108,6 +112,10 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
...
@@ -108,6 +112,10 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
plan_pub_
=
private_nh
.
advertise
<
nav_msgs
::
Path
>
(
"plan"
,
1
);
plan_pub_
=
private_nh
.
advertise
<
nav_msgs
::
Path
>
(
"plan"
,
1
);
make_plan_srv_
=
private_nh
.
advertiseService
(
"make_plan"
,
&
OpendriveGlobalPlanner
::
makePlanService
,
this
);
make_plan_srv_
=
private_nh
.
advertiseService
(
"make_plan"
,
&
OpendriveGlobalPlanner
::
makePlanService
,
this
);
this
->
opendrive_map_pub_
=
private_nh
.
advertise
<
nav_msgs
::
OccupancyGrid
>
(
"opendrive_map"
,
1
,
boost
::
bind
(
&
OpendriveGlobalPlanner
::
map_connect_callback
,
this
,
_1
));
this
->
opendrive_map_service
=
private_nh
.
advertiseService
(
"get_opendrive_map"
,
&
OpendriveGlobalPlanner
::
get_opendrive_map
,
this
);
dsrv_
=
new
dynamic_reconfigure
::
Server
<
iri_opendrive_global_planner
::
OpendriveGlobalPlannerConfig
>
(
ros
::
NodeHandle
(
"~/"
+
name
));
dsrv_
=
new
dynamic_reconfigure
::
Server
<
iri_opendrive_global_planner
::
OpendriveGlobalPlannerConfig
>
(
ros
::
NodeHandle
(
"~/"
+
name
));
dynamic_reconfigure
::
Server
<
iri_opendrive_global_planner
::
OpendriveGlobalPlannerConfig
>::
CallbackType
cb
=
boost
::
bind
(
&
OpendriveGlobalPlanner
::
reconfigureCB
,
this
,
_1
,
_2
);
dynamic_reconfigure
::
Server
<
iri_opendrive_global_planner
::
OpendriveGlobalPlannerConfig
>::
CallbackType
cb
=
boost
::
bind
(
&
OpendriveGlobalPlanner
::
reconfigureCB
,
this
,
_1
,
_2
);
dsrv_
->
setCallback
(
cb
);
dsrv_
->
setCallback
(
cb
);
...
@@ -117,20 +125,85 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
...
@@ -117,20 +125,85 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
ROS_WARN
(
"This planner has already been initialized, you can't call it twice, doing nothing"
);
ROS_WARN
(
"This planner has already been initialized, you can't call it twice, doing nothing"
);
}
}
void
OpendriveGlobalPlanner
::
map_connect_callback
(
const
ros
::
SingleSubscriberPublisher
&
subs
)
{
subs
.
publish
(
this
->
full_path_
);
}
void
OpendriveGlobalPlanner
::
create_opendrive_map
(
std
::
string
&
filename
,
double
resolution
,
double
scale_factor
,
double
min_road_length
)
{
std
::
vector
<
double
>
x
,
partial_x
,
y
,
partial_y
;
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
();
COpendriveRoad
road
;
road
.
load
(
filename
);
road
.
set_resolution
(
resolution
);
road
.
set_scale_factor
(
scale_factor
);
road
.
set_min_road_length
(
min_road_length
);
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
();
this
->
full_path_
.
info
.
resolution
=
resolution
;
for
(
unsigned
int
i
=
0
;
i
<
road
.
get_num_nodes
();
i
++
)
{
const
COpendriveRoadNode
&
node
=
road
.
get_node
(
i
);
for
(
unsigned
int
j
=
0
;
j
<
node
.
get_num_links
();
j
++
)
{
const
COpendriveLink
&
link
=
node
.
get_link
(
j
);
link
.
get_trajectory
(
partial_x
,
partial_y
);
x
.
insert
(
x
.
end
(),
partial_x
.
begin
(),
partial_x
.
end
());
y
.
insert
(
y
.
end
(),
partial_y
.
begin
(),
partial_y
.
end
());
for
(
unsigned
int
k
=
0
;
k
<
partial_x
.
size
();
k
++
)
{
if
(
partial_x
[
k
]
>
max_x
)
max_x
=
partial_x
[
k
];
else
if
(
partial_x
[
k
]
<
min_x
)
min_x
=
partial_x
[
k
];
if
(
partial_y
[
k
]
>
max_y
)
max_y
=
partial_y
[
k
];
else
if
(
partial_y
[
k
]
<
min_y
)
min_y
=
partial_y
[
k
];
}
}
}
this
->
full_path_
.
info
.
width
=
(
max_x
-
min_x
)
/
resolution
;
this
->
full_path_
.
info
.
height
=
(
max_y
-
min_y
)
/
resolution
;
this
->
full_path_
.
info
.
origin
.
position
.
x
=
min_x
;
this
->
full_path_
.
info
.
origin
.
position
.
y
=
min_y
;
this
->
full_path_
.
info
.
origin
.
position
.
z
=
0.0
;
this
->
full_path_
.
info
.
origin
.
orientation
.
x
=
0.0
;
this
->
full_path_
.
info
.
origin
.
orientation
.
y
=
0.0
;
this
->
full_path_
.
info
.
origin
.
orientation
.
z
=
0.0
;
this
->
full_path_
.
info
.
origin
.
orientation
.
w
=
1.0
;
this
->
full_path_
.
data
.
resize
(
this
->
full_path_
.
info
.
width
*
this
->
full_path_
.
info
.
height
,
255
);
for
(
unsigned
int
i
=
0
;
i
<
x
.
size
();
i
++
)
this
->
full_path_
.
data
[
this
->
full_path_
.
info
.
width
*
((
unsigned
int
)((
y
[
i
]
-
min_y
)
/
resolution
))
+
((
unsigned
int
)((
x
[
i
]
-
min_x
)
/
resolution
))]
=
0
;
}
bool
OpendriveGlobalPlanner
::
get_opendrive_map
(
iri_adc_msgs
::
get_opendrive_map
::
Request
&
req
,
iri_adc_msgs
::
get_opendrive_map
::
Response
&
res
)
{
res
.
opendrive_map
=
this
->
full_path_
;
return
true
;
}
void
OpendriveGlobalPlanner
::
reconfigureCB
(
iri_opendrive_global_planner
::
OpendriveGlobalPlannerConfig
&
new_config
,
uint32_t
level
)
void
OpendriveGlobalPlanner
::
reconfigureCB
(
iri_opendrive_global_planner
::
OpendriveGlobalPlannerConfig
&
new_config
,
uint32_t
level
)
{
{
try
{
try
{
this
->
roadmap
.
set_resolution
(
new_config
.
resolution
);
this
->
roadmap
.
set_resolution
(
new_config
.
resolution
);
this
->
roadmap
.
set_scale_factor
(
new_config
.
scale_factor
);
this
->
roadmap
.
set_scale_factor
(
new_config
.
scale_factor
);
this
->
roadmap
.
set_min_road_length
(
new_config
.
min_road_length
);
this
->
roadmap
.
set_min_road_length
(
new_config
.
min_road_length
);
if
(
new_config
.
opendrive_file
!=
this
->
config
.
opendrive_file
||
new_config
.
scale_factor
!=
this
->
config
.
scale_factor
||
new_config
.
min_road_length
!=
this
->
config
.
min_road_length
)
this
->
roadmap
.
load
(
new_config
.
opendrive_file
);
this
->
opendrive_frame_id_
=
new_config
.
opendrive_frame
;
this
->
opendrive_frame_id_
=
new_config
.
opendrive_frame
;
this
->
angle_tol
=
new_config
.
angle_tol
;
this
->
angle_tol
=
new_config
.
angle_tol
;
this
->
dist_tol
=
new_config
.
dist_tol
;
this
->
dist_tol
=
new_config
.
dist_tol
;
this
->
multi_hyp
=
new_config
.
multi_hyp
;
this
->
multi_hyp
=
new_config
.
multi_hyp
;
this
->
config
=
new_config
;
if
(
new_config
.
opendrive_file
!=
this
->
config
.
opendrive_file
||
new_config
.
scale_factor
!=
this
->
config
.
scale_factor
||
new_config
.
min_road_length
!=
this
->
config
.
min_road_length
)
{
this
->
roadmap
.
load
(
new_config
.
opendrive_file
);
this
->
create_opendrive_map
(
new_config
.
opendrive_file
,
new_config
.
resolution
,
new_config
.
scale_factor
,
new_config
.
min_road_length
);
}
this
->
roadmap
.
set_cost_type
((
cost_t
)
new_config
.
cost_type
);
this
->
roadmap
.
set_cost_type
((
cost_t
)
new_config
.
cost_type
);
this
->
config
=
new_config
;
}
catch
(
CException
&
e
){
}
catch
(
CException
&
e
){
ROS_ERROR_STREAM
(
e
.
what
());
ROS_ERROR_STREAM
(
e
.
what
());
}
}
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment