xr4000_ipc.c

卡内基梅隆大学（CMU）开发的移动机器人控制开发软件包。可对多种机器人进行控制

/*********************************************************
 *
 * This source code is part of the Carnegie Mellon Robot
 * Navigation Toolkit (CARMEN)
 *
 * CARMEN Copyright (c) 2002 Michael Montemerlo, Nicholas
 * Roy, Sebastian Thrun, Dirk Haehnel, Cyrill Stachniss,
 * and Jared Glover
 *
 * CARMEN is free software; you can redistribute it and/or 
 * modify it under the terms of the GNU General Public 
 * License as published by the Free Software Foundation; 
 * either version 2 of the License, or (at your option)
 * any later version.
 *
 * CARMEN is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied 
 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 * PURPOSE.  See the GNU General Public License for more 
 * details.
 *
 * You should have received a copy of the GNU General 
 * Public License along with CARMEN; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place, 
 * Suite 330, Boston, MA  02111-1307 USA
 *
 ********************************************************/

#include <carmen/carmen.h>
#include "xr4000_control.h"

static void carmen_xr4000_velocity_handler(MSG_INSTANCE msgRef, 
					 BYTE_ARRAY callData,
					 void *clientData 
					 __attribute__ ((unused)))
{
  IPC_RETURN_TYPE err;
  FORMATTER_PTR formatter;
  carmen_base_velocity_message vel;
 
  formatter = IPC_msgInstanceFormatter(msgRef);
  err = IPC_unmarshallData(formatter, callData, &vel,
			   sizeof(carmen_base_velocity_message));
  IPC_freeByteArray(callData);
  carmen_test_ipc_return(err, "Could not unmarshall", 
		       IPC_msgInstanceName(msgRef));
  carmen_xr4000_set_velocity(0, vel.tv, vel.rv);
}

static void carmen_xr4000_holonomic_velocity_handler(MSG_INSTANCE msgRef, 
						   BYTE_ARRAY callData,
						   void *clientData 
						   __attribute__ ((unused)))
{
  IPC_RETURN_TYPE err;
  FORMATTER_PTR formatter;
  carmen_base_holonomic_velocity_message vel;
  
  formatter = IPC_msgInstanceFormatter(msgRef);
  err = IPC_unmarshallData(formatter, callData, &vel,
			   sizeof(carmen_base_holonomic_velocity_message));
  IPC_freeByteArray(callData);
  carmen_test_ipc_return(err, "Could not unmarshall", 
		       IPC_msgInstanceName(msgRef));
  carmen_xr4000_set_velocity(vel.xv, vel.yv, vel.rv);
}

void carmen_xr4000_register_ipc_messages(void)
{
  IPC_RETURN_TYPE err;

  /* define messages created by this module */
  err = IPC_defineMsg(CARMEN_BASE_ODOMETRY_NAME, IPC_VARIABLE_LENGTH,
                      CARMEN_BASE_ODOMETRY_FMT);
  carmen_test_ipc_exit(err, "Could not define", CARMEN_BASE_ODOMETRY_NAME);

  err = IPC_defineMsg(CARMEN_BASE_SONAR_NAME, IPC_VARIABLE_LENGTH,
                      CARMEN_BASE_SONAR_FMT);
  carmen_test_ipc_exit(err, "Could not define IPC message", 
		     CARMEN_BASE_SONAR_NAME);

  err = IPC_defineMsg(CARMEN_BASE_VELOCITY_NAME, IPC_VARIABLE_LENGTH,
                      CARMEN_BASE_VELOCITY_FMT);
  carmen_test_ipc_exit(err, "Could not define", CARMEN_BASE_VELOCITY_NAME);

  err = IPC_defineMsg(CARMEN_BASE_HOLONOMIC_VELOCITY_NAME, IPC_VARIABLE_LENGTH,
                      CARMEN_BASE_HOLONOMIC_VELOCITY_FMT);
  carmen_test_ipc_exit(err, "Could not define", 
		     CARMEN_BASE_HOLONOMIC_VELOCITY_NAME);

  /* setup incoming message handlers */
  err = IPC_subscribe(CARMEN_BASE_VELOCITY_NAME, 
		      carmen_xr4000_velocity_handler, NULL);
  carmen_test_ipc_exit(err, "Could not subscribe", CARMEN_BASE_VELOCITY_NAME);
  IPC_setMsgQueueLength(CARMEN_BASE_VELOCITY_NAME, 1);

  err = IPC_subscribe(CARMEN_BASE_HOLONOMIC_VELOCITY_NAME, 
		      carmen_xr4000_holonomic_velocity_handler, NULL);
  carmen_test_ipc_exit(err, "Could not subscribe", 
		     CARMEN_BASE_HOLONOMIC_VELOCITY_NAME);
  IPC_setMsgQueueLength(CARMEN_BASE_HOLONOMIC_VELOCITY_NAME, 1);
}