gripper.kl

--   Copyright 2013 Southwest Research Institute
--
--   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.
PROGRAM ros_gripper


%NOLOCKGROUP

%NOPAUSE= COMMAND + TPENABLE + ERROR 
%COMMENT = 'ROS Ind Gripper'


--------------------------------------------------------------------------------
-- 
-- remote types & constants
-- 
--------------------------------------------------------------------------------
%INCLUDE libssock_t
%INCLUDE libind_pkt_t
%INCLUDE libmt_grp_t




--------------------------------------------------------------------------------
-- 
-- local types & constants
-- 
--------------------------------------------------------------------------------
VAR
	sock_        : ssock_t
	sock_fd_     : FILE       -- file descriptor has to be declared here
	pkt_in       : mt_grp_t   -- incoming joint commands
	pkt_out      : mt_grp_t   -- outgoing reply msgs
	stat_        : INTEGER
	sleep_time   : INTEGER
	do_it        : BOOLEAN
	   reply_code   : INTEGER    -- Reply code (see REPLY_TYPES)
	   b_flag       : BOOLEAN    -- temp var for register read
	   r_value      : REAL      -- float var for register read  
	   s        : INTEGER



CONST
	LOG_PFIX     = 'GRP '  -- 

	SCKT_TAG     =  7       -- which server tag to use
   	SCKT_PORT    =  11010   -- which server port

	LOOP_HZ      = 40       -- Hz





--------------------------------------------------------------------------------
-- 
-- remote routine prototypes
-- 
--------------------------------------------------------------------------------
%INCLUDE libssock_h
%INCLUDE libind_log_h
%INCLUDE libind_mth_h
%INCLUDE libind_hdr_h
%INCLUDE libmt_grp_h



--------------------------------------------------------------------------------
-- 
-- Main program
-- 
--------------------------------------------------------------------------------
BEGIN
	-- init
	stat_      = 0
	sleep_time = ROUND(1000.0 / LOOP_HZ)
	do_it = TRUE


	-- enable log output
	log_clear


	-- init server socket
	stat_ = ssock_ctor(sock_, SCKT_PORT, SCKT_TAG)
	IF stat_ <> 0 THEN
		IF stat_ = TAG_CONF_ERR THEN
			log_error_a(LOG_PFIX + 'TAG config error. TAG nr:', SCKT_TAG)
		ELSE
			log_error_a(LOG_PFIX + 'ssock_ctor err:', stat_)
		ENDIF
		-- nothing we can do, abort
		GOTO exit_on_err
	ENDIF


	-- init incoming packet
	mtgrp_ctor(pkt_in)

	-- init reply packet
	mtgrp_ctor(pkt_out)
   
	-- make sure socket is closed
	-- don t care about result
	stat_ = ssock_dconnf(sock_)


	-- 
	WHILE do_it DO

		-- inform user
		log_info(LOG_PFIX + 'Waiting for ROS gripper')
		SET_FILE_ATR(sock_fd_, ATR_UF)

		-- wait for connection
		stat_ = ssock_accpt2(sock_, sock_fd_)
		IF stat_ <> 0 THEN
			log_error_a(LOG_PFIX + 'sock_accept err:', stat_)
			-- can't continue
			GOTO exit_discon
		ENDIF

		-- inform user
		log_info(LOG_PFIX + 'Connected')

		-- got client, start relay loop
		WHILE do_it DO

			-- get new packet from the socket
			stat_ = mtgrp_rqdsrl(pkt_in, sock_fd_)
			IF stat_ <> 0 THEN
				log_error_a(LOG_PFIX + 'socket err:', stat_)

				-- can't continue
				GOTO exit_discon
			ENDIF
			
		         -- check sequence number for special values
		         IF (pkt_in.header_.comm_type_ = RI_CT_SVCREQ) THEN
		            -- 
		            SELECT pkt_in.cmd_ OF
		               -- 
		               CASE (GRP_OPEN):
		                  log_info(LOG_PFIX + 'Gripper open')
		                  RDO[4]=FALSE; RDO[3]=TRUE;
		                  DELAY 500
		                  reply_code = RI_RT_SUCC
		
		               -- 
		               CASE (GRP_CLOSE):
		                  log_info(LOG_PFIX + 'Gripper close')              
		                  RDO[3]=FALSE; RDO[4]=TRUE; WRITE('CLOSE GRIP ',CR);   
		                  DELAY 500
		                  reply_code = RI_RT_SUCC -- 1: success, 2: fail
		                  GET_REG(9,b_flag,reply_code,r_value,s)
		
		               -- unknown special sequence nr
		               ELSE:
		                  log_warn_a(LOG_PFIX + 'unknown cmd:', pkt_in.cmd_)
		                  log_warn(LOG_PFIX + 'please report')
		                  reply_code = RI_RT_FAIL
		
		            ENDSELECT
		            
		            log_info_a(LOG_PFIX + 'Sending reply: ', reply_code)
		            stat_ = mtgrp_rpsrl( pkt_out, sock_fd_, reply_code,  pkt_in.cmd_)
		            
		            IF stat_ < 0 THEN
		               log_error_a(LOG_PFIX + 'socket err:', stat_)
		
		               -- can't continue
		               GOTO exit_discon
		            ENDIF
		
		         ELSE
		         
		         log_warn('Unexpected comm type')
		         log_warn(ihdr_tostr(pkt_in.header_))
		
		            
		
		         ENDIF
		
		         -- no packets waiting; and no motion in progress,
		         -- sleep a little (1/T)
		         DELAY sleep_time


		-- inner WHILE TRUE DO
		ENDWHILE

		-- exit with forced disconnect
exit_discon::
		stat_ = ssock_dconn2(sock_, sock_fd_)

	-- outer WHILE TRUE DO
	ENDWHILE


exit_on_err::
	-- nothing

END ros_gripper