Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F85491456
nmpf_node.cpp
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Sun, Sep 29, 14:00
Size
3 KB
Mime Type
text/x-c
Expires
Tue, Oct 1, 14:00 (2 d)
Engine
blob
Format
Raw Data
Handle
21191381
Attached To
R1517 test_package
nmpf_node.cpp
View Options
#include "nmpf_node.hpp"
#include "ekf_node.h"
using
namespace
casadi
;
void
KiteNMPF_Node
::
filterCallback
(
const
sensor_msgs
::
MultiDOFJointState
::
ConstPtr
&
msg
)
{
boost
::
unique_lock
<
boost
::
mutex
>
scoped_lock
(
m_mutex
,
boost
::
try_to_lock
);
kite_state
=
convertToDM
(
*
msg
);
}
KiteNMPF_Node
::
KiteNMPF_Node
(
const
ros
::
NodeHandle
&
_nh
,
const
KiteProperties
&
kite_props
,
const
AlgorithmProperties
&
algo_props
)
{
controller
=
std
::
make_shared
<
KiteNMPF
>
(
kite_props
,
algo_props
);
nh
=
std
::
make_shared
<
ros
::
NodeHandle
>
(
_nh
);
/** set control constraints */
double
angle_sat
=
math_utils
::
deg2rad
(
7.0
);
DM
lbu
=
DM
::
vertcat
({
0
,
-
angle_sat
,
-
angle_sat
,
0
});
DM
ubu
=
DM
::
vertcat
({
0.3
,
angle_sat
,
angle_sat
,
1
});
controller
->
setLBU
(
lbu
);
controller
->
setUBU
(
ubu
);
/** set variable constraints */
DM
lbx
=
DM
::
vertcat
({
5
,
-
DM
::
inf
(
1
),
-
DM
::
inf
(
1
),
-
M_PI
,
-
M_PI
,
-
M_PI
,
-
DM
::
inf
(
1
),
-
DM
::
inf
(
1
),
-
DM
::
inf
(
1
),
-
DM
::
inf
(
1
),
-
DM
::
inf
(
1
),
-
DM
::
inf
(
1
),
-
DM
::
inf
(
1
),
-
DM
::
inf
(
1
),
-
1
});
DM
ubx
=
DM
::
vertcat
({
12
,
DM
::
inf
(
1
),
DM
::
inf
(
1
),
M_PI
,
M_PI
,
M_PI
,
DM
::
inf
(
1
),
DM
::
inf
(
1
),
DM
::
inf
(
1
),
DM
::
inf
(
1
),
DM
::
inf
(
1
),
DM
::
inf
(
1
),
DM
::
inf
(
1
),
DM
::
inf
(
1
),
12
});
controller
->
setLBX
(
lbx
);
controller
->
setUBX
(
ubx
);
/** test cheb */
controller
->
createNLP
();
/** initialize subscribers and publishers */
control_pub
=
nh
->
advertise
<
test_package
::
aircraft_controls
>
(
"kite_controls"
,
100
);
std
::
string
state_topic
=
"/kite_state"
;
state_sub
=
nh
->
subscribe
(
state_topic
,
100
,
&
KiteNMPF_Node
::
filterCallback
,
this
);
m_initialized
=
false
;
}
DM
KiteNMPF_Node
::
convertToDM
(
const
sensor_msgs
::
MultiDOFJointState
&
_value
)
{
DM
value
=
DM
::
zeros
(
13
);
value
[
0
]
=
_value
.
twist
.
back
().
linear
.
x
;
value
[
1
]
=
_value
.
twist
.
back
().
linear
.
y
;
value
[
2
]
=
_value
.
twist
.
back
().
linear
.
z
;
value
[
3
]
=
_value
.
twist
.
back
().
angular
.
x
;
value
[
4
]
=
_value
.
twist
.
back
().
angular
.
y
;
value
[
5
]
=
_value
.
twist
.
back
().
angular
.
z
;
value
[
6
]
=
_value
.
transforms
.
back
().
translation
.
x
;
value
[
7
]
=
_value
.
transforms
.
back
().
translation
.
y
;
value
[
8
]
=
_value
.
transforms
.
back
().
translation
.
z
;
value
[
9
]
=
_value
.
transforms
.
back
().
rotation
.
w
;
value
[
10
]
=
_value
.
transforms
.
back
().
rotation
.
x
;
value
[
11
]
=
_value
.
transforms
.
back
().
rotation
.
y
;
value
[
12
]
=
_value
.
transforms
.
back
().
rotation
.
z
;
return
value
;
}
void
KiteNMPF_Node
::
publish
()
{
/** pack estimation to ROS message */
std
::
vector
<
double
>
controls
=
controller
->
getOptimalControl
().
get_nonzeros
();
test_package
::
aircraft_controls
control_msg
;
control_msg
.
header
.
stamp
=
ros
::
Time
::
now
();
control_msg
.
thrust
=
controls
[
0
];
control_msg
.
elevator
=
controls
[
1
];
control_msg
.
rudder
=
controls
[
2
];
/** publish current control */
control_pub
.
publish
(
control_msg
);
}
void
KiteNMPF_Node
::
compute_control
()
{
controller
->
computeControl
(
kite_state
);
}
int
main
(
int
argc
,
char
**
argv
)
{
ros
::
init
(
argc
,
argv
,
"nmpf_node"
);
ros
::
NodeHandle
n
;
/** create a kite object */
std
::
string
kite_params_file
;
n
.
param
<
std
::
string
>
(
"kite_params"
,
kite_params_file
,
"umx_radian.yaml"
);
std
::
cout
<<
kite_params_file
<<
"
\n
"
;
KiteProperties
kite_props
=
kite_utils
::
LoadProperties
(
kite_params_file
);
AlgorithmProperties
algo_props
;
algo_props
.
Integrator
=
RK4
;
algo_props
.
sampling_time
=
0.02
;
/** create a NMPF instance */
KiteNMPF_Node
tracker
(
n
,
kite_props
,
algo_props
);
ros
::
Rate
loop_rate
(
20
);
/** 20 Hz */
while
(
ros
::
ok
())
{
tracker
.
compute_control
();
tracker
.
publish
();
ros
::
spinOnce
();
loop_rate
.
sleep
();
}
return
0
;
}
Event Timeline
Log In to Comment