cob_voltage_control_ros.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 
18 // ROS includes
19 #include <ros/ros.h>
20 
21 // ROS message includes
22 #include <cob_msgs/PowerState.h>
23 #include <cob_msgs/EmergencyStopState.h>
24 #include <std_msgs/Bool.h>
25 #include <std_msgs/Float64.h>
26 
28 #include <cob_phidgets/AnalogSensor.h>
29 #include <cob_phidgets/DigitalSensor.h>
30 
32 {
33  private:
35 
36  public:
38 
41 
44 
47 
51 
52  //
55 
56  // possible states of emergency stop
57  enum
58  {
62  };
63 
65  {
66  topicPub_power_state_ = n_.advertise<cob_msgs::PowerState>("power_state", 1);
67  topicPub_em_stop_state_ = n_.advertise<cob_msgs::EmergencyStopState>("em_stop_state", 1);
68 
69  topicPub_Current_ = n_.advertise<std_msgs::Float64>("current", 10);
70  topicPub_Voltage_ = n_.advertise<std_msgs::Float64>("voltage", 10);
71 
72  topicSub_AnalogInputs = n_.subscribe("input/analog_sensors", 10, &cob_voltage_control_ros::analogPhidgetSignalsCallback, this);
73  topicSub_DigitalInputs = n_.subscribe("input/digital_sensors", 10, &cob_voltage_control_ros::digitalPhidgetSignalsCallback, this);
74 
75  n_.param("battery_max_voltage", component_config_.max_voltage, 48.5);
76  n_.param("battery_min_voltage", component_config_.min_voltage, 44.0);
77  n_.param("robot_max_voltage", component_config_.max_voltage_res, 70.0);
78  n_.param("voltage_analog_port", component_config_.num_voltage_port, 1);
79  n_.param("em_stop_dio_port", component_config_.num_em_stop_port, 0);
80  n_.param("scanner_stop_dio_port", component_config_.num_scanner_em_port, 1);
81 
82  last_rear_em_state = false;
83  last_front_em_state = false;
84 
85  EM_stop_status_ = ST_EM_ACTIVE;
86  component_data_.out_pub_em_stop_state_.scanner_stop = false;
87  }
88 
89  void configure()
90  {
91  component_implementation_.configure();
92  }
93 
94  void update()
95  {
96  component_implementation_.update(component_data_, component_config_);
97  topicPub_Voltage_.publish(component_data_.out_pub_voltage_);
98  topicPub_Current_.publish(component_data_.out_pub_current_);
99  topicPub_power_state_.publish(component_data_.out_pub_power_state_);
100  topicPub_em_stop_state_.publish(component_data_.out_pub_em_stop_state_);
101  }
102 
103  void analogPhidgetSignalsCallback(const cob_phidgets::AnalogSensorConstPtr &msg)
104  {
105  for(int i = 0; i < msg->uri.size(); i++)
106  {
107  if( msg->uri[i] == "voltage")
108  {
109  component_data_.in_phidget_voltage = msg->value[i];
110  }
111  if( msg->uri[i] == "current")
112  {
113  component_data_.in_phidget_current = msg->value[i];
114  }
115  }
116  }
117 
118  void digitalPhidgetSignalsCallback(const cob_phidgets::DigitalSensorConstPtr &msg)
119  {
120  bool front_em_active = false;
121  bool rear_em_active = false;
122  static bool em_caused_by_button = false;
123  cob_msgs::EmergencyStopState EM_msg;
124  bool EM_signal = false;
125  bool got_message = false;
126 
127  for(int i = 0; i < msg->uri.size(); i++)
128  {
129  if( msg->uri[i] == "em_stop_laser_rear")
130  {
131  rear_em_active = !((bool)msg->state[i]);
132  got_message = true;
133  }
134  else if( msg->uri[i] == "em_stop_laser_front")
135  {
136  front_em_active = !((bool)msg->state[i]);
137  got_message = true;
138  }
139  }
140  if(got_message)
141  {
142  if( (front_em_active && rear_em_active) && (!last_front_em_state && !last_rear_em_state))
143  {
144  component_data_.out_pub_em_stop_state_.emergency_button_stop = true;
145  em_caused_by_button = true;
146  }
147  else if((!front_em_active && !rear_em_active) && (last_front_em_state && last_rear_em_state))
148  {
149  component_data_.out_pub_em_stop_state_.emergency_button_stop = false;
150  em_caused_by_button = false;
151  }
152  else if((front_em_active != rear_em_active) && em_caused_by_button)
153  {
154  component_data_.out_pub_em_stop_state_.emergency_button_stop = false;
155  em_caused_by_button = false;
156  component_data_.out_pub_em_stop_state_.scanner_stop = (bool)(front_em_active | rear_em_active);
157  }
158  else
159  {
160  component_data_.out_pub_em_stop_state_.scanner_stop = (bool)(front_em_active | rear_em_active);
161  }
162 
163  EM_signal = component_data_.out_pub_em_stop_state_.scanner_stop | component_data_.out_pub_em_stop_state_.emergency_button_stop;
164 
165  switch (EM_stop_status_)
166  {
167  case ST_EM_FREE:
168  {
169  if (EM_signal == true)
170  {
171  ROS_INFO("Emergency stop was issued");
172  EM_stop_status_ = EM_msg.EMSTOP;
173  }
174  break;
175  }
176  case ST_EM_ACTIVE:
177  {
178  if (EM_signal == false)
179  {
180  ROS_INFO("Emergency stop was confirmed");
181  EM_stop_status_ = EM_msg.EMCONFIRMED;
182  }
183  break;
184  }
185  case ST_EM_CONFIRMED:
186  {
187  if (EM_signal == true)
188  {
189  ROS_INFO("Emergency stop was issued");
190  EM_stop_status_ = EM_msg.EMSTOP;
191  }
192  else
193  {
194  ROS_INFO("Emergency stop released");
195  EM_stop_status_ = EM_msg.EMFREE;
196  }
197  break;
198  }
199  };
200 
201  component_data_.out_pub_em_stop_state_.emergency_state = EM_stop_status_;
202 
203  last_front_em_state = front_em_active;
204  last_rear_em_state = rear_em_active;
205  }
206  }
207 };
208 
209 int main(int argc, char** argv)
210 {
211  ros::init(argc, argv, "cob_voltage_control");
212 
214  node.configure();
215 
216  ros::Rate loop_rate(20); // Hz // if cycle time == 0 do a spin() here without calling node.update()
217 
218  while(node.n_.ok())
219  {
220  node.update();
221  loop_rate.sleep();
222  ros::spinOnce();
223  }
224  return 0;
225 }
void publish(const boost::shared_ptr< M > &message) const
cob_voltage_control_data component_data_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
cob_msgs::PowerState out_pub_power_state_
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
int i
cob_voltage_control_config component_config_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
cob_msgs::EmergencyStopState out_pub_em_stop_state_
bool sleep()
cob_voltage_control_impl component_implementation_
void digitalPhidgetSignalsCallback(const cob_phidgets::DigitalSensorConstPtr &msg)
int main(int argc, char **argv)
bool ok() const
void update(cob_voltage_control_data &data, cob_voltage_control_config config)
ROSCPP_DECL void spinOnce()
void analogPhidgetSignalsCallback(const cob_phidgets::AnalogSensorConstPtr &msg)


cob_voltage_control
Author(s): Alexander Bubeck
autogenerated on Wed Apr 7 2021 02:11:57