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

Publish industrial_msgs::RobotStatus (#5)

Browse Source
This commit is contained in:
Simon Schmeisser
2018-01-02 20:16:41 +01:00
committed by Simon Rasmussen
parent 560affaa78
commit 24eef75d72
6 changed files with 73 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
src/ros/mb_publisher.cpp
View File

@@ -18,6 +18,63 @@ void MBPublisher::publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data
io_pub_.publish(io_msg);
}
void MBPublisher::publishRobotStatus(industrial_msgs::RobotStatus& status, const SharedRobotModeData& data) const
{
//note that this is true as soon as the drives are powered,
//even if the breakes are still closed
//which is in slight contrast to the comments in the
//message definition
status.drives_powered.val = data.robot_power_on;
status.e_stopped.val = data.emergency_stopped;
//I found no way to reliably get information if the robot is moving
//data.programm_running would be true when using this driver to move the robot
//but it would also be true when another programm is running that might not
//in fact contain any movement commands
status.in_motion.val = industrial_msgs::TriState::UNKNOWN;
//the error code, if any, is not transmitted by this protocol
//it can and should be fetched seperately
status.error_code = 0;
//note that e-stop is handled by a seperate variable
status.in_error.val = data.protective_stopped;
status_pub_.publish(status);
}
void MBPublisher::publishRobotStatus(const RobotModeData_V1_X& data) const
{
industrial_msgs::RobotStatus msg;
if (data.robot_mode == robot_mode_V1_X::ROBOT_FREEDRIVE_MODE)
msg.mode.val = industrial_msgs::RobotMode::MANUAL;
else
msg.mode.val = industrial_msgs::RobotMode::AUTO;
//todo: verify that this correct, there is also ROBOT_READY_MODE
msg.motion_possible.val = (data.robot_mode == robot_mode_V1_X::ROBOT_RUNNING_MODE)
? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF;
publishRobotStatus(msg, data);
}
void MBPublisher::publishRobotStatus(const RobotModeData_V3_0__1& data) const
{
industrial_msgs::RobotStatus msg;
msg.motion_possible.val = (data.robot_mode == robot_mode_V3_X::RUNNING)
? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF;
if (data.control_mode == robot_control_mode_V3_X::TEACH)
msg.mode.val = industrial_msgs::RobotMode::MANUAL;
else
msg.mode.val = industrial_msgs::RobotMode::AUTO;
publishRobotStatus(msg, data);
}
bool MBPublisher::consume(MasterBoardData_V1_X& data)
{
ur_msgs::IOStates io_msg;
@@ -42,13 +99,16 @@ bool MBPublisher::consume(MasterBoardData_V3_2& data)
bool MBPublisher::consume(RobotModeData_V1_X& data)
{
publishRobotStatus(data);
return true;
}
bool MBPublisher::consume(RobotModeData_V3_0__1& data)
{
publishRobotStatus(data);
return true;
}
bool MBPublisher::consume(RobotModeData_V3_2& data)
{
publishRobotStatus(data);
return true;
}
}

2
src/ros/rt_publisher.cpp
View File

@@ -116,4 +116,4 @@ bool RTPublisher::consume(RTState_V3_0__1& state)
bool RTPublisher::consume(RTState_V3_2__3& state)
{
return publish(state);
}
}