50 bool start()
override;
52 bool reset()
override;
56 bool resample(
const double value)
override;
58 bool play_action(
const std::string& action_name)
override;
65 std::ostringstream oss;
66 oss << std::fixed << std::setprecision(2);
68 for (
size_t i = 0; i < vec.size(); ++i) {
70 if (i + 1 != vec.size()) oss <<
", ";
107 it->second =
nullptr;
118 it->second =
nullptr;
129 it->second =
nullptr;
137 yInfo() <<
"cleanup complete";
144 yError() <<
"action id not found";
153 if (!driver || !action)
155 yError() <<
"invalid driver/action pointer";
158 yInfo() <<
"action selected:" << id;
159 yDebug() <<
"action controller:" << controller_name;
166 std::string ss =
"actions:\n";
170 ss = ss +
"(" + std::to_string(i++) +
") " + it->second.action_name +
"\n";
180 yError() <<
"Unable to read configuration file!";
186 if (bot_clients.
size() == 0)
188 yError() <<
"Unable to read CLIENTS section";
191 size_t num_of_clients = bot_clients.
size();
192 for (
size_t i = 1; i < num_of_clients; i++)
196 if (bot_client_elem ==
nullptr)
198 yError() <<
"Invalid entry in CLIENTS section";
201 size_t num_of_client_elems = bot_client_elem->
size();
202 if (num_of_client_elems == 2)
205 std::string client_name = bot_client_elem->
get(0).
toString();
206 std::string remoteControlBoards = bot_client_elem->
get(1).
toString();
209 yError() <<
"Duplicate client entry: " << client_name;
214 clientoptions.
put(
"device",
"controlBoard_nwc_yarp");
215 clientoptions.
put(
"remote", remoteControlBoards);
216 clientoptions.
put(
"local",
m_name +
"/client/" + client_name);
217 clientoptions.
put(
"carrier",
"fast_tcp");
220 bool rob_ok = pd_client->
open(clientoptions);
223 yError() <<
"Unable to initialize client: " << client_name;
231 yError() <<
"Invalid number of entries in CLIENTS section";
238 if (bot_cont.
size() == 0)
240 yError() <<
"Unable to read REMAPPERS section";
243 size_t num_of_controllers = bot_cont.
size();
244 for (
size_t i = 1; i < num_of_controllers; i++)
248 if (bot_cont_elem ==
nullptr)
250 yError() <<
"Invalid entry in REMAPPERS section";
253 size_t num_of_celems = bot_cont_elem->
size();
254 if (num_of_celems == 3)
257 std::string remapper_name = bot_cont_elem->
get(0).
toString();
259 std::string axes_names = bot_cont_elem->
get(2).
toString();
260 if (clients_name==
nullptr || clients_name->
size() == 0)
262 yError() <<
"error in clients_name";
265 if (axes_names.empty())
267 yError() <<
"error in axes_names";
274 yError() <<
"Duplicate remapper entry: " << remapper_name;
279 rmoptions.
put(
"device",
"controlboardremapper");
281 rmoptions.
put(
"axesNames",jlist);
282 bool rob_ok = pd_remapper->
open(rmoptions);
285 yError() <<
"Unable to initialize remapper: " << remapper_name;
296 for (
size_t j=0; j<clients_name->
size(); j++)
302 yError() <<
"Configuration error: client '" << cnn <<
"' not found or is invalid.";
306 std::string boardname =
"nwcboard" + std::to_string(j);
307 pdlist.
push(ddnwc, boardname.c_str());
309 bool result_att = ww_rem->
attachAll(pdlist);
312 yError() <<
"Configuration error while trying to attach the remapper: "
313 << remapper_name <<
" with the control_boards: " << clients_name->
toString();
321 yError() <<
"Duplicate controller entry: " << remapper_name;
326 rob_ok &= robDrv->
init();
329 yError() <<
"Unable to initialize controller: " << remapper_name;
337 yError() <<
"Invalid number of entries in REMAPPERS section";
343 if (bot_action.
size() == 0)
345 yError() <<
"Unable to read ACTIONS section";
348 for (
size_t i = 1; i < bot_action.
size(); i++)
350 std::string str = bot_action.
toString();
353 if (bot_act_elem ==
nullptr)
355 yError() <<
"Invalid entry in ACTIONS section";
358 size_t num_of_aelems = bot_act_elem->
size();
359 if (num_of_aelems==3 || num_of_aelems == 4)
362 std::string action_name = bot_act_elem->
get(0).
toString();
363 std::string controller_name = bot_act_elem->
get(1).
toString();
364 std::string action_file_name = bot_act_elem->
get(2).
toString();
370 yError() << controller_name <<
"in action" << action_name <<
"does not exists";
380 if (!tmpAction.
openFile(action_file_name, njoints, 0.010))
382 yError() <<
"Unable to parse file";
385 if (resample_period!=0.0)
391 for (
size_t i=0; i<njoints; i++)
397 if (tolerances_bot->
size() != njoints)
399 yError() <<
"Invalid size of tolerances vector, should be"<<njoints <<
"got " << tolerances_bot->
size();
402 for (
size_t i=0; i<njoints; i++)
414 yError() <<
"Invalid entry in ACTIONS section, size should be 3 or 4, got " <<num_of_aelems;
419 yInfo() <<
"configuration file successfully loaded";
426 yInfo() <<
"Command line:";
427 yInfo() <<
"yarpActionsPlayer --filename `name` [--name `module_name`] [--execute] [--period period_s] [--resample resample_period_s] [--pos_tolerance pos] [--pos_timeout pos] [--pos_strict_check enable] ";
429 yInfo() <<
"`name` : file containing the actions";
430 yInfo() <<
"`module_name` : prefix of the ports opened by the module (default: /yarpActionsPlayer)";
431 yInfo() <<
"`verbose` : if enabled, extra debug messages are shown.";
432 yInfo() <<
"`execute` : if enabled, the yarpActionsPlayer will send commands to the robot. Otherwise, only it will operate in simulation mode only";
433 yInfo() <<
"`pos_tolerance` : the tolerance (in degrees) that will be checked by the initial movement in position mode, before switching to positionDirect mode. Default: 2degrees";
434 yInfo() <<
"`pos_timeout` : the amount of time (in seconds) the robot will attempt to reach the target position within the specified position tolerance. Default: 2s";
435 yInfo() <<
"`pos_strict_check` : if set to true, the system will halt if home position is halted, otherwise it will continue after the timeout expires. Default: false";
436 yInfo() <<
"`period` : the period (in s) of the thread processing the commands. Default 0.010s";
437 yInfo() <<
"`resample`: all the loaded trajectory files are internally resampled at the specified period. Default: not enabled";
438 yInfo() <<
"`initial_move_time`: the duration (in seconds) of the initial homing position of the joints before starting the trajectory. Default: 4s";
444 std::string test_string = rf.
toString();
447 if (rf.
check(
"name"))
450 m_name =
"/yarpActionsPlayer";
457 double period = 0.005;
458 if (rf.
check(
"period") ==
true)
467 if (rf.
check(
"execute")==
true)
469 yInfo() <<
"Enabling iPid->setReference() controller";
474 yInfo() <<
"Not using iPid->setReference() controller";
479 if (rf.
check(
"verbose")==
true)
481 yInfo() <<
"yarpActionPlayer running in verbose mode";
486 yInfo() <<
"yarpActionPlayer running in non-verbose mode";
491 if (rf.
check(
"help") ==
true)
497 if (rf.
check(
"initial_move_time") ==
true)
504 if (rf.
check(
"pos_tolerance") ==
true)
507 yInfo() <<
"Position tolerance set to " << tol <<
"degrees";
512 if (rf.
check(
"pos_timeout") ==
true)
515 yInfo() <<
"Position timeout set to " << timeout <<
"seconds";
520 if (rf.
check(
"pos_strict_check") ==
true)
522 bool enable = rf.
find(
"pos_strict_check").
asBool();
526 double resample_period = 0;
527 if (rf.
check(
"resample") ==
true)
530 yInfo() <<
"Set resample period equal to:" << resample_period <<
"s";
534 if (rf.
check(
"filename")==
true)
542 yError() <<
"Configuration error!";
548 yError() <<
"`filename` option syntax error.";
555 yWarning() <<
"`filename` option not found. No sequence files loaded.";
561 yInfo() <<
"There are no actions!";
566 yInfo() <<
"automatically selecting the first action";
567 std::string first_action_name;
568 first_action_name = this->m_actions.begin()->first;
574 yError() <<
"Working thread did not start, queue will not work";
578 yInfo() <<
"Working thread started";
581 yInfo() <<
"module successfully configured. ready.";
596int main(
int argc,
char *argv[])
604 if (!
yarp.checkNetwork())
606 yError() <<
"yarp.checkNetwork() failed.";
670 std::string current_action_name;
672 yInfo() <<
"current_action: " <<current_action_name;
673 yInfo() << actions_str;
682 yError(
"invalid period value");
686 yInfo(
"Period set to %f", value);
696 yError(
"invalid initial move time");
700 yInfo(
"Initial move time set to %f", value);
contains the definition of a Vector type
bool action_getname(std::string &name)
void setPositionStrictCheck(bool enable)
bool m_enable_execute_joint_command
void setInitialMoveTime(double t)
action_status_enum getStatus()
bool action_resample(double value)
bool action_setSpeedFactor(double factor=1)
void setPositionTolerance(double tolerance)
bool action_change(action_class *action, robotDriver *driver)
void setPositionTimeout(double timeloops)
void interpolate_action_frames(double timestep)
bool openFile(std::string filename, size_t njoints, double timestep=-1)
std::vector< double > m_tolerances
std::string controller_name
std::deque< action_frame > action_frames_vector
bool configure(yarp::dev::PolyDriver *pd)
bool reset() override
Rewinds the currently selected action.
bool speed_factor(const double value) override
Sets the playback speed factor for the currently selected action (default value: 1....
bool show_actions() override
Prints all the loaded actions.
std::string string_list_actions()
bool loadConfiguration(std::string filename, double resample_period)
std::map< std::string, action_class > m_actions
virtual bool updateModule()
Override this to do whatever your module needs to do.
BroadcastingThread * m_bthread
bool resample(const double value) override
Resamples the currently selected action (in seconds, recommended value 0.010s).
bool start() override
Start (or resumes, if stopped) the currently selected action.
bool choose_action(const std::string &action_name) override
Choose the current action and wait for further commands.
bool forever() override
Similar to play, but it will automatically restart the playback when the last frame is reached.
bool set_thread_period(const double value) override
Sets the period of the sampling thread (for advanced use only, default value: 0.010s).
bool print_frames() override
Prints all the frames of the currently selected action.
std::string m_current_action_id
virtual double getPeriod()
You can override this to control the approximate periodicity at which updateModule() is called by run...
std::map< std::string, yarp::dev::PolyDriver * > m_robotClients
virtual bool configure(yarp::os::ResourceFinder &rf)
Configure the module, pass a ResourceFinder object to the module.
bool stop() override
Stops the currently selected (running) action.
bool play_action(const std::string &action_name) override
Play an action one single time.
bool chooseActionByName(std::string id)
virtual bool close()
Close function.
std::map< std::string, yarp::dev::PolyDriver * > m_robotRemappers
bool set_initial_move_time(const double value) override
Sets the uration for the initial homing movement (for advanced use only, default value: 2s).
ControlThread * m_wthread
std::string vectorToString(const std::vector< double > &vec)
std::map< std::string, robotDriver * > m_robotControllers
yarpActionsPlayer_IDL Interface.
Interface for an object that can wrap/attach to to another.
virtual bool attachAll(const PolyDriverList &drivers)=0
Attach to a list of objects.
void push(PolyDriver *p, const char *k)
A container for a device driver.
bool open(const std::string &txt)
Construct and configure a device by its common name.
A simple collection of objects that can be described and transmitted in a portable way.
size_type size() const
Gets the number of elements in the bottle.
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
std::string toString() const override
Gives a human-readable textual representation of the bottle.
Utilities for manipulating the YARP network, including initialization and shutdown.
bool setPeriod(double period)
Set the (new) period of the thread.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
A mini-server for network communication.
void close() override
Stop port activity.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
A class for storing options and configuration information.
bool fromConfigFile(const std::string &fname, bool wipe=true)
Interprets a file as a list of properties.
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
Bottle & findGroup(const std::string &key) const override
Gets a list corresponding to a given keyword.
A base-class for standard YARP modules that supports ResourceFinder.
virtual int runModule()
Calls updateModule() until that returns false.
Helper class for finding config files and other external resources.
bool check(const std::string &key) const override
Check if there exists a property of the given name.
bool setDefaultContext(const std::string &contextName)
Sets the context for the current ResourceFinder object.
bool configure(int argc, char *argv[], bool skipFirstArgument=true)
Sets up the ResourceFinder.
std::string toString() const override
Return a standard text representation of the content of the object.
Value & find(const std::string &key) const override
Gets a value corresponding to a given keyword.
A single value (typically within a Bottle).
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
virtual bool isString() const
Checks if value is a string.
static Value * makeList()
Create a list Value.
virtual bool asBool() const
Get boolean value.
virtual Bottle * asList() const
Get list value.
std::string toString() const override
Return a standard text representation of the content of the object.
virtual std::string asString() const
Get string value.
bool attachAsServer(yarp::os::UnbufferedContactable &port)
Tag this WireLink as a server, receiving commands via the specified port.
yarp::os::WireLink & yarp()
Get YARP state associated with this object.
int main(int argc, char *argv[])
void delay(double seconds)
Wait for a certain number of seconds.
The main, catch-all namespace for YARP.