Fixed and improvements cblaunch2 and cbstop2

robosoft-ai · Nov 6, 2023 · 6040cee · 6040cee
1 parent e391896
commit 6040cee
Show file tree

Hide file tree

Showing 5 changed files with 36 additions and 33 deletions.
diff --git a/smacc2/include/smacc2/client_bases/smacc_ros_launch_client_2.hpp b/smacc2/include/smacc2/client_bases/smacc_ros_launch_client_2.hpp
@@ -43,6 +43,8 @@ void killProcessesRecursive(pid_t pid);
 class ClRosLaunch2 : public ISmaccClient
 {
 public:
+  ClRosLaunch2();
+
   ClRosLaunch2(std::string packageName, std::string launchFilename);
 
   virtual ~ClRosLaunch2();
@@ -52,7 +54,8 @@ class ClRosLaunch2 : public ISmaccClient
   void stop();
 
   static std::future<std::string> executeRosLaunch(
-    std::string packageName, std::string launchFilename, std::function<bool()> cancelCondition,ClRosLaunch2* client=nullptr);
+    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);

diff --git a/smacc2/include/smacc2/client_behaviors/cb_ros_stop_2.hpp b/smacc2/include/smacc2/client_behaviors/cb_ros_stop_2.hpp
@@ -26,12 +26,6 @@ namespace smacc2
 {
 namespace client_behaviors
 {
-enum class RosLaunchMode
-{
-  LAUNCH_DETTACHED,
-  LAUNCH_CLIENT_BEHAVIOR_LIFETIME
-};
-
 class CbRosStop2 : public smacc2::SmaccAsyncClientBehavior
 {
 private:
@@ -57,8 +51,6 @@ class CbRosStop2 : public smacc2::SmaccAsyncClientBehavior
   std::optional<std::string> packageName_;
   std::optional<std::string> launchFileName_;
 
-  RosLaunchMode launchMode_;
-
 protected:
   std::future<std::string> result_;
 

diff --git a/smacc2/src/smacc2/client_bases/smacc_ros_launch_client_2.cpp b/smacc2/src/smacc2/client_bases/smacc_ros_launch_client_2.cpp
@@ -38,6 +38,11 @@ namespace smacc2
 namespace client_bases
 {
 using namespace std::chrono_literals;
+ClRosLaunch2::ClRosLaunch2(/*std::string packageName, std::string launchFilename*/)
+: /*packageName_(std::nullopt), launchFileName_(std::nullopt),*/ cancellationToken_(false)
+{
+}
+
 ClRosLaunch2::ClRosLaunch2(std::string packageName, std::string launchFilename)
 : packageName_(packageName), launchFileName_(launchFilename), cancellationToken_(false)
 {
@@ -47,6 +52,7 @@ ClRosLaunch2::~ClRosLaunch2() {}
 
 void ClRosLaunch2::launch()
 {
+  cancellationToken_.store(false);
   // Iniciar el hilo para la ejecución del lanzamiento
   this->result_ = /*std::async([this]()*/
                   // {
@@ -61,7 +67,8 @@ void ClRosLaunch2::stop()
 }
 
 std::future<std::string> ClRosLaunch2::executeRosLaunch(
-  std::string packageName, std::string launchFileName, std::function<bool()> cancelCondition, ClRosLaunch2* client)
+  std::string packageName, std::string launchFileName, std::function<bool()> cancelCondition,
+  ClRosLaunch2 * client)
 // std::string ClRosLaunch2::executeRosLaunch(std::string packageName, std::string launchFileName, std::function<bool()> cancelCondition)
 {
   return std::async(
@@ -81,9 +88,10 @@ std::future<std::string> ClRosLaunch2::executeRosLaunch(
       {
         throw std::runtime_error("popen() failed!");
       }
-        if(client != nullptr){
+      if (client != nullptr)
+      {
         client->launchPid_ = child.pid;
-        }
+      }
 
       int fd = fileno(child.pipe);
 
@@ -247,9 +255,7 @@ void killProcessesRecursive(pid_t pid)
   }
 }
 
-void killGrandchildren(pid_t originalPid) { 
-    killProcessesRecursive(originalPid);
-}
+void killGrandchildren(pid_t originalPid) { killProcessesRecursive(originalPid); }
 }  // namespace client_bases
 }  // namespace smacc2
 

diff --git a/smacc2/src/smacc2/client_behaviors/cb_ros_launch_2.cpp b/smacc2/src/smacc2/client_behaviors/cb_ros_launch_2.cpp
@@ -28,12 +28,13 @@ std::vector<std::future<std::string>> CbRosLaunch2::detached_futures_;
 CbRosLaunch2::CbRosLaunch2()
 : packageName_(std::nullopt),
   launchFileName_(std::nullopt),
-  launchMode_(RosLaunchMode::LAUNCH_CLIENT_BEHAVIOR_LIFETIME)
+  launchMode_(RosLaunchMode::LAUNCH_CLIENT_BEHAVIOR_LIFETIME),
+  client_(nullptr)
 {
 }
 
 CbRosLaunch2::CbRosLaunch2(std::string package, std::string launchfile, RosLaunchMode launchmode)
-: packageName_(package), launchFileName_(launchfile), launchMode_(launchmode)
+: packageName_(package), launchFileName_(launchfile), launchMode_(launchmode), client_(nullptr)
 {
 }
 
@@ -50,31 +51,34 @@ void CbRosLaunch2::onEntry()
   RCLCPP_INFO_STREAM(getLogger(), "[CbRosLaunch2] OnEntry");
 
   std::string packageName, launchFileName;
-  if (launchFileName_ && packageName_ && launchMode_ == RosLaunchMode::LAUNCH_CLIENT_BEHAVIOR_LIFETIME)
+  if (
+    launchFileName_ && packageName_ &&
+    launchMode_ == RosLaunchMode::LAUNCH_CLIENT_BEHAVIOR_LIFETIME)
   {
     std::function<bool()> breakfunction;
 
-      breakfunction = std::bind(&CbRosLaunch2::isShutdownRequested, this);
-
+    breakfunction = std::bind(&CbRosLaunch2::isShutdownRequested, this);
 
     RCLCPP_INFO_STREAM(
       getLogger(), "[CbRosLaunch2] launching: " << *packageName_ << " , " << *launchFileName_
                                                 << "LaunchMode: " << (int)launchMode_);
 
     auto result_ = smacc2::client_bases::ClRosLaunch2::executeRosLaunch(
+      // this->result_ = smacc2::client_bases::ClRosLaunch2::executeRosLaunch(
       *packageName_, *launchFileName_, breakfunction);
   }
-  else if(launchMode_ == RosLaunchMode::LAUNCH_DETTACHED)
+  else if (launchMode_ == RosLaunchMode::LAUNCH_DETTACHED)
   {
-    if (launchFileName_ && packageName_){
+    this->requiresClient(client_);
+    if (launchFileName_ && packageName_)
+    {
       client_->packageName_ = *packageName_;
       client_->launchFileName_ = *launchFileName_;
     }
 
     RCLCPP_INFO_STREAM(getLogger(), "[CbRosLaunch2] finding Ros Launch client");
-
 
-    this->requiresClient(client_);
+    // this->requiresClient(client_);
     if (client_ != nullptr)
     {
       RCLCPP_INFO_STREAM(
@@ -90,10 +94,12 @@ void CbRosLaunch2::onEntry()
         "[CbRosLaunch2] Inccorrect ros launch operation. No Ros Launch client specified neither "
         "package/roslaunch file path.");
     }
-  }else{
+  }
+  else
+  {
     RCLCPP_ERROR(
-        getLogger(),
-        "[CbRosLaunch2] Inccorrect ros launch operation. Not supported case. Invalid argument.");
+      getLogger(),
+      "[CbRosLaunch2] Inccorrect ros launch operation. Not supported case. Invalid argument.");
   }
 }
 

diff --git a/smacc2/src/smacc2/client_behaviors/cb_ros_stop_2.cpp b/smacc2/src/smacc2/client_behaviors/cb_ros_stop_2.cpp
@@ -25,13 +25,9 @@ namespace client_behaviors
 {
 std::vector<std::future<std::string>> CbRosStop2::detached_futures_;
 
-CbRosStop2::CbRosStop2()
-{
-}
+CbRosStop2::CbRosStop2() {}
 
-CbRosStop2::CbRosStop2(pid_t launchPid)
-{
-}
+CbRosStop2::CbRosStop2(pid_t launchPid) {}
 
 CbRosStop2::~CbRosStop2() {}