diff --git a/abb_driver/rapid/ROS_common.sys b/abb_driver/rapid/ROS_common.sys index b24b0ea4..3bd048f6 100644 --- a/abb_driver/rapid/ROS_common.sys +++ b/abb_driver/rapid/ROS_common.sys @@ -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; @@ -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 \ No newline at end of file +ENDMODULE diff --git a/abb_driver/rapid/ROS_messages.sys b/abb_driver/rapid/ROS_messages.sys index 9127533e..596262bf 100644 --- a/abb_driver/rapid/ROS_messages.sys +++ b/abb_driver/rapid/ROS_messages.sys @@ -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; @@ -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; @@ -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 @@ -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); @@ -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 @@ -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 @@ -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 \ No newline at end of file +ENDMODULE diff --git a/abb_driver/rapid/ROS_motion.mod b/abb_driver/rapid/ROS_motion.mod index 88c9cde6..5a1f65de 100644 --- a/abb_driver/rapid/ROS_motion.mod +++ b/abb_driver/rapid/ROS_motion.mod @@ -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; @@ -68,7 +68,7 @@ PROC main() trajectory_size := 0; ! trajectory done ENDIF - + WaitTime 0.05; ! Throttle loop while waiting for new command ENDWHILE ERROR @@ -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 ) @@ -119,8 +119,8 @@ ENDPROC LOCAL TRAP new_trajectory_handler IF (NOT ROS_new_trajectory) RETURN; - + abort_trajectory; ENDTRAP -ENDMODULE \ No newline at end of file +ENDMODULE diff --git a/abb_driver/rapid/ROS_motionServer.mod b/abb_driver/rapid/ROS_motionServer.mod index b89c14c6..88abc817 100644 --- a/abb_driver/rapid/ROS_motionServer.mod +++ b/abb_driver/rapid/ROS_motionServer.mod @@ -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 @@ -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 @@ -115,5 +115,5 @@ LOCAL PROC activate_trajectory() ROS_new_trajectory := TRUE; ROS_trajectory_lock := FALSE; ! release data-lock ENDPROC - + ENDMODULE diff --git a/abb_driver/rapid/ROS_socket.sys b/abb_driver/rapid/ROS_socket.sys index 901ed332..86a8aaea 100644 --- a/abb_driver/rapid/ROS_socket.sys +++ b/abb_driver/rapid/ROS_socket.sys @@ -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 @@ -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 @@ -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 \ No newline at end of file +ENDMODULE diff --git a/abb_driver/rapid/ROS_stateServer.mod b/abb_driver/rapid/ROS_stateServer.mod index 4cc250ff..0b58ba68 100644 --- a/abb_driver/rapid/ROS_stateServer.mod +++ b/abb_driver/rapid/ROS_stateServer.mod @@ -37,38 +37,39 @@ LOCAL VAR socketdev client_socket; PROC main() TPWrite "StateServer: 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 - send_joints; - WaitTime update_rate; + + WHILE (TRUE) DO + send_joints; + send_status; + WaitTime update_rate; 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 StateServer disconnect", "Connection lost. Waiting for new connection."; ExitCycle; ! restart program - ELSE - TRYNEXT; - ENDIF + ELSE + TRYNEXT; + ENDIF UNDO ENDPROC LOCAL PROC send_joints() - VAR ROS_msg_joint_data message; - VAR jointtarget joints; - + VAR ROS_msg_joint_data message; + VAR jointtarget joints; + ! get current joint position (degrees) - joints := CJointT(); - + joints := CJointT(); + ! create message message.header := [ROS_MSG_TYPE_JOINT, ROS_COM_TYPE_TOPIC, ROS_REPLY_TYPE_INVALID]; message.sequence_id := 0; message.joints := joints.robax; message.ext_axes := joints.extax; - + ! send message to client ROS_send_msg_joint_data client_socket, message; @@ -76,4 +77,87 @@ ERROR RAISE; ! raise errors to calling code ENDPROC -ENDMODULE \ No newline at end of file +! signalExecutionError : System Output +! signalMotionPossible : System Output +! signalMotorOn : System Output +! signalRobotActive : System Output +! signalRobotEStop : System Output +! signalRobotNotMoving : System Output +! signalRosMotionTaskExecuting : System Output +LOCAL PROC send_status() + VAR ROS_msg_robot_status message; + + ! get current joint position (degrees) + ! joints := CJointT(); + + ! create message + message.header := [ROS_MSG_TYPE_STATUS, ROS_COM_TYPE_TOPIC, ROS_REPLY_TYPE_INVALID]; + message.sequence_id := 0; + + ! default values + message.mode := ROS_ROBOT_MODE_UNKNOWN; + message.e_stopped := ROS_TRISTATE_UNKNOWN; + message.drives_powered := ROS_TRISTATE_UNKNOWN; + message.error_code := ROS_TRISTATE_UNKNOWN; + message.in_error := ROS_TRISTATE_UNKNOWN; + message.in_motion := ROS_TRISTATE_UNKNOWN; + message.motion_possible := ROS_TRISTATE_UNKNOWN; + + ! Get operating mode + TEST OpMode() + CASE OP_AUTO: + message.mode := ROS_ROBOT_MODE_AUTO; + CASE OP_MAN_PROG, OP_MAN_TEST: + message.mode := ROS_ROBOT_MODE_MANUAL; + CASE OP_UNDEF: + message.mode := ROS_ROBOT_MODE_UNKNOWN; + ENDTEST + + ! Get E-stop status + IF DOutput(signalRobotEStop) = 1 THEN + message.e_stopped := ROS_TRISTATE_ON; + ELSE + message.e_stopped := ROS_TRISTATE_OFF; + ENDIF + + ! Get whether motors have power + IF DOutput(signalMotorOn) = 1 THEN + message.drives_powered := ROS_TRISTATE_TRUE; + ELSE + message.drives_powered := ROS_TRISTATE_FALSE; + ENDIF + + ! Determine in_error and set error_code if in_error is true + if DOutput(signalExecutionError) = 1 THEN + message.in_error := ROS_TRISTATE_TRUE; + message.error_code := ERRNO; + ELSE + message.in_error := ROS_TRISTATE_FALSE; + message.error_code := 0; + ENDIF + + ! Get in_motion + IF DOutput(signalRobotNotMoving) = 1 THEN + message.in_motion := ROS_TRISTATE_FALSE; + ELSE + message.in_motion := ROS_TRISTATE_TRUE; + ENDIF + + ! Get whether motion is possible + if (DOutput(signalMotionPossible) = 1) AND + (DOutput(signalRobotActive) = 1) AND + (DOutput(signalMotorOn) = 1) AND + (DOutput(signalRosMotionTaskExecuting) = 1) THEN + message.motion_possible := ROS_TRISTATE_TRUE; + ELSE + message.motion_possible := ROS_TRISTATE_FALSE; + ENDIF + + ! send message to client + ROS_send_msg_robot_status client_socket, message; + +ERROR + RAISE; ! raise errors to calling code +ENDPROC + +ENDMODULE