From d8602c2246bf79556eef2340b617c0a1c95d124c Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Fri, 9 Oct 2015 13:18:38 +0200 Subject: [PATCH] Added detection of E-stop and protective stop. --- include/ur_modern_driver/robot_state.h | 31 ++++++- src/robot_state.cpp | 108 +++++++++++++++++++++++-- src/ur_communication.cpp | 1 + src/ur_ros_wrapper.cpp | 71 ++++++++++++++-- 4 files changed, 199 insertions(+), 12 deletions(-) diff --git a/include/ur_modern_driver/robot_state.h b/include/ur_modern_driver/robot_state.h index 2a2b077..48d390f 100644 --- a/include/ur_modern_driver/robot_state.h +++ b/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 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 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_ */ diff --git a/src/robot_state.cpp b/src/robot_state.cpp index d56f7ca..ed5928e 100644 --- a/src/robot_state.cpp +++ b/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; +} diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp index 3b81892..09954ed 100644 --- a/src/ur_communication.cpp +++ b/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_); } } diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 494e6c1..ca34236 100644 --- a/src/ur_ros_wrapper.cpp +++ b/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::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 timestamps; std::vector > 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()); - velocities.push_back(robot_.rt_interface_->robot_state_->getQdActual()); + positions.push_back( + 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_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(); }