Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
L
larm-astro
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Commits
Open sidebar
Inès EL HADRI
larm-astro
Commits
dc60e8e8
Commit
dc60e8e8
authored
Dec 22, 2023
by
Inès EL HADRI
💤
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Convert reactive_move to class
parent
d1857602
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
with
31 additions
and
33 deletions
+31
-33
reactive_move
tuto_kickoff/scripts/reactive_move
+31
-33
No files found.
tuto_kickoff/scripts/reactive_move
View file @
dc60e8e8
#!/usr/bin/python3
import
rclpy
import
math
from
rclpy.node
import
Node
from
sensor_msgs.msg
import
PointCloud2
from
geometry_msgs.msg
import
Twist
import
sensor_msgs_py.point_cloud2
from
std_msgs.msg
import
Header
rosNode
=
None
velocity_publisher
=
None
class
ReactiveMove
(
Node
):
def
__init__
(
self
,
name
=
"reactive_move"
):
super
()
.
__init__
(
name
)
# Create the node
def
scan_callback
(
scanMsg
):
global
rosNode
global
velocity_publisher
points
=
sensor_msgs_py
.
point_cloud2
.
read_points
(
scanMsg
)
# Initialize a subscriber
self
.
create_subscription
(
PointCloud2
,
'scan_points'
,
self
.
scan_callback
,
10
)
x
,
y
=
firstPointInFront
(
points
,
0.5
,
0.25
)
# Initialize a publisher
self
.
_velocity_publisher
=
self
.
create_publisher
(
Twist
,
'cmd_vel'
,
10
)
velocity
=
Twist
()
if
x
and
y
:
if
y
>=
0
:
# Point is in the left part of the rectangle -> Turn right
velocity
.
angular
.
z
=
-
1.0
else
:
# Point is in the right part of the rectangle -> Turn left
velocity
.
angular
.
z
=
1.0
else
:
# If there's not point in front, go straight
velocity
.
linear
.
x
=
0.2
velocity_publisher
.
publish
(
velocity
)
def
scan_callback
(
self
,
scanMsg
):
points
=
sensor_msgs_py
.
point_cloud2
.
read_points
(
scanMsg
)
x
,
y
=
firstPointInFront
(
points
,
0.5
,
0.25
)
velocity
=
Twist
()
if
x
and
y
:
if
y
>=
0
:
# Point is in the left part of the rectangle -> Turn right
velocity
.
angular
.
z
=
-
1.0
else
:
# Point is in the right part of the rectangle -> Turn left
velocity
.
angular
.
z
=
1.0
else
:
# If there's not point in front, go straight
velocity
.
linear
.
x
=
0.2
self
.
_velocity_publisher
.
publish
(
velocity
)
def
firstPointInFront
(
points
:
list
,
maxX
,
maxY
):
for
(
x
,
y
,
_
)
in
points
:
...
...
@@ -42,20 +48,12 @@ def firstPointInFront(points:list, maxX, maxY):
def
main
():
global
velocity_publisher
rclpy
.
init
()
rosNode
=
Node
(
'reactive_move'
)
# Initialize a subscriber:
rosNode
.
create_subscription
(
PointCloud2
,
'scan_points'
,
scan_callback
,
10
)
# Initialize a publisher:
velocity_publisher
=
rosNode
.
create_publisher
(
Twist
,
'/cmd_vel'
,
10
)
while
True
:
rclpy
.
spin_once
(
rosNode
)
rosNode
=
ReactiveMove
()
rclpy
.
spin
(
rosNode
)
scanInterpret
.
destroy_node
()
rosNode
.
destroy_node
()
rclpy
.
shutdown
()
if
__name__
==
"__main__"
:
...
...
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