(fix) Merge pull request #14

CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(sugar)
+project(automatika_ros_sugar)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>sugar</name>
+  <name>automatika_ros_sugar</name>
   <version>0.2.3</version>
   <description>Syntactic sugar for ROS2 nodes creation and management</description>
   <maintainer email="[email protected]">Automatika Robotics</maintainer>

ros_sugar/config/base_attrs.py

@@ -1,6 +1,16 @@
 import json
 from types import NoneType, GenericAlias
-from typing import Any, Callable, Dict, Optional, Union, get_args, List, get_origin, _GenericAlias
+from typing import (
+    Any,
+    Callable,
+    Dict,
+    Optional,
+    Union,
+    get_args,
+    List,
+    get_origin,
+    _GenericAlias,
+)
 from copy import deepcopy
 import numpy as np
 from attrs import asdict, define, fields_dict
@@ -70,8 +80,8 @@ def __is_valid_arg_of_union_type(cls, obj, union_types) -> bool:
         :rtype: _type_
         """
         _types = [
-        get_origin(t) if isinstance(t, (GenericAlias, _GenericAlias)) else t
-        for t in get_args(union_types)
+            get_origin(t) if isinstance(t, (GenericAlias, _GenericAlias)) else t
+            for t in get_args(union_types)
         ]
         return any(isinstance(obj, t) for t in _types)
 
@@ -113,7 +123,11 @@ def __check_value_against_attr_type(
         else:
             # If not a Union type -> check using isinstance
             # Handles only the origin of GenericAlias (dict, list)
-            _attribute_type = get_origin(attribute_type) if isinstance(attribute_type, (GenericAlias, _GenericAlias)) else attribute_type
+            _attribute_type = (
+                get_origin(attribute_type)
+                if isinstance(attribute_type, (GenericAlias, _GenericAlias))
+                else attribute_type
+            )
             if not isinstance(value, _attribute_type):
                 raise TypeError(
                     f"Trying to set with incompatible type. Attribute {key} expecting '{type(attribute_to_set)}' got '{type(value)}'"

ros_sugar/core/component.py

@@ -15,8 +15,8 @@
 from rclpy import lifecycle
 from rclpy.publisher import Publisher as ROSPublisher
 from rclpy.subscription import Subscription
-from sugar.msg import ComponentStatus
-from sugar.srv import (
+from automatika_ros_sugar.msg import ComponentStatus
+from automatika_ros_sugar.srv import (
     ChangeParameter,
     ChangeParameters,
     ConfigureFromYaml,

ros_sugar/core/monitor.py

@@ -5,8 +5,8 @@
 from typing import Any, Callable, Dict, List, Optional, Union
 from rclpy.publisher import Publisher
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup
-from sugar.msg import ComponentStatus
-from sugar.srv import (
+from automatika_ros_sugar.msg import ComponentStatus
+from automatika_ros_sugar.srv import (
     ChangeParameter,
     ChangeParameters,
     ConfigureFromYaml,

ros_sugar/core/status.py

@@ -2,7 +2,7 @@
 
 from typing import List, Optional
 
-from sugar.msg import ComponentStatus
+from automatika_ros_sugar.msg import ComponentStatus
 
 _component_status = {
     0: "Running - Healthy",

ros_sugar/io/callbacks.py

@@ -11,6 +11,7 @@
 from geometry_msgs.msg import Pose
 from jinja2.environment import Template
 from nav_msgs.msg import OccupancyGrid, Odometry
+from std_msgs.msg import Header
 from PIL import Image as PILImage
 from rclpy.logging import get_logger
 from rclpy.subscription import Subscription
@@ -41,10 +42,22 @@ def __init__(self, input_topic, node_name: Optional[str] = None) -> None:
         self.node_name: Optional[str] = node_name
         self.msg = None
 
+        # Coordinates frame of the message data (if available)
+        self._frame_id: Optional[str] = None
+
         self._extra_callback: Optional[Callable] = None
         self._subscriber: Optional[Subscription] = None
         self._post_processors: Optional[List[Union[Callable, socket]]] = None
 
+    @property
+    def frame_id(self) -> Optional[str]:
+        """Getter of the message frame ID if available
+
+        :return: Header frame ID
+        :rtype: Optional[str]
+        """
+        return self._frame_id
+
     def set_node_name(self, node_name: str) -> None:
         """Set node name.
 
@@ -78,6 +91,11 @@ def callback(self, msg) -> None:
         :type msg: Any
         """
         self.msg = msg
+
+        # Get the frame if available
+        if hasattr(msg, 'header') and isinstance(msg.header, Header):
+            self._frame_id = msg.header.frame_id
+
         if self._extra_callback:
             self._extra_callback(
                 msg=msg, topic=self.input_topic, output=self.get_output()
@@ -166,6 +184,7 @@ def got_msg(self):
     def clear_last_msg(self):
         """Clears the last received message on the topic"""
         self.msg = None
+        self._frame_id = None
 
 
 class StdMsgCallback(GenericCallback):
@@ -281,7 +300,7 @@ def __init__(self, input_topic, node_name: Optional[str] = None) -> None:
                         self.msg = wavfile.read()
                 except Exception as e:
                     get_logger(self.node_name).error(
-                        f"Exception occured: {e}. Fixed path {input_topic.fixed} provided for Audio topic is not readable wav file"
+                        f"Exception occurred: {e}. Fixed path {input_topic.fixed} provided for Audio topic is not readable wav file"
                     )
             else:
                 get_logger(self.node_name).error(
@@ -609,7 +628,7 @@ def _get_output(self, **_) -> Optional[np.ndarray]:
         if not self.msg:
             return None
 
-        # If a trnsform is given apply it to the message
+        # If a transform is given apply it to the message
         if self.transformation:
             pose_transformed = self._transform(self.msg.pose, self.transformation)
             return self._process(pose_transformed)

ros_sugar/io/publisher.py

@@ -8,6 +8,9 @@
 from rclpy.logging import get_logger
 from rclpy.publisher import Publisher as ROSPublisher
 
+from std_msgs.msg import Header
+from builtin_interfaces.msg import Time
+
 # patch msgpack for numpy arrays
 m_pack.patch()
 
@@ -83,7 +86,14 @@ def _run_processor(self, processor: Union[Callable, socket], output: Any) -> Any
                 f"Error in external processor for {self.output_topic.name}: {e}"
             )
 
-    def publish(self, output: Any, *args, **kwargs) -> None:
+    def publish(
+        self,
+        output: Any,
+        *args,
+        frame_id: Optional[str] = None,
+        time_stamp: Optional[Time] = None,
+        **kwargs,
+    ) -> None:
         """
         Publish using the publisher
 
@@ -108,4 +118,13 @@ def publish(self, output: Any, *args, **kwargs) -> None:
                     output = pre_output
             msg = self.output_topic.msg_type.convert(output, *args, **kwargs)
             if msg:
+                if (frame_id or time_stamp) and not hasattr(msg, 'header'):
+                    get_logger(self.node_name).warn(
+                        f"Cannot add a header to non-stamped message of type '{type(msg)}'"
+                    )
+                elif frame_id or time_stamp:
+                    # Add a header
+                    msg.header = Header()
+                    msg.header.frame_id = frame_id or ''
+                    msg.header.stamp = time_stamp or Time()
                 self._publisher.publish(msg)

ros_sugar/io/supported_types.py

@@ -2,6 +2,7 @@
 
 from typing import Any, Union, Optional
 import base64
+from logging import getLogger
 
 import numpy as np
 
@@ -17,7 +18,7 @@
 from nav_msgs.msg import OccupancyGrid as ROSOccupancyGrid
 from nav_msgs.msg import Odometry as ROSOdometry
 from nav_msgs.msg import Path as ROSPath
-from sugar.msg import ComponentStatus as ROSComponentStatus
+from automatika_ros_sugar.msg import ComponentStatus as ROSComponentStatus
 
 # SENSOR_MSGS SUPPORTED ROS TYPES
 from sensor_msgs.msg import Image as ROSImage
@@ -263,8 +264,13 @@ def convert(
 
         # flatten by column
         # index (0,0) is the lower right corner of the grid in ROS
-        msg.data = output.flatten("F")
-
+        if output.flags.f_contiguous:
+            msg.data = output.flatten()
+        else:
+            getLogger("OccupancyGrid_Converter").warning(
+                "OccupancyGrid converter expects a column major numpy array but got a raw major array -> Changing the shape before sending to ROS publisher"
+            )
+            msg.data = output.flatten("F")
         return msg
 
 

ros_sugar/launch/launcher.py

@@ -558,7 +558,7 @@ def __listen_for_external_processing(self, sock: socket.socket, func: Callable):
             data = conn.recv(1024)
             if not data:
                 continue
-            # TODO: Retreive errors
+            # TODO: Retrieve errors
             data = msgpack.unpackb(data)
             result = func(**data)
             logger.debug(f"Got result from external processor: {result}")