// Implementation skeleton constructor
RTT_corba_CService_i::RTT_corba_CService_i ( RTT::Service::shared_ptr service, PortableServer::POA_ptr poa)
: RTT_corba_CConfigurationInterface_i( service.get(), PortableServer::POA::_duplicate( poa) ),
RTT_corba_COperationInterface_i( service.get(), PortableServer::POA::_duplicate( poa) ),
RTT::corba::CDataFlowInterface_i( service.get(), PortableServer::POA::_duplicate( poa) ),
mpoa(poa), mservice(service)
{
}
// Fetch remote ports and create local proxies
void TaskContextProxy::fetchPorts(RTT::Service::shared_ptr parent, CDataFlowInterface_ptr dfact)
{
log(Debug) << "Fetching Ports for service "<<parent->getName()<<"."<<endlog();
TypeInfoRepository::shared_ptr type_repo = TypeInfoRepository::Instance();
if (dfact) {
CDataFlowInterface::CPortDescriptions_var objs = dfact->getPortDescriptions();
for ( size_t i=0; i < objs->length(); ++i) {
CPortDescription port = objs[i];
if (parent->getPort( port.name.in() ))
continue; // already added.
TypeInfo const* type_info = type_repo->type(port.type_name.in());
if (!type_info)
{
log(Warning) << "remote port " << port.name
<< " has a type that cannot be marshalled over CORBA: " << port.type_name << ". "
<< "It is ignored by TaskContextProxy" << endlog();
}
else
{
PortInterface* new_port;
if (port.type == RTT::corba::CInput)
new_port = new RemoteInputPort( type_info, dfact, port.name.in(), ProxyPOA() );
else
new_port = new RemoteOutputPort( type_info, dfact, port.name.in(), ProxyPOA() );
parent->addPort(*new_port);
port_proxies.push_back(new_port); // see comment in definition of port_proxies
}
}
}
}
bool loadOperators()
{
RTT::Service::shared_ptr gs = RTT::internal::GlobalService::Instance();
gs->provides("KDL")->addOperation("TwistToMsg",&tf::TwistKDLToMsg);
gs->provides("KDL")->addOperation("MsgToTwist",&tf::TwistMsgToKDL);
gs->provides("KDL")->addOperation("FrameToMsg",&tf::PoseKDLToMsg);
gs->provides("KDL")->addOperation("MsgToFrame",&tf::PoseKDLToMsg);
return true;
}
bool KDLTypekitPlugin::loadConstructors()
{
TypeInfoRepository::shared_ptr ti = TypeInfoRepository::Instance();
ti->type("KDL.Vector")->addConstructor( newConstructor(&vectorxyz) );
ti->type("KDL.Rotation")->addConstructor( newConstructor( ptr_fun( Rotation::RPY )) );
ti->type("KDL.Rotation")->addConstructor( newConstructor(&rotationAngleAxis) );
ti->type("KDL.Frame")->addConstructor( newConstructor(&framerv) );
ti->type("KDL.Frame")->addConstructor( newConstructor(&framevr) );
ti->type("KDL.Wrench")->addConstructor( newConstructor(&wrenchft) );
ti->type("KDL.Twist")->addConstructor( newConstructor(&twistvw) );
RTT::Service::shared_ptr gs = RTT::internal::GlobalService::Instance();
gs->provides("KDL")->provides("Rotation")->addOperation("RotX",&Rotation::RotX).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("RotY",&Rotation::RotY).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("RotZ",&Rotation::RotZ).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("RPY",&Rotation::RPY).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("EulerZYX",&Rotation::EulerZYX).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("EulerZYZ",&Rotation::EulerZYZ).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("Quaternion",&Rotation::Quaternion).doc("");
//ti->type("Frame[]")->addConstructor(newConstructor(stdvector_ctor<Frame>() ) );
//ti->type("Frame[]")->addConstructor(newConstructor(stdvector_ctor2<Frame>() ) );
//ti->type("Frame[]")->addConstructor(new StdVectorBuilder<Frame>() );
//ti->type("Vector[]")->addConstructor(newConstructor(stdvector_ctor<Vector>() ) );
//ti->type("Vector[]")->addConstructor(newConstructor(stdvector_ctor2<Vector>() ) );
//ti->type("Vector[]")->addConstructor(new StdVectorBuilder<Vector>() );
//ti->type("Rotation[]")->addConstructor(newConstructor(stdvector_ctor<Rotation>() ) );
//ti->type("Rotation[]")->addConstructor(newConstructor(stdvector_ctor2<Rotation>() ) );
//ti->type("Rotation[]")->addConstructor(new StdVectorBuilder<Rotation>() );
//ti->type("Wrench[]")->addConstructor(newConstructor(stdvector_ctor<Wrench>() ) );
//ti->type("Wrench[]")->addConstructor(newConstructor(stdvector_ctor2<Wrench>() ) );
//ti->type("Wrench[]")->addConstructor(new StdVectorBuilder<Wrench>() );
//ti->type("Twist[]")->addConstructor(newConstructor(stdvector_ctor<Twist>() ) );
//ti->type("Twist[]")->addConstructor(newConstructor(stdvector_ctor2<Twist>() ) );
//ti->type("Twist[]")->addConstructor(new StdVectorBuilder<Twist>() );
return true;
}
bool KDLTypekitPlugin::loadOperators()
{
OperatorRepository::shared_ptr oreg = OperatorRepository::Instance();
oreg->add( newBinaryOperator( "==", std::equal_to<Frame>() ) );
oreg->add( newBinaryOperator( "!=", std::not_equal_to<Frame>() ) );
oreg->add( newBinaryOperator( "==", std::equal_to<Vector>() ) );
oreg->add( newBinaryOperator( "!=", std::not_equal_to<Vector>() ) );
oreg->add( newBinaryOperator( "==", std::equal_to<Rotation>() ) );
oreg->add( newBinaryOperator( "!=", std::not_equal_to<Rotation>() ) );
oreg->add( newBinaryOperator( "==", std::equal_to<Wrench>() ) );
oreg->add( newBinaryOperator( "!=", std::not_equal_to<Wrench>() ) );
oreg->add( newBinaryOperator( "==", std::equal_to<Twist>() ) );
oreg->add( newBinaryOperator( "!=", std::not_equal_to<Twist>() ) );
oreg->add( newUnaryOperator( "-", std::negate<Vector>() ) );
oreg->add( newBinaryOperator( "*", std::multiplies<Vector>() ) );
oreg->add( newBinaryOperator( "*", std::multiplies<Frame>() ) );
oreg->add( newBinaryOperator( "*", std::multiplies<Rotation>() ) );
oreg->add( newBinaryOperator( "+", std::plus<Vector>() ) );
oreg->add( newBinaryOperator( "-", std::minus<Vector>() ) );
oreg->add( newBinaryOperator( "+", std::plus<Wrench>() ) );
oreg->add( newBinaryOperator( "-", std::minus<Wrench>() ) );
oreg->add( newBinaryOperator( "+", std::plus<Twist>() ) );
oreg->add( newBinaryOperator( "-", std::minus<Twist>() ) );
oreg->add( newBinaryOperator( "*", multiplies3<Vector,int, Vector>() ) );
oreg->add( newBinaryOperator( "*", multiplies3<Vector,Vector, int>() ) );
oreg->add( newBinaryOperator( "*", multiplies3<Vector,double, Vector>() ) );
oreg->add( newBinaryOperator( "*", multiplies3<Vector,Vector, double>() ) );
oreg->add( newBinaryOperator( "*", multiplies3<Wrench, Frame, Wrench>() ) );
oreg->add( newBinaryOperator( "*", multiplies3<Twist, Frame, Twist>() ) );
oreg->add( newBinaryOperator( "*", multiplies3<Vector, Frame, Vector>() ) );
oreg->add( newBinaryOperator( "*", multiplies3<Vector, Rotation, Vector>() ) );
oreg->add( newBinaryOperator( "/", divides3<Vector, Vector, double>() ) );
oreg->add( newBinaryOperator( "/", divides3<Wrench, Wrench, double>() ) );
oreg->add( newBinaryOperator( "/", divides3<Twist, Twist, double>() ) );
RTT::Service::shared_ptr gs = RTT::internal::GlobalService::Instance();
gs->provides("KDL")->provides("Vector")->addOperation("diff",(Vector (*) (const Vector&, const Vector&, double)) &diff).doc("");
gs->provides("KDL")->provides("Vector")->addOperation("addDelta",(Vector (*) (const Vector&, const Vector&, double)) &addDelta).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("diff",(Vector (*) (const Rotation&, const Rotation&, double)) &diff).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("addDelta",(Rotation (*) (const Rotation&, const Vector&, double)) &addDelta).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("Inverse",(Rotation (*) (const Rotation&)) &Inverse).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("GetQuaternion",(void (*)(const Rotation&, double&, double&, double&, double&)) &GetQuaternion).doc("");
gs->provides("KDL")->provides("Twist")->addOperation("diff",(Twist (*) (const Twist&, const Twist&, double)) &diff).doc("");
gs->provides("KDL")->provides("Wrench")->addOperation("diff",(Wrench (*) (const Wrench&, const Wrench&, double)) &diff).doc("");
gs->provides("KDL")->provides("Frame")->addOperation("diff",(Twist (*) (const Frame&, const Frame&, double)) &diff)
.doc("Returns the twist that is needed to move from frame f1 to frame f2 in a time d. The resulting twist is represented in the same reference frame as f1 and f2, and has reference point at the origin of f1");
gs->provides("KDL")->provides("Frame")->addOperation("addDelta", (Frame (*) (const Frame&, const Twist&, double)) &addDelta)
.doc("Constructs a frame that is obtained by: starting from frame f, apply twist t, during time d");
gs->provides("KDL")->provides("Frame")->addOperation("Inverse",(Frame (*) (const Frame&)) &Inverse).doc("");
return true;
}
