adapter_operation_mode.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019 Pilz GmbH & Co. KG
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8 
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13 
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
19 
20 namespace prbt_hardware_support
21 {
22 
23 static const std::string TOPIC_OPERATION_MODE = "/prbt/operation_mode";
24 static const std::string SERVICE_NAME_GET_OPERATION_MODE = "/prbt/get_operation_mode";
25 
26 static constexpr int DEFAULT_QUEUE_SIZE{10};
27 
29  : nh_(nh)
30 {
31  op_mode_.time_stamp = ros::Time::now();
32  op_mode_.value = OperationModes::UNKNOWN;
33 
36  true); // latched publisher
37  // publish initial operation mode before first switch
39 
41 }
42 
44 {
45  operation_mode_server_ = nh_.advertiseService(SERVICE_NAME_GET_OPERATION_MODE,
47  this);
48 }
49 
50 void AdapterOperationMode::updateOperationMode(const OperationModes& new_op_mode)
51 {
52  std::unique_lock<std::mutex> lock(op_mode_mutex_);
53  const int8_t last_op_mode_value {op_mode_.value};
54  op_mode_ = new_op_mode;
55  lock.unlock();
56 
57  if (new_op_mode.value != last_op_mode_value)
58  {
59  ROS_INFO_STREAM( "Operation Mode switch: "
60  << static_cast<int>(last_op_mode_value)
61  << " -> "
62  << static_cast<int>(new_op_mode.value) );
63  operation_mode_pub_.publish(new_op_mode);
64  }
65 }
66 
67 bool AdapterOperationMode::getOperationMode(GetOperationMode::Request& /*req*/,
68  GetOperationMode::Response& res)
69 {
70  std::lock_guard<std::mutex> lock(op_mode_mutex_);
71  res.mode = op_mode_;
72  return true;
73 }
74 
75 } // namespace prbt_hardware_support
ros::NodeHandle & nh_
The node handle.
bool getOperationMode(GetOperationMode::Request &req, GetOperationMode::Response &res)
OperationModes op_mode_
Store the current operation mode according to OperationModes.msg.
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void publish(const boost::shared_ptr< M > &message) const
void updateOperationMode(const OperationModes &mode)
Stores the operation mode and publishes it, if it has changed.
ros::Publisher operation_mode_pub_
Informs about operation mode changes via topic.
void initOperationModeService()
Initializes the operation mode service.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
static const std::string SERVICE_NAME_GET_OPERATION_MODE
#define ROS_INFO_STREAM(args)
static constexpr int DEFAULT_QUEUE_SIZE
static Time now()
std::mutex op_mode_mutex_
Protects read/write of the operation mode.
ros::ServiceServer operation_mode_server_
Server serving a service to ask whether a brake test is currently required.
static const std::string TOPIC_OPERATION_MODE


prbt_hardware_support
Author(s):
autogenerated on Thu Feb 13 2020 03:16:51