Changed the teleop and plan pub tools to work with a remote robot (na…

…sa#759)

management/executive/tools/plan_pub.cc

@@ -42,6 +42,8 @@
 #include <string>
 #include <vector>
 
+#define MAX_COUNT 6
+
 namespace fs = boost::filesystem;
 namespace io = boost::iostreams;
 
@@ -57,6 +59,8 @@ bool ValidateCompression(const char* name, std::string const &value) {
 
 DEFINE_string(compression, "none",
               "Type of compression [none, deflate, gzip]");
+DEFINE_string(ns, "", "Robot namespace");
+DEFINE_bool(remote, false, "Whether target command is remote robot");
 
 constexpr uintmax_t kMaxSize = 128 * 1024;
 
@@ -75,7 +79,10 @@ void on_cf_ack(ff_msgs::CompressedFileAckConstPtr const& cf_ack) {
   // compressed file ack is latched so we need to check the timestamp to make
   // sure this plan is being acked
   // ROS_WARN_STREAM(plan_pub_time << " : " << cf_ack->header.stamp);
-  if (plan_pub_time <= cf_ack->header.stamp) {
+
+  // If remote and in the granite lab, the clocks of the robots might not be
+  // properly synchronized because we do it manually
+  if (plan_pub_time <= cf_ack->header.stamp + ros::Duration(5.0)) {
     ROS_INFO("Compressed file ack is valid! Sending set plan!");
     ff_msgs::CommandStamped cmd;
     cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_SET_PLAN;
@@ -90,7 +97,10 @@ void on_plan_status(ff_msgs::PlanStatusStamped::ConstPtr const& ps) {
   // plan status is latched so we need to check the timestamp to make sure this
   // plan is loaded
   // ROS_WARN_STREAM(plan_pub_time << " : " << ps->header.stamp);
-  if (plan_pub_time <= ps->header.stamp) {
+
+  // If remote and in the granite lab, the clocks of the robots might not be
+  // properly synchronized because we do it manually
+  if (plan_pub_time <= ps->header.stamp + ros::Duration(5.0)) {
     ff_msgs::CommandStamped cmd;
     cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_RUN_PLAN;
     cmd.subsys_name = "Astrobee";
@@ -104,7 +114,7 @@ void on_plan_status(ff_msgs::PlanStatusStamped::ConstPtr const& ps) {
 int main(int argc, char** argv) {
   ff_common::InitFreeFlyerApplication(&argc, &argv);
   ros::init(argc, argv, "plan_pub");
-  ros::NodeHandle n;
+  ros::NodeHandle n(std::string("/") + FLAGS_ns);
 
   ros::Time::waitForValid();
 
@@ -172,11 +182,19 @@ int main(int argc, char** argv) {
   ros::Subscriber plan_status_sub =
     n.subscribe(TOPIC_MANAGEMENT_EXEC_PLAN_STATUS, 10, &on_plan_status);
 
-  while (cf_ack_sub.getNumPublishers() == 0 ||
+  // Timeout if it can't find anything in 3s or if remote
+  // If remote, this wait allows enough spin
+  int count = 0;
+  while ((cf_ack_sub.getNumPublishers() == 0 ||
          plan_status_sub.getNumPublishers() == 0 ||
-         command_pub.getNumSubscribers() == 0) {
+         command_pub.getNumSubscribers() == 0) && count < MAX_COUNT) {
+    count++;
     ros::Duration(0.5).sleep();
   }
+  if (count == MAX_COUNT && !FLAGS_remote) {
+    ROS_ERROR("Could not connect");
+    return 1;
+  }
 
   ros::Publisher plan_pub =
     n.advertise<ff_msgs::CompressedFile>(sub_topic_plan, 10,

management/executive/tools/teleop_tool.cc

@@ -55,6 +55,7 @@ DEFINE_bool(undock, false, "Send undock command");
 DEFINE_bool(relative, false, "Position is relative to current point");
 DEFINE_bool(reset_bias, false, "Send initialize bias command");
 DEFINE_bool(reset_ekf, false, "Send reset ekf command");
+DEFINE_bool(remote, false, "Whether target command is remote robot");
 
 DEFINE_double(accel, -1.0, "Desired acceleration");
 DEFINE_double(alpha, -1.0, "Desired angular acceleration");
@@ -586,6 +587,7 @@ void AckCallback(ff_msgs::AckStampedConstPtr const& ack) {
     std::cout << "\n" << ack->cmd_id << " command failed! " << ack->message;
     std::cout << "\n";
     ros::shutdown();
+    exit(1);
     return;
   }
   if (Finished()) {
@@ -736,7 +738,7 @@ int main(int argc, char** argv) {
 
   // Hacky time out
   int count = 0;
-  while (ack_sub.getNumPublishers() == 0) {
+  while (ack_sub.getNumPublishers() == 0 && !FLAGS_remote) {
     ros::Duration(0.2).sleep();
     // Only wait 2 seconds
     if (count == 9) {
@@ -748,7 +750,7 @@ int main(int argc, char** argv) {
   }
 
   // If the user wants to get pose or move, get the current pose of the robot
-  if (FLAGS_get_pose || FLAGS_move) {
+  if (FLAGS_get_pose || (FLAGS_move && !FLAGS_remote)) {
     std::string ns = FLAGS_ns;
     // Wait for transform listener to start up
     ros::Duration(1.0).sleep();
@@ -802,7 +804,7 @@ int main(int argc, char** argv) {
     dock_sub = nh.subscribe(topic_name, 10, &DockFeedbackCallback);
     // Hacky time out
     int dock_count = 0;
-    while (dock_sub.getNumPublishers() == 0) {
+    while (dock_sub.getNumPublishers() == 0 && !FLAGS_remote) {
       ros::Duration(0.2).sleep();
       // Only wait 2 seconds
       if (dock_count == 9) {
@@ -814,6 +816,16 @@ int main(int argc, char** argv) {
     }
   }
 
+  // If remote, spin for seconds
+  if (FLAGS_remote) {
+    ros::Rate loop_rate(10);
+    ros::Time start_time = ros::Time::now();
+
+    // Spin for 3 seconds
+    while (ros::Time::now() - start_time < ros::Duration(3.0))
+        loop_rate.sleep();
+  }
+
   if (!SendNextCommand()) {
     return 1;
   }