From e7d086a20ca3f76b2396d2292b38ee73e47b75d5 Mon Sep 17 00:00:00 2001 From: r1-user Date: Wed, 26 May 2021 18:05:31 +0200 Subject: [PATCH 1/3] fixed properties not handled correctly --- modules/lineDetector/src/main.cpp | 14 ++++++-------- modules/managerTUG/src/main.cpp | 11 ++++------- 2 files changed, 10 insertions(+), 15 deletions(-) diff --git a/modules/lineDetector/src/main.cpp b/modules/lineDetector/src/main.cpp index eeacd519..1e91c300 100644 --- a/modules/lineDetector/src/main.cpp +++ b/modules/lineDetector/src/main.cpp @@ -298,8 +298,9 @@ class Detector : public RFModule, public lineDetector_IDL cmd.addString(model_name); if (gazeboPort.write(cmd,rep)) { - Property prop(rep.get(0).toString().c_str()); - Bottle *model=prop.find(model_name).asList(); + // Property prop(rep.get(0).toString().c_str()); + // Bottle *model=prop.find(model_name).asList(); + Bottle *model=rep.get(0).asList(); if (Bottle *mod_bottle=model->find("pose_world").asList()) { if(mod_bottle->size()>=7) @@ -416,7 +417,6 @@ class Detector : public RFModule, public lineDetector_IDL Bottle line; line.addList().read(poseProp); prop.put(line_tag,line.get(0)); - if(opc_id<0) { cmd.addList().read(prop); @@ -747,8 +747,8 @@ class Detector : public RFModule, public lineDetector_IDL cmd.addString("get_state"); if (navPort.write(cmd,rep)) { - Property robotState(rep.get(0).toString().c_str()); - if (Bottle *loc=robotState.find("robot-location").asList()) + Bottle *robotState=rep.get(0).asList(); + if (Bottle *loc=robotState->find("robot-location").asList()) { yarp::sig::Vector robot_location(7,0.0); robot_location[0]=loc->get(0).asDouble(); @@ -756,7 +756,7 @@ class Detector : public RFModule, public lineDetector_IDL robot_location[5]=1.0; robot_location[6]=(M_PI/180)*loc->get(2).asDouble(); navFrame=yarp::math::axis2dcm(robot_location.subVector(3,6)); - navFrame.setSubcol(robot_location.subVector(0,2),0,3); + navFrame.setSubcol(robot_location.subVector(0,2),0,3); updated_nav=true; return true; } @@ -772,7 +772,6 @@ class Detector : public RFModule, public lineDetector_IDL cmd.addDouble(x); cmd.addDouble(y); cmd.addDouble(theta); - yDebug()<find("pose_world").asList()) { if(pose->size()>=7) @@ -1963,7 +1961,7 @@ class Manager : public RFModule, public managerTUG_IDL set_walking_speed(rep); } } - human_state=rep.get(0).find("human-state").asString(); + human_state=rep.find("human-state").asString(); yInfo()<<"Human state"<find("robot-location").asList()) { double x=loc->get(0).asDouble(); double line_center=(p0[0]+p1[0])/2; @@ -2256,7 +2254,6 @@ class Manager : public RFModule, public managerTUG_IDL cmd.addInt(0); cmd.addString("pos"); cmd.addList().read(target); - yDebug()<write(cmd,rep)) { if (rep.get(0).asBool()==true) From 151469f1a5a546e47f3c4e0d6b36dc40d42be3fc Mon Sep 17 00:00:00 2001 From: vvasco Date: Fri, 17 Sep 2021 23:18:09 +0200 Subject: [PATCH 2/3] [motionAnalyzer] fixed wrong parameters --- modules/motionAnalyzer/include/Metric.h | 2 +- modules/motionAnalyzer/src/Metric.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/modules/motionAnalyzer/include/Metric.h b/modules/motionAnalyzer/include/Metric.h index 50c93f4e..7fa4db46 100644 --- a/modules/motionAnalyzer/include/Metric.h +++ b/modules/motionAnalyzer/include/Metric.h @@ -79,7 +79,7 @@ class Step : public Metric { yarp::sig::Vector num; yarp::sig::Vector den; - double minv,maxv,thresh,step_window,time_window; + double thresh,step_window,time_window,minv,maxv; } step_params; Step() {;} diff --git a/modules/motionAnalyzer/src/Metric.cpp b/modules/motionAnalyzer/src/Metric.cpp index f6f9b962..1470bcaf 100644 --- a/modules/motionAnalyzer/src/Metric.cpp +++ b/modules/motionAnalyzer/src/Metric.cpp @@ -42,11 +42,11 @@ Rom::Rom(const string &type, const string &name, const RomParams ¶ms) this->type=type; this->name=name; this->rom_params.tag_joint=params.tag_joint; - this->rom_params.tag_plane=rom_params.tag_plane; - this->rom_params.ref_dir=rom_params.ref_dir; - this->rom_params.ref_joint=rom_params.ref_joint; - this->rom_params.minv=rom_params.minv; - this->rom_params.maxv=rom_params.maxv; + this->rom_params.tag_plane=params.tag_plane; + this->rom_params.ref_dir=params.ref_dir; + this->rom_params.ref_joint=params.ref_joint; + this->rom_params.minv=params.minv; + this->rom_params.maxv=params.maxv; properties.push_back("range"); } From 9fdf970b3691ebbee7d341cfa7e057885ed612bc Mon Sep 17 00:00:00 2001 From: vvasco Date: Tue, 21 Sep 2021 12:08:29 +0200 Subject: [PATCH 3/3] added several features for exposing the ground truth and improve computation: - exposing time, success and interaction status - added getTorsoFront and enabled collision to actor torso - using hip to check if line is crossed - added overhead light to tug scenario to improve open pose results - computing steps based only on y position of feet --- .../interface/include/tuginterface.h | 2 + .../interface/include/tugserver.h | 40 +++++++- .../tuginterface/interface/include/utils.h | 2 +- .../interface/src/tuginterface.cpp | 4 +- .../tuginterface/interface/src/tugserver.cpp | 97 ++++++++++++++++++- .../tuginterface/interface/src/utils.cpp | 9 +- .../thrift/tuginterface_rpc.thrift | 32 ++++++ app/gazebo/tug/tug-scenario.world | 39 +++++++- app/gazebo/tug/tugInterface.ini | 5 +- modules/managerTUG/src/idl.thrift | 18 ++++ modules/managerTUG/src/main.cpp | 75 +++++++++----- modules/motionAnalyzer/app/conf/tug.ini | 2 +- modules/motionAnalyzer/include/Manager.h | 1 + modules/motionAnalyzer/src/Manager.cpp | 70 ++++++++----- modules/motionAnalyzer/src/Processor.cpp | 22 +++-- modules/navController/src/idl.thrift | 7 ++ modules/navController/src/main.cpp | 7 ++ 17 files changed, 363 insertions(+), 69 deletions(-) diff --git a/app/gazebo/tug/plugins/tuginterface/interface/include/tuginterface.h b/app/gazebo/tug/plugins/tuginterface/interface/include/tuginterface.h index c5d0a527..bf99bcb9 100644 --- a/app/gazebo/tug/plugins/tuginterface/interface/include/tuginterface.h +++ b/app/gazebo/tug/plugins/tuginterface/interface/include/tuginterface.h @@ -38,6 +38,8 @@ class TugInterface : public ModelPlugin physics::WorldPtr world; physics::ActorPtr actor; Velocity vel; + double walktime; + int nsteps; std::string starting_animation; yarp::sig::Matrix targets; std::map waypoints_map; diff --git a/app/gazebo/tug/plugins/tuginterface/interface/include/tugserver.h b/app/gazebo/tug/plugins/tuginterface/interface/include/tugserver.h index 4bcfabab..2d95c7d5 100644 --- a/app/gazebo/tug/plugins/tuginterface/interface/include/tugserver.h +++ b/app/gazebo/tug/plugins/tuginterface/interface/include/tugserver.h @@ -15,6 +15,10 @@ #include #include #include +#include +#include +#include +#include #include @@ -28,6 +32,8 @@ class TugServer: public TugInterfaceServer gazebo::physics::WorldPtr world; gazebo::physics::ActorPtr actor; Velocity vel; + double walktime,standuptime,sitdowntime; + int nsteps; yarp::sig::Matrix targets; yarp::sig::Matrix line_frame; @@ -54,6 +60,32 @@ class TugServer: public TugInterfaceServer */ virtual double getSpeed(); + /** + * Get time taken to complete the specified animation. + * @param animation_name name of the animation defined in the sdf. + * @param nsamples total number of samples for computing the average time. + * @return returns animation time. + */ + virtual double getTime(const std::string &animation_name, const int nsamples); + + /** + * Get walking time taken to complete the trajectory. + * @return returns walking time. + */ + virtual double getWalkingTime(); + + /** + * Get time taken to stand up and sit down. + * @return returns stand up plus sit down time. + */ + virtual double getStandSitTime(); + + /** + * Get number of steps. + * @return returns number of steps. + */ + virtual int getNumSteps(); + /** * Get the list of animations associated with the actor. * @return returns list of animations associated with the actor. @@ -97,6 +129,12 @@ class TugServer: public TugInterfaceServer */ virtual bool playFromLast(const bool complete); + /** + * Get the torso front surface x coordinate. + * @return returns double defining the torso front surface x coordinate. + */ + virtual double getTorsoFront(); + /** * Get current animation being played. * @return returns string defining the current animation being played. @@ -134,7 +172,7 @@ class TugServer: public TugInterfaceServer void updateMap(const yarp::sig::Matrix &t); - void init(const Velocity &vel, const yarp::sig::Matrix &waypoints); + void init(const Velocity &vel, const yarp::sig::Matrix &waypoints, const double &walktime, const int &nsteps); void attachWorldPointer(gazebo::physics::WorldPtr p); diff --git a/app/gazebo/tug/plugins/tuginterface/interface/include/utils.h b/app/gazebo/tug/plugins/tuginterface/interface/include/utils.h index 94b056db..bc0d367a 100644 --- a/app/gazebo/tug/plugins/tuginterface/interface/include/utils.h +++ b/app/gazebo/tug/plugins/tuginterface/interface/include/utils.h @@ -23,7 +23,7 @@ struct Velocity void updateScript(sdf::ElementPtr &actor_sdf, const std::map &m); -std::map createMap(const yarp::sig::Matrix &t, const Velocity &vel); +std::map createMap(const yarp::sig::Matrix &t, const Velocity &vel, double &walktime, int &nsteps); std::map generateWaypoints(const Velocity &vel, const std::map &m); diff --git a/app/gazebo/tug/plugins/tuginterface/interface/src/tuginterface.cpp b/app/gazebo/tug/plugins/tuginterface/interface/src/tuginterface.cpp index 7fc33ac9..ef7caff7 100644 --- a/app/gazebo/tug/plugins/tuginterface/interface/src/tuginterface.cpp +++ b/app/gazebo/tug/plugins/tuginterface/interface/src/tuginterface.cpp @@ -120,7 +120,7 @@ void TugInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) return; } - waypoints_map=createMap(targets,vel); + waypoints_map=createMap(targets,vel,walktime,nsteps); waypoints_map=generateWaypoints(vel,waypoints_map); sdf::ElementPtr world_sdf=world->SDF(); @@ -143,7 +143,7 @@ void TugInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) m_rpcport.open(portname); server.yarp().attachAsServer(m_rpcport); - server.init(vel,targets); + server.init(vel,targets,walktime,nsteps); // Listen to the update event. This event is broadcast every // simulation iteration. diff --git a/app/gazebo/tug/plugins/tuginterface/interface/src/tugserver.cpp b/app/gazebo/tug/plugins/tuginterface/interface/src/tugserver.cpp index 3c613eec..95c2b97f 100644 --- a/app/gazebo/tug/plugins/tuginterface/interface/src/tugserver.cpp +++ b/app/gazebo/tug/plugins/tuginterface/interface/src/tugserver.cpp @@ -18,7 +18,8 @@ using namespace std; using namespace gazebo; /****************************************************************/ -TugServer::TugServer() : world(0), actor(0), vel(0.0,0.0) +TugServer::TugServer() : world(0), actor(0), vel(0.0,0.0), walktime(0.0), + standuptime(0.0), sitdowntime(0.0), nsteps(0) { line_frame=yarp::math::eye(4,4); } @@ -72,6 +73,22 @@ bool TugServer::playFromLast(const bool complete) return true; } +/****************************************************************/ +double TugServer::getTorsoFront() +{ + auto link=actor->GetLink("LowerBack"); + auto collision=link->GetCollisions()[0]; + auto boxShape = boost::dynamic_pointer_cast( + collision->GetShape()); + auto size = boxShape->Size(); + + auto actor_pose=actor->WorldPose().Pos(); + auto box_pose=collision->WorldPose().Pos(); + + double torso_front=box_pose.X()-actor_pose.X()+size[0]/2; + return torso_front; +} + /****************************************************************/ string TugServer::getState() { @@ -148,7 +165,9 @@ bool TugServer::setTarget(const Pose &p) ty[1]=t.Pos().Y(); ty[2]=t.Rot().Yaw(); targets.setRow(1,ty); + targets(2,0)=ty[0]; updateMap(targets); + yDebug()<<"New target matrix"<vel.lin_vel; } +/****************************************************************/ +double TugServer::getTime(const string &animation_name, const int nsamples) +{ + double time=0.0; + for(int i=0; iPlayWithAnimationName(animation_name,false); + while(actor->IsActive()) + { + // yInfo()<<"Computing time for"<SkeletonAnimations(); + double tottime=0.0; + for (auto it=skel_m.begin(); it!=skel_m.end(); it++) + { + string animation=it->first; + if (animation=="stand_up") + { + double tstart=yarp::os::Time::now(); + actor->PlayWithAnimationName(animation,false); + while(actor->IsActive()) + { +// yInfo()<<"Computing time for"<standuptime=yarp::os::Time::now()-tstart; + yInfo()<<"Standup time"<standuptime; + } + if (animation=="sit_down") + { + double tstart=yarp::os::Time::now(); + actor->PlayWithAnimationName(animation,false); + while(actor->IsActive()) + { +// yInfo()<<"Computing time for"<sitdowntime=yarp::os::Time::now()-tstart; + yInfo()<<"Sitdown time"<sitdowntime; + } + tottime=this->sitdowntime+this->standuptime; + } + + return tottime; +} + + +/****************************************************************/ +double TugServer::getWalkingTime() +{ + return this->walktime; +} + +/****************************************************************/ +int TugServer::getNumSteps() +{ + return this->nsteps; +} + /****************************************************************/ std::vector TugServer::getAnimationList() { @@ -258,7 +345,7 @@ yarp::os::Property TugServer::getModelPos(const string &model_name) /****************************************************************/ void TugServer::updateMap(const yarp::sig::Matrix &t) { - std::map wp_map=createMap(t,this->vel); + std::map wp_map=createMap(t,this->vel,this->walktime,this->nsteps); wp_map=generateWaypoints(this->vel,wp_map); sdf::ElementPtr world_sdf=world->SDF(); @@ -269,10 +356,14 @@ void TugServer::updateMap(const yarp::sig::Matrix &t) /****************************************************************/ void TugServer::init(const Velocity &vel, - const yarp::sig::Matrix &targets) + const yarp::sig::Matrix &targets, + const double &walktime, + const int &nsteps) { this->vel=vel; this->targets=targets; + this->walktime=walktime; + this->nsteps=nsteps; } /****************************************************************/ diff --git a/app/gazebo/tug/plugins/tuginterface/interface/src/utils.cpp b/app/gazebo/tug/plugins/tuginterface/interface/src/utils.cpp index fa602e32..fc629bde 100644 --- a/app/gazebo/tug/plugins/tuginterface/interface/src/utils.cpp +++ b/app/gazebo/tug/plugins/tuginterface/interface/src/utils.cpp @@ -44,15 +44,19 @@ void updateScript(sdf::ElementPtr &actor_sdf, const std::map createMap(const yarp::sig::Matrix &t, const Velocity &vel) +std::map createMap(const yarp::sig::Matrix &t, const Velocity &vel, double &walktime, int &nsteps) { std::map m; yarp::sig::Vector t0=t.getRow(0); + walktime=0.0; double duration=0.0; + double step_length=0.643; + nsteps=0; for (int i=0; i0.0) { duration+=dist/vel.lin_vel; @@ -67,6 +71,9 @@ std::map createMap(const yarp::sig::Matrix &t, c m.insert(std::pair(duration,p)); t0=t1; } + yDebug()<<"updating map.. with duration"< - - - model://sun - - model://ground_plane + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 2.0 0.0 2.0 0 -0 0 + 0.5 0.5 0.5 1 + 0.1 0.1 0.1 1 + 0.1 0.1 -0.9 + + 20 + 0.5 + 0.01 + 0.001 + + 1 + @@ -37,6 +60,9 @@ + + + stand.dae @@ -65,16 +91,19 @@ + model://tugInterface.ini + model://VisitorChair + -0.50 0.0 0.0 0.0 0.0 1.57 diff --git a/app/gazebo/tug/tugInterface.ini b/app/gazebo/tug/tugInterface.ini index f8fea710..aff87d80 100644 --- a/app/gazebo/tug/tugInterface.ini +++ b/app/gazebo/tug/tugInterface.ini @@ -1,8 +1,9 @@ name /tug_input_port starting_animation "stand" targets (0.0 0.0 0.0 - 4.5 0.0 0.0 + 4.0 0.0 0.0 + 4.0 0.0 3.14 0.2 0.0 3.14 0.2 0.0 0.0) linear-velocity 1.0 -angular-velocity 360.0 +angular-velocity 120.0 diff --git a/modules/managerTUG/src/idl.thrift b/modules/managerTUG/src/idl.thrift index 832b6eff..a9e291bd 100644 --- a/modules/managerTUG/src/idl.thrift +++ b/modules/managerTUG/src/idl.thrift @@ -43,4 +43,22 @@ service managerTUG_IDL * @return true/false on success/failure. */ bool set_target(1: double x=4.5, 2: double y=0.0, 3: double theta=0.0); + + /** + * Get the time the user takes to complete the test. + * @return measured time. + */ + double get_measured_time(); + + /** + * Get the success status of the test. + * @return passed / not_passed if the test was passed or not. + */ + string get_success_status(); + + /** + * Returns true when the interaction has finished. + * @return true / false if the interaction has finished or not. + */ + bool has_finished(); } diff --git a/modules/managerTUG/src/main.cpp b/modules/managerTUG/src/main.cpp index e4dad569..e9393242 100644 --- a/modules/managerTUG/src/main.cpp +++ b/modules/managerTUG/src/main.cpp @@ -771,6 +771,8 @@ class Manager : public RFModule, public managerTUG_IDL bool interrupting; mutex mtx; bool start_ex,ok_go,connected,params_set; + string success_status; + bool test_finished; Vector finishline_pose; double line_length; @@ -1043,13 +1045,15 @@ class Manager : public RFModule, public managerTUG_IDL } if (ok_nav) { + yInfo()<<"Asking to go to"<find("pose_world").asList()) - { - if(pose->size()>=7) - { - starting_pose[0]=pose->get(0).asDouble(); - starting_pose[1]=pose->get(1).asDouble(); - starting_pose[2]=pose->get(6).asDouble()*(180.0/M_PI); - } - } - } - } + // if (simulation) + // { + // Bottle cmd,rep; + // cmd.addString("getModelPos"); + // cmd.addString("SIM_CER_ROBOT"); + // if (gazeboPort.write(cmd,rep)) + // { + // Bottle *model=rep.get(0).asList(); + // if (Bottle *pose=model->find("pose_world").asList()) + // { + // if(pose->size()>=7) + // { + // starting_pose[0]=pose->get(0).asDouble(); + // starting_pose[1]=pose->get(1).asDouble(); + // starting_pose[2]=pose->get(6).asDouble()*(180.0/M_PI); + // } + // } + // } + // } state=State::idle; bool ret=false; + yInfo()<<"Asking to go to"<wakeUp(); return start_ex; } @@ -1185,6 +1192,27 @@ class Manager : public RFModule, public managerTUG_IDL return false; } + /****************************************************************/ + double get_measured_time() override + { + lock_guard lg(mtx); + return t; + } + + /****************************************************************/ + string get_success_status() override + { + lock_guard lg(mtx); + return success_status; + } + + /****************************************************************/ + bool has_finished() override + { + lock_guard lg(mtx); + return test_finished; + } + /****************************************************************/ bool stop() override { @@ -1842,7 +1870,7 @@ class Manager : public RFModule, public managerTUG_IDL { x+=line_length; } - double y=finishline_pose[1]-0.5; + double y=finishline_pose[1]-1.0; double theta=starting_pose[2]; ok_go=go_to(Vector({x,y,theta}),false); } @@ -2050,6 +2078,7 @@ class Manager : public RFModule, public managerTUG_IDL if (state==State::finished) { + t=Time::now()-tstart; prev_state=state; yInfo()<<"Test finished in"<> p; @@ -2063,11 +2092,13 @@ class Manager : public RFModule, public managerTUG_IDL s.reset(); s.setKey("greetings"); speak(s); + success_status="passed"; disengage(); } if (state==State::not_passed) { + t=Time::now()-tstart; prev_state=state; Bottle cmd,rep; cmd.addString("stop"); diff --git a/modules/motionAnalyzer/app/conf/tug.ini b/modules/motionAnalyzer/app/conf/tug.ini index 7ae95d0c..432c2b16 100644 --- a/modules/motionAnalyzer/app/conf/tug.ini +++ b/modules/motionAnalyzer/app/conf/tug.ini @@ -1,6 +1,6 @@ [general] type test -finish-line-thresh 0.3 +finish-line-thresh 0.0 standing-thresh 0.025 distance 3.0 time-high 10.0 diff --git a/modules/motionAnalyzer/include/Manager.h b/modules/motionAnalyzer/include/Manager.h index 4db976fc..db66b117 100644 --- a/modules/motionAnalyzer/include/Manager.h +++ b/modules/motionAnalyzer/include/Manager.h @@ -40,6 +40,7 @@ class Manager : public yarp::os::RFModule, yarp::os::RpcClient scalerPort; yarp::os::RpcClient dtwPort; yarp::os::RpcClient actionPort; + yarp::os::RpcClient navPort; yarp::os::BufferedPort scopePort; enum class State { idle, sitting, standing, crossed } state; diff --git a/modules/motionAnalyzer/src/Manager.cpp b/modules/motionAnalyzer/src/Manager.cpp index f99ede43..ad2e2fe3 100644 --- a/modules/motionAnalyzer/src/Manager.cpp +++ b/modules/motionAnalyzer/src/Manager.cpp @@ -668,7 +668,7 @@ bool Manager::runDtw(const Matrix &T) /********************************************************/ bool Manager::start(const bool use_robot_template) { - lock_guard lg(mtx); + lock_guard lg(mtx); this->use_robot_template = use_robot_template; yInfo() << "Start!"; starting=true; @@ -712,6 +712,11 @@ bool Manager::start(const bool use_robot_template) } } for(int i=0; ireset(); + } + bResult.clear(); + for(int i=0; isetStartingTime(Time::now()); } @@ -776,14 +781,9 @@ bool Manager::stop() /****************************************************************/ void Manager::reset() { - for(int i=0; ireset(); - } nsession++; starting=false; skel_tag=""; - bResult.clear(); curr_exercise=NULL; curr_metric=NULL; standing=false; @@ -873,8 +873,9 @@ bool Manager::isSitting() /********************************************************/ bool Manager::hasCrossedFinishLine() { - Vector foot_right=skeletonIn[KeyPointTag::ankle_right]->getPoint(); - Vector foot_left=skeletonIn[KeyPointTag::ankle_left]->getPoint(); + Vector hip=skeletonIn[KeyPointTag::hip_center]->getPoint(); + // Vector foot_right=skeletonIn[KeyPointTag::ankle_right]->getPoint(); + // Vector foot_left=skeletonIn[KeyPointTag::ankle_left]->getPoint(); Vector lp_world(3); lp_world[0]=line_pose[0]; @@ -890,16 +891,24 @@ bool Manager::hasCrossedFinishLine() Vector line_x=lOri.getCol(0); line_x.pop_back(); - Vector fr_lp=lp_world-foot_right; - Vector fl_lp=lp_world-foot_left; - Vector pline=lp_world+line_x; - Vector v1=lp_world-pline; - double dist_fr_line=norm(cross(v1,fr_lp))/norm(v1); - double dist_fl_line=norm(cross(v1,fl_lp))/norm(v1); - yInfo()<<"dist foot right line"<update_planes() && starting) { vector> keyps=skeletonIn.get_unordered(); - all_keypoints.push_back(keyps); + Bottle repNav; + repNav.clear(); + cmd.clear(); + cmd.addString("get_state"); + navPort.write(cmd, repNav); + if(Bottle *rob_state=repNav.get(0).asList()) + { + if(rob_state->check("robot-location")) + { + Bottle *rob_locB=rob_state->find("robot-location").asList(); + double robx=rob_locB->get(0).asDouble(); + double roby=rob_locB->get(1).asDouble(); + double robtheta=rob_locB->get(2).asDouble(); + keyps.push_back(make_pair("robotLocation",Vector{robx,roby,robtheta})); + all_keypoints.push_back(keyps); + } + } } if(skeletonIn[KeyPointTag::shoulder_center]->isUpdated() && curr_exercise->getName()==ExerciseTag::tug) { @@ -1038,6 +1063,7 @@ bool Manager::configure(ResourceFinder &rf) scalerPort.open(("/" + getName() + "/scaler:cmd").c_str()); dtwPort.open(("/" + getName() + "/dtw:cmd").c_str()); actionPort.open(("/" + getName() + "/action:cmd").c_str()); + navPort.open(("/" + getName() + "/nav:cmd").c_str()); rpcPort.open(("/" + getName() + "/cmd").c_str()); attach(rpcPort); @@ -1069,6 +1095,7 @@ bool Manager::interruptModule() scalerPort.interrupt(); dtwPort.interrupt(); actionPort.interrupt(); + navPort.interrupt(); rpcPort.interrupt(); yInfo() << "Interrupted module"; return true; @@ -1096,6 +1123,7 @@ bool Manager::close() scalerPort.close(); dtwPort.close(); actionPort.close(); + navPort.close(); rpcPort.close(); yInfo() << "Closed ports"; return true; @@ -1142,7 +1170,7 @@ bool Manager::updateModule() /********************************************************/ bool Manager::writeStructToMat(const string& name, const vector< vector< pair > >& keypoints_skel, mat_t *matfp) { - int numKeypoints = skeletonIn.getNumKeyPoints(); + int numKeypoints = skeletonIn.getNumKeyPoints()+1; const char *fields[numKeypoints]; for(int i=0; i lck(mtx_update); + distance_target=dist; + return true; + } + /****************************************************************/ public: