bugfix on nested messages

Author: jocacace

behaviortree_ros2/CMakeLists.txt

@@ -17,13 +17,14 @@ set(THIS_PACKAGE_DEPS
     generate_parameter_library
     rosx_introspection
     std_msgs
+    geometry_msgs
     std_srvs
     geometry_msgs
     angles
     tf2
     tf2_geometry_msgs
     tf2_ros
-    )
+)
 
 find_package(ament_cmake REQUIRED)
 

behaviortree_ros2/include/behaviortree_ros2/serialization_policies.hpp

@@ -3,6 +3,8 @@
 
 #include "behaviortree_ros2/parser_utils.hpp"
 #include "behaviortree_eut_plugins/utils/deserialize_json.h"
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/json_parser.hpp>
 
 namespace BT_ROS
 
@@ -89,30 +91,57 @@ struct AutomaticSerialization
             return ports;
         }
 
-        
-        void onNewMessage(const std::shared_ptr<MessageType>& _message, BT::TreeNode& _tree_node,
-            const std::string& _base_port_name = "")
+        // Helper to extract nested fields "header.frame_id" [header][frame_id]
+        nlohmann::json getNestedValue(const nlohmann::json& j, const std::string& dotted_key)
         {
-            if(isMsgEmpty<MessageType>()) { return; }
+            std::istringstream iss(dotted_key);
+            std::string token;
+            const nlohmann::json* current = &j;
+            while (std::getline(iss, token, '.'))
+            {
+                if (!current->contains(token))
+                    throw std::runtime_error("Missing key: " + token + " in " + dotted_key);
+                current = &((*current)[token]);
+            }
+
+            return *current;
+        }
 
+        void onNewMessage( const std::shared_ptr<MessageType>& _message, BT::TreeNode& _tree_node, const std::string& _base_port_name = "")
+        {
+            if(isMsgEmpty<MessageType>()) { return; }
+        
             std::vector<uint8_t> buffer_in = RosMsgParser::BuildMessageBuffer(*(_message.get()), topic_type_);
             std::string json_text;
             RosMsgParser::ROS2_Deserializer deserializer_;
             parser_->deserializeIntoJson(buffer_in, &json_text, &deserializer_, 0, true, true);
             nlohmann::json json_parsed = nlohmann::json::parse(json_text);
-
+        
             const auto& field_ports = fieldPorts<MessageType>({RosMsgParser::ROSType("builtin_interfaces/Time")});
+        
             for(const auto& field_port : field_ports)
             {
-                // Time and duration defaults to ros::Time::now and zero
-                // TODO should we allow users to set this themselves? If so, how would they
-                // write it? Maybe a custom convertFromString()?
                 const auto type_id = field_port.second.type().typeID();
-                if(type_id == RosMsgParser::TIME ||
-                type_id == RosMsgParser::DURATION) { continue; }
-
-                std::string port_name = _base_port_name.empty() ? field_port.first : _base_port_name + "_" + field_port.first;
-                BT::EutUtils::deserializeField(_tree_node, port_name, json_parsed[field_port.first]);
+                if(type_id == RosMsgParser::TIME || type_id == RosMsgParser::DURATION)
+                    continue;
+        
+                std::string port_name = _base_port_name.empty() 
+                    ? field_port.first 
+                    : _base_port_name + "_" + field_port.first;
+        
+                try
+                {
+                    nlohmann::json value = getNestedValue(json_parsed, field_port.first);
+        
+                    BT::EutUtils::deserializeField(_tree_node, port_name, value);
+        
+                }
+                catch(const std::exception& e)
+                {
+                    std::cerr << "Warning: could not deserialize field " << field_port.first 
+                              << ": " << e.what() << std::endl;
+                }
+        
             }
         }
 

behaviortree_ros2/src/plugin.cpp

@@ -19,6 +19,7 @@
 #include <std_msgs/msg/float32.hpp>
 #include <std_msgs/msg/float64.hpp>
 #include <std_msgs/msg/string.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
 
 #include <std_srvs/srv/empty.hpp>
 #include <std_srvs/srv/set_bool.hpp>
@@ -30,11 +31,64 @@
 #include "btcpp_ros2_interfaces/action/execute_tree.hpp"
 #include "btcpp_ros2_interfaces/msg/custom_msg.hpp"
 
+// Simple Action that updates an instance of Position2D in the blackboard
+class SayHi : public BT::StatefulActionNode
+{
+public:
+  SayHi(const std::string& name, const BT::NodeConfig& config)
+    : BT::StatefulActionNode(name, config)
+  {}
+
+  static BT::PortsList providedPorts()
+  {
+    return { BT::InputPort<std::string>("person_name") };
+  }
+
+  BT::NodeStatus onStart() override
+  {
+    const auto name = getInput<std::string>("person_name");
+    if(!name)
+      return BT::NodeStatus::FAILURE;
+
+    person_name_ = name.value();
+    counter_ = 0;
+
+    std::cout << "Starting to say hi to " << person_name_ << "..." << std::endl;
+    return BT::NodeStatus::RUNNING;
+  }
+
+  BT::NodeStatus onRunning() override
+  {
+    if (counter_ < 3)
+    {
+      std::cout << "Hi " << person_name_ << "! (" << counter_+1 << "/3)" << std::endl;
+      counter_++;
+      return BT::NodeStatus::RUNNING;
+    }
+    else
+    {
+      std::cout << "Finished saying hi to " << person_name_ << "!" << std::endl;
+      return BT::NodeStatus::SUCCESS;
+    }
+  }
+
+  void onHalted() override
+  {
+    std::cout << "SayHi halted." << std::endl;
+  }
+
+private:
+  std::string person_name_;
+  int counter_ = 0;
+};
 
 using namespace BT;
 
 BT_REGISTER_ROS_NODES(factory, params)
 {
+
+    factory.registerNodeType<SayHi>("SayHi");
+
     //LOGS
     factory.registerNodeType<DebugLog>("DebugLog");
     factory.registerNodeType<InfoLog>("InfoLog");
@@ -62,6 +116,8 @@ BT_REGISTER_ROS_NODES(factory, params)
     factory.registerNodeType<AutoSerSubscriber<std_msgs::msg::Float32>>("MonitorAutoStdFloat",params);
     factory.registerNodeType<AutoSerSubscriber<std_msgs::msg::Float64>>("MonitorAutoStdDouble",params);
     factory.registerNodeType<AutoSerSubscriber<std_msgs::msg::String>>("MonitorAutoStdString",params);
+    factory.registerNodeType<AutoSerSubscriber<geometry_msgs::msg::PoseStamped>>("MonitorAutoPoseStamped",params);
+    factory.registerNodeType<JsonSerSubscriber<geometry_msgs::msg::PoseStamped>>("MonitorJsonPoseStamped",params);
 
     // Possible variant of PRIMITIVE SUBSCRIBER WITH JSON Serialization
     factory.registerNodeType<JsonSerSubscriber<std_msgs::msg::Int32>>("MonitorJsonStdInt",params);
@@ -80,7 +136,8 @@ BT_REGISTER_ROS_NODES(factory, params)
     factory.registerNodeType<AutoDesPublisher<std_msgs::msg::Float32>>("PublishAutoStdFloat",params);
     factory.registerNodeType<AutoDesPublisher<std_msgs::msg::Float64>>("PublishAutoStdDouble",params);
     factory.registerNodeType<AutoDesPublisher<std_msgs::msg::String>>("PublishAutoStdString",params);
-
+    factory.registerNodeType<AutoDesPublisher<geometry_msgs::msg::PoseStamped>>("PublishAutoPoseStamped",params);
+    
     //PRIMITIVE SERVICES
     factory.registerNodeType<AutoDesJsonSerServiceClient<std_srvs::srv::Empty>>("ServiceAutoCallJsonEmpty",params);
     factory.registerNodeType<AutoDesJsonSerServiceClient<std_srvs::srv::SetBool>>("ServiceAutoCallJsonBool",params);
@@ -97,4 +154,5 @@ BT_REGISTER_ROS_NODES(factory, params)
     factory.registerNodeType<SmartJsonSerSubscriber<btcpp_ros2_interfaces::msg::NodeStatus>>("MonitorSmartJsonNodeStatus",params);
     factory.registerNodeType<SmartJsonSerSubscriber<btcpp_ros2_interfaces::msg::CustomMsg>>("MonitorSmartJsonCustomMsg",params);
 
+
 };