Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Yver Clément
Anafi_ROS_Olympe
Commits
e7071b1e
Commit
e7071b1e
authored
Apr 14, 2022
by
CactusSoyeux
Browse files
Last commit remote anafi
parent
57083641
Changes
3
Hide whitespace changes
Inline
Side-by-side
anafi_base/script/anafi_node.py
View file @
e7071b1e
#!/
home/anafi/code/parrot-olympe/out/olympe-linux/pyenv_root/versions/3.9.5/bin/
python
3
#!/
usr/bin/env
python
#Generic imports
import
os
...
...
@@ -24,6 +24,7 @@ from olympe.messages.ardrone3.SpeedSettingsState import MaxRotationSpeedChanged
from
olympe.messages.gimbal
import
set_max_speed
from
olympe.messages.gimbal
import
max_speed
from
olympe.messages.gimbal
import
max_speed
from
olympe.video.pdraw
import
Pdraw
,
PdrawState
...
...
@@ -51,10 +52,16 @@ class Anafi:
# Create the olympe.Drone object from its IP address
try
:
self
.
drone
=
olympe
.
Drone
(
DRONE_IP
)
self
.
drone
.
connect
()
except
Exception
as
e
:
print
(
"Drone connection failed: %s"
%
e
)
while
not
self
.
drone
.
connection_state
():
try
:
print
(
"Trying to connect to the drone...."
)
self
.
drone
.
connect
(
timeout
=
10
)
except
Exception
as
e
:
print
(
"Drone connection failed: %s"
%
e
)
#Pdraw init
self
.
frame_queue
=
queue
.
Queue
()
self
.
flush_queue_lock
=
threading
.
Lock
()
...
...
@@ -230,47 +237,65 @@ class Anafi:
self
.
drone
.
start_piloting
()
#ROS
rospy
.
loginfo
(
"ROS Anafi base: Start"
)
while
not
rospy
.
is_shutdown
():
#ROS Publishers
if
self
.
anafi_img
is
not
None
:
self
.
img_pub
.
publish
(
self
.
br
.
cv2_to_imgmsg
(
self
.
anafi_img
,
"rgb8"
))
#Not None verified in the frame call back
self
.
bat_pub
.
publish
(
self
.
bat_perc
)
self
.
gnd_dist_pub
.
publish
(
self
.
gnd_dist
)
self
.
air_speed_pub
.
publish
(
self
.
air_speed
)
self
.
quat_pub
.
publish
(
self
.
quat
)
self
.
speed_pub
.
publish
(
self
.
speed
)
self
.
state_pub
.
publish
(
self
.
state
)
self
.
sat_pub
.
publish
(
self
.
sat
)
self
.
gps_pub
.
publish
(
self
.
gps
)
#Convert cmd_vel in drone mouvement
if
(
self
.
vel
!=
Twist
())
&
((
self
.
last_vel_time
-
rospy
.
get_time
())
<=
self
.
time_thrs
)
&
((
self
.
state
==
'FLYING'
)
|
(
self
.
state
==
'HOVERING'
)
&
(
self
.
bat_perc
>
10
)):
self
.
drone
.
piloting
(
np
.
int32
(
self
.
vel
.
linear
.
y
).
item
(),
np
.
int32
(
self
.
vel
.
linear
.
x
).
item
(),
np
.
int32
(
self
.
vel
.
angular
.
z
).
item
(),
np
.
int32
(
self
.
vel
.
linear
.
z
).
item
(),
0
)
elif
((
self
.
state
==
'HOVERING'
)
|
(
self
.
state
==
'FLYING'
))
&
(
self
.
vel
==
Twist
()):
rospy
.
loginfo
(
"ROS Anafi base: Start"
)
while
self
.
drone
.
connection_state
():
#ROS Publishers
if
self
.
anafi_img
is
not
None
:
self
.
img_pub
.
publish
(
self
.
br
.
cv2_to_imgmsg
(
self
.
anafi_img
,
"rgb8"
))
#Not None verified in the frame call back
self
.
bat_pub
.
publish
(
self
.
bat_perc
)
self
.
gnd_dist_pub
.
publish
(
self
.
gnd_dist
)
self
.
air_speed_pub
.
publish
(
self
.
air_speed
)
self
.
quat_pub
.
publish
(
self
.
quat
)
self
.
speed_pub
.
publish
(
self
.
speed
)
self
.
state_pub
.
publish
(
self
.
state
)
self
.
sat_pub
.
publish
(
self
.
sat
)
self
.
gps_pub
.
publish
(
self
.
gps
)
#Convert cmd_vel in drone mouvement
if
(
self
.
vel
!=
Twist
())
&
((
self
.
last_vel_time
-
rospy
.
get_time
())
<=
self
.
time_thrs
)
&
((
self
.
state
==
'FLYING'
)
|
(
self
.
state
==
'HOVERING'
)
&
(
self
.
bat_perc
>
10
)):
self
.
drone
.
piloting
(
np
.
int32
(
self
.
vel
.
linear
.
y
).
item
(),
np
.
int32
(
self
.
vel
.
linear
.
x
).
item
(),
np
.
int32
(
self
.
vel
.
angular
.
z
).
item
(),
np
.
int32
(
self
.
vel
.
linear
.
z
).
item
(),
0
)
self
.
drone
.
piloting
(
0
,
0
,
0
,
0
,
0
)
else
:
elif
((
self
.
state
==
'HOVERING'
)
|
(
self
.
state
==
'FLYING'
))
&
(
self
.
vel
==
Twist
()):
self
.
drone
.
piloting
(
0
,
0
,
0
,
0
,
0
)
else
:
pass
if
self
.
bat_perc
<
10
:
pass
print
(
"Battery low: Please Land"
)
if
self
.
bat_perc
<
10
:
'''
try:
print("Battery low: Call Land")
print(self.takeOffLandService("land"))
except :
print("Service call failed")
'''
print
(
self
.
drone
.
get_state
(
olympe
.
messages
.
gimbal
.
attitude
)[
0
][
'pitch_absolute'
])
self
.
loop_rate
.
sleep
()
print
(
"Battery low: Please La
nd"
)
rospy
.
loginfo
(
"ROS Anafi base: E
nd"
)
'''
while
not
self
.
drone
.
connection_state
():
try
:
print("Battery low: Call Land")
print(self.takeOffLandService("land"))
except :
print("Service call failed")
'''
self
.
loop_rate
.
sleep
()
print
(
"Trying to connect to the drone...."
)
self
.
drone
.
connect
(
timeout
=
10
)
except
Exception
as
e
:
print
(
"Drone connection failed: %s"
%
e
)
self
.
pdraw
.
close
()
self
.
pdraw
.
dispose
()
#Stop piloting
self
.
drone
.
stop_piloting
()
...
...
anafi_control/script/change_node.py
View file @
e7071b1e
#!/
home/anafi/code/parrot-olympe/out/olympe-linux/pyenv_root/versions/3.9.5/bin/
python
3
#!/
usr/bin/env
python
#ROS imports
import
rospy
...
...
anafi_control/script/control_node.py
View file @
e7071b1e
#!/
home/anafi/code/parrot-olympe/out/olympe-linux/pyenv_root/versions/3.9.5/bin/
python
3
#!/
usr/bin/env
python
from
__future__
import
print_function
...
...
@@ -54,11 +54,11 @@ class JoyCommand:
print
(
"Service call failed: %s"
%
e
)
def
ctrl_cb
(
self
,
mode
):
self
.
ctrl
=
mode
self
.
ctrl
=
mode
.
data
def
joy_cb
(
self
,
joy
):
#Update Twist values if one of the DMS (Dead Man Switch) is triggered
if
joy
.
buttons
[
4
]
or
joy
.
buttons
[
5
]
and
(
self
.
ctrl
==
'joy'
):
if
(
joy
.
buttons
[
4
]
or
joy
.
buttons
[
5
]
)
and
(
self
.
ctrl
==
'joy'
):
self
.
vel
.
linear
.
x
=
joy
.
axes
[
4
]
*
self
.
sensivity
self
.
vel
.
linear
.
y
=
-
joy
.
axes
[
3
]
*
self
.
sensivity
self
.
vel
.
linear
.
z
=
((
joy
.
axes
[
2
]
+
1
)
/
2
-
(
joy
.
axes
[
5
]
+
1
)
/
2
)
*
self
.
sensivity
...
...
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