Skip to content
GitLab
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
7ca6636c
Commit
7ca6636c
authored
Sep 28, 2020
by
Sergi Hernandez
Browse files
The returned path now includes the heading for each point.
parent
b03fa0ad
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/opendrive_planner.cpp
View file @
7ca6636c
...
...
@@ -39,7 +39,7 @@
#include
<pluginlib/class_list_macros.h>
#include
<costmap_2d/cost_values.h>
#include
<costmap_2d/costmap_2d.h>
#include
<tf
/transform_datatype
s.h>
#include
<tf
2/util
s.h>
#include
"exceptions.h"
...
...
@@ -162,9 +162,10 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
bool
OpendriveGlobalPlanner
::
makePlan
(
const
geometry_msgs
::
PoseStamped
&
start
,
const
geometry_msgs
::
PoseStamped
&
goal
,
double
tolerance
,
std
::
vector
<
geometry_msgs
::
PoseStamped
>&
plan
)
{
double
roll
,
pitch
,
yaw
;
double
yaw
;
std
::
vector
<
unsigned
int
>
path
;
std
::
vector
<
double
>
x
,
y
;
std
::
vector
<
double
>
x
,
y
,
heading
;
TPoint
real_goal
;
boost
::
mutex
::
scoped_lock
lock
(
mutex_
);
if
(
!
initialized_
)
{
...
...
@@ -212,22 +213,16 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
*/
try
{
ROS_WARN
(
"Make Plan"
);
tf
::
Quaternion
q_start
(
start
.
pose
.
orientation
.
x
,
start
.
pose
.
orientation
.
y
,
start
.
pose
.
orientation
.
z
,
start
.
pose
.
orientation
.
w
);
tf
::
Matrix3x3
m_start
(
q_start
);
m_start
.
getRPY
(
roll
,
pitch
,
yaw
);
ROS_WARN_STREAM
(
"start position: x: "
<<
start
.
pose
.
position
.
x
<<
", y: "
<<
start
.
pose
.
position
.
y
<<
", yaw: "
<<
yaw
);
yaw
=
tf2
::
getYaw
(
start
.
pose
.
orientation
);
this
->
roadmap
.
set_start_point
(
start
.
pose
.
position
.
x
,
start
.
pose
.
position
.
y
,
yaw
);
tf
::
Quaternion
q_goal
(
goal
.
pose
.
orientation
.
x
,
goal
.
pose
.
orientation
.
y
,
goal
.
pose
.
orientation
.
z
,
goal
.
pose
.
orientation
.
w
);
tf
::
Matrix3x3
m_goal
(
q_goal
);
m_goal
.
getRPY
(
roll
,
pitch
,
yaw
);
ROS_WARN_STREAM
(
"start position: x: "
<<
goal
.
pose
.
position
.
x
<<
", y: "
<<
goal
.
pose
.
position
.
y
<<
", yaw: "
<<
yaw
);
this
->
roadmap
.
set_target_point
(
goal
.
pose
.
position
.
x
,
goal
.
pose
.
position
.
y
,
yaw
);
yaw
=
tf2
::
getYaw
(
goal
.
pose
.
orientation
);
real_goal
=
this
->
roadmap
.
set_target_point
(
goal
.
pose
.
position
.
x
,
goal
.
pose
.
position
.
y
,
yaw
);
this
->
roadmap
.
find_shortest_path
();
this
->
roadmap
.
get_path
(
path
);
for
(
unsigned
int
i
=
0
;
i
<
path
.
size
();
i
++
)
std
::
cout
<<
path
[
i
]
<<
","
;
std
::
cout
<<
std
::
endl
;
this
->
roadmap
.
get_trajectory
(
x
,
y
);
//
for(unsigned int i=0;i<path.size();i++)
//
std::cout << path[i] << ",";
//
std::cout << std::endl;
this
->
roadmap
.
get_trajectory
(
x
,
y
,
heading
);
plan
.
resize
(
x
.
size
());
ros
::
Time
plan_time
=
ros
::
Time
::
now
();
for
(
unsigned
int
i
=
0
;
i
<
x
.
size
();
i
++
)
...
...
@@ -237,10 +232,9 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
plan
[
i
].
pose
.
position
.
x
=
x
[
i
];
plan
[
i
].
pose
.
position
.
y
=
y
[
i
];
plan
[
i
].
pose
.
position
.
y
=
y
[
i
];
plan
[
i
].
pose
.
orientation
.
x
=
0.0
;
plan
[
i
].
pose
.
orientation
.
y
=
0.0
;
plan
[
i
].
pose
.
orientation
.
z
=
0.0
;
plan
[
i
].
pose
.
orientation
.
w
=
1.0
;
tf2
::
Quaternion
orientation
;
orientation
.
setRPY
(
0.0
,
0.0
,
heading
[
i
]);
plan
[
i
].
pose
.
orientation
=
tf2
::
toMsg
(
orientation
);
}
}
catch
(
CException
&
e
){
ROS_ERROR_STREAM
(
e
.
what
());
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new 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