Add load and unload nodelet command to the executive. (nasa#222)

astrobee/commands/freeFlyerPlanSchema.json

@@ -1040,7 +1040,7 @@
       "type": "CommandSpec",
       "id": "Admin.loadNodelet",
       "isRapidNative": false,
-      "notes": "Not implemented. This command is intended to load a ROS nodelet in the Astrobee flight software.<p/>There is an internal service advertised by the system monitor that provides a method very similar to this command, but as of Oct 2020, there is no way to invoke it through a standard Astrobee API call. Also note, the underlying system monitor service can't load nodelets that run on the HLP.",
+      "notes": "This command is intended to load a ROS nodelet in the Astrobee flight software. Please note, the underlying system monitor service can't load nodelets that run on the HLP.",
       "availableContext": [
         "teleop"
       ],
@@ -1054,17 +1054,17 @@
         },
         {
           "type": "ParamSpec",
-          "id": "type",
+          "id": "managerName",
           "valueType": "string",
           "default": "",
-          "notes": "Type of nodelet (namespace/classname). If left blank, the system monitor will default to the type specified in its config file, which should normally work fine."
+          "notes": "Which nodelet manager should load the nodelet. If left blank, the system monitor will default to using the managerName specified in the last heartbeat it received from the requested nodelet. (That should normally work fine, assuming the nodelet sent out at least one valid heartbeat.)"
         },
         {
           "type": "ParamSpec",
-          "id": "managerName",
+          "id": "type",
           "valueType": "string",
           "default": "",
-          "notes": "Which nodelet manager should load the nodelet. If left blank, the system monitor will default to using the managerName specified in the last heartbeat it received from the requested nodelet. (That should normally work fine, assuming the nodelet sent out at least one valid heartbeat.)"
+          "notes": "Type of nodelet (namespace/classname). If left blank, the system monitor will default to the type specified in its config file, which should normally work fine."
         },
         {
           "type": "ParamSpec",
@@ -1080,7 +1080,7 @@
       "type": "CommandSpec",
       "id": "Admin.unloadNodelet",
       "isRapidNative": false,
-      "notes": "Not implemented. This command is intended to unload a ROS nodelet in Astrobee's flight software. Its primary use is in the system monitor's fault response table, where it is the usual response if initialization of a nodelet fails. As of Oct 2020, there is an internal service that implements this function, but there is no way to invoke it through a standard Astrobee API call. Also, the internal service can't unload nodes running on the HLP.",
+      "notes": "This command is intended to unload a ROS nodelet in Astrobee's flight software. Its primary use is in the system monitor's fault response table, where it is the usual response if initialization of a nodelet fails. Please note, the internal service can't unload nodes running on the HLP.",
       "availableContext": [
         "teleop"
       ],

astrobee/config/commands.config

@@ -41,11 +41,11 @@ commandConfig = {
               type="RAPID_STRING"
             },
             {
-              key="type",
+              key="managerName",
               type="RAPID_STRING"
             },
             {
-              key="managerName",
+              key="type",
               type="RAPID_STRING"
             },
             {

astrobee/config/management/sys_monitor.config

@@ -36,41 +36,50 @@ time_diff_node = "imu_aug"
 time_drift_thres_sec = 0.25
 
 -- Types of nodelets. Used for loading nodelets.
-nodelet_types={
-                {name="access_control", type="access_control/AccessControl"},
-                {name="executive", type="executive/Executive"},
-                {name="llp_cpu_mem_monitor", type="cpu_mem_monitor/CpuMemMonitor"},
-                {name="mlp_cpu_mem_monitor", type="cpu_mem_monitor/CpuMemMonitor"},
-                {name="llp_disk_monitor", type="disk_monitor/DiskMonitor"},
-                {name="mlp_disk_monitor", type="disk_monitor/DiskMonitor"},
-                {name="image_sampler", type="image_sampler/ImageSampler"},
-                {name="mapper", type="mapper/MapperNodelet"},
-                {name="planner_qp", type="planner_qp/Planner"},
-                {name="choreographer", type="choreographer/ChoreographerNodelet"},
-                {name="planner_trapezoidal", type="planner_trapezoidal/PlannerTrapezoidalNodelet"},
-                {name="dds_ros_bridge", type="dds_ros_bridge/DdsRosBridge"},
-                {name="optical_flow_nodelet", type="lk_optical_flow_node/LKOpticalFlowNodelet"},
-                {name="localization_node", type="localization_node/LocalizationNodelet"},
-                {name="marker_tracking", type="marker_tracking_node/MarkerTrackingNodelet"},
-                {name="handrail_detect", type="handrail_detect/HandrailDetect"},
-                {name="localization_manager", type="localization_manager/LocalizationManagerNodelet"},
-                {name="laser", type="laser/laserNodelet"},
-                {name="epson_imu", type="epson_imu/EpsonImuNodelet"},
-                {name="eps_driver", type="eps_driver/EpsDriverNode"},
-                {name="framestore", type="description/FrameStore"},
-                {name="pmc_actuator", type="pmc_actuator/PmcActuatorNodelet"},
-                {name="flashlight_aft", type="flashlight/FlashlightNodelet"},
-                {name="flashlight_front", type="flashlight/FlashlightNodelet"},
-                {name="dock_cam", type="is_camera/CameraNodelet"},
-                {name="nav_cam", type="is_camera/CameraNodelet"},
-                {name="speed_cam", type="speed_cam/SpeedCamNode"},
-                {name="pico_driver", type="pico_driver/PicoDriverNodelet"},
-                {name="ctl", type="ctl/CtlNodelet"},
-                {name="graph_loc", type="graph_localizer/GraphLocalizerNodelet"},
-                {name="imu_aug", type="imu_augmentor/ImuAugmentorNodelet"},
-                {name="fam", type="fam/FamNodelet"},
-                {name="arm", type="arm/ArmNodelet"},
-                {name="dock", type="dock/DockNodelet"}}
+-- Not listed nodes are calibration_dock_cam, calibration_imu,
+-- calibration_nav_cam, ground_truth_localizer, and vive 
+nodelet_info={
+                {name="access_control", manager="mlp_management", type="access_control/AccessControl"},
+                {name="arm", manager="mlp_arm", type="arm/ArmNodelet"},
+                {name="astrobee_astrobee_bridge", manager="mlp_multibridge", type="dds_ros_bridge/AstrobeeAstrobeeBridge"},
+                {name="choreographer", manager="mlp_mobility", type="choreographer/ChoreographerNodelet"},
+                {name="ctl", manager="llp_gnc", type="ctl/ctl"},
+                {name="data_bagger", manager="mlp_recording", type="data_bagger/DataBagger"},
+                {name="dds_ros_bridge", manager="mlp_communications", type="dds_ros_bridge/DdsRosBridge"},
+                {name="dock", manager="mlp_dock", type="dock/DockNodelet"},
+                {name="dock_cam", manager="mlp_vision", type="is_camera/camera"},
+                {name="eps_driver", manager="llp_i2c", type="eps_driver/EpsDriverNode"},
+                {name="epson_imu", manager="llp_imu", type="epson_imu/EpsonImuNodelet"},
+                {name="executive", manager="mlp_management", type="executive/Executive"},
+                {name="fam", manager="llp_gnc", type="fam/fam"},
+                {name="flashlight_aft", manager="llp_i2c", type="flashlight/FlashlightNodelet"},
+                {name="flashlight_front", manager="llp_i2c", type="flashlight/FlashlightNodelet"},
+                {name="framestore", manager="mlp_mobility", type="mobility/FrameStore"},
+                {name="graph_loc", manager="mlp_graph_localization", type="graph_localizer/GraphLocalizerNodelet"},
+                {name="handrail_detect", manager="mlp_depth_cam", type="handrail_detect/HandrailDetect"},
+                {name="image_sampler", manager="mlp_localization", type="image_sampler/ImageSampler"},
+                {name="imu_aug", manager="llp_imu_aug", type="imu_augmentor/ImuAugmentorNodelet"},
+                {name="laser", manager="llp_i2c", type="laser/LaserNodelet"},
+                {name="light_flow", manager="llp_lights", type="light_flow/LightFlowNodelet"},
+                {name="llp_cpu_mem_monitor", manager="llp_monitors", type="cpu_mem_monitor/CpuMemMonitor"},
+                {name="llp_disk_monitor", manager="llp_monitors", type="disk_monitor/DiskMonitor"},
+                {name="localization_manager", manager="mlp_localization", type="localization_manager/LocalizationManagerNodelet"},
+                {name="localization_node", manager="mlp_vision", type="localization_node/LocalizationNodelet"},
+                {name="mapper", manager="mlp_mapper", type="mapper/MapperNodelet"},
+                {name="marker_tracking", manager="mlp_vision", type="marker_tracking_node/MarkerTrackingNodelet"},
+                {name="mlp_cpu_mem_monitor", manager="mlp_monitors", type="cpu_mem_monitor/CpuMemMonitor"},
+                {name="mlp_disk_monitor", manager="mlp_monitors", type="disk_monitor/DiskMonitor"},
+                {name="nav_cam", manager="mlp_vision", type="is_camera/camera"},
+                {name="optical_flow_nodelet", manager="mlp_vision", type="lk_optical_flow_node/LKOpticalFlowNodelet"},
+                {name="perch", manager="mlp_perch", type="perch/PerchNodelet"},
+                {name="perching_arm", manager="mlp_serial", type="perching_arm/PerchingArmNode"},
+                {name="pico_driver", manager="mlp_depth_cam", type="pico_driver/PicoDriverNodelet"},
+                {name="planner_qp", manager="mlp_depth_cam", type="planner_qp/Planner"},
+                {name="planner_trapezoidal", manager="mlp_mobility", type="planner_trapezoidal/PlannerTrapezoidalNodelet"},
+                {name="pmc_actuator", manager="llp_pmc", type="pmc_actuator/PmcActuatorNodelet"},
+                {name="signal_lights", manager="llp_lights", type="signal_lights/SignalLightsNodelet"},
+                {name="speed_cam", manager="llp_serial", type="speed_cam/SpeedCamNode"},
+                {name="states", manager="mlp_states", type="states/StatesNodelet"}}
 
 -- Keep constant with Command Args message file
 

astrobee/launch/robot/LLP.launch

@@ -50,7 +50,7 @@
   <group unless="$(arg ground_truth_localizer)">
     <include file="$(find ff_util)/launch/ff_nodelet.launch">
       <arg name="class" value="imu_augmentor/ImuAugmentorNodelet" />
-      <arg name="name" value="imu_augmentor_nodelet" />
+      <arg name="name" value="imu_aug" />
       <arg name="manager" value="llp_imu_aug" />
       <arg name="spurn" value="$(arg spurn)" />
       <arg name="nodes" value="$(arg nodes)" />
@@ -61,7 +61,7 @@
   </group>
   <include file="$(find ff_util)/launch/ff_nodelet.launch">
     <arg name="class" value="ctl/ctl" />
-    <arg name="name" value="ctl_node" />
+    <arg name="name" value="ctl" />
     <arg name="manager" value="llp_gnc" />
     <arg name="spurn" value="$(arg spurn)" />
     <arg name="nodes" value="$(arg nodes)" />
@@ -71,7 +71,7 @@
   </include>
   <include file="$(find ff_util)/launch/ff_nodelet.launch">
     <arg name="class" value="fam/fam" />
-    <arg name="name" value="fam_node" />
+    <arg name="name" value="fam" />
     <arg name="manager" value="llp_gnc" />
     <arg name="spurn" value="$(arg spurn)" />
     <arg name="nodes" value="$(arg nodes)" />

astrobee/launch/robot/MLP.launch

@@ -37,6 +37,8 @@
           name="mlp_vision" output="$(arg output)"/>
     <node pkg="nodelet" type="nodelet" args="manager"
           name="mlp_depth_cam" output="$(arg output)"/>
+    <node pkg="nodelet" type="nodelet" args="manager"
+          name="mlp_mapper" output="$(arg output)"/>
     <node pkg="nodelet" type="nodelet" args="manager"
           name="mlp_management" output="$(arg output)"/>
     <node pkg="nodelet" type="nodelet" args="manager"
@@ -89,7 +91,7 @@
   <group unless="$(arg ground_truth_localizer)">
     <include file="$(find ff_util)/launch/ff_nodelet.launch">
       <arg name="class" value="graph_localizer/GraphLocalizerNodelet" />
-      <arg name="name" value="graph_localizer" />
+      <arg name="name" value="graph_loc" />
       <arg name="manager" value="mlp_graph_localization" />
       <arg name="spurn" value="$(arg spurn)" />
       <arg name="nodes" value="$(arg nodes)" />
@@ -127,7 +129,7 @@
   <include file="$(find ff_util)/launch/ff_nodelet.launch">
     <arg name="class" value="mapper/MapperNodelet" />
     <arg name="name" value="mapper" />
-    <arg name="manager" value="mlp_depth_cam" />
+    <arg name="manager" value="mlp_mapper" />
     <arg name="spurn" value="$(arg spurn)" />
     <arg name="nodes" value="$(arg nodes)" />
     <arg name="extra" value="$(arg extra)" />

astrobee/resources/debug.config

@@ -36,8 +36,8 @@ log4j.logger.ros.Astrobee./nav_cam                     = DEBUG
 log4j.logger.ros.Astrobee./dock_cam                    = DEBUG
 log4j.logger.ros.Astrobee./marker_tracking             = DEBUG
 log4j.logger.ros.Astrobee./localization_node           = DEBUG
-log4j.logger.ros.Astrobee./ctl_node                    = DEBUG
-log4j.logger.ros.Astrobee./fam_node                    = DEBUG
+log4j.logger.ros.Astrobee./ctl                         = DEBUG
+log4j.logger.ros.Astrobee./fam                         = DEBUG
 log4j.logger.ros.Astrobee./executive                   = DEBUG
 log4j.logger.ros.Astrobee./dds_ros_bridge              = DEBUG
 log4j.logger.ros.Astrobee./framestore                  = DEBUG

communications/dds_msgs/idl/AstrobeeCommandConstants.idl

@@ -36,21 +36,21 @@ module rapid {
       //@copy-declaration /** Calibrates the bias parameters of Astrobee's IMU.<p/>Astrobee's IMU biases slowly drift over time. While Astrobee is actively running its localization system and maintaining a position fix, its EKF automatically corrects for bias drift under the assumption that the local environment is inertially fixed. If the localization system is turned off or loses its fix for an extended period, this command should be used to explicitly calibrate the biases prior to restarting motion. It works by integrating the robot's linear acceleration and angular velocity for about five seconds, and taking the resulting average values as the new zero. During the calibration period, the robot must remain stationary (e.g. docked or perched on a handrail, not held by an astronaut). */
       const rapid::String32 ADMIN_METHOD_INITIALIZE_BIAS = "initializeBias";
 
-      //@copy-declaration /** Not implemented. This command is intended to load a ROS nodelet in the Astrobee flight software.<p/>There is an internal service advertised by the system monitor that provides a method very similar to this command, but as of Oct 2020, there is no way to invoke it through a standard Astrobee API call. Also note, the underlying system monitor service can't load nodelets that run on the HLP. */
+      //@copy-declaration /** This command is intended to load a ROS nodelet in the Astrobee flight software. Please note, the underlying system monitor service can't load nodelets that run on the HLP. */
       const rapid::String32 ADMIN_METHOD_LOAD_NODELET = "loadNodelet";
 
       //@copy-declaration /** Name of nodelet */
       const rapid::String32 ADMIN_METHOD_LOAD_NODELET_PARAM_NODELET_NAME = "nodeletName";
       const rapid::DataType ADMIN_METHOD_LOAD_NODELET_DTYPE_NODELET_NAME = rapid::RAPID_STRING;
 
-      //@copy-declaration /** Type of nodelet (namespace/classname). If left blank, the system monitor will default to the type specified in its config file, which should normally work fine. */
-      const rapid::String32 ADMIN_METHOD_LOAD_NODELET_PARAM_TYPE = "type";
-      const rapid::DataType ADMIN_METHOD_LOAD_NODELET_DTYPE_TYPE = rapid::RAPID_STRING;
-
       //@copy-declaration /** Which nodelet manager should load the nodelet. If left blank, the system monitor will default to using the managerName specified in the last heartbeat it received from the requested nodelet. (That should normally work fine, assuming the nodelet sent out at least one valid heartbeat.) */
       const rapid::String32 ADMIN_METHOD_LOAD_NODELET_PARAM_MANAGER_NAME = "managerName";
       const rapid::DataType ADMIN_METHOD_LOAD_NODELET_DTYPE_MANAGER_NAME = rapid::RAPID_STRING;
 
+      //@copy-declaration /** Type of nodelet (namespace/classname). If left blank, the system monitor will default to the type specified in its config file, which should normally work fine. */
+      const rapid::String32 ADMIN_METHOD_LOAD_NODELET_PARAM_TYPE = "type";
+      const rapid::DataType ADMIN_METHOD_LOAD_NODELET_DTYPE_TYPE = rapid::RAPID_STRING;
+
       //@copy-declaration /** With the current implementation, you should leave this field blank. Specifying a non-empty string (unique id) will activate bond notifications.<p/>Background: ROS nodelets provide a 'bond' mechanism that enables the process requesting a nodelet load (in our case the system monitor) to receive automatic notification if that nodelet crashes. However, our system monitor instead relies on a custom heartbeat message to detect liveness, and it is not currently configured to do anything useful with bond notifications, even if they are activated. */
       const rapid::String32 ADMIN_METHOD_LOAD_NODELET_PARAM_BOND_ID = "bondId";
       const rapid::DataType ADMIN_METHOD_LOAD_NODELET_DTYPE_BOND_ID = rapid::RAPID_STRING;
@@ -68,7 +68,7 @@ module rapid {
       const rapid::String32 ADMIN_METHOD_SWITCH_LOCALIZATION_PARAM_MODE = "mode";
       const rapid::DataType ADMIN_METHOD_SWITCH_LOCALIZATION_DTYPE_MODE = rapid::RAPID_STRING;
 
-      //@copy-declaration /** Not implemented. This command is intended to unload a ROS nodelet in Astrobee's flight software. Its primary use is in the system monitor's fault response table, where it is the usual response if initialization of a nodelet fails. As of Oct 2020, there is an internal service that implements this function, but there is no way to invoke it through a standard Astrobee API call. Also, the internal service can't unload nodes running on the HLP. */
+      //@copy-declaration /** This command is intended to unload a ROS nodelet in Astrobee's flight software. Its primary use is in the system monitor's fault response table, where it is the usual response if initialization of a nodelet fails. Please note, the internal service can't unload nodes running on the HLP. */
       const rapid::String32 ADMIN_METHOD_UNLOAD_NODELET = "unloadNodelet";
 
       //@copy-declaration /** Name of nodelet */

communications/ff_msgs/srv/UnloadLoadNodelet.srv

@@ -12,8 +12,8 @@ string name           # Name of nodelet. Needed for both unload and load.
 # if the type is specified in the system monitor config.
 string type
 
-# Name of nodelet manager. Needed for load. May not be needed if the system
-# monitor had received a heartbeat before being unloaded.
+# Name of nodelet manager. Needed for both load and unload. May not be needed if
+# the system monitor received a heartbeat before being unloaded.
 string manager_name
 
 string[] remap_source_args  # Leave blank if no source args. Needed for load.