progress sleep for refactoring (#484)

* progress sleep for refactoring

* minor

smacc2/include/smacc2/client_behaviors/cb_sleep_for.hpp

@@ -0,0 +1,47 @@
+// Copyright 2021 RobosoftAI Inc.
+//
+// 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.
+
+/*****************************************************************************************************************
+ *
+ * 	 Authors: Pablo Inigo Blasco, Brett Aldrich
+ *
+ ******************************************************************************************************************/
+
+#pragma once
+
+#include <rclcpp/duration.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <smacc2/smacc_asynchronous_client_behavior.hpp>
+
+namespace smacc2::client_behaviors
+{
+using namespace std::chrono_literals;
+
+class CbSleepFor : public smacc2::SmaccAsyncClientBehavior
+{
+public:
+  CbSleepFor(rclcpp::Duration sleeptime) : sleeptime_(sleeptime) {}
+
+  void onEntry() override
+  {
+    rclcpp::sleep_for(std::chrono::nanoseconds(sleeptime_.nanoseconds()));
+    this->postSuccessEvent();
+  }
+
+  void onExit() override {}
+
+private:
+  rclcpp::Duration sleeptime_;
+};
+}  // namespace smacc2::client_behaviors

smacc2/include/smacc2/client_behaviors/cb_wait_node.hpp

@@ -24,9 +24,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <smacc2/smacc_asynchronous_client_behavior.hpp>
 
-namespace smacc2
-{
-namespace client_behaviors
+namespace smacc2::client_behaviors
 {
 using namespace std::chrono_literals;
 
@@ -44,5 +42,4 @@ class CbWaitNode : public smacc2::SmaccAsyncClientBehavior
 
   rclcpp::Rate rate_;
 };
-}  // namespace client_behaviors
-}  // namespace smacc2
+}  // namespace smacc2::client_behaviors

smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors.hpp

@@ -22,6 +22,7 @@
 #pragma once
 
 #include <sensor_msgs/msg/joint_state.h>
+#include <smacc2/client_behaviors/cb_sleep_for.hpp>
 
 namespace sm_panda_moveit2z_cb_inventory
 {
@@ -32,6 +33,8 @@ using smacc2::default_transition_tags::SUCCESS;
 using namespace smacc2;
 using namespace cl_moveit2z;
 using smacc2::client_behaviors::CbWaitTopicMessage;
+using smacc2::client_behaviors::CbSleepFor;
+using namespace std::chrono_literals;
 
 // STATE DECLARATION
 struct StAcquireSensors : smacc2::SmaccState<StAcquireSensors, SmPandaMoveit2zCbInventory>
@@ -48,6 +51,7 @@ struct StAcquireSensors : smacc2::SmaccState<StAcquireSensors, SmPandaMoveit2zCb
   static void staticConfigure()
   {
     configure_orthogonal<OrArm, CbWaitTopicMessage<sensor_msgs::msg::JointState>>("/joint_states");
+    configure_orthogonal<OrArm, CbSleepFor>(5s);
   };
 
   void runtimeConfigure() {}