Commit dc60e8e8 authored by Inès EL HADRI's avatar Inès EL HADRI 💤

Convert reactive_move to class

parent d1857602
#!/usr/bin/python3 #!/usr/bin/python3
import rclpy import rclpy
import math
from rclpy.node import Node from rclpy.node import Node
from sensor_msgs.msg import PointCloud2 from sensor_msgs.msg import PointCloud2
from geometry_msgs.msg import Twist from geometry_msgs.msg import Twist
import sensor_msgs_py.point_cloud2 import sensor_msgs_py.point_cloud2
from std_msgs.msg import Header
rosNode= None class ReactiveMove(Node):
velocity_publisher = None
def scan_callback(scanMsg): def __init__(self, name="reactive_move"):
global rosNode super().__init__(name) # Create the node
global velocity_publisher
# Initialize a subscriber
self.create_subscription(PointCloud2, 'scan_points', self.scan_callback, 10)
# Initialize a publisher
self._velocity_publisher = self.create_publisher(Twist, 'cmd_vel', 10)
def scan_callback(self, scanMsg):
points = sensor_msgs_py.point_cloud2.read_points(scanMsg) points = sensor_msgs_py.point_cloud2.read_points(scanMsg)
x, y = firstPointInFront(points, 0.5, 0.25) x, y = firstPointInFront(points, 0.5, 0.25)
...@@ -28,7 +34,7 @@ def scan_callback(scanMsg): ...@@ -28,7 +34,7 @@ def scan_callback(scanMsg):
else: # If there's not point in front, go straight else: # If there's not point in front, go straight
velocity.linear.x = 0.2 velocity.linear.x = 0.2
velocity_publisher.publish(velocity) self._velocity_publisher.publish(velocity)
...@@ -42,20 +48,12 @@ def firstPointInFront(points:list, maxX, maxY): ...@@ -42,20 +48,12 @@ def firstPointInFront(points:list, maxX, maxY):
def main(): def main():
global velocity_publisher
rclpy.init() rclpy.init()
rosNode= Node('reactive_move') rosNode = ReactiveMove()
# Initialize a subscriber: rclpy.spin(rosNode)
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 )
scanInterpret.destroy_node() rosNode.destroy_node()
rclpy.shutdown() rclpy.shutdown()
if __name__ == "__main__": if __name__ == "__main__":
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment