Fixed after merge

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>
husarion · Dec 12, 2024 · 863c835 · 863c835
1 parent 624d8ad
commit 863c835
Show file tree

Hide file tree

Showing 19 changed files with 405 additions and 92 deletions.
diff --git a/husarion_ugv_manager/CMakeLists.txt b/husarion_ugv_manager/CMakeLists.txt
@@ -85,11 +85,13 @@ add_executable(safety_manager_node src/safety_manager_node_main.cpp
 ament_target_dependencies(
   safety_manager_node
   behaviortree_ros2
-  panther_msgs
+  geometry_msgs
   husarion_ugv_utils
+  panther_msgs
   rclcpp
   sensor_msgs
-  std_msgs)
+  std_msgs
+  tf2_geometry_msgs)
 
 generate_parameter_library(safety_manager_parameters
                            config/safety_manager_parameters.yaml)
@@ -101,11 +103,13 @@ add_executable(lights_manager_node src/lights_manager_node_main.cpp
 ament_target_dependencies(
   lights_manager_node
   behaviortree_ros2
-  panther_msgs
+  geometry_msgs
   husarion_ugv_utils
+  panther_msgs
   rclcpp
   sensor_msgs
-  std_msgs)
+  std_msgs
+  tf2_geometry_msgs)
 
 generate_parameter_library(lights_manager_parameters
                            config/lights_manager_parameters.yaml)
@@ -222,7 +226,7 @@ if(BUILD_TESTING)
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(
     ${PROJECT_NAME}_test_behavior_tree_utils behaviortree_cpp behaviortree_ros2
-    husarion_ugv_utils)
+    husarion_ugv_utils geometry_msgs tf2_geometry_msgs)
 
   ament_add_gtest(${PROJECT_NAME}_test_behavior_tree_manager
                   test/test_behavior_tree_manager.cpp)
@@ -243,11 +247,13 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_lights_manager_node
     behaviortree_cpp
     behaviortree_ros2
-    panther_msgs
+    geometry_msgs
     husarion_ugv_utils
+    panther_msgs
     rclcpp
     sensor_msgs
-    std_msgs)
+    std_msgs
+    tf2_geometry_msgs)
   target_link_libraries(${PROJECT_NAME}_test_lights_manager_node
                         lights_manager_parameters)
 
@@ -262,11 +268,13 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_lights_behavior_tree
     behaviortree_cpp
     behaviortree_ros2
-    panther_msgs
+    geometry_msgs
     husarion_ugv_utils
+    panther_msgs
     rclcpp
     sensor_msgs
-    std_msgs)
+    std_msgs
+    tf2_geometry_msgs)
   target_link_libraries(${PROJECT_NAME}_test_lights_behavior_tree
                         lights_manager_parameters)
 
@@ -280,11 +288,13 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_safety_manager_node
     behaviortree_cpp
     behaviortree_ros2
-    panther_msgs
+    geometry_msgs
     husarion_ugv_utils
+    panther_msgs
     rclcpp
     sensor_msgs
-    std_msgs)
+    std_msgs
+    tf2_geometry_msgs)
   target_link_libraries(${PROJECT_NAME}_test_safety_manager_node
                         safety_manager_parameters)
 
@@ -299,12 +309,14 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_safety_behavior_tree
     behaviortree_cpp
     behaviortree_ros2
-    panther_msgs
+    geometry_msgs
     husarion_ugv_utils
+    panther_msgs
     rclcpp
     sensor_msgs
     std_msgs
-    std_srvs)
+    std_srvs
+    tf2_geometry_msgs)
   target_link_libraries(${PROJECT_NAME}_test_safety_behavior_tree
                         safety_manager_parameters)
 endif()

diff --git a/husarion_ugv_manager/config/lights_manager_parameters.yaml b/husarion_ugv_manager/config/lights_manager_parameters.yaml
@@ -44,6 +44,12 @@ lights_manager:
     default_value: ""
     description: Path to a BehaviorTree project.
 
+  bt_server_port:
+    type: int
+    default_value: 5555
+    description: Port number for the BehaviorTree server.
+    validation: { gt<>: 0 }
+
   plugin_libs:
     type: string_array
     default_value: []

diff --git a/husarion_ugv_manager/config/safety_manager.yaml b/husarion_ugv_manager/config/safety_manager.yaml
@@ -2,6 +2,7 @@
   safety_manager:
     ros__parameters:
       timer_frequency: 10.0
+      bt_server_port: 6666
       fan_turn_off_timeout: 60.0
       battery:
         temp:

diff --git a/husarion_ugv_manager/config/safety_manager_parameters.yaml b/husarion_ugv_manager/config/safety_manager_parameters.yaml
@@ -12,6 +12,12 @@ safety_manager:
     default_value: ""
     description: Path to a BehaviorTree project.
 
+  bt_server_port:
+    type: int
+    default_value: 6666
+    description: Port number for the BehaviorTree server.
+    validation: { gt<>: 0 }
+
   cpu:
     temp:
       fan_off:

diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/dock_robot_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/dock_robot_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_
-#define PANTHER_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_
+#ifndef HUSARION_UGV_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_
+#define HUSARION_UGV_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_
 
 #include <memory>
 #include <string>
@@ -24,7 +24,7 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <opennav_docking_msgs/action/dock_robot.hpp>
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 class DockRobot : public BT::RosActionNode<opennav_docking_msgs::action::DockRobot>
@@ -76,6 +76,6 @@ class DockRobot : public BT::RosActionNode<opennav_docking_msgs::action::DockRob
   }
 };
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
-#endif  // PANTHER_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_
+#endif  // HUSARION_UGV_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_
diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/undock_robot_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/undock_robot_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_MANAGER_PLUGINS_ACTION_UNDOCK_ROBOT_NODE_HPP_
-#define PANTHER_MANAGER_PLUGINS_ACTION_UNDOCK_ROBOT_NODE_HPP_
+#ifndef HUSARION_UGV_MANAGER_PLUGINS_ACTION_UNDOCK_ROBOT_NODE_HPP_
+#define HUSARION_UGV_MANAGER_PLUGINS_ACTION_UNDOCK_ROBOT_NODE_HPP_
 
 #include <memory>
 #include <string>
@@ -24,7 +24,7 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <opennav_docking_msgs/action/undock_robot.hpp>
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 class UndockRobot : public BT::RosActionNode<opennav_docking_msgs::action::UndockRobot>
@@ -61,6 +61,6 @@ class UndockRobot : public BT::RosActionNode<opennav_docking_msgs::action::Undoc
   }
 };
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
-#endif  // PANTHER_MANAGER_PLUGINS_ACTION_UNDOCK_ROBOT_NODE_HPP_
+#endif  // HUSARION_UGV_MANAGER_PLUGINS_ACTION_UNDOCK_ROBOT_NODE_HPP_
diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_bool_msg.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_bool_msg.hpp
@@ -24,9 +24,9 @@
 
 #include <std_msgs/msg/bool.hpp>
 
-#include "panther_manager/behavior_tree_utils.hpp"
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 // FIXME: There is no possibility to set QoS profile. Add it in the future to subscribe e_stop.
@@ -50,6 +50,6 @@ class CheckBoolMsg : public BT::RosTopicSubNode<std_msgs::msg::Bool>
   }
 };
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
 #endif  // PANTHER_MANAGER_PLUGINS_CONDITION_CHECK_BOOL_MSG_HPP_
diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_joy_msg.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_joy_msg.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_MANAGER_PLUGINS_CONDITION_CHECK_JOY_MSG_HPP_
-#define PANTHER_MANAGER_PLUGINS_CONDITION_CHECK_JOY_MSG_HPP_
+#ifndef HUSARION_UGV_MANAGER_PLUGINS_CONDITION_CHECK_JOY_MSG_HPP_
+#define HUSARION_UGV_MANAGER_PLUGINS_CONDITION_CHECK_JOY_MSG_HPP_
 
 #include <memory>
 #include <mutex>
@@ -24,9 +24,9 @@
 
 #include <sensor_msgs/msg/joy.hpp>
 
-#include "panther_manager/behavior_tree_utils.hpp"
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 class CheckJoyMsg : public BT::RosTopicSubNode<sensor_msgs::msg::Joy>
@@ -63,6 +63,6 @@ class CheckJoyMsg : public BT::RosTopicSubNode<sensor_msgs::msg::Joy>
   bool checkTimeout(const JoyMsg::SharedPtr & last_msg);
 };
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
-#endif  // PANTHER_MANAGER_PLUGINS_CONDITION_CHECK_JOY_MSG_HPP_
+#endif  // HUSARION_UGV_MANAGER_PLUGINS_CONDITION_CHECK_JOY_MSG_HPP_
diff --git a/husarion_ugv_manager/src/plugins/action/dock_robot_node.cpp b/husarion_ugv_manager/src/plugins/action/dock_robot_node.cpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_manager/plugins/action/dock_robot_node.hpp"
-#include "panther_manager/behavior_tree_utils.hpp"
+#include "husarion_ugv_manager/plugins/action/dock_robot_node.hpp"
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 bool DockRobot::setGoal(Goal & goal)
@@ -83,7 +83,7 @@ void DockRobot::onHalt()
   RCLCPP_INFO_STREAM(this->logger(), GetLoggerPrefix(name()) << ": onHalt");
 }
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
 #include "behaviortree_ros2/plugins.hpp"
-CreateRosNodePlugin(panther_manager::DockRobot, "DockRobot");
+CreateRosNodePlugin(husarion_ugv_manager::DockRobot, "DockRobot");
diff --git a/husarion_ugv_manager/src/plugins/action/undock_robot_node.cpp b/husarion_ugv_manager/src/plugins/action/undock_robot_node.cpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_manager/plugins/action/undock_robot_node.hpp"
-#include "panther_manager/behavior_tree_utils.hpp"
+#include "husarion_ugv_manager/plugins/action/undock_robot_node.hpp"
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 bool UndockRobot::setGoal(Goal & goal)
@@ -60,7 +60,7 @@ void UndockRobot::onHalt()
   RCLCPP_INFO_STREAM(this->logger(), GetLoggerPrefix(name()) << ": onHalt");
 }
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
 #include "behaviortree_ros2/plugins.hpp"
-CreateRosNodePlugin(panther_manager::UndockRobot, "UndockRobot");
+CreateRosNodePlugin(husarion_ugv_manager::UndockRobot, "UndockRobot");
diff --git a/husarion_ugv_manager/src/plugins/condition/check_bool_msg.cpp b/husarion_ugv_manager/src/plugins/condition/check_bool_msg.cpp
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_manager/plugins/condition/check_bool_msg.hpp"
+#include "husarion_ugv_manager/plugins/condition/check_bool_msg.hpp"
 
-#include "panther_manager/behavior_tree_utils.hpp"
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 BT::NodeStatus CheckBoolMsg::onTick(const BoolMsg::SharedPtr & last_msg)
@@ -28,7 +28,7 @@ BT::NodeStatus CheckBoolMsg::onTick(const BoolMsg::SharedPtr & last_msg)
                                                        : BT::NodeStatus::FAILURE;
 }
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
 #include "behaviortree_ros2/plugins.hpp"
-CreateRosNodePlugin(panther_manager::CheckBoolMsg, "CheckBoolMsg");
+CreateRosNodePlugin(husarion_ugv_manager::CheckBoolMsg, "CheckBoolMsg");
diff --git a/husarion_ugv_manager/src/plugins/condition/check_joy_msg.cpp b/husarion_ugv_manager/src/plugins/condition/check_joy_msg.cpp
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "panther_manager/plugins/condition/check_joy_msg.hpp"
+#include "husarion_ugv_manager/plugins/condition/check_joy_msg.hpp"
 
-#include "panther_manager/behavior_tree_utils.hpp"
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"
 
-namespace panther_manager
+namespace husarion_ugv_manager
 {
 
 BT::NodeStatus CheckJoyMsg::onTick(const JoyMsg::SharedPtr & last_msg)
@@ -86,7 +86,7 @@ bool CheckJoyMsg::checkTimeout(const JoyMsg::SharedPtr & last_msg)
   return (max_timeout <= 0.0) || time_diff < rclcpp::Duration::from_seconds(max_timeout);
 }
 
-}  // namespace panther_manager
+}  // namespace husarion_ugv_manager
 
 #include "behaviortree_ros2/plugins.hpp"
-CreateRosNodePlugin(panther_manager::CheckJoyMsg, "CheckJoyMsg");
+CreateRosNodePlugin(husarion_ugv_manager::CheckJoyMsg, "CheckJoyMsg");