Robot States

README.md

@@ -102,8 +102,10 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | 🤖🖥️  | `namespace`                  | Add namespace to all launched nodes. <br/> ***string:*** `env(ROBOT_NAMESPACE)`                                                                                                                                                                                                                                                                               |
 | 🤖🖥️  | `publish_robot_state`        | Whether to publish the default Panther robot description. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                |
 | 🖥️   | `robots`                     | The list of the robots spawned in the simulation e.g. `robots:='robot1={x: 1.0, y: -2.0}; robot2={x: 1.0, y: -4.0}'` <br/> ***string:*** `''`                                                                                                                                                                                                                  |
+| 🤖🖥️  | `robot_states_bt_project_path` | Path to BehaviorTree project file, responsible for robot states management. <br/> ***string:*** [`RobotStatesBT.btproj`](./panther_manager/behavior_trees/RobotStatesBT.btproj)                                                                                                                                                                   |
 | 🤖🖥️  | `safety_bt_project_path`     | Path to BehaviorTree project file, responsible for safety and shutdown management. <br/> ***string:*** [`PantherSafetyBT.btproj`](./panther_manager/behavior_trees/PantherSafetyBT.btproj)                                                                                                                                                                   |
 | 🤖🖥️  | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown. <br/> ***string:*** [`shutdown_hosts_config.yaml`](./panther_manager/config/shutdown_hosts_config.yaml)                                                                                                                                                                                                               |
+| 🤖🖥️  | `use_docking`                | Enable docking server. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                                                   |
 | 🤖🖥️  | `use_ekf`                    | Enable or disable EKF. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                                                   |
 | 🤖🖥️  | `use_sim`                    | Whether simulation is used. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                                             |
 | 🤖🖥️  | `user_led_animations_file`   | Path to a YAML file with a description of the user-defined animations. <br/> ***string:*** `''`                                                                                                                                                                                                                                                             |

panther/panther_hardware.repos

@@ -10,7 +10,7 @@ repositories:
   panther_msgs:
     type: git
     url: https://github.com/husarion/panther_msgs.git
-    version: fcee4d9f249a62adc113eb80be4885b08024ee9c
+    version: 6eb04e3a8d30e9700b4600d0ad30415bc70fd6f9
   ros_components_description:
     type: git
     url: https://github.com/husarion/ros_components_description.git

panther/panther_simulation.repos

@@ -10,7 +10,7 @@ repositories:
   panther_msgs:
     type: git
     url: https://github.com/husarion/panther_msgs.git
-    version: fcee4d9f249a62adc113eb80be4885b08024ee9c
+    version: 6eb04e3a8d30e9700b4600d0ad30415bc70fd6f9
   ros_components_description:
     type: git
     url: https://github.com/husarion/ros_components_description.git

panther_docking/launch/docking.launch.py

@@ -13,8 +13,8 @@
 # limitations under the License.
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
 from launch.substitutions import (
     EnvironmentVariable,
     LaunchConfiguration,
@@ -26,12 +26,13 @@
 
 
 def generate_launch_description():
-    use_sim = LaunchConfiguration("use_sim")
-    declare_use_sim_arg = DeclareLaunchArgument(
-        "use_sim",
-        default_value="False",
-        description="Whether simulation is used",
-        choices=[True, False, "True", "False", "true", "false", "1", "0"],
+    docking_server_config_path = LaunchConfiguration("docking_server_config_path")
+    declare_docking_server_config_path_arg = DeclareLaunchArgument(
+        "docking_server_config_path",
+        default_value=PathJoinSubstitution(
+            [FindPackageShare("panther_docking"), "config", "panther_docking_server.yaml"]
+        ),
+        description=("Path to docking server configuration file."),
     )
 
     namespace = LaunchConfiguration("namespace")
@@ -41,13 +42,20 @@ def generate_launch_description():
         description="Add namespace to all launched nodes.",
     )
 
-    docking_server_config_path = LaunchConfiguration("docking_server_config_path")
-    declare_docking_server_config_path_arg = DeclareLaunchArgument(
-        "docking_server_config_path",
-        default_value=PathJoinSubstitution(
-            [FindPackageShare("panther_docking"), "config", "panther_docking_server.yaml"]
-        ),
-        description=("Path to docking server configuration file."),
+    use_docking = LaunchConfiguration("use_docking")
+    declare_use_docking_arg = DeclareLaunchArgument(
+        "use_docking",
+        default_value="True",
+        description="Enable docking server.",
+        choices=["True", "False", "true", "false"],
+    )
+
+    use_sim = LaunchConfiguration("use_sim")
+    declare_use_sim_arg = DeclareLaunchArgument(
+        "use_sim",
+        default_value="False",
+        description="Whether simulation is used.",
+        choices=["True", "False", "true", "false"],
     )
 
     namespaced_docking_server_config = ReplaceString(
@@ -58,18 +66,21 @@ def generate_launch_description():
     docking_server_node = Node(
         package="opennav_docking",
         executable="opennav_docking",
+        namespace=namespace,
         parameters=[
             namespaced_docking_server_config,
             {"use_sim_time": use_sim},
         ],
-        namespace=namespace,
+        remappings=[("~/transition_event", "~/_transition_event")],
         emulate_tty=True,
+        condition=IfCondition(use_docking),
     )
 
     docking_server_activate_node = Node(
         package="nav2_lifecycle_manager",
         executable="lifecycle_manager",
         name="nav2_docking_lifecycle_manager",
+        namespace=namespace,
         parameters=[
             {
                 "autostart": True,
@@ -79,28 +90,15 @@ def generate_launch_description():
                 "use_sim_time": use_sim,
             },
         ],
-        namespace=namespace,
-    )
-
-    station_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    FindPackageShare("panther_docking"),
-                    "launch",
-                    "station.launch.py",
-                ]
-            ),
-        ),
-        launch_arguments={"namespace": namespace}.items(),
+        condition=IfCondition(use_docking),
     )
 
     return LaunchDescription(
         [
-            declare_use_sim_arg,
-            declare_namespace_arg,
             declare_docking_server_config_path_arg,
-            station_launch,
+            declare_namespace_arg,
+            declare_use_docking_arg,
+            declare_use_sim_arg,
             docking_server_node,
             docking_server_activate_node,
         ]

panther_gazebo/launch/simulate_robot.launch.py

@@ -185,6 +185,19 @@ def generate_launch_description():
         emulate_tty=True,
     )
 
+    docking_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("panther_docking"),
+                    "launch",
+                    "docking.launch.py",
+                ]
+            ),
+        ),
+        launch_arguments={"namespace": namespace, "use_sim": "True"}.items(),
+    )
+
     return LaunchDescription(
         [
             declare_battery_config_path_arg,
@@ -200,5 +213,6 @@ def generate_launch_description():
             ekf_launch,
             simulate_components,
             gz_bridge,
+            docking_launch,
         ]
     )

panther_gazebo/package.xml

@@ -34,6 +34,7 @@
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">nav2_common</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_controller</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_description</exec_depend>
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_docking</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_lights</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_localization</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_manager</exec_depend>

panther_manager/CMakeLists.txt

@@ -11,14 +11,17 @@ set(PACKAGE_DEPENDENCIES
     behaviortree_cpp
     behaviortree_ros2
     libssh
+    geometry_msgs
+    opennav_docking_msgs
     panther_msgs
     panther_utils
     rclcpp
     rclcpp_action
     sensor_msgs
+    std_msgs
     std_srvs
-    yaml-cpp
-    opennav_docking_msgs)
+    tf2_geometry_msgs
+    yaml-cpp)
 
 foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES})
   find_package(${PACKAGE} REQUIRED)
@@ -59,9 +62,13 @@ add_library(undock_robot_bt_node SHARED
             src/plugins/action/undock_robot_node.cpp)
 list(APPEND plugin_libs undock_robot_bt_node)
 
-add_library(are_buttons_pressed_bt_node SHARED
-            src/plugins/condition/are_buttons_pressed.cpp)
-list(APPEND plugin_libs are_buttons_pressed_bt_node)
+add_library(check_bool_msg_bt_node SHARED
+            src/plugins/condition/check_bool_msg.cpp)
+list(APPEND plugin_libs check_bool_msg_bt_node)
+
+add_library(check_joy_msg_bt_node SHARED
+            src/plugins/condition/check_joy_msg.cpp)
+list(APPEND plugin_libs check_joy_msg_bt_node)
 
 add_library(tick_after_timeout_bt_node SHARED
             src/plugins/decorator/tick_after_timeout_node.cpp)
@@ -73,16 +80,19 @@ foreach(bt_plugin ${plugin_libs})
 endforeach()
 
 add_executable(
-  safety_manager_node src/safety_manager_node_main.cpp
-                      src/safety_manager_node.cpp src/behavior_tree_manager.cpp)
+  safety_manager_node
+  src/safety_manager_node_main.cpp src/safety_manager_node.cpp
+  src/behavior_tree_manager.cpp src/behavior_tree_manager.cpp)
 ament_target_dependencies(
   safety_manager_node
   behaviortree_ros2
+  geometry_msgs
   panther_msgs
   panther_utils
   rclcpp
   sensor_msgs
-  std_msgs)
+  std_msgs
+  tf2_geometry_msgs)
 target_link_libraries(safety_manager_node ${plugin_libs})
 
 add_executable(
@@ -91,17 +101,35 @@ add_executable(
 ament_target_dependencies(
   lights_manager_node
   behaviortree_ros2
+  geometry_msgs
   panther_msgs
   panther_utils
   rclcpp
   sensor_msgs
-  std_msgs)
+  std_msgs
+  tf2_geometry_msgs)
 target_link_libraries(lights_manager_node ${plugin_libs})
 
+add_executable(
+  robot_states_manager_node
+  src/robot_states_manager_node_main.cpp src/robot_states_manager_node.cpp
+  src/behavior_tree_manager.cpp)
+ament_target_dependencies(
+  robot_states_manager_node
+  behaviortree_ros2
+  geometry_msgs
+  panther_msgs
+  panther_utils
+  rclcpp
+  sensor_msgs
+  std_msgs
+  tf2_geometry_msgs)
+target_link_libraries(robot_states_manager_node ${plugin_libs})
+
 install(TARGETS ${plugin_libs} DESTINATION lib)
 
 install(TARGETS safety_manager_node lights_manager_node
-        DESTINATION lib/${PROJECT_NAME})
+                robot_states_manager_node DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY behavior_trees config launch
         DESTINATION share/${PROJECT_NAME})
@@ -161,10 +189,16 @@ if(BUILD_TESTING)
   list(APPEND plugin_tests ${PROJECT_NAME}_test_undock_robot_node)
 
   ament_add_gtest(
-    ${PROJECT_NAME}_test_are_buttons_pressed
-    test/plugins/condition/test_are_buttons_pressed.cpp
-    src/plugins/condition/are_buttons_pressed.cpp)
-  list(APPEND plugin_tests ${PROJECT_NAME}_test_are_buttons_pressed)
+    ${PROJECT_NAME}_test_check_bool_msg
+    test/plugins/condition/test_check_bool_msg.cpp
+    src/plugins/condition/check_bool_msg.cpp)
+  list(APPEND plugin_tests ${PROJECT_NAME}_test_check_bool_msg)
+
+  ament_add_gtest(
+    ${PROJECT_NAME}_test_check_joy_msg
+    test/plugins/condition/test_check_joy_msg.cpp
+    src/plugins/condition/check_joy_msg.cpp)
+  list(APPEND plugin_tests ${PROJECT_NAME}_test_check_joy_msg)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_tick_after_timeout_node
@@ -197,8 +231,9 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_behavior_tree_utils
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
            $<INSTALL_INTERFACE:include>)
-  ament_target_dependencies(${PROJECT_NAME}_test_behavior_tree_utils
-                            behaviortree_cpp behaviortree_ros2 panther_utils)
+  ament_target_dependencies(
+    ${PROJECT_NAME}_test_behavior_tree_utils behaviortree_cpp behaviortree_ros2
+    geometry_msgs panther_utils tf2_geometry_msgs)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_behavior_tree_manager
@@ -221,10 +256,12 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_lights_manager_node
     behaviortree_cpp
     behaviortree_ros2
+    geometry_msgs
     panther_msgs
     panther_utils
     rclcpp
     sensor_msgs
+    tf2_geometry_msgs
     std_msgs)
 
   ament_add_gtest(
@@ -239,11 +276,13 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_lights_behavior_tree
     behaviortree_cpp
     behaviortree_ros2
+    geometry_msgs
     panther_msgs
     panther_utils
     rclcpp
     sensor_msgs
-    std_msgs)
+    std_msgs
+    tf2_geometry_msgs)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_safety_manager_node test/test_safety_manager_node.cpp
@@ -256,11 +295,13 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_safety_manager_node
     behaviortree_cpp
     behaviortree_ros2
+    geometry_msgs
     panther_msgs
     panther_utils
     rclcpp
     sensor_msgs
-    std_msgs)
+    std_msgs
+    tf2_geometry_msgs)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_safety_behavior_tree
@@ -274,12 +315,14 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_safety_behavior_tree
     behaviortree_cpp
     behaviortree_ros2
+    geometry_msgs
     panther_msgs
     panther_utils
     rclcpp
     sensor_msgs
     std_msgs
-    std_srvs)
+    std_srvs
+    tf2_geometry_msgs)
 endif()
 
 ament_package()

panther_manager/CONFIGURATION.md

@@ -206,6 +206,7 @@ To use your customized project, you can modify the `bt_project_file` ROS paramet
 
 Groot2 also provides a real-time visualization tool that allows you to see and debug actively running trees. To use this tool with trees launched with the `panther_manager` package, you need to specify the port associated with the tree you want to visualize. The ports for each tree are listed below:
 
-- Lights tree: `10.15.20.2:5555`
-- Safety tree: `10.15.20.2:6666`
-- Shutdown tree: `10.15.20.2:7777`
+- Lights tree: `10.15.20.2:5550`
+- RobotState tree: `10.15.20.2:5555`
+- Safety tree: `10.15.20.2:5560`
+- Shutdown tree: `10.15.20.2:5565`