Feature/humble isaac improvements (#532)

* progress on several smacc client behaviors related with ros, wait action, ros launch , sleep, wait topic, etc

* minor

* missing

* Añadidos archivos nuevos y cambios pequeños de archivos preexistentes de smacc2 (loggers y poco más)

* Cierre de procesos lanzados por CbRoslaunch2 de forma recursiva funcionando

* Borrado de funcion residual no utilizada en client_bases/smacc_ros_launch_client_2.cpp

* Limpiado del fichero client_bases/smacc_ros_launch_client_2.cpp

* format

* Cambios hechos para mejorar codigo. Dejar result_ de CbRoslaunch en auto para que se quede como bloqueo de on Entry y lance forceFinish de onEntry, si se pone como this-> sale de onEntry y bloquea. Añadidos archivos en desarrollo cb_ros_stop_2 con ideas iniciales de funcionamiento

* Fixed and improvements cblaunch2 and cbstop2

---------

Co-authored-by: atobaruela-ibrobotics <atobaruela@ibrobotics.com>

smacc2/include/smacc2/client_bases/smacc_action_client_base.hpp

@@ -84,6 +84,14 @@ class SmaccActionClientBase : public ISmaccActionClient
     //client_->wait_for_action_server();
   }
 
+  void waitForServer()
+  {
+    RCLCPP_INFO_STREAM(
+      this->getLogger(),
+      "Waiting for action server '" << name_ << "' of type: " << demangledTypeName<ActionType>());
+    client_->wait_for_action_server();
+  }
+
   static std::string getEventLabel()
   {
     auto type = TypeInfo::getTypeInfoFromType<ActionType>();
@@ -327,13 +335,11 @@ class SmaccActionClientBase : public ISmaccActionClient
     this->lastRequest_ = lastRequest;
 
     RCLCPP_INFO_STREAM(
-      getLogger(),
-      "["
-        << getName()
-        << "] Action request "
-        // << rclcpp_action::to_string(this->goalHandle_->get_goal_id()) <<". Goal sent to " << this->action_endpoint_
-        << "\": " << std::endl
-        << goal);
+      getLogger(), "[" << getName() << "] Action request "
+      // << rclcpp_action::to_string(this->goalHandle_->get_goal_id()) <<". Goal sent to " << this->action_endpoint_
+      // << "\": " << std::endl
+      // << goal
+    );
 
     // if (client_->isServerConnected())
     // {

smacc2/include/smacc2/client_bases/smacc_ros_launch_client.hpp

@@ -51,6 +51,10 @@ class ClRosLaunch : public ISmaccClient
 protected:
   std::future<std::string> result_;
 
+  typedef std::function<void> cancelCallback;
+
+  static std::map<std::future<void>, cancelCallback> detached_futures_;
+
   std::atomic<bool> cancellationToken_ = ATOMIC_VAR_INIT(false);
 };
 }  // namespace client_bases

smacc2/include/smacc2/client_bases/smacc_ros_launch_client_2.hpp

@@ -0,0 +1,80 @@
+// 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 <smacc2/smacc_client.hpp>
+#include <smacc2/smacc_state_machine.hpp>
+
+#include <rclcpp_action/client.hpp>
+#include <thread>
+
+namespace smacc2
+{
+namespace client_bases
+{
+struct ProcessInfo
+{
+  pid_t pid;    // PID del proceso hijo
+  FILE * pipe;  // Pipe para la salida del proceso hijo
+};
+
+ProcessInfo runProcess(const char * command);
+void killGrandchildren(pid_t originalPid);
+void killProcessesRecursive(pid_t pid);
+
+class ClRosLaunch2 : public ISmaccClient
+{
+public:
+  ClRosLaunch2();
+
+  ClRosLaunch2(std::string packageName, std::string launchFilename);
+
+  virtual ~ClRosLaunch2();
+
+  void launch();
+
+  void stop();
+
+  static std::future<std::string> executeRosLaunch(
+    std::string packageName, std::string launchFilename, std::function<bool()> cancelCondition,
+    ClRosLaunch2 * client = nullptr);
+
+  // static std::string executeRosLaunch(
+  //    std::string packageName, std::string launchFilename, std::function<bool()> cancelCondition);
+
+  std::string packageName_;
+
+  std::string launchFileName_;
+
+  pid_t launchPid_;
+
+protected:
+  // std::future<std::string> result_;
+  std::future<std::string> result_;
+
+  typedef std::function<void> cancelCallback;
+
+  static std::map<std::future<void>, cancelCallback> detached_futures_;
+
+  std::atomic<bool> cancellationToken_ = ATOMIC_VAR_INIT(false);
+};
+}  // namespace client_bases
+}  // namespace smacc2

smacc2/include/smacc2/client_behaviors/cb_call_service.hpp

@@ -45,38 +45,41 @@ class CbServiceCall : public smacc2::SmaccAsyncClientBehavior
 
   void onEntry() override
   {
-    RCLCPP_DEBUG_STREAM(
+    RCLCPP_INFO_STREAM(
       getLogger(), "[" << this->getName() << "] creating ros service client: " << serviceName_);
 
     client_ = getNode()->create_client<ServiceType>(serviceName_);
 
-    RCLCPP_DEBUG_STREAM(
+    RCLCPP_INFO_STREAM(
       getLogger(), "[" << this->getName() << "] making service request to " << serviceName_);
 
     resultFuture_ = client_->async_send_request(request_).future.share();
 
     std::future_status status = resultFuture_.wait_for(0s);
 
-    RCLCPP_DEBUG_STREAM(
+    RCLCPP_INFO_STREAM(
       getLogger(), "thread state: " << (int)status << " ok " << rclcpp::ok() << " shutdown "
                                     << this->isShutdownRequested() << "");
+
     while (status != std::future_status::ready && rclcpp::ok() && !this->isShutdownRequested())
     {
-      RCLCPP_DEBUG_STREAM(getLogger(), "[" << this->getName() << "] waiting response ");
+      RCLCPP_INFO_STREAM_THROTTLE(
+        getLogger(), *getNode()->get_clock(), 1000,
+        "[" << this->getName() << "] waiting response ");
       rclcpp::sleep_for(pollRate_);
       status = resultFuture_.wait_for(0s);
     }
 
     if (status == std::future_status::ready)
     {
-      RCLCPP_DEBUG_STREAM(getLogger(), "[" << this->getName() << "] response received ");
+      RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] response received ");
       result_ = resultFuture_.get();
       onServiceResponse(result_);
       this->postSuccessEvent();
     }
     else
     {
-      RCLCPP_DEBUG_STREAM(getLogger(), "[" << this->getName() << "] response not received ");
+      RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] response not received ");
       this->postFailureEvent();
     }
   }
@@ -94,7 +97,7 @@ class CbServiceCall : public smacc2::SmaccAsyncClientBehavior
 
   virtual void onServiceResponse(std::shared_ptr<typename ServiceType::Response> /*result*/)
   {
-    RCLCPP_DEBUG_STREAM(getLogger(), "[" << this->getName() << "] response received ");
+    RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] response received ");
   }
 };
 

smacc2/include/smacc2/client_behaviors/cb_ros_launch.hpp

@@ -26,11 +26,22 @@ namespace smacc2
 {
 namespace client_behaviors
 {
+enum class RosLaunchMode
+{
+  LAUNCH_DETTACHED,
+  LAUNCH_CLIENT_BEHAVIOR_LIFETIME
+};
+
 class CbRosLaunch : public smacc2::SmaccAsyncClientBehavior
 {
+private:
+  static std::vector<std::future<std::string>> detached_futures_;
+
 public:
   CbRosLaunch();
 
+  CbRosLaunch(std::string package, std::string launchfile, RosLaunchMode);
+
   // CbRosLaunch(std::string packageName, std::string launchFileName);
 
   virtual ~CbRosLaunch();
@@ -46,10 +57,14 @@ class CbRosLaunch : public smacc2::SmaccAsyncClientBehavior
   std::optional<std::string> packageName_;
   std::optional<std::string> launchFileName_;
 
+  RosLaunchMode launchMode_;
+
 protected:
   std::string result_;
 
   smacc2::client_bases::ClRosLaunch * client_;
+
+  std::future<std::string> future_;
 };
 }  // namespace client_behaviors
 }  // namespace smacc2

smacc2/include/smacc2/client_behaviors/cb_ros_launch_2.hpp

@@ -0,0 +1,70 @@
+// 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 <smacc2/client_bases/smacc_ros_launch_client_2.hpp>
+#include <smacc2/smacc_asynchronous_client_behavior.hpp>
+
+namespace smacc2
+{
+namespace client_behaviors
+{
+enum class RosLaunchMode
+{
+  LAUNCH_DETTACHED,
+  LAUNCH_CLIENT_BEHAVIOR_LIFETIME
+};
+
+class CbRosLaunch2 : public smacc2::SmaccAsyncClientBehavior
+{
+private:
+  static std::vector<std::future<std::string>> detached_futures_;
+
+public:
+  CbRosLaunch2();
+
+  CbRosLaunch2(std::string package, std::string launchfile, RosLaunchMode);
+
+  // CbRosLaunch2(std::string packageName, std::string launchFileName);
+
+  virtual ~CbRosLaunch2();
+
+  template <typename TOrthogonal, typename TSourceObject>
+  void onOrthogonalAllocation()
+  {
+    smacc2::SmaccAsyncClientBehavior::onOrthogonalAllocation<TOrthogonal, TSourceObject>();
+  }
+
+  void onEntry() override;
+
+  std::optional<std::string> packageName_;
+  std::optional<std::string> launchFileName_;
+
+  RosLaunchMode launchMode_;
+
+protected:
+  std::future<std::string> result_;
+
+  smacc2::client_bases::ClRosLaunch2 * client_;
+
+  std::future<std::string> future_;
+};
+}  // namespace client_behaviors
+}  // namespace smacc2

smacc2/include/smacc2/client_behaviors/cb_ros_stop_2.hpp

@@ -0,0 +1,62 @@
+// 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 <smacc2/client_bases/smacc_ros_launch_client_2.hpp>
+#include <smacc2/smacc_asynchronous_client_behavior.hpp>
+
+namespace smacc2
+{
+namespace client_behaviors
+{
+class CbRosStop2 : public smacc2::SmaccAsyncClientBehavior
+{
+private:
+  static std::vector<std::future<std::string>> detached_futures_;
+
+public:
+  CbRosStop2();
+
+  CbRosStop2(pid_t launchPid);
+
+  // CbRosStop2(std::string packageName, std::string launchFileName);
+
+  virtual ~CbRosStop2();
+
+  template <typename TOrthogonal, typename TSourceObject>
+  void onOrthogonalAllocation()
+  {
+    smacc2::SmaccAsyncClientBehavior::onOrthogonalAllocation<TOrthogonal, TSourceObject>();
+  }
+
+  void onEntry() override;
+
+  std::optional<std::string> packageName_;
+  std::optional<std::string> launchFileName_;
+
+protected:
+  std::future<std::string> result_;
+
+  smacc2::client_bases::ClRosLaunch2 * client_;
+
+  std::future<std::string> future_;
+};
+}  // namespace client_behaviors
+}  // namespace smacc2

smacc2/include/smacc2/client_behaviors/cb_sleep_for.hpp

@@ -35,7 +35,13 @@ class CbSleepFor : public smacc2::SmaccAsyncClientBehavior
 
   void onEntry() override
   {
-    rclcpp::sleep_for(std::chrono::nanoseconds(sleeptime_.nanoseconds()));
+    auto starttime = getNode()->now();
+    while (!this->isShutdownRequested() && (getNode()->now() - starttime) < sleeptime_)
+    {
+      rclcpp::sleep_for(10ms);
+    }
+
+    //rclcpp::sleep_for(std::chrono::nanoseconds(sleeptime_.nanoseconds()));
     this->postSuccessEvent();
   }