smacc_ros_launch_client.cpp

Go to the documentation of this file.

// 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
 *
 ******************************************************************************************************************/
 
#include <smacc2/client_bases/smacc_ros_launch_client.hpp>
 
namespace smacc2
{
namespace client_bases
{
ClRosLaunch::ClRosLaunch(std::string packageName, std::string launchFilename)
: packageName_(packageName), launchFileName_(launchFilename), cancellationToken_(false)
{
}
 
ClRosLaunch::~ClRosLaunch() {}
 
void ClRosLaunch::launch()
{
  this->result_ = executeRosLaunch(
    packageName_, launchFileName_, [this]() { return this->cancellationToken_.load(); });
}
 
void ClRosLaunch::stop() { cancellationToken_.store(true); }
 
std::future<std::string> ClRosLaunch::executeRosLaunch(
  std::string packageName, std::string launchFileName, std::function<bool()> cancelCondition)
{
  return std::async(
    std::launch::async,
    [=]()
    {
      RCLCPP_WARN_STREAM(
        rclcpp::get_logger("smacc2"), "[ClRosLaunch static] starting ros launch thread ");
 
      std::stringstream cmd;
      cmd << "ros2 launch " << packageName << " " << launchFileName;
 
      std::array<char, 128> buffer;
      std::string result;
      auto pipe_deleter = [](FILE * fp)
      {
        if (fp) pclose(fp);
      };
      std::unique_ptr<FILE, decltype(pipe_deleter)> pipe(
        popen(cmd.str().c_str(), "r"), pipe_deleter);
      if (!pipe)
      {
        throw std::runtime_error("popen() failed!");
      }
 
      std::stringstream ss;
      bool cancelled = false;
      try
      {
        while (fgets(buffer.data(), buffer.size(), pipe.get()) != nullptr &&
               !(cancelled = cancelCondition()))
        {
          ss << buffer.data();
          if (cancelled)  // break_pipe
          {
            RCLCPP_WARN_STREAM(
              rclcpp::get_logger("smacc2"), "[ClRosLaunch static] cancelling ros launch thread ");
 
            // break pipe
            pclose(pipe.release());
 
            break;
          }
        }
      }
      catch (const std::exception & e)
      {
        std::cerr << e.what() << '\n';
      }
 
      result = ss.str();
      RCLCPP_WARN_STREAM(
        rclcpp::get_logger("smacc2"), "[ClRosLaunch static]] RESULT = \n " << ss.str());
      return result;
    });
}
 
}  // namespace client_bases
}  // namespace smacc2