diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..a05f8ce --- /dev/null +++ b/.gitmodules @@ -0,0 +1,6 @@ +[submodule "robot_controller"] + path = robot_controller + url = https://git.robofob.ru/sirius/robot_controller +[submodule "unity_robot_controller/robot_controller"] + path = unity_robot_controller/robot_controller + url = https://git.robofob.ru/sirius/robot_controller diff --git a/unity_robot_controller/fire_extinguisher_emulator.py b/unity_robot_controller/fire_extinguisher_emulator.py new file mode 100644 index 0000000..ffddcd9 --- /dev/null +++ b/unity_robot_controller/fire_extinguisher_emulator.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Point, Vector3 +from rclpy.executors import MultiThreadedExecutor +from sensors_msgs.msg import Image, CameraInfo + + +class FireExtinguisherEmulatorNode(Node): + + def __init__(self): + super().__init__('fire_extinguisher_emulator') + + self.declare_parameter('crosshair_alpha', 0.5) + self.declare_parameter('crosshair_speed', 1.) # pixel/sec + + self.crosshair_alpha = self.get_parameter('crosshair_alpha').value + self.crosshair_speed = self.get_parameter('crosshair_speed').value + + self.fire_ext_image_pub = self.create_publisher(Image, '~/fire_extinguisher_image', 10) + self.fire_ext_image_pub = self.create_publisher(Vector3, '~/fire_extinguisher_vector', 10) + + self.target_x = 0.0 + self.target_y = 0.0 + + self.current_x = 0.0 + self.current_y = 0.0 + + self.target_timer = self.create_timer(0.1, self.target_timer_cb) + self.create_subscription(Point, '~/target_point', self.target_cb, 10) + self.create_subscription(CameraInfo, 'camera_info', self.cam_info_cb, 10) + self.create_subscription(Image, 'raw_image', self.raw_image_cb, 10) + + + def target_cb(self, msg): + self.target_x = min( max(-1., msg.x), 1.) + self.target_y = min( max(-1., msg.y), 1.) + + def cam_info_cb(self, msg): + # save info and shutdown that subscription + + + def target_timer_cb(self): + if self.current_x != self.target_x or self.current_y != self.target_x: + # do target smooth movement to target assumint crosshair_speed limits + + def raw_image_cb(self, raw_msg): + # draw some kind of simple crosshair from current x and y + + # using saved info calculate 3d unit vector from camera frame and publish it + + + +def main(args=None): + rclpy.init(args=args) + node = FireExtinguisherEmulatorNode() + executor = MultiThreadedExecutor(num_threads=4) + executor.add_node(node) + try: + rclpy.spin(node, executor) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() + + diff --git a/unity_robot_controller/robot_controller b/unity_robot_controller/robot_controller new file mode 160000 index 0000000..eb6ec13 --- /dev/null +++ b/unity_robot_controller/robot_controller @@ -0,0 +1 @@ +Subproject commit eb6ec135a5a3f4ed8361e75f1f6662b7a19b81ec diff --git a/unity_robot_controller/unity_robot_controller.py b/unity_robot_controller/unity_robot_controller.py new file mode 100644 index 0000000..06c3757 --- /dev/null +++ b/unity_robot_controller/unity_robot_controller.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from unity_robot_controller.robot_controller.robot_controller import RobotController +from geometry_msgs.msg import Twist + + +class UnityRobotController(RobotController): + + def __init__(self, node): + super().__init__() + + self._node = node + + self._node.declare_parameter('max_linear_speed', 0.5) + self._node.declare_parameter('max_angular_speed', 1.0) + + self._node.declare_parameter('fire_ext_pixel_speed', 1.0) + + self._lin_max = self._node.get_parameter('max_linear_speed').value + self._ang_max = self._node.get_parameter('max_angular_speed').value + + self._twist_pub = self._node.create_publisher(Twist, 'cmd_vel', 10) + + + def send_speed_cmd(self, v, w): + v, w = super(UnityRobotController, self).send_speed_cmd(v, w) + + twist_msg = Twist() + twist_msg.linear.x = v * self._lin_max + twist_msg.angular.z = w * self._ang_max + + self._twist_pub.publish(twist_msg) + + def send_fire_ext_burst_cmd(self): + result = super(UnityRobotController, self).send_fire_ext_burst_cmd() + + + + + + +