27 pn_.
param(param_name, val, default_val);
28 if (val != default_val)
38 ROS_INFO(
"%s %s %s => %.02f", __FUNCTION__, msg.c_str(), param_name.c_str(), default_val);
45 pn_.
param(param_name, val, default_val);
46 if (val != default_val)
56 ROS_INFO(
"%s %s %s => %s", __FUNCTION__, msg.c_str(), param_name.c_str(), default_val ?
"true" :
"false");
76 std::map<std::string, uint8_t>::iterator map_itr;
77 for (map_itr = temp_map.begin(); map_itr != temp_map.end(); ++map_itr)
79 pub_map_.insert(std::pair<std::string, ros::Publisher>(map_itr->first,
80 nh_.
advertise<std_msgs::UInt8>(map_itr->first, 2,
true)));
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;
108 for (map_itr = temp_map.begin(); map_itr != temp_map.end(); ++map_itr)
110 bool do_publish =
false;
111 if (publish_all || last_map.size() == 0)
115 else if (temp_map[map_itr->first] != last_map[map_itr->first])
122 ROS_DEBUG(
"do publish state:%d, topic:%s, data:%d",
current_state_.val, map_itr->first.c_str(), map_itr->second);
124 msg.data = map_itr->second;
125 pub_map_[map_itr->first].publish<std_msgs::UInt8>(msg);
std::map< std::string, uint8_t > getPubMap(Action action)
double flash_sec_light_off_
bool setSuspendStarving(bool starving=true)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
packml_stacklight::Utils utils_
bool getSuspendStarving()
void maybeResetState(packml_msgs::State ¤t_state, ros::Time &last_time)
bool getShouldPublish(packml_msgs::State current_state)
double publish_frequency_
double flash_sec_buzzer_off_
ros::Time current_state_time_
void setBoolParam(std::string param_name, bool &default_val)
PackmlStacklight(ros::NodeHandle nh, ros::NodeHandle pn)
double flash_sec_light_on_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
ros::Subscriber status_sub_
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)
packml_msgs::State current_state_
ROSCPP_DECL void spinOnce()
double flash_sec_buzzer_on_
void callBackStatus(const packml_msgs::StatusConstPtr &msg)