mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-10 01:50:46 +02:00
63 lines
1.9 KiB
C++
63 lines
1.9 KiB
C++
/*
|
|
* Copyright 2017, 2018 Simon Rasmussen (refactor)
|
|
*
|
|
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
#include "ur_rtde_driver/ros/urscript_handler.h"
|
|
#include "ur_rtde_driver/log.h"
|
|
|
|
URScriptHandler::URScriptHandler(URCommander& commander) : commander_(commander), state_(RobotState::Error)
|
|
{
|
|
LOG_INFO("Initializing ur_driver/URScript subscriber");
|
|
urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &URScriptHandler::urscriptInterface, this);
|
|
LOG_INFO("The ur_driver/URScript initialized");
|
|
}
|
|
|
|
void URScriptHandler::urscriptInterface(const std_msgs::String::ConstPtr& msg)
|
|
{
|
|
LOG_INFO("Message received");
|
|
std::string str(msg->data);
|
|
if (str.back() != '\n')
|
|
str.append("\n");
|
|
|
|
switch (state_)
|
|
{
|
|
case RobotState::Running:
|
|
if (!commander_.uploadProg(str))
|
|
{
|
|
LOG_ERROR("Program upload failed!");
|
|
}
|
|
break;
|
|
case RobotState::EmergencyStopped:
|
|
LOG_ERROR("Robot is emergency stopped");
|
|
break;
|
|
case RobotState::ProtectiveStopped:
|
|
LOG_ERROR("Robot is protective stopped");
|
|
break;
|
|
case RobotState::Error:
|
|
LOG_ERROR("Robot is not ready, check robot_mode");
|
|
break;
|
|
default:
|
|
LOG_ERROR("Robot is in undefined state");
|
|
break;
|
|
}
|
|
}
|
|
|
|
void URScriptHandler::onRobotStateChange(RobotState state)
|
|
{
|
|
state_ = state;
|
|
}
|