void SSearchTest::monkeyTest(char *params)
{
// ook!
UErrorCode status = U_ZERO_ERROR;
//UCollator *coll = ucol_open(NULL, &status);
UCollator *coll = ucol_openFromShortString("S1", FALSE, NULL, &status);
if (U_FAILURE(status)) {
errcheckln(status, "Failed to create collator in MonkeyTest! - %s", u_errorName(status));
return;
}
CollData *monkeyData = new CollData(coll, status);
USet *expansions = uset_openEmpty();
USet *contractions = uset_openEmpty();
ucol_getContractionsAndExpansions(coll, contractions, expansions, FALSE, &status);
U_STRING_DECL(letter_pattern, "[[:letter:]-[:ideographic:]-[:hangul:]]", 39);
U_STRING_INIT(letter_pattern, "[[:letter:]-[:ideographic:]-[:hangul:]]", 39);
USet *letters = uset_openPattern(letter_pattern, 39, &status);
SetMonkey letterMonkey(letters);
StringSetMonkey contractionMonkey(contractions, coll, monkeyData);
StringSetMonkey expansionMonkey(expansions, coll, monkeyData);
UnicodeString testCase;
UnicodeString alternate;
UnicodeString pattern, altPattern;
UnicodeString prefix, altPrefix;
UnicodeString suffix, altSuffix;
Monkey *monkeys[] = {
&letterMonkey,
&contractionMonkey,
&expansionMonkey,
&contractionMonkey,
&expansionMonkey,
&contractionMonkey,
&expansionMonkey,
&contractionMonkey,
&expansionMonkey};
int32_t monkeyCount = sizeof(monkeys) / sizeof(monkeys[0]);
// int32_t nonMatchCount = 0;
UCollationStrength strengths[] = {UCOL_PRIMARY, UCOL_SECONDARY, UCOL_TERTIARY};
const char *strengthNames[] = {"primary", "secondary", "tertiary"};
int32_t strengthCount = sizeof(strengths) / sizeof(strengths[0]);
int32_t loopCount = quick? 1000 : 10000;
int32_t firstStrength = 0;
int32_t lastStrength = strengthCount - 1; //*/ 0;
if (params != NULL) {
#if !UCONFIG_NO_REGULAR_EXPRESSIONS
UnicodeString p(params);
loopCount = getIntParam("loop", p, loopCount);
m_seed = getIntParam("seed", p, m_seed);
RegexMatcher m(" *strength *= *(primary|secondary|tertiary) *", p, 0, status);
if (m.find()) {
UnicodeString breakType = m.group(1, status);
for (int32_t s = 0; s < strengthCount; s += 1) {
if (breakType == strengthNames[s]) {
firstStrength = lastStrength = s;
break;
}
}
m.reset();
p = m.replaceFirst("", status);
}
if (RegexMatcher("\\S", p, 0, status).find()) {
// Each option is stripped out of the option string as it is processed.
// All options have been checked. The option string should have been completely emptied..
char buf[100];
p.extract(buf, sizeof(buf), NULL, status);
buf[sizeof(buf)-1] = 0;
errln("Unrecognized or extra parameter: %s\n", buf);
return;
}
#else
infoln("SSearchTest built with UCONFIG_NO_REGULAR_EXPRESSIONS: ignoring parameters.");
#endif
}
for(int32_t s = firstStrength; s <= lastStrength; s += 1) {
int32_t notFoundCount = 0;
logln("Setting strength to %s.", strengthNames[s]);
ucol_setStrength(coll, strengths[s]);
// TODO: try alternate prefix and suffix too?
// TODO: alterntaes are only equal at primary strength. Is this OK?
for(int32_t t = 0; t < loopCount; t += 1) {
uint32_t seed = m_seed;
// int32_t nmc = 0;
generateTestCase(coll, monkeys, monkeyCount, pattern, altPattern);
generateTestCase(coll, monkeys, monkeyCount, prefix, altPrefix);
generateTestCase(coll, monkeys, monkeyCount, suffix, altSuffix);
// pattern
notFoundCount += monkeyTestCase(coll, pattern, pattern, altPattern, "pattern", strengthNames[s], seed);
testCase.remove();
testCase.append(prefix);
testCase.append(/*alt*/pattern);
// prefix + pattern
notFoundCount += monkeyTestCase(coll, testCase, pattern, altPattern, "prefix + pattern", strengthNames[s], seed);
testCase.append(suffix);
// prefix + pattern + suffix
notFoundCount += monkeyTestCase(coll, testCase, pattern, altPattern, "prefix + pattern + suffix", strengthNames[s], seed);
testCase.remove();
testCase.append(pattern);
testCase.append(suffix);
// pattern + suffix
notFoundCount += monkeyTestCase(coll, testCase, pattern, altPattern, "pattern + suffix", strengthNames[s], seed);
}
logln("For strength %s the not found count is %d.", strengthNames[s], notFoundCount);
}
uset_close(contractions);
uset_close(expansions);
uset_close(letters);
delete monkeyData;
ucol_close(coll);
}
int main(int argc, char** argv) {
srand( time(0));
ros::init(argc, argv, "ramp_planner");
ros::NodeHandle handle;
// Load ros parameters and obstacle transforms
//loadParameters(handle);
//loadObstacleTF();
num_obs = 3;
ros::Rate r(100);
/*
* Data to collect
*/
std::vector<bool> reached_goal;
std::vector<bool> bestTrajec_fe;
std::vector<double> time_left;
std::vector<bool> stuck_in_ic;
std::vector<bool> ic_occurred;
std::vector<TestCaseTwo> test_cases;
ros::Timer ob_trj_timer;
ob_trj_timer.stop();
int num_tests = 42;
ob_delay.push_back(2);
ob_delay.push_back(2);
ob_delay.push_back(4);
// Make an ObstacleList Publisher
pub_obs = handle.advertise<ramp_msgs::ObstacleList>("obstacles", 1);
ros::ServiceClient trj_gen = handle.serviceClient<ramp_msgs::TrajectorySrv>("trajectory_generator");
ros::Subscriber sub_bestT = handle.subscribe("bestTrajec", 1, bestTrajCb);
ros::Subscriber sub_imminent_collision_ = handle.subscribe("imminent_collision", 1, imminentCollisionCb);
ros::Subscriber sub_update = handle.subscribe("update", 1, updateCb);
std::ofstream f_reached;
f_reached.open("/home/sterlingm/ros_workspace/src/ramp/ramp_planner/system_level_data/8/reached_goal.txt",
std::ios::out | std::ios::app | std::ios::binary);
std::ofstream f_feasible;
f_feasible.open("/home/sterlingm/ros_workspace/src/ramp/ramp_planner/system_level_data/8/feasible.txt", std::ios::out
| std::ios::app | std::ios::binary);
std::ofstream f_time_left;
f_time_left.open("/home/sterlingm/ros_workspace/src/ramp/ramp_planner/system_level_data/8/time_left.txt",
std::ios::out | std::ios::app | std::ios::binary);
std::ofstream f_ic_stuck;
f_ic_stuck.open("/home/sterlingm/ros_workspace/src/ramp/ramp_planner/system_level_data/8/ic_stuck.txt", std::ios::out
| std::ios::app | std::ios::binary);
std::ofstream f_ic_occurred;
f_ic_occurred.open("/home/sterlingm/ros_workspace/src/ramp/ramp_planner/system_level_data/8/ic_occurred.txt",
std::ios::out | std::ios::app | std::ios::binary);
// Set flag signifying that the next test case is not ready
ros::param::set("/ramp/tc_generated", false);
ros::Duration d_history(1);
ros::Duration d_test_case_thresh(20);
for(int i=0;i<num_tests;i++)
{
MotionState initial_state;
//my_planner.randomMS(initial_state);
/*
*
* Generate a test case
*
*/
/*
* Generate the test case
*/
ABTC abtc;
/*
* Create test case where all obstacles stop, move, stop for 1 second each
*/
for(int i_ob=0;i_ob<3;i_ob++)
{
abtc.moving[i_ob] = 0;
abtc.moving[i_ob+3] = 1;
abtc.moving[i_ob+6] = 0;
abtc.times[i_ob] = 1;
abtc.times[i_ob+3] = 1;
abtc.times[i_ob+6] = 1;
}
generateObInfoGrid(initial_state);
/*
* Get test data for the abtc
*/
TestCaseTwo tc = generateTestCase(initial_state, num_obs);
tc.abtc = abtc;
/*
* Get trajectories for each obstacle
*/
ramp_msgs::TrajectorySrv tr_srv;
for(int i=0;i<tc.obs.size();i++)
{
ramp_msgs::TrajectoryRequest tr;
tf::Transform tf;
tf.setOrigin( tf::Vector3(0,0,0) );
tf.setRotation( tf::createQuaternionFromYaw(0) );
MotionType mt = my_planner.findMotionType(tc.obs[i].msg);
ramp_msgs::Path p = my_planner.getObstaclePath(tc.obs[i].msg, mt);
tr.path = p;
tr.type = PREDICTION;
tr_srv.request.reqs.push_back(tr);
}
// Call trajectory generator
if(trj_gen.call(tr_srv))
{
for(int i=0;i<tr_srv.response.resps.size();i++)
{
ROS_INFO("Traj: %s", utility.toString(tr_srv.response.resps[i].trajectory).c_str());
tc.ob_trjs.push_back(tr_srv.response.resps[i].trajectory);
}
}
else
{
ROS_ERROR("Error in getting obstacle trajectories");
}
/*
* Get static version of obstacles
*/
ramp_msgs::ObstacleList obs_stat;
for(int i=0;i<tc.obs.size();i++)
{
obs_stat.obstacles.push_back(getStaticOb(tc.obs[i].msg));
}
ROS_INFO("Generate: obs_stat.size(): %i", (int)obs_stat.obstacles.size());
IC_occur = false;
// Set flag signifying that the next test case is ready
ros::param::set("/ramp/tc_generated", true);
ROS_INFO("Generate: Waiting for planner to prepare");
/*
* Wait for planner to be ready to start test case
*/
bool start_tc = false;
while(!start_tc)
{
handle.getParam("/ramp/ready_tc", start_tc);
r.sleep();
ros::spinOnce();
}
ROS_INFO("Generate: Planner ready, publishing static obstacles");
// Publish static obstacles
pub_obs.publish(obs_stat);
// Wait for 1 second
d_history.sleep();
ROS_INFO("Generate: Done sleeping for 1 second");
// Publish dynamic obstacles
//pub_obs.publish(tc.ob_list);
tc.t_begin = ros::Time::now();
// Create timer to continuously publish obstacle information
ob_trj_timer = handle.createTimer(ros::Duration(1./20.), boost::bind(pubObTrj, _1, tc));
/*
* Wait for planner to run for time threshold
*/
while(start_tc)
{
handle.getParam("ramp/ready_tc", start_tc);
//ROS_INFO("generate_test_case: start_tc: %s", start_tc ? "True" : "False");
r.sleep();
ros::spinOnce();
}
ros::Duration elasped = ros::Time::now() - tc.t_begin;
ROS_INFO("generate_test_case: Test case completed");
// Set flag signifying that the next test case is not ready
ros::param::set("/ramp/tc_generated", false);
ob_trj_timer.stop();
/*
* Collect data
*/
MotionState goal;
goal.msg_.positions.push_back(2);
goal.msg_.positions.push_back(2);
goal.msg_.positions.push_back(PI/4.f);
double dist = utility.positionDistance( goal.msg_.positions, latestUpdate.msg_.positions );
//if(elasped.toSec()+0.01 < d_test_case_thresh.toSec())
//if(bestTrajec.trajectory.points.size() < 3)
if(dist < 0.2)
{
reached_goal.push_back(true);
f_reached<<true<<std::endl;
}
else
{
reached_goal.push_back(false);
f_reached<<false<<std::endl;
}
if(bestTrajec.feasible)
{
bestTrajec_fe.push_back(true);
time_left.push_back( bestTrajec.trajectory.points[ bestTrajec.trajectory.points.size()-1
].time_from_start.toSec());
f_feasible<<true<<std::endl;
f_time_left<<bestTrajec.trajectory.points[ bestTrajec.trajectory.points.size()-1
].time_from_start.toSec()<<std::endl;
}
else
{
bestTrajec_fe.push_back(false);
f_feasible<<false<<std::endl;
//time_left.push_back(9999);
}
if(IC_current)
{
stuck_in_ic.push_back(true);
f_ic_stuck<<true<<std::endl;
}
else
{
stuck_in_ic.push_back(false);
f_ic_stuck<<false<<std::endl;
}
if(IC_occur)
{
ic_occurred.push_back(true);
f_ic_occurred<<true<<std::endl;
}
else
{
ic_occurred.push_back(false);
f_ic_occurred<<false<<std::endl;
}
bestTrajec_at_end.push_back(bestTrajec);
test_cases.push_back(tc);
} // end for
f_reached.close();
f_feasible.close();
f_time_left.close();
f_ic_stuck.close();
f_ic_occurred.close();
std::cout<<"\n\nExiting Normally\n";
ros::shutdown();
return 0;
}
