packml_ros.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (Apache License)
3  *
4  * Copyright (c) 2017 Shaun Edwards
5  *
6  * Licensed under the Apache License, Version 2.0 (the "License");
7  * you may not use this file except in compliance with the License.
8  * You may obtain a copy of the License at
9  *
10  * http://www.apache.org/licenses/LICENSE-2.0
11  *
12  * Unless required by applicable law or agreed to in writing, software
13  * distributed under the License is distributed on an "AS IS" BASIS,
14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15  * See the License for the specific language governing permissions and
16  * limitations under the License.
17  */
18 
19 #include "packml_ros/packml_ros.h"
21 
22 #include <packml_msgs/utils.h>
23 #include "packml_msgs/ItemizedStats.h"
24 
25 namespace packml_ros
26 {
27 PackmlRos::PackmlRos(ros::NodeHandle nh, ros::NodeHandle pn, std::shared_ptr<packml_sm::AbstractStateMachine> sm)
28  : nh_(nh), pn_(pn), sm_(sm)
29 {
30  ros::NodeHandle packml_node("~/packml");
31 
32  status_pub_ = packml_node.advertise<packml_msgs::Status>("status", 10, true);
33 
34  trans_server_ = packml_node.advertiseService("transition", &PackmlRos::transRequest, this);
35  reset_stats_server_ = packml_node.advertiseService("reset_stats", &PackmlRos::resetStats, this);
36  get_stats_server_ = packml_node.advertiseService("get_stats", &PackmlRos::getStats, this);
37 
39 
40  sm_->stateChangedEvent.bind_member_func(this, &PackmlRos::handleStateChanged);
41  sm_->activate();
42 }
43 
45 {
46  if (sm_ != nullptr)
47  {
48  sm_->stateChangedEvent.unbind_member_func(this, &PackmlRos::handleStateChanged);
49  }
50 }
51 
53 {
54  while (ros::ok())
55  {
56  spinOnce();
57  ros::Duration(0.001).sleep();
58  }
59  return;
60 }
61 
63 {
64  ros::spinOnce();
65 }
66 
67 bool PackmlRos::transRequest(packml_msgs::Transition::Request& req, packml_msgs::Transition::Response& res)
68 {
69  bool command_rtn = false;
70  bool command_valid = true;
71  int command_int = static_cast<int>(req.command);
72  std::stringstream ss;
73  ROS_DEBUG_STREAM("Evaluating transition request command: " << command_int);
74 
75  switch (command_int)
76  {
77  case req.ABORT:
78  case req.ESTOP:
79  command_rtn = sm_->abort();
80  break;
81  case req.CLEAR:
82  command_rtn = sm_->clear();
83  break;
84  case req.HOLD:
85  command_rtn = sm_->hold();
86  break;
87  case req.RESET:
88  command_rtn = sm_->reset();
89  break;
90  case req.START:
91  command_rtn = sm_->start();
92  break;
93  case req.STOP:
94  command_rtn = sm_->stop();
95  break;
96  case req.SUSPEND:
97  command_rtn = sm_->suspend();
98  break;
99  case req.UNHOLD:
100  command_rtn = sm_->unhold();
101  break;
102  case req.UNSUSPEND:
103  command_rtn = sm_->unsuspend();
104  break;
105 
106  default:
107  command_valid = false;
108  break;
109  }
110  if (command_valid)
111  {
112  if (command_rtn)
113  {
114  ss << "Successful transition request command: " << command_int;
115  ROS_INFO_STREAM(ss.str());
116  res.success = true;
117  res.error_code = res.SUCCESS;
118  res.message = ss.str();
119  }
120  else
121  {
122  ss << "Invalid transition request command: " << command_int;
123  ROS_ERROR_STREAM(ss.str());
124  res.success = false;
125  res.error_code = res.INVALID_TRANSITION_REQUEST;
126  res.message = ss.str();
127  }
128  }
129  else
130  {
131  ss << "Unrecognized transition request command: " << command_int;
132  ROS_ERROR_STREAM(ss.str());
133  res.success = false;
134  res.error_code = res.UNRECOGNIZED_REQUEST;
135  res.message = ss.str();
136  }
137 }
138 
141 {
142  ROS_DEBUG_STREAM("Publishing state change: " << args.name << "(" << args.value << ")");
143 
144  status_msg_.header.stamp = ros::Time().now();
145  int cur_state = static_cast<int>(args.value);
146  if (packml_msgs::isStandardState(cur_state))
147  {
148  status_msg_.state.val = cur_state;
149  status_msg_.sub_state = packml_msgs::State::UNDEFINED;
150  }
151  else
152  {
153  status_msg_.sub_state = cur_state;
154  }
155 
157 }
158 
159 void PackmlRos::getCurrentStats(packml_msgs::Stats& out_stats)
160 {
161  packml_sm::PackmlStatsSnapshot stats_snapshot;
162  sm_->getCurrentStatSnapshot(stats_snapshot);
163 
164  out_stats.idle_duration.data.fromSec(stats_snapshot.idle_duration);
165  out_stats.exe_duration.data.fromSec(stats_snapshot.exe_duration);
166  out_stats.held_duration.data.fromSec(stats_snapshot.held_duration);
167  out_stats.susp_duration.data.fromSec(stats_snapshot.susp_duration);
168  out_stats.cmplt_duration.data.fromSec(stats_snapshot.cmplt_duration);
169  out_stats.stop_duration.data.fromSec(stats_snapshot.stop_duration);
170  out_stats.abort_duration.data.fromSec(stats_snapshot.abort_duration);
171  out_stats.duration.data.fromSec(stats_snapshot.duration);
172  out_stats.fail_count = stats_snapshot.fail_count;
173  out_stats.success_count = stats_snapshot.success_count;
174  out_stats.availability = stats_snapshot.availability;
175  out_stats.performance = stats_snapshot.performance;
176  out_stats.quality = stats_snapshot.quality;
177  out_stats.overall_equipment_effectiveness = stats_snapshot.overall_equipment_effectiveness;
178 
179  out_stats.error_items.clear();
180  for (const auto& itemized_it : stats_snapshot.itemized_error_map)
181  {
182  packml_msgs::ItemizedStats stat;
183  stat.id = itemized_it.second.id;
184  stat.count = itemized_it.second.count;
185  stat.duration.data.fromSec(itemized_it.second.duration);
186  out_stats.error_items.push_back(stat);
187  }
188 
189  out_stats.quality_items.clear();
190  for (const auto& itemized_it : stats_snapshot.itemized_quality_map)
191  {
192  packml_msgs::ItemizedStats stat;
193  stat.id = itemized_it.second.id;
194  stat.count = itemized_it.second.count;
195  stat.duration.data.fromSec(itemized_it.second.duration);
196  out_stats.quality_items.push_back(stat);
197  }
198 
199  out_stats.header.stamp = ros::Time::now();
200 }
201 
202 bool PackmlRos::getStats(packml_msgs::GetStats::Request& req, packml_msgs::GetStats::Response& response)
203 {
204  packml_msgs::Stats stats;
205  getCurrentStats(stats);
206  response.stats = stats;
207 
208  return true;
209 }
210 
211 bool PackmlRos::resetStats(packml_msgs::ResetStats::Request& req, packml_msgs::ResetStats::Response& response)
212 {
213  packml_msgs::Stats stats;
214  getCurrentStats(stats);
215  response.last_stat = stats;
216 
217  sm_->resetStats();
218 
219  return true;
220 }
221 } // namespace kitsune_robot
std::shared_ptr< packml_sm::AbstractStateMachine > sm_
Definition: packml_ros.h:42
void publish(const boost::shared_ptr< M > &message) const
packml_msgs::Status status_msg_
Definition: packml_ros.h:47
ros::Publisher status_pub_
Definition: packml_ros.h:43
bool sleep() const
ros::ServiceServer get_stats_server_
Definition: packml_ros.h:46
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
ros::ServiceServer reset_stats_server_
Definition: packml_ros.h:45
PackmlRos(ros::NodeHandle nh, ros::NodeHandle pn, std::shared_ptr< packml_sm::AbstractStateMachine > sm)
Definition: packml_ros.cpp:27
std::map< int16_t, PackmlStatsItemized > itemized_quality_map
std::map< int16_t, PackmlStatsItemized > itemized_error_map
void getCurrentStats(packml_msgs::Stats &out_stats)
Definition: packml_ros.cpp:159
ROSCPP_DECL bool ok()
const std::string & getNamespace() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define ROS_DEBUG_STREAM(args)
bool getStats(packml_msgs::GetStats::Request &req, packml_msgs::GetStats::Response &response)
Definition: packml_ros.cpp:202
Status initStatus(std::string node_name)
#define ROS_INFO_STREAM(args)
static Time now()
void handleStateChanged(packml_sm::AbstractStateMachine &state_machine, const packml_sm::StateChangedEventArgs &args)
Definition: packml_ros.cpp:139
bool isStandardState(int state)
#define ROS_ERROR_STREAM(args)
ROSCPP_DECL void spinOnce()
bool transRequest(packml_msgs::Transition::Request &req, packml_msgs::Transition::Response &res)
Definition: packml_ros.cpp:67
ros::ServiceServer trans_server_
Definition: packml_ros.h:44
bool resetStats(packml_msgs::ResetStats::Request &req, packml_msgs::ResetStats::Response &response)
Definition: packml_ros.cpp:211


packml_ros
Author(s): Shaun Edwards
autogenerated on Fri Jul 12 2019 03:31:00