Release 4.8

src/comm_node_manager/manager_base.py

+#!/usr/bin/env python
+"""Comm node manager."""
+
+import rospy
+from core_msgs.msg import CommNodeStatus
+from core_msgs.msg import VehicleStatus
+
+from comm_node_manager.droppable import DroppableList
+
+
+class CommNodeManagerBase(object):
+    def __init__(self):
+        self.buffer = {}
+        self.robots = rospy.get_param('/robots', [])
+        for robot in self.robots:
+            self.buffer[robot] = CommNodeStatus()
+            rospy.Subscriber('/{}/vehicle_status'.format(robot),
+                             VehicleStatus, self.callback)
+
+        # Load static node list
+        base_name = rospy.get_namespace().replace('/', '')
+        self.droppables = DroppableList(base_name)
+
+        # Make the published msgs in 1Hz
+        self.status_agg_pub_ = rospy.Publisher(**{
+            'name': '~status_agg',
+            'data_class': CommNodeStatus,
+            'queue_size': 1,
+            'latch': True,
+        })
+
+    def publish_status(self):
+        # Initialize with base status
+        agg_msg = self.droppables.get_status_msg()
+
+        # Aggregate status from robots
+        for robot in self.robots:
+            agg_msg.remaining += self.buffer[robot].remaining
+            agg_msg.dropped += self.buffer[robot].dropped
+
+        self.status_agg_pub_.publish(agg_msg)
+
+    def callback(self, msg):
+        current_robot_name = msg.name
+        rospy.loginfo("CommNodeManagerBase - Received " + current_robot_name + " data")
+        self.buffer[current_robot_name] = msg.comm_node

src/comm_node_manager/platform.py

+#!/usr/bin/env python
+"""Handles platform"""
+
+import rospy
+
+from spot_msgs.msg import Feedback as SpotFeedback
+from std_srvs.srv import Trigger
+
+
+class PlatformBase(object):
+    def prepare(self):
+        return True
+
+    def exit(self):
+        return True
+
+
+class GenericPlatform(PlatformBase):
+    def __init__(self):
+        super(GenericPlatform, self).__init__()
+        rospy.loginfo("Initialized 'Generic' platform")
+
+
+class SpotPlatform(PlatformBase):
+    def __init__(self):
+        super(SpotPlatform, self).__init__()
+        self.retry_rate = 0.5  # Hz
+        self.feedback = SpotFeedback()
+        self.feedback_sub = rospy.Subscriber(**{
+            'name': 'status/feedback',
+            'data_class': SpotFeedback,
+            'queue_size': 1,
+            'callback': self.feedback_cb,
+        })
+        self.sit = rospy.ServiceProxy("sit", Trigger)
+        self.stand = rospy.ServiceProxy("stand", Trigger)
+
+        rospy.loginfo("Initialized 'Spot' platform")
+
+    def prepare(self):
+        return self.ensure_sit()
+
+    def exit(self):
+        return self.ensure_stand()
+
+    def feedback_cb(self, msg):
+        self.feedback = msg
+
+    def ensure_sit(self, retry=5):
+        r = rospy.Rate(self.retry_rate)
+        for i in range(retry):
+            # Check status
+            rospy.loginfo("Checking Spot platform status...")
+            if self.feedback.sitting:
+                rospy.loginfo("Spot is sitting. Command succeeded")
+                break
+            elif self.feedback.standing:
+                rospy.loginfo("Spot is standing")
+            else:
+                rospy.logwarn("Spot is in an unknown state")
+
+            # Send mobility command
+            rospy.loginfo("Sending 'SIT' command to Spot")
+            try:
+                self.sit()
+            except rospy.ServiceException as e:
+                rospy.logerr("Failed to execute mobility command: %s", e)
+
+            # Adjust rate
+            r.sleep()
+        else:
+            rospy.logerr("Max retry reached for 'SIT' command")
+            return False
+        return True
+
+    def ensure_stand(self, retry=5):
+        r = rospy.Rate(self.retry_rate)
+        for i in range(retry):
+            # Check status
+            rospy.loginfo("Checking Spot platform status...")
+            if self.feedback.sitting:
+                rospy.loginfo("Spot is sitting")
+            elif self.feedback.standing:
+                rospy.loginfo("Spot is standing. Command succeeded")
+                break
+            else:
+                rospy.logwarn("Spot is in an unknown state")
+
+            # Send mobility command
+            rospy.loginfo("Sending 'STAND' command to Spot")
+            try:
+                self.stand()
+            except rospy.ServiceException as e:
+                rospy.logerr("Failed to execute mobility command: %s", e)
+
+            # Adjust rate
+            r.sleep()
+        else:
+            rospy.logerr("Max retry reached for 'STAND' command")
+            return False
+        return True

src/comm_node_manager/pose_graph.py

+#!/usr/bin/env python
+"""Handle pose graph."""
+
+import rospy
+
+from pose_graph_msgs.msg import PoseGraph, PoseGraphNode
+
+
+def parse_key(key):
+    char = chr(key >> 56)
+    key = key & 0x00ffffffff
+    return (char, key)
+
+
+def compose_key(prefix, index):
+    return (ord(prefix) << 56) + index
+
+
+class PoseGraphManager(object):
+    def __init__(self):
+        self.graph_sub = rospy.Subscriber(**{
+            'name': 'lamp/pose_graph_merged',
+            'data_class': PoseGraph,
+            'queue_size': 1,
+            'callback': self.pose_graph_cb,
+        })
+        self.last_node = PoseGraphNode(key=0)
+        self.robot_prefix = rospy.get_param('robot_prefix')
+
+    def pose_graph_cb(self, msg):
+        if self.last_node.key == 0:
+            rospy.loginfo("Received first pose graph message")
+        for n in reversed(msg.nodes):
+            if n.ID in ['key_frame', 'odom_node']:
+                prefix, _ = parse_key(n.key)
+                if prefix == self.robot_prefix:
+                    self.last_node = n
+                    break
+
+    def attach_droppable(self, node):
+        """Attach latest key frame node to the given droppable object."""
+        if self.last_node.key == 0:
+            rospy.logerr("Haven't received pose graph yet")
+        node.pose_graph_node = self.last_node
+        return node

src/comm_node_manager/roadmap.py

+#!/usr/bin/env python
+
+import copy
+import threading
+
+import numpy as np
+
+import rospy
+from roadmap_msgs.msg import Roadmap, RoadmapNode, RoadmapProperty
+from roadmap_msgs.srv import UpdateRoadmap, UpdateRoadmapRequest
+from roadmap_msgs import bridge
+
+from irm_manager import geometry_utils
+
+
+class RoadmapManager(object):
+    _drop_types = [RoadmapNode.TYPE_COMMS_RADIO, RoadmapNode.TYPE_UWB_RADIO]
+
+    def __init__(self):
+        self.lock = threading.Lock()
+        self.roadmap_sub = rospy.Subscriber(**{
+            'name': 'roadmap',
+            'data_class': Roadmap,
+            'queue_size': 1,
+            'callback': self.roadmap_cb,
+        })
+        self.update_roadmap = rospy.ServiceProxy(**{
+            'name': 'roadmap/update',
+            'service_class': UpdateRoadmap,
+        })
+        self.msg = Roadmap()
+
+    def roadmap_cb(self, msg):
+        if not self.msg.nodes:
+            rospy.loginfo("Received first roadmap message")
+        with self.lock:
+            self.msg = msg
+
+    def register(self, droppable):
+        """Register dropped node in roadmap."""
+        # Find closest non-dropped node
+        with self.lock:
+            matched_node = copy.deepcopy(self.find_matched_node(droppable))
+
+        # Compose update message
+        node = droppable.get_roadmap_node_msg()
+        if matched_node:
+            rospy.loginfo("Modifing corresponding roadmap node: key=%d", matched_node.index)
+            node.index = matched_node.index
+            node.properties.append(RoadmapProperty(key='action', value='mod'))
+        else:
+            rospy.loginfo("Adding brand new roadmap node")
+            node.properties.append(RoadmapProperty(key='action', value='add'))
+
+        request = UpdateRoadmapRequest()
+        request.roadmap.nodes.append(node)
+        try:
+            self.update_roadmap(request)
+        except rospy.ServiceException as e:
+            rospy.logwarn("Failed to update comm node information in IRM: %s", e)
+
+    def find_matched_node(self, node):
+        """Find corresponding node in the roadmap."""
+        def dist(n):
+            return np.linalg.norm([n.pose.pose.position.x - node.get_pose().position.x,
+                                   n.pose.pose.position.y - node.get_pose().position.y,
+                                   n.pose.pose.position.z - node.get_pose().position.z])
+        try:
+            closest_node = min(self.find_droppable_nodes(), key=dist)
+            closest_dist = dist(closest_node)
+        except ValueError:
+            rospy.logerr("Could not find roadmap node with droppable type")
+            closest_node = None
+            closest_dist = 1e6
+
+        if closest_node is not None and closest_dist > 30.0:
+            rospy.logwarn("Closest node is not close enough! (%.1f m)", closest_dist)
+            closest_node = None
+
+        return closest_node
+
+    def find_droppable_nodes(self, include_dropped=False):
+        """Find roadmap nodes that have droppable types."""
+        candidates = [n for n in self.msg.nodes if n.type in self._drop_types]
+        if include_dropped:
+            rospy.loginfo("Found %d droppable nodes in roadmap", len(candidates))
+            return candidates
+        else:
+            filtered_candidates = [n for n in candidates
+                                   if 'dropped' not in [p.key for p in n.properties]]
+            rospy.loginfo("Found %d undropped droppable nodes in roadmap (total: %d)",
+                          len(filtered_candidates), len(candidates))
+            return filtered_candidates

test/fake_drop_command.py

+#! /usr/bin/env python
+
+# Test script to simulate the drop command coming from a base station
+# One argument is required -- 0:comm node, 1:uwb (refer to the message file DoDrop.action)
+
+import rospy
+import actionlib
+import sys
+from comm_node_manager.msg import *
+
+def compose_drop_goal(item_type):
+    goal = DoDropGoal()
+    goal.type = item_type
+    return goal
+
+
+def main():
+    rospy.init_node('fake_drop_command')
+
+    client = actionlib.SimpleActionClient('husky1/comm_node_manager/drop', DoDropAction)
+    rospy.loginfo("Wait for action server")
+    client.wait_for_server()
+
+    goal = compose_drop_goal(int(sys.argv[1]))
+
+    timeout = rospy.Duration(10)
+    
+    try:
+        client.send_goal_and_wait(goal, execute_timeout=timeout)
+    except KeyboardInterrupt:
+        rospy.logwarn("Cancel requested by the user")
+        client.cancel_goal()
+        result = None
+    else:
+        rospy.loginfo("Fetch result")
+        result = client.get_result()
+
+
+if __name__ == '__main__':
+    main()

test/fake_uwb_service_server.py

+#! /usr/bin/env python
+
+# Test script to simulate the service server which deals with the UWB drop.
+# No argument is required
+
+import rospy
+from mesh_msgs.srv import *
+
+def handle_service_message(req):
+    rospy.loginfo(req.node.AnchorID)
+    return True
+
+
+def main():
+    rospy.init_node('drop_service_server')
+
+    server = rospy.Service('/husky/drop_uwb_anchor', DroppedArtifact, handle_service_message)
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    main()

test/test_comm_node_manager.launch

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