Merge branch 'master' into feat/find_aruco

urc_bringup/launch/bt.launch.py

@@ -15,7 +15,7 @@ def generate_launch_description():
         "libbt_call_trigger.so",
         "libbt_follow_path.so",
         "libbt_update_current_pose.so",
-        "libbt_status_light_publisher.so"
+        "libbt_status_light_publisher.so",
     ]
     node_lib_path_base = os.path.join(
         Path(get_package_share_directory("urc_bt_nodes")).parent.parent.absolute(),
@@ -28,7 +28,7 @@ def generate_launch_description():
     ros_lib_paths = [
         os.path.join(node_lib_path_base, lib_name) for lib_name in ros_lib_names
     ]
-    bt_file_name = "bt_test.xml"
+    bt_file_name = "status_light.xml"
 
     enable_color = SetEnvironmentVariable(name="RCUTILS_COLORIZED_OUTPUT", value="1")
 

urc_bringup/launch/test_status_light.launch.py

@@ -0,0 +1,56 @@
+import os
+from xacro import process_file
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+from launch.actions import IncludeLaunchDescription, RegisterEventHandler
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.event_handlers import OnProcessExit
+
+
+def generate_launch_description():
+    pkg_urc_bringup = get_package_share_directory("urc_bringup")
+
+    controller_config_file_dir = os.path.join(
+        pkg_urc_bringup, "config", "controller_config.yaml"
+    )
+    xacro_file = os.path.join(
+        get_package_share_directory("urc_hw_description"), "urdf/walli.xacro"
+    )
+    assert os.path.exists(xacro_file), "urdf path doesnt exist in " + str(xacro_file)
+    robot_description_config = process_file(
+        xacro_file, mappings={"use_simulation": "false"}
+    )
+    robot_desc = robot_description_config.toxml()
+
+    control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[controller_config_file_dir, {"robot_description": robot_desc}],
+        output="both",
+    )
+
+    load_status_light_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["-p", controller_config_file_dir, "status_light_controller"],
+    )
+
+    bt_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_urc_bringup, "launch", "bt.launch.py")
+        )
+    )
+
+    return LaunchDescription(
+        [
+            control_node,
+            load_status_light_controller,
+            RegisterEventHandler(
+                event_handler=OnProcessExit(
+                    target_action=load_status_light_controller,
+                    on_exit=[bt_launch],
+                )
+            ),
+        ]
+    )

urc_bringup/strategies/status_light.xml

@@ -0,0 +1,51 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="test_status_light">
+    <RunOnce then_skip="true">
+      <Sequence>
+        <Delay delay_msec="100">
+          <StatusLightPublisher topic_name="/command/status_light"
+                                color="red"
+                                state="blink"/>
+        </Delay>
+        <Delay delay_msec="100">
+          <StatusLightPublisher topic_name="/command/status_light"
+                                color="red"
+                                state="off"/>
+        </Delay>
+        <Delay delay_msec="100">
+          <StatusLightPublisher topic_name="/command/status_light"
+                                color="green"
+                                state="blink"/>
+        </Delay>
+        <Delay delay_msec="100">
+          <StatusLightPublisher topic_name="/command/status_light"
+                                color="green"
+                                state="off"/>
+        </Delay>
+        <Delay delay_msec="100">
+          <StatusLightPublisher topic_name="/command/status_light"
+                                color="blue"
+                                state="blink"/>
+        </Delay>
+        <Delay delay_msec="100">
+          <StatusLightPublisher topic_name="/command/status_light"
+                                color="blue"
+                                state="off"/>
+        </Delay>
+      </Sequence>
+    </RunOnce>
+  </BehaviorTree>
+
+  <!-- Description of Node Models (used by Groot) -->
+  <TreeNodesModel>
+    <Action ID="StatusLightPublisher"
+            editable="true">
+      <input_port name="topic_name"
+                  default="/command/status_light"/>
+      <input_port name="color"/>
+      <input_port name="state"/>
+    </Action>
+  </TreeNodesModel>
+
+</root>

urc_bringup/strategies/urc_strategies.btproj

@@ -1,6 +1,7 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <root BTCPP_format="4" project_name="URC_Strategies">
     <include path="bt_test.xml"/>
+    <include path="status_light.xml"/>
     <include path="test_strategy.xml"/>
     <!-- Description of Node Models (used by Groot) -->
     <TreeNodesModel>
@@ -35,8 +36,9 @@
         <Action ID="RecoverFromSingularity" editable="true"/>
         <Action ID="ServoArm" editable="true"/>
         <Action ID="StatusLightPublisher" editable="true">
-            <input_port name="topic_name"/>
-            <input_port name="value"/>
+            <input_port name="topic_name" default="/command/status_light"/>
+            <input_port name="color"/>
+            <input_port name="state"/>
         </Action>
         <Action ID="UpdateCurrentPose" editable="true">
             <input_port name="topic_name"/>

urc_bt_nodes/include/urc_bt_nodes/actions/status_light_publisher.hpp

@@ -1,34 +1,65 @@
 #ifndef STATUS_LIGHT_PUBLISHER_HPP__
 #define STATUS_LIGHT_PUBLISHER_HPP__
 
+#include <string>
+
 #include "behaviortree_cpp/basic_types.h"
 #include "behaviortree_cpp/tree_node.h"
 #include "behaviortree_cpp/tree_node.h"
 #include "behaviortree_ros2/ros_node_params.hpp"
 #include "behaviortree_ros2/bt_topic_pub_node.hpp"
-#include "string"
-#include "std_msgs/msg/int8.hpp"
+
+#include "urc_msgs/msg/status_light_command.hpp"
 
 namespace behavior::actions
 {
-class StatusLightPublisher : public BT::RosTopicPubNode<std_msgs::msg::Int8>
+
+int stringToColor(std::string color)
+{
+  if (color == "red") {
+    return urc_msgs::msg::StatusLightCommand::RED;
+  }
+  if (color == "green") {
+    return urc_msgs::msg::StatusLightCommand::GREEN;
+  }
+  if (color == "blue") {
+    return urc_msgs::msg::StatusLightCommand::BLUE;
+  }
+
+  throw std::invalid_argument("Invalid color!");
+}
+
+int stringToState(std::string state)
+{
+  if (state == "on") {
+    return urc_msgs::msg::StatusLightCommand::ON;
+  }
+  if (state == "off") {
+    return urc_msgs::msg::StatusLightCommand::OFF;
+  }
+  if (state == "blink") {
+    return urc_msgs::msg::StatusLightCommand::BLINK;
+  }
+
+  throw std::invalid_argument("Invalid state!");
+}
+
+class StatusLightPublisher : public BT::RosTopicPubNode<urc_msgs::msg::StatusLightCommand>
 {
 public:
   StatusLightPublisher(
     const std::string & instance_name, const BT::NodeConfig & conf,
     const BT::RosNodeParams & params)
-  : BT::RosTopicPubNode<std_msgs::msg::Int8>(instance_name, conf, params) {}
+  : BT::RosTopicPubNode<urc_msgs::msg::StatusLightCommand>(instance_name, conf, params) {}
 
   static BT::PortsList providedPorts()
   {
     return providedBasicPorts(
-      {
-        BT::InputPort<std_msgs::msg::Int8>("value"),
-      });
+      {BT::InputPort<std::string>("color"),
+        BT::InputPort<std::string>("state")});
   }
 
-  bool setMessage(std_msgs::msg::Int8 & msg) override;
-
+  bool setMessage(urc_msgs::msg::StatusLightCommand & msg) override;
 };
 }
 

urc_bt_nodes/src/actions/status_light_publisher.cpp

@@ -4,24 +4,22 @@
 
 namespace behavior::actions
 {
-bool StatusLightPublisher::setMessage(std_msgs::msg::Int8 & msg)
+bool StatusLightPublisher::setMessage(urc_msgs::msg::StatusLightCommand & msg)
 {
-  msg = getInput<std_msgs::msg::Int8>("value").value();
+  std::string color = getInput<std::string>("color").value();
+  std::string state = getInput<std::string>("state").value();
+
+  try {
+    msg.color = stringToColor(color);
+    msg.state = stringToState(state);
+  } catch (std::invalid_argument & e) {
+    RCLCPP_ERROR(node_->get_logger(), "Invalid argument: %s", e.what());
+    return false;
+  }
+
   return true;
 }
 }
 
-namespace BT
-{
-template<>
-inline std_msgs::msg::Int8 convertFromString(StringView str)
-{
-  std_msgs::msg::Int8 output;
-  output.data = convertFromString<int8_t>(str);
-  return output;
-}
-} // namespace BT
-
-
 #include "behaviortree_ros2/plugins.hpp"
 CreateRosNodePlugin(behavior::actions::StatusLightPublisher, "StatusLightPublisher");

urc_controllers/include/urc_controllers/status_light_controller.hpp

@@ -21,6 +21,7 @@
 #include <sensor_msgs/msg/detail/imu__struct.hpp>
 #include "std_msgs/msg/int8.hpp"
 #include <std_msgs/msg/detail/int8__struct.hpp>
+#include <urc_msgs/msg/status_light_command.hpp>
 #include <string>
 #include <vector>
 
@@ -62,18 +63,18 @@ class StatusLightController : public controller_interface::ControllerInterface
 protected:
   // status_light related
   std::string status_light_name;
-  std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Int8>> color_command_subscriber_;
-  std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Int8>> display_command_subscriber_;
-  realtime_tools::RealtimeBuffer<double> color_command_;
-  realtime_tools::RealtimeBuffer<double> display_command_;
+  std::shared_ptr<rclcpp::Subscription<urc_msgs::msg::StatusLightCommand>>
+  status_light_command_subscriber_;
+  realtime_tools::RealtimeBuffer<uint8_t> color_command_;
+  realtime_tools::RealtimeBuffer<uint8_t> state_command_;
 
   // command interfaces
   std::unordered_map<std::string,
     std::shared_ptr<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>>
   command_interface_map;
-  const std::vector<std::string> STATUS_LIGHT_INTERFACES{"color", "display"};
+  const std::vector<std::string> STATUS_LIGHT_INTERFACES{"color", "state"};
 };
 
-}  // namespace urc_controllers
+} // namespace urc_controllers
 
-#endif  // URC_CONTROLLERS__IMU_BROADCASTER_HPP_
+#endif // URC_CONTROLLERS__IMU_BROADCASTER_HPP_

urc_controllers/src/status_light_controller.cpp

@@ -20,10 +20,7 @@ namespace urc_controllers
 {
 
 StatusLightController::StatusLightController()
-: color_command_subscriber_(nullptr)
-  , display_command_subscriber_(nullptr)
-  , color_command_(0.0)
-  , display_command_(0.0) {}
+: status_light_command_subscriber_(nullptr), color_command_(0), state_command_(0) {}
 
 controller_interface::CallbackReturn StatusLightController::on_init()
 {
@@ -51,7 +48,7 @@ const
     controller_interface::interface_configuration_type::INDIVIDUAL;
 
   command_interfaces_configuration.names.push_back(status_light_name + "/" + "color");
-  command_interfaces_configuration.names.push_back(status_light_name + "/" + "display");
+  command_interfaces_configuration.names.push_back(status_light_name + "/" + "state");
   return command_interfaces_configuration;
 }
 
@@ -84,15 +81,13 @@ controller_interface::CallbackReturn StatusLightController::on_activate(
     return controller_interface::CallbackReturn::ERROR;
   }
 
-  color_command_subscriber_ = get_node()->create_subscription<std_msgs::msg::Int8>(
-    "/cmd_statuslight_color", rclcpp::SystemDefaultsQoS(),
-    [this](std::shared_ptr<std_msgs::msg::Int8> message) {
-      color_command_.writeFromNonRT(static_cast<double>(message->data));
-    });
-  display_command_subscriber_ = get_node()->create_subscription<std_msgs::msg::Int8>(
-    "/cmd_statuslight_display", rclcpp::SystemDefaultsQoS(),
-    [this](std::shared_ptr<std_msgs::msg::Int8> message) {
-      display_command_.writeFromNonRT(static_cast<double>(message->data));
+  status_light_command_subscriber_ =
+    get_node()->create_subscription<urc_msgs::msg::StatusLightCommand>(
+    "/command/status_light", rclcpp::SystemDefaultsQoS(),
+    [this](std::shared_ptr<urc_msgs::msg::StatusLightCommand> msg)
+    {
+      color_command_.writeFromNonRT(static_cast<uint8_t>(msg->color));
+      state_command_.writeFromNonRT(static_cast<uint8_t>(msg->state));
     });
 
   RCLCPP_INFO(get_node()->get_logger(), "StatusLightController activated!");
@@ -102,32 +97,28 @@ controller_interface::CallbackReturn StatusLightController::on_activate(
 controller_interface::CallbackReturn StatusLightController::on_deactivate(
   const rclcpp_lifecycle::State &)
 {
-  color_command_subscriber_.reset();
-  display_command_subscriber_.reset();
+  status_light_command_subscriber_.reset();
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
 controller_interface::CallbackReturn StatusLightController::on_cleanup(
   const rclcpp_lifecycle::State &)
 {
-  color_command_subscriber_.reset();
-  display_command_subscriber_.reset();
+  status_light_command_subscriber_.reset();
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
 controller_interface::CallbackReturn StatusLightController::on_error(
   const rclcpp_lifecycle::State &)
 {
-  color_command_subscriber_.reset();
-  display_command_subscriber_.reset();
+  status_light_command_subscriber_.reset();
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
 controller_interface::CallbackReturn StatusLightController::on_shutdown(
   const rclcpp_lifecycle::State &)
 {
-  color_command_subscriber_.reset();
-  display_command_subscriber_.reset();
+  status_light_command_subscriber_.reset();
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
@@ -136,8 +127,8 @@ controller_interface::return_type StatusLightController::update(
   const rclcpp::Duration &)
 {
   command_interface_map["color"]->get().set_value(*color_command_.readFromRT());
-  command_interface_map["display"]->get().set_value(*display_command_.readFromRT());
+  command_interface_map["state"]->get().set_value(*state_command_.readFromRT());
   return controller_interface::return_type::OK;
 }
 
-}  // namespace urc_controllers
+} // namespace urc_controllers