adapter_operation_mode.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2019 Pilz GmbH & Co. KG
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.

 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.

 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <prbt_hardware_support/adapter_operation_mode.h>

namespace prbt_hardware_support
{

static const std::string TOPIC_OPERATION_MODE = "/prbt/operation_mode";
static const std::string SERVICE_NAME_GET_OPERATION_MODE = "/prbt/get_operation_mode";

static constexpr int DEFAULT_QUEUE_SIZE{10};

AdapterOperationMode::AdapterOperationMode(ros::NodeHandle& nh)
  : nh_(nh)
{
  op_mode_.time_stamp = ros::Time::now();
  op_mode_.value = OperationModes::UNKNOWN;

  operation_mode_pub_ = nh_.advertise<OperationModes>(TOPIC_OPERATION_MODE,
                                                     DEFAULT_QUEUE_SIZE,
                                                     true);  // latched publisher
  // publish initial operation mode before first switch
  operation_mode_pub_.publish(op_mode_);

  initOperationModeService();
}

void AdapterOperationMode::initOperationModeService()
{
  operation_mode_server_ = nh_.advertiseService(SERVICE_NAME_GET_OPERATION_MODE,
                                                &AdapterOperationMode::getOperationMode,
                                                this);
}

void AdapterOperationMode::updateOperationMode(const OperationModes& new_op_mode)
{
  std::unique_lock<std::mutex> lock(op_mode_mutex_);
  const int8_t last_op_mode_value {op_mode_.value};
  op_mode_ = new_op_mode;
  lock.unlock();

  if (new_op_mode.value != last_op_mode_value)
  {
    ROS_INFO_STREAM( "Operation Mode switch: "
                     << static_cast<int>(last_op_mode_value)
                     << " -> "
                     << static_cast<int>(new_op_mode.value) );
    operation_mode_pub_.publish(new_op_mode);
  }
}

bool AdapterOperationMode::getOperationMode(GetOperationMode::Request&,
                                            GetOperationMode::Response& res)
{
  std::lock_guard<std::mutex> lock(op_mode_mutex_);
  res.mode = op_mode_;
  return true;
}

} // namespace prbt_hardware_support