diff --git a/management/executive/tools/plan_pub.cc b/management/executive/tools/plan_pub.cc index a263e73f7d..5f6a2c2777 100644 --- a/management/executive/tools/plan_pub.cc +++ b/management/executive/tools/plan_pub.cc @@ -42,6 +42,8 @@ #include #include +#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(sub_topic_plan, 10, diff --git a/management/executive/tools/teleop_tool.cc b/management/executive/tools/teleop_tool.cc index f38bffe202..d0759dfe32 100644 --- a/management/executive/tools/teleop_tool.cc +++ b/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; }