Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 10:00:48 +02:00
Code Releases Activity

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

Browse Source
This commit is contained in:
Thomas Timm Andersen
2015-10-20 12:16:38 +02:00
parent 8a4cdb51da
commit 1c3fe82116
3 changed files with 68 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
src/robot_state.cpp
View File

@@ -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;

32
src/ur_ros_wrapper.cpp
View File

@@ -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;