Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Rebased and updated version of PR145 (robot_status msg publishing) #168

Merged
merged 7 commits into from
Jun 30, 2019
Merged

Rebased and updated version of PR145 (robot_status msg publishing) #168

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
ecc6959
driver: use 4-space indent in all RAPID files.
yamokosk Feb 28, 2018
5835b24
driver: add RobotStatus message publishing.
yamokosk Feb 28, 2018
f495bdb
driver: fix the OpMode function syntax and add check for OP_UNDEF.
keerthanamanivannan Jan 7, 2019
1b3067a
driver: implement motion_possible check.
keerthanamanivannan Jan 7, 2019
6673c51
driver: base in_error on Execution Error system output.
keerthanamanivannan Jan 9, 2019
e8328ec
driver: only set error_code if in_error.
keerthanamanivannan Jan 9, 2019
ae0cc35
driver: sort signal defs in state server status msg pub.
gavanderhoorn May 24, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions abb_driver/rapid/ROS_common.sys
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,9 +29,9 @@ MODULE ROS_common(SYSMODULE)
! WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

RECORD ROS_joint_trajectory_pt
robjoint joint_pos;
robjoint joint_pos;
extjoint extax_pos;
num duration;
num duration;
ENDRECORD

CONST num MAX_TRAJ_LENGTH := 100;
Expand All @@ -42,4 +42,4 @@ PERS ROS_joint_trajectory_pt ROS_trajectory{MAX_TRAJ_LENGTH};
PERS num ROS_trajectory_size := 0;
PERS bool ROS_new_trajectory := false; ! can safely READ, but should use lock to WRITE

ENDMODULE
ENDMODULE
78 changes: 63 additions & 15 deletions abb_driver/rapid/ROS_messages.sys
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,10 +53,23 @@ RECORD ROS_msg_joint_data
extjoint ext_axes;
ENDRECORD

RECORD ROS_msg_robot_status
ROS_msg_header header;
num sequence_id;
num drives_powered;
num e_stopped;
num error_code;
num in_error;
num in_motion;
num mode;
num motion_possible;
ENDRECORD

! Message Type Codes (from simple_message/simple_message.h)
CONST num ROS_MSG_TYPE_INVALID := 0;
CONST num ROS_MSG_TYPE_JOINT := 10; ! joint-position feedback
CONST num ROS_MSG_TYPE_JOINT_TRAJ_PT := 11; ! joint-trajectory-point (for path downloading)
CONST num ROS_MSG_TYPE_STATUS := 13; ! robot status message (for reporting the robot state)
CONST num ROS_COM_TYPE_TOPIC := 1;
CONST num ROS_COM_TYPE_SRV_REQ := 2;
CONST num ROS_COM_TYPE_SRV_REPLY := 3;
Expand All @@ -69,38 +82,50 @@ CONST num ROS_TRAJECTORY_START_DOWNLOAD := -1;
CONST num ROS_TRAJECTORY_END := -3;
CONST num ROS_TRAJECTORY_STOP := -4;

! Robot mode codes (from industrial_msgs/RobotMode.msg)
CONST num ROS_ROBOT_MODE_UNKNOWN := -1; ! Unknown or unavailable
CONST num ROS_ROBOT_MODE_MANUAL := 1; ! Teach OR manual mode
CONST num ROS_ROBOT_MODE_AUTO := 2; ! Automatic mode

! Tri-state values (from inudstrial/TriState.msg)
CONST num ROS_TRISTATE_UNKNOWN := -1;
CONST num ROS_TRISTATE_TRUE := 1;
CONST num ROS_TRISTATE_ON := 1;
CONST num ROS_TRISTATE_FALSE := 0;
CONST num ROS_TRISTATE_OFF := 0;

! Other message constants
CONST num ROS_MSG_MAX_JOINTS := 10; ! from joint_data.h

PROC ROS_receive_msg_joint_traj_pt(VAR socketdev client_socket, VAR ROS_msg_joint_traj_pt message, \num wait_time)
VAR ROS_msg raw_message;

! Read raw message data
IF Present(wait_time) THEN
ROS_receive_msg client_socket, raw_message, \wait_time:=wait_time;
ELSE
ROS_receive_msg client_socket, raw_message;
ENDIF

! Integrity Check: Message Type
IF (raw_message.header.msg_type <> ROS_MSG_TYPE_JOINT_TRAJ_PT) THEN
ErrWrite \W, "ROS Socket Type Mismatch", "Unexpected message type",
\RL2:="expected: " + ValToStr(ROS_MSG_TYPE_JOINT_TRAJ_PT),
\RL3:="received: " + ValToStr(raw_message.header.msg_type);
\RL2:="expected: " + ValToStr(ROS_MSG_TYPE_JOINT_TRAJ_PT),
\RL3:="received: " + ValToStr(raw_message.header.msg_type);
RAISE ERR_ARGVALERR; ! TBD: define specific error code
ENDIF

! Integrity Check: Data Size
IF (RawBytesLen(raw_message.data) < 52) THEN
ErrWrite \W, "ROS Socket Missing Data", "Insufficient data for joint_trajectory_pt",
\RL2:="expected: 52",
\RL3:="received: " + ValToStr(RawBytesLen(raw_message.data));
\RL2:="expected: 52",
\RL3:="received: " + ValToStr(RawBytesLen(raw_message.data));
RAISE ERR_OUTOFBND; ! TBD: define specific error code
ENDIF

! Copy Header data
message.header := raw_message.header;

! Unpack data fields
UnpackRawBytes raw_message.data, 1, message.sequence_id, \IntX:=DINT;
UnpackRawBytes raw_message.data, 5, message.joints.rax_1, \Float4;
Expand All @@ -120,7 +145,7 @@ PROC ROS_receive_msg_joint_traj_pt(VAR socketdev client_socket, VAR ROS_msg_join
message.joints := rad2deg_robjoint(message.joints);
message.ext_axes := m2mm_extjoint(message.ext_axes);
! TBD: convert velocity

ERROR
RAISE; ! raise errors to calling code
ENDPROC
Expand All @@ -130,12 +155,12 @@ PROC ROS_send_msg_joint_data(VAR socketdev client_socket, ROS_msg_joint_data mes
VAR robjoint ROS_joints;
VAR extjoint ROS_ext_axes;
VAR num i;

! Force message header to the correct values
raw_message.header.msg_type := ROS_MSG_TYPE_JOINT;
raw_message.header.comm_type := ROS_COM_TYPE_TOPIC;
raw_message.header.reply_code := ROS_REPLY_TYPE_INVALID;

! Convert data from ABB units to ROS units
ROS_joints := deg2rad_robjoint(message.joints);
ROS_ext_axes := mm2m_extjoint(message.ext_axes);
Expand All @@ -159,6 +184,29 @@ ERROR
RAISE; ! raise errors to calling code
ENDPROC

PROC ROS_send_msg_robot_status(VAR socketdev client_socket, ROS_msg_robot_status message)
VAR ROS_msg raw_message;

! Force message header to the correct values
raw_message.header.msg_type := message.header.msg_type;
raw_message.header.comm_type := message.header.comm_type;
raw_message.header.reply_code := message.header.reply_code;

! Pack data into message
PackRawBytes message.drives_powered, raw_message.data, 1, \IntX:=DINT;
PackRawBytes message.e_stopped, raw_message.data, 5, \IntX:=DINT;
PackRawBytes message.error_code, raw_message.data, 9, \IntX:=DINT;
PackRawBytes message.in_error, raw_message.data, 13, \IntX:=DINT;
PackRawBytes message.in_motion, raw_message.data, 17, \IntX:=DINT;
PackRawBytes message.mode, raw_message.data, 21, \IntX:=DINT;
PackRawBytes message.motion_possible, raw_message.data, 25, \IntX:=DINT;

ROS_send_msg client_socket, raw_message;

ERROR
RAISE; ! raise errors to calling code
ENDPROC

LOCAL FUNC num deg2rad(num deg)
RETURN deg * pi / 180;
ENDFUNC
Expand Down Expand Up @@ -188,7 +236,7 @@ LOCAL FUNC extjoint m2mm_extjoint(extjoint eax_in_m)
eax_in_mm.eax_b := eax_in_m.eax_b * 1000;
eax_in_mm.eax_c := eax_in_m.eax_c * 1000;
eax_in_mm.eax_d := eax_in_m.eax_d * 1000;

RETURN eax_in_mm;
ENDFUNC

Expand Down Expand Up @@ -216,8 +264,8 @@ LOCAL FUNC robjoint rad2deg_robjoint(robjoint rad)
deg.rax_4 := rad2deg(rad.rax_4);
deg.rax_5 := rad2deg(rad.rax_5);
deg.rax_6 := rad2deg(rad.rax_6);

RETURN deg;
ENDFUNC

ENDMODULE
ENDMODULE
18 changes: 9 additions & 9 deletions abb_driver/rapid/ROS_motion.mod
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ PROC main()
VAR speeddata move_speed := v10; ! default speed
VAR zonedata stop_mode;
VAR bool skip_move;

! Set up interrupt to watch for new trajectory
IDelete intr_new_trajectory; ! clear interrupt handler, in case restarted with ExitCycle
CONNECT intr_new_trajectory WITH new_trajectory_handler;
Expand Down Expand Up @@ -68,7 +68,7 @@ PROC main()

trajectory_size := 0; ! trajectory done
ENDIF

WaitTime 0.05; ! Throttle loop while waiting for new command
ENDWHILE
ERROR
Expand All @@ -80,17 +80,17 @@ LOCAL PROC init_trajectory()
clear_path; ! cancel any active motions

WaitTestAndSet ROS_trajectory_lock; ! acquire data-lock
trajectory := ROS_trajectory; ! copy to local var
trajectory_size := ROS_trajectory_size; ! copy to local var
ROS_new_trajectory := FALSE;
trajectory := ROS_trajectory; ! copy to local var
trajectory_size := ROS_trajectory_size; ! copy to local var
ROS_new_trajectory := FALSE;
ROS_trajectory_lock := FALSE; ! release data-lock
ENDPROC

LOCAL FUNC bool is_near(jointtarget target, num deg_tol, num mm_tol)
VAR jointtarget curr_jnt;

curr_jnt := CJointT();

! either an external axis is unconfigured/not present OR if it is, then it must be close enough
RETURN ( ABS(curr_jnt.robax.rax_1 - target.robax.rax_1) < deg_tol )
AND ( ABS(curr_jnt.robax.rax_2 - target.robax.rax_2) < deg_tol )
Expand Down Expand Up @@ -119,8 +119,8 @@ ENDPROC

LOCAL TRAP new_trajectory_handler
IF (NOT ROS_new_trajectory) RETURN;

abort_trajectory;
ENDTRAP

ENDMODULE
ENDMODULE
46 changes: 23 additions & 23 deletions abb_driver/rapid/ROS_motionServer.mod
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,53 +39,53 @@ PROC main()
VAR ROS_msg_joint_traj_pt message;

TPWrite "MotionServer: Waiting for connection.";
ROS_init_socket server_socket, server_port;
ROS_init_socket server_socket, server_port;
ROS_wait_for_client server_socket, client_socket;

WHILE ( true ) DO
! Recieve Joint Trajectory Pt Message
! Recieve Joint Trajectory Pt Message
ROS_receive_msg_joint_traj_pt client_socket, message;
trajectory_pt_callback message;
ENDWHILE
trajectory_pt_callback message;
ENDWHILE

ERROR (ERR_SOCK_TIMEOUT, ERR_SOCK_CLOSED)
IF (ERRNO=ERR_SOCK_TIMEOUT) OR (ERRNO=ERR_SOCK_CLOSED) THEN
IF (ERRNO=ERR_SOCK_TIMEOUT) OR (ERRNO=ERR_SOCK_CLOSED) THEN
SkipWarn; ! TBD: include this error data in the message logged below?
ErrWrite \W, "ROS MotionServer disconnect", "Connection lost. Resetting socket.";
ExitCycle; ! restart program
ELSE
TRYNEXT;
ENDIF
ExitCycle; ! restart program
ELSE
TRYNEXT;
ENDIF
UNDO
IF (SocketGetStatus(client_socket) <> SOCKET_CLOSED) SocketClose client_socket;
IF (SocketGetStatus(server_socket) <> SOCKET_CLOSED) SocketClose server_socket;
IF (SocketGetStatus(client_socket) <> SOCKET_CLOSED) SocketClose client_socket;
IF (SocketGetStatus(server_socket) <> SOCKET_CLOSED) SocketClose server_socket;
ENDPROC

LOCAL PROC trajectory_pt_callback(ROS_msg_joint_traj_pt message)
VAR ROS_joint_trajectory_pt point;
VAR jointtarget current_pos;
VAR ROS_joint_trajectory_pt point;
VAR jointtarget current_pos;
VAR ROS_msg reply_msg;

point := [message.joints, message.ext_axes, message.duration];

! use sequence_id to signal start/end of trajectory download
TEST message.sequence_id
CASE ROS_TRAJECTORY_START_DOWNLOAD:
TEST message.sequence_id
CASE ROS_TRAJECTORY_START_DOWNLOAD:
TPWrite "Traj START received";
trajectory_size := 0; ! Reset trajectory size
trajectory_size := 0; ! Reset trajectory size
add_traj_pt point; ! Add this point to the trajectory
CASE ROS_TRAJECTORY_END:
CASE ROS_TRAJECTORY_END:
TPWrite "Traj END received";
add_traj_pt point; ! Add this point to the trajectory
activate_trajectory;
CASE ROS_TRAJECTORY_STOP:
CASE ROS_TRAJECTORY_STOP:
TPWrite "Traj STOP received";
trajectory_size := 0; ! empty trajectory
activate_trajectory;
StopMove; ClearPath; StartMove; ! redundant, but re-issue stop command just to be safe
DEFAULT:
DEFAULT:
add_traj_pt point; ! Add this point to the trajectory
ENDTEST
ENDTEST

! send reply, if requested
IF (message.header.comm_type = ROS_COM_TYPE_SRV_REQ) THEN
Expand All @@ -100,7 +100,7 @@ ENDPROC
LOCAL PROC add_traj_pt(ROS_joint_trajectory_pt point)
IF (trajectory_size = MAX_TRAJ_LENGTH) THEN
ErrWrite \W, "Too Many Trajectory Points", "Trajectory has already reached its maximum size",
\RL2:="max_size = " + ValToStr(MAX_TRAJ_LENGTH);
\RL2:="max_size = " + ValToStr(MAX_TRAJ_LENGTH);
ELSE
Incr trajectory_size;
trajectory{trajectory_size} := point; !Add this point to the trajectory
Expand All @@ -115,5 +115,5 @@ LOCAL PROC activate_trajectory()
ROS_new_trajectory := TRUE;
ROS_trajectory_lock := FALSE; ! release data-lock
ENDPROC

ENDMODULE
12 changes: 6 additions & 6 deletions abb_driver/rapid/ROS_socket.sys
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,11 +62,11 @@ PROC ROS_receive_msg(VAR socketdev client_socket, VAR ROS_msg message, \num wait
IF Present(wait_time) time_val := wait_time;

! TBD: need to determine whether this handles split/merged messages correctly

! Read prefix INT (total message length)
SocketReceive client_socket, \RawData:=buffer, \ReadNoOfBytes:=4, \Time:=time_val;
UnpackRawBytes buffer, 1, msg_length, \IntX:=UDINT;

! Read remaining message bytes
SocketReceive client_socket, \RawData:=buffer, \ReadNoOfBytes:=msg_length, \NoRecBytes:=bytes_rcvd, \Time:=time_val;
IF (bytes_rcvd <> msg_length) THEN
Expand All @@ -75,13 +75,13 @@ PROC ROS_receive_msg(VAR socketdev client_socket, VAR ROS_msg message, \num wait
\RL3:="Received: " + ValToStr(bytes_rcvd);
RETURN;
ENDIF

! Unpack message header/data
UnpackRawBytes buffer, 1, message.header.msg_type, \IntX:=DINT;
UnpackRawBytes buffer, 5, message.header.comm_type, \IntX:=DINT;
UnpackRawBytes buffer, 9, message.header.reply_code, \IntX:=DINT;
CopyRawBytes buffer, 13, message.data, 1;

ERROR
RAISE; ! raise errors to calling code
ENDPROC
Expand All @@ -96,9 +96,9 @@ PROC ROS_send_msg(VAR socketdev client_socket, VAR ROS_msg message)
CopyRawBytes message.data, 1, buffer, 17; ! Message data

SocketSend client_socket \RawData:=buffer;

ERROR
RAISE; ! raise errors to calling code
ENDPROC

ENDMODULE
ENDMODULE
Loading