diff --git a/config/T-29.yaml b/config/T-29.yaml new file mode 100644 index 0000000..92756c0 --- /dev/null +++ b/config/T-29.yaml @@ -0,0 +1,17 @@ +arctic_bot/joy_control: + ros__parameters: + speed_x_axis: 4 + speed_y_axis: 3 + speed_throttle_btn: 7 + + crosshair_plus_x_btn: -1 + crosshair_minus_x_btn: -1 + crosshair_plus_y_btn: -1 + crosshair_minus_y_btn: -1 + + crosshair_step: 0.05 + + crosshair_x_axis: 6 + crosshair_y_axis: 7 + + diff --git a/config/joy_config.yaml b/config/joy_config.yaml new file mode 100644 index 0000000..f15c0fd --- /dev/null +++ b/config/joy_config.yaml @@ -0,0 +1,14 @@ +arctic_bot/joy: + ros__parameters: + # to list connected devices, use: + # ros2 run joy joy_enumerate_devices + device_id: 0 + # device_name can used instead of device_id. device_name takes precedence + device_name: "" + # in [0.; 1.0] + deadzone: 0.3 + # set autorepeat to 0 to disable + autorepeat_rate: 20.0 + sticky_buttons: false + # rate control + coalesce_interval_ms: 1 diff --git a/config/unity.rviz b/config/unity.rviz index 3485ddc..e181b54 100644 --- a/config/unity.rviz +++ b/config/unity.rviz @@ -3,9 +3,7 @@ Panels: Help Height: 87 Name: Displays Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 + Expanded: ~ Splitter Ratio: 0.5 Tree Height: 696 - Class: rviz_common/Selection @@ -73,7 +71,58 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /fire_extinguisher_emulator/fire_extinguisher_image + Value: /arctic_bot/crosshair_image + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: true + back_left_wheel_link: + Value: true + back_right_wheel_link: + Value: true + base_footprint: + Value: true + base_link: + Value: true + camera_link: + Value: true + front_left_wheel_link: + Value: true + front_right_wheel_link: + Value: true + laser_link: + Value: true + map: + Value: true + odom: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_footprint: + base_link: + back_left_wheel_link: + {} + back_right_wheel_link: + {} + camera_link: + {} + front_left_wheel_link: + {} + front_right_wheel_link: + {} + laser_link: + {} + Update Interval: 0 Value: true Enabled: true Global Options: @@ -121,7 +170,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 10 + Distance: 8.9044189453125 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 diff --git a/launch/joy_control.launch.py b/launch/joy_control.launch.py new file mode 100644 index 0000000..5449ce8 --- /dev/null +++ b/launch/joy_control.launch.py @@ -0,0 +1,69 @@ +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( + "joy_model", + description="Used joystick model (default is T-29)", + default_value="T-29.yaml", + ), + DeclareLaunchArgument( + "namespace", default_value="arctic_bot", description="Top-level namespace" + ), + ] + + joy_config = PathJoinSubstitution([pkg_unity_robot_controller, "config", "joy_config.yaml"]) + + model = LaunchConfiguration("joy_model") + joy_params = PathJoinSubstitution([pkg_unity_robot_controller, + "config", + model + ]) + + joy_node = Node( + package="joy", + executable="joy_node", + name="joy", + namespace=LaunchConfiguration("namespace"), + output="both", + parameters=[ + ParameterFile(joy_config), + ], + ) + + joy_control_node = Node( + package="unity_robot_controller", + executable="joy_control_node", + name="joy_control", + namespace=LaunchConfiguration("namespace"), + output="both", + parameters=[ + ParameterFile(joy_params), + ], + ) + + return LaunchDescription( + launch_args + + [ + joy_node, + joy_control_node, + ] + ) diff --git a/launch/unity.launch.py b/launch/unity.launch.py index f859944..f884c93 100644 --- a/launch/unity.launch.py +++ b/launch/unity.launch.py @@ -54,6 +54,7 @@ def generate_launch_description(): fe_emulator = Node( package="unity_robot_controller", executable="fire_extinguisher_emulator_node", + namespace=LaunchConfiguration("namespace"), parameters=[ {"use_sim_time": LaunchConfiguration("use_sim_time")}, ], diff --git a/setup.py b/setup.py index c3b0999..84f3d5a 100644 --- a/setup.py +++ b/setup.py @@ -28,7 +28,8 @@ setup( }, entry_points={ 'console_scripts': [ - 'fire_extinguisher_emulator_node = unity_robot_controller.fire_extinguisher_emulator:main' + 'fire_extinguisher_emulator_node = unity_robot_controller.fire_extinguisher_emulator:main', + 'joy_control_node = unity_robot_controller.joy_control_node:main' ], }, ) diff --git a/unity_robot_controller/fire_extinguisher_emulator.py b/unity_robot_controller/fire_extinguisher_emulator.py index 20738f2..7675099 100644 --- a/unity_robot_controller/fire_extinguisher_emulator.py +++ b/unity_robot_controller/fire_extinguisher_emulator.py @@ -24,8 +24,8 @@ class FireExtinguisherEmulatorNode(Node): self.crosshair_color = self.get_parameter('crosshair_color').value # Publishers - self.fire_ext_image_pub = self.create_publisher(Image, '~/fire_extinguisher_image', 10) - self.fire_ext_vector_pub = self.create_publisher(Vector3, '~/fire_extinguisher_vector', 10) + self.fire_ext_image_pub = self.create_publisher(Image, 'crosshair_image', 10) + self.fire_ext_vector_pub = self.create_publisher(Vector3, 'crosshair_vector', 10) # State variables self.target_x = 0.0 @@ -48,23 +48,15 @@ class FireExtinguisherEmulatorNode(Node): # Timers and subscriptions self.target_timer = self.create_timer(0.01, self.target_timer_cb) # 100 Hz - self.create_subscription(Point, '~/target_point', self.target_cb, 10) + 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) - # UI control subscription (optional) - self.create_subscription(Vector3, '~/ui_control', self.ui_control_cb, 10) - def target_cb(self, msg): """Received target point in normalized coordinates [-1, 1]""" self.target_x = min(max(-1.0, msg.x), 1.0) self.target_y = min(max(-1.0, msg.y), 1.0) - def ui_control_cb(self, msg): - """Manual UI control for crosshair (normalized coordinates)""" - self.target_x = min(max(-1.0, msg.x), 1.0) - self.target_y = min(max(-1.0, msg.y), 1.0) - def cam_info_cb(self, msg): """Save camera info and shutdown subscription""" if self.camera_info is None: @@ -116,6 +108,30 @@ class FireExtinguisherEmulatorNode(Node): self.current_x += step_x self.current_y += step_y + # Calculate 3D unit vector from camera frame + if self.fx > 0 and self.fy > 0: + # Pixel coordinates to normalized coordinates + px = self.current_x + py = self.current_y + + # Inverse projection: (px, py) -> (x, y, z) in camera frame + x = (px - self.cx) / self.fx + y = (py - self.cy) / self.fy + z = 1.0 + + # Normalize to unit vector + norm = np.sqrt(x**2 + y**2 + z**2) + if norm > 0: + x /= norm + y /= norm + z /= norm + + vector_msg = Vector3() + vector_msg.x = x + vector_msg.y = y + vector_msg.z = z + self.fire_ext_vector_pub.publish(vector_msg) + def raw_image_cb(self, raw_msg): """Draw crosshair on image and publish 3D vector""" if self.camera_info is None: @@ -163,29 +179,7 @@ class FireExtinguisherEmulatorNode(Node): except Exception as e: self.get_logger().error(f"Failed to publish image: {e}") - # Calculate 3D unit vector from camera frame - if self.fx > 0 and self.fy > 0: - # Pixel coordinates to normalized coordinates - px = self.current_x - py = self.current_y - - # Inverse projection: (px, py) -> (x, y, z) in camera frame - x = (px - self.cx) / self.fx - y = (py - self.cy) / self.fy - z = 1.0 - # Normalize to unit vector - norm = np.sqrt(x**2 + y**2 + z**2) - if norm > 0: - x /= norm - y /= norm - z /= norm - - vector_msg = Vector3() - vector_msg.x = x - vector_msg.y = y - vector_msg.z = z - self.fire_ext_vector_pub.publish(vector_msg) def destroy_node(self): """Clean up subscriptions""" diff --git a/unity_robot_controller/unity_robot_controller.py b/unity_robot_controller/unity_robot_controller.py index dc9b667..b23d39f 100644 --- a/unity_robot_controller/unity_robot_controller.py +++ b/unity_robot_controller/unity_robot_controller.py @@ -22,7 +22,7 @@ class UnityRobotController(RobotController): self._ang_max = self._node.get_parameter('max_angular_speed').value self._twist_pub = self._node.create_publisher(Twist, 'cmd_vel', 10) - self._target_point_pub = self._node.create_publisher(Point, '/fire_extinguisher_emulator/target_point', 10) + self._target_point_pub = self._node.create_publisher(Point, 'target_point', 10) def send_speed_cmd(self, v, w):