Ejemplos de SerRelayBoard::isEMStop en C++ (Cpp)

Ejemplo n.º 1

Archivo: cob_relayboard_node.cpp Proyecto: clmi-bas/care-o-bot

void NodeClass::sendEmergencyStopStates()
{
	requestBoardStatus();
	bool EM_signal;
	ros::Duration duration_since_EM_confirmed;
	cob_msgs::EmergencyStopState EM_msg;

	// assign input (laser, button) specific EM state
	EM_msg.emergency_button_stop = m_SerRelayBoard->isEMStop();
	EM_msg.scanner_stop = m_SerRelayBoard->isScannerStop();

	// determine current EMStopState
	EM_signal = (EM_msg.emergency_button_stop || EM_msg.scanner_stop);

	switch (EM_stop_status_)
	{
		case ST_EM_FREE:
		{
			if (EM_signal == true)
			{
				ROS_INFO("Emergency stop was issued");
				EM_stop_status_ = EM_msg.EMSTOP;
			}
			break;
		}
		case ST_EM_ACTIVE:
		{
			if (EM_signal == false)
			{
				ROS_INFO("Emergency stop was confirmed");
				EM_stop_status_ = EM_msg.EMCONFIRMED;
				time_of_EM_confirmed_ = ros::Time::now();
			}
			break;
		}
		case ST_EM_CONFIRMED:
		{
			if (EM_signal == true)
			{
				ROS_INFO("Emergency stop was issued");
				EM_stop_status_ = EM_msg.EMSTOP;
			}
			else
			{
				duration_since_EM_confirmed = ros::Time::now() - time_of_EM_confirmed_;
				if( duration_since_EM_confirmed.toSec() > duration_for_EM_free_.toSec() )
				{
					ROS_INFO("Emergency stop released");
					EM_stop_status_ = EM_msg.EMFREE;
				}
			}
			break;
		}
	};
	EM_msg.emergency_state = EM_stop_status_;
	topicPub_isEmergencyStop.publish(EM_msg);
}

Ejemplo n.º 2

Archivo: cob_relayboard_node.cpp Proyecto: sonicboomshanky/care-o-bot

void NodeClass::sendEmergencyStopStates()
{
	requestBoardStatus();
	std_msgs::Bool msg;

	msg.data = m_SerRelayBoard->isEMStop();
	topicPub_isEmergencyStop.publish(msg);
	msg.data = m_SerRelayBoard->isScannerStop();
	topicPub_isScannerStop.publish(msg);
}

Ejemplo n.º 3

Archivo: cob_relayboard_node.cpp Proyecto: beds-beisheng/cob_driver

void NodeClass::sendEmergencyStopStates()
{
	requestBoardStatus();

	if(!relayboard_available) return;

	sendBatteryVoltage();

	
	bool EM_signal;
	ros::Duration duration_since_EM_confirmed;
	cob_relayboard::EmergencyStopState EM_msg;
	pr2_msgs::PowerBoardState pbs;
	pbs.header.stamp = ros::Time::now();

	// assign input (laser, button) specific EM state TODO: Laser and Scanner stop can't be read independently (e.g. if button is stop --> no informtion about scanner, if scanner ist stop --> no informtion about button stop)
	EM_msg.emergency_button_stop = m_SerRelayBoard->isEMStop();
	EM_msg.scanner_stop = m_SerRelayBoard->isScannerStop();

	// determine current EMStopState
	EM_signal = (EM_msg.emergency_button_stop || EM_msg.scanner_stop);

	switch (EM_stop_status_)
	{
		case ST_EM_FREE:
		{
			if (EM_signal == true)
			{
				ROS_INFO("Emergency stop was issued");
				EM_stop_status_ = EM_msg.EMSTOP;
			}
			break;
		}
		case ST_EM_ACTIVE:
		{
			if (EM_signal == false)
			{
				ROS_INFO("Emergency stop was confirmed");
				EM_stop_status_ = EM_msg.EMCONFIRMED;
				time_of_EM_confirmed_ = ros::Time::now();
			}
			break;
		}
		case ST_EM_CONFIRMED:
		{
			if (EM_signal == true)
			{
				ROS_INFO("Emergency stop was issued");
				EM_stop_status_ = EM_msg.EMSTOP;
			}
			else
			{
				duration_since_EM_confirmed = ros::Time::now() - time_of_EM_confirmed_;
				if( duration_since_EM_confirmed.toSec() > duration_for_EM_free_.toSec() )
				{
					ROS_INFO("Emergency stop released");
					EM_stop_status_ = EM_msg.EMFREE;
				}
			}
			break;
		}
	};

	
	EM_msg.emergency_state = EM_stop_status_;
	
	// pr2 power_board_state
	if(EM_msg.emergency_button_stop)
	  pbs.run_stop = false;
	else
	  pbs.run_stop = true;
	
	//for cob the wireless stop field is misused as laser stop field
	if(EM_msg.scanner_stop)
	  pbs.wireless_stop = false; 
	else
	  pbs.wireless_stop = true;
  

	//publish EM-Stop-Active-messages, when connection to relayboard got cut
	if(relayboard_online == false) {
		EM_msg.emergency_state = EM_msg.EMSTOP;
	}
	topicPub_isEmergencyStop.publish(EM_msg);
	topicPub_PowerBoardState.publish(pbs);
}