July demo cargo fixes (nasa#225)

* always deploy to 0,0 if commanded * taking out more warnings * adding comment
KOSASIH · Jul 7, 2021 · c76ac1a · c76ac1a
1 parent 45e422f
commit c76ac1a
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 3 deletions.
diff --git a/behaviors/arm/src/arm_nodelet.cc b/behaviors/arm/src/arm_nodelet.cc
@@ -156,9 +156,8 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet {
 
     // STOWED -> DEPLOYING_PANNING
     //   [label="[5]\nGOAL_DEPLOY\nPan(DEPLOY)"];
-    fsm_.Add(STATE::STOWED,
-      GOAL_DEPLOY,
-      [this](FSM::Event const& event) -> FSM::State {
+    fsm_.Add(GOAL_DEPLOY,
+      [this](FSM::State const& state, FSM::Event const& event) -> FSM::State {
         if (!Arm(PAN))
           return Result(RESPONSE::PAN_FAILED);
         return STATE::DEPLOYING_PANNING;

diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer_nodelet.h b/localization/graph_localizer/include/graph_localizer/graph_localizer_nodelet.h
@@ -120,6 +120,9 @@ class GraphLocalizerNodelet : public ff_util::FreeFlyerNodelet {
   GraphLocalizerNodeletParams params_;
   int last_mode_ = -1;
 
+  ros::Time last_time_tf_dock_;
+  ros::Time last_time_tf_handrail_;
+
   // Timers
   localization_common::RosTimer vl_timer_ = localization_common::RosTimer("VL msg");
   localization_common::RosTimer of_timer_ = localization_common::RosTimer("OF msg");

diff --git a/localization/graph_localizer/src/graph_localizer_nodelet.cc b/localization/graph_localizer/src/graph_localizer_nodelet.cc
@@ -262,6 +262,10 @@ void GraphLocalizerNodelet::PublishWorldTDockTF() {
   const auto world_T_dock = graph_localizer_wrapper_.estimated_world_T_dock();
   const auto world_T_dock_tf =
     lc::PoseToTF(world_T_dock, "world", "dock/body", lc::TimeFromRosTime(ros::Time::now()), platform_name_);
+  // If the rate is higher than the sim time, prevent repeated timestamps
+  if (world_T_dock_tf.header.stamp == last_time_tf_dock_)
+    return;
+  last_time_tf_dock_ = world_T_dock_tf.header.stamp;
   transform_pub_.sendTransform(world_T_dock_tf);
 }
 
@@ -270,6 +274,10 @@ void GraphLocalizerNodelet::PublishWorldTHandrailTF() {
   if (!world_T_handrail) return;
   const auto world_T_handrail_tf = lc::PoseToTF(world_T_handrail->pose, "world", "handrail/body",
                                                 lc::TimeFromRosTime(ros::Time::now()), platform_name_);
+  // If the rate is higher than the sim time, prevent repeated timestamps
+  if (world_T_handrail_tf.header.stamp == last_time_tf_handrail_)
+    return;
+  last_time_tf_handrail_ = world_T_handrail_tf.header.stamp;
   transform_pub_.sendTransform(world_T_handrail_tf);
 }