ROS 1 Panther manager BehaviorTree

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)