Fixed an issue with using a simulated robot on firmware <= 1.8

include/ur_modern_driver/robot_state.h

@@ -54,6 +54,38 @@ enum robot_message_type {
 }
 typedef robot_message_types::robot_message_type robotMessageType;
 
+namespace robot_state_type_v18 {
+enum robot_state_type {
+	ROBOT_RUNNING_MODE = 0,
+	ROBOT_FREEDRIVE_MODE = 1,
+	ROBOT_READY_MODE = 2,
+	ROBOT_INITIALIZING_MODE = 3,
+	ROBOT_SECURITY_STOPPED_MODE = 4,
+	ROBOT_EMERGENCY_STOPPED_MODE = 5,
+	ROBOT_FATAL_ERROR_MODE = 6,
+	ROBOT_NO_POWER_MODE = 7,
+	ROBOT_NOT_CONNECTED_MODE = 8,
+	ROBOT_SHUTDOWN_MODE = 9,
+	ROBOT_SAFEGUARD_STOP_MODE = 10
+};
+}
+typedef robot_state_type_v18::robot_state_type robotStateTypeV18;
+namespace robot_state_type_v30 {
+enum robot_state_type {
+	ROBOT_MODE_DISCONNECTED = 0,
+	ROBOT_MODE_CONFIRM_SAFETY = 1,
+	ROBOT_MODE_BOOTING = 2,
+	ROBOT_MODE_POWER_OFF = 3,
+	ROBOT_MODE_POWER_ON = 4,
+	ROBOT_MODE_IDLE = 5,
+	ROBOT_MODE_BACKDRIVE = 6,
+	ROBOT_MODE_RUNNING = 7,
+	ROBOT_MODE_UPDATING_FIRMWARE = 8
+};
+}
+
+typedef robot_state_type_v30::robot_state_type robotStateTypeV30;
+
 struct version_message {
 	uint64_t timestamp;
 	int8_t source;
@@ -115,6 +147,7 @@ private:
 
 	std::condition_variable* pMsg_cond_; //Signals that new vars are available
 	bool new_data_available_; //to avoid spurious wakes
+	unsigned char robot_mode_running_;
 
 	double ntohd(uint64_t nf);
 
@@ -154,6 +187,8 @@ public:
 	bool isProtectiveStopped();
 	bool isProgramRunning();
 	bool isProgramPaused();
+	unsigned char getRobotMode();
+	bool isReady();
 
 	void setDisconnected();
 

src/robot_state.cpp

@@ -13,6 +13,7 @@ RobotState::RobotState(std::condition_variable& msg_cond) {
 	new_data_available_ = false;
 	pMsg_cond_ = &msg_cond;
 	RobotState::setDisconnected();
+	robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING;
 }
 double RobotState::ntohd(uint64_t nf) {
 	double x;
@@ -129,6 +130,9 @@ void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
 	version_msg_.svn_revision = ntohl(version_msg_.svn_revision);
 	memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset);
 	version_msg_.build_date[len - offset] = '\0';
+	if (version_msg_.major_version < 2) {
+		robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE;
+	}
 }
 
 void RobotState::unpackRobotMode(uint8_t * buf, unsigned int offset) {
@@ -355,6 +359,15 @@ bool RobotState::isProgramRunning() {
 bool RobotState::isProgramPaused() {
 	return robot_mode_.isProgramPaused;
 }
+unsigned char RobotState::getRobotMode() {
+	return robot_mode_.robotMode;
+}
+bool RobotState::isReady() {
+	if (robot_mode_.robotMode == robot_mode_running_) {
+		return true;
+	}
+	return false;
+}
 
 void RobotState::setDisconnected() {
 	robot_mode_.isRobotConnected = false;

src/ur_ros_wrapper.cpp

@@ -193,19 +193,26 @@ private:
 	void goalCB(
 			actionlib::ServerGoalHandle<
 					control_msgs::FollowJointTrajectoryAction> gh) {
+		std::string buf;
 		print_info("on_goal");
-		if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) {
+		if (!robot_.sec_interface_->robot_state_->isReady()) {
 			result_.error_code = -100; //nothing is defined for this...?
-			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; //nothing is defined for this...?
-			result_.error_string =
-					"Cannot accept new trajectories: Robot is not enabled";
+
+			if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) {
+				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_string =
+						"Cannot accept new trajectories: Robot is not enabled";
+				gh.setRejected(result_, result_.error_string);
+				print_error(result_.error_string);
+				return;
+			}
+			result_.error_string = "Cannot accept new trajectories. (Debug: Robot mode is " + std::to_string(robot_.sec_interface_->robot_state_->getRobotMode()) + ")";
 			gh.setRejected(result_, result_.error_string);
 			print_error(result_.error_string);
 			return;
@@ -226,6 +233,7 @@ private:
 			print_error(result_.error_string);
 			return;
 		}
+
 		actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
 				*gh.getGoal(); //make a copy that we can modify
 		if (has_goal_) {
@@ -280,7 +288,7 @@ private:
 		if (!has_limited_velocities()) {
 			result_.error_code = result_.INVALID_GOAL;
 			result_.error_string =
-					"Received a goal with velocities that are higher than %f", max_velocity_;
+					"Received a goal with velocities that are higher than " + std::to_string(max_velocity_);
 			gh.setRejected(result_, result_.error_string);
 			print_error(result_.error_string);
 			return;