////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboTreasure::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
model_ = _model;
world_ = _model->GetWorld();
link_ = _model->GetLink();
link_name_ = link_->GetName();
namespace_.clear();
vel_x = vel_y = vel_yaw = 0;
static_object_ = false;
last_time_ = world_->GetSimTime();
terminated_ = false;
// load parameters from sdf
if (_sdf->HasElement("robotNamespace")) namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
if (_sdf->HasElement("bodyName") && _sdf->GetElement("bodyName")->GetValue())
{
link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
link_ = _model->GetLink(link_name_);
}
if (_sdf->HasElement("staticObject") && _sdf->GetElement("staticObject")->GetValue())
{
static_object_ = _sdf->GetElement("staticObject")->Get<std::string>() == "true"?true:false;
}
// boost::random::mt19937 rng;
boost::random::random_device rng;
boost::random::uniform_int_distribution<> x_rand(-40, 40);
boost::random::uniform_int_distribution<> y_rand(-20, 20);
double x = x_rand(rng);
double y = y_rand(rng);
model_->SetLinkWorldPose(math::Pose(x, y, 0.2, 0, 0, 0), link_);
if (!link)
{
ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str());
return;
}
// Make sure the ROS node for Gazebo has already been initialized
if (!ros::isInitialized())
{
ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
<< "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
return;
}
node_handle_ = new ros::NodeHandle(namespace_);
pub_score_ = node_handle_->advertise<std_msgs::String>("score", 1, true); // set latch true
ros::NodeHandle param_handle(*node_handle_, "controller");
update_connection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboTreasure::Update, this));
}
// --------------------------------------------------------------------------------------------------
// create a random number (integer), between two values, including the boundaries(!)
// So, y_rand_between(0,4) can result in these values: 0,1,2,3,4
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// example usage:
// int random;
// random = y_rand_between(0, 10); // generate a random number between 0 and 10.
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
int y_rand_between(int lowerbound, int upperbound)
{
int roll;
if ((lowerbound>upperbound) || ((upperbound - lowerbound) == 0))
{
lr_error_message("y_rand called with nonsensical arguments. (lowerbound should be less than upperbound)");
return -1; // hmmm. is this correct?
}
roll = y_rand() % ((upperbound + 1 - lowerbound)) + lowerbound;
return roll;
}
/*! \brief Choose a flow from a list of flows.
\param [in] flow_list An array of y_flow structs, each describing a specific choice in a clickpath.
\param [in] flow_count The number of flows in the list.
\return a pointer to a randomly chosen flow, or NULL if somehow the weights don't add up. (flow list corrupt, flow_count incorrect, etc.)
\see y_flow_list.c, y_exec_flow()
\note The flow_count argument should exactly match the number of flows in the array or the script might blow up with MEMORY_ACCESS_VIOLATION errors.
\author Floris Kraak
*/
y_flow* y_choose_flow(y_flow flow_list[], int flow_count)
{
int i, lowerbound = 0;
int cursor = 0;
int roll = y_rand() % y_calc_flow_weight_total(flow_list, flow_count);
lr_log_message("Roll: %d", roll);
for(i=0; i < flow_count; i++)
{
y_flow *flow = &flow_list[i];
int weight = flow->weight;
cursor += weight;
lr_log_message("weight cursor: %d", cursor);
if(roll < cursor)
{
return flow;
}
}
return NULL;
}
// --------------------------------------------------------------------------------------------------
// Usage: y_random_string_buffer("parameterNameWhichGetsTheRandomValue", minimumlength, maximumlength);
//
// ex. randomString("randomFeedback", 100, 200); will fill a parameter with between 100 and 200 characters.
// to use the random value, use the parameter name provided.
// To make it look like real sentences, this function inserts spaces at random.
// The "words" will be minimal 1 character long, and max. 8 characters.
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// example usage:
// y_random_string_buffer("par1", 10,10); // creates a string of exactly 10 characters and saves it into string {par1}
// y_random_string_buffer("par1", 5,10); // creates a string of min 5 and max 10 char and saves it into string {par1}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
y_random_string_buffer(const char *parameter, int minimumLength, int maximumLength)
{
const char characterSet[] = {
'a','b','c','d','e','f','g','h','i','j','k','l','m',
'n','o','p','q','r','s','t','u','v','w','x','y','z',
'A','B','C','D','E','F','G','H','I','J','K','L','M',
'N','O','P','Q','R','S','T','U','V','W','X','Y','Z'};
/*
'1','2','3','4','5','6','7','8','9','0','?','!','-',
',','.',';',
'`','~','@','#','$','%','^','&','*','(',')',
'=','_','+','[',']','{','}','|',':','/',
'<','>', };
*/
char *buffer;
int charSetSize = 52; // length of the above array
int length = 0;
int max = 0;
char randomNumber;
int lettersInWord;
// error checks - lots of code that saves us headaches later
if( minimumLength < 1 ) {
lr_error_message( "minimumLength smaller than 0 (%d)", minimumLength );
}
else if( maximumLength < 1 ) {
lr_error_message( "maximumLength smaller than 0 (%d)", maximumLength );
}
else if( maximumLength > (1024 * 1024) ) {
lr_error_message( "maximumLength too big (%d)", maximumLength );
}
else if( maximumLength < minimumLength ) {
lr_error_message( "minimumLength (%d) bigger than maximumLength (%d)", minimumLength, maximumLength );
}
else if(maximumLength > minimumLength) {
// Not an error
max = (y_rand() % (maximumLength-minimumLength)) + minimumLength;
}
else if(maximumLength == minimumLength) {
// Not an error either
max = maximumLength;
}
else {
lr_error_message("This can never happen. If we reach this point it's a bug.");
}
// if we got an error
if( max == 0 )
{
lr_set_transaction_status(LR_FAIL);
// Not sure this is the right exit code that we want to use here, but ok.
lr_exit(LR_EXIT_ITERATION_AND_CONTINUE, LR_FAIL);
}
// get memory for the buffer
buffer = (char *)y_mem_alloc( max +1 );
// note: if this fails y_mem_alloc() aborts the script, so no error handling needed.
while( length < max )
{
lettersInWord = ((y_rand() % 8) + 2);
while( lettersInWord-- && (length < (max)) )
{
randomNumber = (char) (y_rand() % charSetSize);
buffer[length++] = characterSet[randomNumber];
}
if(length!=max)
{
buffer[length++] = ' ';
}
}
buffer[length++] = '\0';
lr_save_string(buffer, parameter);
free(buffer);
return 0;
}
/*!
This function uses a given set of characters to create words, separated by spaces.
The words are minimal \e minWordLength characters long, and maximum \e minWordLength characters.
The total length of the line is minimal \e minimumLength and maimum \e maximumLength long.
\b Example:
\code
// Generates a string of minimal 3 and max 20 characters,
// with words of minimal 1 and maximal 3 charactes.
// Chooses only characters a, c, d or d.
y_random_string_buffer_core("uitvoer", 3,20, 1, 3, "abcd");
// Generates some sort of mock morse-code of exactly 30 characters.
// with words of minimal 1 and maximal 3 charactes.
// Chooses only characters a, c, d or d.
y_random_string_buffer_core("uitvoer", 3,20, 1, 3, "abcd"); // could result in "ccc db dac c"
\endcode
@param[out] parameter Name of the LR-parameter in which the result is stored
@param[in] minimumLength Minumum length of the string
@param[in] maximumLength Maximum length of the string
@param[in] minWordLength Minimum length of the words within the string
@param[in] maxWordLength Minimum length of the words within the string
@param[in] characterSet The string is build from this string of characters
\return void
\author Floris Kraak / Raymond de Jongh
\sa y_random_number_buffer
\sa y_random_string_buffer_curses
\sa y_random_string_buffer
\sa y_random_string_buffer_hex
*/
void y_random_string_buffer_core(const char *parameter, int minimumLength, int maximumLength,
int minWordLength, int maxWordLength, char *characterSet)
{
char *buffer;
int charSetSize; // length of the characterSet
int length = 0;
int max = -1;
char randomNumber;
int lettersInWord;
charSetSize=strlen(characterSet);
//lr_message("minimumLength %d -- maximumLength %d -- minWordLength %d -- maxWordLength %d",
// minimumLength, maximumLength, minWordLength, maxWordLength);
// error checks - lots of code that saves us headaches later
if( minimumLength < 0 ) {
lr_error_message( "minimumLength less than 0 (%d)", minimumLength );
}
else if( maximumLength < 1 ) {
lr_error_message( "maximumLength less than 1 (%d)", maximumLength );
}
else if( maximumLength > (1024 * 1024) ) {
lr_error_message( "maximumLength too big (%d)", maximumLength );
}
else if( maximumLength < minimumLength ) {
lr_error_message( "minimumLength (%d) bigger than maximumLength (%d)", minimumLength, maximumLength );
}
else if(maximumLength > minimumLength) {
// Not an error
max = y_rand_between(minimumLength, maximumLength);
lr_log_message("Max: %d", max);
}
else if(maximumLength == minimumLength) {
// Not an error either
max = maximumLength;
}
else {
lr_error_message("This can never happen. If we reach this point it's a bug.");
}
// if we got an error
if( max < 0 )
{
lr_set_transaction_status(LR_FAIL);
lr_exit(LR_EXIT_ITERATION_AND_CONTINUE, LR_FAIL);
}
// get memory for the buffer
buffer = (char *)y_mem_alloc( max +1 );
// note: if this fails y_mem_alloc() aborts the script, so no error handling needed.
while( length < max )
{
// lr_message("Length: %d max: %d", length, max);
// lettersInWord = ((y_rand() % 8) + 2);
if( maxWordLength == 0 )
{
lettersInWord = maximumLength;
}
else
{
lettersInWord = y_rand_between(minWordLength, maxWordLength);
if( lettersInWord < 0 )
{
lr_error_message( "y_rand_between() returned an errorcode (%d)", lettersInWord );
lr_set_transaction_status(LR_FAIL);
lr_exit(LR_EXIT_ITERATION_AND_CONTINUE, LR_FAIL);
}
}
while( lettersInWord-- && (length < (max)) )
{
randomNumber = (char) (y_rand() % charSetSize);
buffer[length++] = characterSet[randomNumber];
}
if( maxWordLength != 0 )
{
if( length < max -1 )
{
buffer[length++] = ' ';
}
}
}
buffer[max] = '\0';
lr_save_string(buffer, parameter);
free(buffer);
}
