Added detection of E-stop and protective stop.

include/ur_modern_driver/robot_state.h

@@ -88,13 +88,28 @@ struct masterboard_data {
 	int euromapOutputBits;
 	float euromapVoltage;
 	float euromapCurrent;
+};
 
+struct robot_mode_data {
+	uint64_t timestamp;
+	bool isRobotConnected;
+	bool isRealRobotEnabled;
+	bool isPowerOnRobot;
+	bool isEmergencyStopped;
+	bool isProtectiveStopped;
+	bool isProgramRunning;
+	bool isProgramPaused;
+	unsigned char robotMode;
+	unsigned char controlMode;
+	double targetSpeedFraction;
+	double speedScaling;
 };
 
 class RobotState {
 private:
 	version_message version_msg_;
 	masterboard_data mb_data_;
+	robot_mode_data robot_mode_;
 
 	std::recursive_mutex val_lock_; // Locks the variables while unpack parses data;
 
@@ -119,6 +134,7 @@ public:
 	char getAnalogOutputDomain1();
 	double getAnalogOutput0();
 	double getAnalogOutput1();
+	std::vector<double> getVActual();
 	float getMasterBoardTemperature();
 	float getRobotVoltage48V();
 	float getRobotCurrent();
@@ -130,16 +146,27 @@ public:
 	int getEuromapOutputBits();
 	float getEuromapVoltage();
 	float getEuromapCurrent();
+
+	bool isRobotConnected();
+	bool isRealRobotEnabled();
+	bool isPowerOnRobot();
+	bool isEmergencyStopped();
+	bool isProtectiveStopped();
+	bool isProgramRunning();
+	bool isProgramPaused();
+
+	void setDisconnected();
+
 	bool getNewDataAvailable();
 	void finishedReading();
-	std::vector<double> getVActual();
+
 	void unpack(uint8_t * buf, unsigned int buf_length);
 	void unpackRobotMessage(uint8_t * buf, unsigned int offset, uint32_t len);
 	void unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
 			uint32_t len);
-
 	void unpackRobotState(uint8_t * buf, unsigned int offset, uint32_t len);
 	void unpackRobotStateMasterboard(uint8_t * buf, unsigned int offset);
+	void unpackRobotMode(uint8_t * buf, unsigned int offset);
 };
 
 #endif /* ROBOT_STATE_H_ */

src/robot_state.cpp

@@ -12,6 +12,7 @@ RobotState::RobotState(std::condition_variable& msg_cond) {
 	version_msg_.minor_version = 0;
 	new_data_available_ = false;
 	pMsg_cond_ = &msg_cond;
+	RobotState::setDisconnected();
 }
 double RobotState::ntohd(uint64_t nf) {
 	double x;
@@ -28,7 +29,7 @@ void RobotState::unpack(uint8_t* buf, unsigned int buf_length) {
 		memcpy(&len, &buf[offset], sizeof(len));
 		len = ntohl(len);
 		if (len + offset > buf_length) {
-			return ;
+			return;
 		}
 		memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type));
 		switch (message_type) {
@@ -63,7 +64,7 @@ void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset,
 	switch (robot_message_type) {
 	case robotMessageType::ROBOT_MESSAGE_VERSION:
 		val_lock_.lock();
-		version_msg_.timestamp = RobotState::ntohd(timestamp);
+		version_msg_.timestamp = timestamp;
 		version_msg_.source = source;
 		version_msg_.robot_message_type = robot_message_type;
 		RobotState::unpackRobotMessageVersion(buf, offset, len);
@@ -83,11 +84,18 @@ void RobotState::unpackRobotState(uint8_t * buf, unsigned int offset,
 		uint8_t package_type;
 		memcpy(&length, &buf[offset], sizeof(length));
 		length = ntohl(length);
-		memcpy(&package_type, &buf[offset+sizeof(length)], sizeof(package_type));
+		memcpy(&package_type, &buf[offset + sizeof(length)],
+				sizeof(package_type));
 		switch (package_type) {
+		case packageType::ROBOT_MODE_DATA:
+			val_lock_.lock();
+			RobotState::unpackRobotMode(buf, offset + 5);
+			val_lock_.unlock();
+			break;
+
 		case packageType::MASTERBOARD_DATA:
 			val_lock_.lock();
-			RobotState::unpackRobotStateMasterboard(buf, offset+5);
+			RobotState::unpackRobotStateMasterboard(buf, offset + 5);
 			val_lock_.unlock();
 			break;
 		default:
@@ -123,10 +131,73 @@ void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
 	version_msg_.build_date[len - offset] = '\0';
 }
 
+void RobotState::unpackRobotMode(uint8_t * buf, unsigned int offset) {
+	memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp));
+	offset += sizeof(robot_mode_.timestamp);
+	uint8_t tmp;
+	memcpy(&tmp, &buf[offset], sizeof(tmp));
+	if (tmp > 0)
+		robot_mode_.isRobotConnected = true;
+	else
+		robot_mode_.isRobotConnected = false;
+	offset += sizeof(tmp);
+	memcpy(&tmp, &buf[offset], sizeof(tmp));
+	if (tmp > 0)
+		robot_mode_.isRealRobotEnabled = true;
+	else
+		robot_mode_.isRealRobotEnabled = false;
+	offset += sizeof(tmp);
+	memcpy(&tmp, &buf[offset], sizeof(tmp));
+	//printf("PowerOnRobot: %d\n", tmp);
+	if (tmp > 0)
+		robot_mode_.isPowerOnRobot = true;
+	else
+		robot_mode_.isPowerOnRobot = false;
+	offset += sizeof(tmp);
+	memcpy(&tmp, &buf[offset], sizeof(tmp));
+	if (tmp > 0)
+		robot_mode_.isEmergencyStopped = true;
+	else
+		robot_mode_.isEmergencyStopped = false;
+	offset += sizeof(tmp);
+	memcpy(&tmp, &buf[offset], sizeof(tmp));
+	if (tmp > 0)
+		robot_mode_.isProtectiveStopped = true;
+	else
+		robot_mode_.isProtectiveStopped = false;
+	offset += sizeof(tmp);
+	memcpy(&tmp, &buf[offset], sizeof(tmp));
+	if (tmp > 0)
+		robot_mode_.isProgramRunning = true;
+	else
+		robot_mode_.isProgramRunning = false;
+	offset += sizeof(tmp);
+	memcpy(&tmp, &buf[offset], sizeof(tmp));
+	if (tmp > 0)
+		robot_mode_.isProgramPaused = true;
+	else
+		robot_mode_.isProgramPaused = false;
+	offset += sizeof(tmp);
+	memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode));
+	offset += sizeof(robot_mode_.robotMode);
+	uint64_t temp;
+	if (RobotState::getVersion() > 2.) {
+		memcpy(&robot_mode_.controlMode, &buf[offset],
+				sizeof(robot_mode_.controlMode));
+		offset += sizeof(robot_mode_.controlMode);
+		memcpy(&temp, &buf[offset], sizeof(temp));
+		offset += sizeof(temp);
+		robot_mode_.targetSpeedFraction = RobotState::ntohd(temp);
+	}
+	memcpy(&temp, &buf[offset], sizeof(temp));
+	offset += sizeof(temp);
+	robot_mode_.speedScaling = RobotState::ntohd(temp);
+}
+
 void RobotState::unpackRobotStateMasterboard(uint8_t * buf,
 		unsigned int offset) {
 	if (RobotState::getVersion() < 3.0) {
-				int16_t digital_input_bits, digital_output_bits;
+		int16_t digital_input_bits, digital_output_bits;
 		memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
 		offset += sizeof(digital_input_bits);
 		memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits));
@@ -263,3 +334,30 @@ double RobotState::getAnalogOutput0() {
 double RobotState::getAnalogOutput1() {
 	return mb_data_.analogOutput1;
 }
+bool RobotState::isRobotConnected() {
+	return robot_mode_.isRobotConnected;
+}
+bool RobotState::isRealRobotEnabled() {
+	return robot_mode_.isRealRobotEnabled;
+}
+bool RobotState::isPowerOnRobot() {
+	return robot_mode_.isPowerOnRobot;
+}
+bool RobotState::isEmergencyStopped() {
+	return robot_mode_.isEmergencyStopped;
+}
+bool RobotState::isProtectiveStopped() {
+	return robot_mode_.isProtectiveStopped;
+}
+bool RobotState::isProgramRunning() {
+	return robot_mode_.isProgramRunning;
+}
+bool RobotState::isProgramPaused() {
+	return robot_mode_.isProgramPaused;
+}
+
+void RobotState::setDisconnected() {
+	robot_mode_.isRobotConnected = false;
+	robot_mode_.isRealRobotEnabled = false;
+	robot_mode_.isPowerOnRobot = false;
+}

src/ur_communication.cpp

@@ -122,6 +122,7 @@ void UrCommunication::run() {
 				robot_state_->unpack(buf, bytes_read);
 			} else {
 				connected_ = false;
+				robot_state_->setDisconnected();
 				close(sec_sockfd_);
 			}
 		}

src/ur_ros_wrapper.cpp

@@ -181,6 +181,38 @@ private:
 			actionlib::ServerGoalHandle<
 					control_msgs::FollowJointTrajectoryAction> gh) {
 		print_info("on_goal");
+		if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) {
+			result_.error_code = -100;
+			result_.error_string =
+					"Cannot accept new trajectories: Robot arm is not powered on";
+			gh.setRejected(result_, result_.error_string);
+			print_error(result_.error_string);
+			return;
+		}
+		if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) {
+			result_.error_code = -100;
+			result_.error_string =
+					"Cannot accept new trajectories: Robot is not enabled";
+			gh.setRejected(result_, result_.error_string);
+			print_error(result_.error_string);
+			return;
+		}
+		if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) {
+			result_.error_code = -100;
+			result_.error_string =
+					"Cannot accept new trajectories: Robot is emergency stopped";
+			gh.setRejected(result_, result_.error_string);
+			print_error(result_.error_string);
+			return;
+		}
+		if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) {
+			result_.error_code = -100;
+			result_.error_string =
+					"Cannot accept new trajectories: Robot is protective stopped";
+			gh.setRejected(result_, result_.error_string);
+			print_error(result_.error_string);
+			return;
+		}
 		actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
 				*gh.getGoal(); //make a copy that we can modify
 		if (goal_handle_ != NULL) {
@@ -200,12 +232,14 @@ private:
 							+ outp_joint_names;
 			gh.setRejected(result_, result_.error_string);
 			print_error(result_.error_string);
+			return;
 		}
 		if (!has_positions()) {
 			result_.error_code = result_.INVALID_GOAL;
 			result_.error_string = "Received a goal without positions";
 			gh.setRejected(result_, result_.error_string);
 			print_error(result_.error_string);
+			return;
 		}
 
 		if (!has_velocities()) {
@@ -213,6 +247,7 @@ private:
 			result_.error_string = "Received a goal without velocities";
 			gh.setRejected(result_, result_.error_string);
 			print_error(result_.error_string);
+			return;
 		}
 
 		if (!traj_is_finite()) {
@@ -220,6 +255,7 @@ private:
 			result_.error_code = result_.INVALID_GOAL;
 			gh.setRejected(result_, result_.error_string);
 			print_error(result_.error_string);
+			return;
 		}
 
 		if (!has_limited_velocities()) {
@@ -228,6 +264,7 @@ private:
 					"Received a goal with velocities that are higher than %f", max_velocity_;
 			gh.setRejected(result_, result_.error_string);
 			print_error(result_.error_string);
+			return;
 		}
 
 		reorder_traj_joints(goal.trajectory);
@@ -235,10 +272,13 @@ private:
 		std::vector<double> timestamps;
 		std::vector<std::vector<double> > positions, velocities;
 		if (goal.trajectory.points[0].time_from_start.toSec() != 0.) {
-			print_warning("Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory");
+			print_warning(
+					"Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory");
 			timestamps.push_back(0.0);
-			positions.push_back(robot_.rt_interface_->robot_state_->getQActual());
+			positions.push_back(
-			velocities.push_back(robot_.rt_interface_->robot_state_->getQdActual());
+					robot_.rt_interface_->robot_state_->getQActual());
+			velocities.push_back(
+					robot_.rt_interface_->robot_state_->getQdActual());
 		}
 		for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
 			timestamps.push_back(
@@ -260,10 +300,9 @@ private:
 	void cancelCB(
 			actionlib::ServerGoalHandle<
 					control_msgs::FollowJointTrajectoryAction> gh) {
-		print_info("on_cancel");
 		// set the action state to preempted
+			print_info("on_cancel");
 		if (goal_handle_ != NULL) {
-			print_info("Stopping previous traj");
 			robot_.stopTraj();
 			result_.error_code = result_.SUCCESSFUL;
 			result_.error_string = "Goal cancelled by client";
@@ -500,6 +539,7 @@ private:
 	}
 
 	void publishMbMsg() {
+		bool warned = false;
 		ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>(
 				"ur_driver/io_states", 1);
 
@@ -541,6 +581,27 @@ private:
 			io_msg.analog_out_states.push_back(ana);
 			io_pub.publish(io_msg);
 
+			if (robot_.sec_interface_->robot_state_->isEmergencyStopped()
+					or robot_.sec_interface_->robot_state_->isProtectiveStopped()) {
+				if (robot_.sec_interface_->robot_state_->isEmergencyStopped()
+						and !warned) {
+					print_error("Emergency stop pressed!");
+				} else if (robot_.sec_interface_->robot_state_->isProtectiveStopped()
+						and !warned) {
+					print_error("Robot is protective stopped!");
+				}
+				if (goal_handle_ != NULL) {
+					print_error("Aborting trajectory");
+					robot_.stopTraj();
+					result_.error_code = result_.SUCCESSFUL;
+					result_.error_string = "Robot was halted";
+					goal_handle_->setAborted(result_, result_.error_string);
+					goal_handle_ = NULL;
+				}
+				warned = true;
+			} else
+				warned = false;
+
 			robot_.sec_interface_->robot_state_->finishedReading();
 
 		}