[TeamTalk 326]: [862] trunk/moast-bth: 1) Import from moast.
tk@edam.speech.cs.cmu.edu
tk at edam.speech.cs.cmu.edu
Mon Oct 29 16:24:46 EDT 2007
An HTML attachment was scrubbed...
URL: http://mailman.srv.cs.cmu.edu/pipermail/teamtalk-developers/attachments/20071029/6a4cd4a2/attachment-0001.html
-------------- next part --------------
Modified: trunk/moast-bth/TeamTalkSimulator/TeamTalkSimulator.cc
===================================================================
--- trunk/moast-bth/TeamTalkSimulator/TeamTalkSimulator.cc 2007-10-29 18:21:44 UTC (rev 861)
+++ trunk/moast-bth/TeamTalkSimulator/TeamTalkSimulator.cc 2007-10-29 20:24:46 UTC (rev 862)
@@ -75,7 +75,7 @@
void sendFullMap(Boeing::MapServer *mserver) {
readMapFile();
- Boeing::MsgMap::MsgMapFactory(Boeing::MAP_RLE,
+ Boeing::MsgMap::MsgMapFactory(Boeing::MAP_X_MAJOR,
msgMap, currentMap, mapWidth*mapHeight,
0, 0, mapWidth, mapHeight);
//debug << "raw: " << msgMap->map[0]
@@ -86,7 +86,7 @@
void sendDiffMap(Boeing::MapServer *mserver) {
readMapFile();
- Boeing::MsgMap::MsgMapFactory(Boeing::MAP_RLE,
+ Boeing::MsgMap::MsgMapFactory(Boeing::MAP_X_MAJOR,
msgMap, diffMap, mapWidth*mapHeight,
0, 0, mapWidth, mapHeight);
//debug << "raw: " << msgMap->map[0]
Modified: trunk/moast-bth/moast/bin/Makefile.am
===================================================================
--- trunk/moast-bth/moast/bin/Makefile.am 2007-10-29 18:21:44 UTC (rev 861)
+++ trunk/moast-bth/moast/bin/Makefile.am 2007-10-29 20:24:46 UTC (rev 862)
@@ -1,4 +1,4 @@
-bin_PROGRAMS = moastNmlSvr simWare servoShell primSPMain primMisMain primMobMain primShell segmentTest polyIntersect exampleUtmConvert gpsToSerial amSPMain amMisMain amMobMain amShell vehSPMain vehMobPLMain vehShell sectShell sectMobPL exampleDijkTabs exampleDijkHeap exampleDijkHeapN exampleAStarHeapA exampleAStarHeapB exampleAStarHeapNA exampleAStarHeapNB exampleScrollMap spPlay moastErrorLog nmlPrint inifind exampleVisGraph vehDrone nmlConvert spqrConvert slamStub trafficShell linkMon linkMonShell
+bin_PROGRAMS = moastNmlSvr simWare servoShell primSPMain primMisMain primMobMain primShell segmentTest polyIntersect exampleUtmConvert gpsToSerial amSPMain amMisMain amMobMain amShell vehSPMain vehMobPLMain vehShell sectShell sectMobPL exampleDijkTabs exampleDijkHeap exampleDijkHeapN exampleAStarHeapA exampleAStarHeapB exampleAStarHeapNA exampleAStarHeapNB exampleScrollMap spPlay moastErrorLog nmlPrint inifind exampleVisGraph vehDrone nmlConvert spqrConvert slamStub trafficShell
INCLUDES = -I$(RCSLIB_DIR)/include
Modified: trunk/moast-bth/moast/bin/run
===================================================================
--- trunk/moast-bth/moast/bin/run 2007-10-29 18:21:44 UTC (rev 861)
+++ trunk/moast-bth/moast/bin/run 2007-10-29 20:24:46 UTC (rev 862)
@@ -3,7 +3,7 @@
# USERS EDIT THIS AREA
#
# set up to use ackerman steering
-#export USE_ACKERMAN=2
+export USE_ACKERMAN=1
# set up to use the submarine
#export USE_NAUTIC=1.0
@@ -15,7 +15,7 @@
VEH=no
# if VEH=no, set AM=yes to run AM, prim, and servo, else AM=no
-AM=no
+AM=yes
# if AM=no, set PRIM=yes to run prim, and servo, else PRIM=no
PRIM=yes
Modified: trunk/moast-bth/moast/bin/runJoy
===================================================================
--- trunk/moast-bth/moast/bin/runJoy 2007-10-29 18:21:44 UTC (rev 861)
+++ trunk/moast-bth/moast/bin/runJoy 2007-10-29 20:24:46 UTC (rev 862)
@@ -16,11 +16,13 @@
#Zerg
#PLATFORM=7; JOYMODE=1
#Talon
-PLATFORM=8; JOYMODE=1
+#PLATFORM=8; JOYMODE=1
#TeleMax
#PLATFORM=9; JOYMODE=1
#AirRobot
#PLATFORM=10; JOYMODE=4
+#Unit Loader
+PLATFORM=12; JOYMODE=2
#done with user area
if [ $# = 0 ]; then
@@ -161,8 +163,8 @@
#./joySDL -d1 -x3 -y3 -z3 -w3 -m$JOYMODE&
# Settings for the AirRobot
-./robojoy -d1 -x1.5708 -y-5 -z5 -w-5 &
-
+#./robojoy -d1 -x1.5708 -y-5 -z5 -w-5 &
+./robojoy &
# Settings for other robots (robojoy will adjust scale factors automatically)
#./robojoy -d1&
pid11=$!
Modified: trunk/moast-bth/moast/etc/moast.ini
===================================================================
--- trunk/moast-bth/moast/etc/moast.ini 2007-10-29 18:21:44 UTC (rev 861)
+++ trunk/moast-bth/moast/etc/moast.ini 2007-10-29 20:24:46 UTC (rev 862)
@@ -75,13 +75,21 @@
; V_CUTOFF_ANGLE = 10.0
; W_CUTOFF_ANGLE = 45.0
+; unit loader
+ACKERMAN_MAX_TRAN_VEL = 2.5
+ACKERMAN_MAX_TRAN_ACC = 1
+ACKERMAN_MAX_ROT_VEL = 60
+ACKERMAN_MAX_ROT_ACC = 145
+ACKERMAN_V_CUTOFF_ANGLE = 150
+ACKERMAN_W_CUTOFF_ANGLE = 10
+
; 60 mph = 26.9224 m/s
-;MAX_TRAN_VEL = 100
-;MAX_TRAN_ACC = 2
-;MAX_ROT_VEL = 60
-;MAX_ROT_ACC = 145
-;V_CUTOFF_ANGLE = 150
-;W_CUTOFF_ANGLE = 10
+;ACKERMAN_MAX_TRAN_VEL = 100
+;ACKERMAN_MAX_TRAN_ACC = 2
+;ACKERMAN_MAX_ROT_VEL = 60
+;ACKERMAN_MAX_ROT_ACC = 145
+;ACKERMAN_V_CUTOFF_ANGLE = 150
+;ACKERMAN_W_CUTOFF_ANGLE = 10
; USARSim robot driving
MAX_TRAN_VEL = 2.5
@@ -138,7 +146,9 @@
PLATFORM_TYPE_8=Talon
PLATFORM_TYPE_9=TeleMax
PLATFORM_TYPE_10=AirRobot
-PLATFORM_TYPE_11=SegwayRMP
+PLATFORM_TYPE_11=Sedan
+PLATFORM_TYPE_12=UnitLoader
+PLATFORM_TYPE_13=SegwayRMP
; if < 0 then turn off trace function
TRACE_FUNCTION=-0.1
@@ -251,6 +261,20 @@
UTM_START_POSE_2 = 239.9 129 -0.2 0 0 0
UTM_START_POSE_3 = 255.5 130 -0.2 0 0 0
+[DM-Factory]
+UNREAL_UTM_OFFSET = 0 0 0
+RESOLUTION = 0.0196
+UTM_LETTER = N
+UTM_ZONE = 18
+UTM_START_POSE_COUNT = 7
+UTM_START_POSE_1 = 12.34 6.3 1. 0 0 0
+UTM_START_POSE_2 = 12.61 -2.62 1. 0 0 0
+UTM_START_POSE_3 = 17.36 6.41 1. 0 0 0
+UTM_START_POSE_4 = 17.9 -2.71 1. 0 0 0
+UTM_START_POSE_5 = 23.17 6.28 1. 0 0 0
+UTM_START_POSE_6 = 23.1 2.10 1. 0 0 0
+UTM_START_POSE_7 = 23.22 -2.86 1. 0 0 0
+
[DM-NISTBuild_V3]
UNREAL_UTM_OFFSET = 0 0 0
RESOLUTION = 0.0196
@@ -434,6 +458,14 @@
UTM_START_POSE_COUNT = 1
UTM_START_POSE_1 = 79.38 48.45 -1.8 0 0 0
+[DM-Mobility_125]
+UNREAL_UTM_OFFSET = 0. 0. 0
+RESOLUTION = 0.01905
+UTM_LETTER = N
+UTM_ZONE = 18
+UTM_START_POSE_COUNT = 1
+UTM_START_POSE_1 = 157.9 85.5 1.5 0 0 0
+
[DM-competitionWorld1]
UNREAL_UTM_OFFSET = 0. 0. 0
RESOLUTION = 0.01905
Modified: trunk/moast-bth/moast/include/servoMisJA.hh
===================================================================
--- trunk/moast-bth/moast/include/servoMisJA.hh 2007-10-29 18:21:44 UTC (rev 861)
+++ trunk/moast-bth/moast/include/servoMisJA.hh 2007-10-29 20:24:46 UTC (rev 862)
@@ -26,8 +26,8 @@
\code CVS Status:
$Author: cj_scrapper $
- $Revision: 1.4 $ $
- $Date: 2007/03/12 17:32:07 $
+ $Revision: 1.6 $ $
+ $Date: 2007/10/18 17:53:45 $
\endcode
\author Chris Scrapper
@@ -75,7 +75,8 @@
{
SERVO_MIS_JA_INVALID_JOINT_TYPE = -1,
SERVO_MIS_JA_PRISMATIC_JOINT_TYPE,
- SERVO_MIS_JA_REVOLUTE_JOINT_TYPE
+ SERVO_MIS_JA_REVOLUTE_JOINT_TYPE,
+ SERVO_MIS_JA_SCISSOR_JOINT_TYPE
}ServoMisJAJointType;
typedef enum SERVO_MIS_JA_LINK_CMD_TYPE
@@ -250,6 +251,12 @@
Modification history:
$Log: servoMisJA.hh,v $
+ Revision 1.6 2007/10/18 17:53:45 cj_scrapper
+ Adding Scissor Joint
+
+ Revision 1.5 2007/10/18 17:42:43 cj_scrapper
+ Adding Scissor joint
+
Revision 1.4 2007/03/12 17:32:07 cj_scrapper
Added new servo Mission package commands
Modified: trunk/moast-bth/moast/src/nml/servoMisJA.cc
===================================================================
--- trunk/moast-bth/moast/src/nml/servoMisJA.cc 2007-10-29 18:21:44 UTC (rev 861)
+++ trunk/moast-bth/moast/src/nml/servoMisJA.cc 2007-10-29 20:24:46 UTC (rev 862)
@@ -22,7 +22,7 @@
/*
* Initializer for ServoMisJALinkSet
* Automatically generated by NML CodeGen Java Applet.
-* on Mon Mar 12 16:18:53 EDT 2007
+* on Thu Oct 18 13:45:07 EDT 2007
*/
void initialize_ServoMisJALinkSet(ServoMisJALinkSet* x)
{
@@ -39,7 +39,7 @@
/*
* Initializer for ServoMisJALinkCmd
* Automatically generated by NML CodeGen Java Applet.
-* on Mon Mar 12 16:18:53 EDT 2007
+* on Thu Oct 18 13:45:07 EDT 2007
*/
void initialize_ServoMisJALinkCmd(ServoMisJALinkCmd* x)
{
@@ -52,7 +52,7 @@
/*
* Initializer for ServoMisJALinkStat
* Automatically generated by NML CodeGen Java Applet.
-* on Mon Mar 12 16:18:53 EDT 2007
+* on Thu Oct 18 13:45:07 EDT 2007
*/
void initialize_ServoMisJALinkStat(ServoMisJALinkStat* x)
{
@@ -110,6 +110,62 @@
// Enumerated Type Constants
+// moastSteerType
+#ifndef MAX_ENUM_MOASTSTEERTYPE_STRING_LENGTH
+#define MAX_ENUM_MOASTSTEERTYPE_STRING_LENGTH 29
+#endif
+#ifndef ENUM_MOASTSTEERTYPE_LENGTH
+#define ENUM_MOASTSTEERTYPE_LENGTH 9
+#endif
+
+static const char enum_moastSteerType_string_list[ENUM_MOASTSTEERTYPE_LENGTH][MAX_ENUM_MOASTSTEERTYPE_STRING_LENGTH]= {
+ "MOAST_ACKERMAN_STEER_TYPE", /* 0,1 */
+ "MOAST_FIXED_WING_STEER_TYPE", /* 1,5 */
+ "MOAST_INVALID_STEER_TYPE", /* 2,-1 */
+ "MOAST_OMNI_STEER_TYPE", /* 3,2 */
+ "MOAST_ROTARY_WING_STEER_TYPE", /* 4,4 */
+ "MOAST_SKID_STEER_TYPE", /* 5,0 */
+ "MOAST_SUBMARINE_STEER_TYPE", /* 6,3 */
+ "MOAST_UNKNOWN_STEER_TYPE", /* 7,6 */
+ ""};
+
+static const int enum_moastSteerType_int_list[ENUM_MOASTSTEERTYPE_LENGTH]= {
+ MOAST_ACKERMAN_STEER_TYPE, /* 0,1 */
+ MOAST_FIXED_WING_STEER_TYPE, /* 1,5 */
+ MOAST_INVALID_STEER_TYPE, /* 2,-1 */
+ MOAST_OMNI_STEER_TYPE, /* 3,2 */
+ MOAST_ROTARY_WING_STEER_TYPE, /* 4,4 */
+ MOAST_SKID_STEER_TYPE, /* 5,0 */
+ MOAST_SUBMARINE_STEER_TYPE, /* 6,3 */
+ MOAST_UNKNOWN_STEER_TYPE, /* 7,6 */
+ };
+
+const char *servoMisJA_enum_moastSteerType_symbol_lookup(long v)
+{
+ switch(v)
+ {
+ case MOAST_ACKERMAN_STEER_TYPE: return("MOAST_ACKERMAN_STEER_TYPE"); /* 1 */
+ case MOAST_FIXED_WING_STEER_TYPE: return("MOAST_FIXED_WING_STEER_TYPE"); /* 5 */
+ case MOAST_INVALID_STEER_TYPE: return("MOAST_INVALID_STEER_TYPE"); /* -1 */
+ case MOAST_OMNI_STEER_TYPE: return("MOAST_OMNI_STEER_TYPE"); /* 2 */
+ case MOAST_ROTARY_WING_STEER_TYPE: return("MOAST_ROTARY_WING_STEER_TYPE"); /* 4 */
+ case MOAST_SKID_STEER_TYPE: return("MOAST_SKID_STEER_TYPE"); /* 0 */
+ case MOAST_SUBMARINE_STEER_TYPE: return("MOAST_SUBMARINE_STEER_TYPE"); /* 3 */
+ case MOAST_UNKNOWN_STEER_TYPE: return("MOAST_UNKNOWN_STEER_TYPE"); /* 6 */
+ default:break;
+ }
+ return(NULL);
+}
+
+static const struct cms_enum_info enum_moastSteerType_info_struct={
+ "moastSteerType",
+ (const char **)enum_moastSteerType_string_list,
+ enum_moastSteerType_int_list,
+ MAX_ENUM_MOASTSTEERTYPE_STRING_LENGTH,
+ ENUM_MOASTSTEERTYPE_LENGTH,
+ (cms_symbol_lookup_function_t)servoMisJA_enum_moastSteerType_symbol_lookup
+ };
+
// RCS_ADMIN_STATE
#ifndef MAX_ENUM_RCS_ADMIN_STATE_STRING_LENGTH
#define MAX_ENUM_RCS_ADMIN_STATE_STRING_LENGTH 20
@@ -244,19 +300,21 @@
#define MAX_ENUM_SERVOMISJAJOINTTYPE_STRING_LENGTH 34
#endif
#ifndef ENUM_SERVOMISJAJOINTTYPE_LENGTH
-#define ENUM_SERVOMISJAJOINTTYPE_LENGTH 4
+#define ENUM_SERVOMISJAJOINTTYPE_LENGTH 5
#endif
static const char enum_ServoMisJAJointType_string_list[ENUM_SERVOMISJAJOINTTYPE_LENGTH][MAX_ENUM_SERVOMISJAJOINTTYPE_STRING_LENGTH]= {
"SERVO_MIS_JA_INVALID_JOINT_TYPE", /* 0,-1 */
"SERVO_MIS_JA_PRISMATIC_JOINT_TYPE", /* 1,0 */
"SERVO_MIS_JA_REVOLUTE_JOINT_TYPE", /* 2,1 */
+ "SERVO_MIS_JA_SCISSOR_JOINT_TYPE", /* 3,2 */
""};
static const int enum_ServoMisJAJointType_int_list[ENUM_SERVOMISJAJOINTTYPE_LENGTH]= {
SERVO_MIS_JA_INVALID_JOINT_TYPE, /* 0,-1 */
SERVO_MIS_JA_PRISMATIC_JOINT_TYPE, /* 1,0 */
SERVO_MIS_JA_REVOLUTE_JOINT_TYPE, /* 2,1 */
+ SERVO_MIS_JA_SCISSOR_JOINT_TYPE, /* 3,2 */
};
const char *servoMisJA_enum_ServoMisJAJointType_symbol_lookup(long v)
@@ -266,6 +324,7 @@
case SERVO_MIS_JA_INVALID_JOINT_TYPE: return("SERVO_MIS_JA_INVALID_JOINT_TYPE"); /* -1 */
case SERVO_MIS_JA_PRISMATIC_JOINT_TYPE: return("SERVO_MIS_JA_PRISMATIC_JOINT_TYPE"); /* 0 */
case SERVO_MIS_JA_REVOLUTE_JOINT_TYPE: return("SERVO_MIS_JA_REVOLUTE_JOINT_TYPE"); /* 1 */
+ case SERVO_MIS_JA_SCISSOR_JOINT_TYPE: return("SERVO_MIS_JA_SCISSOR_JOINT_TYPE"); /* 2 */
default:break;
}
return(NULL);
@@ -280,6 +339,138 @@
(cms_symbol_lookup_function_t)servoMisJA_enum_ServoMisJAJointType_symbol_lookup
};
+// moastFlipperType
+#ifndef MAX_ENUM_MOASTFLIPPERTYPE_STRING_LENGTH
+#define MAX_ENUM_MOASTFLIPPERTYPE_STRING_LENGTH 22
+#endif
+#ifndef ENUM_MOASTFLIPPERTYPE_LENGTH
+#define ENUM_MOASTFLIPPERTYPE_LENGTH 5
+#endif
+
+static const char enum_moastFlipperType_string_list[ENUM_MOASTFLIPPERTYPE_LENGTH][MAX_ENUM_MOASTFLIPPERTYPE_STRING_LENGTH]= {
+ "MOAST_FL_FLIPPER_TYPE", /* 0,1 */
+ "MOAST_FR_FLIPPER_TYPE", /* 1,0 */
+ "MOAST_RL_FLIPPER_TYPE", /* 2,3 */
+ "MOAST_RR_FLIPPER_TYPE", /* 3,2 */
+ ""};
+
+static const int enum_moastFlipperType_int_list[ENUM_MOASTFLIPPERTYPE_LENGTH]= {
+ MOAST_FL_FLIPPER_TYPE, /* 0,1 */
+ MOAST_FR_FLIPPER_TYPE, /* 1,0 */
+ MOAST_RL_FLIPPER_TYPE, /* 2,3 */
+ MOAST_RR_FLIPPER_TYPE, /* 3,2 */
+ };
+
+const char *servoMisJA_enum_moastFlipperType_symbol_lookup(long v)
+{
+ switch(v)
+ {
+ case MOAST_FL_FLIPPER_TYPE: return("MOAST_FL_FLIPPER_TYPE"); /* 1 */
+ case MOAST_FR_FLIPPER_TYPE: return("MOAST_FR_FLIPPER_TYPE"); /* 0 */
+ case MOAST_RL_FLIPPER_TYPE: return("MOAST_RL_FLIPPER_TYPE"); /* 3 */
+ case MOAST_RR_FLIPPER_TYPE: return("MOAST_RR_FLIPPER_TYPE"); /* 2 */
+ default:break;
+ }
+ return(NULL);
+}
+
+static const struct cms_enum_info enum_moastFlipperType_info_struct={
+ "moastFlipperType",
+ (const char **)enum_moastFlipperType_string_list,
+ enum_moastFlipperType_int_list,
+ MAX_ENUM_MOASTFLIPPERTYPE_STRING_LENGTH,
+ ENUM_MOASTFLIPPERTYPE_LENGTH,
+ (cms_symbol_lookup_function_t)servoMisJA_enum_moastFlipperType_symbol_lookup
+ };
+
+// moastRobotType
+#ifndef MAX_ENUM_MOASTROBOTTYPE_STRING_LENGTH
+#define MAX_ENUM_MOASTROBOTTYPE_STRING_LENGTH 26
+#endif
+#ifndef ENUM_MOASTROBOTTYPE_LENGTH
+#define ENUM_MOASTROBOTTYPE_LENGTH 7
+#endif
+
+static const char enum_moastRobotType_string_list[ENUM_MOASTROBOTTYPE_LENGTH][MAX_ENUM_MOASTROBOTTYPE_STRING_LENGTH]= {
+ "MOAST_AERIAL_ROBOT_TYPE", /* 0,2 */
+ "MOAST_GROUND_VEHICLE_TYPE", /* 1,0 */
+ "MOAST_INVALID_ROBOT_TYPE", /* 2,-1 */
+ "MOAST_LEGGED_ROBOT_TYPE", /* 3,3 */
+ "MOAST_NAUTIC_ROBOT_TYPE", /* 4,1 */
+ "MOAST_UNKNOWN_ROBOT_TYPE", /* 5,4 */
+ ""};
+
+static const int enum_moastRobotType_int_list[ENUM_MOASTROBOTTYPE_LENGTH]= {
+ MOAST_AERIAL_ROBOT_TYPE, /* 0,2 */
+ MOAST_GROUND_VEHICLE_TYPE, /* 1,0 */
+ MOAST_INVALID_ROBOT_TYPE, /* 2,-1 */
+ MOAST_LEGGED_ROBOT_TYPE, /* 3,3 */
+ MOAST_NAUTIC_ROBOT_TYPE, /* 4,1 */
+ MOAST_UNKNOWN_ROBOT_TYPE, /* 5,4 */
+ };
+
+const char *servoMisJA_enum_moastRobotType_symbol_lookup(long v)
+{
+ switch(v)
+ {
+ case MOAST_AERIAL_ROBOT_TYPE: return("MOAST_AERIAL_ROBOT_TYPE"); /* 2 */
+ case MOAST_GROUND_VEHICLE_TYPE: return("MOAST_GROUND_VEHICLE_TYPE"); /* 0 */
+ case MOAST_INVALID_ROBOT_TYPE: return("MOAST_INVALID_ROBOT_TYPE"); /* -1 */
+ case MOAST_LEGGED_ROBOT_TYPE: return("MOAST_LEGGED_ROBOT_TYPE"); /* 3 */
+ case MOAST_NAUTIC_ROBOT_TYPE: return("MOAST_NAUTIC_ROBOT_TYPE"); /* 1 */
+ case MOAST_UNKNOWN_ROBOT_TYPE: return("MOAST_UNKNOWN_ROBOT_TYPE"); /* 4 */
+ default:break;
+ }
+ return(NULL);
+}
+
+static const struct cms_enum_info enum_moastRobotType_info_struct={
+ "moastRobotType",
+ (const char **)enum_moastRobotType_string_list,
+ enum_moastRobotType_int_list,
+ MAX_ENUM_MOASTROBOTTYPE_STRING_LENGTH,
+ ENUM_MOASTROBOTTYPE_LENGTH,
+ (cms_symbol_lookup_function_t)servoMisJA_enum_moastRobotType_symbol_lookup
+ };
+
+// anonymous_enum_______include_moastTypes_hh_210
+#ifndef MAX_ENUM_ANONYMOUS_ENUM_______INCLUDE_MOASTTYPES_HH_210_STRING_LENGTH
+#define MAX_ENUM_ANONYMOUS_ENUM_______INCLUDE_MOASTTYPES_HH_210_STRING_LENGTH 17
+#endif
+#ifndef ENUM_ANONYMOUS_ENUM_______INCLUDE_MOASTTYPES_HH_210_LENGTH
+#define ENUM_ANONYMOUS_ENUM_______INCLUDE_MOASTTYPES_HH_210_LENGTH 3
+#endif
+
+static const char enum_anonymous_enum_______include_moastTypes_hh_210_string_list[ENUM_ANONYMOUS_ENUM_______INCLUDE_MOASTTYPES_HH_210_LENGTH][MAX_ENUM_ANONYMOUS_ENUM_______INCLUDE_MOASTTYPES_HH_210_STRING_LENGTH]= {
+ "MOAST_ROTATE_CCW", /* 0,1 */
+ "MOAST_ROTATE_CW", /* 1,0 */
+ ""};
+
+static const int enum_anonymous_enum_______include_moastTypes_hh_210_int_list[ENUM_ANONYMOUS_ENUM_______INCLUDE_MOASTTYPES_HH_210_LENGTH]= {
+ MOAST_ROTATE_CCW, /* 0,1 */
+ MOAST_ROTATE_CW, /* 1,0 */
+ };
+
+const char *servoMisJA_enum_anonymous_enum_______include_moastTypes_hh_210_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(NULL);
+}
+
+static const struct cms_enum_info enum_anonymous_enum_______include_moastTypes_hh_210_info_struct={
+ "anonymous_enum_______include_moastTypes_hh_210",
+ (const char **)enum_anonymous_enum_______include_moastTypes_hh_210_string_list,
+ enum_anonymous_enum_______include_moastTypes_hh_210_int_list,
+ MAX_ENUM_ANONYMOUS_ENUM_______INCLUDE_MOASTTYPES_HH_210_STRING_LENGTH,
+ ENUM_ANONYMOUS_ENUM_______INCLUDE_MOASTTYPES_HH_210_LENGTH,
+ (cms_symbol_lookup_function_t)servoMisJA_enum_anonymous_enum_______include_moastTypes_hh_210_symbol_lookup
+ };
+
/*
Estimated_size ServoMisJACfgCycleTime 32
Estimated_size ServoMisJACfgDebug 32
@@ -298,7 +489,7 @@
/*
* NML/CMS Format function : servoMisJA_format
* Automatically generated by NML CodeGen Java Applet.
-* on Mon Mar 12 16:18:53 EDT 2007
+* on Thu Oct 18 13:45:07 EDT 2007
*/
int servoMisJA_format(NMLTYPE type, void *buffer, CMS *cms)
{
@@ -380,7 +571,7 @@
/*
* NML/CMS Update function for ServoMisJACmdMove
* Automatically generated by NML CodeGen Java Applet.
-* on Mon Mar 12 16:18:53 EDT 2007
+* on Thu Oct 18 13:45:07 EDT 2007
*/
void ServoMisJACmdMove::update(CMS *cms)
{
@@ -405,7 +596,7 @@
/*
* NML/CMS Update function for ServoMisJALinkSet
* Automatically generated by NML CodeGen Java Applet.
-* on Mon Mar 12 16:18:53 EDT 2007
+* on Thu Oct 18 13:45:07 EDT 2007
*/
void nmlupdate(CMS *cms,ServoMisJALinkSet *x)
{
@@ -429,7 +620,7 @@
/*
* NML/CMS Update function for ServoMisJACfgDebug
* Automatically generated by NML CodeGen Java Applet.
-* on Mon Mar 12 16:18:53 EDT 2007
+* on Thu Oct 18 13:45:07 EDT 2007
*/
void ServoMisJACfgDebug::update(CMS *cms)
{
@@ -446,7 +637,7 @@
/*
* NML/CMS Update function for ServoMisJASet
* Automatically generated by NML CodeGen Java Applet.
-* on Mon Mar 12 16:18:53 EDT 2007
+* on Thu Oct 18 13:45:07 EDT 2007
*/
void ServoMisJASet::update(CMS *cms)
{
@@ -475,7 +666,7 @@
/*
* NML/CMS Update function for ServoMisJACmdShutdown
* Automatically generated by NML CodeGen Java Applet.
-* on Mon Mar 12 16:18:53 EDT 2007
+* on Thu Oct 18 13:45:07 EDT 2007
*/
void ServoMisJACmdShutdown::update(CMS *cms)
{
@@ -491,7 +682,7 @@
/*
* NML/CMS Update function for ServoMisJACfgCycleTime
* Automatically generated by NML CodeGen Java Applet.
-* on Mon Mar 12 16:18:53 EDT 2007
+* on Thu Oct 18 13:45:07 EDT 2007
*/
void ServoMisJACfgCycleTime::update(CMS *cms)
{
@@ -508,7 +699,7 @@
/*
* NML/CMS Update function for ServoMisJACmdInit
* Automatically generated by NML CodeGen Java Applet.
-* on Mon Mar 12 16:18:53 EDT 2007
+* on Thu Oct 18 13:45:07 EDT 2007
*/
void ServoMisJACmdInit::update(CMS *cms)
{
@@ -524,7 +715,7 @@
/*
* NML/CMS Update function for ServoMisJALinkCmd
* Automatically generated by NML CodeGen Java Applet.
-* on Mon Mar 12 16:18:53 EDT 2007
+* on Thu Oct 18 13:45:07 EDT 2007
*/
void nmlupdate(CMS *cms,ServoMisJALinkCmd *x)
{
@@ -542,7 +733,7 @@
/*
* NML/CMS Update function for ServoMisJACmdAbort
* Automatically generated by NML CodeGen Java Applet.
-* on Mon Mar 12 16:18:53 EDT 2007
+* on Thu Oct 18 13:45:07 EDT 2007
*/
void ServoMisJACmdAbort::update(CMS *cms)
{
@@ -558,7 +749,7 @@
/*
* NML/CMS Update function for ServoMisJAStat
* Automatically generated by NML CodeGen Java Applet.
-* on Mon Mar 12 16:18:53 EDT 2007
+* on Thu Oct 18 13:45:07 EDT 2007
*/
void ServoMisJAStat::update(CMS *cms)
{
@@ -590,7 +781,7 @@
/*
* NML/CMS Update function for ServoMisJACmdHalt
* Automatically generated by NML CodeGen Java Applet.
-* on Mon Mar 12 16:18:53 EDT 2007
+* on Thu Oct 18 13:45:07 EDT 2007
*/
void ServoMisJACmdHalt::update(CMS *cms)
{
@@ -606,7 +797,7 @@
/*
* NML/CMS Update function for ServoMisJALinkStat
* Automatically generated by NML CodeGen Java Applet.
-* on Mon Mar 12 16:18:53 EDT 2007
+* on Thu Oct 18 13:45:07 EDT 2007
*/
void nmlupdate(CMS *cms,ServoMisJALinkStat *x)
{
Modified: trunk/moast-bth/moast/src/prim/primMobMain.cc
===================================================================
--- trunk/moast-bth/moast/src/prim/primMobMain.cc 2007-10-29 18:21:44 UTC (rev 861)
+++ trunk/moast-bth/moast/src/prim/primMobMain.cc 2007-10-29 20:24:46 UTC (rev 862)
@@ -16,8 +16,8 @@
\code CVS Status:
$Author: dr_steveb $
- $Revision: 1.30 $
- $Date: 2007/05/10 13:18:29 $
+ $Revision: 1.32 $
+ $Date: 2007/10/17 21:35:55 $
\endcode
\author Fred Proctor
@@ -286,7 +286,7 @@
ackerMsg.velocity,
ackerMsg.steerAngle,
trash);
- ackerMsg.crabAngle = 0;
+ ackerMsg.crabAngle = -ackerMsg.steerAngle;
SET_AND_RECORD(ackerMsg);
sub_cmd_buf->write(&ackerMsg);
} else if (engine->isNautic()) {
@@ -329,7 +329,7 @@
ackerMsg.velocity,
ackerMsg.steerAngle,
trash);
- ackerMsg.crabAngle = 0;
+ ackerMsg.crabAngle = -ackerMsg.steerAngle;
SET_AND_RECORD(ackerMsg);
sub_cmd_buf->write(&ackerMsg);
} else if (engine->isNautic()) {
@@ -455,7 +455,7 @@
stat->status = engine->doCmdVel(S0, cmd->v, cmd->w,
stat->state,
ackerMsg.velocity, ackerMsg.steerAngle, trash );
- ackerMsg.crabAngle = 0;
+ ackerMsg.crabAngle = -ackerMsg.steerAngle;
SET_AND_RECORD(ackerMsg);
sub_cmd_buf->write(&ackerMsg);
} else if (engine->isNautic()) {
@@ -765,15 +765,16 @@
#define INI_INVALID 2
#endif
-static int iniLoad(const char * prog, const char * filename, PrimMobJASet * set)
+static int iniLoad(const char * prog, const char * filename, PrimMobJASet * set, PrimMobEngine *engine)
{
INIFILE inifile;
- const char * key;
const char * section;
const char * inistring;
+ const char * extra;
double d;
int i;
int retval = INI_OK;
+ char key[80];
if (0 != inifile.open(filename)) {
rcs_print_error("%s: can't open INI file %s, using defaults for PrimMobJASet\n", prog, filename);
@@ -789,7 +790,14 @@
section = "PRIM_MOB_JA";
- key = "CYCLE_TIME";
+ if( engine->isAckerman() )
+ extra = "ACKERMAN_";
+ else if( engine->isNautic() )
+ extra = "NAUTIC_";
+ else
+ extra = "";
+
+ sprintf( key, "%s", "CYCLE_TIME" );
if (NULL != (inistring = inifile.find(key, section))) {
if (1 == sscanf(inistring, "%lf", &d) &&
d > 0.0) {
@@ -812,7 +820,7 @@
retval = (retval == INI_OK ? INI_DEFAULT : retval);
}
- key = "MAX_TRAN_VEL";
+ sprintf(key, "%s%s", extra,"MAX_TRAN_VEL" );
if (NULL != (inistring = inifile.find(key, section))) {
if (1 == sscanf(inistring, "%lf", &d) &&
d > 0.0) {
@@ -834,7 +842,8 @@
retval = (retval == INI_OK ? INI_DEFAULT : retval);
}
- key = "MAX_TRAN_ACC";
+
+ sprintf( key, "%s%s", extra, "MAX_TRAN_ACC" );
if (NULL != (inistring = inifile.find(key, section))) {
if (1 == sscanf(inistring, "%lf", &d) &&
d > 0.0) {
@@ -856,7 +865,7 @@
retval = (retval == INI_OK ? INI_DEFAULT : retval);
}
- key = "MAX_ROT_VEL";
+ sprintf( key, "%s%s", extra, "MAX_ROT_VEL" );
if (NULL != (inistring = inifile.find(key, section))) {
if (1 == sscanf(inistring, "%lf", &d) &&
d > 0.0) {
@@ -878,7 +887,7 @@
retval = (retval == INI_OK ? INI_DEFAULT : retval);
}
- key = "MAX_ROT_ACC";
+ sprintf(key, "%s%s", extra, "MAX_ROT_ACC");
if (NULL != (inistring = inifile.find(key, section))) {
if (1 == sscanf(inistring, "%lf", &d) &&
d > 0.0) {
@@ -900,7 +909,7 @@
retval = (retval == INI_OK ? INI_DEFAULT : retval);
}
- key = "V_CUTOFF_ANGLE";
+ sprintf( key, "%s%s", extra, "V_CUTOFF_ANGLE" );
if (NULL != (inistring = inifile.find(key, section))) {
if (1 == sscanf(inistring, "%lf", &d) &&
d > 0.0) {
@@ -922,7 +931,7 @@
retval = (retval == INI_OK ? INI_DEFAULT : retval);
}
- key = "W_CUTOFF_ANGLE";
+ sprintf( key, "%s%s", extra, "W_CUTOFF_ANGLE" );
if (NULL != (inistring = inifile.find(key, section))) {
if (1 == sscanf(inistring, "%lf", &d) &&
d > 0.0) {
@@ -944,7 +953,7 @@
retval = (retval == INI_OK ? INI_DEFAULT : retval);
}
- key = "DEBUG";
+ sprintf( key, "%s", "DEBUG" );
if (NULL != (inistring = inifile.find(key, section))) {
if (1 == sscanf(inistring, "%i", &i)) {
// found, and valid
@@ -1086,7 +1095,7 @@
// others will come from .ini file shortly...
// read in the INI file params
- if (INI_INVALID == iniLoad(argv[0], INI_FILE, &primMobJASet)) {
+ if (INI_INVALID == iniLoad(argv[0], INI_FILE, &primMobJASet, engine)) {
rcs_print_error("%s: error loading INI file '%s'\n", argv[0], INI_FILE);
return 1;
}
@@ -1408,6 +1417,12 @@
Modification history:
$Log: primMobMain.cc,v $
+ Revision 1.32 2007/10/17 21:35:55 dr_steveb
+ Added reading of ackerman and nautic specific parameters from .ini file
+
+ Revision 1.31 2007/10/17 19:15:20 dr_steveb
+ Added ability to drive rear-wheel steer vehicles
+
Revision 1.30 2007/05/10 13:18:29 dr_steveb
Separated the prim mobility planning engine from the interface wrappers. This will allow a controller to be created (the traffic controller) that controls a large number of vehicles without the overhead of individual NML channels for each vehicle. No changes in the algorithms were made.
Modified: trunk/moast-bth/moast/src/section/rcsSectMobPlEngine.cc
===================================================================
--- trunk/moast-bth/moast/src/section/rcsSectMobPlEngine.cc 2007-10-29 18:21:44 UTC (rev 861)
+++ trunk/moast-bth/moast/src/section/rcsSectMobPlEngine.cc 2007-10-29 20:24:46 UTC (rev 862)
@@ -2092,7 +2092,7 @@
dataPt[count++] = cell;
}
}
- Boeing::MsgMap::MsgMapFactory(Boeing::MAP_RLE,
+ Boeing::MsgMap::MsgMapFactory(Boeing::MAP_X_MAJOR,
boeingMapMsg,
dataPt, count,
0, 0,
Modified: trunk/moast-bth/moast/src/sim/simWare/swKBTypes.hh
===================================================================
--- trunk/moast-bth/moast/src/sim/simWare/swKBTypes.hh 2007-10-29 18:21:44 UTC (rev 861)
+++ trunk/moast-bth/moast/src/sim/simWare/swKBTypes.hh 2007-10-29 20:24:46 UTC (rev 862)
@@ -34,6 +34,7 @@
typedef enum SIMWARE_JOINT_TYPE {
SW_KB_PRISMATIC_JOINT_TYPE,
+ SW_KB_SCISSOR_JOINT_TYPE,
SW_KB_REVOLUTE_JOINT_TYPE,
SW_KB_UNKNOWN_JOINT_TYPE
}SIMwareJointType;
Modified: trunk/moast-bth/moast/src/sim/simWare/swServoMis.cc
===================================================================
--- trunk/moast-bth/moast/src/sim/simWare/swServoMis.cc 2007-10-29 18:21:44 UTC (rev 861)
+++ trunk/moast-bth/moast/src/sim/simWare/swServoMis.cc 2007-10-29 20:24:46 UTC (rev 862)
@@ -164,6 +164,10 @@
case SW_KB_REVOLUTE_JOINT_TYPE:
((ServoMisJASet *)setMsg)->linkSet[count].jType=SERVO_MIS_JA_REVOLUTE_JOINT_TYPE;
break;
+
+ case SW_KB_SCISSOR_JOINT_TYPE:
+ ((ServoMisJASet *)setMsg)->linkSet[count].jType=SERVO_MIS_JA_SCISSOR_JOINT_TYPE;
+ break;
default:
((ServoMisJASet *)setMsg)->linkSet[count].jType=SERVO_MIS_JA_INVALID_JOINT_TYPE;
Modified: trunk/moast-bth/moast/src/sim/simWare/usarInfSkin.cc
===================================================================
--- trunk/moast-bth/moast/src/sim/simWare/usarInfSkin.cc 2007-10-29 18:21:44 UTC (rev 861)
+++ trunk/moast-bth/moast/src/sim/simWare/usarInfSkin.cc 2007-10-29 20:24:46 UTC (rev 862)
@@ -1518,6 +1518,8 @@
linkPtr->setJointType(SW_KB_REVOLUTE_JOINT_TYPE);
else if( !nocase_cmp (toks[i], "Prismatic"))
linkPtr->setJointType(SW_KB_PRISMATIC_JOINT_TYPE);
+ else if( !nocase_cmp (toks[i], "Scissor"))
+ linkPtr->setJointType(SW_KB_SCISSOR_JOINT_TYPE);
else
linkPtr->setJointType(SW_KB_UNKNOWN_JOINT_TYPE);
}
Modified: trunk/moast-bth/moast/src/skel/Makefile
===================================================================
--- trunk/moast-bth/moast/src/skel/Makefile 2007-10-29 18:21:44 UTC (rev 861)
+++ trunk/moast-bth/moast/src/skel/Makefile 2007-10-29 20:24:46 UTC (rev 862)
@@ -5,11 +5,11 @@
include Makefile.rcs
NML_HEADERS = skel.hh
-NML_SOURCES = $(NML_HEADERS:%.hh=%_n.cc)
+NML_SOURCES = $(NML_HEADERS:%.hh=%_n.cc) skel.nml
NML_OBJS = $(NML_SOURCES:%.cc=%.o)
OTHER_HEADERS = skelCommon.hh
-OTHER_SOURCES = skelSvr.cc skelApp.cc skelShell.cc
+OTHER_SOURCES = skelSvr.cc skelApp.cc skelShell.cc nmlRead.cc nmlWrite.cc
OTHER_OBJS = $(OTHER_SOURCES:%.cc=%.o)
HEADERS = $(NML_HEADERS) $(OTHER_HEADERS)
@@ -20,7 +20,7 @@
LIBS = libskel_nml.a
-BINS = skelSvr skelApp skelShell
+BINS = skelSvr skelApp skelShell nmlRead nmlWrite
all : $(NML_SOURCES) $(OBJS) $(LIBS) $(BINS)
@@ -30,6 +30,7 @@
%_n.cc : %.hh
echo load $< > $(TEMP_CODEGEN)
echo clear >> $(TEMP_CODEGEN)
+ echo _generate_symbol_lookups=true >> $(TEMP_CODEGEN)
echo 'select_from_file $<' >> $(TEMP_CODEGEN)
echo 'set_format_function $(@:%_n.cc=%)_format' >> $(TEMP_CODEGEN)
echo 'generate C++ format >$@' >> $(TEMP_CODEGEN)
@@ -40,7 +41,16 @@
rm $(TEMP_CODEGEN)
indent $@
+#
+# build the nml configuration file
#
+NMLCFG = $(RCSDIR)/bin/nmlcfg
+
+%.nml : %.cfg
+ @ echo building $@ from $<
+ $(NMLCFG) $< -o $@
+
+#
# Explicit rule for the skeleton NML library
#
libskel_nml.a : $(NML_OBJS)
Added: trunk/moast-bth/moast/src/skel/nmlRead.cc
===================================================================
--- trunk/moast-bth/moast/src/skel/nmlRead.cc (rev 0)
+++ trunk/moast-bth/moast/src/skel/nmlRead.cc 2007-10-29 20:24:46 UTC (rev 862)
@@ -0,0 +1,133 @@
+/*****************************************************************************
+ 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 nmlRead.cc
+ \brief example of reading an NML buffer
+
+ \code CVS Status:
+ $Author: dr_steveb $
+ $Revision: 1.3 $
+ $Date: 2007/10/16 17:13:08 $
+ \endcode
+
+ \author Stephen Balakisky
+ \date 10/15/2007
+*/
+
+#include <stddef.h> // NULL
+#include <stdlib.h> // getenv()
+#include "rcs.hh" // RCS_CMD,STAT_CHANNEL, rcs_print()
+#include "skelCommon.hh"
+#include "skel.hh"
+
+static RCS_CMD_CHANNEL * skel_cmd_buf= NULL;
+
+#define DEFAULT_NML_FILE "./skel.nml"
+#define SKEL_CMD_NAME "skelCmd" // NML cmd buffer name
+#define THIS_PROCESS "nmlRead" // NML process name for the server
+
+static int init_nml_buffers(char * config_file)
+{
+ if (NULL != skel_cmd_buf) delete skel_cmd_buf;
+ skel_cmd_buf = new RCS_CMD_CHANNEL(skel_format, SKEL_CMD_NAME, THIS_PROCESS, config_file);
+ if (! skel_cmd_buf->valid()) {
+ delete skel_cmd_buf;
+ skel_cmd_buf = 0;
+ return 1;
+ }
+ skel_cmd_buf->clear();
+ return 0;
+}
+
+#define max(a,b) ((a) > (b) ? (a) : (b))
+
+int main(int argc, char *argv[])
+{
+ char config_file_default[] = DEFAULT_NML_FILE;
+ char * config_file_env;
+ char * NML_FILE = config_file_default;
+ int retval;
+ NMLTYPE cmdType;
+ RCS_TIMER timer(.1);
+
+ // get the config file name from arg, if provided, next environment,
+ // if there, else default
+ if (argc > 1) {
+ NML_FILE = argv[1];
+ rcs_print("%s: using specified config file %s\n", argv[0], NML_FILE);
+ } else {
+ config_file_env = getenv("CONFIG_NML");
+ if (NULL != config_file_env) {
+ NML_FILE = config_file_env;
+ rcs_print("%s: using environment config file %s\n", argv[0], NML_FILE);
+ } else {
+ rcs_print("%s: using default config file %s\n", argv[0], NML_FILE);
+ }
+ }
+
+ // initial allocate all the buffers
+ retval = init_nml_buffers(NML_FILE);
+
+ if (retval) {
+ rcs_print_error("%s: can't allocate NML buffers\n", argv[0]);
+ return 1;
+ }
+
+ // read a value from the buffer
+ /******************************************************************
+ * READ COMMAND - Command information is received, and the system
+ * is prepared to execute the command. When a new
+ * command is received, the old command is immediately
+ * replaced by this command. If the command is not
+ * able to be interrupted, error flags are set and
+ * will be reported during the writng of status
+ */
+
+ // read command
+ for(;;)
+ {
+ cmdType = skel_cmd_buf->read();
+ switch (cmdType) {
+ case 0:
+ // no new command
+ break;
+ case -1:
+ // comm error
+ rcs_print_error("%s: NML command read error\n", argv[0]);
+ break;
+ case SKEL_CMD_GO_TYPE:
+ rcs_print( "new Go command received with value: %f\n", ((SkelCmdGo *) skel_cmd_buf->get_address())->where );
+ break;
+ case SKEL_CMD_SHUTDOWN_TYPE:
+ nml_cleanup();
+ rcs_print( "Exiting...\n" );
+ return 0;
+ }
+ timer.wait();
+ }
+ // will never get here
+ return 0;
+}
+
+/*
+ Modification history:
+
+ $Log: nmlRead.cc,v $
+ Revision 1.3 2007/10/16 17:13:08 dr_steveb
+ Added timer to loop
+
+ Revision 1.2 2007/10/16 12:53:48 dr_steveb
+ Added the ability to have programs exit and some comments.
+
+ Revision 1.1 2007/10/15 21:09:55 dr_steveb
+ Changes made for IEEE workshop presentation
+
+*/
Added: trunk/moast-bth/moast/src/skel/nmlWrite.cc
===================================================================
--- trunk/moast-bth/moast/src/skel/nmlWrite.cc (rev 0)
+++ trunk/moast-bth/moast/src/skel/nmlWrite.cc 2007-10-29 20:24:46 UTC (rev 862)
@@ -0,0 +1,115 @@
+/*****************************************************************************
+ 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 nmlWrite.cc
+ \brief example of writing an NML buffer
+
+ \code CVS Status:
+ $Author: dr_steveb $
+ $Revision: 1.2 $
+ $Date: 2007/10/16 12:53:48 $
+ \endcode
+
+ \author Stephen Balakisky
+ \date 10/15/2007
+*/
+
+#include <stddef.h> // NULL
+#include <stdlib.h> // getenv()
+#include <stdio.h>
+#include "rcs.hh" // RCS_CMD,STAT_CHANNEL, rcs_print()
+#include "skelCommon.hh"
+#include "skel.hh"
+
+static RCS_CMD_CHANNEL * skel_cmd_buf= NULL;
+
+#define DEFAULT_NML_FILE "./skel.nml"
+#define SKEL_CMD_NAME "skelCmd" // NML cmd buffer name
+#define THIS_PROCESS "nmlWrite" // NML process name for the server
+
+static int init_nml_buffers(char * config_file)
+{
+ if (NULL != skel_cmd_buf) delete skel_cmd_buf;
+ skel_cmd_buf = new RCS_CMD_CHANNEL(skel_format, SKEL_CMD_NAME, THIS_PROCESS, config_file);
+ if (! skel_cmd_buf->valid()) {
+ delete skel_cmd_buf;
+ skel_cmd_buf = 0;
+ return 1;
+ }
+ return 0;
+}
+
+#define max(a,b) ((a) > (b) ? (a) : (b))
+
+int main(int argc, char *argv[])
+{
+ char config_file_default[] = DEFAULT_NML_FILE;
+ char * config_file_env;
+ char * NML_FILE = config_file_default;
+ int retval;
+ SkelCmdGo goCmd;
+ SkelCmdShutdown shutdownCmd;
+
+ printf("usage: please run skelSvr before running this program.\n");
+ printf("Next, run this program (nmlWrite) and nmlRead.\n");
+ printf("Type a value <CR> and the value should appear at the nmlRead application.\n");
+ printf("Type a negative value to exit.\n");
+ // get the config file name from arg, if provided, next environment,
+ // if there, else default
+ if (argc > 1) {
+ NML_FILE = argv[1];
+ rcs_print("%s: using specified config file %s\n", argv[0], NML_FILE);
+ } else {
+ config_file_env = getenv("CONFIG_NML");
+ if (NULL != config_file_env) {
+ NML_FILE = config_file_env;
+ rcs_print("%s: using environment config file %s\n", argv[0], NML_FILE);
+ } else {
+ rcs_print("%s: using default config file %s\n", argv[0], NML_FILE);
+ }
+ }
+
+ // initial allocate all the buffers
+ retval = init_nml_buffers(NML_FILE);
+
+ if (retval) {
+ rcs_print_error("%s: can't allocate NML buffers\n", argv[0]);
+ return 1;
+ }
+ for(;;)
+ {
+ fscanf(stdin, "%lf", &goCmd.where);
+ if( goCmd.where < 0 )
+ {
+ skel_cmd_buf->write(shutdownCmd);
+ nml_cleanup();
+ printf( "Exiting...\n" );
+ return 0;
+ }
+ else
+ skel_cmd_buf->write(goCmd);
+ }
+
+ // will never get here
+ return 0;
+}
+
+/*
+ Modification history:
+
+ $Log: nmlWrite.cc,v $
+ Revision 1.2 2007/10/16 12:53:48 dr_steveb
+ Added the ability to have programs exit and some comments.
+
+ Revision 1.1 2007/10/15 21:09:56 dr_steveb
+ Changes made for IEEE workshop presentation
+
+*/
Added: trunk/moast-bth/moast/src/skel/skel.cfg
===================================================================
--- trunk/moast-bth/moast/src/skel/skel.cfg (rev 0)
+++ trunk/moast-bth/moast/src/skel/skel.cfg 2007-10-29 20:24:46 UTC (rev 862)
@@ -0,0 +1,22 @@
+b bufname=skelCmd size=1024
+b bufname=skelStat size=1024
+b bufname=skelCfg size=1024
+b bufname=skelSet size=1024
+
+p bufname=skelCmd name=skelSvr master=1 server=1 proctype=local
+p bufname=skelStat name=skelSvr master=1 server=1 proctype=local
+p bufname=skelCfg name=skelSvr master=1 server=1 proctype=local
+p bufname=skelSet name=skelSvr master=1 server=1 proctype=local
+
+p bufname=skelCmd name=skelApp master=0 server=0 proctype=local
+p bufname=skelStat name=skelApp master=0 server=0 proctype=local
+p bufname=skelCfg name=skelApp master=0 server=0 proctype=local
+p bufname=skelSet name=skelApp master=0 server=0 proctype=local
+
+p bufname=skelCmd name=skelShell master=0 server=0 proctype=local
+p bufname=skelStat name=skelShell master=0 server=0 proctype=local
+p bufname=skelCfg name=skelShell master=0 server=0 proctype=local
+p bufname=skelSet name=skelShell master=0 server=0 proctype=local
+
+p bufname=skelCmd name=nmlWrite master=0 server=0 proctype=local
+p bufname=skelCmd name=nmlRead master=0 server=0 proctype=local
Modified: trunk/moast-bth/moast/src/skel/skel.nml
===================================================================
--- trunk/moast-bth/moast/src/skel/skel.nml 2007-10-29 18:21:44 UTC (rev 861)
+++ trunk/moast-bth/moast/src/skel/skel.nml 2007-10-29 20:24:46 UTC (rev 862)
@@ -1,30 +1,30 @@
# skel.nml
+
+# Buffers:
+# name type host size neut 0 buf# max_proc . . .
+B skelCmd SHMEM localhost 1088 0 * 1 * 30701 TCP=25700 xdr
+B skelStat SHMEM localhost 1088 0 * 2 * 30702 TCP=25701 xdr
+B skelCfg SHMEM localhost 1088 0 * 3 * 30703 TCP=25702 xdr
+B skelSet SHMEM localhost 1088 0 * 4 * 30704 TCP=25703 xdr
-# Buffers
-# name type host size neut (old) buf# max_proc shmem_key, ...
+# Processes:
+# Name Buffer type host ops server timeout master c_num . . .
+P skelSvr skelCmd LOCAL localhost RW 1 INF 1 0
+P skelSvr skelStat LOCAL localhost RW 1 INF 1 0
+P skelSvr skelCfg LOCAL localhost RW 1 INF 1 0
+P skelSvr skelSet LOCAL localhost RW 1 INF 1 0
+
+P skelApp skelCmd LOCAL localhost RW 0 INF 0 1
+P skelApp skelStat LOCAL localhost RW 0 INF 0 1
+P skelApp skelCfg LOCAL localhost RW 0 INF 0 1
+P skelApp skelSet LOCAL localhost RW 0 INF 0 1
+
+P skelShell skelCmd LOCAL localhost RW 0 INF 0 2
+P skelShell skelStat LOCAL localhost RW 0 INF 0 2
+P skelShell skelCfg LOCAL localhost RW 0 INF 0 2
+P skelShell skelSet LOCAL localhost RW 0 INF 0 2
+
+P nmlWrite skelCmd LOCAL localhost RW 0 INF 0 3
+P nmlRead skelCmd LOCAL localhost RW 0 INF 0 4
-B skelCmd SHMEM localhost 1024 0 * 1 * 101 TCP=25101 xdr
-B skelStat SHMEM localhost 1024 0 * 2 * 102 TCP=25102 xdr
-B skelCfg SHMEM localhost 1024 0 * 3 * 103 TCP=25103 xdr
-B skelSet SHMEM localhost 1024 0 * 4 * 104 TCP=25104 xdr
-
-# Processes
-# name buffer type host ops server timeout master c_num waitformaster, ...
-
-P skelSvr skelCmd LOCAL localhost RW 1 INF 1 0
-P skelSvr skelStat LOCAL localhost RW 1 INF 1 0
-P skelSvr skelCfg LOCAL localhost RW 1 INF 1 0
-P skelSvr skelSet LOCAL localhost RW 1 INF 1 0
-
-P skelApp skelCmd LOCAL localhost RW 0 INF 0 1 waitformaster
-P skelApp skelStat LOCAL localhost RW 0 INF 0 1 waitformaster
-P skelApp skelCfg LOCAL localhost RW 0 INF 0 1 waitformaster
-P skelApp skelSet LOCAL localhost RW 0 INF 0 1 waitformaster
-
-P skelShell skelCmd LOCAL localhost RW 0 INF 0 2 waitformaster
-P skelShell skelStat LOCAL localhost RW 0 INF 0 2 waitformaster
-P skelShell skelCfg LOCAL localhost RW 0 INF 0 2 waitformaster
-P skelShell skelSet LOCAL localhost RW 0 INF 0 2 waitformaster
-
-P default default AUTO localhost RW 0 INF 0 10 waitformaster
Modified: trunk/moast-bth/moast/src/skel/skel_n.cc
===================================================================
--- trunk/moast-bth/moast/src/skel/skel_n.cc 2007-10-29 18:21:44 UTC (rev 861)
+++ trunk/moast-bth/moast/src/skel/skel_n.cc 2007-10-29 20:24:46 UTC (rev 862)
@@ -110,7 +110,7 @@
default:
break;
}
- return (NULL);
+ return ("!!UNDEFINED_SYMBOL!!");
}
static const struct cms_enum_info enum_RCS_STATUS_info_struct = {
@@ -123,9 +123,23 @@
};
/*
+Estimated_size SkelCfgCycleTime 32
+Estimated_size SkelCfgDebug 32
+Estimated_size SkelCfgLength 32
+Estimated_size SkelCfgWidth 32
+Estimated_size SkelCmdAbort 24
+Estimated_size SkelCmdGo 32
+Estimated_size SkelCmdHalt 24
+Estimated_size SkelCmdInit 24
+Estimated_size SkelCmdShutdown 24
+Estimated_size SkelSet 176
+Estimated_size SkelStat 144
+Estimated_size MAXIMUM 176
+*/
+/*
* NML/CMS Format function : skel_format
* Automatically generated by NML CodeGen Java Applet.
-* on Tue Oct 18 17:39:25 EDT 2005
+* on Mon Oct 15 16:53:22 EDT 2007
*/
int
skel_format (NMLTYPE type, void *buffer, CMS * cms)
@@ -217,62 +231,81 @@
}
/*
-* NML/CMS Update function for SkelCfgWidth
+* NML/CMS Update function for SkelCfgCycleTime
* Automatically generated by NML CodeGen Java Applet.
-* on Tue Oct 18 17:39:25 EDT 2005
+* on Mon Oct 15 16:53:22 EDT 2007
*/
void
-SkelCfgWidth::update (CMS * cms)
+SkelCfgCycleTime::update (CMS * cms)
{
- cms->beginClass ("SkelCfgWidth", "RCS_CMD_MSG");
+ cms->beginClass ("SkelCfgCycleTime", "RCS_CMD_MSG");
RCS_CMD_MSG::update_cmd_msg_base (cms);
- cms->update_with_name ("width", width);
+ cms->update_with_name ("cycleTime", cycleTime);
- cms->endClass ("SkelCfgWidth", "RCS_CMD_MSG");
+ cms->endClass ("SkelCfgCycleTime", "RCS_CMD_MSG");
}
/*
-* NML/CMS Update function for SkelCmdShutdown
+* NML/CMS Update function for SkelCfgDebug
* Automatically generated by NML CodeGen Java Applet.
-* on Tue Oct 18 17:39:25 EDT 2005
+* on Mon Oct 15 16:53:22 EDT 2007
*/
void
-SkelCmdShutdown::update (CMS * cms)
+SkelCfgDebug::update (CMS * cms)
{
- cms->beginClass ("SkelCmdShutdown", "RCS_CMD_MSG");
+ cms->beginClass ("SkelCfgDebug", "RCS_CMD_MSG");
RCS_CMD_MSG::update_cmd_msg_base (cms);
+ cms->update_with_name ("debug", debug);
- cms->endClass ("SkelCmdShutdown", "RCS_CMD_MSG");
+ cms->endClass ("SkelCfgDebug", "RCS_CMD_MSG");
}
/*
-* NML/CMS Update function for SkelCfgCycleTime
+* NML/CMS Update function for SkelCfgLength
* Automatically generated by NML CodeGen Java Applet.
-* on Tue Oct 18 17:39:25 EDT 2005
+* on Mon Oct 15 16:53:22 EDT 2007
*/
void
-SkelCfgCycleTime::update (CMS * cms)
+SkelCfgLength::update (CMS * cms)
{
- cms->beginClass ("SkelCfgCycleTime", "RCS_CMD_MSG");
+ cms->beginClass ("SkelCfgLength", "RCS_CMD_MSG");
RCS_CMD_MSG::update_cmd_msg_base (cms);
- cms->update_with_name ("cycleTime", cycleTime);
+ cms->update_with_name ("length", length);
- cms->endClass ("SkelCfgCycleTime", "RCS_CMD_MSG");
+ cms->endClass ("SkelCfgLength", "RCS_CMD_MSG");
}
/*
+* NML/CMS Update function for SkelCfgWidth
+* Automatically generated by NML CodeGen Java Applet.
+* on Mon Oct 15 16:53:22 EDT 2007
+*/
+void
+SkelCfgWidth::update (CMS * cms)
+{
+
+ cms->beginClass ("SkelCfgWidth", "RCS_CMD_MSG");
+ RCS_CMD_MSG::update_cmd_msg_base (cms);
+ cms->update_with_name ("width", width);
+
+ cms->endClass ("SkelCfgWidth", "RCS_CMD_MSG");
+
+}
+
+
+/*
* NML/CMS Update function for SkelCmdAbort
* Automatically generated by NML CodeGen Java Applet.
-* on Tue Oct 18 17:39:25 EDT 2005
+* on Mon Oct 15 16:53:22 EDT 2007
*/
void
SkelCmdAbort::update (CMS * cms)
@@ -289,7 +322,7 @@
/*
* NML/CMS Update function for SkelCmdGo
* Automatically generated by NML CodeGen Java Applet.
-* on Tue Oct 18 17:39:25 EDT 2005
+* on Mon Oct 15 16:53:22 EDT 2007
*/
void
SkelCmdGo::update (CMS * cms)
@@ -307,7 +340,7 @@
/*
* NML/CMS Update function for SkelCmdHalt
* Automatically generated by NML CodeGen Java Applet.
-* on Tue Oct 18 17:39:25 EDT 2005
+* on Mon Oct 15 16:53:22 EDT 2007
*/
void
SkelCmdHalt::update (CMS * cms)
@@ -322,47 +355,9 @@
/*
-* NML/CMS Update function for SkelCfgLength
-* Automatically generated by NML CodeGen Java Applet.
-* on Tue Oct 18 17:39:25 EDT 2005
-*/
-void
-SkelCfgLength::update (CMS * cms)
-{
-
- cms->beginClass ("SkelCfgLength", "RCS_CMD_MSG");
- RCS_CMD_MSG::update_cmd_msg_base (cms);
- cms->update_with_name ("length", length);
-
- cms->endClass ("SkelCfgLength", "RCS_CMD_MSG");
-
-}
-
-
-/*
-* NML/CMS Update function for SkelStat
-* Automatically generated by NML CodeGen Java Applet.
-* on Tue Oct 18 17:39:25 EDT 2005
-*/
-void
-SkelStat::update (CMS * cms)
-{
-
- cms->beginClass ("SkelStat", "RCS_STAT_MSG");
-
- RCS_STAT_MSG::update_stat_msg_base (cms);
- cms->update_with_name ("where", where);
- cms->update_with_name ("here", here);
-
- cms->endClass ("SkelStat", "RCS_STAT_MSG");
-
-}
-
-
-/*
* NML/CMS Update function for SkelCmdInit
* Automatically generated by NML CodeGen Java Applet.
-* on Tue Oct 18 17:39:25 EDT 2005
+* on Mon Oct 15 16:53:22 EDT 2007
*/
void
SkelCmdInit::update (CMS * cms)
@@ -377,19 +372,18 @@
/*
-* NML/CMS Update function for SkelCfgDebug
+* NML/CMS Update function for SkelCmdShutdown
* Automatically generated by NML CodeGen Java Applet.
-* on Tue Oct 18 17:39:25 EDT 2005
+* on Mon Oct 15 16:53:22 EDT 2007
*/
void
-SkelCfgDebug::update (CMS * cms)
+SkelCmdShutdown::update (CMS * cms)
{
- cms->beginClass ("SkelCfgDebug", "RCS_CMD_MSG");
+ cms->beginClass ("SkelCmdShutdown", "RCS_CMD_MSG");
RCS_CMD_MSG::update_cmd_msg_base (cms);
- cms->update_with_name ("debug", debug);
- cms->endClass ("SkelCfgDebug", "RCS_CMD_MSG");
+ cms->endClass ("SkelCmdShutdown", "RCS_CMD_MSG");
}
@@ -397,7 +391,7 @@
/*
* NML/CMS Update function for SkelSet
* Automatically generated by NML CodeGen Java Applet.
-* on Tue Oct 18 17:39:25 EDT 2005
+* on Mon Oct 15 16:53:22 EDT 2007
*/
void
SkelSet::update (CMS * cms)
@@ -416,3 +410,23 @@
cms->endClass ("SkelSet", "RCS_STAT_MSG");
}
+
+
+/*
+* NML/CMS Update function for SkelStat
+* Automatically generated by NML CodeGen Java Applet.
+* on Mon Oct 15 16:53:22 EDT 2007
+*/
+void
+SkelStat::update (CMS * cms)
+{
+
+ cms->beginClass ("SkelStat", "RCS_STAT_MSG");
+
+ RCS_STAT_MSG::update_stat_msg_base (cms);
+ cms->update_with_name ("where", where);
+ cms->update_with_name ("here", here);
+
+ cms->endClass ("SkelStat", "RCS_STAT_MSG");
+
+}
Modified: trunk/moast-bth/moast/src/tools/linkMonitor/linkMon.cc
===================================================================
--- trunk/moast-bth/moast/src/tools/linkMonitor/linkMon.cc 2007-10-29 18:21:44 UTC (rev 861)
+++ trunk/moast-bth/moast/src/tools/linkMonitor/linkMon.cc 2007-10-29 20:24:46 UTC (rev 862)
@@ -26,8 +26,8 @@
\code CVS Status:
$Author: dr_steveb $
- $Revision: 1.3 $
- $Date: 2007/09/18 20:03:49 $
+ $Revision: 1.5 $
+ $Date: 2007/09/20 20:14:57 $
\endcode
\author Stephen Balakirsky
@@ -56,6 +56,9 @@
static LinkMonStat linkMonStat;
static int done = 0;
static int server_fd = -1; // connection to unreal
+static int *server_listen = NULL; // connection to message sent over comm server
+static int *server_listenCon = NULL; // connection to message sent over comm server
+
static float radioCutOff = 60;
#define DEFAULT_NML_FILE "../etc/moast.nml"
@@ -65,6 +68,7 @@
static char *USARSimHostName;
static int USARSimPort = 5874;
+static int USARSimListenPortBase = 7795;
// global variables and defines for searcher
#define DIJK_MAP_SIZE MAX_TRAFFIC_VEHICLES * MAX_TRAFFIC_VEHICLES
@@ -142,6 +146,94 @@
return sockfd;
}
+/*
+ * Accept a new TCP connection and write a message about who was
+ * accepted to the system log.
+ */
+
+static int tcp_accept(int listen)
+{
+ struct sockaddr_storage ss;
+ socklen_t ss_len = sizeof(ss);
+ char host[NI_MAXHOST];
+ char serv[NI_MAXSERV];
+ int n, fd;
+
+ fd = accept(listen, (struct sockaddr *) &ss, &ss_len);
+ if (fd == -1)
+ {
+ printf("accept failed: %s\n", strerror(errno));
+ return -1;
+ }
+
+ n = getnameinfo((struct sockaddr *) &ss, ss_len,
+ host, sizeof(host), serv, sizeof(serv),
+ NI_NUMERICHOST);
+ if (n)
+ {
+ printf("getnameinfo failed: %s\n", gai_strerror(n));
+ }
+ else
+ {
+ printf("connection from %s:%s\n", host, serv);
+ }
+ return fd;
+}
+
+static int tcp_listen(char *port)
+{
+ struct addrinfo hints, *ai_list, *ai;
+ int n, fd = 0, on = 1;
+
+ memset(&hints, 0, sizeof(hints));
+ hints.ai_flags = AI_PASSIVE;
+ hints.ai_family = AF_UNSPEC;
+ hints.ai_socktype = SOCK_STREAM;
+
+ n = getaddrinfo(NULL, port, &hints, &ai_list);
+ if (n)
+ {
+ fprintf(stderr, "%s: getaddrinfo failed: %s\n",
+ THIS_PROCESS, gai_strerror(n));
+ return -1;
+ }
+
+ for (ai = ai_list; ai; ai = ai->ai_next)
+ {
+ fd = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol);
+ if (fd < 0)
+ {
+ continue;
+ }
+
+ setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on));
+ if (bind(fd, ai->ai_addr, ai->ai_addrlen) == 0)
+ {
+ break;
+ }
+ close(fd);
+ }
+
+ freeaddrinfo(ai_list);
+
+ if (ai == NULL)
+ {
+ fprintf(stderr, "%s: bind failed for port %s\n",
+ THIS_PROCESS, port);
+ return -1;
+ }
+
+ if (listen(fd, 42) < 0)
+ {
+ fprintf(stderr, "%s: listen failed: %s\n",
+ THIS_PROCESS, strerror(errno));
+ close(fd);
+ return -1;
+ }
+
+ return fd;
+}
+
/////////////////////////////////////////////////////////////////////////////////////////
//// graph search routines from Tom Kramer and me
/////////////////////////////////////////////////////////////////////////////////////////
@@ -269,12 +361,40 @@
/////////////////////////////////////////////////////////////////////////////////////////
bool wssWrite( char *robotName, char *msg )
{
+ int nn;
+ char mess[128];
+
printf( "Need to send message %s to robot %s\n", msg, robotName );
+ sprintf( mess, "SEND %d %s;", strlen(msg)+2, msg );
+ printf( "Sending message: %s\n", mess );
+ nn = write(server_listenCon[atoi(robotName)], mess, strlen(mess+1));
+ // nn = write(server_listenCon[0], mess, strlen(mess+1));
+ if( nn == -1 )
+ {
+ printf("Error form write\n");
+ return false;
+ }
+ // close the connection ?
+
return true;
}
char *wssRead()
{
+ int nn;
+ char msg[128];
+ int trash = 0;
+
+ if( trash == 0 )
+ nn = recv(server_listenCon[1], msg, sizeof(msg)-1, MSG_DONTWAIT);
+ else
+ nn = read(server_listenCon[1], msg, sizeof(msg)-1);
+ if( nn == -1 )
+ {
+ return NULL;
+ }
+ msg[nn] = '\0';
+ printf( "Message received: %s\n", msg );
return NULL;
}
@@ -318,6 +438,8 @@
state_next(stat, S10);
return;
}
+ server_listen = new int[set->robotInfo_length];
+ server_listenCon = new int[set->robotInfo_length];
status_next(stat, RCS_EXEC);
state_next(stat, S1);
}
@@ -326,8 +448,7 @@
//Register with the Server
for( i=0; i<set->robotInfo_length; i++ )
{
- stat->admin_state = ADMIN_INITIALIZED;
- sprintf( msg, "register %s %s;", set->robotInfo[i].robotName, "129.6.33.142" );
+ sprintf( msg, "REGISTER %s %s;", set->robotInfo[i].robotName, "129.6.33.142" );
printf("%s: sending %d length message: %s\n", THIS_PROCESS, strlen(msg), msg);
nn = send( server_fd, msg, strlen(msg)+1, 0);
// send( server_fd, "\r\n", 2, 0);
@@ -356,11 +477,113 @@
}
else
{
+ state_next(stat, S2);
+
+ }
+ }
+ }
+ if(state_match(stat, S2))
+ {
+ //set robots to listen REMEMBER that robot 0 is the comm station
+ for( i=0; i<set->robotInfo_length; i++ )
+ //for( i=0; i<2; i++ )
+ {
+ sprintf( msg, "listen %s %d;", set->robotInfo[i].robotName, USARSimListenPortBase + 3*i);
+ printf("%s: sending %d length message: %s\n", THIS_PROCESS, strlen(msg), msg);
+ nn = send( server_fd, msg, strlen(msg)+1, 0);
+ // send( server_fd, "\r\n", 2, 0);
+ if (nn != (int)strlen(msg)+1)
+ {
+ fprintf(stderr, "Write Failed\n");
+ status_next(stat, RCS_ERROR);
state_next(stat, S10);
+ return;
+ }
+ //Wait for OK response
+ nn = recv(server_fd, msg, sizeof(msg)-1, 0);
+ if( nn == -1 )
+ {
+ status_next(stat, RCS_ERROR);
+ state_next(stat, S10);
+ return;
+ }
+ msg[nn] = '\0';
+ printf("%s: message received:%s\n", THIS_PROCESS, msg);
+ if(msg[0]!='O' || msg[1]!='K')
+ {
+ status_next(stat, RCS_ERROR);
+ state_next(stat, S10);
+ return;
+ }
+ else
+ {
+ state_next(stat, S3);
+ stat->admin_state = ADMIN_INITIALIZED;
+ }
+ }
+ }
+ if(state_match(stat, S3))
+ {
+ //open connections between robot 0 and the other robots
+ for( i=1; i<set->robotInfo_length; i++ )
+ // for( i=1; i<2; i++ )
+ {
+ sprintf( msg, "OPEN %s %d %s %d;", set->robotInfo[0].robotName, USARSimListenPortBase,
+ set->robotInfo[i].robotName, USARSimListenPortBase+3*i);
+ printf("%s: sending %d length message: %s\n", THIS_PROCESS, strlen(msg), msg);
+ nn = send( server_fd, msg, strlen(msg)+1, 0);
+ // send( server_fd, "\r\n", 2, 0);
+ if (nn != (int)strlen(msg)+1)
+ {
+ fprintf(stderr, "Write Failed\n");
+ status_next(stat, RCS_ERROR);
+ state_next(stat, S10);
+ return;
}
+ //Wait for OK response
+ nn = recv(server_fd, msg, sizeof(msg)-1, 0);
+ if( nn == -1 )
+ {
+ status_next(stat, RCS_ERROR);
+ state_next(stat, S10);
+ return;
+ }
+ msg[nn] = '\0';
+ printf("%s: message received:%s\n", THIS_PROCESS, msg);
+ if(msg[0]!='O' || msg[1]!='K')
+ {
+ status_next(stat, RCS_ERROR);
+ state_next(stat, S10);
+ return;
+ }
+ else
+ {
+ state_next(stat, S4);
+ }
}
}
+ if(state_match(stat, S4))
+ {
+ for( i=0; i<set->robotInfo_length; i++ )
+ {
+ printf( "Listening to listen port %d\n", USARSimListenPortBase+i*3 );
+ sprintf(msg, "%d", USARSimListenPortBase+i*3 );
+ if( -1 == (server_listen[i] = tcp_listen(msg)))
+ {
+ status_next(stat, RCS_ERROR);
+ state_next(stat, S10);
+ }
+ else if (-1 == (server_listenCon[i] = tcp_accept(server_listen[i])))
+ {
+ status_next(stat, RCS_ERROR);
+ state_next(stat, S10);
+ }
+ state_next(stat, S5);
+ }
+ stat->admin_state = ADMIN_INITIALIZED;
+ printf( "Done with listening\n" );
+ }
// else wait here
else
state_default(stat);
@@ -410,6 +633,10 @@
void doCmdWrite( LinkMonStat * stat, LinkMonCmdWrite *cmdIn )
{
+ int nn;
+ char msg[128];
+ TrafficCtrlSet * set = (TrafficCtrlSet *) trafficCtrlSetNml->get_address();
+
if(state_match(stat, NEW_COMMAND))
{
state_new(stat);
@@ -420,14 +647,54 @@
}
else
{
- state_next(stat, S0);
+ state_next(stat, S1);
status_next(stat, RCS_EXEC);
}
}
+ // state NOT USED at this time
if(state_match(stat, S0))
{
+ //open connections between robot 0 and the other robots
+ sprintf( msg, "open %s %d %s %d;",
+ set->robotInfo[atoi(cmdIn->robotToWrite)].robotName,
+ USARSimListenPortBase+3*atoi(cmdIn->robotToWrite),
+ set->robotInfo[0].robotName, USARSimListenPortBase);
+ printf("%s: sending %d length message: %s\n", THIS_PROCESS, strlen(msg), msg);
+ nn = send( server_fd, msg, strlen(msg)+1, 0);
+ // send( server_fd, "\r\n", 2, 0);
+ if (nn != (int)strlen(msg)+1)
+ {
+ fprintf(stderr, "Write Failed\n");
+ status_next(stat, RCS_ERROR);
+ state_next(stat, S10);
+ return;
+ }
+ //Wait for OK response
+ nn = recv(server_fd, msg, sizeof(msg)-1, 0);
+ if( nn == -1 )
+ {
+ status_next(stat, RCS_ERROR);
+ state_next(stat, S10);
+ return;
+ }
+ msg[nn] = '\0';
+ printf("%s: message received:%s\n", THIS_PROCESS, msg);
+ if(msg[0]!='O' || msg[1]!='K')
+ {
+ status_next(stat, RCS_ERROR);
+ state_next(stat, S10);
+ return;
+ }
+ else
+ {
+ state_next(stat, S1);
+ }
+ }
+ if(state_match(stat, S1))
+ {
+
if( !wssWrite(cmdIn->robotToWrite, cmdIn->msg ) )
{
status_next(stat, RCS_ERROR);
@@ -439,6 +706,42 @@
state_next(stat, S2);
}
}
+ if(state_match(stat, S2))
+ {
+ //open connections between robot 0 and the other robots
+ sprintf( msg, "CLOSE;");
+ printf("%s: sending %d length message: %s\n", THIS_PROCESS, strlen(msg), msg);
+ nn = send( server_fd, msg, strlen(msg)+1, 0);
+ // send( server_fd, "\r\n", 2, 0);
+ if (nn != (int)strlen(msg)+1)
+ {
+ fprintf(stderr, "Write Failed\n");
+ status_next(stat, RCS_ERROR);
+ state_next(stat, S10);
+ return;
+ }
+ //Wait for OK response
+ nn = recv(server_fd, msg, sizeof(msg)-1, 0);
+ if( nn == -1 )
+ {
+ status_next(stat, RCS_ERROR);
+ state_next(stat, S10);
+ return;
+ }
+ msg[nn] = '\0';
+ printf("%s: message received:%s\n", THIS_PROCESS, msg);
+ if(msg[0]!='O' || msg[1]!='K')
+ {
+ status_next(stat, RCS_ERROR);
+ state_next(stat, S10);
+ return;
+ }
+ else
+ {
+ state_next(stat, S4);
+ }
+ }
+
}
void doCmdMonitor( LinkMonStat * stat )
@@ -722,8 +1025,11 @@
// read buffers
cmd_type = linkMonCmdNml->read();
trafficCtrlSetNml->read();
- if( (msgIn=wssRead()) != NULL )
- printf( "Message received: %s\n", msgIn );
+ if( linkMonStat.admin_state == ADMIN_INITIALIZED )
+ {
+ if( (msgIn=wssRead()) != NULL )
+ printf( "Message received: %s\n", msgIn );
+ }
// listen to all robots over USARSim commServer
@@ -803,6 +1109,12 @@
Modification history:
$Log: linkMon.cc,v $
+ Revision 1.5 2007/09/20 20:14:57 dr_steveb
+ Better, but still not working
+
+ Revision 1.4 2007/09/20 19:26:37 dr_steveb
+ Added additional link connection code for USARSim. Not working yet!
+
Revision 1.3 2007/09/18 20:03:49 dr_steveb
Added write command
Modified: trunk/moast-bth/moast/src/tools/robojoy.cc
===================================================================
--- trunk/moast-bth/moast/src/tools/robojoy.cc 2007-10-29 18:21:44 UTC (rev 861)
+++ trunk/moast-bth/moast/src/tools/robojoy.cc 2007-10-29 20:24:46 UTC (rev 862)
@@ -16,8 +16,8 @@
\code CVS Status:
$Author: dr_steveb $
- $Revision: 1.27 $
- $Date: 2007/08/29 18:28:34 $
+ $Revision: 1.28 $
+ $Date: 2007/10/17 17:18:43 $
\endcode
\author Fred Proctor
@@ -52,7 +52,7 @@
/*
define INHIBIT_PRISMATIC to inhibit the motion of prismatic joints
during Cartesian motion
- */
+*/
#define INHIBIT_PRISMATIC
/*
@@ -133,9 +133,103 @@
static double flipperIncr[SERVO_MOB_JA_FLIPPER_TYPE_MAX];
static ServoMobJAFlipperSet flipperSet[SERVO_MOB_JA_FLIPPER_TYPE_MAX];
bool needToWrite;
+bool userScale;
+bool button8Down;
#define MY_NAME_BASE "joystick"
+
+bool openMission( int vehID )
+{
+ bool retval = true;
+ char *configNml = getenv("CONFIG_NML");
+ char chanName[MOAST_NML_BUFFER_NAME_LEN];
+ char myName[MOAST_NML_BUFFER_NAME_LEN];
+ ServoMisJACmdInit servoMisInit;
+ int gotIt;
+ double end;
+
+ sprintf(myName, "%s", MY_NAME_BASE);
+
+ if (NULL == configNml) {
+ configNml = "../etc/moast.nml";
+ }
+
+ if( servoMisJACmdNml != NULL )
+ {
+ delete servoMisJACmdNml;
+ servoMisJACmdNml = NULL;
+ }
+ 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;
+ }
+ }
+
+ if( servoMisJAStatNml != NULL )
+ {
+ delete servoMisJAStatNml;
+ servoMisJAStatNml = NULL;
+ }
+ if (NULL == servoMisJAStatNml) {
+ sprintf(chanName, "%s%d_%d", SERVO_MIS_JA_STAT_NAME, vehID, compID);
@@ Diff output truncated at 60000 characters. @@
More information about the TeamTalk-developers
mailing list