move base handler

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

free_fleet/free_fleet/ros1_types.py

@@ -74,6 +74,11 @@ class GoalID:
     msg_type = ROS1_STORE.types[type_name]
 
 
+class GoalStatus:
+    type_name = 'actionlib_msgs/msg/GoalStatus'
+    msg_type = ROS1_STORE.types[type_name]
+
+
 class GoalStatusArray:
     type_name = 'actionlib_msgs/msg/GoalStatusArray'
     msg_type = ROS1_STORE.types[type_name]

free_fleet_adapter/free_fleet_adapter/nav1_robot_adapter.py

@@ -34,6 +34,7 @@
 #     Time,
 # )
 from free_fleet.ros1_types import (
+    GoalStatus,
     ROS1_STORE,
     TFMessage
 )
@@ -46,7 +47,7 @@
 from geometry_msgs.msg import TransformStamped
 # import numpy as np
 import rclpy
-# import rmf_adapter.easy_full_control as rmf_easy
+import rmf_adapter.easy_full_control as rmf_easy
 # from rmf_adapter.robot_update_handle import ActivityIdentifier
 # from tf_transformations import euler_from_quaternion, quaternion_from_euler
 
@@ -109,6 +110,7 @@ def get_transform(self) -> TransformStamped | None:
 
 
 class Nav1MoveBaseHandler:
+
     def __init__(self, robot_name, zenoh_session, node):
         self.robot_name = robot_name
         self.zenoh_session = zenoh_session
@@ -117,5 +119,18 @@ def __init__(self, robot_name, zenoh_session, node):
         def _move_base_status_callback(sample: zenoh.Sample):
             raise NotImplementedError
 
-    def get_move_base_status(self) -> str | None:
+    def get_move_base_status(self) -> GoalStatus | None:
+        return None
+
+    def is_navigation_done(self) -> bool:
+        raise NotImplementedError
+
+    def navigate_to_pose(
+        self,
+        destination: rmf_easy.Destination,
+        execution: rmf_easy.CommandExecution
+    ):
+        raise NotImplementedError
+
+    def stop_navigation(self):
         raise NotImplementedError

free_fleet_adapter/tests/integration/test_move_base_cancel.py

@@ -16,53 +16,59 @@
 
 # https://github.com/eclipse-zenoh/zenoh-plugin-ros1/issues/131
 
-import zenoh, time
-from rosbags.typesys import Stores, get_types_from_msg, get_typestore
+# import zenoh, time
+# from rosbags.typesys import Stores, get_types_from_msg, get_typestore
 
-BRIDGE_NAMESPACE = '/tb3_0'
-TOPIC = '/move_base/cancel'
-TYPE = 'actionlib_msgs/msg/GoalID'
+# BRIDGE_NAMESPACE = '/tb3_0'
+# TOPIC = '/move_base/cancel'
+# TYPE = 'actionlib_msgs/msg/GoalID'
 
-def get_zenoh_name_of_ros1_topic(ros1_store, topic: str, msg_type: str) -> str:
-    # Get md5 and encode msg_type to construct zenoh topic
-    msg_type_split = msg_type.split('/')
-    msg_type_encoded = '/'.join([msg_type_split[0],msg_type_split[2]]).encode('utf-8').hex()
-    md5 = ros1_store.generate_msgdef(msg_type)[1]
-    zenoh_topic = '/'.join([msg_type_encoded, md5, topic[1:]])
+# def get_zenoh_name_of_ros1_topic(
+#     ros1_store,
+#     topic: str,
+#     msg_type: str
+# ) -> str:
+#     # Get md5 and encode msg_type to construct zenoh topic
+#     msg_type_split = msg_type.split('/')
+#     msg_type_encoded = \
+#         '/'.join([msg_type_split[0],msg_type_split[2]]).encode('utf-8').hex()
+#     md5 = ros1_store.generate_msgdef(msg_type)[1]
+#     zenoh_topic = '/'.join([msg_type_encoded, md5, topic[1:]])
 
-    return zenoh_topic
+#     return zenoh_topic
 
 
-if __name__ == "__main__":
-    session = zenoh.open(zenoh.Config())
-    ros1_store = get_typestore(Stores.ROS1_NOETIC)
-    GoalID = ros1_store.types[TYPE]
-    Stamp = ros1_store.types['builtin_interfaces/msg/Time']
+# if __name__ == "__main__":
+#     session = zenoh.open(zenoh.Config())
+#     ros1_store = get_typestore(Stores.ROS1_NOETIC)
+#     GoalID = ros1_store.types[TYPE]
+#     Stamp = ros1_store.types['builtin_interfaces/msg/Time']
 
-    zenoh_topic = get_zenoh_name_of_ros1_topic(
-        ros1_store,
-        topic=f'{BRIDGE_NAMESPACE}{TOPIC}',
-        msg_type=TYPE
-    )
-    print(f'ROS topic {TOPIC} is converted to Zenoh {zenoh_topic}')
+#     zenoh_topic = get_zenoh_name_of_ros1_topic(
+#         ros1_store,
+#         topic=f'{BRIDGE_NAMESPACE}{TOPIC}',
+#         msg_type=TYPE
+#     )
+#     print(f'ROS topic {TOPIC} is converted to Zenoh {zenoh_topic}')
 
-    # Declare a publisher (optional but allows Zenoh to perform some optimizations)
-    pub = session.declare_publisher(zenoh_topic)
+#     # Declare a publisher (optional but allows Zenoh to perform some
+#     # optimizations)
+#     pub = session.declare_publisher(zenoh_topic)
 
-    stamp = Stamp(sec=0, nanosec=0)
-    msg = GoalID(
-        stamp=stamp,
-        id='/move_base-1-36.21000000'
-    )
-
-    print('Publishing: "{0}"'.format(msg))
-    # Publish the serialized message
-    serialized_msg = ros1_store.serialize_ros1(msg, GoalID.__msgtype__)
-    pub.put(serialized_msg.tobytes())
-    time.sleep(1)
-    pub.undeclare()
-    session.close()
+#     stamp = Stamp(sec=0, nanosec=0)
+#     msg = GoalID(
+#         stamp=stamp,
+#         id='/move_base-1-36.21000000'
+#     )
 
+#     print('Publishing: "{0}"'.format(msg))
+#     # Publish the serialized message
+#     serialized_msg = ros1_store.serialize_ros1(msg, GoalID.__msgtype__)
+#     pub.put(serialized_msg.tobytes())
+#     time.sleep(1)
+#     pub.undeclare()
+#     session.close()
 
 # rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped
-# '{ header: { frame_id:  "map"}, pose: { position: { x: 1.808, y: 0.503 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'
+# '{ header: { frame_id:  "map"}, pose: { position: { x: 1.808, y: 0.503 },
+# orientation: { x: 0, y: 0, z: 0, w: 1 } } }'

free_fleet_adapter/tests/integration/test_move_base_simple_goal.py

@@ -16,62 +16,69 @@
 
 # https://github.com/eclipse-zenoh/zenoh-plugin-ros1/issues/131
 
-import zenoh, time
-from rosbags.typesys import Stores, get_types_from_msg, get_typestore
+# import zenoh, time
+# from rosbags.typesys import Stores, get_types_from_msg, get_typestore
 
-BRIDGE_NAMESPACE = '/tb3_0'
-TOPIC = '/move_base_simple/goal'
-TYPE = 'geometry_msgs/msg/PoseStamped'
+# BRIDGE_NAMESPACE = '/tb3_0'
+# TOPIC = '/move_base_simple/goal'
+# TYPE = 'geometry_msgs/msg/PoseStamped'
 
-def get_zenoh_name_of_ros1_topic(ros1_store, topic: str, msg_type: str) -> str:
-    # Get md5 and encode msg_type to construct zenoh topic
-    msg_type_split = msg_type.split('/')
-    msg_type_encoded = '/'.join([msg_type_split[0],msg_type_split[2]]).encode('utf-8').hex()
-    md5 = ros1_store.generate_msgdef(msg_type)[1]
-    zenoh_topic = '/'.join([msg_type_encoded, md5, topic[1:]])
+# def get_zenoh_name_of_ros1_topic(
+#     ros1_store,
+#     topic: str,
+#     msg_type: str
+# ) -> str:
+#     # Get md5 and encode msg_type to construct zenoh topic
+#     msg_type_split = msg_type.split('/')
+#     msg_type_encoded = \
+#         '/'.join([msg_type_split[0],msg_type_split[2]]).encode('utf-8').hex()
+#     md5 = ros1_store.generate_msgdef(msg_type)[1]
+#     zenoh_topic = '/'.join([msg_type_encoded, md5, topic[1:]])
 
-    return zenoh_topic
+#     return zenoh_topic
 
 
-if __name__ == "__main__":
-    session = zenoh.open(zenoh.Config())
-    ros1_store = get_typestore(Stores.ROS1_NOETIC)
-    PoseStamped = ros1_store.types[TYPE]
-    Stamp = ros1_store.types['builtin_interfaces/msg/Time']
-    Header = ros1_store.types['std_msgs/msg/Header']
-    Pose = ros1_store.types['geometry_msgs/msg/Pose']
-    Point = ros1_store.types['geometry_msgs/msg/Point']
-    Quaternion = ros1_store.types['geometry_msgs/msg/Quaternion']
+# if __name__ == "__main__":
+#     session = zenoh.open(zenoh.Config())
+#     ros1_store = get_typestore(Stores.ROS1_NOETIC)
+#     PoseStamped = ros1_store.types[TYPE]
+#     Stamp = ros1_store.types['builtin_interfaces/msg/Time']
+#     Header = ros1_store.types['std_msgs/msg/Header']
+#     Pose = ros1_store.types['geometry_msgs/msg/Pose']
+#     Point = ros1_store.types['geometry_msgs/msg/Point']
+#     Quaternion = ros1_store.types['geometry_msgs/msg/Quaternion']
 
-    zenoh_topic = get_zenoh_name_of_ros1_topic(
-        ros1_store,
-        topic=f'{BRIDGE_NAMESPACE}{TOPIC}',
-        msg_type=TYPE
-    )
-    print(f'ROS topic {TOPIC} is converted to Zenoh {zenoh_topic}')
+#     zenoh_topic = get_zenoh_name_of_ros1_topic(
+#         ros1_store,
+#         topic=f'{BRIDGE_NAMESPACE}{TOPIC}',
+#         msg_type=TYPE
+#     )
+#     print(f'ROS topic {TOPIC} is converted to Zenoh {zenoh_topic}')
 
-    # Declare a publisher (optional but allows Zenoh to perform some optimizations)
-    pub = session.declare_publisher(zenoh_topic)
+#     # Declare a publisher (optional but allows Zenoh to perform some
+#     # optimizations)
+#     pub = session.declare_publisher(zenoh_topic)
 
-    stamp = Stamp(sec=0, nanosec=0)
-    header = Header(seq=0, stamp=stamp, frame_id='map')
-    position = Point(x=-1.6, y=-0.5, z=0)
-    orientation = Quaternion(x=0, y=0, z=0, w=1)
-    pose = Pose(position=position, orientation=orientation)
+#     stamp = Stamp(sec=0, nanosec=0)
+#     header = Header(seq=0, stamp=stamp, frame_id='map')
+#     position = Point(x=-1.6, y=-0.5, z=0)
+#     orientation = Quaternion(x=0, y=0, z=0, w=1)
+#     pose = Pose(position=position, orientation=orientation)
 
-    msg = PoseStamped(
-        header=header,
-        pose=pose
-    )
+#     msg = PoseStamped(
+#         header=header,
+#         pose=pose
+#     )
 
-    print('Publishing: "{0}"'.format(msg))
-    # Publish the serialized message
-    serialized_msg = ros1_store.serialize_ros1(msg, PoseStamped.__msgtype__)
-    pub.put(serialized_msg.tobytes())
-    time.sleep(1)
-    pub.undeclare()
-    session.close()
+#     print('Publishing: "{0}"'.format(msg))
+#     # Publish the serialized message
+#     serialized_msg = ros1_store.serialize_ros1(msg, PoseStamped.__msgtype__)
+#     pub.put(serialized_msg.tobytes())
+#     time.sleep(1)
+#     pub.undeclare()
+#     session.close()
 
 
-# rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped
-# '{ header: { frame_id:  "map"}, pose: { position: { x: 1.808, y: 0.503 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'
+# # rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped
+# # '{ header: { frame_id:  "map"}, pose: { position: { x: 1.808, y: 0.503 },
+# # orientation: { x: 0, y: 0, z: 0, w: 1 } } }'

free_fleet_adapter/tests/integration/test_move_base_status.py

@@ -16,34 +16,42 @@
 
 # https://github.com/eclipse-zenoh/zenoh-plugin-ros1/issues/131
 
-import zenoh, time
-from rosbags.typesys import Stores, get_types_from_msg, get_typestore
-
-BRIDGE_NAMESPACE = '/tb3_0'
-TOPIC = '/move_base/status'
-TYPE = 'actionlib_msgs/msg/GoalStatusArray'
-
-def get_zenoh_name_of_ros1_topic(ros1_store, topic: str, msg_type: str) -> str:
-    # Get md5 and encode msg_type to construct zenoh topic
-    msg_type_split = msg_type.split('/')
-    msg_type_encoded = '/'.join([msg_type_split[0],msg_type_split[2]]).encode('utf-8').hex()
-    md5 = ros1_store.generate_msgdef(msg_type)[1]
-    zenoh_topic = '/'.join([msg_type_encoded, md5, topic[1:]])
-
-    return zenoh_topic
-
-
-def listener(sample: zenoh.Sample):
-    msg = ros1_store.deserialize_ros1(sample.payload.to_bytes(), TYPE)
-    print(f'ROS1 msg: {msg}')
-
-
-if __name__ == "__main__":
-    session = zenoh.open(zenoh.Config())
-    ros1_store = get_typestore(Stores.ROS1_NOETIC)
-
-    zenoh_topic = get_zenoh_name_of_ros1_topic(ros1_store, topic=f'{BRIDGE_NAMESPACE}{TOPIC}', msg_type=TYPE)
-    print(f'ROS topic {TOPIC} is converted to Zenoh {zenoh_topic}')
-    sub = session.declare_subscriber(zenoh_topic, listener)
-    time.sleep(60)
-
+# import zenoh, time
+# from rosbags.typesys import Stores, get_types_from_msg, get_typestore
+
+# BRIDGE_NAMESPACE = '/tb3_0'
+# TOPIC = '/move_base/status'
+# TYPE = 'actionlib_msgs/msg/GoalStatusArray'
+
+# def get_zenoh_name_of_ros1_topic(
+#     ros1_store,
+#     topic: str,
+#     msg_type: str
+# ) -> str:
+#     # Get md5 and encode msg_type to construct zenoh topic
+#     msg_type_split = msg_type.split('/')
+#     msg_type_encoded = \
+#         '/'.join([msg_type_split[0],msg_type_split[2]]).encode('utf-8').hex()
+#     md5 = ros1_store.generate_msgdef(msg_type)[1]
+#     zenoh_topic = '/'.join([msg_type_encoded, md5, topic[1:]])
+
+#     return zenoh_topic
+
+
+# def listener(sample: zenoh.Sample):
+#     msg = ros1_store.deserialize_ros1(sample.payload.to_bytes(), TYPE)
+#     print(f'ROS1 msg: {msg}')
+
+
+# if __name__ == "__main__":
+#     session = zenoh.open(zenoh.Config())
+#     ros1_store = get_typestore(Stores.ROS1_NOETIC)
+
+#     zenoh_topic = get_zenoh_name_of_ros1_topic(
+#         ros1_store,
+#         topic=f'{BRIDGE_NAMESPACE}{TOPIC}',
+#         msg_type=TYPE
+#     )
+#     print(f'ROS topic {TOPIC} is converted to Zenoh {zenoh_topic}')
+#     sub = session.declare_subscriber(zenoh_topic, listener)
+#     time.sleep(60)