diff --git a/launch/base_gesture_control.launch.py b/launch/base_gesture_control.launch.py new file mode 100644 index 0000000..da671e3 --- /dev/null +++ b/launch/base_gesture_control.launch.py @@ -0,0 +1,44 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import ( + PathJoinSubstitution, + LaunchConfiguration, + TextSubstitution, +) +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node +from launch_ros.descriptions import ParameterFile + +def generate_launch_description(): + pkg_unity_robot_controller = FindPackageShare(package="").find("unity_robot_controller") + + + # Launch arguments + launch_args = [ + DeclareLaunchArgument( + "use_sim_time", + description="Use clock from simulation", + default_value="True", + ), + DeclareLaunchArgument( + "namespace", default_value="sirius_bot", description="Top-level namespace" + ), + ] + + base_gesture_control_node = Node( + package="unity_robot_controller", + executable="base_gesture_control_node", + name="base_gesture_control", + namespace=LaunchConfiguration("namespace"), + output="both", + parameters=[ + {"fire_ext_max_capacity": 100}, + ], + ) + + return LaunchDescription( + launch_args + + [ + base_gesture_control_node, + ] + ) diff --git a/setup.py b/setup.py index 84f3d5a..f0c73ec 100644 --- a/setup.py +++ b/setup.py @@ -29,7 +29,8 @@ setup( entry_points={ 'console_scripts': [ 'fire_extinguisher_emulator_node = unity_robot_controller.fire_extinguisher_emulator:main', - 'joy_control_node = unity_robot_controller.joy_control_node:main' + 'joy_control_node = unity_robot_controller.joy_control_node:main', + 'base_gesture_control_node = unity_robot_controller.base_gesture_control_node:main' ], }, ) diff --git a/unity_robot_controller/base_gesture_control_node.py b/unity_robot_controller/base_gesture_control_node.py new file mode 100644 index 0000000..895846e --- /dev/null +++ b/unity_robot_controller/base_gesture_control_node.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from unity_robot_controller.unity_robot_controller import UnityRobotController +from sensor_msgs.msg import Joy +from cv_bridge import CvBridge +import cv2 +import numpy as np +from sensor_msgs.msg import Image + +class BaseGestureControlNode(Node): + + def __init__(self, RobotController): + + super().__init__('joy_control') + + self.URC = RobotController(self) + + + self.bridge = CvBridge() + self.cv_image = None + + # subscriptions + self.create_subscription(Image, 'image', self.image_cb, 10) + + + def image_cb(self, msg): + try: + self.cv_image = self.bridge.imgmsg_to_cv2(raw_msg, desired_encoding='bgr8') + except Exception as e: + self.get_logger().error(f"Failed to convert image: {e}") + self.cv_image = None + return + + def run(self): + try: + while rclpy.ok(): + + if not self.cv_image is None: + + # DO DETECTION HERE + self.cv_image + + # SELECT COMMAND + + # self.URC.send_speed_cmd(v, w) # speed control in -1 1 both + # self.URC.send_fire_ext_pose_cmd(x, y) # crosshair in -1 1 both + # self.URC.send_fire_ext_burst_cmd() + + rclpy.spin_once(self, timeout_sec=0.1) + finally: + self.destroy_node() + rclpy.shutdown() + + + + +def main(args=None): + rclpy.init(args=args) + + node = BaseGestureControlNode(UnityRobotController) + node.run() + #rclpy.run()#spin(node) + #node.destroy_node() + #rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/unity_robot_controller/gesture_rec b/unity_robot_controller/gesture_rec index 266adb8..0415ff9 160000 --- a/unity_robot_controller/gesture_rec +++ b/unity_robot_controller/gesture_rec @@ -1 +1 @@ -Subproject commit 266adb892ad59d5fdc70f6202480c77b98d94cdc +Subproject commit 0415ff9f429d9f23bd14897447a0d555955eb17a