void
GeneratorBusinessH::doOperation(IR__::OperationDef_ptr operation)
{
out << "\n//\n// " << operation->id() << "\n//\n";
std::string name = mapName(operation);
out << "virtual " << map_return_type(operation->result_def()) << " " << name << "(";
//
// parameters
//
IR__::ParDescriptionSeq* pards = operation->params();
for(CORBA::ULong i = pards->length(); i > 0; i--)
{
if(i < pards->length())
{
out << ", ";
}
IR__::ParameterDescription pardescr = (*pards)[i - 1];
if (pardescr.mode == IR__::PARAM_IN) {
out << map_in_parameter_type (pardescr.type_def) << " " << mapName(string(pardescr.name));
}
if (pardescr.mode == IR__::PARAM_OUT) {
out << map_out_parameter_type (pardescr.type_def) << " " << mapName(string(pardescr.name));
}
if (pardescr.mode == IR__::PARAM_INOUT) {
out << map_inout_parameter_type (pardescr.type_def) << " " << mapName(string(pardescr.name));
}
};
out << ")\n";
out << " throw(CORBA::SystemException";
handleException(operation);
out << ");\n";
}
void
GeneratorBusinessC::doFinder(IR__::FinderDef_ptr finder)
{
std::string finder_name = mapName(finder);
out << "::Components::EnterpriseComponent_ptr\n";
out << class_name_ << "::" << finder_name << "(";
//
// parameters
//
IR__::ParDescriptionSeq* pards = finder->params();
for(CORBA::ULong i = pards->length(); i > 0; i--)
{
if(i < pards->length())
{
out << ", ";
}
IR__::ParameterDescription pardescr = (*pards)[i - 1];
out << map_in_parameter_type (pardescr.type_def) << " " << mapName(string(pardescr.name));
}
out << ")\n";
out << " throw(CORBA::SystemException";
handleException(finder);
out << ")\n{\n";
out.insertUserSection(class_name_ + "::" + finder->name(), 0);
out << "}\n\n\n";
}
void
GeneratorPersistenceH::genPersistentOperation(IR__::OperationDef_ptr operation, IR__::ComponentDef_ptr component, bool isFinder)
{
std::string strDummy = "";
out << component->name() << "Persistence* " << mapName(operation) << "(";
//
// parameters
//
strDummy = mapName(operation) + component->name();
int iLength = strDummy.length() + 14;
strDummy = "";
strDummy.append(iLength, ' ');
if(!isFinder)
{
out << "Pid* pid,\n";
out << strDummy;
out << "ShortPid* shortPid,\n";
out << strDummy;
}
IR__::ParDescriptionSeq* pards = operation->params();
for( CORBA::ULong i=pards->length(); i > 0; i--)
{
IR__::ParameterDescription pardescr = (*pards)[i - 1];
if (pardescr.mode == IR__::PARAM_IN) {
out << map_in_parameter_type (pardescr.type_def) << " " << string(pardescr.name);
}
if (pardescr.mode == IR__::PARAM_OUT) {
out << map_out_parameter_type (pardescr.type_def) << " " << string(pardescr.name);
}
if (pardescr.mode == IR__::PARAM_INOUT) {
out << map_inout_parameter_type (pardescr.type_def) << " " << string(pardescr.name);
}
if( (i-1)!=0 ) out << ",\n" << strDummy;
}
if(isFinder)
{
out << ")\n";
out.indent();
out << "throw(CosPersistentState::NotFound);\n";
out.unindent();
}
else
out << ");\n";
}
/*
* escape each name with _cxx_ if keyword
*/
std::string
CPPBase::mapFullName(IR__::Contained_ptr obj)
{
std::string name;
name = obj->absolute_name();
std::string::size_type pos = 0;
std::string::size_type begin;
std::string::size_type end;
while(pos != std::string::npos)
{
begin = pos;
end = std::string::npos;
pos = name.find("::", pos);
if(pos != std::string::npos)
{
pos += 2;
begin = pos;
pos = name.find("::", pos);
end = pos;
}
name.replace(begin, end - begin, mapName(name.substr(begin, end - begin)));
}
return name;
}
void
GeneratorServantH::genOperation(IR__::OperationDef_ptr operation, IR__::IDLType_ptr ret_type)
{
out << "\n//\n// " << operation->id() << "\n//\n";
out << map_return_type(ret_type) << " " << mapName(operation) << "(";
//
// parameters
//
IR__::ParDescriptionSeq* pards = operation->params();
CORBA::ULong i;
for( i= pards->length(); i > 0; i--)
{
if(i < pards->length()) { out << ", "; }
IR__::ParameterDescription pardescr = (*pards)[i - 1];
if (pardescr.mode == IR__::PARAM_IN) {
out << map_in_parameter_type (pardescr.type_def) << " " << string(pardescr.name);
};
if (pardescr.mode == IR__::PARAM_OUT) {
out << map_out_parameter_type (pardescr.type_def) << " " << string(pardescr.name);
};
if (pardescr.mode == IR__::PARAM_INOUT) {
out << map_inout_parameter_type (pardescr.type_def) << " " << string(pardescr.name);
};
};
out << ")\n";
out.indent();
out << "throw(CORBA::SystemException";
handleException(operation);
out << ");\n";
out.unindent();
}
void
GeneratorPersistenceH::genAttributeWithOtherType(IR__::AttributeDef_ptr attribute, CORBA::TCKind att_type_kind)
{
std::string attribute_name = mapName(attribute);
IR__::IDLType_var attr_type = attribute->type_def();
switch ( attr_type->type()->kind() )
{
case CORBA::tk_string:
case CORBA::tk_wstring:
if(bAbstract_) out << "virtual ";
out << map_psdl_return_type(attr_type, false) << " " << attribute_name << "(";
out << map_psdl_parameter_type(attr_type, false) << " param)";
bAbstract_ ? out << " = 0;\n" : out << ";\n";
}
if(bAbstract_) out << "virtual ";
out << map_psdl_return_type(attr_type, false) << " " << attribute_name << "(";
out << "CosPersistentState::ForUpdate fu)";
bAbstract_ ? out << " = 0;\n" : out << ";\n";
if(bAbstract_) out << "virtual ";
out << "void " << attribute_name << "(";
out << map_psdl_parameter_type(attr_type, true) << " param)";
bAbstract_ ? out << " = 0;\n" : out << ";\n";
}
void
GeneratorPersistenceH::genAttributeWithNomalType(IR__::AttributeDef_ptr attribute, CORBA::TCKind att_type_kind)
{
std::string attribute_name = mapName(attribute);
IR__::IDLType_var attr_type = attribute->type_def();
switch ( attr_type->type()->kind() )
{
case CORBA::tk_string:
case CORBA::tk_wstring:
if(bAbstract_) out << "virtual ";
out << map_psdl_return_type(attr_type, false) << " " << attribute_name << "(";
out << map_psdl_parameter_type(attr_type, false) << " param)";
bAbstract_ ? out << " = 0;\n" : out << ";\n";
}
if(bAbstract_) out << "virtual ";
out << "void " << attribute_name << "(";
out << map_psdl_parameter_type(attr_type, true) << " param)";
bAbstract_ ? out << " = 0;\n" : out << ";\n";
// the fourth operation only for string and wstring
if(attr_type->type()->kind()==CORBA::tk_string)
{
if(bAbstract_) out << "virtual ";
out << "void " << attribute_name << "(CORBA::String_var& param)";
bAbstract_ ? out << " = 0;\n" : out << ";\n";
}
else if(attr_type->type()->kind()==CORBA::tk_wstring)
{
if(bAbstract_) out << "virtual ";
out << "void " << attribute_name << "(CORBA::WString_var& param)";
bAbstract_ ? out << " = 0;\n" : out << ";\n";
}
}
void
GeneratorValuetypesC::doValueMember(IR__::ValueMemberDef_ptr member)
{
IR__::IDLType_ptr type = member->type_def();
std::string member_name = mapName(member);
out << member_name << "( v." << member_name << "() );\n";
}
void
GeneratorValuetypesC::doAttribute(IR__::AttributeDef_ptr attribute)
{
std::string attribute_name = mapName(attribute);
//
// not read only
//
if(attribute->mode() == IR__::ATTR_NORMAL)
{
out << "void\n";
out << class_name_ << "::" << attribute_name << "(";
out << map_in_parameter_type(attribute->type_def()) << " param)\n";
out << " throw(CORBA::SystemException";
handleException(attribute);
out << ")\n{\n";
out.insertUserSection(class_name_ + "::_" + attribute->name(), 0);
out << "}\n\n\n";
}
out << map_return_type(attribute->type_def()) << "\n";
out << class_name_ << "::" << attribute_name << "()\n";
out << " throw(CORBA::SystemException";
handleException(attribute);
out << ")\n{\n";
out.insertUserSection(class_name_ + "::" + attribute->name(), 0);
out << "}\n\n\n";
}
void
GeneratorPersistenceH::genOperation(IR__::OperationDef_ptr operation, IR__::IDLType_ptr ret_type)
{
out << "\n//\n// " << operation->id() << "\n//\n";
if(bAbstract_) out << "virtual ";
out << map_psdl_return_type(ret_type, false) << " " << mapName(operation) << "(";
// parameters
IR__::ParDescriptionSeq_var pards = operation->params();
for( CORBA::ULong i=0; i<pards->length(); i++)
{
IR__::ParameterDescription pardescr = (*pards)[i];
if (pardescr.mode == IR__::PARAM_IN) {
out << map_in_parameter_type (pardescr.type_def) << " " << std::string(pardescr.name);
};
if (pardescr.mode == IR__::PARAM_OUT) {
out << map_out_parameter_type (pardescr.type_def) << " " << std::string(pardescr.name);
};
if (pardescr.mode == IR__::PARAM_INOUT) {
out << map_inout_parameter_type (pardescr.type_def) << " " << std::string(pardescr.name);
};
if((i+1)!=pards->length()) { out << ", "; }
};
bAbstract_ ? out << ") = 0;\n" : out << ");\n";
}
void
GeneratorBusinessH::doConsumes(IR__::ConsumesDef_ptr consumes, IR__::ComponentDef_ptr component)
{
out << "\n//\n// " << consumes->id() << "\n//\n";
// push operation
out << "void push_" << mapName(consumes->event()) << "(" << mapFullName(consumes->event()) << "* ev)\n";
out << " throw (CORBA::SystemException);\n\n";
}
// Subscribe to models of interest. Currently, we find and subscribe
// to the first 'laser' model and the first 'position' model. Returns
// 0 on success (both models subscribed), -1 otherwise.
//
// Eventually, we should provide a general way to map stage models onto ROS
// topics, similar to Player .cfg files.
int
StageNode::SubscribeModels()
{
n_.setParam("/use_sim_time", true);
for (size_t r = 0; r < this->positionmodels.size(); r++)
{
if(this->lasermodels[r])
{
this->lasermodels[r]->Subscribe();
}
else
{
ROS_ERROR("no laser");
return(-1);
}
if(this->positionmodels[r])
{
this->positionmodels[r]->Subscribe();
}
else
{
ROS_ERROR("no position");
return(-1);
}
if(this->blobfndrmodels[r])
{
this->blobfndrmodels[r]->Subscribe();
}
else
{
ROS_ERROR("no blobfinder");
return(-1);
}
laser_pubs_.push_back(n_.advertise<sensor_msgs::LaserScan>(mapName(BASE_SCAN,r), 10));
odom_pubs_.push_back(n_.advertise<nav_msgs::Odometry>(mapName(ODOM,r), 10));
ground_truth_pubs_.push_back(n_.advertise<nav_msgs::Odometry>(mapName(BASE_POSE_GROUND_TRUTH,r), 10));
blob_pubs_.push_back(n_.advertise<my_stage::Color>(mapName(COLOR_BLOB,r), 10));
cmdvel_subs_.push_back(n_.subscribe<geometry_msgs::Twist>(mapName(CMD_VEL,r), 10, boost::bind(&StageNode::cmdvelReceived, this, r, _1)));
}
clock_pub_ = n_.advertise<rosgraph_msgs::Clock>("/clock",10);
return(0);
}
void SingleSceneFrame::initContent()
{
auto frame = Sprite::createWithSpriteFrameName("chapter_frame.png");
addChild(frame);
string mapName("ui/chapter/miniMaps/mini_");
mapName = mapName + Value(sceneId).asString() + ".png";
auto map = Sprite::create(mapName);
map->setPosition(Vec2(12,5));
addChild(map);
}
void
GeneratorPersistenceH::genKey(IR__::OperationDef_ptr operation, IR__::IDLType_ptr ret_type, bool bRef)
{
if(!bRef) out << "\n//\n// " << operation->id() << "\n//\n";
if(bAbstract_) out << "virtual ";
char* szReturnType = map_psdl_return_type(ret_type, false);
//since the definition of a abstract stoage type is not yet supported,
//we have to replcace the "*" with "Ref" for operation find_by_ref_...
if(bRef)
{
char* pdest = strstr( szReturnType, "*" );
if( pdest != NULL )
{
memset(pdest, '\0', 1);
strcat(szReturnType, "Ref");
}
}
out << szReturnType;
bRef ? out << " find_ref_by_" : out << " find_by_";
out << mapName(operation) << "(";
// parameters
IR__::ParDescriptionSeq* pards = operation->params();
for( CORBA::ULong i=0; i<pards->length(); i++)
{
IR__::ParameterDescription pardescr = (*pards)[i];
if (pardescr.mode == IR__::PARAM_IN) {
out << map_in_parameter_type (pardescr.type_def) << " " << std::string(pardescr.name);
};
if (pardescr.mode == IR__::PARAM_OUT) {
out << map_out_parameter_type (pardescr.type_def) << " " << std::string(pardescr.name);
};
if (pardescr.mode == IR__::PARAM_INOUT) {
out << map_inout_parameter_type (pardescr.type_def) << " " << std::string(pardescr.name);
};
if((i+1)!=pards->length()) { out << ", "; }
};
out << ")";
if(!bRef)
{
out << "\n";
out.indent();
out << "throw(CosPersistentState::NotFound";
handleException(operation);
out << ")";
out.unindent();
}
bAbstract_ ? out << " = 0;\n" : out << ";\n";
}
void
GeneratorBusinessH::doFinder(IR__::FinderDef_ptr finder)
{
out << "\n//\n// " << finder->id() << "\n//\n";
out << "virtual ::Components::EnterpriseComponent_ptr " << mapName(finder) << "(";
// Parameter of the operation
IR__::ParDescriptionSeq* pards = finder->params();
for(CORBA::ULong i = pards->length(); i > 0; i--)
{
if(i < pards->length())
{
out << ", ";
}
IR__::ParameterDescription pardescr = (*pards)[i - 1];
out << map_in_parameter_type (pardescr.type_def) << " " << mapName(string(pardescr.name));
};
out << ")\n";
out << " throw(CORBA::SystemException";
handleException(finder);
out << ");\n";
}
void
GeneratorPersistenceC::doPSSKey(IR__::PSSKeyDef_ptr psskey)
{
out << "\n//\n// " << psskey->id() << "\n//\n";
out << "virtual ::Components::EnterpriseComponent_ptr " << mapName(psskey) << "(";
// Parameter of the operation
IR__::ParDescriptionSeq* pards = psskey->params();
for(CORBA::ULong i = pards->length(); i > 0; i--)
{
if(i < pards->length())
{
out << ", ";
}
IR__::ParameterDescription pardescr = (*pards)[i - 1];
out << map_in_parameter_type (pardescr.type_def) << " " << mapName(string(pardescr.name));
};
out << ")\n";
out << " throw(CORBA::SystemException";
handleException(psskey);
out << ");\n";
}
void
GeneratorValuetypesC::doOperation(IR__::OperationDef_ptr operation)
{
std::string operation_name = mapName(operation);
out << map_return_type(operation->result_def()) << "\n";
out << class_name_ << "::" << operation_name << "(";
//
// parameters
//
IR__::ParDescriptionSeq* pards = operation->params();
for(CORBA::ULong i = pards->length(); i > 0; i--)
{
if(i < pards->length())
{
out << ", ";
}
IR__::ParameterDescription pardescr = (*pards)[i - 1];
if (pardescr.mode == IR__::PARAM_IN) {
out << map_in_parameter_type (pardescr.type_def) << " " << mapName(string(pardescr.name));
}
if (pardescr.mode == IR__::PARAM_OUT) {
out << map_out_parameter_type (pardescr.type_def) << " " << mapName(string(pardescr.name));
}
if (pardescr.mode == IR__::PARAM_INOUT) {
out << map_inout_parameter_type (pardescr.type_def) << " " << mapName(string(pardescr.name));
}
}
out << ")\n";
out << " throw(CORBA::SystemException";
handleException(operation);
out << ")\n{\n";
out.insertUserSection(class_name_ + "::" + operation->name(), 0);
out << "}\n\n\n";
}
void ManageAction::gameStart(Packet * packet)
{
auto& data = packet->getData();
int id = data[0];
std::string tmpPort("");
std::string mapName("");
for (unsigned int i=2; i<data.size() - 5; ++i)
mapName.push_back(data[i]);
for (unsigned long i=data.size() - 4; i<data.size(); ++i)
{
tmpPort += data[i];
}
_win.getLevel().setCurrentFocusMap(mapName);
_win.getWindowGame()->loadNewGame(mapName);
_win.play = true;
_win._port = static_cast<int>(std::stoi(tmpPort));
_win._playerId = id;
}
bool
GeneratorValuetypesC::generateMemberParam(IR__::ValueDef_ptr value, bool comma )
{
bool set_comma = comma;
//
// inheritance
//
// base value
IR__::ValueDef_var base = value->base_value();
if(! CORBA::is_nil(base))
{
set_comma = generateMemberParam( base, set_comma );
}
// abstract base values
IR__::ValueDefSeq_var abstr = value->abstract_base_values();
CORBA::ULong i;
for( i = 0; i < abstr->length(); i++ )
{
set_comma = generateMemberParam( abstr[i], set_comma );
};
//
// members
//
IR__::ContainedSeq_var contained_seq = value->contents(CORBA__::dk_ValueMember, false);
CORBA::ULong len = contained_seq->length();
for( i = 0; i < len; i++ )
{
if(set_comma)
{
out << ", ";
}
IR__::ValueMemberDef_var a_member = IR__::ValueMemberDef::_narrow(((*contained_seq)[i]));
out << map_in_parameter_type (a_member->type_def()) << " ";
out << mapName(string(a_member->name()));
set_comma = true;
};
return set_comma;
}
void
GeneratorPersistenceH::genFactory(IR__::OperationDef_ptr operation, IR__::IDLType_ptr ret_type)
{
out << "\n//\n// " << operation->id() << "\n//\n";
int iLength = 0;
std::string strDummy = "";
stringstream strDisplay;
if(bAbstract_)
strDisplay << "virtual ";
strDisplay << map_psdl_return_type(ret_type, false) << " " << mapName(operation) << "(";
out << strDisplay.str();
iLength = strDisplay.str().length();
strDummy.append(iLength, ' ');
out << "Pid* pid,\n";
out << strDummy.c_str();
out << "ShortPid* shortPid,\n";
// parameters
IR__::ParDescriptionSeq_var pards = operation->params();
for( CORBA::ULong i=0; i<pards->length(); i++)
{
out << strDummy.c_str();
IR__::ParameterDescription pardescr = (*pards)[i];
if (pardescr.mode == IR__::PARAM_IN) {
out << map_in_parameter_type (pardescr.type_def) << " " << std::string(pardescr.name);
};
if (pardescr.mode == IR__::PARAM_OUT) {
out << map_out_parameter_type (pardescr.type_def) << " " << std::string(pardescr.name);
};
if (pardescr.mode == IR__::PARAM_INOUT) {
out << map_inout_parameter_type (pardescr.type_def) << " " << std::string(pardescr.name);
};
if((i+1)!=pards->length()) { out << ",\n"; }
};
bAbstract_ ? out << ") = 0;\n" : out << ");\n";
}
void
GeneratorPersistenceH::genCreateOperation(IR__::StorageHomeDef_ptr storagehome, bool bRef)
{
int iLength = 0;
std::string strDummy = "";
char* szDisplay = map_psdl_return_type(storagehome->managed_storagetype(), false);
bRef ? iLength = strlen(szDisplay) + 11 : iLength = strlen(szDisplay) + 9;
if(bRef)
{
char* pdest = strstr( szDisplay, "*" );
if( pdest != NULL )
{
memset(pdest, '\0', 1);
strcat(szDisplay, "Ref");
}
}
out << szDisplay << " _create(";
out << "Pid* pid,\n";
strDummy.append(iLength, ' ');
out << strDummy.c_str();
out << "ShortPid* shortPid,\n";
IR__::AttributeDefSeq state_members;
storagehome->managed_storagetype()->get_state_members(state_members, CORBA__::dk_Create);
CORBA::ULong ulLen = state_members.length();
for(CORBA::ULong i=0; i<ulLen; i++)
{
IR__::AttributeDef_var attribute = IR__::AttributeDef::_narrow(state_members[i]);
out << strDummy.c_str() << map_in_parameter_type(attribute->type_def()) << " " << mapName(attribute);
if( (i+1)!=ulLen )
out << ",\n";
}
if(bRef)
out << ",\n" << strDummy.c_str() << "CosPersistentState::YieldRef yr";
out << ");\n\n";
}
void
GeneratorPersistenceH::doAttribute(IR__::AttributeDef_ptr attribute)
{
out << "\n//\n// " << attribute->id() << "\n//\n";
std::string attribute_name = mapName(attribute);
IR__::IDLType_var attr_type = attribute->type_def();
// read only
if(bAbstract_) out << "virtual ";
out << map_psdl_return_type(attr_type, true) << " " << attribute_name << "() const";
bAbstract_ ? out << " = 0;\n" : out << ";\n";
// not read only
if(attribute->mode() == IR__::ATTR_NORMAL)
{
CORBA::TCKind att_type_kind = attr_type->type()->kind();
switch ( att_type_kind )
{
case CORBA::tk_short:
case CORBA::tk_long:
case CORBA::tk_longlong:
case CORBA::tk_ushort:
case CORBA::tk_ulong:
case CORBA::tk_ulonglong:
case CORBA::tk_float:
case CORBA::tk_double:
case CORBA::tk_longdouble:
case CORBA::tk_boolean:
case CORBA::tk_char:
case CORBA::tk_wchar:
case CORBA::tk_octet:
case CORBA::tk_string:
case CORBA::tk_wstring:
genAttributeWithNomalType(attribute, att_type_kind);
break;
default:
genAttributeWithOtherType(attribute, att_type_kind);
}
}
}
bool
CPPBase::open_module(Printer& out, IR__::Contained* cur_cont, std::string prefix)
{
IR__::Container_ptr a_container = cur_cont->defined_in();
if(a_container->def_kind() == CORBA__::dk_Module)
{
IR__::ModuleDef_var a_module = IR__::ModuleDef::_narrow(a_container);
if(! this->open_module(out, a_module, prefix))
{
// the prefix must be written first
out << "namespace " << prefix;
}
else
{
out << "namespace ";
}
out << mapName(a_module) << " {\n";
out.indent();
return true;
}
return false;
};
void
GeneratorBusinessH::doAttribute(IR__::AttributeDef_ptr attribute)
{
out << "\n//\n// " << attribute->id() << "\n//\n";
std::string name = mapName(attribute);
//
// not read only
//
if(attribute->mode() == IR__::ATTR_NORMAL)
{
out << "void " << name << "(";
out << map_in_parameter_type(attribute->type_def()) << " param)\n";
out << " throw(CORBA::SystemException";
handleException(attribute);
out << ");\n";
}
out << map_return_type(attribute->type_def()) << " " << name << "()\n";
out << " throw(CORBA::SystemException";
handleException(attribute);
out << ");\n";
}
void
GeneratorPersistenceH::genMemberVariable(IR__::ComponentDef_ptr component)
{
IR__::AttributeDefSeq state_members;
component->get_state_members(state_members, CORBA__::dk_Variable);
CORBA::ULong ulLen = state_members.length();
out << "private:\n\n";
out.indent();
for(CORBA::ULong i=0; i<ulLen; i++)
{
IR__::AttributeDef_var attribute = IR__::AttributeDef::_narrow(state_members[i]);
std::string attribute_name = mapName(attribute);
if( strcmp("char*", (const char*)(map_attribute_type(attribute->type_def())))==0 )
out << "std::string ";
else
out << map_attribute_type(attribute->type_def()) << " ";
out << attribute_name << "_;\n";
}
out.unindent();
}
void
StageNode::WorldCallback()
{
boost::mutex::scoped_lock lock(msg_lock);
this->sim_time.fromSec(world->SimTimeNow() / 1e6);
// We're not allowed to publish clock==0, because it used as a special
// value in parts of ROS, #4027.
if(this->sim_time.sec == 0 && this->sim_time.nsec == 0)
{
ROS_DEBUG("Skipping initial simulation step, to avoid publishing clock==0");
return;
}
// TODO make this only affect one robot if necessary
if((this->base_watchdog_timeout.toSec() > 0.0) &&
((this->sim_time - this->base_last_cmd) >= this->base_watchdog_timeout))
{
for (size_t r = 0; r < this->positionmodels.size(); r++)
this->positionmodels[r]->SetSpeed(0.0, 0.0, 0.0);
}
// Get latest laser data
for (size_t r = 0; r < this->lasermodels.size(); r++)
{
const std::vector<Stg::ModelRanger::Sensor>& sensors = this->lasermodels[r]->GetSensors();
if( sensors.size() > 1 )
ROS_WARN( "ROS Stage currently supports rangers with 1 sensor only." );
// for now we access only the zeroth sensor of the ranger - good
// enough for most laser models that have a single beam origin
const Stg::ModelRanger::Sensor& s = sensors[0];
if( s.ranges.size() )
{
// Translate into ROS message format and publish
this->laserMsgs[r].angle_min = -s.fov/2.0;
this->laserMsgs[r].angle_max = +s.fov/2.0;
this->laserMsgs[r].angle_increment = s.fov/(double)(s.sample_count-1);
this->laserMsgs[r].range_min = s.range.min;
this->laserMsgs[r].range_max = s.range.max;
this->laserMsgs[r].ranges.resize(s.ranges.size());
this->laserMsgs[r].intensities.resize(s.intensities.size());
for(unsigned int i=0; i<s.ranges.size(); i++)
{
this->laserMsgs[r].ranges[i] = s.ranges[i];
this->laserMsgs[r].intensities[i] = (uint8_t)s.intensities[i];
}
this->laserMsgs[r].header.frame_id = mapName("base_laser_link", r);
this->laserMsgs[r].header.stamp = sim_time;
this->laser_pubs_[r].publish(this->laserMsgs[r]);
}
// Also publish the base->base_laser_link Tx. This could eventually move
// into being retrieved from the param server as a static Tx.
Stg::Pose lp = this->lasermodels[r]->GetPose();
tf::Quaternion laserQ;
laserQ.setRPY(0.0, 0.0, lp.a);
tf::Transform txLaser = tf::Transform(laserQ,
tf::Point(lp.x, lp.y, 0.15));
tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
mapName("base_link", r),
mapName("base_laser_link", r)));
// Send the identity transform between base_footprint and base_link
tf::Transform txIdentity(tf::createIdentityQuaternion(),
tf::Point(0, 0, 0));
tf.sendTransform(tf::StampedTransform(txIdentity,
sim_time,
mapName("base_footprint", r),
mapName("base_link", r)));
// Get latest odometry data
// Translate into ROS message format and publish
this->odomMsgs[r].pose.pose.position.x = this->positionmodels[r]->est_pose.x;
this->odomMsgs[r].pose.pose.position.y = this->positionmodels[r]->est_pose.y;
this->odomMsgs[r].pose.pose.orientation = tf::createQuaternionMsgFromYaw(this->positionmodels[r]->est_pose.a);
Stg::Velocity v = this->positionmodels[r]->GetVelocity();
this->odomMsgs[r].twist.twist.linear.x = v.x;
this->odomMsgs[r].twist.twist.linear.y = v.y;
this->odomMsgs[r].twist.twist.angular.z = v.a;
//@todo Publish stall on a separate topic when one becomes available
//this->odomMsgs[r].stall = this->positionmodels[r]->Stall();
//
this->odomMsgs[r].header.frame_id = mapName("odom", r);
this->odomMsgs[r].header.stamp = sim_time;
this->odom_pubs_[r].publish(this->odomMsgs[r]);
// broadcast odometry transform
tf::Quaternion odomQ;
tf::quaternionMsgToTF(odomMsgs[r].pose.pose.orientation, odomQ);
tf::Transform txOdom(odomQ,
tf::Point(odomMsgs[r].pose.pose.position.x,
odomMsgs[r].pose.pose.position.y, 0.0));
tf.sendTransform(tf::StampedTransform(txOdom, sim_time,
mapName("odom", r),
mapName("base_footprint", r)));
// Also publish the ground truth pose and velocity
Stg::Pose gpose = this->positionmodels[r]->GetGlobalPose();
Stg::Velocity gvel = this->positionmodels[r]->GetGlobalVelocity();
// Note that we correct for Stage's screwed-up coord system.
tf::Quaternion q_gpose;
q_gpose.setRPY(0.0, 0.0, gpose.a-M_PI/2.0);
tf::Transform gt(q_gpose, tf::Point(gpose.y, -gpose.x, 0.0));
tf::Quaternion q_gvel;
q_gvel.setRPY(0.0, 0.0, gvel.a-M_PI/2.0);
tf::Transform gv(q_gvel, tf::Point(gvel.y, -gvel.x, 0.0));
this->groundTruthMsgs[r].pose.pose.position.x = gt.getOrigin().x();
this->groundTruthMsgs[r].pose.pose.position.y = gt.getOrigin().y();
this->groundTruthMsgs[r].pose.pose.position.z = gt.getOrigin().z();
this->groundTruthMsgs[r].pose.pose.orientation.x = gt.getRotation().x();
this->groundTruthMsgs[r].pose.pose.orientation.y = gt.getRotation().y();
this->groundTruthMsgs[r].pose.pose.orientation.z = gt.getRotation().z();
this->groundTruthMsgs[r].pose.pose.orientation.w = gt.getRotation().w();
this->groundTruthMsgs[r].twist.twist.linear.x = gv.getOrigin().x();
this->groundTruthMsgs[r].twist.twist.linear.y = gv.getOrigin().y();
//this->groundTruthMsgs[r].twist.twist.angular.z = tf::getYaw(gv.getRotation());
//this->groundTruthMsgs[r].twist.twist.linear.x = gvel.x;
//this->groundTruthMsgs[r].twist.twist.linear.y = gvel.y;
this->groundTruthMsgs[r].twist.twist.angular.z = gvel.a;
this->groundTruthMsgs[r].header.frame_id = mapName("odom", r);
this->groundTruthMsgs[r].header.stamp = sim_time;
this->ground_truth_pubs_[r].publish(this->groundTruthMsgs[r]);
}
this->clockMsg.clock = sim_time;
this->clock_pub_.publish(this->clockMsg);
}
void
GeneratorBusinessH::doComposition(CIDL::CompositionDef_ptr composition)
{
CORBA::ULong i ;
composition_ = CIDL::CompositionDef::_duplicate(composition);
filename_ = "";
//
// determine whether defined in module (needed for namespace opening and closing)
//
IR__::Contained_ptr module_def = 0;
string id = composition->id();
string::size_type pos = id.find_last_of("/");
if(pos != string::npos)
{
id.replace(pos, string::npos, ":1.0");
module_def = repository_->lookup_id(id.c_str());
filename_ = getAbsoluteName(module_def, "_");
filename_.append("_");
}
filename_.append(composition->name());
string header_name = filename_;
filename_.append(".h");
//
// parse for user sections and write header in the output file
//
out.initUserSections(filename_.c_str());
out.open(filename_.c_str());
out << "//\n";
out << "// generated by Qedo\n";
out << "//\n\n";
out << "#ifndef _" << header_name << "_H_\n";
out << "#define _" << header_name << "_H_\n\n\n";
out.insertUserSection("file_pre", 2);
out << "#include <CORBA.h>\n";
out << "#include \"" << file_prefix_ << "_BUSINESS.h\"\n";
if(composition->lifecycle()==CIDL::lc_Entity || composition->lifecycle()==CIDL::lc_Process)
out << "#include \"" << file_prefix_ << "_PSS.h\"\n";
out << "#include \"valuetypes.h\"\n";
out << "#include \"RefCountBase.h\"\n";
if(composition->lifecycle()==CIDL::lc_Entity)
out << "#include \"CCMContext.h\"\n";
out << "#include <string>\n\n\n";
out.insertUserSection("file_post", 2);
if(module_def)
{
open_module(out, module_def);
out << "namespace " << mapName(module_def) << "\n{\n";
out.indent();
}
CIDL::SegmentDefSeq_var segment_seq = composition->executor_def()->segments();
std::string executor_name = composition->executor_def()->name();
std::string executor_class_name = mapName(executor_name);
std::string executor_locator_name = composition->name();
std::string executor_locator_class_name = mapName(executor_locator_name);
std::string home_name = composition->home_executor()->name();
std::string home_class_name = mapName(home_name);
//
// executor
//
out << "\n//\n// executor\n//\n";
out << "class " << executor_class_name << "\n";
out.indent();
out << ": public virtual CORBA::LocalObject\n";
out << ", public virtual " << mapScopeName(composition) << "::CCM_" << executor_name << "\n";
out.unindent(); out.unindent();
out << "#ifndef MICO_ORB\n";
out.indent(); out.indent();
out << ", public virtual Qedo::RefCountLocalObject\n";
out.unindent(); out.unindent();
out << "#endif\n";
out.indent(); out.indent();
out.insertUserSection(string("INHERITANCE_") + executor_name, 0);
out.unindent();
out << "{\n\n";
out << "private:\n\n";
out.indent();
out << mapFullNameLocal(composition->ccm_component()) << "_ContextImpl_var context_;\n\n";
out.unindent();
out << "public:\n\n";
out.indent();
switch(composition->lifecycle())
{
case CIDL::lc_Session :
out << executor_class_name << "();\n";
break;
case CIDL::lc_Entity :
out << executor_class_name << "(" << mapFullNamePK(composition->ccm_home()->primary_key()) << "* pkey);\n";
break;
default :
out << "// not supported lifecycle\n";
}
out << "virtual ~" << executor_class_name << "();\n\n";
out << "void set_context(" << mapFullNameLocal(composition->ccm_component()) << "_ContextImpl_ptr context)\n";
out << " throw (CORBA::SystemException, Components::CCMException);\n\n";
out << "void configuration_complete()\n";
out << " throw (CORBA::SystemException, Components::InvalidConfiguration);\n\n";
out << "void remove()\n";
out << " throw (CORBA::SystemException);\n\n";
if(composition->lifecycle() == CIDL::lc_Service)
{
out << "void preinvoke(const char* comp_id, const char* operation)\n";
out << " throw (CORBA::SystemException);\n\n";
out << "void postinvoke(const char* comp_id, const char* operation)\n";
out << " throw (CORBA::SystemException);\n\n";
}
need_push_ = true;
doComponent(composition->ccm_component());
out.unindent();
out << "\n";
out.insertUserSection(executor_name);
out << "};\n\n";
//
// segment
//
for (i = 0; i < segment_seq->length(); i++)
{
std::string segment_name = segment_seq[i]->name();
std::string segment_class_name = mapName(segment_name);
out << "\n//\n// segment\n//\n";
out << "class " << segment_class_name << "\n";
out.indent();
out << ": public virtual CORBA::LocalObject\n";
out << ", public virtual " << mapScopeName(composition) << "::CCM_" << segment_name << "\n";
out.unindent(); out.unindent();
out << "#ifndef MICO_ORB\n";
out.indent(); out.indent();
out << ", public virtual Qedo::RefCountLocalObject\n";
out.unindent(); out.unindent();
out << "#endif\n";
out.indent(); out.indent();
out.insertUserSection(string("INHERITANCE_") + segment_name, 0);
out.unindent();
out << "{\n\n";
out << "private:\n\n";
out.indent();
out << mapFullNameLocal(composition->ccm_component()) << "_ContextImpl_var context_;\n\n";
out.unindent();
out << "public:\n\n";
out.indent();
out << segment_class_name << "();\n";
out << "virtual ~" << segment_class_name << "();\n\n";
out << "void set_context(" << mapFullNameLocal(composition->ccm_component()) << "_ContextImpl_ptr context)\n";
out << " throw (CORBA::SystemException, Components::CCMException);\n\n";
out << "void configuration_complete()\n";
out << " throw (CORBA::SystemException, Components::InvalidConfiguration);\n\n";
//
// for each implemented facet
//
IR__::ProvidesDefSeq_var provided_seq = segment_seq[i]->provided_facets();
handled_interfaces_.clear();
for (CORBA::ULong ii = 0; ii < provided_seq->length(); ii++)
{
IR__::InterfaceDef_var intf = IR__::InterfaceDef::_narrow(provided_seq[ii]->interface_type());
if( !CORBA::is_nil(intf) )
{
doInterface(intf);
}
}
out.unindent();
out << "\n";
out.insertUserSection(segment_name);
out << "};\n\n";
}
//
// executor locator
//
out << "\n//\n// executor locator\n//\n";
out << "class " << executor_locator_class_name << "\n";
out.indent();
out << ": public virtual CORBA::LocalObject\n";
//
// determin the lifecycle
//
CIDL::LifecycleCategory lc = composition->lifecycle();
switch(lc) {
case (CIDL::lc_Service) :
{
// this is a prelimenary version of container service
out << ", public virtual Components::CCMService\n";
}
case (CIDL::lc_Session) :
{
out << ", public virtual Components::SessionExecutorLocator\n";
break;
}
case (CIDL::lc_Entity) :
{
out << ", public virtual Components::EntityExecutorLocator\n";
break;
}
case (CIDL::lc_Extension) :
{
out << ", public virtual Components::ExtensionExecutorLocator\n";
break;
}
default:
{
//unsupported lifecycle category
}
}
out.unindent(); out.unindent();
out << "#ifndef MICO_ORB\n";
out.indent(); out.indent();
out << ", public virtual Qedo::RefCountLocalObject\n";
out.unindent(); out.unindent();
out << "#endif\n";
out.indent(); out.indent();
out.insertUserSection(string("INHERITANCE_") + executor_locator_name, 0);
out.unindent();
out << "{\n\n";
out << "private:\n\n";
out.indent();
out << mapFullNameLocal(composition->ccm_component()) << "_ContextImpl_var context_;\n\n";
out << mapName(composition->executor_def()) << "* component_;\n\n";
for (i = 0; i < segment_seq->length(); i++) {
out << mapName(segment_seq[i]) << "* " << segment_seq[i]->name() << "_;\n\n";
}
out.unindent();
out << "public:\n\n";
out.indent();
switch(composition->lifecycle())
{
case CIDL::lc_Session :
out << executor_locator_class_name << "();\n";
break;
case CIDL::lc_Entity :
out << executor_locator_class_name << "(" << mapFullNamePK(composition->ccm_home()->primary_key()) << "* pkey);\n";
break;
default :
out << "// not supported lifecycle\n";
}
out << "virtual ~" << executor_locator_class_name << "();\n\n";
IR__::InterfaceDef_ptr executor_locator;
switch(lc) {
case (CIDL::lc_Service) :
{
//prelimenary version of container services
executor_locator = IR__::InterfaceDef::_narrow(repository_->lookup_id("IDL:Components/CCMService:1.0"));
break;
}
case (CIDL::lc_Session) :
{
executor_locator = IR__::InterfaceDef::_narrow(repository_->lookup_id("IDL:Components/SessionExecutorLocator:1.0"));
break;
}
case (CIDL::lc_Entity) :
{
executor_locator = IR__::InterfaceDef::_narrow(repository_->lookup_id("IDL:Components/EntityExecutorLocator:1.0"));
break;
}
case (CIDL::lc_Extension) :
{
executor_locator = IR__::InterfaceDef::_narrow(repository_->lookup_id("IDL:Components/ExtensionExecutorLocator:1.0"));
break;
}
default:
{
//unsupported lifecycle category
}
}
doInterface(executor_locator);
out.unindent();
out << "\n";
out.insertUserSection(executor_locator_name);
out << "};\n\n";
//
// home executor
//
out << "\n//\n// home executor\n//\n";
out << "class " << home_class_name << "\n";
out.indent();
out << ": public virtual CORBA::LocalObject\n";
out << ", public virtual " << getLocalName(composition->ccm_home()) << "\n";
out.unindent(); out.unindent();
out << "#ifndef MICO_ORB\n";
out.indent(); out.indent();
out << ", public virtual Qedo::RefCountLocalObject\n";
out.unindent(); out.unindent();
out << "#endif\n";
out.indent(); out.indent();
out.insertUserSection(std::string("INHERITANCE_") + home_name, 0);
out.unindent();
out << "{\n\n";
out << "private:\n\n";
out.indent();
out << "Components::HomeContext_var context_;\n\n";
out.unindent();
out << "public:\n";
out.indent();
out << home_class_name << "();\n";
out << "virtual ~" << home_class_name << "();\n\n";
out << "//\n// IDL:Components/HomeExecutorBase/set_context:1.0\n//\n";
out << "virtual void set_context (Components::HomeContext_ptr ctx)\n";
out << " throw (CORBA::SystemException, Components::CCMException);\n\n";
out << "//\n// IDL:Components/HomeExecutorBase/set_storagehome_in_context:1.0\n//\n";
out << "virtual void set_storagehome_in_context (::CosPersistentState::StorageHomeBase_ptr storHomeBase)\n";
out << " throw (CORBA::SystemException, Components::CCMException);\n\n";
out << "//\n// IDL:Components/HomeExecutorBase/get_storagehome_from_context:1.0\n//\n";
out << "virtual ::CosPersistentState::StorageHomeBase_ptr get_storagehome_from_context ()\n";
out << " throw (CORBA::SystemException, Components::CCMException);\n\n";
out << "//\n// IDL:.../create:1.0\n//\n";
switch(composition->lifecycle())
{
case CIDL::lc_Session :
out << "virtual ::Components::EnterpriseComponent_ptr create()\n";
out << " throw (CORBA::SystemException, Components::CreateFailure);\n";
break;
case CIDL::lc_Entity :
out << "virtual ::Components::EnterpriseComponent_ptr create(" << mapFullNamePK(composition->ccm_home()->primary_key()) << "* pkey)\n";
out << " throw(CORBA::SystemException, Components::CreateFailure, Components::DuplicateKeyValue, Components::InvalidKey);\n\n";
/*
out << "virtual ::Components::EnterpriseComponent_ptr find_by_primary_key(" << mapFullNamePK(composition->ccm_home()->primary_key()) << "* pkey)\n";
out << " throw(CORBA::SystemException, Components::FinderFailure, Components::UnknownKeyValue, Components::InvalidKey);\n\n";
out << "void remove(" << mapFullNamePK(composition->ccm_home()->primary_key()) << "* pkey)\n";
out << " throw(CORBA::SystemException, Components::RemoveFailure, Components::UnknownKeyValue, Components::InvalidKey);\n\n";
*/
//get_primary_key(...)??? !!!
break;
default :
out << "// not supported lifecycle\n";
}
doHome(composition->ccm_home());
out.unindent();
out << "\n";
out.insertUserSection(home_name);
out << "};\n\n";
// close namespace
if(module_def)
{
out.unindent();
out << "};\n";
close_module(out, module_def);
}
//
// entry point
//
out << "\n//\n// entry point\n//\n";
out << "extern \"C\" {\n";
out << "#ifdef _WIN32\n";
out << "__declspec(dllexport)\n";
out << "#else\n";
out << "#endif\n";
out << "::Components::HomeExecutorBase_ptr create_" << composition->ccm_home()->name() << "E(void);\n";
out << "}\n\n";
out << "#endif\n";
out.close();
}
void
GeneratorValuetypesC::doValueMember(IR__::ValueMemberDef_ptr member)
{
std::string member_name = mapName(member);
out << member_name << "( v." << member_name << "() );\n";
}
void
GeneratorValuetypesC::doValue(IR__::ValueDef_ptr value)
{
open_module(out, value);
class_name_ = mapName(value) + "Impl";
out << "void\n";
out << class_name_ << "::operator= (const " << class_name_ << "& v)\n{\n";
out.indent();
handleValueMember(value);
out.insertUserSection(class_name_ + "::operator=", 0);
out.unindent();
out << "}\n\n\n";
out << class_name_ << "::" << class_name_ << "(const " << class_name_ << "& v)\n{\n";
out.indent();
handleValueMember(value);
out.insertUserSection(class_name_ + "::" + class_name_ + "1", 0);
out.unindent();
out << "}\n\n\n";
// check whether this value(eventtype) has members.
IR__::ContainedSeq_var contained_seq = value->contents(CORBA__::dk_ValueMember, false);
if( contained_seq->length() > 0 ) // if has some, generate the constructor with parameters
{
out << class_name_ << "::" << class_name_ << "(";
generateMemberParam( value, false );
out << ")\n: " << mapFullNameWithPrefix(value, "OBV_") << "(";
generateMemberInit( value, false );
out << ")\n{\n";
out.indent();
out.insertUserSection(class_name_ + "::" + class_name_ + "2", 0);
out.unindent();
out << "}\n\n\n";
}
out << class_name_ << "::" << class_name_ << "()\n{\n";
out.indent();
out.insertUserSection(class_name_ + "::" + class_name_ + "3", 0);
out.unindent();
out << "}\n\n\n";
out << class_name_ << "::~" << class_name_ << "()\n{\n";
out.indent();
out.insertUserSection(class_name_ + "::~" + class_name_, 0);
out.unindent();
out << "}\n\n\n";
out << "CORBA::ValueBase*\n";
out << class_name_ << "::_copy_value()\n{\n";
out.indent();
out << class_name_ << " *val = new " << class_name_ << "(* this);\n";
out.insertUserSection(class_name_ + "::_copy_value", 0);
out << "return val;\n";
out.unindent();
out << "}\n\n\n";
//
// supported interfaces
//
CORBA::ULong i;
handled_interfaces_.clear();
IR__::InterfaceDefSeq_var supp_intfs = value->supported_interfaces();
for(i = 0; i < supp_intfs->length(); i++)
{
handleAttribute((*supp_intfs)[i]);
handleOperation((*supp_intfs)[i]);
};
//
// base value
//
IR__::ValueDef_var base = value->base_value();
if(! CORBA::is_nil(base))
{
// todo
}
//
// abstract base values
//
IR__::ValueDefSeq_var abstr = value->abstract_base_values();
for(i = 0; i < abstr->length(); i++)
{
// todo
};
//
// value type factory
//
out << "CORBA::ValueBase *\n";
out << value->name() << "FactoryImpl::create_for_unmarshal ()\n{\n";
out.indent();
out << "return new " << mapFullName( value ) << "Impl();\n";
out.unindent();
out << "}\n\n\n";
//
// initializers
//
IR__::InitializerSeq_var ini = value->initializers();
for(i = 0; i < ini->length(); i++)
{
// todo
};
//
// static factory cleaner
//
out << "static Qedo::ValueFactoryCleaner " << value->name() << "Factory_cleaner( new ";
out << value->name() << "FactoryImpl(), \"" << value->id() << "\" );\n";
close_module(out, value);
}
std::string
CPPBase::mapName(IR__::Contained_ptr obj)
{
return mapName(obj->name());
}
