22 #include <cob_msgs/PowerState.h> 23 #include <cob_msgs/EmergencyStopState.h> 24 #include <std_msgs/Bool.h> 25 #include <std_msgs/Float64.h> 28 #include <cob_phidgets/AnalogSensor.h> 29 #include <cob_phidgets/DigitalSensor.h> 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);
69 topicPub_Current_ = n_.
advertise<std_msgs::Float64>(
"current", 10);
70 topicPub_Voltage_ = n_.
advertise<std_msgs::Float64>(
"voltage", 10);
82 last_rear_em_state =
false;
83 last_front_em_state =
false;
96 component_implementation_.
update(component_data_, component_config_);
105 for(
int i = 0;
i < msg->uri.size();
i++)
107 if( msg->uri[
i] ==
"voltage")
111 if( msg->uri[
i] ==
"current")
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;
127 for(
int i = 0;
i < msg->uri.size();
i++)
129 if( msg->uri[
i] ==
"em_stop_laser_rear")
131 rear_em_active = !((bool)msg->state[
i]);
134 else if( msg->uri[
i] ==
"em_stop_laser_front")
136 front_em_active = !((bool)msg->state[
i]);
142 if( (front_em_active && rear_em_active) && (!last_front_em_state && !
last_rear_em_state))
145 em_caused_by_button =
true;
147 else if((!front_em_active && !rear_em_active) && (last_front_em_state &&
last_rear_em_state))
150 em_caused_by_button =
false;
152 else if((front_em_active != rear_em_active) && em_caused_by_button)
155 em_caused_by_button =
false;
165 switch (EM_stop_status_)
169 if (EM_signal ==
true)
171 ROS_INFO(
"Emergency stop was issued");
172 EM_stop_status_ = EM_msg.EMSTOP;
178 if (EM_signal ==
false)
180 ROS_INFO(
"Emergency stop was confirmed");
181 EM_stop_status_ = EM_msg.EMCONFIRMED;
187 if (EM_signal ==
true)
189 ROS_INFO(
"Emergency stop was issued");
190 EM_stop_status_ = EM_msg.EMSTOP;
194 ROS_INFO(
"Emergency stop released");
195 EM_stop_status_ = EM_msg.EMFREE;
203 last_front_em_state = front_em_active;
204 last_rear_em_state = rear_em_active;
209 int main(
int argc,
char** argv)
211 ros::init(argc, argv,
"cob_voltage_control");
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)
ros::Subscriber topicSub_DigitalInputs
ros::Publisher topicPub_Voltage_
ros::Publisher topicPub_Current_
cob_msgs::PowerState out_pub_power_state_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
cob_voltage_control_config component_config_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher topicPub_power_state_
cob_msgs::EmergencyStopState out_pub_em_stop_state_
ros::Subscriber topicSub_AnalogInputs
std_msgs::Float64 out_pub_voltage_
cob_voltage_control_ros()
std_msgs::Float64 out_pub_current_
ros::Publisher topicPub_em_stop_state_
cob_voltage_control_impl component_implementation_
void digitalPhidgetSignalsCallback(const cob_phidgets::DigitalSensorConstPtr &msg)
int main(int argc, char **argv)
void update(cob_voltage_control_data &data, cob_voltage_control_config config)
ROSCPP_DECL void spinOnce()
void analogPhidgetSignalsCallback(const cob_phidgets::AnalogSensorConstPtr &msg)