joy control

launch/joy.launch.py

@@ -17,6 +17,11 @@ def generate_launch_description():
         DeclareLaunchArgument(
             "namespace", default_value="sirius_bot", description="Top-level namespace"
         ),
+        DeclareLaunchArgument(
+            "joy_model",
+            description="Used joystick model (default is T-29)",
+            default_value="T-29.yaml",
+        ),
         # DeclareLaunchArgument(
         #     "start_rviz", default_value="True", description="Use basic rviz"
         # ),
@@ -35,10 +40,28 @@ def generate_launch_description():
         ],
     )
 
+    model = LaunchConfiguration("joy_model")
+    joy_params = PathJoinSubstitution([pkg_unity_robot_controller,
+                                       "config",
+                                       model
+                                       ])
+
+    joy_control = Node(
+        package="wave_rover_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
             ]
     )

wave_rover_controller/joy_control_node.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 
 import rclpy
-from wave_rover_controller.wave_rover_controller import WaveRoverController
+from wave_rover_controller.wave_rover_controller2 import WaveRoverController
 from unity_robot_controller.joy_control_node import JoyControlNode
 
 def main(args=None):

wave_rover_controller/wave_rover_controller2.py

@@ -0,0 +1,89 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from wave_rover_controller.robot_controller.robot_controller import RobotController
+from geometry_msgs.msg import Twist, Point
+from builtin_interfaces.msg import Time
+from std_srvs.srv import Trigger
+from std_msgs.msg import Bool, String
+import numpy as np
+from tf2_ros import Buffer, TransformListener
+
+
+class WaveRoverController(RobotController):
+
+    def __init__(self, node):
+
+
+        self._node = node
+
+        self._node.declare_parameter('max_linear_speed', 1.0)
+        self._node.declare_parameter('max_angular_speed', 1.5)
+
+        self._node.declare_parameter('fire_ext_max_capacity', 5)
+
+        self._lin_max = self._node.get_parameter('max_linear_speed').value
+        self._ang_max = self._node.get_parameter('max_angular_speed').value
+
+        fire_ext_max_capacity = self._node.get_parameter('fire_ext_max_capacity').value
+
+        super().__init__(fire_ext_max_capacity = fire_ext_max_capacity)
+
+        self._twist_pub = self._node.create_publisher(Twist, 'cmd_vel', 10)
+
+        self.fe_burst_srv = self._node.create_client(Trigger, 'fe_burst')
+
+        self.start_time = self.get_time_seconds() # TODO save it when first cmd is given
+
+
+    def get_time_seconds(self):
+        current_time = self._node.get_clock().now()
+        return current_time.nanoseconds / 1e9
+
+    def get_relative_time(self):
+        return self.get_time_seconds() - self.start_time
+
+    def send_speed_cmd(self, v, w):
+        v, w = super(WaveRoverController, 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(WaveRoverController, self).send_fire_ext_burst_cmd()
+        if not result:
+            self._node.get_logger().error("Fire extinguisher is out of fuel!")
+            return False
+
+        self._node.get_logger().info("Fire extingusher burst is performing")
+        future = self.fe_burst_srv.call_async(Trigger.Request())
+        #future.add_done_callback(self._fe_burst_done_cb)
+        return True
+
+    # def _fe_burst_done_cb(self, future):
+    #     result = future.result()
+    #     if result.success:
+    #         self._register_exted_sof(self.get_relative_time(), {'id': result.message})
+    #         self.pub_status()
+
+    def send_fall_reset_cmd(self):
+        pass
+
+    def send_lights_cmd(self):
+        pass
+
+
+
+
+
+
+
+
+
+
+
+