void HuobiApp::submitLimitOrder(const char *symbol, OrderSide side, double price, double amount, const char *userId)
{
int ts = timestamp();
FIX44::NewOrderSingle message;
message.set(Account(_publicKey));
if (userId)
message.set(ClOrdID(userId));
else
message.set(ClOrdID("HuobiOrder"));
message.set(Price(price));
message.set(MinQty(amount));
message.set(OrdType(OrdType_LIMIT));
message.set(Symbol(symbol));
message.set(Side(side == OrderBuy ? Side_BUY : Side_SELL));
message.set(TransactTime());
message.setField(IntField(957, ts));
message.setField(StringField(958, _publicKey));
char str[1024];
sprintf(str, "access_key=%s&amount=%s&coin_type=%d&created=%d&method=%s&price=%s&secret_key=%s",
_publicKey.c_str(),
float2str(amount).c_str(),
getCoinType(symbol),
ts,
side == OrderBuy ? "buy" : "sell",
float2str(price).c_str(),
_privateKey.c_str());
message.setField(StringField(959, md5::MD5String(str, false)));
Session *session = Session::lookupSession(_sessionID);
if (session)
session->send(message);
}
/**
* @brief QrSlam::addObservationPos
* 添加landmark 点（x,y） 到观测量中
* @param landmarktemp 2d mark角点
* @param id 2d mark ID
* @return 点集（x,y,id） id = ID*5 + j (j=01234)
*/
Point3ffi QrSlam::addObservationPos(ConerPoint landmarktemp ,int id )
{
//Generate observations.假设传感器能观察到机器人周围sd米内的所有特征 @param[in] sd
Point2f delta;
float dst; //特征距离
float theta; //特征角w.r.t.robot frame
Point3ffi mark_temp;
delta = Point2f(landmarktemp.X,landmarktemp.Y) ;//-Point2f(RobotPos.X,RobotPos.Y); //已经是delta值了
dst = norm(delta);
theta = atan2(delta.y, delta.x) ;//- robot_info_.Theta ;// ??? the true path of vehicle
// if(delta.x < 0)
// theta = 3.14159 + theta; //- robot_info_.Theta ;// ??? the true path of vehicle
// dst+= genGaussianValue(sigma_r*sigma_r);
// theta+= genGaussianValue(sigma_phi*sigma_phi); //给测量量添加噪声
angleWrap(theta);
mark_temp.x = dst;
mark_temp.y = theta;
mark_temp.z = id;//temp value
if( (id%5==4) && (id/5 ==19))
{
std::string text_id ="id: "+ int2str(id/5 );
cv::putText(cv_camera_,text_id,cvPoint(20,100),CV_FONT_HERSHEY_COMPLEX,1,CV_RGB(0,0,255));
std::string text1 ="d: "+ float2str(mark_temp.x );
cv::putText(cv_camera_,text1,cvPoint(20,150),CV_FONT_HERSHEY_COMPLEX,1,CV_RGB(0,0,255));
std::string text2 ="theta: "+ float2str(mark_temp.y*180/ 3.14159);
cv::putText(cv_camera_,text2,cvPoint(20,200),CV_FONT_HERSHEY_COMPLEX,1,CV_RGB(0,0,255));
}
return mark_temp;
}
void Colormap::render(float min, float max, int minorTicks) {
int step = 1;
int width = 50;
glTranslatef(10, 10, 10);
glColor3f(1, 1, 1);
glBegin(GL_LINES);
glVertex2f(1, 1);
glVertex2f(1, 256 * step + 2);
glVertex2f(1, 256 * step + 2);
glVertex2f(width, 256 * step + 2);
glVertex2f(width, 256 * step + 2);
glVertex2f(width, 1);
glVertex2f(width, 1);
glVertex2f(1, 1);
glEnd();
glBegin(GL_QUADS);
for (size_t i = 0; i < 256; i++) {
glColor3f(colors[i].red, colors[i].green, colors[i].blue);
glVertex2f(1, 1 + (i * step) + step); // Top Left
glVertex2f(width - 1, 1 + (i * step) + step); // Top Right
glColor3f(colors[i].red, colors[i].green, colors[i].blue);
glVertex2f(width - 1, 1 + i * step); // Bottom Right
glVertex2f(1, 1 + i * step); // Bottom Left
}
glEnd();
glColor3f(1, 1, 1);
glBegin(GL_LINES);
glVertex2f(width, 2);
glVertex2f(width + 5, 2);
glEnd();
printText(width + 8, 1 - 3.5, float2str(min));
minorTicks = minorTicks + 1;
for (int tick = 1; tick < minorTicks; tick++) {
glBegin(GL_LINES);
glVertex2f(width, tick * ((256 * step) / minorTicks));
glVertex2f(width + 2, tick * ((256 * step) / minorTicks));
glEnd();
float val = min + tick * (fabs(min - max) / minorTicks);
printText(width + 5, tick * ((256 * step) / minorTicks) - 3.5, float2str(val));
}
glBegin(GL_LINES);
glVertex2f(width, 256 * step + 1);
glVertex2f(width + 5, 256 * step + 1);
glEnd();
printText(width + 8, 256 * step - 3.5, float2str(max));
glTranslatef(-10, -10, -10);
}
string t_hdr_timestamp::encode_value(void) const {
string s;
if (!populated) return s;
s += float2str(timestamp, 3);
if (delay != 0) {
s += " ";
s += float2str(delay, 3);
}
return s;
}
void SpecificWorker::updateSymbolWithTag(AGMModelSymbol::SPtr symbol, const AprilTagModel &tag)
{
innerModel->updateTransformValues("trTag", tag.tx, tag.ty, tag.tz, tag.rx+M_PIl, tag.ry, tag.rz);
QVec T = innerModel->transform("robot", QVec::vec3(0,0,0), "trTag");
QVec R = innerModel->getRotationMatrixTo("trTag", "robot").extractAnglesR_min();
//threshold to update set to 20 mm and 0.1 rad
if ( fabs(str2float((symbol->attributes["tx"])) - T(0)) > 20 ||
fabs(str2float((symbol->attributes["ty"])) - T(1)) > 20 ||
fabs(str2float((symbol->attributes["tz"])) - T(2)) > 20 ||
fabs(str2float((symbol->attributes["rx"])) - R(0)) > 0.1 ||
fabs(str2float((symbol->attributes["ry"])) - R(1)) > 0.1 ||
fabs(str2float((symbol->attributes["rz"])) - R(2)) > 0.1 )
{
symbol->attributes["tx"] = float2str(T(0));
symbol->attributes["ty"] = float2str(T(1));
symbol->attributes["tz"] = float2str(T(2));
symbol->attributes["rx"] = float2str(R(0));
symbol->attributes["ry"] = float2str(R(1));
symbol->attributes["rz"] = float2str(R(2));
RoboCompAGMWorldModel::Node symbolICE;
AGMModelConverter::fromInternalToIce(symbol, symbolICE);
agmagenttopic->update(symbolICE);
}
}
int main(int argc, char *argv[])
{
float A, B, C, correct1;
int t_success=0, t_count=0;
char inA[70],inB[70],outC[70],corC[70];
if( sizeof(long) != sizeof(float) )
{
puts("error sizeof long != sizeof float");
return -1;
}
while(scanf("%5s",inA) == 1)
{
if(strcmp(inA,"TEST1") == 0)
{
scanf("%s %s %s", inA, inB, outC);
A = str2float(inA);
B = str2float(inB);
C = str2float(outC);
correct1 = A + B;
float2str(correct1, corC);
}
else if(strcmp(inA,"TEST2") == 0)
{
scanf("%s %s %s %s", inA, inB, outC, corC);
A = str2float(inA);
B = str2float(inB);
C = str2float(outC);
correct1 = str2float(corC);
}
else
{
while(getchar() != '\n');//skip until end of the line
continue;
}
if(! fequal(corC, outC))
{
printf("Test #%d failed: %e + %e = %e != %e\n", t_count+1, A, B, correct1, C);
printf(" A %s\n +B %s\n == %s\n != %s\n", inA, inB, corC, outC);
}
else
{
//printf("Test OK! (%.2f + %.2f = %.2f)\n", A, B, C);
t_success++;
}
t_count++;
}
printf("Succeded %d tests, Failed %d tests\n", t_success, t_count - t_success);
printf("Sucess: %.2lf%%\n",100 * t_success / (double)t_count );
return 0;
}
string FuncFtoFPolynomial::getString(string strResult, string strInput)
{
std::stringstream ss;
ss << strResult << " = ";
for (uint i = 0; i < m_constants.getSize(); i++)
{
if (i!=0)
ss << " + ";
float c = m_constants[i];
string strC = float2str(c);
ss << strC;
for (uint j = i+1; j < m_constants.getSize(); j++)
ss << "*" << strInput;
}
return ss.str();
}
void SpecificWorker::updateWristPose()
{
// Make sure we have the robot in the model, otherwise there's nothing to do yet...
int32_t robotId = worldModel->getIdentifierByType("robot");
if (robotId == -1)
{
return;
}
AGMModelSymbol::SPtr robot = worldModel->getSymbol(robotId);
// Set current T and R
QVec T = innerModel->transform("robot", QVec::vec3(0,0,0), "arm_right_8");
QVec R = innerModel->getRotationMatrixTo("arm_right_8", "robot").extractAnglesR_min();
// Set back T and R
QVec T_back, R_back;
bool force = false;
try
{
T_back = QVec::vec3(
str2float(robot->attributes["rightwrist_tx"]),
str2float(robot->attributes["rightwrist_ty"]),
str2float(robot->attributes["rightwrist_tz"]));
R_back = QVec::vec3(
str2float(robot->attributes["rightwrist_rx"]),
str2float(robot->attributes["rightwrist_ry"]),
str2float(robot->attributes["rightwrist_rz"]));
}
catch(...)
{
printf("exception in updateWristPose %d\n", __LINE__);
force = true;
}
#warning These thresholds should be set in the config file!!!
#warning These thresholds should be set in the config file!!!
#warning These thresholds should be set in the config file!!!
#warning These thresholds should be set in the config file!!!
if ( force or (T-T_back).norm2()>15 or (R-R_back).norm2()>0.05)
{
robot->attributes["rightwrist_tx"] = float2str(T(0));
robot->attributes["rightwrist_ty"] = float2str(T(1));
robot->attributes["rightwrist_tz"] = float2str(T(2));
robot->attributes["rightwrist_rx"] = float2str(R(0));
robot->attributes["rightwrist_ry"] = float2str(R(1));
robot->attributes["rightwrist_rz"] = float2str(R(2));
RoboCompAGMWorldModel::Node nodeDst;
AGMModelConverter::fromInternalToIce(robot, nodeDst);
agmagenttopic->update(nodeDst);
}
}
void SpecificWorker::includeObjectInModel(AGMModel::SPtr &newModel, const AprilTagModel &tag)
{
int32_t robotId = newModel->getIdentifierByType("robot");
if (robotId == -1)
{
return;
}
/// object
AGMModelSymbol::SPtr newTag = newModel->newSymbol("ball");
newModel->addEdgeByIdentifiers(robotId, newTag->identifier, "sees");
newTag->attributes["id"] = int2str(tag.id);
newTag->attributes["tx"] = float2str(tag.tx);
newTag->attributes["ty"] = float2str(tag.ty);
newTag->attributes["tz"] = float2str(tag.tz);
newTag->attributes["rx"] = float2str(tag.rx);
newTag->attributes["ry"] = float2str(tag.ry);
newTag->attributes["rz"] = float2str(tag.rz);
}
void SpecificWorker::innerToAGM(InnerModelNode* node, int &symbolID, QList<QString> lNode)
{
QList<InnerModelNode*>::iterator i;
int p=symbolID;
for (i=node->children.begin(); i!=node->children.end(); i++)
{
if ( !lNode.contains((*i)->id) )
{
//Search name (key) of the innerModel node in AGM
int existingID = findName ( (*i)->id ) ;
if ( existingID == -1 )
{
qDebug()<<node->id<<"link"<<(*i)->id;
//symbol
AGMModelSymbol::SPtr newSym = ImNodeToSymbol((*i));
//edge
std::map<std::string, std::string> linkAttrs;
linkAttrs.insert ( std::pair<std::string,std::string>("tx",float2str((*i)->getTr().x())) );
linkAttrs.insert ( std::pair<std::string,std::string>("ty",float2str((*i)->getTr().y())) );
linkAttrs.insert ( std::pair<std::string,std::string>("tz",float2str((*i)->getTr().z())) );
linkAttrs.insert ( std::pair<std::string,std::string>("rx",float2str((*i)->getRxValue())));
linkAttrs.insert ( std::pair<std::string,std::string>("ry",float2str((*i)->getRyValue())));
linkAttrs.insert ( std::pair<std::string,std::string>("rz",float2str((*i)->getRzValue())));
int32_t id = newSym->identifier;
worldModel->addEdgeByIdentifiers(p,id,"RT",linkAttrs);
innerToAGM((*i),id,lNode);
}
else
{
innerToAGM((*i),existingID,lNode);
}
}
}
}
AGMModelSymbol::SPtr SpecificWorker::ImNodeToSymbol(InnerModelNode* node)
{
std::map<std::string, std::string> attrs;
attrs.insert ( std::pair<std::string,std::string>("imName", node->id.toStdString()) );
AGMModelSymbol::SPtr newSym;
//TODO innerModelNode cast to the type and translate the name.
// attribute "type" = mesh,plane,joint..
string type;
if (dynamic_cast<InnerModelJoint*>(node) != NULL)
{
InnerModelJoint *joint = dynamic_cast<InnerModelJoint*>(node);
//QString id_, float lx_, float ly_, float lz_, float hx_, float hy_, float hz_, float tx_, float ty_, float tz_, float rx_, ;
//float ry_, float rz_, float min_, float max_, uint32_t port_, std::string axis_, float home_,
// attrs.insert ( std::pair<std::string,std::string>("lx",float2str(0.) ) );
// attrs.insert ( std::pair<std::string,std::string>("ly",float2str(0.) ) );
// attrs.insert ( std::pair<std::string,std::string>("lz",float2str(0.) ) );
//
// attrs.insert ( std::pair<std::string,std::string>("hx",float2str(joint->backhX) ) );
// attrs.insert ( std::pair<std::string,std::string>("hy",float2str(joint->backhY) ) );
// attrs.insert ( std::pair<std::string,std::string>("hz",float2str(joint->backhZ) ) );
attrs.insert ( std::pair<std::string,std::string>("min",float2str(joint->min) ) );
attrs.insert ( std::pair<std::string,std::string>("max",float2str(joint->max) ) );
attrs.insert ( std::pair<std::string,std::string>("port",int2str(joint->port) ) );
attrs.insert ( std::pair<std::string,std::string>("axis",joint->axis) );
attrs.insert ( std::pair<std::string,std::string>("home",float2str(joint->home)) );
type= "joint";
attrs.insert ( std::pair<std::string,std::string>("imType",type ) );
}
else if (dynamic_cast<InnerModelPrismaticJoint*>(node) != NULL)
{
InnerModelPrismaticJoint *p = dynamic_cast<InnerModelPrismaticJoint*>(node);
// float value, offset;
// float min, max;
// float home;
// uint32_t port;
// std::string axis;
attrs.insert ( std::pair<std::string,std::string>("value",float2str(p->value) ) );
attrs.insert ( std::pair<std::string,std::string>("offset",float2str(p->offset) ) );
attrs.insert ( std::pair<std::string,std::string>("min",float2str(p->min) ) );
attrs.insert ( std::pair<std::string,std::string>("max",float2str(p->max) ) );
attrs.insert ( std::pair<std::string,std::string>("home",float2str(p->home)) );
attrs.insert ( std::pair<std::string,std::string>("port",int2str(p->port)) );
attrs.insert ( std::pair<std::string,std::string>("axis",p->axis) );
type= "prismaticJoint";
attrs.insert ( std::pair<std::string,std::string>("imType",type ) );
}
else if (dynamic_cast<InnerModelTouchSensor*>(node) != NULL)
{
InnerModelTouchSensor *touch = dynamic_cast<InnerModelTouchSensor*>(node);
// float nx, ny, nz;
attrs.insert ( std::pair<std::string,std::string>("nx",float2str(touch->nx) ));
attrs.insert ( std::pair<std::string,std::string>("ny",float2str(touch->ny)) );
attrs.insert ( std::pair<std::string,std::string>("nz",float2str(touch->nz)) );
//float min, max;// float value;
attrs.insert ( std::pair<std::string,std::string>("min",float2str(touch->min) ));
attrs.insert ( std::pair<std::string,std::string>("max",float2str(touch->max)) );
attrs.insert ( std::pair<std::string,std::string>("value",float2str(touch->value)) );
// uint32_t port;
attrs.insert ( std::pair<std::string,std::string>("port",int2str(touch->port)) );
// QString stype;
attrs.insert ( std::pair<std::string,std::string>("stype",touch->stype.toStdString()) );
type = "touchSensor";
attrs.insert ( std::pair<std::string,std::string>("imType",type ) );
}
else if (dynamic_cast<InnerModelDifferentialRobot*>(node) != NULL)
{
InnerModelDifferentialRobot *diff = dynamic_cast<InnerModelDifferentialRobot*>(node);
//uint32_t port;
//float noise;
//bool collide;
attrs.insert ( std::pair<std::string,std::string>("port",int2str(diff->port)) );
attrs.insert ( std::pair<std::string,std::string>("noise",float2str(diff->noise)) );
string v="false";
if (diff->collide)
v="true";
attrs.insert ( std::pair<std::string,std::string>("collide",v) );
type = "differentialRobot";
attrs.insert ( std::pair<std::string,std::string>("imType",type ) );
}
else if (dynamic_cast<InnerModelOmniRobot*>(node) != NULL)
{
InnerModelOmniRobot *omni = dynamic_cast<InnerModelOmniRobot*>(node);
//uint32_t port;
//float noise;
//bool collide;
attrs.insert ( std::pair<std::string,std::string>("port",int2str(omni->port)) );
attrs.insert ( std::pair<std::string,std::string>("noise",float2str(omni->noise)) );
string v="false";
if (omni->collide)
v="true";
attrs.insert ( std::pair<std::string,std::string>("collide",v) );
type = "omniRobot";
attrs.insert ( std::pair<std::string,std::string>("imType",type ) );
}
else if (dynamic_cast<InnerModelPlane*>(node) != NULL)
{
//QString id, InnerModelNode *parent, QString texture, float width, float height, float depth, int repeat,
//float nx, float ny, float nz, float px, float py, float pz, bool collidable)
InnerModelPlane* plane = dynamic_cast<InnerModelPlane*>(node);
attrs.insert ( std::pair<std::string,std::string>("width",float2str(plane->width) ));
attrs.insert ( std::pair<std::string,std::string>("height",float2str(plane->height)) );
attrs.insert ( std::pair<std::string,std::string>("depth",float2str(plane->depth)) );
attrs.insert ( std::pair<std::string,std::string>("nx",float2str(plane->normal.x()) ));
attrs.insert ( std::pair<std::string,std::string>("ny",float2str(plane->normal.y()) ));
attrs.insert ( std::pair<std::string,std::string>("nz",float2str(plane->normal.z()) ));
attrs.insert ( std::pair<std::string,std::string>("px",float2str(plane->point.x()) ));
attrs.insert ( std::pair<std::string,std::string>("py",float2str(plane->point.y()) ));
attrs.insert ( std::pair<std::string,std::string>("pz",float2str(plane->point.z()) ));
string v="false";
if (plane->collidable)
v="true";
attrs.insert ( std::pair<std::string,std::string>("collidable",v) );
attrs.insert ( std::pair<std::string,std::string>("repeat",int2str(plane->repeat)) );
attrs.insert ( std::pair<std::string,std::string>("texture",plane->texture.toStdString()) );
type = "plane";
attrs.insert ( std::pair<std::string,std::string>("imType",type ) );
}
else if (dynamic_cast<InnerModelRGBD*>(node) != NULL)
{
InnerModelRGBD* rgbd = dynamic_cast<InnerModelRGBD*>(node);
attrs.insert ( std::pair<std::string,std::string>("port",int2str(rgbd->port)) );
attrs.insert ( std::pair<std::string,std::string>("noise",float2str(rgbd->noise) ) );
attrs.insert ( std::pair<std::string,std::string>("focal",float2str(rgbd->focal) ) );
attrs.insert ( std::pair<std::string,std::string>("height",float2str(rgbd->height) ) );
attrs.insert ( std::pair<std::string,std::string>("width",float2str(rgbd->width) ) );
attrs.insert ( std::pair<std::string,std::string>("ifconfig",rgbd->ifconfig.toStdString()) );
type ="rgbd";
attrs.insert ( std::pair<std::string,std::string>("imType",type ) );
}
else if (dynamic_cast<InnerModelCamera*>(node) != NULL)
{
InnerModelCamera* cam = dynamic_cast<InnerModelCamera*>(node);
attrs.insert ( std::pair<std::string,std::string>("focal",float2str(cam->focal) ) );
attrs.insert ( std::pair<std::string,std::string>("height",float2str(cam->height) ) );
attrs.insert ( std::pair<std::string,std::string>("width",float2str(cam->width) ) );
type = "camera";
attrs.insert ( std::pair<std::string,std::string>("imType",type ) );
}
else if (dynamic_cast<InnerModelIMU*>(node) != NULL)
{
InnerModelIMU* imu = dynamic_cast<InnerModelIMU*>(node);
attrs.insert ( std::pair<std::string,std::string>("port",int2str(imu->port)) );
type = "imu";
attrs.insert ( std::pair<std::string,std::string>("imType",type ) );
}
else if (dynamic_cast<InnerModelLaser*>(node) != NULL)
{
InnerModelLaser* laser = dynamic_cast<InnerModelLaser*>(node);
//public: uint32_t port; uint32_t min, max; float angle; uint32_t measures; QString ifconfig;
attrs.insert ( std::pair<std::string,std::string>("port",int2str(laser->port)) );
attrs.insert ( std::pair<std::string,std::string>("min",int2str(laser->min)) );
attrs.insert ( std::pair<std::string,std::string>("max",int2str(laser->max)) );
attrs.insert ( std::pair<std::string,std::string>("measures",int2str(laser->measures)) );
attrs.insert ( std::pair<std::string,std::string>("angle",float2str(laser->angle)) );
attrs.insert ( std::pair<std::string,std::string>("ifconfig",laser->ifconfig.toStdString()) );
type = "laser";
attrs.insert ( std::pair<std::string,std::string>("imType",type ) );
}
else if (dynamic_cast<InnerModelMesh*>(node) != NULL)
{
InnerModelMesh* mesh = dynamic_cast<InnerModelMesh*>(node);
attrs.insert ( std::pair<std::string,std::string>("path",mesh->meshPath.toStdString()) );
attrs.insert ( std::pair<std::string,std::string>("scalex",float2str(mesh->scalex)) );
attrs.insert ( std::pair<std::string,std::string>("scaley",float2str(mesh->scaley)) );
attrs.insert ( std::pair<std::string,std::string>("scalez",float2str(mesh->scalez)) );
string v="false";
if (mesh->collidable)
v="true";
attrs.insert ( std::pair<std::string,std::string>("collidable",v) );
//enum RenderingModes { NormalRendering=0, WireframeRendering=1};
v="NormalRendering";
if (mesh->render==1)
v="WireframeRendering";
attrs.insert ( std::pair<std::string,std::string>("render",v) );
type = "mesh";
attrs.insert ( std::pair<std::string,std::string>("imType",type ) );
}
else if (dynamic_cast<InnerModelPointCloud*>(node) != NULL)
{
type = "pointCloud";
attrs.insert ( std::pair<std::string,std::string>("imType",type ) );
}
else if (dynamic_cast<InnerModelTransform*>(node) != NULL)
{
InnerModelTransform *t = dynamic_cast<InnerModelTransform*>(node);
type="transform";
attrs.insert ( std::pair<std::string,std::string>("engine",t->engine.toStdString()) );
attrs.insert ( std::pair<std::string,std::string>("mass",float2str(t->mass)) );
attrs.insert ( std::pair<std::string,std::string>("imType",type ) );
}
else
{
string err;
err = "error: Type of node " + node->id.toStdString() + " is unknown.";
throw err;
}
int32_t id =worldModel->getNewId();
return worldModel->newSymbol(id,type,attrs);
}
void SpecificWorker::compute()
{
QMutexLocker l(mutex);
if (worldModel->size() == 0)
{
printf("Waiting for AGM*\n");
return;
}
static bool first = true;
static std::map<std::string, QTime> backTimes;
static RoboCompJointMotor::MotorStateMap backMotors;
if (first)
{
try
{
jointmotor_proxy->getAllMotorState(backMotors);
for (auto j : backMotors)
{
backTimes[j.first] = QTime::currentTime().addSecs(-1000000);
}
first = false;
}
catch (const Ice::Exception &ex)
{
std::cout << __FILE__ << ":" << __LINE__ << " --> Can't update InnerModel" << std::endl;
}
}
RoboCompJointMotor::MotorStateMap mMap;
std::vector<AGMModelEdge> edge_sequence;
AGMModel::SPtr newModel(new AGMModel(worldModel));
try
{
jointmotor_proxy->getAllMotorState(mMap);
for (auto j : mMap)
{
printf("Updating: %s (%f)\n", j.first.c_str(), mMap[j.first].pos);
if (backTimes[j.first].elapsed()>500 or abs(backMotors[j.first].pos-mMap[j.first].pos) > 0.25*M_PIl/180.) /* send if it changed more than half degree */
{
backTimes[j.first] = QTime::currentTime();
backMotors[j.first] = j.second;
// printf("Updating: %s (%d)\n", j.first.c_str(), newModel->size());
bool found = false;
for (AGMModel::iterator symbol_it=newModel->begin(); symbol_it!=newModel->end(); symbol_it++)
{
const AGMModelSymbol::SPtr &symbol = *symbol_it;
std::string imName;
// printf(" symbol id: %d\n", symbol->identifier);
try
{
// printf("<< %d >>\n", symbol->identifier);
try { imName = symbol->getAttribute("imName"); } catch(...) { }
if (imName == j.first)
{
// printf("found!\n");
found = true;
auto parent = newModel->getParentByLink(symbol->identifier, "RT");
// printf("%d (%d -> %d)\n", __LINE__, parent->identifier, symbol->identifier);
AGMModelEdge &e = newModel->getEdgeByIdentifiers(parent->identifier, symbol->identifier, "RT");
// printf("edge %s\n", e.toString(newModel).c_str());
// printf("edge rx %s\n", e.getAttribute("rx").c_str());
e.setAttribute("rx", "0");
e.setAttribute("ry", "0");
e.setAttribute("rz", "0");
std::string axis = symbol->getAttribute("axis");
e.setAttribute("r"+symbol->getAttribute("axis"), float2str(j.second.pos));
// printf(" axis %s\n", symbol->getAttribute("axis").c_str());
// printf(" edge rx %s\n", e.getAttribute("rx").c_str());
// printf(" edge rz %s\n", e.getAttribute("rz").c_str());
// printf(" edge ry %s\n", e.getAttribute("ry").c_str());
edge_sequence.push_back(e);
break;
}
}
catch(...)
{
printf(" couldn't retrieve node\n");
}
}
if (not found)
printf(" couln't find joint: %s\n", j.first.c_str());
}
}
//publish
if (edge_sequence.size() > 0)
{
try
{
if( edge_sequence.size() == 1)
AGMMisc::publishEdgeUpdate(edge_sequence.front(), agmexecutive_proxy);
else
AGMMisc::publishEdgesUpdate(edge_sequence, agmexecutive_proxy);
}
catch(...)
{
printf(" can't update node\n");
}
}
usleep(500);
// printf(" done!\n");
}
catch (const Ice::Exception &ex)
{
std::cout<<"--> Exception updating InnerModel"<<std::endl;
}
manageRestPositionEdge(mMap);
}
