Astrobee comms work (nasa#504)

* Changed astrobee comms trigger service to enable.

* Changed response for start astrobee intercomms srv

The start astrobee intercommunication service now returns an error
message with what went wrong if the service fails. The astrobee to
astrobee bridge no longer kills itself if its config is wrong. This
allows an operator to fix the config and resend the enable astrobee
intercommunication command during run time.

* Added the enable astrobee intercomms command.

* Fixed lint error.

* Fixed major typo in apk print version script. (nasa#499)

astrobee/commands/freeFlyerPlanSchema.json

@@ -1238,6 +1238,19 @@
       ],
       "parent": "Command",
       "params": []
+    },
+
+    {
+      "type": "CommandSpec",
+      "id": "Settings.enableAstrobeeIntercomms",
+      "isRapidNative": false,
+      "notes": "Starts the Astrobee intercommunication software. This enables the robot to send and receive a subset of data to and from other Astrobees in the network. Please note, this command must be executed on each robot needed in the communication.",
+      "availableContext": [
+        "teleop",
+        "planner"
+      ],
+      "parent": "Command",
+      "params": []
     }
   ]
 }

astrobee/config/commands.config

@@ -332,6 +332,10 @@ commandConfig = {
     },
     {
       commands={
+        {
+          name="enableAstrobeeIntercomms",
+          parameters={}
+        },
         {
           name="setCamera",
           parameters={

communications/dds_msgs/idl/AstrobeeCommandConstants.idl

@@ -237,6 +237,9 @@ module rapid {
 
       const rapid::String32 SETTINGS = "Settings";
 
+      //@copy-declaration /** Starts the Astrobee intercommunication software. This enables the robot to send and receive a subset of data to and from other Astrobees in the network. Please note, this command must be executed on each robot needed in the communication. */
+      const rapid::String32 SETTINGS_METHOD_ENABLE_ASTROBEE_INTERCOMMS = "enableAstrobeeIntercomms";
+
       //@copy-declaration /** Sets camera parameters.<p/>The Astrobee camera control life cycle is as follows:<ul><li>When the Astrobee flight software stack is started, recording and streaming are initially disabled for all cameras, and the default camera parameters are as specified in the astrobee/config/cameras.config file.</li><li>For each camera, while recording and streaming are disabled, you may use this Settings.setCamera command to adjust its parameters.</li><li>For each camera, you may enable/disable recording imagery to onboard storage using Settings.setCameraRecording.</li><li>For each camera, you may enable/disable live imagery downlink using Settings.setCameraStreaming.</li></ul> */
       const rapid::String32 SETTINGS_METHOD_SET_CAMERA = "setCamera";
 

communications/dds_ros_bridge/include/dds_ros_bridge/astrobee_astrobee_bridge.h

@@ -20,7 +20,6 @@
 #define DDS_ROS_BRIDGE_ASTROBEE_ASTROBEE_BRIDGE_H_
 
 #include <pluginlib/class_list_macros.h>
-#include <std_srvs/Empty.h>
 
 #include <map>
 #include <memory>
@@ -38,6 +37,8 @@
 #include "dds_ros_bridge/ros_ekf_rapid_ekf.h"
 #include "dds_ros_bridge/ros_guest_science.h"
 
+#include "ff_msgs/ResponseOnly.h"
+
 #include "ff_util/ff_names.h"
 #include "ff_util/ff_nodelet.h"
 
@@ -74,11 +75,12 @@ class AstrobeeAstrobeeBridge : public ff_util::FreeFlyerNodelet {
   virtual void Initialize(ros::NodeHandle *nh);
   bool ReadParams();
   bool ReadSharedItemConf(config_reader::ConfigReader::Table &conf,
-                            std::string &topic_name, bool &enable, float &rate);
+                          std::string &topic_name, bool &enable, float &rate);
 
  private:
-  void Run();
-  bool Trigger(std_srvs::Empty::Request& req, std_srvs::Empty::Response & res);
+  bool Run();
+  bool Start(ff_msgs::ResponseOnly::Request& req,
+             ff_msgs::ResponseOnly::Response& res);
 
   config_reader::ConfigReader config_params_;
 
@@ -91,7 +93,7 @@ class AstrobeeAstrobeeBridge : public ff_util::FreeFlyerNodelet {
 
   std::map<std::string, ff::RosSubRapidPubPtr> ros_sub_rapid_pubs_;
   std::shared_ptr<kn::DdsEntitiesFactorySvc> dds_entities_factory_;
-  std::string agent_name_, participant_name_;
+  std::string agent_name_, participant_name_, read_params_error_;
   std::vector<ff::RapidPubPtr> rapid_pubs_;
   std::vector<ff::RapidSubRosPubPtr> rapid_sub_ros_pubs_;
 };

communications/dds_ros_bridge/src/astrobee_astrobee_bridge.cc

@@ -23,7 +23,8 @@ AstrobeeAstrobeeBridge::AstrobeeAstrobeeBridge() :
   ff_util::FreeFlyerNodelet(NODE_ASTROBEE_ASTROBEE_BRIDGE, true),
   components_(0),
   started_(false),
-  agent_name_("Bumble") {
+  agent_name_("Bumble"),
+  read_params_error_("") {
 }
 
 AstrobeeAstrobeeBridge::~AstrobeeAstrobeeBridge() {
@@ -170,25 +171,36 @@ void AstrobeeAstrobeeBridge::Initialize(ros::NodeHandle *nh) {
   dds_entities_factory_->init(dds_params);
 
   trigger_srv_ = nh->advertiseService(
-                      SERVICE_COMMUNICATIONS_ASTROBEE_ASTROBEE_BRIDGE_TRIGGER,
-                      &AstrobeeAstrobeeBridge::Trigger, this);
+                              SERVICE_COMMUNICATIONS_ENABLE_ASTROBEE_INTERCOMMS,
+                              &AstrobeeAstrobeeBridge::Start,
+                              this);
 
   if (run_on_start_) {
-    Run();
+    if (!Run()) {
+      ROS_ERROR_STREAM(read_params_error_);
+    }
   }
 }
 
-bool AstrobeeAstrobeeBridge::Trigger(std_srvs::Empty::Request& req,
-                                            std_srvs::Empty::Response & res) {
-  Run();
+bool AstrobeeAstrobeeBridge::Start(ff_msgs::ResponseOnly::Request& req,
+                                   ff_msgs::ResponseOnly::Response & res) {
+  if (!Run()) {
+    ROS_ERROR_STREAM(read_params_error_);
+    res.success = false;
+    res.status = read_params_error_;
+  } else {
+    res.success = true;
+  }
+
   return true;
 }
 
-void AstrobeeAstrobeeBridge::Run() {
+bool AstrobeeAstrobeeBridge::Run() {
   if (!started_ && !ReadParams()) {
-    exit(EXIT_FAILURE);
+    return false;
   }
   started_ = true;
+  return true;
 }
 
 bool AstrobeeAstrobeeBridge::ReadParams() {
@@ -200,11 +212,18 @@ bool AstrobeeAstrobeeBridge::ReadParams() {
 
   components_ = 0;
 
+  // Reload files. This allows the config file to be fixed if something is wrong
+  // during run time
+  if (!config_params_.ReadFiles()) {
+    read_params_error_ = "AstrobeeAstrobeeBridge: Error reading config files.";
+    return false;
+  }
   // ROS -> RAPID
   // -----------------------------
   // Load individual shared topics
   if (!config_params_.GetTable("share_topics", &share_topics)) {
-    ROS_FATAL("AstrobeeAstrobeeBridge: share_topics table not specified!");
+    read_params_error_ =
+                  "AstrobeeAstrobeeBridge: share_topics table not specified!";
     return false;
   }
 
@@ -221,14 +240,16 @@ bool AstrobeeAstrobeeBridge::ReadParams() {
                             "", "", nh_);
 
       if (ros_sub_rapid_pubs_.count(item_name) == 0) {
-        ROS_ERROR("AstrobeeAstrobeeBridge: %s not added and it is needed",
-                                                            item_name.c_str());
+        read_params_error_ = "AstrobeeAstrobeeBridge: ";
+        read_params_error_ += item_name.c_str();
+        read_params_error_ += " not added and it is needed.";
         return false;
       }
 
       if (rate < 0) {
-        ROS_ERROR("AstrobeeAstrobeeBridge: %s requires a non-negative rate",
-                                                            item_name.c_str());
+        read_params_error_ = "AstrobeeAstrobeeBridge: ";
+        read_params_error_ += item_name.c_str();
+        read_params_error_ += " requires a non-negative rate.";
         return false;
       }
 
@@ -243,7 +264,8 @@ bool AstrobeeAstrobeeBridge::ReadParams() {
 
   // Load shared topic groups
   if (!config_params_.GetTable("share_topic_groups", &share_topic_groups)) {
-    ROS_FATAL("AstrobeeAstrobeeBridge: share_topic_groups table not specified!");
+    read_params_error_ =
+              "AstrobeeAstrobeeBridge: share_topic_groups table not specified!";
     return false;
   }
 
@@ -270,23 +292,25 @@ bool AstrobeeAstrobeeBridge::ReadParams() {
   // ----------------------
   // Read listeners options
   if (!config_params_.GetTable("agents", &agents)) {
-    ROS_FATAL("AstrobeeAstrobeeBridge: agents not specified!");
+    read_params_error_ = "AstrobeeAstrobeeBridge: agents not specified!";
     return false;
   }
 
   for (int i = 1; i <= agents.GetSize(); i++) {
     // Agent configuration
     agents.GetTable(i, &item_conf);
     if (!item_conf.GetStr("name", &item_name)) {
-      ROS_FATAL("AstrobeeAstrobeeBridge: agent name not specified!");
+      read_params_error_ = "AstrobeeAstrobeeBridge: agent name not specified!";
       return false;
     }
     if (!item_conf.GetBool("enable", &enable)) {
-      ROS_FATAL("AstrobeeAstrobeeBridge: agent enable not specified!");
+      read_params_error_ =
+                          "AstrobeeAstrobeeBridge: agent enable not specified!";
       return false;
     }
     if (!item_conf.GetTable("topics", &topics)) {
-      ROS_FATAL("AstrobeeAstrobeeBridge: agent topics not specified!");
+      read_params_error_ =
+                          "AstrobeeAstrobeeBridge: agent topics not specified!";
       return false;
     }
 
@@ -305,11 +329,13 @@ bool AstrobeeAstrobeeBridge::ReadParams() {
       // Agent topic configuration
       topics.GetTable(j, &item_conf);
       if (!item_conf.GetStr("name", &topic_name)) {
-        ROS_FATAL("AstrobeeAstrobeeBridge: agent topic name not specified!");
+        read_params_error_ =
+                      "AstrobeeAstrobeeBridge: agent topic name not specified!";
         return false;
       }
       if (!item_conf.GetBool("enable", &topic_enable)) {
-        ROS_FATAL("AstrobeeAstrobeeBridge: agent topic enable not specified!");
+        read_params_error_ =
+                    "AstrobeeAstrobeeBridge: agent topic enable not specified!";
         return false;
       }
 
@@ -339,15 +365,18 @@ bool AstrobeeAstrobeeBridge::ReadSharedItemConf(
                           config_reader::ConfigReader::Table &conf,
                           std::string &topic_name, bool &enable, float &rate) {
   if (!conf.GetStr("name", &topic_name)) {
-    ROS_FATAL("AstrobeeAstrobeeBridge: share topic name not specified!");
+    read_params_error_ =
+                      "AstrobeeAstrobeeBridge: share topic name not specified!";
     return false;
   }
   if (!conf.GetBool("enable", &enable)) {
-    ROS_FATAL("AstrobeeAstrobeeBridge: share topic enable not specified!");
+    read_params_error_ =
+                    "AstrobeeAstrobeeBridge: share topic enable not specified!";
     return false;
   }
   if (!conf.GetReal("rate", &rate)) {
-    ROS_FATAL("AstrobeeAstrobeeBridge: ekf state rate not specified!");
+    read_params_error_ =
+                      "AstrobeeAstrobeeBridge: ekf state rate not specified!";
     return false;
   }
 

communications/ff_msgs/msg/CommandConstants.msg

@@ -91,6 +91,7 @@ string CMD_NAME_SKIP_PLAN_STEP = skipPlanStep
 string CMD_NAME_WAIT = wait
 string CMD_NAME_POWER_OFF_ITEM = powerOffItem
 string CMD_NAME_POWER_ON_ITEM = powerOnItem
+string CMD_NAME_ENABLE_ASTROBEE_INTERCOMMS = enableAstrobeeIntercomms
 string CMD_NAME_SET_CAMERA = setCamera
 string CMD_NAME_SET_CAMERA_RECORDING = setCameraRecording
 string CMD_NAME_SET_CAMERA_STREAMING = setCameraStreaming

communications/ff_msgs/srv/ResponseOnly.srv

@@ -0,0 +1,20 @@
+# Copyright (c) 2017, United States Government, as represented by the
+# Administrator of the National Aeronautics and Space Administration.
+# 
+# All rights reserved.
+# 
+# The Astrobee platform is licensed under the Apache License, Version 2.0
+# (the "License"); you may not use this file except in compliance with the
+# License. You may obtain a copy of the License at
+# 
+#     http://www.apache.org/licenses/LICENSE-2.0
+# 
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+# License for the specific language governing permissions and limitations
+# under the License.
+
+---
+bool success                  # Did the service work?
+string status                 # If not, why did it fail?

management/executive/include/executive/executive.h

@@ -51,6 +51,7 @@
 #include <ff_msgs/MotionAction.h>
 #include <ff_msgs/PerchAction.h>
 #include <ff_msgs/PlanStatusStamped.h>
+#include <ff_msgs/ResponseOnly.h>
 #include <ff_msgs/SetDataToDisk.h>
 #include <ff_msgs/SetFloat.h>
 #include <ff_msgs/SetInertia.h>
@@ -204,6 +205,7 @@ class Executive : public ff_util::FreeFlyerNodelet {
   bool CustomGuestScience(ff_msgs::CommandStampedPtr const& cmd);
   bool DeployArm(ff_msgs::CommandStampedPtr const& cmd);
   bool Dock(ff_msgs::CommandStampedPtr const& cmd);
+  bool EnableAstrobeeIntercomms(ff_msgs::CommandStampedPtr const& cmd);
   bool Fault(ff_msgs::CommandStampedPtr const& cmd);
   bool GripperControl(ff_msgs::CommandStampedPtr const& cmd);
   bool IdlePropulsion(ff_msgs::CommandStampedPtr const& cmd);
@@ -310,6 +312,7 @@ class Executive : public ff_util::FreeFlyerNodelet {
   ros::ServiceClient set_inertia_client_, set_rate_client_;
   ros::ServiceClient set_data_client_, enable_recording_client_;
   ros::ServiceClient eps_terminate_client_;
+  ros::ServiceClient enable_astrobee_intercommunication_client_;
   ros::ServiceClient unload_load_nodelet_client_;
   ros::ServiceClient set_collision_distance_client_;
 

management/executive/src/executive.cc

@@ -1664,11 +1664,44 @@ bool Executive::Dock(ff_msgs::CommandStampedPtr const& cmd) {
   return successful;
 }
 
+bool Executive::EnableAstrobeeIntercomms(ff_msgs::CommandStampedPtr const&
+                                                                          cmd) {
+  NODELET_INFO("Executive executing enable astrobee intercomms command!");
+
+  ff_msgs::ResponseOnly enable_astrobee_intercomms_srv;
+
+  if (!CheckServiceExists(enable_astrobee_intercommunication_client_,
+                          "Enable astrobee intercommunication",
+                          cmd->cmd_id)) {
+    return false;
+  }
+
+  if (!enable_astrobee_intercommunication_client_.call(
+                                              enable_astrobee_intercomms_srv)) {
+    state_->AckCmd(cmd->cmd_id,
+                   ff_msgs::AckCompletedStatus::EXEC_FAILED,
+                   "Enable astrobee intercommunication service returned false");
+    return false;
+  }
+
+  if (!enable_astrobee_intercomms_srv.response.success) {
+    state_->AckCmd(cmd->cmd_id,
+                   ff_msgs::AckCompletedStatus::EXEC_FAILED,
+                   ("Enable astrobee intercommunication failed with result: " +
+                    enable_astrobee_intercomms_srv.response.status));
+    return false;
+  }
+
+  state_->AckCmd(cmd->cmd_id);
+  return true;
+}
+
 bool Executive::Fault(ff_msgs::CommandStampedPtr const& cmd) {
   NODELET_INFO("Executive executing fault command!");
+
   // Only transition to the fault state if the fault command came from the
   // system monitor or executive. The only way to transition out of the fault
-  // state is if the system monitor state changes to functional so we don't wan
+  // state is if the system monitor state changes to functional so we don't want
   // the ground to issue a fault command because there will be no way to
   // transition out of the fault state
   if (cmd->cmd_src == "sys_monitor" || cmd->cmd_src == "executive") {
@@ -1692,6 +1725,7 @@ bool Executive::GripperControl(ff_msgs::CommandStampedPtr const& cmd) {
 
 bool Executive::IdlePropulsion(ff_msgs::CommandStampedPtr const& cmd) {
   NODELET_INFO("Executive executing idle propulsion command!");
+
   // Cancel any motion actions being executed include the arm
   unsigned int i = 0;
   for (i = 0; i < running_actions_.size(); i++) {
@@ -3735,6 +3769,10 @@ void Executive::Initialize(ros::NodeHandle *nh) {
   eps_terminate_client_ = nh_.serviceClient<ff_hw_msgs::ClearTerminate>(
                                           SERVICE_HARDWARE_EPS_CLEAR_TERMINATE);
 
+  enable_astrobee_intercommunication_client_ =
+      nh_.serviceClient<ff_msgs::ResponseOnly>(
+                            SERVICE_COMMUNICATIONS_ENABLE_ASTROBEE_INTERCOMMS);
+
   unload_load_nodelet_client_ = nh_.serviceClient<ff_msgs::UnloadLoadNodelet>(
                             SERVICE_MANAGEMENT_SYS_MONITOR_UNLOAD_LOAD_NODELET);
 

management/executive/src/op_state.cc

@@ -51,7 +51,9 @@ OpState* OpState::HandleCmd(ff_msgs::CommandStampedPtr const& cmd,
                             bool& successful) {
   completed = true;
   successful = true;
-  if (cmd->cmd_name == CommandConstants::CMD_NAME_NO_OP) {
+  if (cmd->cmd_name == CommandConstants::CMD_NAME_ENABLE_ASTROBEE_INTERCOMMS) {
+    successful = exec_->EnableAstrobeeIntercomms(cmd);
+  } else if (cmd->cmd_name == CommandConstants::CMD_NAME_NO_OP) {
     successful = exec_->NoOp(cmd);
   } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SET_CAMERA) {
     successful = exec_->SetCamera(cmd);

management/executive/src/utils/sequencer/command_conversion.cc

@@ -342,6 +342,8 @@ extern const std::unordered_map<std::string, CommandInfo> kCmdGenMap = {
   { jl::kCmdStartRecord,
     { cc::CMD_NAME_START_RECORDING, cc::CMD_SUBSYS_DATA, GenStartRecording } },
   { jl::kCmdStopRecord, { cc::CMD_NAME_STOP_RECORDING, cc::CMD_SUBSYS_DATA, GenNoop } },
+  { jl::kCmdEnableAstrobeeIntercomms,
+    { cc::CMD_NAME_ENABLE_ASTROBEE_INTERCOMMS, cc::CMD_SUBSYS_SETTINGS, GenNoop } }
 };
 
 }  // namespace internal