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
27236f60
Commit
27236f60
authored
Sep 13, 2021
by
Sergi Hernandez
Browse files
Merge branch 'master' into 'development'
Master See merge request
!1
parents
192b9ec5
faf5eb47
Changes
3
Show whitespace changes
Inline
Side-by-side
cfg/OpendriveGlobalPlanner.cfg
View file @
27236f60
...
...
@@ -13,7 +13,7 @@ gen.const("time", int_t, 1, "Use distance and speed to compute costs"),
gen
.
add
(
"opendrive_file"
,
str_t
,
0
,
"Opendrive map file"
,
""
)
gen
.
add
(
"opendrive_frame"
,
str_t
,
0
,
"Opendrive frame ID"
,
""
)
gen
.
add
(
"angle_tol"
,
double_t
,
0
,
"Angle tolerance to find start and end positions on the road map"
,
0.5
,
0.1
,
1
.5707
)
gen
.
add
(
"angle_tol"
,
double_t
,
0
,
"Angle tolerance to find start and end positions on the road map"
,
0.5
,
0.1
,
1
0.0
)
gen
.
add
(
"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
)
...
...
include/iri_opendrive_global_planner/opendrive_planner.h
View file @
27236f60
...
...
@@ -49,6 +49,7 @@
#include <tf2_ros/transform_listener.h>
#include <ros/single_subscriber_publisher.h>
#include <iri_adc_msgs/get_opendrive_map.h>
#include <iri_adc_msgs/get_opendrive_nodes.h>
#include "opendrive_road_map.h"
...
...
@@ -124,12 +125,16 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
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
);
ros
::
ServiceServer
opendrive_nodes_service
;
bool
get_opendrive_nodes
(
iri_adc_msgs
::
get_opendrive_nodes
::
Request
&
req
,
iri_adc_msgs
::
get_opendrive_nodes
::
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_
;
COpendriveRoadMap
roadmap
;
std
::
vector
<
unsigned
int
>
best_path
;
ros
::
Time
best_path_stamp
;
double
angle_tol
;
double
dist_tol
;
bool
multi_hyp
;
...
...
src/opendrive_planner.cpp
View file @
27236f60
...
...
@@ -116,6 +116,8 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
this
->
opendrive_map_service
=
private_nh
.
advertiseService
(
"get_opendrive_map"
,
&
OpendriveGlobalPlanner
::
get_opendrive_map
,
this
);
this
->
opendrive_nodes_service
=
private_nh
.
advertiseService
(
"get_opendrive_nodes"
,
&
OpendriveGlobalPlanner
::
get_opendrive_nodes
,
this
);
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
);
dsrv_
->
setCallback
(
cb
);
...
...
@@ -187,6 +189,18 @@ bool OpendriveGlobalPlanner::get_opendrive_map(iri_adc_msgs::get_opendrive_map::
return
true
;
}
bool
OpendriveGlobalPlanner
::
get_opendrive_nodes
(
iri_adc_msgs
::
get_opendrive_nodes
::
Request
&
req
,
iri_adc_msgs
::
get_opendrive_nodes
::
Response
&
res
)
{
res
.
opendrive_nodes
.
header
.
frame_id
=
this
->
opendrive_frame_id_
;
res
.
opendrive_nodes
.
header
.
stamp
=
this
->
best_path_stamp
;
res
.
opendrive_nodes
.
nodes
.
resize
(
this
->
best_path
.
size
());
for
(
unsigned
int
i
=
0
;
i
<
this
->
best_path
.
size
();
i
++
)
res
.
opendrive_nodes
.
nodes
[
i
]
=
this
->
best_path
[
i
];
res
.
opendrive_nodes
.
nodes
=
this
->
best_path
;
return
true
;
}
void
OpendriveGlobalPlanner
::
reconfigureCB
(
iri_opendrive_global_planner
::
OpendriveGlobalPlannerConfig
&
new_config
,
uint32_t
level
)
{
try
{
...
...
@@ -263,7 +277,6 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
{
double
yaw
,
best_cost
;
unsigned
int
best_path_index
;
std
::
vector
<
unsigned
int
>
best_path
;
std
::
vector
<
double
>
x
,
y
,
heading
;
std
::
vector
<
std
::
vector
<
unsigned
int
>
>
paths
;
std
::
vector
<
double
>
costs
;
...
...
@@ -354,13 +367,14 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
std
::
cout
<<
paths
[
i
][
j
]
<<
","
;
std
::
cout
<<
std
::
endl
;
}
best_path_index
=
this
->
roadmap
.
get_best_path
(
best_cost
,
best_path
);
best_path_index
=
this
->
roadmap
.
get_best_path
(
best_cost
,
this
->
best_path
);
if
(
best_path_index
==-
1
)
return
false
;
std
::
cout
<<
"best path index: "
<<
best_path_index
<<
std
::
endl
;
for
(
unsigned
int
i
=
0
;
i
<
best_path
.
size
();
i
++
)
std
::
cout
<<
best_path
[
i
]
<<
","
;
for
(
unsigned
int
i
=
0
;
i
<
this
->
best_path
.
size
();
i
++
)
std
::
cout
<<
this
->
best_path
[
i
]
<<
","
;
std
::
cout
<<
std
::
endl
;
this
->
best_path_stamp
=
ros
::
Time
::
now
();
this
->
roadmap
.
get_trajectory
(
best_path_index
,
x
,
y
,
heading
);
plan
.
resize
(
x
.
size
());
try
{
...
...
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