light and fall recovery

config/T-29.yaml

@@ -10,6 +10,8 @@ sirius_bot/joy_control:
 
 
     fe_burst_btn: 2
+    lights_btn: 1
+    fall_reset_btn: 3
 
     crosshair_plus_x_btn: -1
     crosshair_minus_x_btn: -1

config/unity.rviz

@@ -5,7 +5,7 @@ Panels:
     Property Tree Widget:
       Expanded: ~
       Splitter Ratio: 0.5
-    Tree Height: 652
+    Tree Height: 348
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -217,25 +217,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/ThirdPersonFollower
-      Distance: 72.5179443359375
+      Distance: 11.205231666564941
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 5.716884613037109
+        X: 0
-        Y: 3.2075774669647217
+        Y: 0
-        Z: -3.7333018553908914e-05
+        Z: 0
-      Focal Shape Fixed Size: true
+      Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5053315162658691
+      Pitch: 0.5403984189033508
       Target Frame: base_link
       Value: ThirdPersonFollower (rviz_default_plugins)
-      Yaw: 3.1497514247894287
+      Yaw: 2.730400323867798
     Saved:
       - Class: rviz_default_plugins/ThirdPersonFollower
         Distance: 2.978720188140869
@@ -285,7 +285,7 @@ Window Geometry:
   Height: 1008
   Hide Left Dock: true
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000330fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000004c00000330000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000039500000330fc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000004c000001980000000000fffffffaffffffff0100000002fb000000060052004100570000000000ffffffff0000005c01000003fb0000000a005600690065007700730000000670000001100000011001000003fb0000001200430072006f007300730068006100690072010000004c000003300000002201000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004cfc0100000002fb0000000800540069006d0065010000000000000780000002bd01000003fb0000000800540069006d00650100000000000004500000000000000000000003ea0000033000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000330fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000004c00000330000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000039500000330fc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000004c000001100000000000fffffffaffffffff0100000002fb000000060052004100570000000000ffffffff0000005c01000003fb0000000a005600690065007700730000000670000001100000011001000003fb0000001200430072006f007300730068006100690072010000004c000003300000002201000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004cfc0100000002fb0000000800540069006d0065010000000000000780000002bd01000003fb0000000800540069006d00650100000000000004500000000000000000000003ea0000033000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   RAW:
     collapsed: false
   Selection:

launch/unity.launch.py

@@ -83,18 +83,15 @@ def generate_launch_description():
 
     pkg_gen_location_on_mesh = FindPackageShare(package="").find("gen_locations_on_mesh")
 
-    mesh_path = PathJoinSubstitution([pkg_gen_location_on_mesh, "share", "sirius_scene_floor.ply"])
+    mesh_path = PathJoinSubstitution([pkg_gen_location_on_mesh, "share", "sirius_scene_floor_and_objs.ply"])
-
-    cache_path = PathJoinSubstitution([pkg_gen_location_on_mesh, "share", "cache.pkl"])
 
     start_gen_location_on_mesh_cmd = Node(
         name='gen_locations_on_mesh',
         package='gen_locations_on_mesh',
         executable='gen_locations_on_mesh',
         parameters=[{'mesh_filename': mesh_path,
-                   'cache_filename': cache_path,
                    'init_number_of_points': 100,
-                   'max_z_coord': 3.}])
+                   'max_z_coord': 10.}])
 
     return LaunchDescription(
         launch_args

unity_robot_controller/joy_control_node.py

@@ -23,6 +23,8 @@ class JoyControlNode(Node):
         self.declare_parameter('crosshair_minus_y_btn', 4)
 
         self.declare_parameter('fe_burst_btn', 5)
+        self.declare_parameter('lights_btn', 6)
+        self.declare_parameter('fall_reset_btn', 7)
 
         self.declare_parameter('crosshair_step', 0.01)
 
@@ -40,6 +42,8 @@ class JoyControlNode(Node):
         self.crosshair_minus_y_btn = self.get_parameter('crosshair_minus_y_btn').value
 
         self.fe_burst_btn = self.get_parameter('fe_burst_btn').value
+        self.lights_btn = self.get_parameter('lights_btn').value
+        self.fall_reset_btn = self.get_parameter('fall_reset_btn').value
 
         self.crosshair_x_axis = self.get_parameter('crosshair_x_axis').value
         self.crosshair_y_axis = self.get_parameter('crosshair_y_axis').value
@@ -128,7 +132,13 @@ class JoyControlNode(Node):
         if self.was_button_pressed(msg, self.fe_burst_btn):
             self.URC.send_fire_ext_burst_cmd()
 
+        # lights control
+        if self.was_button_pressed(msg, self.lights_btn):
+            self.URC.send_lights_cmd()
 
+        # fall reset
+        if self.was_button_pressed(msg, self.fall_reset_btn):
+            self.URC.send_fall_reset_cmd()
 
 def main(args=None):
     rclpy.init(args=args)

unity_robot_controller/unity_robot_controller.py

@@ -24,7 +24,7 @@ class UnityRobotController(RobotController):
         self._node.declare_parameter('fire_ext_pixel_speed', 1.0)
         self._node.declare_parameter('fire_ext_max_capacity', 5)
 
-        self._node.declare_parameter('max_tilt_deg', 15.)
+        self._node.declare_parameter('max_tilt_deg', 90.)
 
         self._lin_max = self._node.get_parameter('max_linear_speed').value
         self._ang_max = self._node.get_parameter('max_angular_speed').value
@@ -32,6 +32,8 @@ class UnityRobotController(RobotController):
         self._max_tilt = np.deg2rad(self._node.get_parameter('max_tilt_deg').value)
         self._FALL = False
 
+        self._LIGHTS = False
+
         fire_ext_max_capacity = self._node.get_parameter('fire_ext_max_capacity').value
 
         super().__init__(fire_ext_max_capacity = fire_ext_max_capacity)
@@ -46,6 +48,9 @@ class UnityRobotController(RobotController):
 
         self.fe_burst_srv = self._node.create_client(Trigger, 'fe_burst')
 
+        self.lights_srv = self._node.create_client(Trigger, '/unity/light_trigger')
+        self.fall_reset_srv = self._node.create_client(Trigger, '/unity/robot_reset_trigger')
+
         self.start_time = self.get_time_seconds() # TODO save it when first cmd is given
 
         # TODO create collision subscriber from Unity
@@ -92,7 +97,7 @@ class UnityRobotController(RobotController):
             self._node.get_logger().error("Fire extinguisher is out of fuel!")
             return False
 
-        self._node.get_logger().error("Fire extingusher burst is performing")
+        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
@@ -153,6 +158,32 @@ class UnityRobotController(RobotController):
             self.pub_status()
         self._FALL = fall
 
+    def send_lights_cmd(self):
+        future = self.lights_srv.call_async(Trigger.Request())
+        future.add_done_callback(self._lights_done_cb)
+
+
+    def _lights_done_cb(self, future):
+        result = future.result()
+        self._LIGHTS = not self._LIGHTS
+        self._node.get_logger().info(f"Lights is {'ON' if self._LIGHTS else 'OFF'}")
+
+    def send_fall_reset_cmd(self):
+        if self._FALL:
+            self._node.get_logger().info("Trying to reset robot...")
+            future = self.fall_reset_srv.call_async(Trigger.Request())
+            future.add_done_callback(self._fall_reset_done_cb)
+        else:
+            self._node.get_logger().error("Robot didnot fall yet")
+
+
+    def _fall_reset_done_cb(self, future):
+        result = future.result()
+        self._FALL = False
+        self._node.get_logger().info(f"Fall reset done {result.success} {result.message}")
+
+
+