ROS 1 Panther manager BehaviorTree (#145)

* BT manager node draft (#100)

* BT manager node draft

* rename package | add AsyncSpinner

* update package version

* ROS 1 manager bt add basic plugins (#101)

* add basic ROS service plugins

* improve logging and naming

* Ros1 manager bt create basic trees (#104)

* create basic trees and update plugins

* update README | small fixes

* change shutdown nodes to stateful

* change param naming

* fix shutdown hosts node

* small fixes

* Update panther_manager/include/panther_manager/moving_average.hpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/include/panther_manager/moving_average.hpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/include/panther_manager/shutdown_host_node.hpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/src/manager_bt_node.cpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/src/manager_bt_node.cpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/README.md

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/config/PantherManagerBT.xml

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/config/PantherManagerBT.xml

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/config/PantherManagerBT.xml

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/src/manager_bt_node.cpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* review fixes

* move nh | change set up tree method

* Update panther_manager/src/manager_bt_node.cpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/src/manager_bt_node.cpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/src/manager_bt_node.cpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/src/manager_bt_node.cpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/src/manager_bt_node.cpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/src/manager_bt_node.cpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/plugins/action/call_trigger_service_node.cpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/include/panther_manager/plugins/ros_service_node.hpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* review changes

* moving average initial value

* Update panther_manager/include/panther_manager/plugins/ros_service_node.hpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* use bat percent average

* update includes | unify sleep

* review fixes

* README draft

* moving average unique ptr

* Update panther_manager/include/panther_manager/manager_bt_node.hpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/include/panther_manager/manager_bt_node.hpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/plugins/action/call_set_led_animation_service_node.cpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/plugins/action/call_trigger_service_node.cpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/include/panther_manager/moving_average.hpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* review fixes

* fix moving avg | review fixes

---------

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* ros plugin | pass nh by reference (#116)

* ros plugin | pass nh by reference

* formating

* Ros1 manager bt signal shutdown (#117)

* add SignalShutdown node

* name type

* review fixes

* revert output port

* Ros1 bt shutdown nodes (#114)

* new shutdown structure

* small fixes

* remove old file

* small fix

* review fixes

* move Host class

* add fsm to ShutdownHost

* hosts list as shared_ptr

* change remove duplicates method

* remove shutdown_timeout bb entry

* Update panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/include/panther_manager/plugins/shutdown_host.hpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* review fixes

* Update panther_manager/include/panther_manager/plugins/shutdown_host.hpp

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* change ssh_execute_command

* fix

---------

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* update shutdown nodes (#120)

* update shutdown nodes

* review fixes

* review fix

* Ros1 manager bt update (#121)

* Light dimmer (#105)

* Light dimmer

* Update

* delete debug

* add brake

* fix

* formated

* battery_animation 3 stage

* fix

* add david suggestion

* Update panther_lights/src/animation/battery_animation.py

Co-authored-by: Dawid Kmak <[email protected]>

---------

Co-authored-by: aayli <[email protected]>
Co-authored-by: Dawid Kmak <[email protected]>

* add battery health info (#102)

* add health info

* Update README.md

* add logs

* add voltage mean count

* update republisher node

* fix typo

* Update panther_battery/src/adc_node.py

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update adc_node.py

* Update roboteq_republisher_node.py

* fix typo

---------

Co-authored-by: Dawid Kmak <[email protected]>
Co-authored-by: Krzysztof Wojciechowski <[email protected]>

* Update changelog

* 1.0.1

* fix bug (#112)

* fix bug

* Pawel sugestions

---------

Co-authored-by: aayli <[email protected]>

* Update changelog

* 1.0.2

* Ros1 motor enable srv (#113)

* add motors enable srv

* add reseting can callback times

* disable motors if script reset fails

* working solution

* fix

* IO initial state pth1.05

* remove unneccessary method

* fix type

* update README

* Update panther_power_control/README.md

Co-authored-by: Paweł Kowalski <[email protected]>

* review changes

---------

Co-authored-by: Paweł Kowalski <[email protected]>

* Update changelog

* 1.0.3

* Thread lock (#110)

* thread lock

* undo fcn naming

* few more lock

* revert nodes

* Paweł suggestions

* change

* e_stop

---------

Co-authored-by: aayli <[email protected]>

* Update changelog

* 1.0.4

* APA102 cpp implementation (#108)

* Use CPP for APA102

* Add color correction

* Utilise gpiod

* Remove ::

* Fix lights after tests

* Fix default pin number

* Add set brightness service

* Invert LED_SBC_SEL pin

* Change SPI mode

* Change clock polarity

* Reduce number of ROS lgger instances

* Celan up code, update to libgpiod 2.0

* Update panther_lights/include/panther_lights/apa102.hpp

Co-authored-by: Dawid Kmak <[email protected]>

* Update panther_lights/src/lights_driver_node.cpp

Co-authored-by: Dawid Kmak <[email protected]>

* Update panther_lights/src/lights_driver_node.cpp

Co-authored-by: Dawid Kmak <[email protected]>

* Update panther_lights/include/panther_lights/apa102.hpp

Co-authored-by: Dawid Kmak <[email protected]>

* Review changes

* Update panther_lights/src/apa102.cpp

Co-authored-by: Dawid Kmak <[email protected]>

* Review cleanup

* Add build dependencies

* FIx launchfile

* Update panther_lights/CMakeLists.txt

Co-authored-by: Dawid Kmak <[email protected]>

* Update panther_lights/CMakeLists.txt

Co-authored-by: Dawid Kmak <[email protected]>

* Update panther_lights/src/apa102.cpp

Co-authored-by: Dawid Kmak <[email protected]>

* Update panther_lights/src/driver_node.cpp

Co-authored-by: Dawid Kmak <[email protected]>

* Update panther_lights/src/driver_node.cpp

Co-authored-by: Dawid Kmak <[email protected]>

* Review changes

* Fix inlude order

* Fix references and APA102 constructor

* Add build dependencies for libgpiod v2

* Bring back libgpiod v1

---------

Co-authored-by: Dawid Kmak <[email protected]>

* Update changelog

* 1.0.5

* Rviz lights plugin (#111)

* basic plugin usage - POC

* add rear animation texture

* fix rviz config

* fix .rviz file

* Update changelog

* 1.0.6

* check can err (#119)

* Update changelog

* 1.0.7

* update bt project and launch files

* remove scheduler_node

* fix queuing

* remove old variable

---------

Co-authored-by: rafal-gorecki <[email protected]>
Co-authored-by: aayli <[email protected]>
Co-authored-by: Paweł Kowalski <[email protected]>
Co-authored-by: Krzysztof Wojciechowski <[email protected]>
Co-authored-by: action-bot <[email protected]>

* ROS 1 manager bt dosc (#126)

* update docs

* update docs

* ad medyfying bt section

* update lights tree png

* update README

* Update panther_manager/README.md

Co-authored-by: Paweł Kowalski <[email protected]>

* Update panther_manager/README.md

Co-authored-by: Paweł Kowalski <[email protected]>

* Update panther_manager/README.md

Co-authored-by: Paweł Kowalski <[email protected]>

* Update panther_manager/README.md

Co-authored-by: Paweł Kowalski <[email protected]>

* Update panther_manager/README.md

Co-authored-by: Paweł Kowalski <[email protected]>

* Update panther_manager/README.md

Co-authored-by: Paweł Kowalski <[email protected]>

* review fixes

---------

Co-authored-by: Paweł Kowalski <[email protected]>

* add battery health check (#141)

* add battery health check

* update readme

* fix typo

* fix args

* Ros1 update bt images (#143)

* add battery health check

* update readme

* fix typo

* fix args

* update README imgs

* update README

* merge fix

* Update fan on/off thresholds (#146)

* install BT from rosdep

* fix rosdep install

* fix manager linking

---------

Co-authored-by: Krzysztof Wojciechowski <[email protected]>
Co-authored-by: rafal-gorecki <[email protected]>
Co-authored-by: aayli <[email protected]>
Co-authored-by: Paweł Kowalski <[email protected]>
Co-authored-by: action-bot <[email protected]>

panther_battery/README.md

@@ -32,4 +32,4 @@ Node publishing Panther battery state read from motor controllers. Used in Panth
 
 #### Parameters 
 
-- `~high_bat_temp` [*float*, default: **55.0**]: The temperature of the battery at which the battery health state is incorrect.
+- `~fatal_bat_temp` [*float*, default: **62.0**]: The temperature of the battery at which the battery health state is incorrect.

panther_battery/src/adc_node.py

@@ -23,7 +23,7 @@ class ADCNode:
     def __init__(self, name: str) -> None:
         rospy.init_node(name, anonymous=False)
 
-        self._high_bat_temp = rospy.get_param('~high_bat_temp', 55.0)
+        self._high_bat_temp = rospy.get_param('~fatal_bat_temp', 62.0)
 
         self._driver_battery_last_info_time: Optional[float] = None
         self._I_driv: Optional[float] = None

panther_bringup/launch/bringup.launch

@@ -6,7 +6,8 @@
   <arg name="panther_common_config_file" default="$(find panther_bringup)/config/panther_common.yaml" />
   <arg name="test_animations" default="false" />
   <arg name="user_animations_file" default="" />
-  <arg name="launch_lights_scheduler" default="true" />
+  <arg name="bt_project_file" default="$(find panther_manager)/config/Panther12BT.btproj" if="$(eval panther_version >= 1.2)" />
+  <arg name="bt_project_file" default="$(find panther_manager)/config/Panther106BT.btproj" unless="$(eval panther_version >= 1.2)" />
   <arg name="shutdown_hosts_config_file" default="$(find panther_bringup)/config/shutdown_hosts.yaml" />
   <arg name="disable_manager" default="false" doc="used for testing purposes" />
   <arg name="exit_on_wrong_hw" default="true" doc="kill ROS when incorrect hardware detected" />
@@ -20,6 +21,16 @@
   <arg name="wheel_config_file" value="$(find panther_description)/config/WH04.yaml"
     if="$(eval wheel_type == 'WH04')" />
 
+  <arg name="robot_description" if="$(arg publish_robot_state)"
+    default="$(find xacro)/xacro $(find panther_description)/urdf/panther.urdf.xacro
+              wheel_config_path:=$(arg wheel_config_file)
+              imu_pos_x:=$(optenv PANTHER_IMU_LOCALIZATION_X 0.168)
+              imu_pos_y:=$(optenv PANTHER_IMU_LOCALIZATION_Y 0.028)
+              imu_pos_z:=$(optenv PANTHER_IMU_LOCALIZATION_Z 0.083)
+              imu_rot_r:=$(optenv PANTHER_IMU_ORIENTATION_R 3.14)
+              imu_rot_p:=$(optenv PANTHER_IMU_ORIENTATION_P -1.57)
+              imu_rot_y:=$(optenv PANTHER_IMU_ORIENTATION_Y 0.0)" />
+
   <!-- load default params -->
   <rosparam command="load" file="$(find panther_bringup)/config/panther_default.yaml" />
   <!-- override default params -->
@@ -30,7 +41,7 @@
 
   <!-- Do not start ROS packages if hardware is detected to be incorrect -->
   <group if="$(optenv PANTHER_HW_CONFIG_CORRECT false)">
-    
+
     <arg name="robot_description" if="$(arg publish_robot_state)"
       default="$(find xacro)/xacro $(find panther_description)/urdf/panther.urdf.xacro
                 wheel_config_path:=$(find panther_description)/config/$(arg wheel_type).yaml
@@ -50,7 +61,6 @@
       </include>
 
       <include file="$(find panther_lights)/launch/lights.launch">
-        <arg name="launch_scheduler" value="$(arg launch_lights_scheduler)" />
         <arg name="test" value="$(arg test_animations)" />
         <arg name="user_animations_file" value="$(arg user_animations_file)" />
       </include>
@@ -63,13 +73,14 @@
         <arg name="panther_version" value="$(arg panther_version)" />
       </include>
 
-      <include file="$(find panther_manager)/launch/manager.launch" unless="$(arg disable_manager)">
+      <include file="$(find panther_manager)/launch/manager_bt.launch" unless="$(arg disable_manager)">
         <arg name="panther_version" value="$(arg panther_version)" />
+        <arg name="bt_project_file" value="$(arg bt_project_file)" />
         <arg name="shutdown_hosts_config_file" value="$(arg shutdown_hosts_config_file)" />
       </include>
 
       <include file="$(find panther_bringup)/launch/imu.launch" />
-      
+
       <include file="$(find panther_bringup)/launch/ekf.launch" if="$(arg use_ekf)" />
 
     </group>
@@ -80,4 +91,4 @@
 
   </group>
 
-</launch>
+</launch>

panther_lights/README.md

@@ -52,29 +52,6 @@ This node is responsible for displaying frames on the Husarion Panther robot LED
 - `~global_brightness` [*float*, default: **1.0**]: LED global brightness. Range between [0,1].
 - `~num_led` [*int*, default: **46**]: number of LEDs in a single panel.
 
-### scheduler_node.py
-
-This node is responsible for scheduling animations displayed on LED panels based on the Husarion Panther robot's system state.
-
-#### Subscribes
-
-- `/panther/battery` [*sensor_msgs/BatteryState*]: robot battery state.
-- `/panther/hardware/e_stop` [*std_msgs/Bool*]: informs if robot is in emergency stop state.
-
-#### Services subscribed
-
-- `/panther/lights/controller/set/animation` [*panther_msgs/SetLEDAnimation*]: allows setting animation on LED panel based on animation ID.
-
-#### Parameters
-
-- `~battery_state_anim_period` [*float*, default: **120.0**]: time in seconds to wait before repeating animation representing current battery percentage.
-- `~charging_battery_anim_period` [*float*, default: **20.0**]: time in seconds to wait before updating the charging battery animation if the battery percentage has changed by the value specified in the `update_charging_anim_step` param.
-- `~critical_battery_anim_period` [*float*, default: **15.0**]: time in seconds to wait before repeating animation indicating a critical battery state.
-- `~critical_battery_threshold_percent` [*float*, default: **0.1**]: if battery percentage drops below this value, animation indicating a critical battery state will start being displayed.
-- `~low_battery_anim_period` [*float*, default: **30.0**]: time in seconds to wait before repeating animation indicating a low battery state.
-- `~low_battery_threshold_percent` [*float*, default: **0.4**]: if the battery percentage drops below this value, animation indicating a low battery state will start being displayed.
-- `~update_charging_anim_step` [*float*, default: **0.1**]: percentage value representing a step for updating the charging battery animation.
-
 ## Animations
 
 Basic animations provided by Husarion are loaded upon node start from [`panther_lights_animations.yaml`](config/panther_lights_animations.yaml) and parsed as a list using the ROS parameter. Supported keys are:

panther_lights/launch/lights.launch

@@ -1,5 +1,4 @@
 <launch>
-  <arg name="launch_scheduler" default="true" />
   <arg name="test" default="false" />
   <arg name="user_animations_file" default="" />
 
@@ -14,7 +13,4 @@
     <param name="test" value="$(arg test)" />
   </node>
 
-  <node pkg="panther_lights" type="scheduler_node.py" name="lights_scheduler_node"
-    required="true" output="screen" if="$(arg launch_scheduler)" />
-
 </launch>

panther_lights/src/controller_node.py

@@ -304,7 +304,7 @@ def _rgb_frame_to_img_msg(self, rgb_frame: list, brightness: int, frame_id: str)
     def _add_animation_to_queue(self, animation: PantherAnimation) -> None:
         if animation.repeating:
             interupting_animation = deepcopy(animation)
-            interupting_animation.init_time = float('inf')
+            interupting_animation.init_time -= 0.0001
             if interupting_animation.priority > 2:
                 interupting_animation.priority = 2
             self._anim_queue.put(interupting_animation)