[TeamTalk 143]: [680] moast-bth/moast: Update from moast.
tk@edam.speech.cs.cmu.edu
tk at edam.speech.cs.cmu.edu
Wed Aug 1 13:41:34 EDT 2007
An HTML attachment was scrubbed...
URL: http://mailman.srv.cs.cmu.edu/pipermail/teamtalk-developers/attachments/20070801/0732dc3d/attachment-0001.html
-------------- next part --------------
Modified: moast-bth/moast/bin/Makefile.am
===================================================================
--- moast-bth/moast/bin/Makefile.am 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/bin/Makefile.am 2007-08-01 17:41:33 UTC (rev 680)
@@ -211,7 +211,6 @@
INCLUDES += @GTK_CFLAGS@
bin_PROGRAMS += servoGui gtksplash displayGraph imageview notifier
-bin_PROGRAMS += gtksplash displayGraph imageview notifier
servoGui_SOURCES = ../src/servo/servoGui.cc
servoGui_CXXFLAGS = @GTK_CFLAGS@ $(AM_CXXFLAGS)
servoGui_LDADD = @GTK_LIBS@ ../lib/libmoast.a -L$(RCSLIB_DIR)/lib -lrcs -lposemath -lm
Modified: moast-bth/moast/bin/run
===================================================================
--- moast-bth/moast/bin/run 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/bin/run 2007-08-01 17:41:33 UTC (rev 680)
@@ -26,6 +26,13 @@
# END OF USER EDIT AREA
#
+# get RCS directory and add to shared library load path
+RCSLIB_DIR=`grep RCSLIB_DIR ../Makefile | awk '{print $3}'`
+if test x$RCSLIB_DIR != x ; then
+ LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$RCSLIB_DIR/lib
+ export LD_LIBRARY_PATH
+fi
+
echo `dirname $0`/..
if [ $# = 0 ]; then
ID=1
Modified: moast-bth/moast/configure.ac
===================================================================
--- moast-bth/moast/configure.ac 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/configure.ac 2007-08-01 17:41:33 UTC (rev 680)
@@ -79,15 +79,15 @@
AC_HAVE_MYSQL
# Checks for Tk headers and library.
-ACX_TK_INCLUDE
-ACX_TCL_LIB
-ACX_TK_LIB
+# ACX_TK_INCLUDE
+# ACX_TCL_LIB
+# ACX_TK_LIB
# Checks for Perl devel.
-AC_HAVE_PERL_DEVEL
+# AC_HAVE_PERL_DEVEL
# Checks for Python devel.
-AC_HAVE_PYTHON_DEVEL
+# AC_HAVE_PYTHON_DEVEL
# Checks for SDL.
ACX_HAVE_SDL
Modified: moast-bth/moast/etc/moast.ini
===================================================================
--- moast-bth/moast/etc/moast.ini 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/etc/moast.ini 2007-08-01 17:41:33 UTC (rev 680)
@@ -326,16 +326,6 @@
UTM_START_POSE_2 = -41.2 10.60 -10.0 0.0 0.0 0
UTM_START_POSE_1 = 5.23 18.81 1.4 0.0 0.0 0
-[DM-RoboticsLab_250]
-UNREAL_UTM_OFFSET = 0.0 0.0 0.0
-RESOLUTION = 0.01905
-UTM_LETTER = N
-UTM_ZONE = 18
-UTM_START_POST_COUNT = 2
-UTM_START_POSE_1 = 25 8.5 -2.0 0 0 0 0
-UTM_START_POSE_2 = 25 6.5 -2.0 0 0 0 0
-UTM_START_POSE_1 = 26 7.5 -2.0 0 0 0 0
-
[DM-ValidationTest_250]
UREAL_UTM_OFFSET = 0. 0. 0
RESOLUTION = 0.01905
@@ -390,11 +380,11 @@
RESOLUTION = 0.01905
UTM_LETTER = N
UTM_ZONE = 18
-UTM_START_POSE_COUNT = 2
-UTM_START_POSE_1 = 25.0 6.5 -1. 0. 0. 3.14159265
-UTM_START_POSE_2 = 25.0 7.5 -1. 0. 0. 3.14159265
+UTM_START_POSE_COUNT = 3
+UTM_START_POSE_1 = 25. 6.5 -1. 0. 0. 3.14159265
+UTM_START_POSE_2 = 25. 7.5 -1. 0. 0. 3.14159265
+UTM_START_POSE_3 = 26. 7.5 -1. 0. 0. 3.14159265
-
[DM-TallTestWorld_250]
UNREAL_UTM_OFFSET = 0. 0. 0
RESOLUTION = 0.01905
Modified: moast-bth/moast/etc/moast.nml
===================================================================
--- moast-bth/moast/etc/moast.nml 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/etc/moast.nml 2007-08-01 17:41:33 UTC (rev 680)
@@ -332,11 +332,13 @@
B trafficCtrlCfg1 SHMEM localhost 1088 0 * 273 * 25473 TCP=20474 xdr
B trafficCtrlSet1 SHMEM localhost 1088 0 * 274 * 25474 TCP=20475 xdr
+B joystickData SHMEM localhost 1088 0 * 275 * 25475 TCP=20476 xdr
+
# SPQR Buffers
-B MapBufP2DX SHMEM localhost 160192 0 * 275 * 25475 TCP=20476 xdr
-B MapBufZerg SHMEM localhost 160192 0 * 276 * 25476 TCP=20477 xdr
-B PosBufMapper SHMEM localhost 1088 0 * 277 * 25477 TCP=20478 xdr
-B PosBufVictim SHMEM localhost 1088 0 * 278 * 25478 TCP=20479 xdr
+B MapBufP2DX SHMEM localhost 160192 0 * 276 * 25476 TCP=20477 xdr
+B MapBufZerg SHMEM localhost 160192 0 * 277 * 25477 TCP=20478 xdr
+B PosBufMapper SHMEM localhost 1088 0 * 278 * 25478 TCP=20479 xdr
+B PosBufVictim SHMEM localhost 1088 0 * 279 * 25479 TCP=20480 xdr
# MOAST NML Svr Configuration
@@ -479,6 +481,7 @@
P moastNmlSvr MapBufZerg LOCAL localhost RW 1 INF 1 0
P moastNmlSvr PosBufMapper LOCAL localhost RW 1 INF 1 0
P moastNmlSvr PosBufVictim LOCAL localhost RW 1 INF 1 0
+P moastNmlSvr joystickData LOCAL localhost RW 1 INF 1 0
# SIM-ware Configuration
@@ -727,6 +730,7 @@
P servoShell servoMisJACfg4_2 LOCAL localhost RW 0 INF 0 1
P servoShell servoMisJACfg4_3 LOCAL localhost RW 0 INF 0 1
P servoShell servoMisJACfg4_4 LOCAL localhost RW 0 INF 0 1
+P servoShell joystickData LOCAL localhost RW 0 INF 0 1
# Prim Echelon Configuration
P primSP navDataExt1 LOCAL localhost RW 0 INF 0 1
@@ -1467,6 +1471,7 @@
P joystick servoMisJACfg4_2 LOCAL localhost RW 0 INF 0 3
P joystick servoMisJACfg4_3 LOCAL localhost RW 0 INF 0 3
P joystick servoMisJACfg4_4 LOCAL localhost RW 0 INF 0 3
+P joystick joystickData REMOTE localhost RW 0 INF 0 2
P moastErrorLog moastError LOCAL localhost RW 0 INF 0 3
Modified: moast-bth/moast/etc/moastBuffers.cfg
===================================================================
--- moast-bth/moast/etc/moastBuffers.cfg 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/etc/moastBuffers.cfg 2007-08-01 17:41:33 UTC (rev 680)
@@ -381,3 +381,6 @@
b bufname=trafficCtrlStat1 size=1024
b bufname=trafficCtrlCfg1 size=1024
b bufname=trafficCtrlSet1 size=1024
+
+# joystick buffers
+b bufname=joystickData size=1024
Modified: moast-bth/moast/etc/moastServo.cfg
===================================================================
--- moast-bth/moast/etc/moastServo.cfg 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/etc/moastServo.cfg 2007-08-01 17:41:33 UTC (rev 680)
@@ -275,4 +275,6 @@
p bufname=servoMisJACfg4_2 name=servoShell master=0 server=0 proctype=local
p bufname=servoMisJACfg4_3 name=servoShell master=0 server=0 proctype=local
p bufname=servoMisJACfg4_4 name=servoShell master=0 server=0 proctype=local
+# ...
+p bufname=joystickData name=servoShell master=0 server=0 proctype=local
Modified: moast-bth/moast/etc/moastSvr.cfg
===================================================================
--- moast-bth/moast/etc/moastSvr.cfg 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/etc/moastSvr.cfg 2007-08-01 17:41:33 UTC (rev 680)
@@ -171,3 +171,6 @@
p bufname=MapBufZerg name=moastNmlSvr master=1 server=1 proctype=local
p bufname=PosBufMapper name=moastNmlSvr master=1 server=1 proctype=local
p bufname=PosBufVictim name=moastNmlSvr master=1 server=1 proctype=local
+#
+p bufname=joystickData name=moastNmlSvr master=1 server=1 proctype=local
+
Modified: moast-bth/moast/etc/moastTools.cfg
===================================================================
--- moast-bth/moast/etc/moastTools.cfg 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/etc/moastTools.cfg 2007-08-01 17:41:33 UTC (rev 680)
@@ -176,6 +176,8 @@
p bufname=servoMisJACfg4_2 name=joystick master=0 server=0 proctype=local
p bufname=servoMisJACfg4_3 name=joystick master=0 server=0 proctype=local
p bufname=servoMisJACfg4_4 name=joystick master=0 server=0 proctype=local
+#
+p bufname=joystickData name=joystick master=0 server=0 proctype=remote
#
# moastErrorLog
Added: moast-bth/moast/include/joystickData.hh
===================================================================
--- moast-bth/moast/include/joystickData.hh (rev 0)
+++ moast-bth/moast/include/joystickData.hh 2007-08-01 17:41:33 UTC (rev 680)
@@ -0,0 +1,63 @@
+/*
+ DISCLAIMER:
+ This software was produced by the National Institute of Standards
+ and Technology (NIST), an agency of the U.S. government, and by statute is
+ not subject to copyright in the United States. Recipients of this software
+ assume all responsibility associated with its operation, modification,
+ maintenance, and subsequent redistribution.
+
+ See NIST Administration Manual 4.09.07 b and Appendix I.
+*/
+
+/*!
+ \file joystickData.hh
+
+ \brief NML declarations for joystick data channel.
+
+ This channel information about the state of joystick control,
+ such as which mode is active (joint, mobility, world, tool).
+
+ \code CVS Status:
+ $Author: proctor $
+ $Revision: 1.1 $
+ $Date: 2007/07/24 20:30:36 $
+ \endcode
+
+ \author Fred Proctor
+*/
+
+#ifndef JOYSTICK_DATA_HH
+#define JOYSTICK_DATA_HH
+
+#include "moastNmlOffsets.hh" // JOYSTICK_DATA_BASE
+
+#define JOYSTICK_DATA_TYPE (JOYSTICK_DATA_BASE + 1)
+
+#define JOYSTICK_MODE_MOBILITY 1
+#define JOYSTICK_MODE_JOINT 2
+#define JOYSTICK_MODE_TOOL 3
+#define JOYSTICK_MODE_WORLD 4
+
+class JoystickData : public NMLmsg {
+public:
+ JoystickData() : NMLmsg(JOYSTICK_DATA_TYPE, sizeof(JoystickData)) {
+ mode = JOYSTICK_MODE_MOBILITY;
+ };
+ void update(CMS *);
+
+ int mode; //!< one of JOYSTICK_MODE_MOBILITY, ...
+};
+
+extern int joystickData_format(NMLTYPE type, void * buf, CMS * cms);
+extern const char *joystickData_symbol_lookup(long type);
+
+#endif // JOYSTICK_DATA_HH
+
+/*
+ Modification history:
+
+ $Log: joystickData.hh,v $
+ Revision 1.1 2007/07/24 20:30:36 proctor
+ Initial checkin
+
+*/
Modified: moast-bth/moast/include/moastNmlGlobal.hh
===================================================================
--- moast-bth/moast/include/moastNmlGlobal.hh 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/include/moastNmlGlobal.hh 2007-08-01 17:41:33 UTC (rev 680)
@@ -16,9 +16,9 @@
functions.
\code CVS Status:
- $Author: dr_steveb $
- $Revision: 1.10 $
- $Date: 2007/06/28 12:14:12 $
+ $Author: proctor $
+ $Revision: 1.11 $
+ $Date: 2007/07/24 18:38:22 $
\endcode
\author Fred Proctor
@@ -175,6 +175,8 @@
extern NML *spqrPosBufVictim;
#endif
+extern NML * joystickDataBuf;
+
extern int moastZeroNmlBuffers(void);
extern int moastInitNmlBuffers(char *config_file,
@@ -190,6 +192,9 @@
Modification history:
$Log: moastNmlGlobal.hh,v $
+ Revision 1.11 2007/07/24 18:38:22 proctor
+ Added JoystickData
+
Revision 1.10 2007/06/28 12:14:12 dr_steveb
Removed 0 length arrays that Microsoft compiliers complain about
Modified: moast-bth/moast/include/moastNmlOffsets.hh
===================================================================
--- moast-bth/moast/include/moastNmlOffsets.hh 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/include/moastNmlOffsets.hh 2007-08-01 17:41:33 UTC (rev 680)
@@ -15,9 +15,9 @@
\brief Starting numbers for MOAST NMLtype declarations
\code CVS Status:
- $Author: dr_steveb $
- $Revision: 1.3 $
- $Date: 2007/04/17 17:13:35 $
+ $Author: proctor $
+ $Revision: 1.4 $
+ $Date: 2007/07/24 18:38:22 $
\endcode
\author Fred Proctor
@@ -78,12 +78,19 @@
//! used by trafficCtrl.hh
#define TRAFFIC_CTRL_BASE 16500
+
+//! used by joystickData.hh
+#define JOYSTICK_DATA_BASE 17000
+
#endif // MOAST_NML_OFFSETS_HH
/*
Modification history:
$Log: moastNmlOffsets.hh,v $
+ Revision 1.4 2007/07/24 18:38:22 proctor
+ Added JoystickData
+
Revision 1.3 2007/04/17 17:13:35 dr_steveb
Added traffic simulation offsets
Modified: moast-bth/moast/lib/Makefile.am
===================================================================
--- moast-bth/moast/lib/Makefile.am 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/lib/Makefile.am 2007-08-01 17:41:33 UTC (rev 680)
@@ -1,7 +1,7 @@
INCLUDES = -I$(RCSLIB_DIR)/include
lib_LIBRARIES = libmoast.a
-libmoast_a_SOURCES = ../src/nml/moastTypes.cc ../src/nml/moastNmlGlobal.cc ../src/nml/moastError.cc ../src/nml/navDataExt.cc ../src/nml/servoSP.cc ../src/nml/servoMobJA.cc ../src/nml/servoMisJA.cc ../src/nml/primSP.cc ../src/nml/primMobJA.cc ../src/nml/primMisJA.cc ../src/nml/amSP.cc ../src/nml/amMisJA.cc ../src/nml/amMobJA.cc ../src/nml/sensorData.cc ../src/common/acmRandom.c ../src/common/utmConvert.cc ../src/nml/vehSP.cc ../src/nml/vehMobPL.cc ../src/nml/sectMobPL.cc ../src/common/dijkTabs.cc ../src/common/dijkHeap.cc ../src/common/dijkHeapN.cc ../src/common/aStarHeap.cc ../src/common/aStarHeapN.cc ../src/common/visGraph.cc ../src/common/moastGeom.cc ../src/common/getopt_long.c ../src/nml/nmlMap.cc ../src/nml/nmlPosition.cc ../src/nml/trafficCtrl.cc ../src/nml/trafficData.cc
+libmoast_a_SOURCES = ../src/nml/moastTypes.cc ../src/nml/moastNmlGlobal.cc ../src/nml/moastError.cc ../src/nml/navDataExt.cc ../src/nml/servoSP.cc ../src/nml/servoMobJA.cc ../src/nml/servoMisJA.cc ../src/nml/primSP.cc ../src/nml/primMobJA.cc ../src/nml/primMisJA.cc ../src/nml/amSP.cc ../src/nml/amMisJA.cc ../src/nml/amMobJA.cc ../src/nml/sensorData.cc ../src/common/acmRandom.c ../src/common/utmConvert.cc ../src/nml/vehSP.cc ../src/nml/vehMobPL.cc ../src/nml/sectMobPL.cc ../src/common/dijkTabs.cc ../src/common/dijkHeap.cc ../src/common/dijkHeapN.cc ../src/common/aStarHeap.cc ../src/common/aStarHeapN.cc ../src/common/visGraph.cc ../src/common/moastGeom.cc ../src/common/getopt_long.c ../src/nml/nmlMap.cc ../src/nml/nmlPosition.cc ../src/nml/trafficCtrl.cc ../src/nml/trafficData.cc ../src/nml/joystickData.cc
if ! HAVE_GETOPT_H
libmoast_a_SOURCES += ../src/common/getopt_long.c
Modified: moast-bth/moast/src/am/amMobMain.cc
===================================================================
--- moast-bth/moast/src/am/amMobMain.cc 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/src/am/amMobMain.cc 2007-08-01 17:41:33 UTC (rev 680)
@@ -23,8 +23,8 @@
\code CVS Status:
$Author: dr_steveb $
- $Revision: 1.47 $
- $Date: 2007/04/25 15:28:48 $
+ $Revision: 1.48 $
+ $Date: 2007/07/30 20:08:43 $
\endcode
\author Tom Kramer
@@ -2642,7 +2642,7 @@
stts.amMobJAError = AM_MOB_JA_ERROR_WAYPOINT;
state_next(&stts, S2);
}
- else if (updatePosition(0.01, 30))
+ else if (updatePosition(0.01, 300)) // was 30; made absurbly large
{
writePrimCmd(&primStopCmd, subStatPtr->echo_serial_number);
status_next(&stts, RCS_ERROR);
@@ -2830,7 +2830,7 @@
rcs_print_error("%s: amMob not initialized, "
"cannot execute command\n", processName);
}
- else if (updatePosition(0.01, 5) > 0)
+ else if (updatePosition(0.01, 300) > 0) // was 5, made absurbly large
{
writePrimCmd(&primStopCmd, subStatPtr->echo_serial_number);
status_next(&stts, RCS_ERROR);
@@ -7517,6 +7517,9 @@
Modification history:
$Log: amMobMain.cc,v $
+ Revision 1.48 2007/07/30 20:08:43 dr_steveb
+ Made am wait longer for navData
+
Revision 1.47 2007/04/25 15:28:48 dr_steveb
Backed out accidental mods.
Modified: moast-bth/moast/src/nml/Makefile.local
===================================================================
--- moast-bth/moast/src/nml/Makefile.local 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/src/nml/Makefile.local 2007-08-01 17:41:33 UTC (rev 680)
@@ -24,6 +24,7 @@
nmlPosition.cc \
trafficData.cc \
trafficCtrl.cc \
+ joystickData.cc \
all
# get RCSLIB_DIR, "all" et al, although we really only need RCSLIB_DIR
Added: moast-bth/moast/src/nml/joystickData.cc
===================================================================
--- moast-bth/moast/src/nml/joystickData.cc (rev 0)
+++ moast-bth/moast/src/nml/joystickData.cc 2007-08-01 17:41:33 UTC (rev 680)
@@ -0,0 +1,138 @@
+/*
+* New C++ File starts here.
+* This file should be named moastTypes.cc
+*/
+
+// Include all NML, CMS, and RCS classes and functions
+#include "rcs.hh"
+
+// Include command and status message definitions
+#include "moastTypes.hh"
+#include "moastNmlOffsets.hh"
+#include "joystickData.hh"
+
+// Forward Function Prototypes
+
+
+#ifndef MAX_JOYSTICKDATA_NAME_LENGTH
+#define MAX_JOYSTICKDATA_NAME_LENGTH 13
+#endif
+#ifndef JOYSTICKDATA_NAME_LIST_LENGTH
+#define JOYSTICKDATA_NAME_LIST_LENGTH 2
+#endif
+
+
+/* This list must be in alphabetical order and the three lists must correspond. */
+const char joystickData_name_list[JOYSTICKDATA_NAME_LIST_LENGTH][MAX_JOYSTICKDATA_NAME_LENGTH]= {
+ "JoystickData", /* 0,1 */
+ ""};
+const NMLTYPE joystickData_id_list[JOYSTICKDATA_NAME_LIST_LENGTH]= {
+ JOYSTICK_DATA_TYPE, /* 0,1 */
+ -1};
+const size_t joystickData_size_list[JOYSTICKDATA_NAME_LIST_LENGTH]= {
+ sizeof(JoystickData),
+ 0};
+const char *joystickData_symbol_lookup(long type);
+
+
+// Enumerated Type Constants
+
+// anonymous_enum_______include_moastTypes_hh_180
+#ifndef MAX_ENUM_ANONYMOUS_ENUM_______INCLUDE_MOASTTYPES_HH_180_STRING_LENGTH
+#define MAX_ENUM_ANONYMOUS_ENUM_______INCLUDE_MOASTTYPES_HH_180_STRING_LENGTH 17
+#endif
+#ifndef ENUM_ANONYMOUS_ENUM_______INCLUDE_MOASTTYPES_HH_180_LENGTH
+#define ENUM_ANONYMOUS_ENUM_______INCLUDE_MOASTTYPES_HH_180_LENGTH 3
+#endif
+
+static const char enum_anonymous_enum_______include_moastTypes_hh_180_string_list[ENUM_ANONYMOUS_ENUM_______INCLUDE_MOASTTYPES_HH_180_LENGTH][MAX_ENUM_ANONYMOUS_ENUM_______INCLUDE_MOASTTYPES_HH_180_STRING_LENGTH]= {
+ "MOAST_ROTATE_CCW", /* 0,1 */
+ "MOAST_ROTATE_CW", /* 1,0 */
+ ""};
+
+static const int enum_anonymous_enum_______include_moastTypes_hh_180_int_list[ENUM_ANONYMOUS_ENUM_______INCLUDE_MOASTTYPES_HH_180_LENGTH]= {
+ MOAST_ROTATE_CCW, /* 0,1 */
+ MOAST_ROTATE_CW, /* 1,0 */
+ };
+
+const char *joystickData_enum_anonymous_enum_______include_moastTypes_hh_180_symbol_lookup(long v)
+{
+ switch(v)
+ {
+ case MOAST_ROTATE_CCW: return("MOAST_ROTATE_CCW"); /* 1 */
+ case MOAST_ROTATE_CW: return("MOAST_ROTATE_CW"); /* 0 */
+ default:break;
+ }
+ return("!!UNDEFINED_SYMBOL!!");
+}
+
+static const struct cms_enum_info enum_anonymous_enum_______include_moastTypes_hh_180_info_struct={
+ "anonymous_enum_______include_moastTypes_hh_180",
+ (const char **)enum_anonymous_enum_______include_moastTypes_hh_180_string_list,
+ enum_anonymous_enum_______include_moastTypes_hh_180_int_list,
+ MAX_ENUM_ANONYMOUS_ENUM_______INCLUDE_MOASTTYPES_HH_180_STRING_LENGTH,
+ ENUM_ANONYMOUS_ENUM_______INCLUDE_MOASTTYPES_HH_180_LENGTH,
+ (cms_symbol_lookup_function_t)joystickData_enum_anonymous_enum_______include_moastTypes_hh_180_symbol_lookup
+ };
+
+/*
+Estimated_size JoystickData 24
+Estimated_size MAXIMUM 24
+*/
+/*
+* NML/CMS Format function : joystickData_format
+* Automatically generated by NML CodeGen Java Applet.
+* on Tue Jul 24 11:48:42 EDT 2007
+*/
+int joystickData_format(NMLTYPE type, void *buffer, CMS *cms)
+{
+
+ type = cms->check_type_info(type,buffer,"joystickData",
+ (cms_symbol_lookup_function_t) joystickData_symbol_lookup,
+ (const char **)joystickData_name_list,
+ joystickData_id_list,joystickData_size_list,
+ JOYSTICKDATA_NAME_LIST_LENGTH,
+ MAX_JOYSTICKDATA_NAME_LENGTH);
+
+ switch(type)
+ {
+ case JOYSTICK_DATA_TYPE:
+ ((JoystickData *) buffer)->update(cms);
+ break;
+
+ default:
+ return(0);
+ }
+ return 1;
+}
+
+
+// NML Symbol Lookup Function
+const char *joystickData_symbol_lookup(long type)
+{
+ switch(type)
+ {
+ case JOYSTICK_DATA_TYPE:
+ return "JoystickData";
+ default:
+ return"UNKNOWN";
+ break;
+ }
+ return(NULL);
+}
+
+/*
+* NML/CMS Update function for JoystickData
+* Automatically generated by NML CodeGen Java Applet.
+* on Tue Jul 24 11:48:42 EDT 2007
+*/
+void JoystickData::update(CMS *cms)
+{
+
+ cms->beginClass("JoystickData","NMLmsg");
+ cms->update_with_name("mode",mode);
+
+ cms->endClass("JoystickData","NMLmsg");
+
+}
+
Modified: moast-bth/moast/src/nml/moastNmlGlobal.cc
===================================================================
--- moast-bth/moast/src/nml/moastNmlGlobal.cc 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/src/nml/moastNmlGlobal.cc 2007-08-01 17:41:33 UTC (rev 680)
@@ -15,9 +15,9 @@
functions
\code CVS Status:
- $Author: dr_steveb $
- $Revision: 1.7 $
- $Date: 2006/10/31 09:30:42 $
+ $Author: proctor $
+ $Revision: 1.8 $
+ $Date: 2007/07/24 18:34:12 $
\endcode
\author Fred Proctor
@@ -45,6 +45,7 @@
#ifdef USE_SPQR
#include "nmlMap.hh"
#include "nmlPosition.hh"
+#include "joystickData.hh"
NML *spqrMapBufZerg = NULL;
NML *spqrMapBufP2DX = NULL;
@@ -130,6 +131,8 @@
RCS_CMD_CHANNEL *sectMobPLCfgBuf[NUM_SECT_MOB_PL_BUFFERS] = {NULL};
RCS_STAT_CHANNEL *sectMobPLSetBuf[NUM_SECT_MOB_PL_BUFFERS] = {NULL};
+NML * joystickDataBuf = NULL;
+
int moastZeroNmlBuffers(void)
{
int bufnum;
@@ -259,6 +262,8 @@
sectMobPLCfgBuf[bufnum] = NULL; // TK added 6/10/5
}
+ joystickDataBuf = NULL;
+
return 0;
}
@@ -504,6 +509,11 @@
sectMobPLSetBuf[count] = 0;
}
+ if (NULL != joystickDataBuf) {
+ delete joystickDataBuf;
+ joystickDataBuf = NULL;
+ }
+
return 0;
}
@@ -895,6 +905,15 @@
return 1;
}
+ joystickDataBuf = new NML(joystickData_format, "joystickData",
+ this_process, config_file);
+ if (! joystickDataBuf->valid() &&
+ joystickDataBuf->error_type != NML_INVALID_CONFIGURATION) {
+ delete joystickDataBuf;
+ joystickDataBuf = NULL;
+ return 1;
+ }
+
return 0;
}
@@ -902,6 +921,9 @@
Modification history:
$Log: moastNmlGlobal.cc,v $
+ Revision 1.8 2007/07/24 18:34:12 proctor
+ Added joystickData
+
Revision 1.7 2006/10/31 09:30:42 dr_steveb
Changes for SPQR integration
Modified: moast-bth/moast/src/servo/servoShell.cc
===================================================================
--- moast-bth/moast/src/servo/servoShell.cc 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/src/servo/servoShell.cc 2007-08-01 17:41:33 UTC (rev 680)
@@ -15,9 +15,9 @@
\brief Command line shell to vehicle servo level NML buffers.
\code CVS Status:
- $Author: cj_scrapper $
- $Revision: 1.26 $
- $Date: 2007/06/07 20:16:02 $
+ $Author: proctor $
+ $Revision: 1.27 $
+ $Date: 2007/07/24 18:36:28 $
\endcode
\author Chris Scrapper
@@ -52,6 +52,7 @@
#include "servoMobJA.hh" // servoMobJA_format()
#include "servoMisJA.hh" // servoMisJA_format()
#include "servoSP.hh" // servoSP_format()
+#include "joystickData.hh"
//#include "swKBUtils.hh" // parsing utilities
#define DEFAULT_NML_FILE "../etc/moast.nml"
@@ -67,9 +68,9 @@
RCS_STAT_CHANNEL *statBuf;
RCS_CMD_CHANNEL *cfgBuf;
RCS_STAT_CHANNEL *setBuf;
- RCS_CMD_MSG * cmdMsg;
- RCS_STAT_MSG_V2 * statMsg;
- RCS_STAT_MSG * setMsg;
+ RCS_CMD_MSG *cmdMsg;
+ RCS_STAT_MSG_V2 *statMsg;
+ RCS_STAT_MSG *setMsg;
ServoNodeNmlBuffers();
bool openNML(string nmlCfgFile,
string cmdName,
@@ -112,6 +113,9 @@
ServoMisJACmdShutdown misCmdShutdown;
ServoMisJACmdMove misCmdMove;
+static NML * joystickDataBuf = NULL;
+static JoystickData * joystickDataPtr = NULL;
+
pthread_t scanThr;
#define rcs_admin_state_to_string(s) (s) == ADMIN_INITIALIZED ? "INITIALIZED" : (s) == ADMIN_UNINITIALIZED ? "UNINITIALIZED" : (s) == ADMIN_SHUT_DOWN ? "SHUTDOWN" : "?"
@@ -1107,6 +1111,10 @@
printf(" Status : %s\n", rcs_status_to_string(((ServoMisJAStat *)mis[i].statMsg)->status));
printf(" HeartBeat : %d\n", ((ServoMisJAStat *)mis[i].statMsg)->heartbeat);
}
+ if (joystickDataBuf != NULL && joystickDataPtr != NULL) {
+ joystickDataBuf->read();
+ printf("Joystick: %d\n", joystickDataPtr->mode);
+ }
}
void doAdminView(const vector<string> & toks)
{
@@ -1326,7 +1334,28 @@
break;
}
}
+
+ joystickDataBuf = new NML(joystickData_format,
+ "joystickData",
+ PROCESS_NAME,
+ nmlFile.c_str());
+ if (! joystickDataBuf->valid()) {
+ printf("** ERROR ** Error Initializing Joystick Data Buffer\n");
+ delete joystickDataBuf;
+ joystickDataBuf = NULL;
+ joystickDataPtr = NULL;
+ } else {
+ joystickDataPtr = (JoystickData *) (joystickDataBuf->get_address());
+ }
+
runServoShell();
+
+ if (NULL != joystickDataBuf) {
+ delete joystickDataBuf;
+ }
+ joystickDataBuf = NULL;
+ joystickDataPtr = NULL;
+
return 0;
}
Modified: moast-bth/moast/src/sim/simWare/moastSupSkin.cc
===================================================================
--- moast-bth/moast/src/sim/simWare/moastSupSkin.cc 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/src/sim/simWare/moastSupSkin.cc 2007-08-01 17:41:33 UTC (rev 680)
@@ -36,6 +36,8 @@
{
INIFILE if_d;
const char *strPtr;
+ extern char *SIMWARE_INIFILE_PATH;
+
if (0)
printf ("<MoastSupSkin::openSkin> : Opening inifile %s\n",
SIMWARE_INIFILE_PATH);
Modified: moast-bth/moast/src/sim/simWare/simWare.cc
===================================================================
--- moast-bth/moast/src/sim/simWare/simWare.cc 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/src/sim/simWare/simWare.cc 2007-08-01 17:41:33 UTC (rev 680)
@@ -41,6 +41,7 @@
int platTypeID = 1;
int stPoseID = 0;
int verbosity = 0;
+char *SIMWARE_INIFILE_PATH;
void swShutdown()
{
@@ -196,14 +197,19 @@
}
// set options
+ // inialize inifile path
+ SIMWARE_INIFILE_PATH = DEFINED_SIMWARE_INIFILE_PATH;
while (1)
{
- option = getopt (argc, argv, "p:i:s:v:");
+ option = getopt (argc, argv, "p:i:s:v:f:");
if (option == -1)
break;
switch (option)
{
+ case 'f':
+ SIMWARE_INIFILE_PATH = optarg;
+ break;
case 'p':
if (1 != sscanf (optarg, "%i", &platTypeID))
{
Modified: moast-bth/moast/src/sim/simWare/simWareKB.hh
===================================================================
--- moast-bth/moast/src/sim/simWare/simWareKB.hh 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/src/sim/simWare/simWareKB.hh 2007-08-01 17:41:33 UTC (rev 680)
@@ -14,9 +14,9 @@
\brief
\code CVS Status:
- $Author: cj_scrapper $
- $Revision: 1.11 $
- $Date: 2007/04/04 20:24:34 $
+ $Author: dr_steveb $
+ $Revision: 1.12 $
+ $Date: 2007/07/25 20:01:42 $
\endcode
\author Chris Scrapper
@@ -37,7 +37,7 @@
#include "swKBSen.hh"
#include "swKBMis.hh"
-#define SIMWARE_INIFILE_PATH "../etc/moast.ini"
+#define DEFINED_SIMWARE_INIFILE_PATH "../etc/moast.ini"
class SIMwareKB {
private:
Modified: moast-bth/moast/src/sim/simWare/simpleSimInfSkin.cc
===================================================================
--- moast-bth/moast/src/sim/simWare/simpleSimInfSkin.cc 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/src/sim/simWare/simpleSimInfSkin.cc 2007-08-01 17:41:33 UTC (rev 680)
@@ -14,9 +14,9 @@
\brief Implementation of the simple simulation inferior skin
\code CVS Status:
- $Author: cj_scrapper $
- $Revision: 1.14 $
- $Date: 2007/06/07 20:13:46 $
+ $Author: dr_steveb $
+ $Revision: 1.15 $
+ $Date: 2007/07/25 20:01:42 $
\endcode
\author Fred Proctor
*/
@@ -1312,6 +1312,7 @@
void
SimpleSimInfSkin::configInfSkin(SIMwareKB * k, SIMwareSupSkin * s)
{
+ extern char *SIMWARE_INIFILE_PATH;
SIMwareKBGroundVehicle gv;
int t;
@@ -1489,6 +1490,9 @@
Modification history:
$Log: simpleSimInfSkin.cc,v $
+ Revision 1.15 2007/07/25 20:01:42 dr_steveb
+ Made .ini file a command line option.
+
Revision 1.14 2007/06/07 20:13:46 cj_scrapper
Added the ability to control flippers in SIMware. seServoMob takes MOAST flipskid command and translates it to a skid drive cmd and a flipper cmd. Also added parsing utilities for setting and configuration information for flippers and the aerial robot
Modified: moast-bth/moast/src/sim/simWare/swKBBot.hh
===================================================================
--- moast-bth/moast/src/sim/simWare/swKBBot.hh 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/src/sim/simWare/swKBBot.hh 2007-08-01 17:41:33 UTC (rev 680)
@@ -15,8 +15,8 @@
\brief
\code CVS Status:
$Author: cj_scrapper $
- $Revision: 1.11 $
- $Date: 2007/06/07 20:13:46 $
+ $Revision: 1.12 $
+ $Date: 2007/07/18 15:27:17 $
\endcode
\author Chris Scrapper
@@ -90,7 +90,7 @@
double bLvl;
//! Fuel Level
double fLevel;
- //! max speed rads/s
+ //! Timestamp
double time;
public:
//! Constructor
Modified: moast-bth/moast/src/sim/simWare/swKBWorld.cc
===================================================================
--- moast-bth/moast/src/sim/simWare/swKBWorld.cc 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/src/sim/simWare/swKBWorld.cc 2007-08-01 17:41:33 UTC (rev 680)
@@ -31,6 +31,7 @@
stringstream tmpS;
PM_CARTESIAN cart;
PM_RPY rpy;
+ extern char *SIMWARE_INIFILE_PATH;
wName=worldName;
Modified: moast-bth/moast/src/sim/simWare/trafficSim.cc
===================================================================
--- moast-bth/moast/src/sim/simWare/trafficSim.cc 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/src/sim/simWare/trafficSim.cc 2007-08-01 17:41:33 UTC (rev 680)
@@ -31,6 +31,7 @@
// defines
///////////////////////////////////////////////////////
#define DEFAULT_INI_FILE "../etc/traffic.ini"
+char *SIMWARE_INIFILE_PATH;
////////////////////////////////////////////////////////
// structures
@@ -150,7 +151,8 @@
char *ini_file_env;
char ini_file_default[] = DEFAULT_INI_FILE;
TrafficVehicleSpawn spawnPoint;
-
+
+ SIMWARE_INIFILE_PATH = DEFINED_SIMWARE_INIFILE_PATH;
// set up signals
if( signal (SIGINT, sigHandler) == SIG_ERR )
{
Modified: moast-bth/moast/src/sim/simWare/trafficSupSkin.cc
===================================================================
--- moast-bth/moast/src/sim/simWare/trafficSupSkin.cc 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/src/sim/simWare/trafficSupSkin.cc 2007-08-01 17:41:33 UTC (rev 680)
@@ -44,6 +44,7 @@
bool TrafficSupSkin::openSkin()
{
+ extern char *SIMWARE_INIFILE_PATH;
double vmax, amax, wmax, alphamax, vCutoffAngle, wCutoffAngle;
INIFILE if_d;
const char *strPtr;
Modified: moast-bth/moast/src/sim/simWare/usarInfSkin.cc
===================================================================
--- moast-bth/moast/src/sim/simWare/usarInfSkin.cc 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/src/sim/simWare/usarInfSkin.cc 2007-08-01 17:41:33 UTC (rev 680)
@@ -81,6 +81,7 @@
struct sockaddr_in local_addr;
struct hostent *ent;
struct in_addr *in_a;
+ extern char *SIMWARE_INIFILE_PATH;
pthread_attr_t parAttr;
int retval;
@@ -173,14 +174,19 @@
/* connect to the server */
if (0)
printf ("<USARSimInfSkin::openSkin> : Connecting to Server\n");
- tmp_i =
- connect (socket_fd, (struct sockaddr *) &server_addr,
- sizeof (struct sockaddr_in));
- if (tmp_i == -1)
+ tmp_i = -1;
+ while(tmp_i == -1)
{
- printf
- ("**Error : Connecting to Server : Make sure server is running\n");
- return false;
+ tmp_i =
+ connect (socket_fd, (struct sockaddr *) &server_addr,
+ sizeof (struct sockaddr_in));
+ if (tmp_i == -1)
+ {
+ printf
+ ("**Waiting : Connecting to Server : Make sure server is running\n");
+ // return false;
+ usleep(1000);
+ }
}
if (0)
printf ("<USARSimInfSkin::openSkin> : Socket Open\n");
@@ -509,6 +515,7 @@
INIFILE if_d;
const char *strPtr;
double traceParam=-1;
+ extern char *SIMWARE_INIFILE_PATH;
// Get platform type name from inifile
//open inifile
Modified: moast-bth/moast/src/tools/imageView/interface.c
===================================================================
--- moast-bth/moast/src/tools/imageView/interface.c 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/src/tools/imageView/interface.c 2007-08-01 17:41:33 UTC (rev 680)
@@ -8,7 +8,7 @@
#include <sys/types.h>
#include <sys/stat.h>
-#include <unistd.h>
+#include <getopt.h>
#include <string.h>
#include <stdio.h>
Modified: moast-bth/moast/src/tools/imageView/main.c
===================================================================
--- moast-bth/moast/src/tools/imageView/main.c 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/src/tools/imageView/main.c 2007-08-01 17:41:33 UTC (rev 680)
@@ -10,7 +10,7 @@
#include <stdio.h> /* EOF, printf */
#include <stdlib.h>
#include <string.h>
-#include <unistd.h>
+#include <getopt.h>
#include <gtk/gtk.h>
#include "interface.h"
Modified: moast-bth/moast/src/tools/imageView/support.c
===================================================================
--- moast-bth/moast/src/tools/imageView/support.c 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/src/tools/imageView/support.c 2007-08-01 17:41:33 UTC (rev 680)
@@ -8,7 +8,7 @@
#include <sys/types.h>
#include <sys/stat.h>
-#include <unistd.h>
+#include <getopt.h>
#include <string.h>
#include <stdio.h>
Modified: moast-bth/moast/src/tools/robojoy.cc
===================================================================
--- moast-bth/moast/src/tools/robojoy.cc 2007-08-01 15:51:16 UTC (rev 679)
+++ moast-bth/moast/src/tools/robojoy.cc 2007-08-01 17:41:33 UTC (rev 680)
@@ -16,8 +16,8 @@
\code CVS Status:
$Author: proctor $
- $Revision: 1.19 $
- $Date: 2007/06/28 13:21:44 $
+ $Revision: 1.22 $
+ $Date: 2007/07/26 17:56:14 $
\endcode
\author Fred Proctor
@@ -36,8 +36,7 @@
#include "moastTypes.hh"
#include "servoMobJA.hh"
#include "servoMisJA.hh"
-#include "servoSP.hh"
-#include "navDataExt.hh"
+#include "joystickData.hh"
#ifdef HAVE_GETOPT_H
#include <getopt.h>
#else
@@ -48,7 +47,6 @@
#include "gomath.h"
}
-#undef USE_NAV
#undef VERBOSE
/*
define CLAMP_FLIPPERS if you want the commanded positions of the
@@ -69,32 +67,29 @@
// Serial number to servo level
static int servoMobSerial = WHONUM;
-static int servoSPSerial = WHONUM;
static int servoMisSerial = WHONUM;
// just add two each time and stay odd
#define NEXTS(s) (s += 2)
// NML Channels
-static RCS_CMD_CHANNEL *servoMobJACmdNml;
-static RCS_STAT_CHANNEL *servoMobJAStatNml;
-static RCS_CMD_CHANNEL *servoMobJACfgNml;
-static RCS_STAT_CHANNEL *servoMobJASetNml;
+
+static RCS_CMD_CHANNEL *servoMobJACmdNml = NULL;
+static RCS_STAT_CHANNEL *servoMobJAStatNml = NULL;
+static RCS_STAT_CHANNEL *servoMobJASetNml = NULL;
+
static RCS_CMD_CHANNEL * servoMisJACmdNml = NULL;
static RCS_STAT_CHANNEL * servoMisJAStatNml = NULL;
-static RCS_CMD_CHANNEL * servoMisJACfgNml = NULL;
static RCS_STAT_CHANNEL * servoMisJASetNml = NULL;
-static RCS_CMD_CHANNEL * servoSPCmdNml = NULL;
-static RCS_STAT_CHANNEL * servoSPStatNml = NULL;
-static RCS_STAT_MSG * setMsg = NULL; // Used to read robot's settings such as
-static RCS_STAT_CHANNEL * setBuf = NULL; // robot type, maximum speed, etc...
-static NML * navDataExtNml = NULL;
+static NML * joystickDataNml = NULL;
+static JoystickData joystickData;
+
static ServoMisJAStat * servoMisJAStatPtr = NULL;
static ServoMisJASet * servoMisJASetPtr = NULL;
+
static ServoMobJAStat * servoMobJAStatPtr = NULL;
static ServoMobJASet * servoMobJASetPtr = NULL;
-static NavDataExt * navDataExtPtr = NULL;
static ServoMobJACmdSkid skidMsg;
static ServoMobJACmdFlipSkid flipSkidMsg;
@@ -137,121 +132,100 @@
char *configNml = getenv("CONFIG_NML");
char chanName[MOAST_NML_BUFFER_NAME_LEN];
char myName[MOAST_NML_BUFFER_NAME_LEN];
+ bool retval = true;
sprintf(myName, "%s", MY_NAME_BASE);
- if (!configNml) {
- fprintf(stderr, "Please set CONFIG_NML environment variable.\n");
- return (false);
+ if (NULL == configNml) {
+ configNml = "../etc/moast.nml";
}
- sprintf(chanName, "%s%d", SERVO_MOB_JA_CMD_NAME, vehID);
- servoMobJACmdNml = new RCS_CMD_CHANNEL(servoMobJA_format, chanName,
- myName, configNml);
- if (NULL == servoMobJACmdNml || !servoMobJACmdNml->valid()) {
- rcs_print_error("%s: Error - Can't open %s buffer\n", myName, chanName);
- return (false);
+ if (NULL == servoMobJACmdNml) {
+ sprintf(chanName, "%s%d", SERVO_MOB_JA_CMD_NAME, vehID);
+ servoMobJACmdNml = new RCS_CMD_CHANNEL(servoMobJA_format, chanName, myName, configNml);
+ if (! servoMobJACmdNml->valid()) {
+ delete servoMobJACmdNml;
+ servoMobJACmdNml = NULL;
+ printf("can't open %s\n", chanName);
+ retval = false;
+ }
}
- sprintf(chanName, "%s%d", SERVO_MOB_JA_STAT_NAME, vehID);
- servoMobJAStatNml = new RCS_STAT_CHANNEL(servoMobJA_format, chanName,
- myName, configNml);
- if (NULL == servoMobJAStatNml || !servoMobJAStatNml->valid()) {
- rcs_print_error("%s: Error - Can't open %s buffer\n", myName, chanName);
- return (false);
+ if (NULL == servoMobJAStatNml) {
+ sprintf(chanName, "%s%d", SERVO_MOB_JA_STAT_NAME, vehID);
+ servoMobJAStatNml = new RCS_STAT_CHANNEL(servoMobJA_format, chanName, myName, configNml);
+ if (! servoMobJAStatNml->valid()) {
+ delete servoMobJAStatNml;
+ servoMobJAStatNml = NULL;
+ printf("can't open %s\n", chanName);
+ retval = false;
+ } else {
+ servoMobJAStatPtr = (ServoMobJAStat *) (servoMobJAStatNml->get_address());
+ }
}
- servoMobJAStatPtr = (ServoMobJAStat *) (servoMobJAStatNml->get_address());
- sprintf(chanName, "%s%d", SERVO_MOB_JA_SET_NAME, vehID);
- servoMobJASetNml = new RCS_STAT_CHANNEL(servoMobJA_format, chanName,
- myName, configNml);
- if (NULL == servoMobJASetNml || !servoMobJASetNml->valid()) {
- rcs_print_error("%s: Error - Can't open %s buffer\n", myName, chanName);
- return (false);
+ if (NULL == servoMobJASetNml) {
+ sprintf(chanName, "%s%d", SERVO_MOB_JA_SET_NAME, vehID);
+ servoMobJASetNml = new RCS_STAT_CHANNEL(servoMobJA_format, chanName, myName, configNml);
+ if (! servoMobJASetNml->valid()) {
+ delete servoMobJASetNml;
+ servoMobJASetNml = NULL;
+ printf("can't open %s\n", chanName);
+ retval = false;
+ } else {
+ servoMobJASetPtr = (ServoMobJASet *) (servoMobJASetNml->get_address());
+ }
}
- servoMobJASetPtr = (ServoMobJASet *) (servoMobJASetNml->get_address());
- sprintf(chanName, "%s%d", SERVO_MOB_JA_CFG_NAME, vehID);
- servoMobJACfgNml = new RCS_CMD_CHANNEL(servoMobJA_format, chanName,
- myName, configNml);
- if (NULL == servoMobJACfgNml || !servoMobJACfgNml->valid()) {
- rcs_print_error("%s: Error - Can't open %s buffer\n", myName, chanName);
- return (false);
+ if (NULL == servoMisJACmdNml) {
+ sprintf(chanName, "%s%d_%d", SERVO_MIS_JA_CMD_NAME, vehID, compID);
+ servoMisJACmdNml = new RCS_CMD_CHANNEL(servoMisJA_format, chanName, myName, configNml);
+ if (! servoMisJACmdNml->valid()) {
+ delete servoMisJACmdNml;
+ servoMisJACmdNml = NULL;
+ printf("can't open %s\n", chanName);
+ retval = false;
+ }
}
- sprintf(chanName, "%s%d_%d", SERVO_MIS_JA_CMD_NAME, vehID, compID);
- servoMisJACmdNml = new RCS_CMD_CHANNEL(servoMisJA_format, chanName,
- myName, configNml);
- if (NULL == servoMisJACmdNml || !servoMisJACmdNml->valid()) {
- rcs_print_error("%s: Error - Can't open %s buffer\n", myName, chanName);
- return (false);
+ if (NULL == servoMisJAStatNml) {
+ sprintf(chanName, "%s%d_%d", SERVO_MIS_JA_STAT_NAME, vehID, compID);
+ servoMisJAStatNml = new RCS_STAT_CHANNEL(servoMisJA_format, chanName, myName, configNml);
+ if (! servoMisJAStatNml->valid()) {
+ delete servoMisJAStatNml;
+ servoMisJAStatNml = NULL;
+ printf("can't open %s\n", chanName);
+ retval = false;
+ } else {
+ servoMisJAStatPtr = (ServoMisJAStat *) (servoMisJAStatNml->get_address());
+ }
}
- sprintf(chanName, "%s%d_%d", SERVO_MIS_JA_STAT_NAME, vehID, compID);
- servoMisJAStatNml = new RCS_STAT_CHANNEL(servoMisJA_format, chanName,
- myName, configNml);
- if (NULL == servoMisJAStatNml || !servoMisJAStatNml->valid()) {
- rcs_print_error("%s: Error - Can't open %s buffer\n", myName, chanName);
- return (false);
+ if (NULL == servoMisJASetNml) {
+ sprintf(chanName, "%s%d_%d", SERVO_MIS_JA_SET_NAME, vehID, compID);
+ servoMisJASetNml = new RCS_STAT_CHANNEL(servoMisJA_format, chanName, myName, configNml);
+ if (! servoMisJASetNml->valid()) {
+ delete servoMisJASetNml;
+ servoMisJASetNml = NULL;
+ printf("can't open %s\n", chanName);
+ retval = false;
+ } else {
+ servoMisJASetPtr = (ServoMisJASet *) (servoMisJASetNml->get_address());
+ }
}
- servoMisJAStatPtr = (ServoMisJAStat *) (servoMisJAStatNml->get_address());
- sprintf(chanName, "%s%d_%d", SERVO_MIS_JA_CFG_NAME, vehID, compID);
- servoMisJACfgNml = new RCS_CMD_CHANNEL(servoMisJA_format, chanName,
- myName, configNml);
- if (NULL == servoMisJACfgNml || !servoMisJACfgNml->valid()) {
- rcs_print_error("%s: Error - Can't open %s buffer\n", myName, chanName);
- return (false);
+ if (NULL == joystickDataNml) {
+ sprintf(chanName, "joystickData");
+ joystickDataNml = new NML(joystickData_format, chanName, myName, configNml);
+ if (! joystickDataNml->valid()) {
+ delete joystickDataNml;
+ joystickDataNml = NULL;
+ printf("can't open %s\n", chanName);
+ retval = false;
+ }
}
- sprintf(chanName, "%s%d_%d", SERVO_MIS_JA_SET_NAME, vehID, compID);
- servoMisJASetNml = new RCS_STAT_CHANNEL(servoMisJA_format, chanName,
- myName, configNml);
- if (NULL == servoMisJASetNml || !servoMisJASetNml->valid()) {
- rcs_print_error("%s: Error - Can't open %s buffer\n", myName, chanName);
- return (false);
- }
- servoMisJASetPtr = (ServoMisJASet *) (servoMisJASetNml->get_address());
-
- sprintf(chanName, "%s%d", SERVO_SP_CMD_NAME, vehID);
- servoSPCmdNml = new RCS_CMD_CHANNEL(servoSP_format, chanName,
- myName, configNml);
- if (NULL == servoSPCmdNml || !servoSPCmdNml->valid()) {
- rcs_print_error("%s: Error - Can't open %s buffer\n", myName, chanName);
- return (false);
- }
-
- sprintf(chanName, "%s%d", SERVO_SP_STAT_NAME, vehID);
- servoSPStatNml = new RCS_STAT_CHANNEL(servoSP_format, chanName,
- myName, configNml);
- if (NULL == servoSPStatNml || !servoSPStatNml->valid()) {
- rcs_print_error("%s: Error - Can't open %s buffer\n", myName, chanName);
- return (false);
- }
-
- sprintf(chanName, "%s%d", SERVO_MOB_JA_SET_NAME, vehID);
- setBuf = new RCS_STAT_CHANNEL(servoMobJA_format, chanName,
- myName, configNml);
- if (NULL == setBuf || !setBuf->valid()) {
- rcs_print_error("%s: Error - Can't open %s buffer\n", myName, chanName);
- delete setBuf;
- setBuf = NULL;
- return (false);
- }
- setMsg= setBuf->get_address();
-
-#ifdef USE_NAV
- sprintf(chanName, "%s%d", NAV_DATA_EXT_NAME, vehID);
- navDataExtNml = new NML(navDataExt_format, chanName,
- myName, configNml);
- if (NULL == navDataExtNml || !navDataExtNml->valid()) {
- rcs_print_error("%s: Error - Can't open %s buffer\n", myName, chanName);
- return (false);
- }
- navDataExtPtr = (NavDataExt *) (navDataExtNml->get_address());
-#endif
-
- return (true);
+ return retval;
}
/*!
@@ -302,43 +276,33 @@
}
servoMisJACmdNml->write(misCmdMove);
+ if (servoMobJACmdNml != NULL)
+ delete servoMobJACmdNml;
servoMobJACmdNml = NULL;
+
if (servoMobJAStatNml != NULL)
delete servoMobJAStatNml;
servoMobJAStatNml = NULL;
+
if (servoMobJASetNml != NULL)
delete servoMobJASetNml;
servoMobJASetNml = NULL;
- if (servoMobJACfgNml != NULL)
- delete servoMobJACfgNml;
- servoMobJACfgNml = NULL;
- if (servoSPCmdNml != NULL)
- delete servoSPCmdNml;
+
if (servoMisJACmdNml != NULL)
delete servoMisJACmdNml;
servoMisJACmdNml = NULL;
+
if (servoMisJAStatNml != NULL)
delete servoMisJAStatNml;
servoMisJAStatNml = NULL;
+
if (servoMisJASetNml != NULL)
delete servoMisJASetNml;
servoMisJASetNml = NULL;
- if (servoMisJACfgNml != NULL)
- delete servoMisJACfgNml;
- servoMisJACfgNml = NULL;
- if (servoSPCmdNml != NULL)
- delete servoSPCmdNml;
- servoSPCmdNml = NULL;
- if (servoSPStatNml != NULL)
- delete servoSPStatNml;
- servoSPStatNml = NULL;
- if (setBuf != NULL)
- delete setBuf;
- setBuf = NULL;
- setMsg = NULL;
- if (navDataExtNml != NULL)
- delete navDataExtNml;
- navDataExtNml = NULL;
+
+ if (joystickDataNml != NULL)
+ delete joystickDataNml;
+ joystickDataNml = NULL;
}
enum {JOINTS = 8};
@@ -810,9 +774,6 @@
if (doInit) {
type = servoMisJAStatNml->read();
-#ifdef USE_NAV
- type = navDataExtNml->read();
-#endif
if (doMob) {
switch (mode) {
case 0:
@@ -1049,16 +1010,11 @@
}
}
}
-#ifdef USE_NAV
- printf("%f %f %f %f %f %f %f ",
- (double) navDataExtPtr->time,
- (double) navDataExtPtr->tranAbs.x,
- (double) navDataExtPtr->tranAbs.y,
- (double) navDataExtPtr->tranAbs.z,
- (double) navDataExtPtr->rpyAbs.r,
- (double) navDataExtPtr->rpyAbs.p,
- (double) navDataExtPtr->rpyAbs.y);
-#endif
+ joystickData.mode =
+ doMob ? JOYSTICK_MODE_MOBILITY :
+ coordMode == COORD_JOINT ? JOYSTICK_MODE_JOINT :
+ toolMode ? JOYSTICK_MODE_TOOL : JOYSTICK_MODE_WORLD;
+ joystickDataNml->write(joystickData);
} // if (doInit)
} // for (done)
}
@@ -1068,11 +1024,8 @@
int joystickNum;
double end;
int gotIt;
- ServoMisJAStat * servoMisJAStat;
- ServoSPStat * servoSPStat;
+ ServoMobJACmdInit servoMobInit;
ServoMisJACmdInit servoMisInit;
- ServoSPCmdInit servoSPInit;
- ServoSPCmdGo servoSPGo;
char c;
int i;
SDL_Joystick *joystick;
@@ -1195,76 +1148,53 @@
}
if (doInit) {
-
- if (!openNML(vehID)) {
- fprintf(stderr, "Error opening nml buffers\n");
- return 1;
+ while (! openNML(vehID)) {
+ printf("waiting for NML...\n");
+ esleep(1.0);
}
- servoMisJAStat = (ServoMisJAStat *) servoMisJAStatNml->get_address();
- servoSPStat = (ServoSPStat *) servoSPStatNml->get_address();
- // send init command to servo mis
- servoMisInit.serial_number = NEXTS(servoMisSerial);
- servoMisJACmdNml->write(servoMisInit);
+ // send init command to servo mob
+ servoMobInit.serial_number = NEXTS(servoMobSerial);
+ servoMobJACmdNml->write(servoMobInit);
for (end = etime() + 3.0, gotIt = 0; etime() < end; esleep(0.1)) {
- servoMisJAStatNml->read();
- if ( servoMisJAStat->echo_serial_number == servoMisSerial &&
- servoMisJAStat->status == RCS_DONE) {
+ servoMobJAStatNml->read();
+ if ( servoMobJAStatPtr->echo_serial_number == servoMobSerial &&
+ servoMobJAStatPtr->status == RCS_DONE) {
gotIt = 1;
}
}
if (!gotIt) {
- fprintf(stderr, "can't initialize servo mis\n");
+ fprintf(stderr, "can't initialize servo mob\n");
closeNML();
return 1;
}
- // send init command to servo sp
- servoSPInit.serial_number = NEXTS(servoSPSerial);
- servoSPCmdNml->write(servoSPInit);
+ // send init command to servo mis
+ servoMisInit.serial_number = NEXTS(servoMisSerial);
+ servoMisJACmdNml->write(servoMisInit);
for (end = etime() + 3.0, gotIt = 0; etime() < end; esleep(0.1)) {
- servoSPStatNml->read();
- if (servoSPStat->echo_serial_number == servoSPSerial &&
- servoSPStat->status == RCS_DONE) {
+ servoMisJAStatNml->read();
+ if ( servoMisJAStatPtr->echo_serial_number == servoMisSerial &&
+ servoMisJAStatPtr->status == RCS_DONE) {
gotIt = 1;
}
}
if (!gotIt) {
- fprintf(stderr, "can't initialize servo sp\n");
+ fprintf(stderr, "can't initialize servo mis\n");
closeNML();
return 1;
}
- // send go command to servo sp
- servoSPGo.serial_number = NEXTS(servoSPSerial);
- servoSPCmdNml->write(servoSPGo);
- for (end = etime() + 3.0, gotIt = 0; etime() < end; esleep(0.1)) {
- servoSPStatNml->read();
- if (servoSPStat->echo_serial_number == servoSPSerial &&
- servoSPStat->status == RCS_EXEC) {
- gotIt = 1;
- }
- }
- if (!gotIt) {
- fprintf(stderr, "can't go-ify servo sp\n");
- closeNML();
- return 1;
- }
- /* Here, we read the setting's channel and set up the mode of operation for the joystick
- * as well as the scale factors when applicable
- */
- setBuf->read();
-
- switch(((ServoMobJASet *)setMsg)->steerType)
- {
+ servoMobJASetNml->read();
+ switch (((ServoMobJASet *)servoMobJASetPtr)->steerType) {
case MOAST_SKID_STEER_TYPE:
mode = 1;
if(scale[1]==0) // If scale factor 1 has not been set
- scale[1] = -((ServoMobJASetGrdVeh *)setMsg)->maxWheelRot; // Scale for the left wheel spin speed
+ scale[1] = -((ServoMobJASetGrdVeh *)servoMobJASetPtr)->maxWheelRot; // Scale for the left wheel spin speed
if(scale[3]==0) // If scale factor 3 has not been set
- scale[3] = -((ServoMobJASetGrdVeh *)setMsg)->maxWheelRot; // Scale for the right wheel spin speed
+ scale[3] = -((ServoMobJASetGrdVeh *)servoMobJASetPtr)->maxWheelRot; // Scale for the right wheel spin speed
break;
@@ -1272,13 +1202,13 @@
mode = 2;
if(scale[1]==0) // If scale factor 1 has not been set
- scale[1] = -((ServoMobJASetGrdVeh *)setMsg)->maxWheelRot; // Scale for the wheel spin speed
+ scale[1] = -((ServoMobJASetGrdVeh *)servoMobJASetPtr)->maxWheelRot; // Scale for the wheel spin speed
if(scale[2]==0) // If scale factor 2 has not been set
- scale[2] = -((ServoMobJASetGrdVeh *)setMsg)->maxSteerAngle; // Scale for the front steer angle
+ scale[2] = -((ServoMobJASetGrdVeh *)servoMobJASetPtr)->maxSteerAngle; // Scale for the front steer angle
if(scale[3]==0) // If scale factor 3 has not been set
- scale[3] = -((ServoMobJASetGrdVeh *)setMsg)->maxCrabAngle; // Scale for the rear steer angle
+ scale[3] = -((ServoMobJASetGrdVeh *)servoMobJASetPtr)->maxCrabAngle; // Scale for the rear steer angle
break;
@@ -1286,16 +1216,16 @@
mode = 4;
if(scale[1]==0)
- scale[1] = -((ServoMobJASetAirBot *)setMsg)->maxVertVel; // Scale for Altitude (up/down) Velocity
+ scale[1] = -((ServoMobJASetAirBot *)servoMobJASetPtr)->maxVertVel; // Scale for Altitude (up/down) Velocity
if(scale[3]==0)
- scale[3] = -((ServoMobJASetAirBot *)setMsg)->maxLinVel; // Scale for Linear (forward/backward) Velocity
+ scale[3] = -((ServoMobJASetAirBot *)servoMobJASetPtr)->maxLinVel; // Scale for Linear (forward/backward) Velocity
if(scale[2]==0)
- scale[2] = ((ServoMobJASetAirBot *)setMsg)->maxLatVel; // Scale for Lateral (left/right) Velocity
+ scale[2] = ((ServoMobJASetAirBot *)servoMobJASetPtr)->maxLatVel; // Scale for Lateral (left/right) Velocity
if(scale[0]==0)
- scale[0] = ((ServoMobJASetAirBot *)setMsg)->maxRotVel; // Scale for Rotational Velocity
+ scale[0] = ((ServoMobJASetAirBot *)servoMobJASetPtr)->maxRotVel; // Scale for Rotational Velocity
break;
default:
@@ -1304,8 +1234,7 @@
if(scale[2]==0) scale[2] = 1;
if(scale[3]==0) scale[3] = -1;
break;
- }
-
+ }
} // doInit
/* Initialize SDL (Note: video is required to start event loop) */
@@ -1340,6 +1269,16 @@
Modification history:
$Log: robojoy.cc,v $
+ Revision 1.22 2007/07/26 17:56:14 proctor
+ Took out nav data and servo SP, and added both mob and mis init, and added
+ NML wait loop so you can bring this up before the controller
+
+ Revision 1.21 2007/07/24 18:35:43 proctor
+ Added joystickData NML buffer to robojoy.cc
+
+ Revision 1.20 2007/07/18 18:53:07 proctor
+ Made ../etc/moast.nml the default NML file
+
Revision 1.19 2007/06/28 13:21:44 proctor
Changed usleep to esleep, switching from Unix-specific to the one in RCS lib
More information about the TeamTalk-developers
mailing list