Release 4.8

.gitignore

+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE

CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(comm_node_manager)
+
+add_compile_options(-std=c++11)
+
+find_package(catkin REQUIRED
+  COMPONENTS
+    actionlib
+    actionlib_msgs
+    core_msgs
+    geometry_msgs
+    mesh_msgs
+    message_generation
+    roadmap_msgs
+    rospy
+    std_msgs
+)
+
+catkin_python_setup()
+
+# TODO: Move to mission_msgs
+add_action_files(
+  FILES
+    DoDrop.action
+    DoReconfigure.action
+)
+
+generate_messages (
+  DEPENDENCIES
+    std_msgs
+    actionlib_msgs
+    geometry_msgs
+)
+
+
+# Package set up
+catkin_package(
+  CATKIN_DEPENDS
+    actionlib_msgs
+    std_msgs
+    geometry_msgs
+    message_runtime
+)

LICENSE

+Copyright 2022 NeBula Project
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of
+this software and associated documentation files (the "Software"), to deal in
+the Software without restriction, including without limitation the rights to
+use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
+the Software, and to permit persons to whom the Software is furnished to do so,
+subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
+FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
\ No newline at end of file

action/DoDrop.action

+# Goal
+geometry_msgs/Pose robotPose
+int8 type
+int8 TYPE_COMM=0
+int8 TYPE_UWB=1
+---
+# Result
+int32 unused2
+---
+# Feedback
+int32 unused3

action/DoReconfigure.action

+# Goal
+bool unused1
+---
+#result
+int32 unused2
+---
+# Feedback
+int32 unused3

config/droppable_list.yaml

+# List of items to be dropped
+# If an item is not mounted on the robot, it should be blank.
+# ex) No_0: comm_id:'radio0' uwb_id:''
+
+# name:     generic identifier
+# servo_id: ID of the comm dropping mechanism
+# hostname: Hostname of comm radio (if any)
+# uwb_id:   ID of the UWB beacon (if any)
+
+
+base1:
+  - name: 'scom-base1'
+    servo_id: 0
+    hostname: 'scom-base1'
+    uwb_id: ''
+    pose_graph_key:
+      prefix: 'z'
+      index: 0
+
+husky1: # Servo ID is incremented by one because of HW issue
+  # 4 node dropper starts at 1
+  - name: 'scom13'
+    servo_id: 1
+  - name: 'scom14'
+    servo_id: 2
+  - name: 'scom15'
+    servo_id: 3
+  - name: 'scom16'
+    servo_id: 4
+
+husky2:
+  # - name: 'scom1'
+  #   servo_id: 0
+  # - name: 'scom2'
+  #   servo_id: 1
+  # - name: 'scom3'
+  #   servo_id: 2
+  - name: 'scom4'
+    servo_id: 3
+  - name: 'scom5'
+    servo_id: 4
+  - name: 'scom6'
+    servo_id: 5
+husky3:
+  - name: 'scom7'
+    servo_id: 0
+  - name: 'scom3'
+    servo_id: 1
+  - name: 'scom9'
+    servo_id: 2
+  - name: 'scom10'
+    servo_id: 3
+  - name: 'scom11'
+    servo_id: 4
+  - name: 'scom12'
+    servo_id: 5
+
+husky4: []
+#  # Servo ID is incremented by one because of HW issue
+#  - name: 'scom1'
+#    servo_id: 1
+#  - name: 'scom2'
+#    servo_id: 2
+#  - name: 'scom3'
+#    servo_id: 3
+#  - name: 'scom4'
+#    servo_id: 4
+
+
+telemax1: []
+
+spot1:
+  - name: 'scom17'
+    servo_id: 0
+  - name: 'scom18'
+    servo_id: 1
+
+spot2: []
+
+spot3:
+  - name: 'scom19'
+    servo_id: 0
+  - name: 'scom20'
+    servo_id: 1
+
+spot4:
+  - name: 'scom21'
+    servo_id: 0
+  - name: 'scom22'
+    servo_id: 1
+
+spot5:
+  - name: 'scom1'
+    servo_id: 0
+  - name: 'scom2'
+    servo_id: 1

config/droppable_list_sim.yaml

+# List of items to be dropped
+# If an item is not mounted on the robot, it should be blank.
+# ex) No_0: comm_id:'radio0' uwb_id:''
+
+# name:     generic identifier
+# servo_id: ID of the comm dropping mechanism
+# hostname: Hostname of comm radio (if any)
+# uwb_id:   ID of the UWB beacon (if any)
+
+
+base1:
+  - name: 'scom-base1'
+    servo_id: 0
+    hostname: 'scom-base1'
+    uwb_id: ''
+    pose_graph_key:
+      prefix: 'z'
+      index: 0
+
+husky1:
+  - name: 'scom0'
+    servo_id: 0
+  - name: 'scom1'
+    servo_id: 1
+  - name: 'scom2'
+    servo_id: 2
+  - name: 'scom3'
+    servo_id: 3
+  - name: 'scom4'
+    servo_id: 4
+  - name: 'scom5'
+    servo_id: 5
+
+husky2:
+  - name: 'scom10'
+    servo_id: 0
+  - name: 'scom11'
+    servo_id: 1
+  - name: 'scom12'
+    servo_id: 2
+  - name: 'scom13'
+    servo_id: 3
+  - name: 'scom14'
+    servo_id: 4
+  - name: 'scom15'
+    servo_id: 5
+
+husky3:
+  - name: 'scom20'
+    servo_id: 0
+  - name: 'scom21'
+    servo_id: 1
+  - name: 'scom22'
+    servo_id: 2
+  - name: 'scom23'
+    servo_id: 3
+  - name: 'scom24'
+    servo_id: 4
+  - name: 'scom25'
+    servo_id: 5
+
+husky4:
+  - name: 'scom30'
+    servo_id: 0
+  - name: 'scom31'
+    servo_id: 1
+  - name: 'scom32'
+    servo_id: 2
+  - name: 'scom33'
+    servo_id: 3
+  - name: 'scom34'
+    servo_id: 4
+  - name: 'scom35'
+    servo_id: 5
+
+spot1:
+  - name: 'scom40'
+    servo_id: 0
+  - name: 'scom41'
+    servo_id: 1
+
+spot2:
+  - name: 'scom50'
+    servo_id: 0
+  - name: 'scom51'
+    servo_id: 1
+
+spot3:
+  - name: 'scom60'
+    servo_id: 0
+  - name: 'scom61'
+    servo_id: 1
+
+spot4:
+  - name: 'scom70'
+    servo_id: 0
+  - name: 'scom71'
+    servo_id: 1

launch/comm_node_manager.launch

+<launch>
+  <arg name="base_namespace" default="base1"/>
+  <arg name="robot_namespace" default="husky1"/>
+  <arg name="simulation" default="false"/>
+
+  <arg name="has_dropper" value="$(eval robot_namespace in ['spot1', 'spot3', 'spot4', 'husky1', 'husky2', 'husky3', 'husky4'])"/>
+
+  <group ns="$(arg robot_namespace)">
+    <node name="comm_node_manager"
+          pkg="comm_node_manager"
+          type="comm_node_manager_node.py"
+          output="screen">
+      <rosparam file="$(find comm_node_manager)/config/droppable_list.yaml" unless="$(arg simulation)"/>
+      <rosparam file="$(find comm_node_manager)/config/droppable_list_sim.yaml" if="$(arg simulation)"/>
+    </node>
+
+    <node if="$(eval simulation or not has_dropper)"
+          name="fake_comm_node_dropper"
+          pkg="comm_node_manager"
+          type="fake_comm_node_dropper.py"/>
+
+    <node name="comm_node_status_relay"
+          pkg="topic_tools"
+          type="relay"
+          args="/$(arg base_namespace)/comm_node_manager/status_agg
+                /$(arg robot_namespace)/comm_node_manager/status_agg"
+          respawn="true"/>
+
+    <node name="comm_node_pose_publisher"
+          pkg="comm_node_manager"
+          type="comm_node_pose.py"
+          output="screen"
+          respawn="true">
+      <param name="target_frame" value="$(arg robot_namespace)/odom"/>
+    </node>
+  </group>
+
+</launch>
\ No newline at end of file

launch/comm_node_manager_base_station.launch

+<launch>
+    <arg name="robot_namespace" default="base_station"/>
+
+    <node name="comm_node_manager"
+          pkg="comm_node_manager"
+          type="comm_node_manager_node_base_station.py"
+          ns="$(arg robot_namespace)"
+          output="screen">
+      <rosparam file="$(find comm_node_manager)/config/droppable_list.yaml"/>
+    </node>
+</launch>

launch/test_comm_node_manager_base_station.launch

+<launch>
+  <arg name="robot_namespace" default="base_station"/>
+
+  <rosparam param="/robots">
+    - husky1
+    - husky2
+  </rosparam>
+
+  <include file="$(find comm_node_manager)/launch/comm_node_manager_base_station.launch">
+    <arg name="robot_namespace" value="$(arg robot_namespace)"/>
+  </include>
+
+  <include file="$(find comm_node_manager)/launch/comm_node_manager.launch">
+    <arg name="robot_namespace" value="husky1"/>
+    <arg name="simulation" value="true"/>
+  </include>
+
+  <include file="$(find comm_node_manager)/launch/comm_node_manager.launch">
+    <arg name="robot_namespace" value="husky2"/>
+    <arg name="simulation" value="true"/>
+  </include>
+
+  <include file="$(find telemetry_tools)/launch/telemetry_husky.launch">
+    <arg name="robot_namespace" value="husky1"/>
+  </include>
+
+  <include file="$(find telemetry_tools)/launch/telemetry_husky.launch">
+    <arg name="robot_namespace" value="husky2"/>
+  </include>
+
+  <include file="$(find capability_common)/launch/robot_state_publisher.launch">
+    <arg name="robot_namespace" value="husky1"/>
+  </include>
+
+  <include file="$(find capability_common)/launch/robot_state_publisher.launch">
+    <arg name="robot_namespace" value="husky2"/>
+  </include>
+</launch>
\ No newline at end of file

package.xml

+<?xml version="1.0"?>
+<package format="2">
+  <name>comm_node_manager</name>
+  <version>0.0.0</version>
+
+  <maintainer email="otsu@jpl.nasa.gov">Kyon Otsu</maintainer>
+  <description>On robot node for managing comm node dropping and routes</description>
+
+  <license>TODO</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>actionlib</depend>
+  <depend>actionlib_msgs</depend>
+  <depend>core_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>mesh_msgs</depend>
+  <depend>roadmap_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>rospy</depend>
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+  <export>
+  </export>
+</package>

scripts/comm_node_manager_node.py

+#!/usr/bin/env python
+"""Comm node manager node."""
+
+import rospy
+
+from comm_node_manager import manager
+
+
+def main():
+    rospy.init_node('comm_node_manager')
+    m = manager.CommNodeManager()
+    m.run()
+
+
+if __name__ == '__main__':
+    main()

scripts/comm_node_manager_node_base_station.py

+#!/usr/bin/env python
+"""Comm node manager node."""
+
+import rospy
+
+from comm_node_manager import manager_base
+
+
+def main():
+    rospy.init_node('comm_node_manager')
+    m = manager_base.CommNodeManagerBase()
+    r = rospy.Rate(1.0)  # 1Hz
+    while not rospy.is_shutdown():
+        r.sleep()
+        m.publish_status()
+
+
+if __name__ == '__main__':
+    main()

scripts/comm_node_pose.py

+#!/usr/bin/env python
+"""Publish comm node pose as PoseArray."""
+
+import rospy
+
+from core_msgs.msg import CommNodeStatus
+from pose_graph_msgs.msg import PoseGraph
+from geometry_msgs.msg import PoseArray, Pose, PoseStamped
+from visualization_msgs.msg import Marker, MarkerArray
+
+from tf import transformations as tfm
+import tf2_ros
+from tf2_geometry_msgs import *
+
+from comm_node_manager.pose_graph import parse_key
+
+
+class CommNodePosePublisher(object):
+    def __init__(self):
+        self.pose_graph = PoseGraph()
+        self.comm_node_keys = set()
+        self.relative_poses = {}
+
+        self.robot_name = rospy.get_namespace().split('/')[1]
+        self.target_frame = rospy.get_param('~target_frame', 'world')
+        self.pose_pub = rospy.Publisher('comm_node_manager/poses', PoseArray, latch=True)
+        self.marker_pub = rospy.Publisher('comm_node_manager/markers', MarkerArray, latch=True)
+        self.status_sub = rospy.Subscriber('comm_node_manager/status',
+                                           CommNodeStatus, self.status_cb)
+        self.pg_sub = rospy.Subscriber('lamp/pose_graph', PoseGraph, self.pose_graph_cb)
+        self.agg_status_sub = rospy.Subscriber('comm_node_manager/status_agg',
+                                               CommNodeStatus, self.status_cb)
+        self.tf_buf = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buf)
+
+    def status_cb(self, msg):
+        prev_keys = self.comm_node_keys.copy()
+        for n in msg.dropped:
+            # Prefix 'z' do not have position
+            prefix, index = parse_key(n.pose_graph_key)
+            if prefix == 'z':
+                continue
+
+            self.comm_node_keys.add(n.pose_graph_key)
+            self.relative_poses[n.pose_graph_key] = n.relative_pose
+
+        if self.comm_node_keys != prev_keys:
+            rospy.loginfo("New comm node dropped")
+            self.publish_poses()
+
+    def pose_graph_cb(self, msg):
+        self.pose_graph = msg
+        self.publish_poses()
+
+    def publish_poses(self):
+        if not self.comm_node_keys:
+            return
+
+        rospy.loginfo("Publishing poses for %d comm nodes", len(self.comm_node_keys))
+        poses = []
+
+        # Extract pose from merged pose graph
+        for key in self.comm_node_keys:
+            relative_pose = self.relative_poses[key]
+            for node in self.pose_graph.nodes:
+                if node.key == key:
+                    pose = PoseStamped()
+                    pose.header.stamp = rospy.Time(0)
+                    pose.header.frame_id = 'world'  # TODO: Fix original frame ID
+                    m_world_key = self.pose2matrix(node.pose)
+                    m_key_node = self.pose2matrix(relative_pose)
+                    m_world_node = tfm.concatenate_matrices(m_world_key, m_key_node)
+                    pose.pose = self.matrix2pose(m_world_node)
+                    poses.append(pose)
+                    break
+
+        rospy.loginfo("Found %d keys in merged pose graph", len(poses))
+
+        # Transform poses to target frame
+        msg = PoseArray()
+        msg.header.stamp = rospy.Time.now()
+        msg.header.frame_id = self.target_frame
+        for pose in poses:
+            try:
+                pose_tfm = self.tf_buf.transform(pose, self.target_frame,
+                                                 timeout=rospy.Duration(1))
+            except Exception as e:
+                rospy.logwarn("TF failure: %s", e)
+                continue
+            msg.poses.append(pose_tfm.pose)
+
+        self.pose_pub.publish(msg)
+        rospy.loginfo("Published %d valid poses", len(msg.poses))
+
+        # Convert to markers for costmap layer
+        marker_msg = MarkerArray()
+        for i, pose in enumerate(msg.poses):
+            m = Marker()
+            m.id = i
+            m.header = msg.header
+            m.type = Marker.CYLINDER
+            m.action = Marker.ADD
+            m.pose = pose
+            m.scale.x = 0.5
+            m.scale.y = 0.5
+            m.scale.z = 0.45
+            m.color.r = 0.3
+            m.color.g = 0.5
+            m.color.b = 0.3
+            m.color.a = 1.0
+            marker_msg.markers.append(m)
+
+        self.marker_pub.publish(marker_msg)
+        rospy.loginfo("Published markers")
+
+    def pose2matrix(self, pose):
+        q = [pose.orientation.x, pose.orientation.y,
+             pose.orientation.z, pose.orientation.w]
+        t = [pose.position.x, pose.position.y, pose.position.z]
+        m = tfm.quaternion_matrix(q)
+        m[:3, 3] = t
+        return m
+
+    def matrix2pose(self, m):
+        t = m[:3, 3]
+        q = tfm.quaternion_from_matrix(m)
+        pose = Pose()
+        pose.position.x = t[0]
+        pose.position.y = t[1]
+        pose.position.z = t[2]
+        pose.orientation.x = q[0]
+        pose.orientation.y = q[1]
+        pose.orientation.z = q[2]
+        pose.orientation.w = q[3]
+        return pose
+
+
+def main():
+    rospy.init_node('comm_node_pose')
+    handler = CommNodePosePublisher()
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    main()

scripts/fake_comm_node_dropper.py

+#!/usr/bin/env python
+"""Fake comm node dropper for simulation."""
+
+import subprocess
+
+import rospy
+import tf2_ros
+from tf2_geometry_msgs import *
+from hw_msgs.msg import CommDropperStatus
+from std_msgs.msg import Empty, Int16
+from geometry_msgs.msg import PointStamped
+
+
+status_msg = CommDropperStatus()
+status_msg.status = [CommDropperStatus.UNKNOWN
+                     for i in range(len(status_msg.status))]
+
+result_pub = None
+tf_buf = None
+robot_name = None
+
+
+def drop_cb(msg):
+    global status_msg
+
+    # Get the first available node index
+    try:
+        servo_id = status_msg.status.index(CommDropperStatus.LOADED)
+    except ValueError:
+        rospy.logwarn("No more radio mounted")
+        result_pub.publish(-1)
+        return
+    rospy.loginfo("Received drop command for servo %d", servo_id)
+
+    # Spawn gazebo model
+    try:
+        point = get_drop_position()
+    except Exception:
+        rospy.logerr("Robot pose is not available")
+        point = None
+
+    if point:
+        rospy.loginfo("Spawning radio model in gazebo")
+        model_name = 'radio_{}_{}'.format(robot_name, servo_id)
+        cmd_template = 'rosrun gazebo_ros spawn_model ' \
+                       '-model "{model_name}" ' \
+                       '-sdf -database Radio ' \
+                       '-x "{x}" -y "{y}" -z "{z}" -R 1.57'
+        cmd = cmd_template.format(
+                    model_name=model_name,
+                    x=point.point.x,
+                    y=point.point.y,
+                    z=point.point.z,
+                  )
+        rospy.loginfo("Executing: %s", cmd)
+        subprocess.check_call(cmd, shell=True)
+
+    # Update status
+    status_msg.header.stamp = rospy.Time.now()
+    status_msg.status[servo_id] = CommDropperStatus.EMPTY
+
+    # Report result
+    result_pub.publish(servo_id)
+
+    servo_id += 1
+
+
+def get_drop_position():
+    point = PointStamped()
+    point.header.frame_id = robot_name + '/base_link'
+    point.point.x = -0.8
+    point.point.y = 0
+    point.point.z = -0.1
+    
+    return tf_buf.transform(point, 'world', timeout=rospy.Duration(1))
+
+
+def main():
+    global tf_buf, robot_name, result_pub
+
+    rospy.init_node('fake_comm_node_dropper')
+
+    robot_name = rospy.get_namespace().split('/')[1]
+
+    # Get loaded radios from config file
+    droppable_list = rospy.get_param('comm_node_manager/{}'.format(robot_name))
+    for droppable in droppable_list:
+        status_msg.status[droppable['servo_id']] = CommDropperStatus.LOADED
+
+    status_pub = rospy.Publisher('comm_dropper/status', CommDropperStatus)
+    result_pub = rospy.Publisher('comm_dropper/drop_result', Int16)
+    drop_pub = rospy.Subscriber('comm_dropper/drop', Empty, drop_cb)
+    tf_buf = tf2_ros.Buffer()
+    tf_listener = tf2_ros.TransformListener(tf_buf)
+
+    r = rospy.Rate(1)
+    while not rospy.is_shutdown():
+        status_pub.publish(status_msg)
+        r.sleep()
+
+
+if __name__ == '__main__':
+    main()

setup.py

+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+
+setup_args = generate_distutils_setup(
+        packages=['comm_node_manager'],
+        package_dir={'': 'src'},)
+
+setup(**setup_args)

src/comm_node_manager/droppable.py

+#!/usr/bin/env python
+"""Base droppable class."""
+
+import pprint
+
+import rospy
+
+from tf import transformations as tfm
+
+from core_msgs.msg import CommNodeStatus, CommNodeInfo
+from geometry_msgs.msg import Pose
+from roadmap_msgs.msg import RoadmapNode, RoadmapProperty
+from pose_graph_msgs.msg import PoseGraphNode
+
+from comm_node_manager import pose_graph as pg
+
+
+class Droppable(object):
+    def __init__(self, name, servo_id, **kwargs):
+        self.name = self.hostname = name
+        self.servo_id = servo_id
+        self.uwb_id = kwargs.get('uwb_id', None)
+        if 'pose_graph_key' in kwargs:
+            self.pose_graph_node = PoseGraphNode()
+            self.pose_graph_node.pose.orientation.w = 1
+            key = kwargs['pose_graph_key']
+            self.pose_graph_node.key = pg.compose_key(key['prefix'], key['index'])
+        else:
+            self.pose_graph_node = None
+        self.robot_name = kwargs.get('robot_name', None)
+        self.pose = None
+        self.dropped = True  # Empty
+
+    def is_dropped(self):
+        return self.dropped
+
+    def set_dropped(self, status):
+        self.dropped = status
+
+    def get_pose(self):
+        if self.pose:
+            return self.pose
+        elif self.pose_graph_node.pose:
+            return self.pose_graph_node.pose
+        return Pose()
+
+    def set_pose(self, pose):
+        self.pose = pose
+
+    def get_roadmap_node_msg(self):
+        msg = RoadmapNode()
+        msg.stamp = rospy.Time.now()
+        if self.hostname:
+            msg.type = RoadmapNode.TYPE_COMMS_RADIO
+        else:
+            msg.type = RoadmapNode.TYPE_UWB_RADIO
+        msg.pose.pose = self.get_pose()
+        msg.properties.append(RoadmapProperty(key='hostname', value=self.hostname))
+        msg.properties.append(RoadmapProperty(key='uwb_id', value=self.uwb_id))
+        msg.properties.append(RoadmapProperty(key='dropped', value='success' if self.is_dropped() else 'failure'))
+        if self.is_dropped():
+            msg.properties.append(RoadmapProperty(key='drop_time', value=str(int(msg.stamp.to_sec()))))
+            pass
+        return msg
+
+    def get_pose_graph_key(self):
+        if self.pose_graph_node is None:
+            return None
+        return pg.parse_key(self.pose_graph_node.key)
+
+    def get_info_msg(self):
+        if self.is_dropped() and self.pose_graph_node is not None:
+            relative_pose = self.get_relative_pose(
+                self.pose_graph_node.pose, self.get_pose())
+            pose_graph_key = self.pose_graph_node.key
+        else:
+            relative_pose = Pose()
+            relative_pose.orientation.w = 1
+            pose_graph_key = 0
+
+        return CommNodeInfo(**{
+            'hostname': self.hostname,
+            'uwb_id': self.uwb_id,
+            'pose_graph_key': pose_graph_key,
+            'robot_name': self.robot_name,
+            'relative_pose': relative_pose,
+        })
+
+    def get_relative_pose(self, pose1, pose2):
+        m1 = self.pose2matrix(pose1)
+        m2 = self.pose2matrix(pose2)
+        m12 = tfm.concatenate_matrices(tfm.inverse_matrix(m1), m2)
+        return self.matrix2pose(m12)
+
+    def pose2matrix(self, pose):
+        q = [pose.orientation.x, pose.orientation.y,
+             pose.orientation.z, pose.orientation.w]
+        t = [pose.position.x, pose.position.y, pose.position.z]
+        m = tfm.quaternion_matrix(q)
+        m[:3, 3] = t
+        return m
+
+    def matrix2pose(self, m):
+        t = m[:3, 3]
+        q = tfm.quaternion_from_matrix(m)
+        pose = Pose()
+        pose.position.x = t[0]
+        pose.position.y = t[1]
+        pose.position.z = t[2]
+        pose.orientation.x = q[0]
+        pose.orientation.y = q[1]
+        pose.orientation.z = q[2]
+        pose.orientation.w = q[3]
+        return pose
+
+
+class DroppableList(object):
+    def __init__(self, robot_name):
+        self.robot_name = robot_name
+
+        # Load nodes mounted on the robot
+        try:
+            config = rospy.get_param('~{}'.format(self.robot_name))
+        except KeyError:
+            rospy.logwarn("Droppable configuration not found for %s", self.robot_name)
+            config = []
+        self.items = {
+            c["servo_id"]: Droppable(robot_name=self.robot_name, **c)
+            for c in config
+        }
+        rospy.loginfo("%d nodes mounted on %s", len(self.items), self.robot_name)
+
+    def get_num_remaining(self):
+        """Return the number of remaining nodes."""
+        return len([n for n in self.items.values() if not n.is_dropped()])
+
+    def get(self, servo_id):
+        """Get the node specs from the servo ID."""
+        try:
+            return self.items[servo_id]
+        except KeyError:
+            raise KeyError("Invalid servo ID: {}".format(servo_id))
+
+    def remove(self, servo_id):
+        self.items.pop(servo_id, None)
+
+    def get_status_msg(self):
+        msg = CommNodeStatus()
+        msg.header.stamp = rospy.Time.now()
+
+        for node in self.items.values():
+            if node.is_dropped():
+                msg.dropped.append(node.get_info_msg())
+            else:
+                msg.remaining.append(node.get_info_msg())
+
+        return msg
+
+    def dumps(self, pretty=False):
+        output = {i: n.__dict__ for i, n in enumerate(self.items.values())}
+        return pprint.pformat(output) if pretty else output
+
+    def add_dummy_droppable(self, servo_id):
+        """Mount a dummy droppable for the specified servo ID."""
+        self.items[servo_id] = Droppable(
+            name="UNKNOWN",
+            hostname="UNKNOWN",
+            servo_id=servo_id,
+            robot_name=self.robot_name,
+        )

src/comm_node_manager/dropper.py

+#!/usr/bin/env python
+"""Dropper interface."""
+
+import rospy
+from std_msgs.msg import Empty, Int16
+from hw_msgs.msg import CommDropperStatus
+
+
+class Dropper(object):
+    def __init__(self):
+        self.drop_timeout = rospy.Duration(40)
+        self.status = [CommDropperStatus.UNKNOWN
+                       for i in range(len(CommDropperStatus().status))]
+        self.status_sub = rospy.Subscriber(**{
+            'name': 'comm_dropper/status',
+            'data_class': CommDropperStatus,
+            'queue_size': 1,
+            'callback': self.status_cb,
+        })
+        self.servo_pub = rospy.Publisher(**{
+            'name': 'comm_dropper/drop',
+            'data_class': Empty,
+            'queue_size': 1,
+        })
+        self.result = None
+        self.result_sub = rospy.Subscriber(**{
+            'name': 'comm_dropper/drop_result',
+            'data_class': Int16,
+            'queue_size': 1,
+            'callback': self.result_cb,
+        })
+
+    def send(self):
+        # Reset the previous result
+        self.result = None
+
+        # Send drop command
+        rospy.loginfo("Sending drop command")
+        self.servo_pub.publish(Empty())
+
+        # Wait for the feedback from the dropper
+        start_time = rospy.Time.now()
+        while not rospy.is_shutdown():
+            if rospy.Time.now() - start_time > self.drop_timeout:
+                rospy.logerr("Failed to get drop result within timeout")
+                return None
+            if self.result is not None:
+                rospy.loginfo("Got response from the dropper!")
+                dropped_idx = self.result.data
+                break
+
+        if dropped_idx < 0:
+            rospy.logerr("Dropper could not drop the comm node")
+            return None
+
+        rospy.loginfo("Successfully deployed comm node from slot %s", dropped_idx)
+        return dropped_idx
+
+    def status_cb(self, msg):
+        self.status = [ord(status) for status in msg.status]
+
+    def is_loaded(self, servo_id):
+        """Check if a comm node is loaded on a specific slot."""
+        return self.status[servo_id] == CommDropperStatus.LOADED
+
+    def is_empty(self, servo_id):
+        """Check if a comm node is empty on a specific slot."""
+        return self.status[servo_id] == CommDropperStatus.EMPTY
+
+    def is_failed(self, servo_id):
+        """Check if a comm node is stuck on a specific slot."""
+        return self.status[servo_id] == CommDropperStatus.ERROR
+
+    def result_cb(self, msg):
+        self.result = msg

src/comm_node_manager/manager.py

+#!/usr/bin/env python
+"""Comm node manager."""
+
+import copy
+
+import rospy
+
+import tf2_ros
+import tf2_geometry_msgs
+
+from actionlib import SimpleActionServer
+
+from comm_node_manager.msg import *
+from core_msgs.msg import CommNodeStatus
+from mesh_msgs.msg import CommNode as UWBNode
+from sensor_msgs.msg import Joy
+from std_msgs.msg import UInt8
+from geometry_msgs.msg import PoseStamped
+
+from comm_node_manager.dropper import Dropper
+from comm_node_manager.droppable import DroppableList
+from comm_node_manager.pose_graph import PoseGraphManager
+from comm_node_manager.roadmap import RoadmapManager
+from comm_node_manager import platform
+
+
+class CommNodeManager(object):
+    def __init__(self):
+        # Set up action server
+        self.reconfigure_server = SimpleActionServer(**{
+            'name': '~reconfigure',
+            'ActionSpec': DoReconfigureAction,
+            'execute_cb': self.reconfigure_cb,
+            'auto_start': False,
+        })
+        self.drop_server = SimpleActionServer(**{
+            'name': '~drop',
+            'ActionSpec': DoDropAction,
+            'execute_cb': self.drop_cb,
+            'auto_start': False,
+        })
+
+        # Set up ROS subscriber/publisher
+        self.status_pub = rospy.Publisher(**{
+            'name': '~status',
+            'data_class': CommNodeStatus,
+            'queue_size': 1,
+            'latch': True,
+        })
+        self.num_remaining_pub = rospy.Publisher(**{
+            'name': '~num_remaining',
+            'data_class': UInt8,
+            'queue_size': 1,
+            'latch': True,
+        })
+        self.uwb_anchor_pub = rospy.Publisher(**{
+            'name': '~uwb_anchor',
+            'data_class': UWBNode,
+            'queue_size': 1,
+        })
+        self.joy_sub = rospy.Subscriber(**{
+            'name': 'joy_teleop/joy',
+            'data_class': Joy,
+            'queue_size': 1,
+            'callback': self.joy_cb,
+        })
+        self.tf_buf = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buf)
+
+        # Initialize support objects
+        self.dropper = Dropper()
+        self.pose_graph = PoseGraphManager()
+        self.roadmap = RoadmapManager()
+
+        # Initialize droppable list
+        self.robot_name = rospy.get_namespace().replace('/', '')
+        self.droppables = DroppableList(self.robot_name)
+
+        rospy.loginfo("List of droppables:\n%s", self.droppables.dumps(pretty=True))
+
+        # Initialize platform interface
+        if self.robot_name.startswith("spot"):
+            self.platform = platform.SpotPlatform()
+        else:
+            self.platform = platform.GenericPlatform()
+
+        # Start action server
+        self.reconfigure_server.start()
+        self.drop_server.start()
+
+    def __del__(self):
+        self.reconfigure_server.stop()
+        self.drop_server.stop()
+
+    def run(self):
+        rate = rospy.Rate(1)
+        prev_status = copy.copy(self.dropper.status)
+        while not rospy.is_shutdown():
+            if prev_status != self.dropper.status:
+                rospy.logwarn(
+                    "Dropper status updated %s -> %s",
+                    prev_status,
+                    self.dropper.status,
+                )
+
+                # Update the node status
+                to_be_removed = []
+                for droppable in self.droppables.items.values():
+                    if self.dropper.is_loaded(droppable.servo_id):
+                        droppable.set_dropped(False)
+                    elif self.dropper.is_failed(droppable.servo_id):
+                        rospy.logerr(
+                            "Removing %s (slot %d) due to HW error",
+                            droppable.name,
+                            droppable.servo_id,
+                        )
+                        to_be_removed.append(droppable)
+
+                for droppable in to_be_removed:
+                    self.droppables.remove(droppable.servo_id)
+
+                prev_status = copy.copy(self.dropper.status)
+
+            self.publish_status()
+            rate.sleep()
+
+    def publish_status(self):
+        """Publish status as ROS message."""
+        status = self.droppables.get_status_msg()
+
+        rospy.logdebug("Publishing comm node status (dropped: %d, remaining: %d)",
+                       len(status.dropped), len(status.remaining))
+
+        self.status_pub.publish(status)
+        self.num_remaining_pub.publish(len(status.remaining))
+
+    def publish_uwb_anchor(self, node):
+        now = rospy.Time.now()
+        uwb_anchor_msg = UWBNode()
+        uwb_anchor_msg.AnchorID = node.uwb_id
+        uwb_anchor_msg.DropTime = now
+        uwb_anchor_msg.header.stamp = now
+        uwb_anchor_msg.point.header.frame_id = 'world'
+        uwb_anchor_msg.point.header.stamp = now
+        uwb_anchor_msg.point.point = node.get_pose().position
+        self.uwb_anchor_pub.publish(uwb_anchor_msg)
+
+    def reconfigure_cb(self, goal):
+        # This is heritage. Do nothing for now.
+        rospy.loginfo("Reconfigure action called")
+        self.reconfigure_server.set_succeeded()
+
+    def drop_cb(self, goal):
+        rospy.loginfo("Drop action called")
+
+        if not self.platform.prepare():
+            rospy.logerr("Failed to prepare for comm node dropping")
+            self.drop_server.set_aborted()
+            return
+
+        success = False
+        try:
+            success = self.do_drop()
+            if success:
+                rospy.loginfo("Successfully dropped a comm node")
+            else:
+                rospy.logerr("Failed to drop a comm node")
+        finally:
+            exit_result = self.platform.exit()
+            if not exit_result:
+                rospy.logwarn("Could not properly terminate the drop sequence")
+                success = False
+
+        if success:
+            self.drop_server.set_succeeded()
+        else:
+            self.drop_server.set_aborted()
+
+    def do_drop(self):
+        # Check if a node is mounted
+        if self.droppables.get_num_remaining() == 0:
+            rospy.logerr("No droppable mounted")
+            return False
+
+        # Drop a node
+        rospy.loginfo("Dropping")
+        servo_id = self.dropper.send()
+        rospy.sleep(5.0)
+
+        if servo_id is None:
+            rospy.logerr("Failed to deploy droppable: Hardware error")
+            return False
+
+        try:
+            node = self.droppables.get(servo_id)
+        except KeyError:
+            rospy.logwarn("Hardware returned unconfigured servo ID: %d", servo_id)
+            self.droppables.add_dummy_droppable(servo_id)
+            node = self.droppables.get(servo_id)
+
+        rospy.loginfo("Dropped a node %r", node.name)
+
+        # Update drop status
+        node.set_dropped(True)
+
+        # Determine dropped location
+        try:
+            node.set_pose(self.get_drop_location().pose)
+        except Exception as e:
+            rospy.logerr("Failed to query actual drop location: %s", e)
+        self.pose_graph.attach_droppable(node)
+        rospy.loginfo("Deployed at (%.1f, %.1f, %.1f) (PG prefix=%c key=%d)",
+                      node.get_pose().position.x,
+                      node.get_pose().position.y,
+                      node.get_pose().position.z,
+                      *node.get_pose_graph_key())
+
+        # Update roadmap and pose graph
+        rospy.loginfo("Updating roadmap")
+        self.roadmap.register(node)
+
+        if node.uwb_id:
+            rospy.loginfo("Updating UWB anchor")
+            self.publish_uwb_anchor(node)
+
+        return True
+
+    def get_drop_location(self):
+        """Get location of dropped node."""
+        pose = PoseStamped()
+        pose.header.frame_id = '{}/base_link'.format(self.robot_name)
+        pose.pose.position.x = -0.8
+        pose.pose.orientation.w = 1
+        return self.tf_buf.transform(pose, 'world', timeout=rospy.Duration(1.0))
+
+    def joy_cb(self, msg):
+        """Handle direct command from joystick"""
+        START_BUTTON = 7
+        try:
+            requested = msg.buttons[START_BUTTON]
+        except IndexError:
+            requested = False
+
+        if requested:
+            rospy.loginfo("Drop action called from joystick")
+
+            if not self.platform.prepare():
+                rospy.logerr("Failed to prepare for comm node dropping")
+                return
+
+            try:
+                success = self.do_drop()
+                if success:
+                    rospy.loginfo("Successfully dropped a comm node")
+                else:
+                    rospy.logerr("Failed to drop a comm node")
+            finally:
+                exit_result = self.platform.exit()
+                if not exit_result:
+                    rospy.logwarn("Could not properly terminate the drop sequence")