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 static const std::string TOPIC_OPERATION_MODE = "/prbt/operation_mode";
23 static const std::string SERVICE_NAME_GET_OPERATION_MODE = "/prbt/get_operation_mode";
24 
25 static constexpr int DEFAULT_QUEUE_SIZE{ 10 };
26 
28 {
29  op_mode_.time_stamp = ros::Time::now();
30  op_mode_.value = OperationModes::UNKNOWN;
31 
33  true); // latched publisher
34  // publish initial operation mode before first switch
36 
38 }
39 
41 {
43  nh_.advertiseService(SERVICE_NAME_GET_OPERATION_MODE, &AdapterOperationMode::getOperationMode, this);
44 }
45 
46 void AdapterOperationMode::updateOperationMode(const OperationModes& new_op_mode)
47 {
48  std::unique_lock<std::mutex> lock(op_mode_mutex_);
49  const int8_t last_op_mode_value{ op_mode_.value };
50  op_mode_ = new_op_mode;
51  lock.unlock();
52 
53  if (new_op_mode.value != last_op_mode_value)
54  {
55  ROS_INFO_STREAM("Operation Mode switch: " << static_cast<int>(last_op_mode_value) << " -> "
56  << static_cast<int>(new_op_mode.value));
57  operation_mode_pub_.publish(new_op_mode);
58  }
59 }
60 
61 bool AdapterOperationMode::getOperationMode(GetOperationMode::Request& /*req*/, GetOperationMode::Response& res)
62 {
63  std::lock_guard<std::mutex> lock(op_mode_mutex_);
64  res.mode = op_mode_;
65  return true;
66 }
67 
68 } // 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 Sat May 16 2020 03:13:49