packml_stacklight.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (Apache License)
3  *
4  * Copyright (c) 2019 Joshua Hatzenbuehler
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 
20 
21 namespace packml_stacklight
22 {
23 void PackmlStacklight::setDoubleParam(std::string param_name, double& default_val)
24 {
25  double val = 0.0;
26  std::string msg = "";
27  pn_.param(param_name, val, default_val);
28  if (val != default_val)
29  {
30  default_val = val;
31  msg = "changed";
32  }
33  else
34  {
35  msg = "default";
36  }
37 
38  ROS_INFO("%s %s %s => %.02f", __FUNCTION__, msg.c_str(), param_name.c_str(), default_val);
39 }
40 
41 void PackmlStacklight::setBoolParam(std::string param_name, bool& default_val)
42 {
43  bool val = false;
44  std::string msg = "";
45  pn_.param(param_name, val, default_val);
46  if (val != default_val)
47  {
48  default_val = val;
49  msg = "changed";
50  }
51  else
52  {
53  msg = "default";
54  }
55 
56  ROS_INFO("%s %s %s => %s", __FUNCTION__, msg.c_str(), param_name.c_str(), default_val ? "true" : "false");
57 }
58 
60  : nh_(nh), pn_(pn), current_state_time_(ros::Time(0))
61 {
62  setDoubleParam("light_on_secs", utils_.flash_sec_light_on_);
63  setDoubleParam("light_off_secs", utils_.flash_sec_light_off_);
64  setDoubleParam("buzzer_on_secs", utils_.flash_sec_buzzer_on_);
65  setDoubleParam("buzzer_off_secs", utils_.flash_sec_buzzer_off_);
66  setDoubleParam("publish_frequency", utils_.publish_frequency_);
67  setDoubleParam("status_timeout", utils_.status_timeout_);
68 
69  bool suspend_default = utils_.getSuspendStarving();
70  setBoolParam("treat_suspend_starving", suspend_default);
71  utils_.setSuspendStarving(suspend_default);
72 
73  status_sub_ = nh_.subscribe<packml_msgs::Status>("status", 1, &PackmlStacklight::callBackStatus, this);
74 
75  std::map<std::string, uint8_t> temp_map = utils_.getPubMap(current_state_);
76  std::map<std::string, uint8_t>::iterator map_itr;
77  for (map_itr = temp_map.begin(); map_itr != temp_map.end(); ++map_itr)
78  {
79  pub_map_.insert(std::pair<std::string, ros::Publisher>(map_itr->first,
80  nh_.advertise<std_msgs::UInt8>(map_itr->first, 2, true)));
81  }
82 }
83 
85 {
86  if (pub_map_.size() > 0)
87  {
88  pub_map_.clear();
89  }
90 }
91 
92 void PackmlStacklight::callBackStatus(const packml_msgs::StatusConstPtr& msg)
93 {
94  current_state_ = msg->state;
96 }
97 
99 {
100  static std::map<std::string, uint8_t> last_map;
101  std::map<std::string, uint8_t>::iterator map_itr;
102  bool publish_all = false;
103 
105  publish_all = utils_.getShouldPublish(current_state_);
106 
107  std::map<std::string, uint8_t> temp_map = utils_.getPubMap(current_state_);
108  for (map_itr = temp_map.begin(); map_itr != temp_map.end(); ++map_itr)
109  {
110  bool do_publish = false;
111  if (publish_all || last_map.size() == 0)
112  {
113  do_publish = true;
114  }
115  else if (temp_map[map_itr->first] != last_map[map_itr->first])
116  {
117  do_publish = true;
118  }
119 
120  if (do_publish)
121  {
122  ROS_DEBUG("do publish state:%d, topic:%s, data:%d", current_state_.val, map_itr->first.c_str(), map_itr->second);
123  std_msgs::UInt8 msg;
124  msg.data = map_itr->second;
125  pub_map_[map_itr->first].publish<std_msgs::UInt8>(msg);
126  }
127  }
128 
129  last_map = temp_map;
130 
131  return;
132 }
133 
135 {
136  while (ros::ok())
137  {
138  spinOnce();
139  ros::Duration(0.001).sleep();
140  }
141 }
142 
144 {
145  ros::spinOnce();
146  processCurState();
147 }
148 
149 } // namespace packml_stacklight
std::map< std::string, uint8_t > getPubMap(Action action)
Definition: utils.cpp:269
double flash_sec_light_off_
Definition: utils.h:39
bool setSuspendStarving(bool starving=true)
Definition: utils.cpp:178
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool sleep() const
double status_timeout_
Definition: utils.h:43
void maybeResetState(packml_msgs::State &current_state, ros::Time &last_time)
Definition: utils.cpp:338
bool getShouldPublish(packml_msgs::State current_state)
Definition: utils.cpp:236
double publish_frequency_
Definition: utils.h:42
double flash_sec_buzzer_off_
Definition: utils.h:41
void setBoolParam(std::string param_name, bool &default_val)
PackmlStacklight(ros::NodeHandle nh, ros::NodeHandle pn)
double flash_sec_light_on_
Definition: utils.h:38
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ROSCPP_DECL bool ok()
std::map< std::string, ros::Publisher > pub_map_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void setDoubleParam(std::string param_name, double &default_val)
static Time now()
ROSCPP_DECL void spinOnce()
double flash_sec_buzzer_on_
Definition: utils.h:40
void callBackStatus(const packml_msgs::StatusConstPtr &msg)
#define ROS_DEBUG(...)


packml_stacklight
Author(s): Joshua Hatzenbuehler
autogenerated on Fri Jul 12 2019 03:31:01