feat: movement library based on callback function (#18)

* adding skeleton for movement library

* movement library package updated

* code from john/machine-code-sync

packages/movement_library/launch/movement_launch.py

@@ -0,0 +1,16 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            Node(
+                package="movement_library",
+                executable="movement_node",
+                name="movement_node",
+                output="screen",
+            )
+        ]
+    )

packages/movement_library/movement_library/main.py

@@ -1,5 +1,3 @@
-from passive_sound_localization.logger import setup_logger
-
 from rclpy.node import Node
 import logging
 import rclpy
@@ -14,41 +12,60 @@
 from example_interfaces.msg import Bool
 from passive_sound_localization_msgs.msg import LocalizationResult
 
-class SimpleHagridDemoMic(Node):
-    def __init__(self):
-        super().__init__('simple_hagrid_demo')
-        self.cmd_vel_publisher=self.create_publisher(Twist,'rcm/cmd_vel',1)
-        self.enable_publisher=self.create_publisher(Bool,'rcm/enabled',1)
-
-        self.batterySubscription=self.create_subscription(Float32,'rcm/battery',self.battery_callback,1)
-        self.batterySubscription # prevent warning
 
-        # LOCALIZATION RESULT HERE... 
-        # self.create_subscription(LocalizationResult,'passive_sound_localization_msgs',1)
-        self.localizationSubscription={"distance": 0.91, "angle": 180}
+class MovementNode(Node):
+    def __init__(self):
+        super().__init__("movement_node")
+        self.logger = get_logger(__name__)
+        self.logger.info("Starting movement_node")
+        self.cmd_vel_publisher = self.create_publisher(Twist, "rcm/cmd_vel", 1)
+        self.enable_publisher = self.create_publisher(Bool, "rcm/enabled", 1)
+
+        self.batterySubscription = self.create_subscription(
+            Float32, "rcm/battery", self.battery_callback, 1
+        )
+        self.batterySubscription  # prevent warning
+
+        # LOCALIZATION RESULT HERE...
+        self.create_subscription(
+            LocalizationResult, "localization_results", self.localizer_callback, 10
+        )
+        # self.localizationSubscription={"distance": 0.91, "angle": 180}
         # self.create_subscription(...)
 
-        self.loop_time_period=1.0/10.0
-        self.loop_timer=self.create_timer(self.loop_time_period,self.loop)
-        self.time=0.0
+        self.loop_time_period = 1.0 / 10.0
+        self.loop_timer = self.create_timer(self.loop_time_period, self.loop)
+        self.time = 0.0
 
         # Temporary boolean that is set to true after movement is finished...
         # Do we want the robot to self-adjust to new vocal queues as it's moving?
-        self.executing=False
+        self.executing = False
+        self.logger = logging.getLogger(__name__)
 
-    def string_to_dict(self, msg):
-        ...
+    # def string_to_dict(self, msg): ...
 
     def battery_callback(self, msg):
-        self.get_logger().info('battery voltage "%d"' % msg.data)
+        self.logger.info('battery voltage "%d"' % msg.data)
 
     def calculate_time_xyz(self, distance, velocity):
-        return distance/(velocity*1.03)
-    
+        return distance / (velocity * 1.03)
+
     def calculate_time_ang(self, angle, ang_velocity):
-        radians = angle*math.pi/180
-        return radians/(ang_velocity*0.9881)
-
+        radians = angle * math.pi / 180
+        return radians / (ang_velocity * 0.9881)
+
+    def localizer_callback(self, msg):
+        self.logger.info("Got a message")
+        angle = 360-msg.angle
+        distance = msg.distance
+        if self.executing:
+            pass
+        else:
+            self.localizationSubscription={"distance": distance, "angle": angle}
+            self.time = 0.0
+            self.executing = True
+        self.logger.info(f"Got {str(angle)} {str(distance)}")
+
     def loop(self):
         # something to stop time from incrementing or reset it when we get a new input
         self.time=self.time+self.loop_time_period
@@ -73,12 +90,12 @@ def loop(self):
         time_ang = self.calculate_time_ang(self.localizationSubscription["angle"], spdang)
         buff = 1
         wait = 2
-        
+
         time_ang = time_ang+wait
         # print(time_xyz)
         # print(time_ang)
         # print(time_xyz+time_ang+buff)
-        
+
         if self.time<=time_ang and self.time>wait:
             velocityMsg.angular.z=spdang
         else:
@@ -91,23 +108,83 @@ def loop(self):
             self.localizationSubscription["distance"]=0
             self.localizationSubscription["angle"]=0
             velocityMsg.linear.x=0.0
-            self.time=0  # not sure if needed     
-
+            self.time=0  # not sure if needed
+            self.executing=False
+
         # print(velocityMsg.linear.x)
         # print(velocityMsg.angular.z)
-        
+
         self.cmd_vel_publisher.publish(velocityMsg)
-        
+
 
 def main(args=None):
     rclpy.init(args=args)
 
-    simple_hagrid_demo=SimpleHagridDemoMic()
+    node = MovementNode()
 
-    rclpy.spin(simple_hagrid_demo)
+    rclpy.spin(node)
 
-    simple_hagrid_demo.destroy_node()
+    node.destroy_node()
     rclpy.shutdown()
 
-if __name__ == '__main__':
-    main()
+
+if __name__ == "__main__":
+    main()
+
+
+# class MoveRobot(Node):
+#     def __init__(self):
+#         super().__init__('move_robot_node')
+
+#         # Subscriber to get data (e.g., sensor distance)
+#         self.subscription = self.create_subscription(
+#             Float32,  # Adjust type based on your topic
+#             'sensor_topic',  # Replace with the actual topic name
+#             self.sensor_callback,
+#             10)
+
+#         # Publisher to control the robot
+#         self.publisher = self.create_publisher(Twist, 'cmd_vel', 10)
+
+#         # Initializing a timer for continuous control updates
+#         timer_period = 0.1  # seconds
+#         self.timer = self.create_timer(timer_period, self.update_control)
+
+#         # Store sensor data and command variables
+#         self.sensor_data = 0.0
+#         self.move_cmd = Twist()
+
+#     def sensor_callback(self, msg):
+#         # Update sensor data when a new message arrives
+#         self.sensor_data = msg.data
+#         self.get_logger().info(f'Received sensor data: {self.sensor_data}')
+
+#     def update_control(self):
+#         # Process the sensor data and update the Twist message
+#         if self.sensor_data < 1.0:
+#             # Stop if too close to an object
+#             self.move_cmd.linear.x = 0.0
+#             self.move_cmd.angular.z = 0.0
+#         else:
+#             # Move forward if there's enough space
+#             self.move_cmd.linear.x = 0.5
+#             self.move_cmd.angular.z = 0.0
+
+#         # Publish the movement command
+#         self.publisher.publish(self.move_cmd)
+
+# def main(args=None):
+#     rclpy.init(args=args)
+#     move_robot_node = MoveRobot()
+
+#     try:
+#         rclpy.spin(move_robot_node)
+#     except KeyboardInterrupt:
+#         pass
+
+#     # Shutdown and cleanup
+#     move_robot_node.destroy_node()
+#     rclpy.shutdown()
+
+# if __name__ == '__main__':
+#     main()

packages/movement_library/setup.py

@@ -10,8 +10,7 @@
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
         ("share/" + package_name, ["package.xml"]),
-        # ("share/" + package_name, ["launch/localization_launch.py"]),
-        # ("share/" + package_name, ["config/localization_params.yaml"]),
+        ("share/" + package_name, ["launch/movement_launch.py"]),
     ],
     install_requires=[
         "setuptools",