diff --git a/launch/joy_wave_rover.launch.py b/launch/joy_wave_rover.launch.py new file mode 100644 index 0000000..edd73e1 --- /dev/null +++ b/launch/joy_wave_rover.launch.py @@ -0,0 +1,67 @@ +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.substitutions import ( + PathJoinSubstitution, + LaunchConfiguration, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +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( + "joy_model", + description="Used joystick model (default is T-29)", + default_value="T-29.yaml", + ), + DeclareLaunchArgument( + "namespace", default_value="sirius_bot", description="Top-level namespace" + ), + # DeclareLaunchArgument( + # "start_rviz", default_value="True", description="Use basic rviz" + # ), + ] + + 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_wave_rover_controller_node = Node( + package="wave_rover_controller", + executable="joy_control_node", + name="joy_control", + # output="screen", + namespace=LaunchConfiguration("namespace"), + parameters=[ + ParameterFile(joy_params), + {'serial_port': '/dev/ttyUSB0'}] + ) + + + return LaunchDescription( + launch_args + + [ + joy_node, + joy_wave_rover_controller_node, + ] + ) diff --git a/setup.py b/setup.py index d8897b6..5c328d9 100644 --- a/setup.py +++ b/setup.py @@ -13,9 +13,10 @@ setup( ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), (os.path.join('share', package_name, 'launch'), [f for f in glob('launch/*') if os.path.isfile(f)]) , - (os.path.join('share', package_name, 'config'), [f for f in glob('config/*') if os.path.isfile(f)]) , + #(os.path.join('share', package_name, 'config'), [f for f in glob('config/*') if os.path.isfile(f)]) , ], - install_requires=['setuptools'], + install_requires=['setuptools', + 'pyserial'], zip_safe=True, maintainer='anton', maintainer_email='moscowskyad@yandex.ru', diff --git a/wave_rover_controller/joy_control_node b/wave_rover_controller/joy_control_node.py similarity index 77% rename from wave_rover_controller/joy_control_node rename to wave_rover_controller/joy_control_node.py index ab909ad..4d84dbf 100644 --- a/wave_rover_controller/joy_control_node +++ b/wave_rover_controller/joy_control_node.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 import rclpy -from wave_rover_controller.robot_controller.robot_controller import WaveRoverController +from wave_rover_controller.wave_rover_controller import WaveRoverController from unity_robot_controller.joy_control_node import JoyControlNode def main(args=None): diff --git a/wave_rover_controller/robot_controller b/wave_rover_controller/robot_controller index 9f65622..b34711e 160000 --- a/wave_rover_controller/robot_controller +++ b/wave_rover_controller/robot_controller @@ -1 +1 @@ -Subproject commit 9f65622b633900e07ee1f9dbe9e33a5add3167ae +Subproject commit b34711ebc690cf9199b15bc3074e08f258303003 diff --git a/wave_rover_controller/wave_rover_controller.py b/wave_rover_controller/wave_rover_controller.py index 0e20d6f..f0381aa 100644 --- a/wave_rover_controller/wave_rover_controller.py +++ b/wave_rover_controller/wave_rover_controller.py @@ -4,8 +4,9 @@ import rclpy from rclpy.node import Node from wave_rover_controller.robot_controller.robot_controller import RobotController from serial import Serial -from serial.exceptions import SerialException +from serial import SerialException import json +import time class WaveRoverController(RobotController): @@ -14,9 +15,11 @@ class WaveRoverController(RobotController): self._node = node + self._node.declare_parameter('serial_port', '/dev/ttyUSB0') + self._node.declare_parameter('baudrate', 115200) - self._serial_port = self.declare_parameter('serial_port', '/dev/ttyUSB0').value - self._baudrate = self.declare_parameter('baudrate', 115200).value + self._serial_port = self._node.get_parameter('serial_port').value + self._baudrate = self._node.get_parameter('baudrate').value wheel_max = 0.5 # popugais self._linear_max = wheel_max @@ -71,8 +74,8 @@ class WaveRoverController(RobotController): """Twist -> скорости колес для дифференциального привода""" wheel_base = 0.2 # м - left_speed = (linear_x - (wheel_base * angular_z) / 2) / self.max_linear_speed - right_speed = (linear_x + (wheel_base * angular_z) / 2) / self.max_linear_speed + left_speed = (linear_x - (wheel_base * angular_z) / 2) #/ self.max_linear_speed + right_speed = (linear_x + (wheel_base * angular_z) / 2) #/ self.max_linear_speed left_speed = max(-1.0, min(1.0, left_speed)) right_speed = max(-1.0, min(1.0, right_speed))